Merge branch 'solo-playground' of https://github.com/MISTLab/ROSBuzz into solo-playground

include/roscontroller.h

@@ -5,7 +5,9 @@
 #include "mavros_msgs/CommandCode.h"
 #include "mavros_msgs/CommandInt.h"
 #include "mavros_msgs/CommandLong.h"
+#include "mavros_msgs/CommandBool.h"
 #include "mavros_msgs/ExtendedState.h"
+#include "mavros_msgs/SetMode.h"
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
@@ -71,6 +73,11 @@ private:
   	std::vector<std::string> m_sMySubscriptions;
   	std::map<std::string, std::string> m_smTopic_infos;
 
+  	mavros_msgs::CommandBool m_cmdBool;
+  	ros::ServiceClient arm_client;
+
+  	mavros_msgs::SetMode m_cmdSetMode;
+  	ros::ServiceClient mode_client;
 
 	void Initialize_pub_sub(ros::NodeHandle n_c);
 
@@ -134,6 +141,10 @@ private:
 
 	void GetSubscriptionParameters(ros::NodeHandle node_handle);
 
+	void Arm();
+
+	void SetMode();
+
 	void Subscribe(ros::NodeHandle n_c);
 };
 

src/buzzuav_closures.cpp

@@ -157,7 +157,7 @@ int buzzuav_moveto(buzzvm_t vm) {
 	}
    }*/
    cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
-   printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
+   printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
    buzz_cmd=2;
    return buzzvm_ret0(vm);
 }
@@ -215,25 +215,25 @@ int buzzuav_takeoff(buzzvm_t vm) {
    buzzvm_lnum_assert(vm, 1);
    buzzvm_lload(vm, 1); /* Altitude */
    buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
-   goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value;
+   goto_pos[2] = buzzvm_stack_at(vm, 1) -> f.value;
    height = goto_pos[2];
    cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
    printf(" Buzz requested Take off !!! \n");
-   buzz_cmd=1;
+   buzz_cmd = 1;
    return buzzvm_ret0(vm);
 }
 
 int buzzuav_land(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
    printf(" Buzz requested Land !!! \n");
-   buzz_cmd=1;
+   buzz_cmd = 1;
    return buzzvm_ret0(vm);
 }
 
 int buzzuav_gohome(buzzvm_t vm) {
    cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
    printf(" Buzz requested gohome !!! \n");
-   buzz_cmd=1;
+   buzz_cmd = 1;
    return buzzvm_ret0(vm);
 }
 

src/roscontroller.cpp

@@ -186,6 +186,8 @@ namespace rosbzz_node{
 		//}
 		//else {
 			//TODO: Here
+		    arm_client = n_c.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
+		    mode_client =  n_c.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
 			mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
 		//}
 
@@ -235,6 +237,29 @@ namespace rosbzz_node{
 		dbgfname = bzzfile_in_compile.str();
    		
 	}
+
+	void roscontroller::Arm(){
+		mavros_msgs::CommandBool arming_message;
+		arming_message.request.value = true;
+		if(arm_client.call(arming_message)) {
+			ROS_INFO("Service called!");
+		} else {
+			ROS_INFO("Service call failed!");
+		}
+	}
+
+	void roscontroller::SetMode(){
+		mavros_msgs::SetMode set_mode_message;
+		set_mode_message.request.base_mode = 0;
+		set_mode_message.request.custom_mode = "GUIDED"; // shit!
+		if(mode_client.call(set_mode_message)) {
+			ROS_INFO("Service called!");
+		} else {
+			ROS_INFO("Service call failed!");
+		}
+	}
+
+
 	/*Prepare messages for each step and publish*/
 	/*******************************************************************************************************/
 	/* Message format of payload (Each slot is uint64_t)						       */
@@ -552,7 +577,7 @@ namespace rosbzz_node{
 		else if (msg->mode == "LAND")
 			buzzuav_closures::flight_status_update(4);
 		else // ground standby = LOITER?
-			buzzuav_closures::flight_status_update(5);//?
+			buzzuav_closures::flight_status_update(1);//?
 	}
 
 	/*flight extended status update*/
@@ -668,6 +693,9 @@ namespace rosbzz_node{
 			case mavros_msgs::CommandCode::NAV_TAKEOFF:
    				ROS_INFO("RC_call: TAKE OFF!!!!");
 				rc_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
+				/* arming */
+				SetMode();
+				Arm();
 				buzzuav_closures::rc_call(rc_cmd);
 				res.success = true;
 				break;

src/test1.bzz

@@ -106,25 +106,23 @@ if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/
 }
 
 function takeoff() {
-	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
-		barrier_set(ROBOTS,hexagon)
-		barrier_ready()
-	}
-	else if( flight.status !=3){
+	#if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
+	#	barrier_set(ROBOTS,hexagon)
+	#	barrier_ready()
+	#}
+	if( flight.status !=3){
+		log("Altitude: ", TARGET_ALTITUDE)
 		neighbors.broadcast("cmd", 22)
 		uav_takeoff(TARGET_ALTITUDE)
 	}
 }
 function land() {
-	log("Land: ", flight.status)
-	if( flight.status == 1) {
-		barrier_set(ROBOTS,idle)
-		barrier_ready()
-	}
-	else if(flight.status!=0 and flight.status!=4){
-		neighbors.broadcast("cmd", 21)
-		uav_land()
-	}
+	#log("Land: ", flight.status)
+	#if(flight.status != 0 and flight.status != 4){
+	#	neighbors.broadcast("cmd", 21)
+	#	uav_land()
+	#}
+	uav_land()
 }
 
 # Executed once at init time.
@@ -135,19 +133,22 @@ function init() {
 # Executed at each time step.
 function step() {
 
-	if(flight.rc_cmd==22 and flight.status==1) {
+	if(flight.rc_cmd==22) {
+		log("cmd 22")
 		flight.rc_cmd=0
-		statef=takeoff		
+		statef = takeoff		
 		neighbors.broadcast("cmd", 22)
-	} else if(flight.rc_cmd==21 and flight.status==2) {
+	} else if(flight.rc_cmd==21) {
+		log("cmd 21")
+		log("To land")
 		flight.rc_cmd=0
-		statef=land
+		statef = land
 		neighbors.broadcast("cmd", 21)
-	} else if(flight.rc_cmd==16 and flight.status==2) {
+	} else if(flight.rc_cmd==16) {
 		flight.rc_cmd=0
 		uav_goto()
-	}
-statef()
+	} 
+	statef()
 }
  
 # Executed once when the robot (or the simulator) is reset.