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

new fix for localpose

This commit is contained in:
dave 2017-04-21 16:31:05 -04:00
parent 84d64a1979
commit 421d78bacc
1 changed files with 6 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/roscontroller.cpp
View File

@ -461,8 +461,7 @@ namespace rosbzz_node{
Arm(); Arm();
} }
} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
/*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/ roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
roscontroller::SetLocalPosition(goto_pos[1], goto_pos[0], goto_pos[2], 0);
//roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0); //roscontroller::SetLocalPositionNonRaw(goto_pos[1], goto_pos[0], goto_pos[2], 0);
} }
@ -695,8 +694,9 @@ namespace rosbzz_node{
double local_pos[3]; double local_pos[3];
cvt_ned_coordinates(cur_pos,local_pos,home); cvt_ned_coordinates(cur_pos,local_pos,home);
ROS_INFO("ROSBuzz LocalPos: %.7f,%.7f",local_pos[0],local_pos[1]); ROS_INFO("ROSBuzz LocalPos: %.7f,%.7f",local_pos[0],local_pos[1]);
moveMsg.pose.position.x = local_pos[0]+x; /*prepare the goto publish message ATTENTION: ENU FRAME FOR MAVROS STANDARD (then converted to NED)*/
moveMsg.pose.position.y = local_pos[1]+y; moveMsg.pose.position.x = local_pos[1]+y;
moveMsg.pose.position.y = local_pos[0]+x;
moveMsg.pose.position.z = z; moveMsg.pose.position.z = z;
moveMsg.pose.orientation.x = 0; moveMsg.pose.orientation.x = 0;
@ -705,10 +705,10 @@ namespace rosbzz_node{
moveMsg.pose.orientation.w = 1; moveMsg.pose.orientation.w = 1;
if(fabs(x)>0.01 || fabs(y)>0.01) { //if(fabs(x)>0.01 || fabs(y)>0.01) {
localsetpoint_nonraw_pub.publish(moveMsg); localsetpoint_nonraw_pub.publish(moveMsg);
ROS_INFO("Sent local NON RAW position message!"); ROS_INFO("Sent local NON RAW position message!");
} //}
} }
void roscontroller::SetMode(std::string mode, int delay_miliseconds){ void roscontroller::SetMode(std::string mode, int delay_miliseconds){