Neighbour array addition commit
This commit is contained in:
Vivek Shankar Varadharajan
parent
8d6efae326
commit
29145bdafd
|
|
@ -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 ##
|
||||
#############
|
||||
|
|
|
|||
|
|
@ -0,0 +1,2 @@
|
|||
Header header
|
||||
sensor_msgs/NavSatFix[] pos_neigh
|
||||
|
|
@ -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>
|
||||
|
|
|
|||
|
|
@ -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],
|
||||
neighbour[1],
|
||||
neighbour[2],
|
||||
neighbour[3]);
|
||||
pos_vect[i].id,
|
||||
pos_vect[i].x,
|
||||
pos_vect[i].y,
|
||||
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;
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
||||
|
|
|
|||
105
src/out.basm
105
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
|
||||
|
|
|
|||
BIN
src/out.bdbg
BIN
src/out.bdbg
Binary file not shown.
BIN
src/out.bo
BIN
src/out.bo
Binary file not shown.
|
|
@ -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,
|
||||
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;i<pos_vect.size();i++){
|
||||
latitude=pos_vect[i].x;
|
||||
longitude = pos_vect[i].y;
|
||||
altitude=pos_vect[i].z;
|
||||
pos_vect[i].x=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
|
||||
pos_vect[i].y=atan(longitude/latitude);
|
||||
pos_vect[i].z=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
|
||||
ROS_INFO("[Debug] Converted for neighbour : %d radius : [%15f]",pos_vect[i].id, pos_vect[i].x);
|
||||
ROS_INFO("[Debug] Converted for neighbour : %d azimuth : [%15f]",pos_vect[i].id, pos_vect[i].y);
|
||||
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 neigbours position*/
|
||||
|
||||
double latitude =(msg->latitude-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<sensor_msgs::NavSatFix>::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());
|
||||
|
|
|
|||
21
src/test.bzz
21
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())
|
||||
|
||||
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue