diff --git a/CMakeLists.txt b/CMakeLists.txt
index fca67ba..ba98d34 100644
--- a/CMakeLists.txt
+++ b/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(
-# FILES
-# Message1.msg
+ add_message_files(
+ DIRECTORY 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(
-# DEPENDENCIES
-# st_msgs
-# )
+ generate_messages(
+ DEPENDENCIES
+ 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 ##
#############
diff --git a/msg/neighbour_pos.msg b/msg/neighbour_pos.msg
new file mode 100644
index 0000000..8db4257
--- /dev/null
+++ b/msg/neighbour_pos.msg
@@ -0,0 +1,2 @@
+Header header
+sensor_msgs/NavSatFix[] pos_neigh
diff --git a/package.xml b/package.xml
index d47c4cc..6e779ce 100644
--- a/package.xml
+++ b/package.xml
@@ -40,11 +40,12 @@
catkin
-
+ message_generation
+ message_runtime
-
\ No newline at end of file
+
diff --git a/src/buzz_utility.cpp b/src/buzz_utility.cpp
index 53019a6..6d7abe1 100644
--- a/src/buzz_utility.cpp
+++ b/src/buzz_utility.cpp
@@ -14,8 +14,8 @@
#include
#include
#include
-
#include
+#include
/****************************************/
/****************************************/
@@ -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_vect){
/* Reset neighbor information */
buzzneighbors_reset(VM);
/* Get robot id and update neighbor information */
+for(int i=0;i
+
+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_vect);
unsigned long int* out_msg_process();
diff --git a/src/out.basm b/src/out.basm
index 06f57ba..f6ef3d4 100644
--- a/src/out.basm
+++ b/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
- pushcn @__label_5
+ pushs 9
+ 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
- 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
+ pushs 1 |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
- pushl @__label_4 |12,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+ pushs 4 |11,21,/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
- 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
+ ret0 |18,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
-@__label_4
- pushs 2 |13,15,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
- pushs 6 |13,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
+@__label_2
+ pushs 1 |13,15,/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
- 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
+ ret0 |34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
diff --git a/src/out.bdbg b/src/out.bdbg
index 0f2dffd..5d37dfe 100644
Binary files a/src/out.bdbg and b/src/out.bdbg differ
diff --git a/src/out.bo b/src/out.bo
index 277f0d8..9c03aff 100644
Binary files a/src/out.bo and b/src/out.bo differ
diff --git a/src/rosbuzz.cpp b/src/rosbuzz.cpp
index ade0ea5..a574d02 100644
--- a/src/rosbuzz.cpp
+++ b/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
#include
#include "buzzuav_closures.h"
@@ -24,10 +26,9 @@
#include
+std::vector 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,
- double longitude,
- double altitude){
-/** convert the current position coordination system from cartetion system (gps) to sperical system (Buzz) type **/
-
- neighbor_pos[0] = 01;
- neighbor_pos[1] = sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
- neighbor_pos[2] = atan(longitude/latitude);
- neighbor_pos[3] = atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
-fprintf(stdout, "%.15f :distance value\n", neighbor_pos[1]);
-fprintf(stdout, "%.15f :elevation\n", neighbor_pos[2]);
-fprintf(stdout, "%.15f :azimuth\n", neighbor_pos[3]);
-
+void cvt_spherical_coordinates(){
+double latitude,longitude,altitude;
+for(int i=0;ilatitude-cur_pos[0]);
-double longitude =(msg->longitude-cur_pos[1]);
-double altitude =(msg->altitude-cur_pos[2]);
-
- ROS_INFO("I heard neighbour latitude: [%15f]", latitude);
- ROS_INFO("I heard neighbour longitude: [%15f]", longitude);
- ROS_INFO("I heard neighbour altitude: [%15f]", altitude);
-
-cvt_spherical_coordinates(latitude,longitude,altitude);
-//neighbour_location_handler( distance, azimuth, elevation, 01);
-
+/*obtain the neighbours position*/
+int i=0;
+/*clear all the previous neighbour position*/
+pos_vect.clear();
+for (std::vector::const_iterator it = msg->pos_neigh.begin(); it != msg->pos_neigh.end(); ++it){
+sensor_msgs::NavSatFix cur_neigh = *it;
+sensor_msgs::NavSatStatus stats = cur_neigh.status;
+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("[Debug]I heard neighbour: %d from latitude: [%15f]",pos_vect[i].id, pos_vect[i].x);
+ 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);
+i++;
+}
+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());
diff --git a/src/test.bzz b/src/test.bzz
index b555800..b19ff10 100644
--- a/src/test.bzz
+++ b/src/test.bzz
@@ -1,10 +1,10 @@
# Executed once at init time.
function init() {
-if(id == 0) {
+#if(id == 0) {
# Source robot
- mydist = 0.
- }
- else {
+# mydist = 0.
+# }
+ # 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)
-#if(a==1){ uav_takeoff()
-# a=0
-# print("Buzz: take off")
-# }
-#else {uav_land()
-# a=1
-# print("Buzz: land")
-# }
+print("Distance to source:", mydist, neighbors.count())
}