ensure compatibility with M100

include/roscontroller.h

@@ -194,6 +194,7 @@ private:
 	void SetStreamRate(int id, int rate, int on_off);
 	
 	void get_number_of_robots();
+	void GetRobotId();
 };
 
 }

launch/launch_config/m100.yaml

@@ -5,7 +5,11 @@ topics:
   fcclient :   /dji_mavcmd
   armclient:   /dji_mavarm
   modeclient:  /dji_mavmode
+  altitude: /rel_alt
+  stream:  /set_stream_rate
+
 type:
   gps  :  sensor_msgs/NavSatFix
   battery :   mavros_msgs/BatteryStatus
   status  :   mavros_msgs/ExtendedState
+  altitude: std_msgs/Float64

src/roscontroller.cpp

@@ -20,8 +20,10 @@ namespace rosbzz_node{
 		SetMode("LOITER", 0);
 		armstate = 0;
 		multi_msg = true;
-		// set stream rate
+		// set stream rate - wait for the FC to be started
 		SetStreamRate(0, 10, 1);
+		/// Get Robot Id - wait for Xbee to be started
+		GetRobotId();
 	}
 
 	/*---------------------
@@ -36,26 +38,25 @@ namespace rosbzz_node{
    		uav_done();
 	}
 
-	/*-------------------------------------------------
-	/rosbuzz_node loop method executed once every step
-	/--------------------------------------------------*/
-	void roscontroller::RosControllerRun(){
+	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;
-
-        /*
+        	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);
-			loop_rate.sleep();
+			ros::Duration(0.1).sleep();
 			ROS_ERROR("Waiting for Xbee to respond to get device ID");
 		}
 
 		robot_id=robot_id_srv_response.value.integer;
-		*/
-        robot_id = 32;
 
-        //robot_id = 84;
+	}
+
+	/*-------------------------------------------------
+	/rosbuzz_node loop method executed once every step
+	/--------------------------------------------------*/
+	void roscontroller::RosControllerRun(){
+
 		/* Set the Buzz bytecode */
 		if(buzz_utility::buzz_script_set(bcfname.c_str(), dbgfname.c_str(),robot_id)) {
 			fprintf(stdout, "Bytecode file found and set\n");
@@ -69,7 +70,7 @@ namespace rosbzz_node{
 				/*Check updater state and step code*/
   				update_routine(bcfname.c_str(), dbgfname.c_str());
 				/*Step buzz script */
-      			buzz_utility::buzz_script_step();
+      				buzz_utility::buzz_script_step();
 				/*Prepare messages and publish them in respective topic*/
 		  		prepare_msg_and_publish();
 				/*call flight controler service to set command long*/
@@ -380,7 +381,6 @@ 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();
@@ -401,7 +401,7 @@ namespace rosbzz_node{
 					SetMode("LOITER", 0);
 					armstate = 1;
 					Arm();
-					loop_rate.sleep();
+					ros::Duration(0.1).sleep();
 				}
 				if(current_mode != "GUIDED")
 					SetMode("GUIDED", 2000); // for real solo, just add 2000ms delay (it should always be in loiter after arm/disarm)
@@ -526,9 +526,9 @@ namespace rosbzz_node{
 		// TODO: Handle landing
 		std::cout << "Message: " << msg->mode << std::endl;
 		if(msg->mode == "GUIDED")
-			buzzuav_closures::flight_status_update(1);
+			buzzuav_closures::flight_status_update(2);
 		else if (msg->mode == "LAND")
-			buzzuav_closures::flight_status_update(4);
+			buzzuav_closures::flight_status_update(1);
 		else // ground standby = LOITER?
 			buzzuav_closures::flight_status_update(7);//?
 	}
@@ -543,7 +543,7 @@ namespace rosbzz_node{
 	/ Update current position into BVM from subscriber
 	/-------------------------------------------------------------*/
 	void roscontroller::current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg){
-		ROS_INFO("Altitude out: %f", cur_rel_altitude);
+		//ROS_INFO("Altitude out: %f", cur_rel_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);
 	}
@@ -551,7 +551,7 @@ namespace rosbzz_node{
 	/ Update altitude into BVM from subscriber
 	/-------------------------------------------------------------*/
 	void roscontroller::current_rel_alt(const std_msgs::Float64::ConstPtr& msg){
-		ROS_INFO("Altitude in: %f", msg->data);
+		//ROS_INFO("Altitude in: %f", msg->data);
 		cur_rel_altitude = (double)msg->data;
 
 	}
@@ -811,7 +811,7 @@ namespace rosbzz_node{
 
 		while(!stream_client.call(message)){
 			ROS_INFO("Set stream rate call failed!, trying again...");
-			std::this_thread::sleep_for( std::chrono::milliseconds ( 2000 ) );
+			ros::Duration(0.1).sleep();
 		}
 		ROS_INFO("Set stream rate call successful");
 	}

src/testflockfev.bzz

@@ -43,14 +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)
 
-  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.5,0.0)
+#  	}
 }
  
 ########################################
@@ -115,7 +116,7 @@ function takeoff() {
 	log("TakeOff: ", flight.status)
 	log("Relative position: ", position.altitude)
 	
-	if( flight.status == 1 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS,hexagon)
 		barrier_ready()
 		#statef=hexagon
@@ -130,7 +131,7 @@ function land() {
 	CURSTATE = "LAND"
 	statef=land
 	log("Land: ", flight.status)
-	if(flight.status == 2 or flight.status == 1){
+	if(flight.status == 2 or flight.status == 3){
 		neighbors.broadcast("cmd", 21)
 		uav_land()
 	}
@@ -200,4 +201,4 @@ function reset() {
  
 # Executed once at the end of experiment.
 function destroy() {
-}
+}