changes to NED and REL_ALT

include/roscontroller.h

@@ -18,6 +18,7 @@
 #include "mavros_msgs/PositionTarget.h"
 #include "mavros_msgs/StreamRate.h"
 #include "mavros_msgs/ParamGet.h"
+#include "std_msgs/Float64.h"
 #include <sensor_msgs/LaserScan.h>
 #include <rosbuzz/neigh_pos.h>
 #include <sstream>
@@ -59,6 +60,7 @@ private:
 	}; typedef struct num_robot_count Num_robot_count ;
 
  	double cur_pos[3];
+ 	double cur_rel_altitude;
 	uint64_t payload;
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
 	std::map< int,  buzz_utility::Pos_struct> raw_neighbours_pos_map;
@@ -76,6 +78,7 @@ private:
 	int old_val=0;	
 	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
 	std::string stream_client_name;
+	std::string relative_altitude_sub_name;
 	bool rcclient;
 	bool multi_msg;
 	Num_robot_count count_robots;
@@ -91,6 +94,7 @@ private:
 	ros::Subscriber flight_status_sub;
 	ros::Subscriber obstacle_sub;
 	ros::Subscriber Robot_id_sub;
+	ros::Subscriber relative_altitude_sub;
 	ros::ServiceClient stream_client;
 
 	/*Commands for flight controller*/
@@ -154,6 +158,9 @@ private:
 	/*current position callback*/
 	void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg);
 
+	/*current relative altitude callback*/
+	void current_rel_alt(const std_msgs::Float64::ConstPtr& msg);
+
 	/*payload callback callback*/
 	void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
 

launch/launch_config/solo.yaml

@@ -6,6 +6,7 @@ topics:
   armclient:  /mavros/cmd/arming
   modeclient: /mavros/set_mode
   stream:  /mavros/set_stream_rate
+  altitude: /mavros/global_position/rel_alt
 type:
   gps  :  sensor_msgs/NavSatFix
   # for SITL Solo 
@@ -13,5 +14,6 @@ type:
   # for solo
   #battery :   mavros_msgs/BatteryStatus
   status  :   mavros_msgs/State
+  altitude: std_msgs/Float64
 environment : 
   environment : solo-simulator

launch/rosbuzz.launch

@@ -3,13 +3,12 @@
 <launch>
 		<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"/>
-		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testalone.bzz" />
+		<param name="bzzfile_name" value="/home/ivan/catkin_ws/src/rosbuzz/src/testmoveto.bzz" />
         <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="xbee_status_srv" value="/xbee_status"/>
 		<param name="stand_by" value="$(env ROS_WS)/src/ROSBuzz/src/stand_by.bo"/>
-		</node>
+		</node>		
-		
 </launch>

src/roscontroller.cpp

