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

Out messages Implemenation Correction to Payload64 array commit

This commit is contained in:
Vivek Shankar Varadharajan 2016-10-05 23:51:44 -04:00
parent 29145bdafd
commit ec4b2a6958
7 changed files with 133 additions and 123 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

80
src/buzz_utility.cpp
View File

@ -16,7 +16,9 @@
#include <sys/socket.h> #include <sys/socket.h>
#include <pthread.h> #include <pthread.h>
#include <vector> #include <vector>
#include <iostream>
#include <bitset>
using namespace std;
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -24,7 +26,7 @@ static buzzvm_t VM = 0;
static char* BO_FNAME = 0; static char* BO_FNAME = 0;
static uint8_t* BO_BUF = 0; static uint8_t* BO_BUF = 0;
static buzzdebug_t DBG_INFO = 0; static buzzdebug_t DBG_INFO = 0;
static int MSG_SIZE = 8; static int MSG_SIZE = 32;
static int TCP_LIST_STREAM = -1; static int TCP_LIST_STREAM = -1;
static int TCP_COMM_STREAM = -1; static int TCP_COMM_STREAM = -1;
static uint8_t* STREAM_SEND_BUF = NULL; static uint8_t* STREAM_SEND_BUF = NULL;
@ -34,9 +36,8 @@ static uint8_t* STREAM_SEND_BUF = NULL;
/****************************************/ /****************************************/
void neighbour_pos_callback(std::vector<pos_struct> pos_vect){
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++){ for(int i=0;i<pos_vect.size();i++){
@ -46,6 +47,25 @@ for(int i=0;i<pos_vect.size();i++){
pos_vect[i].y, pos_vect[i].y,
pos_vect[i].z); pos_vect[i].z);
} }
}
uint16_t* u64_cvt_u16(uint64_t u64){
uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF;
uint32_t int32_2 = (u64 & 0xFFFFFFFF00000000 ) >> 32;
out[0] = int32_1 & 0xFFFF;
out[1] = (int32_1 & (0xFFFF0000) ) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000) ) >> 16;
//cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
void in_msg_process(unsigned long int payload[]){
printf("inside payload");
/* 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 */
@ -73,14 +93,15 @@ buzzvm_process_inmsgs(VM);
} }
unsigned long int* out_msg_process(){ uint64_t* out_msg_process(){
// buzzvm_process_outmsgs(VM); // buzzvm_process_outmsgs(VM);
buzzvm_process_outmsgs(VM); buzzvm_process_outmsgs(VM);
unsigned long int* buff_send =(unsigned long int*)malloc(MSG_SIZE); uint8_t* buff_send =(uint8_t*)malloc(MSG_SIZE);
memset(buff_send, 0, MSG_SIZE); memset(buff_send, 0, MSG_SIZE);
ssize_t tot = sizeof(uint16_t); ssize_t tot = sizeof(uint16_t);
/* Send robot id */ /* Send robot id */
*(uint16_t*)buff_send = VM->robot; *(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
tot += sizeof(uint16_t);
/* Send messages from FIFO */ /* Send messages from FIFO */
do { do {
/* Are there more messages? */ /* Are there more messages? */
@ -91,22 +112,59 @@ unsigned long int* out_msg_process(){
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t) if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
> >
MSG_SIZE) { MSG_SIZE) {
printf("buzz payload does not fit");
buzzmsg_payload_destroy(&m); buzzmsg_payload_destroy(&m);
break; break;
} }
/* Add message length to data buffer */ /* Add message length to data buffer */
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m); *(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t); tot += sizeof(uint16_t);
/* Add payload to data buffer */ /* Add payload to data buffer */
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m)); memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m); tot += buzzmsg_payload_size(m);
//fprintf(stderr, "[DEBUG] coppied = %u\n",
// *(uint64_t*)(buff_send+tot));
/* Get rid of message */ /* Get rid of message */
buzzoutmsg_queue_next(VM->outmsgs); buzzoutmsg_queue_next(VM->outmsgs);
buzzmsg_payload_destroy(&m); buzzmsg_payload_destroy(&m);
} while(1); } while(1);
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 */ /* Send message */
return buff_send; return payload_64;
} }
/****************************************/ /****************************************/
@ -325,3 +383,5 @@ int buzz_script_done() {
/****************************************/ /****************************************/
/****************************************/ /****************************************/

8
src/buzz_utility.h
View File

@ -1,7 +1,7 @@
#ifndef BUZZ_UTILITY_H #ifndef BUZZ_UTILITY_H
#define BUZZ_UTILITY_H #define BUZZ_UTILITY_H
#include <vector> #include <vector>
#include <stdint.h>
struct pos_struct struct pos_struct
{ {
int id; int id;
@ -9,12 +9,14 @@ struct pos_struct
pos_struct(int id,double x,double y,double z):id(id),x(x),y(y),z(z){}; pos_struct(int id,double x,double y,double z):id(id),x(x),y(y),z(z){};
}; };
uint16_t* u64_cvt_u16(uint64_t u64);
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[], std::vector<pos_struct> pos_vect); void neighbour_pos_callback(std::vector<pos_struct> pos_vect);
void in_msg_process(unsigned long int payload[]);
unsigned long int* out_msg_process(); uint64_t* out_msg_process();
extern int buzz_script_set(const char* bo_filename, extern int buzz_script_set(const char* bo_filename,
const char* bdbg_filename); const char* bdbg_filename);

