From 29145bdafd2883e3090dc91386303ec25e9777c6 Mon Sep 17 00:00:00 2001 From: Vivek Shankar Varadharajan Date: Sun, 2 Oct 2016 17:40:01 -0400 Subject: [PATCH] Neighbour array addition commit --- CMakeLists.txt | 23 +++++---- msg/neighbour_pos.msg | 2 + package.xml | 5 +- src/buzz_utility.cpp | 16 ++++--- src/buzz_utility.h | 11 ++++- src/out.basm | 105 ++++++++++++++++++++---------------------- src/out.bdbg | Bin 4352 -> 4012 bytes src/out.bo | Bin 354 -> 354 bytes src/rosbuzz.cpp | 71 ++++++++++++++-------------- src/test.bzz | 21 +++------ 10 files changed, 130 insertions(+), 124 deletions(-) create mode 100644 msg/neighbour_pos.msg 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 0f2dffd5663bcc37033ce59757234eedc8e70c00..5d37dfee5651d2746655d515477855da9dcad691 100644 GIT binary patch literal 4012 zcmb`KsZRqz6o-d|asJbHrR?XA>h<8h>iJbCPk$PeO6b}tkTJs;(;(xP>f*Y#1!OD;ssppS? zIqs+~tjj$yy7l}NDHyhE`#@Fgy0%kaZ5c}f{a9uU^f`TGE`6ZO3d=DHGM)r-JTr#b zd$#McD&Vrl7!KHv7v_=z8Lw2=p=-^)vF*pYfXfDBxS-3MfXh2E%zo_wb8Iq~d62Qi z7#Wc9A>i`K7`s9AOy|#xPE{Iiid)3A)4>V-{q@1sGk7F$d;2p}JOG8v_{$ zfgDKz#wl|#XFJ>Hqn~n^_iQuRj}$S?*~RqMHpdxrF=t&{#yK&}nxg$k3%Cq2MilJn og@DT?W0*6QZBNIT3wr(-XAJb6eZ^c*#x-N0-whMY1^q_;1)k1JKL7v# literal 4352 zcmb`KyE6k(6o=O%9*IZ1f}xNoVuoam3=NHjhK7blB$>>}WXMc5VkUozhK5F?q0wk) zXlOJV8Vwo^4Sw0|nK|t_xwpwS=eytU+`D&gVhxtnZPiA?`;A-nPGL7|mviN8-p-^- z`P^Rez_FcT#x53|^nRt{K9{miDW0xW;szLZm^LMl;Suy|Ta}D^)R9M*k}(aB zL9e!5$#`HM{Yu8H0gp%KF`)FAV;+7b;|X;gUTuq#G0!~ul#B(&(5}lf=B9nxru0~3 z9#cxjivf>UFnYcDq2vzf`LSdm%Q9oclpb%Wqb!X|#)<)tRWOFU`F%={cb26|$@oAW zy(ds|hjeW}QAOuDeP^VMH3R)vXAC+=q#j=^i}ozsFyOJt7}Q&7mTv=Dei$Q!=jxDp zY#GRs1Y^85y@f}2NIkYu7_F0$0%NjH#v$tIebs*Fe!-x9G3@zB`|*c5y8qC(O3FAg zkmZ;$w0EgytV_LFd|=S|MQ5V4A15q}_8xS~7}|5pZy-y6F|_xfGi0If8{N63Jqge3i?y)fQpz~eYj|gMTD6?F%EZRL5HQ+J97zt&T8*6j^8_D! z!Fb>o9b!?$$A7eDUc5)LoUh%JGl}htZZDMjxemh6zyiP9N8xy3YKhY zDkaL){=g6qOp?pmcDbZ@gn-}c52clB#pjvggW_k!M@6fMnec}f;CtE18L^Rb>OwB4 zZ@-jY-OkiIG2})rsf%hfr_NlRcWw5%*U42iv^t*#T5~$R(WK5#3sWq&v>Umj{u{p6 N>z;zH*ZIyE^9SsAIbZ+) literal 354 zcmYjKTT%is5FEq^Dn9tiRtX0n&OBcb&sJg|E1^)SA4f)_L05TxQ0@HH6(GZDKIZ z)*$8>ozE0)k}aEZ3@2j3dp!dxz8AV02z?a#S?H5cBa{Y=kMtZs)8z%oSAZAM)%(vs z{ryq#5L)v!aK;L__yx@6pyoTF5>}}a!8hQVN5EMzbbme;=S`x@dd;`Rkd#rEg@mqY UB9qe8CG=GfuyyZ9W380>10G5 #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()) }