Neighbour array addition commit

CMakeLists.txt

@@ -9,6 +9,9 @@ project(rosbuzz)
 find_package(catkin REQUIRED COMPONENTS
   roscpp
   std_msgs
+  genmsg
+  mavros_msgs
+  sensor_msgs
 )
 
 
@@ -41,11 +44,12 @@ find_package(catkin REQUIRED COMPONENTS
 ##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
 
 ## Generate messages in the 'msg' folder
-# add_message_files(
+ add_message_files(
-#   FILES
+   DIRECTORY msg
-#   Message1.msg
+   FILES
+   neighbour_pos.msg
 #   Message2.msg
-# )
+ )
 
 ## Generate services in the 'srv' folder
 # add_service_files(
@@ -62,10 +66,10 @@ find_package(catkin REQUIRED COMPONENTS
 # )
 
 ## Generate added messages and services with any dependencies listed here
-# generate_messages(
+ generate_messages(
-#   DEPENDENCIES
+   DEPENDENCIES
-#   st_msgs
+   sensor_msgs
-# )
+ )
 
 ################################################
 ## Declare ROS dynamic reconfigure parameters ##
@@ -97,6 +101,7 @@ find_package(catkin REQUIRED COMPONENTS
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
+CATKIN_DEPENDS message_runtime
 #  INCLUDE_DIRS include
 #  LIBRARIES buzzros
 #  CATKIN_DEPENDS Buzz roscpp st_msgs
@@ -141,7 +146,7 @@ add_executable(rosbuzz_node src/rosbuzz.cpp
   src/buzzuav_closures.cpp
   src/uav_utility.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)
 #############
 ## Install ##
 #############

msg/neighbour_pos.msg

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

package.xml

@@ -40,11 +40,12 @@
   <!-- Use test_depend for packages you need only for testing: -->
   <!--   <test_depend>gtest</test_depend> -->
   <buildtool_depend>catkin</buildtool_depend>
-
+  <build_depend>message_generation</build_depend>
+  <run_depend>message_runtime</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
     <!-- Other tools can request additional information be placed here -->
 
   </export>
-</package>
+</package>

src/buzz_utility.cpp

@@ -14,8 +14,8 @@
 #include <netdb.h>
 #include <sys/types.h>
 #include <sys/socket.h>
-
 #include <pthread.h>
+#include <vector>
 
 /****************************************/
 /****************************************/
@@ -35,21 +35,23 @@ static uint8_t*    STREAM_SEND_BUF = NULL;
 /****************************************/
 
 
-void in_msg_process(unsigned long int payload[],double neighbour[]){
+void in_msg_process(unsigned long int payload[], std::vector<pos_struct> pos_vect){
    /* Reset neighbor information */
    buzzneighbors_reset(VM);
   /* Get robot id and update neighbor information */
+for(int i=0;i<pos_vect.size();i++){
       buzzneighbors_add(VM,
-                        neighbour[0],
+                        pos_vect[i].id,
-                        neighbour[1],
+                        pos_vect[i].x,
-                        neighbour[2],
+                        pos_vect[i].y,
-                        neighbour[3]);
+                        pos_vect[i].z);
+}
    /* Go through messages and add them to the FIFO */
    for(size_t i = 0; i < sizeof(payload)/sizeof(payload[0]); ++i) {
       /* Copy packet into temporary buffer */
       long unsigned int* pl = (long unsigned int*) &payload[i];
       size_t tot = 0;
-       fprintf(stderr, "[DEBUG] Processing packet %p from %f\n", pl, neighbour[0]);
+       //fprintf(stderr, "[DEBUG] Processing packet %p from %f\n", pl, neighbour[0]);
       
       /* Go through the messages until there's nothing else to read */
       uint16_t unMsgSize;

src/buzz_utility.h

@@ -1,9 +1,18 @@
 #ifndef BUZZ_UTILITY_H
 #define BUZZ_UTILITY_H
+#include <vector>
+
+struct pos_struct
+{
+  int id;
+  double x,y,z;
+  pos_struct(int id,double x,double y,double z):id(id),x(x),y(y),z(z){};
+};
+
 
 extern int buzz_listen(const char* type,
                        int msg_size);
-void in_msg_process(unsigned long int payload[], double negighbour[]);
+void in_msg_process(unsigned long int payload[], std::vector<pos_struct> pos_vect);
 
 unsigned long int* out_msg_process();
 

src/out.basm

@@ -1,6 +1,5 @@
 !16
 'init
-'id
 'mydist
 'neighbors
 'listen
@@ -9,24 +8,25 @@
 'min
 'get
 'distance
-'a
 'step
 'broadcast
 'print
+'Distance to source:
+'count
 'reset
 'destroy
 
 	pushs 0
 	pushcn @__label_1
 	gstore
-	pushs 11
+	pushs 9
-	pushcn @__label_5
+	pushcn @__label_3
 	gstore
 	pushs 14
-	pushcn @__label_6
+	pushcn @__label_4
 	gstore
 	pushs 15
-	pushcn @__label_7
+	pushcn @__label_5
 	gstore
 	nop
 
@@ -36,49 +36,35 @@
 	done
 
 @__label_1
-	pushs 1	|3,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|9,11,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|3,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 0	|3,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	eq	|3,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	jumpz @__label_2	|3,12,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 2	|5,11,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushf 0.	|5,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gstore	|5,15,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	jump @__label_3	|7,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-@__label_2	|7,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 2	|9,11,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	pushf 1000.	|9,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	gstore	|9,18,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 3	|11,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 2	|11,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	gload	|11,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 4	|11,14,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 3	|11,14,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	tget	|11,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 5	|11,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 4	|11,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushl @__label_4	|12,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushl @__label_2	|12,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	pushi 2	|16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	callc	|16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-@__label_3	|17,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|18,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 10	|18,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 1	|18,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gstore	|18,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	ret0	|19,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 
-@__label_4
+@__label_2
-	pushs 2	|13,15,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|13,15,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 6	|13,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 5	|13,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	gload	|13,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 7	|13,22,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 6	|13,22,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	tget	|13,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 2	|14,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|14,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	gload	|14,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 3	|15,19,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 2	|15,19,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	gload	|15,19,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 8	|15,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 7	|15,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	tget	|15,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	lload 3	|15,32,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	pushi 1	|15,33,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	callc	|15,33,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 9	|15,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 8	|15,34,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	tget	|15,43,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	lload 2	|15,50,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	add	|15,50,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@@ -87,26 +73,33 @@
 	gstore	|15,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 	ret0	|16,7,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
 
+@__label_3
+	pushs 2	|22,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|22,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 10	|22,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|22,19,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 4	|22,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|22,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|22,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 2	|22,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|22,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 11	|23,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|23,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 12	|23,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 1	|23,35,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|23,35,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 2	|23,46,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	gload	|23,46,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushs 13	|23,47,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	tget	|23,52,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 0	|23,54,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|23,54,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	pushi 3	|23,55,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	callc	|23,55,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|26,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+
+@__label_4
+	ret0	|30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+
 @__label_5
-	pushs 3	|23,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+	ret0	|34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|23,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 12	|23,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	tget	|23,19,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 5	|23,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 2	|23,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|23,44,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 2	|23,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	callc	|23,45,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 13	|24,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|24,5,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushs 2	|24,12,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	gload	|24,12,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	pushi 1	|24,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	callc	|24,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-	ret0	|35,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-
-@__label_6
-	ret0	|39,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-
-@__label_7
-	ret0	|43,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz

src/rosbuzz.cpp

@@ -13,6 +13,8 @@
 #include "mavros_msgs/State.h"
 #include "mavros_msgs/BatteryStatus.h"
 #include "mavros_msgs/Mavlink.h"
+#include "sensor_msgs/NavSatStatus.h"
+#include "rosbuzz/neighbour_pos.h"
 #include <sstream>
 #include <buzz/buzzasm.h>
 #include "buzzuav_closures.h"
@@ -24,10 +26,9 @@
 #include <signal.h>
 
 
+std::vector<pos_struct> pos_vect; // vector of struct to store neighbours position 
 static int done = 0;
-
 static double cur_pos[3];
-double neighbor_pos[4]; 
 uint64_t payload;
 
 /**
@@ -59,19 +60,19 @@ cur_pos [2] =altitude;
 }
 
 /*convert from catresian to spherical coordinate system callback */
-void cvt_spherical_coordinates(double latitude,
+void cvt_spherical_coordinates(){
- 		  double longitude,
+double latitude,longitude,altitude;
-		  double altitude){
+for(int i=0;i<pos_vect.size();i++){
-/** convert the current position coordination system from cartetion system (gps) to sperical system (Buzz) type **/ 
+latitude=pos_vect[i].x;
- 
+longitude = pos_vect[i].y;
- neighbor_pos[0] = 01;
+altitude=pos_vect[i].z;
- neighbor_pos[1]  = sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
+pos_vect[i].x=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
- neighbor_pos[2] = atan(longitude/latitude);
+pos_vect[i].y=atan(longitude/latitude);
- neighbor_pos[3] = atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
+pos_vect[i].z=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
-fprintf(stdout, "%.15f :distance value\n", neighbor_pos[1]);
+    ROS_INFO("[Debug] Converted for neighbour : %d radius    : [%15f]",pos_vect[i].id, pos_vect[i].x);
-fprintf(stdout, "%.15f :elevation\n", neighbor_pos[2]);
+    ROS_INFO("[Debug] Converted for neighbour : %d azimuth   : [%15f]",pos_vect[i].id, pos_vect[i].y);
-fprintf(stdout, "%.15f :azimuth\n", neighbor_pos[3]);
+    ROS_INFO("[Debug] Converted for neighbour : %d elevation : [%15f]",pos_vect[i].id, pos_vect[i].z);
-
+}
 }
 
 /*battery status callback*/ 
@@ -97,28 +98,30 @@ int i = 0;
 		message_obt[i] = *it;
 		i++;
         }
-in_msg_process(message_obt, neighbor_pos);
+in_msg_process(message_obt, pos_vect);
 
 }
 
 /*neighbours position call back */
-void neighbour_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
+ 
+void neighbour_pos(const rosbuzz::neighbour_pos::ConstPtr& msg)
 {
-  
+/*obtain the neighbours position*/
-/*obtain the neigbours position*/
+int i=0;
-
+/*clear all the previous neighbour position*/
-double latitude =(msg->latitude-cur_pos[0]);
+pos_vect.clear();
-double longitude =(msg->longitude-cur_pos[1]);
+for (std::vector<sensor_msgs::NavSatFix>::const_iterator it = msg->pos_neigh.begin(); it != msg->pos_neigh.end(); ++it){
-double altitude =(msg->altitude-cur_pos[2]);
+sensor_msgs::NavSatFix cur_neigh = *it;
-
+sensor_msgs::NavSatStatus stats = cur_neigh.status;
-  ROS_INFO("I heard neighbour latitude: [%15f]", latitude);
+pos_vect.push_back(pos_struct(stats.status,(cur_neigh.latitude-cur_pos[0]),(cur_neigh.longitude-cur_pos[1]),(cur_neigh.altitude-cur_pos[2])));
-  ROS_INFO("I heard neighbour longitude: [%15f]", longitude);
+    ROS_INFO("[Debug]I heard neighbour: %d from latitude: [%15f]",pos_vect[i].id, pos_vect[i].x);
-  ROS_INFO("I heard neighbour altitude: [%15f]", altitude);
+    ROS_INFO("[Debug]I heard neighbour: %d from longitude: [%15f]",pos_vect[i].id, pos_vect[i].y);
-
+    ROS_INFO("[Debug]I heard neighbour: %d from altitude: [%15f]",pos_vect[i].id, pos_vect[i].z);
-cvt_spherical_coordinates(latitude,longitude,altitude);
+i++;
-//neighbour_location_handler( distance, azimuth, elevation, 01);
+} 
-
+cvt_spherical_coordinates();
 }
+
 int oldcmdID=0;
 int rc_cmd;
 bool rc_callback(mavros_msgs::CommandInt::Request  &req,
@@ -143,13 +146,13 @@ if(req.command==oldcmdID)
 		res.success = true;
 		break;
 	case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH:
-   		ROS_INFO("GO HOME!!!!");
+   		ROS_INFO("RC_Call: GO HOME!!!!");
 		rc_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
 		rc_call(rc_cmd);
 		res.success = true;
 		break;
 	case mavros_msgs::CommandCode::NAV_WAYPOINT:
-   		ROS_INFO("GO TO!!!!");
+   		ROS_INFO("RC_Call: GO TO!!!! x = %f , y = %f , Z = %f",req.param1,req.param2,req.param3);
 		double rc_goto[3];
    		rc_goto[0]=req.param1;
 		rc_goto[1]=req.param2;
@@ -246,7 +249,7 @@ int main(int argc, char **argv)
     cmd_srv.request.param1 = goto_pos[0];
     cmd_srv.request.param2 = goto_pos[1];
     cmd_srv.request.param3 = goto_pos[2];
-    ROS_INFO("set value X = %f, Y =%f, Z= %f ",cmd_srv.request.param1,cmd_srv.request.param2,cmd_srv.request.param3);
+    //ROS_INFO("set value X = %f, Y =%f, Z= %f ",cmd_srv.request.param1,cmd_srv.request.param2,cmd_srv.request.param3);
     /*prepare commands for takeoff,land and gohome*/
     //char tmp[20];
     //sprintf(tmp,"%i",getcmd());

src/test.bzz

@@ -1,10 +1,10 @@
 # Executed once at init time.
 function init() {
-if(id == 0) {
+#if(id == 0) {
     # Source robot
-    mydist = 0.
+#    mydist = 0.
-  }
+#  }
-  else {
+ # else {
     # Other robots
     mydist = 1000.
     # Listen to other robots' distances
@@ -14,22 +14,13 @@ if(id == 0) {
           mydist,
           neighbors.get(robot_id).distance + value)
       })
-  }
+ # }
-a=1
 }
  
 # Executed at each time step.
 function step() {
 neighbors.broadcast("dist_to_source", mydist)
-print(mydist)
+print("Distance to source:", mydist, neighbors.count())
-#if(a==1){ uav_takeoff()
-#       a=0
-#       print("Buzz: take off")
-#     }
-#else {uav_land()
-#       a=1
-#       print("Buzz: land")
-#      }
 
 
 }