Spiri/ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fixed moveto command

Browse Source
This commit is contained in:
vivek-shankar 2018-07-22 15:10:13 -04:00
parent 14ceab1047
commit 22aacee14e
2 changed files with 81 additions and 81 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/buzzuav_closures.cpp
View File

@ -228,8 +228,8 @@ int buzzuav_moveto(buzzvm_t vm)
goto_pos[2] = height + dh; goto_pos[2] = height + dh;
goto_pos[3] = dyaw; goto_pos[3] = dyaw;
// DEBUG // DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0], ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]); goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros? buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }

158
src/roscontroller.cpp
View File

@ -706,88 +706,88 @@ script
armstate = 0; armstate = 0;
break; break;
// case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!! case buzzuav_closures::COMMAND_GOHOME: // TODO: NOT FULLY IMPLEMENTED/TESTED !!!
// cmd_srv.request.param5 = home.latitude; cmd_srv.request.param5 = home.latitude;
// cmd_srv.request.param6 = home.longitude; cmd_srv.request.param6 = home.longitude;
// cmd_srv.request.param7 = home.altitude; cmd_srv.request.param7 = home.altitude;
// cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
// if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
// { {
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
// } }
// else else
// ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
// break; break;
// case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!! case buzzuav_closures::COMMAND_GOTO: // TOOD: NOT FULLY IMPLEMENTED/TESTED !!!
// goto_pos = buzzuav_closures::getgoto(); goto_pos = buzzuav_closures::getgoto();
// cmd_srv.request.param5 = goto_pos[0]; cmd_srv.request.param5 = goto_pos[0];
// cmd_srv.request.param6 = goto_pos[1]; cmd_srv.request.param6 = goto_pos[1];
// cmd_srv.request.param7 = goto_pos[2]; cmd_srv.request.param7 = goto_pos[2];
// cmd_srv.request.command = buzzuav_closures::getcmd(); cmd_srv.request.command = buzzuav_closures::getcmd();
// if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
// { {
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
// } }
// else else
// ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
// cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
// if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
// { {
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
// } }
// else else
// ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
// break; break;
// case buzzuav_closures::COMMAND_ARM: case buzzuav_closures::COMMAND_ARM:
// if (!armstate) if (!armstate)
// { {
// SetMode("LOITER", 0); SetMode("LOITER", 0);
// armstate = 1; armstate = 1;
// Arm(); Arm();
// } }
// break; break;
// case buzzuav_closures::COMMAND_DISARM: case buzzuav_closures::COMMAND_DISARM:
// if (armstate) if (armstate)
// { {
// armstate = 0; armstate = 0;
// SetMode("LOITER", 0); SetMode("LOITER", 0);
// Arm(); Arm();
// } }
// break; break;
// case buzzuav_closures::COMMAND_MOVETO: case buzzuav_closures::COMMAND_MOVETO:
// goto_pos = buzzuav_closures::getgoto(); goto_pos = buzzuav_closures::getgoto();
// roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]); roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
// break; break;
// case buzzuav_closures::COMMAND_GIMBAL: case buzzuav_closures::COMMAND_GIMBAL:
// gimbal = buzzuav_closures::getgimbal(); gimbal = buzzuav_closures::getgimbal();
// cmd_srv.request.param1 = gimbal[0]; cmd_srv.request.param1 = gimbal[0];
// cmd_srv.request.param2 = gimbal[1]; cmd_srv.request.param2 = gimbal[1];
// cmd_srv.request.param3 = gimbal[2]; cmd_srv.request.param3 = gimbal[2];
// cmd_srv.request.param4 = gimbal[3]; cmd_srv.request.param4 = gimbal[3];
// cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL; cmd_srv.request.command = mavros_msgs::CommandCode::CMD_DO_MOUNT_CONTROL;
// if (mav_client.call(cmd_srv)) if (mav_client.call(cmd_srv))
// { {
// ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success);
// } }
// else else
// ROS_ERROR("Failed to call service from flight controller"); ROS_ERROR("Failed to call service from flight controller");
// break; break;
// case buzzuav_closures::COMMAND_PICTURE: case buzzuav_closures::COMMAND_PICTURE:
// ROS_INFO("TAKING A PICTURE HERE!! --------------"); ROS_INFO("TAKING A PICTURE HERE!! --------------");
// mavros_msgs::CommandBool capture_command; mavros_msgs::CommandBool capture_command;
// if (capture_srv.call(capture_command)) if (capture_srv.call(capture_command))
// { {
// ROS_INFO("Reply: %ld", (long int)capture_command.response.success); ROS_INFO("Reply: %ld", (long int)capture_command.response.success);
// } }
// else else
// ROS_ERROR("Failed to call service from camera streamer"); ROS_ERROR("Failed to call service from camera streamer");
// break; break;
} }
} }
@ -998,9 +998,9 @@ void roscontroller::SetLocalPosition(float x, float y, float z, float yaw)
moveMsg.pose.orientation.w = q[3]; moveMsg.pose.orientation.w = q[3];
// To prevent drifting from stable position, uncomment // To prevent drifting from stable position, uncomment
// if(fabs(x)>0.005 || fabs(y)>0.005) { if(fabs(x)>0.005 || fabs(y)>0.005) {
localsetpoint_nonraw_pub.publish(moveMsg); localsetpoint_nonraw_pub.publish(moveMsg);
// } }
} }
void roscontroller::SetMode(std::string mode, int delay_miliseconds) void roscontroller::SetMode(std::string mode, int delay_miliseconds)