diff --git a/include/roscontroller.h b/include/roscontroller.h index 8f85965..87bd038 100644 --- a/include/roscontroller.h +++ b/include/roscontroller.h @@ -18,6 +18,7 @@ #include "mavros_msgs/PositionTarget.h" #include "mavros_msgs/StreamRate.h" #include "mavros_msgs/ParamGet.h" +#include "geometry_msgs/PoseStamped.h" #include "std_msgs/Float64.h" #include #include @@ -79,6 +80,7 @@ private: std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name, setpoint_name; std::string stream_client_name; std::string relative_altitude_sub_name; + std::string setpoint_nonraw; bool rcclient; bool multi_msg; Num_robot_count count_robots; @@ -87,6 +89,7 @@ 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; @@ -97,6 +100,9 @@ private: ros::Subscriber relative_altitude_sub; ros::ServiceClient stream_client; + int setpoint_counter; + double my_x = 0, my_y = 0; + /*Commands for flight controller*/ //mavros_msgs::CommandInt cmd_srv; mavros_msgs::CommandLong cmd_srv; @@ -191,6 +197,8 @@ private: void SetLocalPosition(float x, float y, float z, float yaw); + void SetLocalPositionNonRaw(float x, float y, float z, float yaw); + void SetStreamRate(int id, int rate, int on_off); void get_number_of_robots(); diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml index a7e2e20..650ba45 100644 --- a/launch/launch_config/solo.yaml +++ b/launch/launch_config/solo.yaml @@ -4,6 +4,7 @@ topics: status : /mavros/state fcclient: /mavros/cmd/command setpoint: /mavros/setpoint_raw/local + setpoint_nonraw: /mavros/setpoint_position/local armclient: /mavros/cmd/arming modeclient: /mavros/set_mode stream: /mavros/set_stream_rate diff --git a/launch/rosbuzz-testing-sitl.launch b/launch/rosbuzz-testing-sitl.launch index a6e6405..141f0a0 100644 --- a/launch/rosbuzz-testing-sitl.launch +++ b/launch/rosbuzz-testing-sitl.launch @@ -16,13 +16,14 @@ --> - + - ---> + + diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp index 39819d3..d029c5e 100644 --- a/src/roscontroller.cpp +++ b/src/roscontroller.cpp @@ -24,6 +24,7 @@ namespace rosbzz_node{ SetStreamRate(0, 10, 1); /// Get Robot Id - wait for Xbee to be started GetRobotId(); + setpoint_counter = 0; } /*--------------------- @@ -40,7 +41,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)){ @@ -49,7 +50,8 @@ namespace rosbzz_node{ } robot_id=robot_id_srv_response.value.integer; - + */ + robot_id = 8; } /*------------------------------------------------- @@ -178,6 +180,8 @@ namespace rosbzz_node{ else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} if(node_handle.getParam("topics/stream", stream_client_name)); else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");} + if(node_handle.getParam("topics/setpoint_nonraw", setpoint_nonraw)); + else {ROS_ERROR("Provide a mode setpoint non raw client in Launch file"); system("rosnode kill rosbuzz_node");} } /*-------------------------------------------------------- @@ -198,6 +202,7 @@ namespace rosbzz_node{ payload_pub = n_c.advertise(out_payload, 1000); neigh_pos_pub = n_c.advertise("/neighbours_pos",1000); localsetpoint_pub = n_c.advertise(setpoint_name,1000); + localsetpoint_nonraw_pub = n_c.advertise(setpoint_nonraw,1000); /* Service Clients*/ arm_client = n_c.serviceClient(armclient); mode_client = n_c.serviceClient(modeclient); @@ -403,7 +408,7 @@ namespace rosbzz_node{ SetMode("LOITER", 0); armstate = 1; Arm(); - ros::Duration(0.1).sleep(); + ros::Duration(0.5).sleep(); } if(current_mode != "GUIDED") SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm) @@ -443,7 +448,9 @@ namespace rosbzz_node{ } } 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[1], goto_pos[0], 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); } } /*---------------------------------------------- @@ -773,14 +780,17 @@ namespace rosbzz_node{ moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_OFFSET_NED; moveMsg.type_mask = mavros_msgs::PositionTarget::IGNORE_AFX | mavros_msgs::PositionTarget::IGNORE_AFY | - mavros_msgs::PositionTarget::IGNORE_AFZ | - mavros_msgs::PositionTarget::IGNORE_VX | - mavros_msgs::PositionTarget::IGNORE_VY | - mavros_msgs::PositionTarget::IGNORE_VZ; + mavros_msgs::PositionTarget::IGNORE_AFZ; + // alt: 0b0000111111111000 + moveMsg.header.frame_id = 1; + moveMsg.header.seq = setpoint_counter++; moveMsg.header.stamp = ros::Time::now(); - moveMsg.position.x = x; - moveMsg.position.y = y; - moveMsg.position.z = z; + moveMsg.position.x = 0.5; + moveMsg.position.y = 0.0; + moveMsg.position.z = 2; + moveMsg.velocity.x = 0.5; + moveMsg.velocity.y = 0.0; + moveMsg.velocity.z = 0.5; moveMsg.yaw = 0; localsetpoint_pub.publish(moveMsg); @@ -788,6 +798,33 @@ namespace rosbzz_node{ ROS_INFO("Sent local position message!"); } + void roscontroller::SetLocalPositionNonRaw(float x, float y, float z, float yaw){ + // http://docs.ros.org/kinetic/api/mavros_msgs/html/msg/PositionTarget.html + // http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned + + geometry_msgs::PoseStamped moveMsg; + moveMsg.header.stamp = ros::Time::now(); + moveMsg.header.seq = setpoint_counter++; + moveMsg.header.frame_id = 1; + + my_x += x; + my_y += y; + + moveMsg.pose.position.x = my_x; + moveMsg.pose.position.y = my_y; + moveMsg.pose.position.z = 3; + + moveMsg.pose.orientation.x = 0; + moveMsg.pose.orientation.y = 0; + moveMsg.pose.orientation.z = 0; + moveMsg.pose.orientation.w = 1; + + + localsetpoint_nonraw_pub.publish(moveMsg); + + ROS_INFO("Sent local NON RAW position message!"); + } + void roscontroller::SetMode(std::string mode, int delay_miliseconds){ // wait if necessary if (delay_miliseconds != 0){ diff --git a/src/testflockfev.bzz b/src/testflockfev.bzz index 9546df2..8ef9a02 100644 --- a/src/testflockfev.bzz +++ b/src/testflockfev.bzz @@ -43,15 +43,15 @@ function hexagon() { math.vec2.scale(accum, 1.0 / neighbors.count()) # Move according to vector #print("Robot ", id, "must push ",accum.length, "; ", accum.angle) - uav_moveto(accum.x,accum.y) +# uav_moveto(accum.x,accum.y) -# if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS -# timeW =0 -# statef=land -# } else { -# timeW = timeW+1 -# uav_moveto(0.5,0.0) -# } + if(timeW>=WAIT_TIMEOUT) { #FOR MOVETO TESTS + timeW =0 + statef=land + } else { + timeW = timeW+1 + uav_moveto(0.0,0.02) + } } ########################################