Merge commit 'e5e7500b20770df29d594a7d07d1cd8e9f6ee6f9' into sim

buzz_scripts/include/act/states.bzz

@@ -12,7 +12,7 @@ TARGET_ALTITUDE = 15.0 # m.
 BVMSTATE = "TURNEDOFF"
 PICTURE_WAIT = 20 # steps
 
-GOTO_MAXVEL = 0.85 # m/steps
+GOTO_MAXVEL = 1.5 # m/steps
 GOTO_MAXDIST = 150 # m.
 GOTODIST_TOL = 0.4  # m.
 GOTOANG_TOL = 0.1 # rad.
@@ -77,7 +77,7 @@ function stop() {
 	if(V_TYPE == 0 or V_TYPE == 1) {	# flying vehicle so LAND
 		neighbors.broadcast("cmd", 21)
 		uav_land()
-		if(pose.position.altitude <= 0.3) {
+		if(pose.position.altitude <= 0.2) {
 			BVMSTATE = "TURNEDOFF"
 			#barrier_set(ROBOTS,"TURNEDOFF","STOP", 21)
 			#barrier_ready(21)
@@ -105,7 +105,7 @@ function goto_gps(transf) {
 	m_navigation = vec_from_gps(rc_goto.latitude, rc_goto.longitude, 0)
 	#print(" has to move ", math.vec2.length(m_navigation), math.vec2.angle(m_navigation))
  	if(math.vec2.length(m_navigation)>GOTO_MAXDIST)
-		log("Sorry this is too far.")
+		log("Sorry this is too far (", math.vec2.length(m_navigation), " / ", GOTO_MAXDIST, " )")
 	else if(math.vec2.length(m_navigation) < GOTODIST_TOL and math.vec2.angle(m_navigation) < GOTOANG_TOL) # reached destination
 		transf()
 	else {

buzz_scripts/main.bzz

@@ -9,6 +9,7 @@ include "vstigenv.bzz"
 include "utils/takeoff_heights.bzz"
 
 #State launched after takeoff
+
 AUTO_LAUNCH_STATE = "TASK_ALLOCATE"
 TARGET     = 9.0
 EPSILON    = 30.0

include/roscontroller.h

@@ -98,6 +98,7 @@ private:
   typedef struct POSE ros_pose;
 
   ros_pose target, home, cur_pos;
+  double* goto_pos;
 
   struct MsgData
   {
@@ -141,6 +142,7 @@ private:
   uint8_t old_val = 0;
   bool debug = false;
   bool setmode = false;
+  bool BClpose = false;
   std::string bzzfile_name;
   std::string bcfname, dbgfname;
   std::string stand_by;

src/roscontroller.cpp

@@ -39,6 +39,7 @@ logical_clock(ros::Time()), previous_step_time(ros::Time())
   cur_pos.longitude = 0;
   cur_pos.latitude = 0;
   cur_pos.altitude = 0;
+  goto_pos = buzzuav_closures::getgoto();
 
   // set stream rate - wait for the FC to be started
   SetStreamRate(0, 10, 1);
@@ -162,6 +163,9 @@ void roscontroller::RosControllerRun()
       prepare_msg_and_publish();
       // Call the flight controler service
       flight_controller_service_call();
+      // Broadcast local position to FCU
+      if(BClpose)
+	SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       //  Set ROBOTS variable (swarm size)
       get_number_of_robots();
       buzz_utility::set_robot_var(no_of_robots);
@@ -737,34 +741,40 @@ script
 /-------------------------------------------------------------------------------*/
 {
   //  flight controller client call if requested from Buzz
-  double* goto_pos;
   float* gimbal;
   switch (buzzuav_closures::bzz_cmd())
   {
     case NAV_TAKEOFF:
       goto_pos = buzzuav_closures::getgoto();
+      goto_pos[0] = 0.0;
+      goto_pos[1] = 0.0;
       cmd_srv.request.param5 = cur_pos.latitude;
       cmd_srv.request.param6 = cur_pos.longitude;
       cmd_srv.request.param7 = goto_pos[2];
       cmd_srv.request.command = buzzuav_closures::getcmd();
       if (!armstate)
       {
-        if(setmode)
+        if(setmode){
           SetMode("LOITER", 0);
+          ros::Duration(0.5).sleep();
+        }
         armstate = 1;
         Arm();
-        ros::Duration(0.5).sleep();
         // Registering HOME POINT.
         home = cur_pos;
+	BClpose = true;
       }
       if (current_mode != "GUIDED" && setmode)
         SetMode("GUIDED", 2000);  // added for compatibility with 3DR Solo
+      if(setmode)
+      {
         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");
+      }
       break;
 
     case NAV_LAND:
@@ -778,10 +788,11 @@ script
         armstate = 0;
         Arm();
       }
-      else if(cur_pos.altitude < 0.3) //disarm only when close to ground
+      else if(cur_pos.altitude < 0.4) //disarm only when close to ground
       {
         armstate = 0;
         Arm();
+	BClpose = false;
       }
       if (mav_client.call(cmd_srv))
       {
@@ -829,7 +840,6 @@ script
 
     case NAV_SPLINE_WAYPOINT:
       goto_pos = buzzuav_closures::getgoto();
-      roscontroller::SetLocalPosition(goto_pos[0], goto_pos[1], goto_pos[2], goto_pos[3]);
       break;
 
     case DO_MOUNT_CONTROL:
@@ -1005,6 +1015,14 @@ void roscontroller::local_pos_callback(const geometry_msgs::PoseStamped::ConstPt
   cur_pos.x = msg->pose.position.x;
   cur_pos.y = msg->pose.position.y;
 
+  if(!BClpose)
+  {
+  goto_pos[0]=cur_pos.x;
+  goto_pos[1]=cur_pos.y;
+  goto_pos[2]=0.0;
+  BClpose = true;
+  }
+
   buzzuav_closures::set_currentNEDpos(msg->pose.position.y,msg->pose.position.x);
   //  cur_pos.z = pose->pose.position.z; // Using relative altitude topic instead
   tf::Quaternion q(