From 32b0e44e9fff6b19adb9f7b7f1848e723508c5f8 Mon Sep 17 00:00:00 2001
From: pyhs <pierreyves@humanitas.io>
Date: Tue, 5 Sep 2017 15:39:03 -0400
Subject: [PATCH] Added capture image service

---
 include/roscontroller.h | 19 +++++++++++++++++--
 src/roscontroller.cpp   | 25 +++++++++++++++++--------
 2 files changed, 34 insertions(+), 10 deletions(-)

diff --git a/include/roscontroller.h b/include/roscontroller.h
index bc5e117..cebee9d 100644
--- a/include/roscontroller.h
+++ b/include/roscontroller.h
@@ -55,6 +55,8 @@ public:
   //void RosControllerInit();
   void RosControllerRun();
 
+  static const string CAPTURE_SRV_DEFAULT_NAME;
+
 private:
   struct num_robot_count
   {
@@ -70,7 +72,7 @@ private:
     double latitude=0.0;
     float  altitude=0.0;
   }; typedef struct gps GPS ; // not useful in cpp
-  
+
   GPS target, home, cur_pos;
   double cur_rel_altitude;
 
@@ -92,7 +94,19 @@ private:
   /*tmp to be corrected*/
   uint8_t no_cnt=0;
   uint8_t old_val=0	;
-  std::string bzzfile_name, fcclient_name, armclient, modeclient, rcservice_name,bcfname,dbgfname,out_payload,in_payload,obstacles_topic,stand_by,xbeesrv_name, setpoint_name;
+  std::string bzzfile_name;
+  std::string fcclient_name;
+  std::string armclient;
+  std::string modeclient;
+  std::string rcservice_name;
+  std::string bcfname,dbgfname;
+  std::string out_payload;
+  std::string in_payload;
+  std::string obstacles_topic;
+  std::string stand_by;
+  std::string xbeesrv_name;
+  std::string capture_srv_name;
+  std::string setpoint_name;
   std::string stream_client_name;
   std::string relative_altitude_sub_name;
   std::string setpoint_nonraw;
@@ -102,6 +116,7 @@ private:
   Num_robot_count count_robots;
   ros::ServiceClient mav_client;
   ros::ServiceClient xbeestatus_srv;
+  ros::ServiceClient capture_srv;
   ros::Publisher payload_pub;
   ros::Publisher MPpayload_pub;
   ros::Publisher neigh_pos_pub;
diff --git a/src/roscontroller.cpp b/src/roscontroller.cpp
index 2711a73..766b41b 100644
--- a/src/roscontroller.cpp
+++ b/src/roscontroller.cpp
@@ -2,6 +2,8 @@
 #include <thread>
 namespace rosbzz_node {
 
+const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
+
 /*---------------
 /Constructor
 ---------------*/
@@ -200,7 +202,7 @@ void roscontroller::RosControllerRun()
     while (ros::ok() && !buzz_utility::buzz_script_done()) {
       /*Update neighbors position inside Buzz*/
       // buzz_closure::neighbour_pos_callback(neighbours_pos_map);
-      
+
       /*log ROS Time stamp, pos (lat,long,alt), Swarm size, no. of neigh,
         neigh pos, RSSI val, Packet loss, filtered packet loss*/
       log<<ros::Time::now()<<",";
@@ -215,17 +217,17 @@ void roscontroller::RosControllerRun()
           log << (double)it->second.x << "," << (double)it->second.y
               << "," << (double)it->second.z << ",";
         }
-      const uint8_t shrt_id= 0xFF; 
+      const uint8_t shrt_id= 0xFF;
       float result;
-      if ( GetAPIRssi(shrt_id, result) ) 
+      if ( GetAPIRssi(shrt_id, result) )
       	log<<result<<",";
       else
         log<<"0,";
-      if ( GetRawPacketLoss(shrt_id, result) ) 
+      if ( GetRawPacketLoss(shrt_id, result) )
       	log<<result<<",";
       else
         log<<"0,";
-      if ( GetFilteredPacketLoss(shrt_id, result) ) 
+      if ( GetFilteredPacketLoss(shrt_id, result) )
       	log<<result<<",";
       else
         log<<"0,";
@@ -264,7 +266,7 @@ void roscontroller::RosControllerRun()
       // no_of_robots=get_number_of_robots();
       get_number_of_robots();
       buzz_utility::set_robot_var(no_of_robots);
-      /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF 
+      /*Retrive the state of the graph and uav and log TODO WARNING :PLEASE REMOVE IF
 	SCRIPT IS NOT graphform.bzz*/
       static buzzvm_t     VM = buzz_utility::get_vm();
       buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
@@ -366,6 +368,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
   } else
     n_c.getParam("xbee_status_srv", xbeesrv_name);
 
+  if(!n_c.getParam("xbee_status_srv", capture_srv_name))
+  {
+    capture_srv_name = CAPTURE_SRV_DEFAULT_NAME;
+  }
+
   GetSubscriptionParameters(n_c);
   // initialize topics to null?
 }
@@ -474,6 +481,7 @@ void roscontroller::Initialize_pub_sub(ros::NodeHandle &n_c)
     service = n_c.advertiseService(rcservice_name, &roscontroller::rc_callback, this);
   ROS_INFO("Ready to receive Mav Commands from RC client");
   xbeestatus_srv = n_c.serviceClient<mavros_msgs::ParamGet>(xbeesrv_name);
+  capture_srv = n_c.serviceClient<mavros_msgs::CommandBool>(capture_srv_name);
   stream_client = n_c.serviceClient<mavros_msgs::StreamRate>(stream_client_name);
 
   users_sub = n_c.subscribe("users_pos", 5, &roscontroller::users_pos, this);
@@ -751,8 +759,9 @@ void roscontroller::flight_controller_service_call()
   } else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/
     roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], 0);
   } else if (tmp == buzzuav_closures::COMMAND_PICTURE) { /* TODO: Buzz call to take a picture*/
-    ROS_INFO("TAKING A PICTURE HERE!! --------------")
-    ;
+    ROS_INFO("TAKING A PICTURE HERE!! --------------");
+    mavros_msgs::CommandBool capture_command;
+    capture_srv.call(capture_command);
   }
 }
 /*----------------------------------------------