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[3] = dyaw;
// DEBUG
// 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]);
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]);
buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
return buzzvm_ret0(vm);
}

158
src/roscontroller.cpp
View File

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