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
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 ##
#############

2
msg/neighbour_pos.msg Normal file
View File

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

View File

@ -40,7 +40,8 @@
<!-- 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>

View File

@ -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;

View File

@ -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();

View File

@ -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

Binary file not shown.

Binary file not shown.

View File

@ -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());

View File

@ -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())
}