Neighbours pos publisher addition

CMakeLists.txt

@@ -11,6 +11,21 @@ find_package(catkin REQUIRED COMPONENTS
   std_msgs
   mavros_msgs
   sensor_msgs
+  message_generation
+)
+
+##############################
+#Generate messages#
+##############################
+
+add_message_files(
+ FILES
+ neigh_pos.msg
+)
+
+generate_messages(
+DEPENDENCIES
+sensor_msgs
 )
 
 ###################################
@@ -20,7 +35,7 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package(
   INCLUDE_DIRS include
 #  LIBRARIES xbee_ros_node
-  CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs
+  CATKIN_DEPENDS roscpp std_msgs mavros_msgs sensor_msgs message_runtime
 #  DEPENDS system_lib
 )
 
@@ -41,6 +56,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
   src/uav_utility.cpp 
   src/buzz_update.cpp)
 target_link_libraries(rosbuzz_node ${catkin_LIBRARIES} /usr/local/lib/libbuzz.so /usr/local/lib/libbuzzdbg.so -lpthread)
+add_dependencies(rosbuzz_node rosbuzz_generate_messages_cpp)
 
 # Executables and libraries for installation to do
 install(TARGETS rosbuzz_node

include/roscontroller.h

@@ -9,6 +9,7 @@
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
 #include "sensor_msgs/NavSatStatus.h"
+#include <rosbuzz/neigh_pos.h>
 #include <sstream>
 #include <buzz/buzzasm.h>
 #include "buzz_utility.h"
@@ -38,6 +39,7 @@ private:
  	double cur_pos[3];
 	uint64_t payload;
 	std::map< int,  buzz_utility::Pos_struct> neighbours_pos_map;
+	
 	int timer_step=0;
 	int robot_id=0;
         //int oldcmdID=0;
@@ -47,6 +49,7 @@ private:
 	bool rcclient;
 	ros::ServiceClient mav_client;
 	ros::Publisher payload_pub;
+	ros::Publisher neigh_pos_pub;
 	ros::ServiceServer service;
 	ros::Subscriber current_position_sub;
 	ros::Subscriber battery_sub;

msg/neigh_pos.msg

@@ -0,0 +1,2 @@
+Header header
+sensor_msgs/NavSatFix[] pos_neigh

package.xml

@@ -48,8 +48,8 @@
   <run_depend>mavros_msgs</run_depend>
   <build_depend>sensor_msgs</build_depend>
   <run_depend>sensor_msgs</run_depend>
-  <!-- <build_depend>message_generation</build_depend> -->
-  <!-- <run_depend>message_runtime</run_depend> -->
+  <build_depend>message_generation</build_depend> 
+  <run_depend>message_runtime</run_depend> 
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/buzz_utility.cpp

@@ -18,7 +18,7 @@ namespace buzz_utility{
 	static uint8_t*     BO_BUF          = 0;
 	static buzzdebug_t  DBG_INFO        = 0;
 	static uint8_t      MSG_SIZE        = 100;   // Only 100 bytes of Buzz messages every step
-	static int          MAX_MSG_SIZE    = 10100; // Maximum Msg size for sending update packets 
+	static int          MAX_MSG_SIZE    = 10000; // Maximum Msg size for sending update packets 
 	
 	
 	/****************************************/

src/roscontroller.cpp

@@ -37,6 +37,19 @@ namespace rosbzz_node{
   			{
       				/*Update neighbors position inside Buzz*/
      				buzz_utility::neighbour_pos_callback(neighbours_pos_map);
+				map< int, buzz_utility::Pos_struct >::iterator it;
+				rosbuzz::neigh_pos neigh_pos_array; //neigh_pos_array.clear(); 
+				sensor_msgs::NavSatFix tmp;
+    				for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
+					tmp.position_covariance_type=it->first; //custom robot id storage
+                        		tmp.longitude=(it->second).x;
+                        		tmp.latitude=(it->second).y;
+                        		tmp.altitude=(it->second).z;
+
+    					neigh_pos_array.pos_neigh.push_back(tmp);
+					    					
+					}
+				neigh_pos_pub.publish(neigh_pos_array);
 				/*Check updater state and step code*/
   				if(update_routine(bcfname.c_str(), dbgfname.c_str(),0)==CODE_RUNNING)
       				/*Step buzz script */
@@ -99,7 +112,7 @@ namespace rosbzz_node{
 		flight_status_sub =n_c.subscribe("/flight_status",100, &roscontroller::flight_status_update,this);
   		/*publishers*/
 		payload_pub = n_c.advertise<mavros_msgs::Mavlink>(out_payload, 1000);
-		cout<<out_payload<<endl;
+		neigh_pos_pub = n_c.advertise<rosbuzz::neigh_pos>("/neighbours_pos",1000);	
 		/* Clients*/
   		mav_client = n_c.serviceClient<mavros_msgs::CommandInt>(fcclient_name);
 
@@ -286,7 +299,7 @@ namespace rosbzz_node{
 		buzz_utility::in_msg_process((message_obt+3));
    
 	}
- 
+ 	
 	/* RC command service */
 	bool roscontroller::rc_callback(mavros_msgs::CommandInt::Request  &req,
 		         mavros_msgs::CommandInt::Response &res){

src/test1.bzz

@@ -19,32 +19,20 @@ function barrier_set(threshold, transf) {
     barrier_wait(threshold, transf);
   }
   barrier = stigmergy.create(BARRIER_VSTIG)
-  statestr = "barrier"
 }
 
 #
 # Make yourself ready
 #
 function barrier_ready() {
- 
   barrier.put(id, 1)
-  print ("before put")
-  barrier.get(id)
-  print("barrier put : ")
 }
 
 #
 # Executes the barrier
 #
-function table_print(t) {
-  foreach(t, function(key, value) {
-      log(key, " -> ", value)
-    })
-}
 function barrier_wait(threshold, transf) {
- #table_print(barrier)
   barrier.get(id)
-  extradbg = barrier.size()
   if(barrier.size() >= threshold) {
     barrier = nil
     transf()
@@ -69,7 +57,7 @@ neighbors.listen("cmd",
 
 function takeoff() {
 	if( flight.status == 2) {
-		barrier_set(ROBOTS,transition_to_land)
+		barrier_set(ROBOTS,idle)
 		barrier_ready()
 	}
 	else if(flight.status!=3)
@@ -84,31 +72,14 @@ function land() {
 		uav_land()
 }
 
-
-function transition_to_land() {
-	statef=transition_to_land
-	if(battery.capacity<50){
-		print("Low battery! Landing the fleet")
-		statef=land
-		neighbors.broadcast("cmd", 21)
-	}
-}
-
 # Executed once at init time.
 function init() {
-i = 0
-a = 0
-val = 0
-oldcmd = 0
-tim=0
-statef=idle
+	statef=idle
 }
- 
+
 # Executed at each time step.
 function step() {
 
-#if(flight.rc_cmd!=oldcmd) {
-	
 	if(flight.rc_cmd==22 and flight.status==1) {
 		flight.rc_cmd=0
 		statef=takeoff		
@@ -118,8 +89,6 @@ function step() {
 		statef=land
 		neighbors.broadcast("cmd", 21)
 	}
-#	oldcmd=flight.rc_cmd
-#}
 
 statef()
 }