From 3af2a03ffc8b08be7a34f094af1f7426e33a81b7 Mon Sep 17 00:00:00 2001 From: isvogor Date: Thu, 13 Apr 2017 09:39:45 -0400 Subject: [PATCH] topic fix --- include/roscontroller.h | 1 - src/roscontroller.cpp | 6 ++++-- src/testflockfev.bzz | 2 +- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/include/roscontroller.h b/include/roscontroller.h index d9aa948..738371a 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -92,7 +92,6 @@ private: ros::Publisher payload_pub; ros::Publisher neigh_pos_pub; ros::Publisher localsetpoint_pub; - ros::Publisher localsetpoint_nonraw_pub; ros::ServiceServer service; ros::Subscriber current_position_sub; ros::Subscriber battery_sub; diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 5a13086..47473d1 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -42,6 +42,7 @@ namespace rosbzz_node{ void roscontroller::GetRobotId() { + mavros_msgs::ParamGet::Request robot_id_srv_request; robot_id_srv_request.param_id="id"; mavros_msgs::ParamGet::Response robot_id_srv_response; while(!xbeestatus_srv.call(robot_id_srv_request,robot_id_srv_response)){ @@ -50,6 +51,8 @@ namespace rosbzz_node{ } robot_id=robot_id_srv_response.value.integer; + + //robot_id = 8; } /*------------------------------------------------- @@ -793,8 +796,7 @@ namespace rosbzz_node{ moveMsg.pose.orientation.z = 0; moveMsg.pose.orientation.w = 1; - - localsetpoint_nonraw_pub.publish(moveMsg); + localsetpoint_pub.publish(moveMsg); ROS_INFO("Sent local NON RAW position message!"); } diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 7b1df3f..30e5b80 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -50,7 +50,7 @@ function hexagon() { statef=land } else { timeW = timeW+1 - uav_moveto(0.0,0.35) + uav_moveto(0.0,0.0) } }