Neighbour array addition commit

This commit is contained in:
Vivek Shankar Varadharajan 2016-10-02 17:40:01 -04:00
parent 8d6efae326
commit 29145bdafd
10 changed files with 130 additions and 124 deletions

View File

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

2
msg/neighbour_pos.msg Normal file
View File

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

View File

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

View File

@ -14,8 +14,8 @@
#include <netdb.h> #include <netdb.h>
#include <sys/types.h> #include <sys/types.h>
#include <sys/socket.h> #include <sys/socket.h>
#include <pthread.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 */ /* Reset neighbor information */
buzzneighbors_reset(VM); buzzneighbors_reset(VM);
/* Get robot id and update neighbor information */ /* Get robot id and update neighbor information */
for(int i=0;i<pos_vect.size();i++){
buzzneighbors_add(VM, 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 */ /* Go through messages and add them to the FIFO */
for(size_t i = 0; i < sizeof(payload)/sizeof(payload[0]); ++i) { for(size_t i = 0; i < sizeof(payload)/sizeof(payload[0]); ++i) {
/* Copy packet into temporary buffer */ /* Copy packet into temporary buffer */
long unsigned int* pl = (long unsigned int*) &payload[i]; long unsigned int* pl = (long unsigned int*) &payload[i];
size_t tot = 0; 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 */ /* Go through the messages until there's nothing else to read */
uint16_t unMsgSize; uint16_t unMsgSize;

View File

@ -1,9 +1,18 @@
#ifndef BUZZ_UTILITY_H #ifndef BUZZ_UTILITY_H
#define 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, extern int buzz_listen(const char* type,
int msg_size); 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(); unsigned long int* out_msg_process();

View File

@ -1,6 +1,5 @@
!16 !16
'init 'init
'id
'mydist 'mydist
'neighbors 'neighbors
'listen 'listen
@ -9,24 +8,25 @@
'min 'min
'get 'get
'distance 'distance
'a
'step 'step
'broadcast 'broadcast
'print 'print
'Distance to source:
'count
'reset 'reset
'destroy 'destroy
pushs 0 pushs 0
pushcn @__label_1 pushcn @__label_1
gstore gstore
pushs 11 pushs 9
pushcn @__label_5 pushcn @__label_3
gstore gstore
pushs 14 pushs 14
pushcn @__label_6 pushcn @__label_4
gstore gstore
pushs 15 pushs 15
pushcn @__label_7 pushcn @__label_5
gstore gstore
nop nop
@ -36,49 +36,35 @@
done done
@__label_1 @__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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 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 gstore |15,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
ret0 |16,7,/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 @__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

Binary file not shown.

Binary file not shown.

View File

@ -13,6 +13,8 @@
#include "mavros_msgs/State.h" #include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/BatteryStatus.h"
#include "mavros_msgs/Mavlink.h" #include "mavros_msgs/Mavlink.h"
#include "sensor_msgs/NavSatStatus.h"
#include "rosbuzz/neighbour_pos.h"
#include <sstream> #include <sstream>
#include <buzz/buzzasm.h> #include <buzz/buzzasm.h>
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
@ -24,10 +26,9 @@
#include <signal.h> #include <signal.h>
std::vector<pos_struct> pos_vect; // vector of struct to store neighbours position
static int done = 0; static int done = 0;
static double cur_pos[3]; static double cur_pos[3];
double neighbor_pos[4];
uint64_t payload; uint64_t payload;
/** /**
@ -59,19 +60,19 @@ cur_pos [2] =altitude;
} }
/*convert from catresian to spherical coordinate system callback */ /*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*/ /*battery status callback*/
@ -97,28 +98,30 @@ int i = 0;
message_obt[i] = *it; message_obt[i] = *it;
i++; i++;
} }
in_msg_process(message_obt, neighbor_pos); in_msg_process(message_obt, pos_vect);
} }
/*neighbours position call back */ /*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 oldcmdID=0;
int rc_cmd; int rc_cmd;
bool rc_callback(mavros_msgs::CommandInt::Request &req, bool rc_callback(mavros_msgs::CommandInt::Request &req,
@ -143,13 +146,13 @@ if(req.command==oldcmdID)
res.success = true; res.success = true;
break; break;
case mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH: 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_cmd=mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
rc_call(rc_cmd); rc_call(rc_cmd);
res.success = true; res.success = true;
break; break;
case mavros_msgs::CommandCode::NAV_WAYPOINT: 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]; double rc_goto[3];
rc_goto[0]=req.param1; rc_goto[0]=req.param1;
rc_goto[1]=req.param2; 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.param1 = goto_pos[0];
cmd_srv.request.param2 = goto_pos[1]; cmd_srv.request.param2 = goto_pos[1];
cmd_srv.request.param3 = goto_pos[2]; 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*/ /*prepare commands for takeoff,land and gohome*/
//char tmp[20]; //char tmp[20];
//sprintf(tmp,"%i",getcmd()); //sprintf(tmp,"%i",getcmd());

View File

@ -1,10 +1,10 @@
# Executed once at init time. # Executed once at init time.
function init() { function init() {
if(id == 0) { #if(id == 0) {
# Source robot # Source robot
mydist = 0. # mydist = 0.
} # }
else { # else {
# Other robots # Other robots
mydist = 1000. mydist = 1000.
# Listen to other robots' distances # Listen to other robots' distances
@ -14,22 +14,13 @@ if(id == 0) {
mydist, mydist,
neighbors.get(robot_id).distance + value) neighbors.get(robot_id).distance + value)
}) })
} # }
a=1
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
neighbors.broadcast("dist_to_source", mydist) 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")
# }
} }