Spiri
/
ROSBuzz_MISTLab
13
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
CMakeLists.txt
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

3
package.xml
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>

16
src/buzz_utility.cpp
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;

11
src/buzz_utility.h
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();

105
src/out.basm
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

BIN
src/out.bdbg
View File

Binary file not shown.

BIN
src/out.bo
View File

Binary file not shown.

71
src/rosbuzz.cpp
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());

21
src/test.bzz
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())
}