Changed message to CommandLong, works! Todo: landing

include/roscontroller.h

@@ -4,6 +4,7 @@
 #include "mavros_msgs/GlobalPositionTarget.h"
 #include "mavros_msgs/CommandCode.h"
 #include "mavros_msgs/CommandInt.h"
+#include "mavros_msgs/CommandLong.h"
 #include "mavros_msgs/ExtendedState.h"
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
@@ -64,7 +65,8 @@ private:
 	ros::Subscriber flight_status_sub;
 	ros::Subscriber obstacle_sub;
 	/*Commands for flight controller*/
-  	mavros_msgs::CommandInt cmd_srv;	
+  	//mavros_msgs::CommandInt cmd_srv;
+  	mavros_msgs::CommandLong cmd_srv;
 
   	std::vector<std::string> m_sMySubscriptions;
   	std::map<std::string, std::string> m_smTopic_infos;
@@ -126,8 +128,7 @@ private:
 	void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg);
 
 	/* RC commands service */
-	bool rc_callback(mavros_msgs::CommandInt::Request  &req,
+	bool rc_callback(mavros_msgs::CommandLong::Request  &req, mavros_msgs::CommandLong::Response &res);
-		         mavros_msgs::CommandInt::Response &res);
 
 	void obstacle_dist(const sensor_msgs::LaserScan::ConstPtr& msg);
 

launch/launch_config/solo.yaml

@@ -9,4 +9,5 @@ type:
   # for solo
   #battery :   mavros_msgs/BatteryStatus
   status  :   mavros_msgs/State
-  
+environment : 
+  environment : solo-simulator

launch/rosbuzz.launch

@@ -7,7 +7,7 @@
         <param name="rcclient" value="true" /> 
 		<param name="rcservice_name" value="/buzzcmd" />
 		<!-- todo: verify if this will work, or use "command" -->
-		<param name="fcclient_name" value="/mavros/cmd/command_int" />
+		<param name="fcclient_name" value="/mavros/cmd/command" />
 		<param name="in_payload" value="/inMavlink"/>
 		<param name="out_payload" value="/outMavlink"/>
 		<param name="robot_id" value="3"/>

src/buzzuav_closures.cpp

@@ -171,40 +171,40 @@ int buzzuav_goto(buzzvm_t vm) {
 }
 /******************************/
 
-double* getgoto(){
+double* getgoto() {
-return goto_pos;
+	return goto_pos;
 }
 /******************************/
-int getcmd(){
+int getcmd() {
- return cur_cmd;
+	return cur_cmd;
 }
 
-void set_goto(double pos[]){
+void set_goto(double pos[]) {
-goto_pos[0]=pos[0];
+	goto_pos[0] = pos[0];
-goto_pos[1]=pos[1];
+	goto_pos[1] = pos[1];
-goto_pos[2]=pos[2];
+	goto_pos[2] = pos[2];
 
 }
 
-int bzz_cmd(){
+int bzz_cmd() {
-int cmd = buzz_cmd;
+	int cmd = buzz_cmd;
-buzz_cmd=0;
+	buzz_cmd = 0;
-return cmd;
+	return cmd;
 }
 
-void rc_set_goto(double pos[]){
+void rc_set_goto(double pos[]) {
-rc_goto_pos[0]=pos[0];
+	rc_goto_pos[0] = pos[0];
-rc_goto_pos[1]=pos[1];
+	rc_goto_pos[1] = pos[1];
-rc_goto_pos[2]=pos[2];
+	rc_goto_pos[2] = pos[2];
 
 }
-void rc_call(int rc_cmd_in){
+void rc_call(int rc_cmd_in) {
-rc_cmd=rc_cmd_in;
+	rc_cmd = rc_cmd_in;
 }
 
-void set_obstacle_dist(float dist[]){
+void set_obstacle_dist(float dist[]) {
-	for(int i=0; i<5;i++)
+	for (int i = 0; i < 5; i++)
-		obst[i]=dist[i];	
+		obst[i] = dist[i];
 }
 
 

src/roscontroller.cpp

@@ -179,7 +179,15 @@ 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);	
 		/* Clients*/
-  		mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
+
+		//if(fcclient_name == "/mavros/cmd/command"){
+		//	mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
+		//}
+		//else {
+			//TODO: Here
+			mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
+		//}
+
 		multi_msg=true;
 	}
 	
@@ -241,15 +249,16 @@ namespace rosbzz_node{
 		int tmp = buzzuav_closures::bzz_cmd();
     		double* goto_pos = buzzuav_closures::getgoto();
 		if (tmp == 1){
-    			cmd_srv.request.z = goto_pos[2];
+			cmd_srv.request.param7 = goto_pos[2];
+			//cmd_srv.request.z = goto_pos[2];
 			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
 			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 == 2) { /*FC call for goto*/ 
 			/*prepare the goto publish message */
-			cmd_srv.request.x = goto_pos[0]*pow(10,7);
+			cmd_srv.request.param5 = goto_pos[0]*pow(10,7);
-    			cmd_srv.request.y = goto_pos[1]*pow(10,7);
+    			cmd_srv.request.param6 = goto_pos[1]*pow(10,7);
-    			cmd_srv.request.z = goto_pos[2];
+    			cmd_srv.request.param7 = goto_pos[2];
     			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
 			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"); 
@@ -535,6 +544,7 @@ namespace rosbzz_node{
 	/*flight extended status update*/
 	void roscontroller::flight_status_update(const mavros_msgs::State::ConstPtr& msg){
 		// http://wiki.ros.org/mavros/CustomModes
+		// TODO: Handle landing
 		std::cout << "Message: " << msg->mode << std::endl;
 		if(msg->mode == "GUIDED")
 			buzzuav_closures::flight_status_update(1);
@@ -647,8 +657,8 @@ namespace rosbzz_node{
 	}
  	
 	/* RC command service */
-	bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request  &req,
+	bool roscontroller::rc_callback(mavros_msgs::CommandLong::Request  &req,
-		         mavros_msgs::CommandInt::Response &res){
+		         mavros_msgs::CommandLong::Response &res){
 		int rc_cmd;
 /*		if(req.command==oldcmdID)
 		return true;
@@ -675,9 +685,13 @@ namespace rosbzz_node{
 			case mavros_msgs::CommandCode::NAV_WAYPOINT:
    				ROS_INFO("RC_Call: GO TO!!!! ");
 				double rc_goto[3];
-   				rc_goto[0]=req.x/pow(10,7);
+   				//rc_goto[0]=req.x/pow(10,7);
-				rc_goto[1]=req.y/pow(10,7);
+				//rc_goto[1]=req.y/pow(10,7);
-				rc_goto[2]=req.z;
+				//rc_goto[2]=req.z;
+				rc_goto[0] = req.param5 / pow(10, 7);
+				rc_goto[1] = req.param6 / pow(10, 7);
+				rc_goto[2] = req.param7;
+
 				buzzuav_closures::rc_set_goto(rc_goto);
 				rc_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 				buzzuav_closures::rc_call(rc_cmd);

src/test1.bzz

@@ -116,6 +116,7 @@ function takeoff() {
 	}
 }
 function land() {
+	log("Land: ", flight.status)
 	if( flight.status == 1) {
 		barrier_set(ROBOTS,idle)
 		barrier_ready()