local enum for internal command passing and proxy table to fit argos

include/buzzuav_closures.h

@@ -10,6 +10,17 @@
 //#include "roscontroller.h"
 
 namespace buzzuav_closures{
+	typedef enum {
+      		COMMAND_NIL = 0,     // Dummy command
+      		COMMAND_TAKEOFF,     // Take off
+		COMMAND_LAND,
+		COMMAND_GOHOME,
+		COMMAND_ARM,
+		COMMAND_DISARM,
+		COMMAND_GOTO,
+		COMMAND_MOVETO,
+	 } Custom_CommandCode;
+
 /*
  * prextern int() function in Buzz
  * This function is used to print data from buzz

src/buzzuav_closures.cpp

@@ -108,7 +108,7 @@ namespace buzzuav_closures{
 	   gps_from_rb(d, b, goto_pos);
 	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;*/
 	   printf(" Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f  \n",goto_pos[0], goto_pos[1], goto_pos[2]);
-	   buzz_cmd=5;
+	   buzz_cmd= COMMAND_MOVETO; // TO DO what should we use
 	   return buzzvm_ret0(vm);
 	}
 
@@ -120,7 +120,7 @@ namespace buzzuav_closures{
 	   set_goto(rc_goto_pos);
 	   cur_cmd=mavros_msgs::CommandCode::NAV_WAYPOINT;
 	   printf(" Buzz requested Go To, to Latitude: %.7f , Longitude: %.7f, Altitude: %.7f  \n",goto_pos[0],goto_pos[1],goto_pos[2]);
-	   buzz_cmd=2;
+	   buzz_cmd=COMMAND_GOTO;
 	   return buzzvm_ret0(vm);
 	}
 