93
src/out.basm
View File

@ -1,33 +1,22 @@
!16 !5
'init 'init
'mydist 'i
'neighbors
'listen
'dist_to_source
'math
'min
'get
'distance
'step 'step
'broadcast
'print
'Distance to source:
'count
'reset 'reset
'destroy 'destroy
pushs 0 pushs 0
pushcn @__label_1 pushcn @__label_1
gstore gstore
pushs 9 pushs 2
pushcn @__label_2
gstore
pushs 3
pushcn @__label_3 pushcn @__label_3
gstore gstore
pushs 14 pushs 4
pushcn @__label_4 pushcn @__label_4
gstore gstore
pushs 15
pushcn @__label_5
gstore
nop nop
@__label_0 @__label_0
@ -36,70 +25,16 @@
done done
@__label_1 @__label_1
pushs 1 |9,11,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushs 1 |3,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushf 1000. |9,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz pushi 0 |3,2,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gstore |9,18,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz gstore |3,3,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
pushs 2 |11,13,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |4,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
gload |11,13,/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 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
ret0 |18,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_2 @__label_2
pushs 1 |13,15,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |21,1,/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 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 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 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 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 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
pushi 2 |15,51,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
callc |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
@__label_3 @__label_3
pushs 2 |22,9,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |25,1,/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 @__label_4
ret0 |30,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz ret0 |29,1,/home/vivek/catkin_ws/src/rosbuzz/src/test.bzz
@__label_5
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.

44
src/rosbuzz.cpp
View File

@ -24,8 +24,8 @@
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#include <signal.h> #include <signal.h>
#include <ostream>
using namespace std;
std::vector<pos_struct> pos_vect; // vector of struct to store neighbours position 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];
@ -69,10 +69,11 @@ 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].x=sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
pos_vect[i].y=atan(longitude/latitude); pos_vect[i].y=atan(longitude/latitude);
pos_vect[i].z=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);
ROS_INFO("[Debug] Converted for neighbour : %d radius : [%15f]",pos_vect[i].id, pos_vect[i].x); // 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 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); // ROS_INFO("[Debug] Converted for neighbour : %d elevation : [%15f]",pos_vect[i].id, pos_vect[i].z);
} }
neighbour_pos_callback(pos_vect);
} }
/*battery status callback*/ /*battery status callback*/
@ -98,7 +99,8 @@ int i = 0;
message_obt[i] = *it; message_obt[i] = *it;
i++; i++;
} }
in_msg_process(message_obt, pos_vect); in_msg_process(message_obt);
} }
@ -114,9 +116,9 @@ for (std::vector<sensor_msgs::NavSatFix>::const_iterator it = msg->pos_neigh.beg
sensor_msgs::NavSatFix cur_neigh = *it; sensor_msgs::NavSatFix cur_neigh = *it;
sensor_msgs::NavSatStatus stats = cur_neigh.status; 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]))); 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 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 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); // ROS_INFO("[Debug]I heard neighbour: %d from altitude: [%15f]",pos_vect[i].id, pos_vect[i].z);
i++; i++;
} }
cvt_spherical_coordinates(); cvt_spherical_coordinates();
@ -258,12 +260,28 @@ int main(int argc, char **argv)
if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); } if (mav_client.call(cmd_srv)){ ROS_INFO("Reply: %ld", (long int)cmd_srv.response.success); }
else{ ROS_ERROR("Failed to call service 'djicmd'"); } else{ ROS_ERROR("Failed to call service 'djicmd'"); }
/*Prepare Pay load to be sent*/ /*Prepare Pay load to be sent*/
unsigned long int* payload_out_ptr= out_msg_process(); uint64_t* payload_out_ptr= out_msg_process();
uint16_t* out = u64_cvt_u16(payload_out_ptr[0]);
mavros_msgs::Mavlink payload_out; mavros_msgs::Mavlink payload_out;
payload_out.payload64.push_back(*payload_out_ptr); for(int i=0;i<out[0];i++){
/*publish prepared messages in respective topic*/ payload_out.payload64.push_back(payload_out_ptr[i]);
payload_pub.publish(payload_out); }
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;
/*run once*/ /*run once*/
ros::spinOnce(); ros::spinOnce();
/*sleep for the mentioned loop rate*/ /*sleep for the mentioned loop rate*/

29
src/test.bzz
View File

@ -1,26 +1,21 @@
# Executed once at init time. # Executed once at init time.
function init() { function init() {
#if(id == 0) { i=0
# Source robot
# mydist = 0.
# }
# else {
# Other robots
mydist = 1000.
# Listen to other robots' distances
neighbors.listen("dist_to_source",
function(value_id, value, robot_id) {
mydist = math.min(
mydist,
neighbors.get(robot_id).distance + value)
})
# }
} }
# Executed at each time step. # Executed at each time step.
function step() { function step() {
neighbors.broadcast("dist_to_source", mydist) #s = swarm.create(10)
print("Distance to source:", mydist, neighbors.count()) #s.join()
#if(i==1){
#s.leave()
#i=0
#}
#else{
#i=1
#}
#s.select(uav_takeoff)
#print("Distance to source:", mydist, neighbors.count())
} }