merge rosbuzz dev

buzz_scripts/graphformGPS.bzz

@@ -543,7 +543,6 @@ function DoJoining(){
     set_rc_goto()
 	else
     gotoWP(TransitionToJoined)
-
 	#pack the communication package
 	m_selfMessage.State=s2i(UAVSTATE)
 	m_selfMessage.Label=m_nLabel

buzz_scripts/include/barrier.bzz

@@ -30,7 +30,7 @@ function barrier_create() {
 
 function barrier_set(threshold, transf, resumef, bdt) {
   statef = function() {
-    barrier_wait(threshold, transf, resumef, bdt);
+    barrier_wait(threshold, transf, resumef);
   }
   UAVSTATE = "BARRIERWAIT"
   barrier_create()
@@ -40,6 +40,7 @@ function barrier_set(threshold, transf, resumef, bdt) {
 # Make yourself ready
 #
 function barrier_ready() {
+  log("BARRIER READY -------")
   barrier.put(id, 1)
   barrier.put("d", 0)
 }

buzz_scripts/include/uavstates.bzz

@@ -27,10 +27,16 @@ function idle() {
     UAVSTATE = "IDLE"
 }
 
+firstpassT = 1
 function takeoff() {
 	UAVSTATE = "TAKEOFF"
 	statef=takeoff
 
+  if(firstpassT) {
+    barrier_create(TAKEOFF_VSTIG)
+    firstpassT = 0
+  }
+	
 	if( flight.status == 2 and position.altitude >= TARGET_ALTITUDE-TARGET_ALTITUDE/20.0) {
 		barrier_set(ROBOTS, action, land, -1)
 		barrier_ready()
@@ -41,6 +47,7 @@ function takeoff() {
   }
 }
 
+firstpassL = 1
 function land() {
 	UAVSTATE = "LAND"
 	statef=land

include/roscontroller.h

@@ -55,6 +55,8 @@ public:
   //void RosControllerInit();
   void RosControllerRun();
 
+  static const string CAPTURE_SRV_DEFAULT_NAME;
+
 private:
   struct num_robot_count
   {
@@ -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;

src/roscontroller.cpp

@@ -2,6 +2,8 @@
 #include <thread>
 namespace rosbzz_node {
 
+const string roscontroller::CAPTURE_SRV_DEFAULT_NAME = "/image_sender/capture_image";
+
 /*---------------
 /Constructor
 ---------------*/
@@ -268,12 +270,12 @@ void roscontroller::RosControllerRun()
       /*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));
-            	buzzvm_gload(VM);
-            	buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
-            	buzzvm_pop(VM);
-		std::stringstream state_buff;
-		state_buff<< graph_state->s.value.str<<",";
+      std::stringstream state_buff;
+      //buzzvm_pushs(VM, buzzvm_string_register(VM, "m_eState",1));
+      //      	buzzvm_gload(VM);
+      //      	buzzobj_t graph_state = buzzvm_stack_at(VM, 1);
+      //      	buzzvm_pop(VM);
+      //	state_buff<< graph_state->s.value.str<<",";
       buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE",1));
             	buzzvm_gload(VM);
             	buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
@@ -367,6 +369,11 @@ void roscontroller::Rosparameters_get(ros::NodeHandle &n_c)
   } else
     n_c.getParam("xbee_status_srv", xbeesrv_name);
 
+  if(!n_c.getParam("capture_image_srv", capture_srv_name))
+  {
+    capture_srv_name = CAPTURE_SRV_DEFAULT_NAME;
+  }
+
   GetSubscriptionParameters(n_c);
   // initialize topics to null?
 }
@@ -475,6 +482,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);
@@ -753,7 +761,8 @@ void roscontroller::flight_controller_service_call()
     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::Duration(0.2).sleep();
+    mavros_msgs::CommandBool capture_command;
+    capture_srv.call(capture_command);
   }
 }
 /*----------------------------------------------