@@ -130,13 +130,13 @@ namespace buzzuav_closures{
 	int buzzuav_arm(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
 	   printf(" Buzz requested Arm \n");
-	   buzz_cmd=3;
+	   buzz_cmd=COMMAND_ARM;
 	   return buzzvm_ret0(vm);
 	}
 	int buzzuav_disarm(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
 	   printf(" Buzz requested Disarm  \n");
-	   buzz_cmd=4;
+	   buzz_cmd=COMMAND_DISARM;
 	   return buzzvm_ret0(vm);
 	}
 
@@ -151,21 +151,21 @@ namespace buzzuav_closures{
 	   height = goto_pos[2];
 	   cur_cmd=mavros_msgs::CommandCode::NAV_TAKEOFF;
 	   printf(" Buzz requested Take off !!! \n");
-	   buzz_cmd = 1;
+	   buzz_cmd = COMMAND_TAKEOFF;
 	   return buzzvm_ret0(vm);
 	}
 
 	int buzzuav_land(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::NAV_LAND;
 	   printf(" Buzz requested Land !!! \n");
-	   buzz_cmd = 1;
+	   buzz_cmd = COMMAND_LAND;
 	   return buzzvm_ret0(vm);
 	}
 
 	int buzzuav_gohome(buzzvm_t vm) {
 	   cur_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
 	   printf(" Buzz requested gohome !!! \n");
-	   buzz_cmd = 1;
+	   buzz_cmd = COMMAND_GOHOME;
 	   return buzzvm_ret0(vm);
 	}
 
@@ -306,9 +306,65 @@ namespace buzzuav_closures{
 
 	/******************************************************/
 	/*Create an obstacle Buzz table from proximity sensors*/
+	/*  Acessing proximity in buzz script 
+	    proximity[0].angle and proximity[0].value - front
+		""          ""          "" 	      - right and back
+	    proximity[3].angle and proximity[3].value - left
+	    proximity[4].angle = -1 and proximity[4].value -bottom */
 	/******************************************************/
 
 	int buzzuav_update_prox(buzzvm_t vm) {
+
+	buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
+        buzzvm_pusht(vm);
+        buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
+        buzzvm_gstore(vm);
+      
+        /* Fill into the proximity table */
+        buzzobj_t tProxRead;
+        float angle =0;
+        for(size_t i = 0; i < 4; ++i) {
+           /* Create table for i-th read */
+           buzzvm_pusht(vm);
+           tProxRead = buzzvm_stack_at(vm, 1);
+           buzzvm_pop(vm);
+           /* Fill in the read */
+	   buzzvm_push(vm, tProxRead);
+   	   buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
+   	   buzzvm_pushf(vm, obst[i+1]);
+   	   buzzvm_tput(vm);
+	   buzzvm_push(vm, tProxRead);
+   	   buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
+   	   buzzvm_pushf(vm, angle);
+   	   buzzvm_tput(vm);
+           /* Store read table in the proximity table */
+          buzzvm_push(vm, tProxTable);
+   	  buzzvm_pushi(vm, i);
+  	  buzzvm_push(vm, tProxRead);
+ 	  buzzvm_tput(vm);
+	   angle+=1.5708;
+        }
+	/* Create table for bottom read */
+	 angle =-1;
+         buzzvm_pusht(vm);
+         tProxRead = buzzvm_stack_at(vm, 1);
+         buzzvm_pop(vm);
+         /* Fill in the read */
+	 buzzvm_push(vm, tProxRead);
+   	 buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
+   	 buzzvm_pushf(vm, obst[0]);
+   	 buzzvm_tput(vm);
+	 buzzvm_push(vm, tProxRead);
+   	 buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
+   	 buzzvm_pushf(vm, angle);
+   	 buzzvm_tput(vm);
+	 /*Store read table in the proximity table*/
+	 buzzvm_push(vm, tProxTable);
+   	 buzzvm_pushi(vm, 4);
+  	 buzzvm_push(vm, tProxRead);
+ 	 buzzvm_tput(vm);
+
+	 /*
 	  buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
 	   buzzvm_pusht(vm);
 	   buzzvm_dup(vm);
@@ -331,7 +387,7 @@ namespace buzzuav_closures{
 	   buzzvm_pushs(vm, buzzvm_string_register(vm, "left", 1));
 	   buzzvm_pushf(vm, obst[4]);
 	   buzzvm_tput(vm);
-	   buzzvm_gstore(vm);
+	   buzzvm_gstore(vm);*/
 	   return vm->state;
 	}
 

src/roscontroller.cpp

@@ -353,13 +353,13 @@ namespace rosbzz_node{
 		/*FC call for takeoff,land, gohome and arm/disarm*/
 		int tmp = buzzuav_closures::bzz_cmd();
     		double* goto_pos = buzzuav_closures::getgoto();
-		if (tmp == 1){
+		if (tmp == buzzuav_closures::COMMAND_TAKEOFF || tmp== buzzuav_closures::COMMAND_LAND || tmp==buzzuav_closures::COMMAND_GOHOME){
 			cmd_srv.request.param7 = goto_pos[2];
 			//cmd_srv.request.z = goto_pos[2];
 			cmd_srv.request.command =  buzzuav_closures::getcmd();  		
 			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"); 
-		} else if (tmp == 2) { /*FC call for goto*/ 
+		} else if (tmp == buzzuav_closures::COMMAND_GOTO) { /*FC call for goto*/ 
 			/*prepare the goto publish message */
 			cmd_srv.request.param5 = goto_pos[0];
     			cmd_srv.request.param6 = goto_pos[1];
@@ -370,13 +370,13 @@ namespace rosbzz_node{
     			cmd_srv.request.command = mavros_msgs::CommandCode::CMD_MISSION_START;
 			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"); 
-		} else if (tmp == 3) { /*FC call for arm*/
+		} else if (tmp == buzzuav_closures::COMMAND_ARM) { /*FC call for arm*/
 			armstate=1;
 			Arm(); 
-		} else if (tmp == 4){
+		} else if (tmp == buzzuav_closures::COMMAND_DISARM){
 			armstate=0;
 			Arm();
-		} else if (tmp == 5) { /*Buzz call for moveto*/ 
+		} else if (tmp == buzzuav_closures::COMMAND_MOVETO) { /*Buzz call for moveto*/ 
 			/*prepare the goto publish message */
 			mavros_msgs::PositionTarget pt;
 			pt.type_mask = mavros_msgs::PositionTarget::IGNORE_VX | mavros_msgs::PositionTarget::IGNORE_VY | mavros_msgs::PositionTarget::IGNORE_VZ | mavros_msgs::PositionTarget::IGNORE_AFX | mavros_msgs::PositionTarget::IGNORE_AFY | mavros_msgs::PositionTarget::IGNORE_AFZ | mavros_msgs::PositionTarget::IGNORE_YAW_RATE;