Pay load out Implementation corrections and testing, and change of neighbour array implementaion correction and testing commit
This commit is contained in:
Vivek Shankar Varadharajan
parent
ec4b2a6958
commit
b83c6eae0c
|
|
@ -18,6 +18,7 @@
|
|||
#include <vector>
|
||||
#include <iostream>
|
||||
#include <bitset>
|
||||
#include <map>
|
||||
using namespace std;
|
||||
/****************************************/
|
||||
/****************************************/
|
||||
|
|
@ -36,17 +37,19 @@ static uint8_t* STREAM_SEND_BUF = NULL;
|
|||
|
||||
/****************************************/
|
||||
|
||||
void neighbour_pos_callback(std::vector<pos_struct> pos_vect){
|
||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map){
|
||||
/* 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,
|
||||
pos_vect[i].id,
|
||||
pos_vect[i].x,
|
||||
pos_vect[i].y,
|
||||
pos_vect[i].z);
|
||||
}
|
||||
map< int, Pos_struct >::iterator it;
|
||||
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
|
||||
buzzneighbors_add(VM,
|
||||
it->first,
|
||||
(it->second).x,
|
||||
(it->second).y,
|
||||
(it->second).z);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -63,38 +66,38 @@ return out;
|
|||
|
||||
}
|
||||
|
||||
void in_msg_process(unsigned long int payload[]){
|
||||
void in_msg_process(uint64_t* payload){
|
||||
|
||||
printf("inside payload");
|
||||
/* Go through messages and add them to the FIFO */
|
||||
for(size_t i = 0; i < sizeof(payload)/sizeof(payload[0]); ++i) {
|
||||
uint16_t* data= u64_cvt_u16((uint64_t)payload[0]);
|
||||
uint16_t size=data[0]*sizeof(uint64_t);
|
||||
delete[] data;
|
||||
uint8_t* pl =(uint8_t*)malloc(size);
|
||||
memset(pl, 0,size);
|
||||
/* 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]);
|
||||
memcpy(pl, payload ,size);
|
||||
|
||||
size_t tot = sizeof(uint32_t);
|
||||
|
||||
/* Go through the messages until there's nothing else to read */
|
||||
uint16_t unMsgSize;
|
||||
do {
|
||||
/* Get payload size */
|
||||
unMsgSize = *(uint16_t*)(pl + tot);
|
||||
tot += sizeof(uint16_t);
|
||||
tot += sizeof(uint16_t);
|
||||
/* Append message to the Buzz input message queue */
|
||||
if(unMsgSize > 0 && unMsgSize <= MSG_SIZE - tot) {
|
||||
if(unMsgSize > 0 && unMsgSize <= size*sizeof(uint64_t) - tot) {
|
||||
buzzinmsg_queue_append(VM->inmsgs,
|
||||
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
|
||||
tot += unMsgSize;
|
||||
|
||||
}
|
||||
}while(MSG_SIZE - tot > sizeof(uint16_t) && unMsgSize > 0);
|
||||
}
|
||||
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
|
||||
|
||||
/* Process messages */
|
||||
buzzvm_process_inmsgs(VM);
|
||||
|
||||
}
|
||||
|
||||
uint64_t* out_msg_process(){
|
||||
// buzzvm_process_outmsgs(VM);
|
||||
buzzvm_process_outmsgs(VM);
|
||||
uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE);
|
||||
memset(buff_send, 0, MSG_SIZE);
|
||||
|
|
@ -112,7 +115,6 @@ uint64_t* out_msg_process(){
|
|||
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
|
||||
>
|
||||
MSG_SIZE) {
|
||||
printf("buzz payload does not fit");
|
||||
buzzmsg_payload_destroy(&m);
|
||||
break;
|
||||
}
|
||||
|
|
@ -125,8 +127,6 @@ uint64_t* out_msg_process(){
|
|||
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
|
||||
tot += buzzmsg_payload_size(m);
|
||||
|
||||
//fprintf(stderr, "[DEBUG] coppied = %u\n",
|
||||
// *(uint64_t*)(buff_send+tot));
|
||||
/* Get rid of message */
|
||||
buzzoutmsg_queue_next(VM->outmsgs);
|
||||
buzzmsg_payload_destroy(&m);
|
||||
|
|
@ -134,35 +134,12 @@ uint64_t* out_msg_process(){
|
|||
|
||||
int total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
|
||||
*(uint16_t*)buff_send = (uint16_t) total_size;
|
||||
|
||||
for(int i=0;i<16;i++){
|
||||
uint16_t* temp_buff =(uint16_t*)(buff_send+(i*2));
|
||||
cout << "buff_send address :" << (uint16_t*) buff_send+(i*2);
|
||||
uint16_t *p = reinterpret_cast<uint16_t*>(temp_buff);
|
||||
cout <<" buff_send data " << *p <<endl;
|
||||
}
|
||||
//uint16_t temp_buff1[4]; //=32;
|
||||
|
||||
|
||||
uint64_t* payload_64 = new uint64_t[total_size];
|
||||
//payload_64= temp_buff1 | (temp_buff1 << 16) | (temp_buff1 << 32) | (temp_buff1 << 48) ;
|
||||
|
||||
memcpy((void*)payload_64, (void*)buff_send, total_size*sizeof(uint64_t));
|
||||
//cout << "payload64 "<<payload_64[0];
|
||||
// std::string binary = std::bitset<64>(payload_64[0]).to_string(); //to binary
|
||||
// std::cout<<"payload_64 binary " <<binary<<"\n";
|
||||
|
||||
// unsigned long decimal = std::bitset<64>(binary).to_ulong();
|
||||
// std::cout<< " decimal "<<decimal<<"\n";
|
||||
|
||||
/*
|
||||
uint16_t* out;
|
||||
for(int dumb =0;dumb<total_size;dumb++){
|
||||
out = u64_cvt_u16(payload_64[dumb]);
|
||||
for(int i=0; i<4;i++){
|
||||
cout<< " cvt value"<<out[i]<<endl;
|
||||
}
|
||||
}
|
||||
delete[] out;
|
||||
/*
|
||||
/* Send message */
|
||||
return payload_64;
|
||||
}
|
||||
|
|
@ -225,7 +202,8 @@ int buzz_script_set(const char* bo_filename,
|
|||
gethostname(hstnm, 30);
|
||||
/* Make numeric id from hostname */
|
||||
/* NOTE: here we assume that the hostname is in the format Knn */
|
||||
int id = strtol(hstnm + 1, NULL, 10);
|
||||
int id =0; // strtol(hstnm + 1, NULL, 10);
|
||||
cout << " Robot ID" << id<< endl;
|
||||
/* Reset the Buzz VM */
|
||||
if(VM) buzzvm_destroy(&VM);
|
||||
VM = buzzvm_new(id);
|
||||
|
|
@ -341,14 +319,14 @@ void buzz_script_step() {
|
|||
/* Print swarm */
|
||||
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
||||
/* Check swarm state */
|
||||
int status = 1;
|
||||
/* int status = 1;
|
||||
buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
||||
if(status == 1 &&
|
||||
buzzdict_size(VM->swarmmembers) < 9)
|
||||
status = 2;
|
||||
buzzvm_pushs(VM, buzzvm_string_register(VM, "swarm_status", 1));
|
||||
buzzvm_pushi(VM, status);
|
||||
buzzvm_gstore(VM);
|
||||
buzzvm_gstore(VM);*/
|
||||
}
|
||||
|
||||
/****************************************/
|
||||
|
|
|
|||
|
|
@ -2,19 +2,21 @@
|
|||
#define BUZZ_UTILITY_H
|
||||
#include <vector>
|
||||
#include <stdint.h>
|
||||
#include <map>
|
||||
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){};
|
||||
pos_struct(double x,double y,double z):x(x),y(y),z(z){};
|
||||
pos_struct(){}
|
||||
};
|
||||
typedef struct pos_struct Pos_struct ;
|
||||
|
||||
uint16_t* u64_cvt_u16(uint64_t u64);
|
||||
|
||||
extern int buzz_listen(const char* type,
|
||||
int msg_size);
|
||||
void neighbour_pos_callback(std::vector<pos_struct> pos_vect);
|
||||
void in_msg_process(unsigned long int payload[]);
|
||||
void neighbour_pos_callback(std::map< int, Pos_struct> neighbours_pos_map);
|
||||
void in_msg_process(uint64_t* payload);
|
||||
|
||||
uint64_t* out_msg_process();
|
||||
|
||||
|
|
|
|||
93
src/out.basm
93
src/out.basm
|
|
@ -1,22 +1,40 @@
|
|||
!5
|
||||
!23
|
||||
'init
|
||||
'i
|
||||
'v
|
||||
'stigmergy
|
||||
'create
|
||||
'step
|
||||
's
|
||||
'swarm
|
||||
'join
|
||||
'neighbors
|
||||
'foreach
|
||||
'print
|
||||
'robot
|
||||
':
|
||||
'distance =
|
||||
'distance
|
||||
',
|
||||
'azimuth =
|
||||
'azimuth
|
||||
'elevation =
|
||||
'elevation
|
||||
'reset
|
||||
'destroy
|
||||
|
||||
pushs 0
|
||||
pushcn @__label_1
|
||||
gstore
|
||||
pushs 2
|
||||
pushs 5
|
||||
pushcn @__label_2
|
||||
gstore
|
||||
pushs 3
|
||||
pushcn @__label_3
|
||||
gstore
|
||||
pushs 4
|
||||
pushs 21
|
||||
pushcn @__label_4
|
||||
gstore
|
||||
pushs 22
|
||||
pushcn @__label_5
|
||||
gstore
|
||||
nop
|
||||
|
||||
@__label_0
|
||||
|
|
@ -28,13 +46,68 @@
|
|||
pushs 1 |3,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |4,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 2 |4,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 3 |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |4,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 4 |4,14,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |4,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |4,21,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |4,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |6,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_2
|
||||
ret0 |21,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 6 |10,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 7 |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |10,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 4 |10,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |10,16,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 10 |10,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gstore |10,20,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 6 |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |11,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 8 |11,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |11,6,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 0 |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |11,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 9 |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |13,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 10 |13,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |13,17,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushl @__label_3 |14,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 1 |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |18,42,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_3
|
||||
ret0 |25,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 11 |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
gload |15,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 12 |15,10,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 1 |15,23,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 13 |15,25,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 14 |16,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 2 |16,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 15 |16,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |16,37,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 16 |16,39,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 17 |17,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 2 |17,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 18 |17,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |17,36,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 16 |17,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 19 |18,8,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
lload 2 |18,28,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushs 20 |18,29,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
tget |18,38,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
pushi 11 |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
callc |18,40,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |18,41,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_4
|
||||
ret0 |29,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
ret0 |34,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
|
||||
|
||||
@__label_5
|
||||
ret0 |38,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.
151
src/rosbuzz.cpp
151
src/rosbuzz.cpp
|
|
@ -25,19 +25,32 @@
|
|||
#include <math.h>
|
||||
#include <signal.h>
|
||||
#include <ostream>
|
||||
#include <map>
|
||||
using namespace std;
|
||||
std::vector<pos_struct> pos_vect; // vector of struct to store neighbours position
|
||||
|
||||
static int done = 0;
|
||||
static double cur_pos[3];
|
||||
uint64_t payload;
|
||||
|
||||
static uint64_t payload;
|
||||
static std::map< int, Pos_struct> neighbours_pos_map;
|
||||
static int timer_step=0;
|
||||
/**
|
||||
* This program implements Buzz node in ros.
|
||||
*/
|
||||
/*
|
||||
void maintain_pos(int tim_step){
|
||||
if(timer_step >=10){
|
||||
neighbours_pos_map.clear();
|
||||
timer_step=0;
|
||||
}
|
||||
|
||||
* Print usage information
|
||||
*/
|
||||
}
|
||||
void neighbours_pos_maintain(int id, Pos_struct pos_arr ){
|
||||
map< int, Pos_struct >::iterator it = neighbours_pos_map.find(id);
|
||||
if(it!=neighbours_pos_map.end())
|
||||
neighbours_pos_map.erase(it);
|
||||
neighbours_pos_map.insert(make_pair(id, pos_arr));
|
||||
}
|
||||
|
||||
/*usage*/
|
||||
void usage(const char* path, int status) {
|
||||
fprintf(stderr, "Usage:\n");
|
||||
fprintf(stderr, "\t%s <stream> <msg_size> <file.bo> <file.bdb>\n\n", path);
|
||||
|
|
@ -60,20 +73,16 @@ cur_pos [2] =altitude;
|
|||
}
|
||||
|
||||
/*convert from catresian to spherical coordinate system callback */
|
||||
void cvt_spherical_coordinates(){
|
||||
double* cvt_spherical_coordinates(double neighbours_pos_payload []){
|
||||
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);
|
||||
}
|
||||
neighbour_pos_callback(pos_vect);
|
||||
//for(int i=0;i<pos_vect.size();i++){
|
||||
latitude=neighbours_pos_payload[0];
|
||||
longitude = neighbours_pos_payload[1];
|
||||
altitude=neighbours_pos_payload[2];
|
||||
neighbours_pos_payload[0]=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
|
||||
neighbours_pos_payload[1]=atan(longitude/latitude);
|
||||
neighbours_pos_payload[2]=atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
|
||||
return neighbours_pos_payload;
|
||||
}
|
||||
|
||||
/*battery status callback*/
|
||||
|
|
@ -88,41 +97,41 @@ void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
|
|||
set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
|
||||
}
|
||||
|
||||
/*payload callback*/
|
||||
/*payload callback callback*/
|
||||
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
|
||||
{
|
||||
long unsigned int message_obt[(msg->payload64.end())-(msg->payload64.begin())];
|
||||
uint64_t message_obt[(msg->payload64.end())-(msg->payload64.begin())];
|
||||
int i = 0;
|
||||
|
||||
// print all the remaining numbers
|
||||
for(std::vector<long unsigned int>::const_iterator it = msg->payload64.begin(); it != msg->payload64.end(); ++it)
|
||||
{
|
||||
message_obt[i] = *it;
|
||||
message_obt[i] =(uint64_t) *it;
|
||||
cout<<" obtaind message "<<message_obt[i]<<endl;
|
||||
i++;
|
||||
}
|
||||
in_msg_process(message_obt);
|
||||
|
||||
|
||||
}
|
||||
double neighbours_pos_payload[3];
|
||||
memcpy(neighbours_pos_payload, message_obt, 3*sizeof(uint64_t));
|
||||
//cout<<"neighbours pos recived "<<" lat " << (double)neighbours_pos_payload[0]<<" long "<<(double)neighbours_pos_payload[1] <<" alti "<<(double)neighbours_pos_payload[2]<<endl;
|
||||
|
||||
/*neighbours position call back */
|
||||
|
||||
void neighbour_pos(const rosbuzz::neighbour_pos::ConstPtr& msg)
|
||||
{
|
||||
/*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();
|
||||
for(i=0;i<3;i++){
|
||||
neighbours_pos_payload[i]=neighbours_pos_payload[i]-cur_pos[i];
|
||||
}
|
||||
double* cvt_neighbours_pos_payload = cvt_spherical_coordinates(neighbours_pos_payload);
|
||||
//cout<<" neighbours pos cvt " <<" lat " << cvt_neighbours_pos_payload[0]<<" long "<<cvt_neighbours_pos_payload[1] <<" alti "<<cvt_neighbours_pos_payload[2]<<endl;
|
||||
|
||||
uint16_t* out = u64_cvt_u16((uint64_t)*(message_obt+3));
|
||||
//cout << " values " <<out[0] <<" "<<(int)out[1] <<" "<<out[2] <<" "<<out[3] <<" "<<endl;
|
||||
struct pos_struct n_pos;
|
||||
n_pos.x=cvt_neighbours_pos_payload[0];
|
||||
n_pos.y=cvt_neighbours_pos_payload[1];
|
||||
n_pos.z=cvt_neighbours_pos_payload[2];
|
||||
neighbours_pos_maintain((int)out[1],n_pos);
|
||||
delete[] out;
|
||||
in_msg_process((message_obt+3));
|
||||
|
||||
}
|
||||
|
||||
|
||||
int oldcmdID=0;
|
||||
int rc_cmd;
|
||||
|
|
@ -199,19 +208,13 @@ int main(int argc, char **argv)
|
|||
/*subscribers*/
|
||||
ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
|
||||
|
||||
ros::Subscriber neighbour_position_sub = n_c.subscribe("neighbour_pos", 1000, neighbour_pos);
|
||||
|
||||
ros::Subscriber battery_sub = n_c.subscribe("battery_state", 1000, battery);
|
||||
|
||||
ros::Subscriber payload_sub = n_c.subscribe("pay_load_in", 1000, payload_obt);
|
||||
ros::Subscriber payload_sub = n_c.subscribe("payload_in", 1000, payload_obt);
|
||||
|
||||
|
||||
/*publishers*/
|
||||
//ros::Publisher goto_pub = n_c.advertise<mavros_msgs::GlobalPositionTarget>("go_to", 1000);
|
||||
|
||||
//ros::Publisher cmds_pub = n_c.advertise<mavros_msgs::State>("newstate", 1000);
|
||||
|
||||
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("pay_load_out", 1000);
|
||||
ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("payload_out", 1000);
|
||||
|
||||
/* Clients*/
|
||||
ros::ServiceClient mav_client = n_c.serviceClient<mavros_msgs::CommandInt>("djicmd");
|
||||
|
|
@ -219,6 +222,7 @@ int main(int argc, char **argv)
|
|||
/*Services*/
|
||||
ros::ServiceServer service = n_c.advertiseService("djicmd_rc", rc_callback);
|
||||
ROS_INFO("Ready to receive Mav Commands from dji RC client");
|
||||
|
||||
/*loop rate of ros*/
|
||||
ros::Rate loop_rate(1);
|
||||
|
||||
|
|
@ -236,14 +240,17 @@ int main(int argc, char **argv)
|
|||
if(buzz_script_set(bcfname, dbgfname)) {
|
||||
fprintf(stdout, "Bytecode file found and set\n");
|
||||
|
||||
// buzz setting
|
||||
|
||||
mavros_msgs::CommandInt cmd_srv;
|
||||
int count = 0;
|
||||
while (ros::ok() && !done && !buzz_script_done())
|
||||
{
|
||||
|
||||
|
||||
/*Update neighbors position inside Buzz*/
|
||||
neighbour_pos_callback(neighbours_pos_map);
|
||||
|
||||
/* Main loop */
|
||||
buzz_script_step();
|
||||
buzz_script_step();
|
||||
|
||||
/*prepare the goto publish message */
|
||||
|
||||
|
|
@ -251,34 +258,25 @@ 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);
|
||||
/*prepare commands for takeoff,land and gohome*/
|
||||
//char tmp[20];
|
||||
//sprintf(tmp,"%i",getcmd());
|
||||
|
||||
cmd_srv.request.command = getcmd();
|
||||
/* diji client call*/
|
||||
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
|
||||
else{ ROS_ERROR("Failed to call service 'djicmd'"); }
|
||||
/*Prepare Pay load to be sent*/
|
||||
uint64_t* payload_out_ptr= out_msg_process();
|
||||
uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);
|
||||
uint64_t position[3];
|
||||
memcpy(position, cur_pos, 3*sizeof(uint64_t));
|
||||
mavros_msgs::Mavlink payload_out;
|
||||
payload_out.payload64.push_back(position[0]);
|
||||
payload_out.payload64.push_back(position[1]);
|
||||
payload_out.payload64.push_back(position[2]);
|
||||
|
||||
uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);
|
||||
for(int i=0;i<out[0];i++){
|
||||
payload_out.payload64.push_back(payload_out_ptr[i]);
|
||||
}
|
||||
|
||||
for (std::vector<long unsigned int>::iterator it = payload_out.payload64.begin() ; it != payload_out.payload64.end(); ++it){
|
||||
out = u64_cvt_u16((uint64_t)*it);
|
||||
for(int i=0; i<4;i++){
|
||||
cout<< "[ROS BUZZ] cvt value"<<out[i]<<endl;
|
||||
}
|
||||
}
|
||||
// delete[] out;
|
||||
/*publish prepared messages in respective topic*/
|
||||
//char tmp[100];
|
||||
//sprintf(tmp,"%d",payload_out_ptr);
|
||||
//ROS_INFO("%p",payload_out_ptr);
|
||||
//cout << (*payload_out_ptr) <<endl;
|
||||
payload_pub.publish(payload_out);
|
||||
delete[] out;
|
||||
delete[] payload_out_ptr;
|
||||
|
|
@ -287,6 +285,15 @@ int main(int argc, char **argv)
|
|||
/*sleep for the mentioned loop rate*/
|
||||
loop_rate.sleep();
|
||||
++count;
|
||||
timer_step+=1;
|
||||
maintain_pos(timer_step);
|
||||
/* cout<<"time step"<<timer_step<<endl;
|
||||
map< int, Pos_struct >::iterator it;
|
||||
for (it=neighbours_pos_map.begin(); it!=neighbours_pos_map.end(); ++it){
|
||||
std::cout << it->first << " => " << (it->second).x <<" " << (it->second).y <<" " << (it->second).z<<endl;
|
||||
}
|
||||
*/
|
||||
|
||||
}
|
||||
/* Cleanup */
|
||||
buzz_script_destroy();
|
||||
|
|
|
|||
13
src/test.bzz
13
src/test.bzz
|
|
@ -1,12 +1,21 @@
|
|||
# Executed once at init time.
|
||||
function init() {
|
||||
i=0
|
||||
v = stigmergy.create(1)
|
||||
#v.put("a", 6)
|
||||
}
|
||||
|
||||
# Executed at each time step.
|
||||
function step() {
|
||||
#s = swarm.create(10)
|
||||
#s.join()
|
||||
s = swarm.create(10)
|
||||
s.join()
|
||||
|
||||
neighbors.foreach(
|
||||
function(rid, data) {
|
||||
print("robot ", rid, ": ",
|
||||
"distance = ", data.distance, ", ",
|
||||
"azimuth = ", data.azimuth, ", ",
|
||||
"elevation = ", data.elevation) })
|
||||
#if(i==1){
|
||||
#s.leave()
|
||||
#i=0
|
||||
|
|
|
|||
Loading…
Reference in New Issue