temp with setpoint_local - tested outside: working - TODO: handle local position home

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 <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
@@ -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();

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

launch/rosbuzz-testing-sitl.launch

@@ -16,13 +16,14 @@
 		<arg name="log_output" value="$(arg log_output)" />
 	</include>
 -->
-	<!-- run xbee 
+	<!-- run xbee -->
+	
 	<node pkg="xbee_ros_node" type="xbee_mav" name="xbee_mav" args="slave swarm" output="screen" />
 	<param name="Xbee_In_From_Buzz" type="str" value="outMavlink" />
 	<param name="Xbee_Out_To_Buzz" type="str" value="inMavlink" />
 	<param name="Xbee_In_From_Controller" type="str" value="xbee_cmd" />
-	<param name="No_of_dev" type="int" value="3" />
+	<param name="Xbee_Out_To_Controller" type="str" value="mav_dji_cmd" />
--->
+
 	<!-- run rosbuzz -->
 	<node name="rosbuzz_node" pkg="rosbuzz" type="rosbuzz_node" respawn="false" output="screen" >	
 		<rosparam file="/home/ivan/catkin_ws/src/rosbuzz/launch/launch_config/solo.yaml"/>

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<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);
 		localsetpoint_pub = n_c.advertise<mavros_msgs::PositionTarget>(setpoint_name,1000);
+		localsetpoint_nonraw_pub = n_c.advertise<geometry_msgs::PoseStamped>(setpoint_nonraw,1000);
 		/* Service Clients*/
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(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_AFZ;
-		                                     mavros_msgs::PositionTarget::IGNORE_VX  |
+		// alt: 0b0000111111111000
-		                                     mavros_msgs::PositionTarget::IGNORE_VY  |
+		moveMsg.header.frame_id = 1;
-		                                     mavros_msgs::PositionTarget::IGNORE_VZ;
+		moveMsg.header.seq = setpoint_counter++;
 		moveMsg.header.stamp = ros::Time::now();
-		moveMsg.position.x = x;
+		moveMsg.position.x = 0.5;
-		moveMsg.position.y = y;
+		moveMsg.position.y = 0.0;
-		moveMsg.position.z = z;
+		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){

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
+  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
-#  	timeW =0
+  	timeW =0
-#  	statef=land
+  	statef=land
-#  	} else {
+  	} else {
-#  	timeW = timeW+1
+  	timeW = timeW+1
-#  	uav_moveto(0.5,0.0)
+  	uav_moveto(0.0,0.02)
-#  	}
+  	}
 }
  
 ########################################