@@ -43,7 +43,7 @@ namespace rosbzz_node{
 		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)){
     			ros::spinOnce();
 			ros::Rate loop_rate(10);
@@ -52,6 +52,8 @@ namespace rosbzz_node{
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
+		*/
+        robot_id = 32;
 
         //robot_id = 84;
 		/* Set the Buzz bytecode */
@@ -159,6 +161,11 @@ namespace rosbzz_node{
 		node_handle.getParam("type/status", status_type);
 		m_smTopic_infos.insert(pair <std::string, std::string>(status_topic, status_type));
 
+		std::string altitude_topic, altitude_type;
+		node_handle.getParam("topics/altitude", altitude_topic);
+		node_handle.getParam("type/altitude", altitude_type);
+		m_smTopic_infos.insert(pair <std::string, std::string>(altitude_topic, altitude_type));
+
   		/*Obtain fc client name from parameter server*/
   		if(node_handle.getParam("topics/fcclient", fcclient_name));
   		else {ROS_ERROR("Provide a fc client name in Launch file"); system("rosnode kill rosbuzz_node");}  
@@ -215,6 +222,9 @@ namespace rosbzz_node{
   			else if(it->second == "sensor_msgs/NavSatFix"){
   		  		current_position_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_pos, this);
  			}
+  			else if(it->second == "std_msgs/Float64"){
+  				relative_altitude_sub = n_c.subscribe(it->first, 1000, &roscontroller::current_rel_alt, this);
+  			}
 
 	  		std::cout << "Subscribed to: " << it->first << endl;
   		}
@@ -370,6 +380,7 @@ namespace rosbzz_node{
 		/*FC call for takeoff,land, gohome and arm/disarm*/
 		int tmp = buzzuav_closures::bzz_cmd();
 		double* goto_pos = buzzuav_closures::getgoto();
+		ros::Rate loop_rate(0.25);
 		if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME) {
 			cmd_srv.request.param7 = goto_pos[2];
 			cmd_srv.request.command =  buzzuav_closures::getcmd();
@@ -378,19 +389,29 @@ namespace rosbzz_node{
 			switch(buzzuav_closures::getcmd()){
 			case mavros_msgs::CommandCode::NAV_LAND:
 				if(current_mode != "LAND")
-					SetMode("LAND", 0);
+					{SetMode("LAND", 0);}
+				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");}
+				armstate = 0;
 				break;
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
+				if(!armstate){
+					SetMode("LOITER", 0);
+					armstate = 1;
+					Arm();
+					loop_rate.sleep();
+				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
+				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;
 			}
 
-			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");
-
 		} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/
 			/*prepare the goto publish message */
 			cmd_srv.request.param5 = goto_pos[0];
@@ -407,16 +428,20 @@ namespace rosbzz_node{
 			} else
 				ROS_ERROR("Failed to call service from flight controller");
 		} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
-			SetMode("LOITER", 0);
+			if(!armstate){
-			armstate = 1;
+				SetMode("LOITER", 0);
-			Arm();
+				armstate = 1;
+				Arm();
+			}
 		} else if (tmp == buzzuav_closures::COMMAND_DISARM) {
-			armstate = 0;
+			if(armstate){
-		SetMode("LOITER", 0);
+				armstate = 0;
-			Arm();
+				SetMode("LOITER", 0);
+				Arm();
+			}
 		} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
-			/*prepare the goto publish message */
+			/*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);
 		}
 	}
 	/*----------------------------------------------
@@ -460,7 +485,6 @@ namespace rosbzz_node{
 		cur_pos [2] =altitude;
 	}
 
-
 	/*-----------------------------------------------------------
 	/ Compute Range and Bearing of a neighbor in a local reference frame
 	/ from GPS coordinates
@@ -519,8 +543,17 @@ namespace rosbzz_node{
 	/ Update current position into BVM from subscriber
 	/-------------------------------------------------------------*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
-		set_cur_pos(msg->latitude,msg->longitude, msg->altitude);
+		ROS_INFO("Altitude out: %f", cur_rel_altitude);
-		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude,msg->altitude);
+		set_cur_pos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
+		buzzuav_closures::set_currentpos(msg->latitude,msg->longitude, cur_rel_altitude);//msg->altitude);
+	}
+	/*-------------------------------------------------------------
+	/ Update altitude into BVM from subscriber
+	/-------------------------------------------------------------*/
+	void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){
+		ROS_INFO("Altitude in: %f", msg->data);
+		cur_rel_altitude = (double)msg->data;
+
 	}
 	/*-------------------------------------------------------------
 	/Set obstacle Obstacle distance table into BVM from subscriber
@@ -735,7 +768,7 @@ namespace rosbzz_node{
 		// http://ardupilot.org/dev/docs/copter-commands-in-guided-mode.html#copter-commands-in-guided-mode-set-position-target-local-ned
 
 		mavros_msgs::PositionTarget moveMsg;
-		moveMsg.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_OFFSET_NED;
+		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 |

src/testflockfev.bzz

@@ -11,7 +11,7 @@ function updated_neigh(){
 neighbors.broadcast(updated, update_no)
 }
 
-TARGET_ALTITUDE = 5.0
+TARGET_ALTITUDE = 3.0
 CURSTATE = "TURNEDOFF"
  
 # Lennard-Jones parameters
@@ -43,7 +43,14 @@ 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)
+
+  if(timeW>=WAIT_TIMEOUT) {	#FOR MOVETO TESTS
+  	timeW =0
+  	statef=land
+  	} else {
+  	timeW = timeW+1
+  	uav_moveto(0.5,0.0)
+  	}
 }
  
 ########################################
@@ -106,16 +113,17 @@ function takeoff() {
 	CURSTATE = "TAKEOFF"
 	statef=takeoff
 	log("TakeOff: ", flight.status)
-	uav_takeoff(TARGET_ALTITUDE)
+	log("Relative position: ", position.altitude)
 	
-	if( flight.status == 1) {
+	if( flight.status == 1 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 		#statef=hexagon
 	}
-	else if( flight.status == 7 ){
+	else {
 		log("Altitude: ", TARGET_ALTITUDE)
 		neighbors.broadcast("cmd", 22)
+		uav_takeoff(TARGET_ALTITUDE)
 	}
 }
 function land() {