From ea95bb1b484400102c37a72f0995d279dd30e8ea Mon Sep 17 00:00:00 2001
From: David <david.st-onge@polymtl.ca>
Date: Fri, 17 Feb 2017 22:00:50 -0500
Subject: [PATCH] added arm/mode dummies

---
 include/roscontroller.h        |  3 +--
 launch/launch_config/m100.yaml |  2 ++
 launch/launch_config/solo.yaml |  2 ++
 src/roscontroller.cpp          | 19 ++++++++-----------
 4 files changed, 13 insertions(+), 13 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index df706d4..2892094 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -3,7 +3,6 @@
 #include <sensor_msgs/NavSatFix.h>
 #include "mavros_msgs/GlobalPositionTarget.h"
 #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"
@@ -54,7 +53,7 @@ private:
         //int oldcmdID=0;
 	int rc_cmd;
 	int barrier;
-	std::string bzzfile_name, fcclient_name, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by; //, rcclient;
+	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
 	bool rcclient;
 	bool multi_msg;
 	ros::ServiceClient mav_client;
diff --git a/launch/launch_config/m100.yaml b/launch/launch_config/m100.yaml
index fc24c48..40dfb42 100644
--- a/launch/launch_config/m100.yaml
+++ b/launch/launch_config/m100.yaml
@@ -3,6 +3,8 @@ topics:
   battery  :   /power_status
   status   :   /flight_status
   fcclient :   /dji_mavcmd
+  armclient:   /dji_mavarming
+  modeclient:  /dji_mavmode
 type:
   gps  :  sensor_msgs/NavSatFix
   battery :   mavros_msgs/BatteryStatus
diff --git a/launch/launch_config/solo.yaml b/launch/launch_config/solo.yaml
index 508b7a1..7d3ceb4 100644
--- a/launch/launch_config/solo.yaml
+++ b/launch/launch_config/solo.yaml
@@ -3,6 +3,8 @@ topics:
   battery :   /mavros/battery
   status  :   /mavros/state
   fcclient:   /mavros/cmd/command
+  armclient:  /mavros/cmd/arming
+  modeclient: /mavros/set_mode
 type:
   gps  :  sensor_msgs/NavSatFix
   # for SITL Solo 
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 7a81935..19234f3 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -163,6 +163,10 @@ namespace rosbzz_node{
   		/*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");}  
+  		if(node_handle.getParam("topics/armclient", armclient));
+  		else {ROS_ERROR("Provide an arm client name in Launch file"); system("rosnode kill rosbuzz_node");}  
+  		if(node_handle.getParam("topics/modeclient", modeclient));
+  		else {ROS_ERROR("Provide a mode client name in Launch file"); system("rosnode kill rosbuzz_node");}  
 	}
 
 	void roscontroller::Initialize_pub_sub(ros::NodeHandle n_c){
@@ -179,17 +183,10 @@ namespace rosbzz_node{
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
 		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
-		/* Clients*/
-
-		//if(fcclient_name == "/mavros/cmd/command"){
-		//	mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
-		//}
-		//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);
-		//}
+		/* Service Clients*/
+		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armingclient);
+		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
+		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
 
 		multi_msg=true;
 	}