From 29b234cd9d9d3770e97f7bbab04097b3623e813d Mon Sep 17 00:00:00 2001
From: David St-Onge <david.st-onge@polymtl.ca>
Date: Sun, 2 Apr 2017 14:19:45 -0400
Subject: [PATCH] extend xbeestatus

---
 include/roscontroller.h  |  6 ++----
 src/buzzuav_closures.cpp | 18 ++++++++++--------
 src/roscontroller.cpp    |  8 +++++---
 3 files changed, 17 insertions(+), 15 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index f1ac04c..456d5eb 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -59,11 +59,11 @@ private:
 	int armstate;
 	int barrier;
 	int message_number=0;
-	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by;
+	std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,stand_by,xbeesrv_name;
 	bool rcclient;
 	bool multi_msg;
 	ros::ServiceClient mav_client;
-	ros::ServiceClient robot_id_client;
+	ros::ServiceClient xbeestatus_srv;
 	ros::Publisher payload_pub;
 	ros::Publisher neigh_pos_pub;
 	ros::Publisher localsetpoint_pub;
@@ -77,8 +77,6 @@ private:
 	/*Commands for flight controller*/
   	//mavros_msgs::CommandInt cmd_srv;
   	mavros_msgs::CommandLong cmd_srv;
-	mavros_msgs::ParamGet::Request robot_id_srv_request;
-	mavros_msgs::ParamGet::Response robot_id_srv_response;
   	std::vector<std::string> m_sMySubscriptions;
   	std::map<std::string, std::string> m_smTopic_infos;
 
diff --git a/src/buzzuav_closures.cpp b/src/buzzuav_closures.cpp
index 55343cc..a77c79d 100644
--- a/src/buzzuav_closures.cpp
+++ b/src/buzzuav_closures.cpp
@@ -29,40 +29,42 @@ namespace buzzuav_closures{
 
 	int buzzros_print(buzzvm_t vm) {
 	int i;
+	char buffer [50] = "";
 	   for(i = 1; i < buzzdarray_size(vm->lsyms->syms); ++i) {
 	      buzzvm_lload(vm, i);
 	      buzzobj_t o = buzzvm_stack_at(vm, 1);
 	      buzzvm_pop(vm);
 	      switch(o->o.type) {
 		 case BUZZTYPE_NIL:
-		    ROS_INFO("BUZZ - [nil]");
+		    sprintf(buffer,"%s BUZZ - [nil]", buffer);
 		    break;
 		 case BUZZTYPE_INT:
-		    ROS_INFO("%d", o->i.value);
+		    sprintf(buffer,"%s %d", buffer, o->i.value);
 		    //fprintf(stdout, "%d", o->i.value);
 		    break;
 		 case BUZZTYPE_FLOAT:
-		    ROS_INFO("%f", o->f.value);
+		    sprintf(buffer,"%s %f", buffer, o->f.value);
 		    break;
 		 case BUZZTYPE_TABLE:
-		    ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
+		    sprintf(buffer,"%s [table with %d elems]", buffer, (buzzdict_size(o->t.value)));
 		    break;
 		 case BUZZTYPE_CLOSURE:
 		    if(o->c.value.isnative)
-			ROS_INFO("[n-closure @%d]", o->c.value.ref);
+		    	sprintf(buffer,"%s [n-closure @%d]", buffer, o->c.value.ref);
 		    else
-			ROS_INFO("[c-closure @%d]", o->c.value.ref);
+		    	sprintf(buffer,"%s [c-closure @%d]", buffer, o->c.value.ref);
 		    break;
 		 case BUZZTYPE_STRING:
-		    ROS_INFO("%s", o->s.value.str);
+		    sprintf(buffer,"%s %s", buffer, o->s.value.str);
 		    break;
 		 case BUZZTYPE_USERDATA:
-		    ROS_INFO("[userdata @%p]", o->u.value);
+		    sprintf(buffer,"%s [userdata @%p]", buffer, o->u.value);
 		    break;
 		 default:
 		    break;
 	      }
 	   }
+	   ROS_INFO(buffer);
 	   //fprintf(stdout, "\n");
 	   return buzzvm_ret0(vm);
 	}
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 441a367..27aa2a5 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -36,8 +36,9 @@ namespace rosbzz_node{
 	/rosbuzz_node loop method executed once every step
 	/--------------------------------------------------*/
 	void roscontroller::RosControllerRun(){
-		
-		while(!robot_id_client.call(robot_id_srv_request,robot_id_srv_response)){
+		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)){
 			/*run once*/
     			ros::spinOnce();
 			/*loop rate of ros*/
@@ -118,6 +119,7 @@ namespace rosbzz_node{
 		n_c.getParam("No_of_Robots", barrier);
 		/*Obtain standby script to run during update*/
 		n_c.getParam("stand_by", stand_by);
+		n_c.getParam("xbee_status_srv", xbeesrv_name);
 		
 		GetSubscriptionParameters(n_c);
 		// initialize topics to null?
@@ -176,7 +178,7 @@ namespace rosbzz_node{
 		arm_client = n_c.serviceClient<mavros_msgs::CommandBool>(armclient);
 		mode_client =  n_c.serviceClient<mavros_msgs::SetMode>(modeclient);
 		mav_client = n_c.serviceClient<mavros_msgs::CommandLong>(fcclient_name);
-		robot_id_client = n_c.serviceClient<mavros_msgs::ParamGet>("/Robot_ID_srv");
+		xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
 		multi_msg=true;
 	}
 	/*---------------------------------------