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

Messaging payload integration commit

This commit is contained in:
Vivek 2016-09-24 18:23:52 -04:00
parent f8dc591efc
commit 5aceb59236
5 changed files with 165 additions and 455 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

397
src/buzz_utility.cpp
View File

@ -24,243 +24,90 @@ 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 = -1; static int MSG_SIZE = 8;
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;
#define TCP_LIST_STREAM_PORT "24580"
/* Pointer to a function that sends a message on the stream */
static void (*STREAM_SEND)() = NULL;
/* PThread handle to manage incoming messages */
static pthread_t INCOMING_MSG_THREAD;
/****************************************/
/****************************************/
/* PThread mutex to manage the list of incoming packets */
static pthread_mutex_t INCOMING_PACKET_MUTEX;
/* List of packets received over the stream */
struct incoming_packet_s {
/* Id of the message sender */
int id;
/* Payload */
uint8_t* payload;
/* Next message */
struct incoming_packet_s* next;
};
/* The list of incoming packets */
static struct incoming_packet_s* PACKETS_FIRST = NULL;
static struct incoming_packet_s* PACKETS_LAST = NULL;
/*
void incoming_packet_add(int id,
const uint8_t* pl) {
/* Create packet
struct incoming_packet_s* p =
(struct incoming_packet_s*)malloc(sizeof(struct incoming_packet_s));
/* Fill in the data
p->id = id;
p->payload = malloc(MSG_SIZE - sizeof(uint16_t));
memcpy(p->payload, pl, MSG_SIZE - sizeof(uint16_t));
p->next = NULL;
/* Lock mutex
pthread_mutex_lock(&INCOMING_PACKET_MUTEX);
/* Add as last to list
if(PACKETS_FIRST != NULL)
PACKETS_LAST->next = p;
else
PACKETS_FIRST = p;
PACKETS_LAST = p;
/* Unlock mutex
pthread_mutex_unlock(&INCOMING_PACKET_MUTEX);
/* fprintf(stderr, "[DEBUG] Added packet from %d\n", id);
}*/
/****************************************/ /****************************************/
void neighbour_location_handler(double distance, double azimuth, double elevation, int robot_id){
void in_msg_process(unsigned long int payload[],double neighbour[]){
/* Reset neighbor information */
buzzneighbors_reset(VM); buzzneighbors_reset(VM);
buzzneighbors_add(VM, robot_id, distance, azimuth, elevation); /* Get robot id and update neighbor information */
// buzzinmsg_queue_append( buzzneighbors_add(VM,
// VM->inmsgs, neighbour[0],
// buzzmsg_payload_frombuffer(pl + tot, msgsz)); neighbour[1],
/* Process messages */ neighbour[2],
//buzzvm_process_inmsgs(VM); neighbour[3]);
/* 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]);
/* 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);
/* Append message to the Buzz input message queue */
if(unMsgSize > 0 && unMsgSize <= MSG_SIZE - tot) {
buzzinmsg_queue_append(VM->inmsgs,
buzzmsg_payload_frombuffer(pl +tot, unMsgSize));
tot += unMsgSize;
}
}while(MSG_SIZE - tot > sizeof(uint16_t) && unMsgSize > 0);
}
/* Process messages */
buzzvm_process_inmsgs(VM);
}
unsigned long int* out_msg_process(){
// buzzvm_process_outmsgs(VM);
buzzvm_process_outmsgs(VM);
unsigned long int* buff_send =(unsigned long int*)malloc(MSG_SIZE);
memset(buff_send, 0, MSG_SIZE);
ssize_t tot = sizeof(uint16_t);
/* Send robot id */
*(uint16_t*)buff_send = VM->robot;
/* Send messages from FIFO */
do {
/* Are there more messages? */
if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
/* Get first message */
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs);
/* Make sure the next message fits the data buffer */
if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
>
MSG_SIZE) {
buzzmsg_payload_destroy(&m);
break;
}
/* Add message length to data buffer */
*(uint16_t*)(buff_send + tot) = (uint16_t)buzzmsg_payload_size(m);
tot += sizeof(uint16_t);
/* Add payload to data buffer */
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m);
/* Get rid of message */
buzzoutmsg_queue_next(VM->outmsgs);
buzzmsg_payload_destroy(&m);
} while(1);
/* Send message */
return buff_send;
} }
/****************************************/ /****************************************/
/*void* buzz_stream_incoming_thread_tcp(void* args) {
/* Create buffer for message
uint8_t* buf = calloc(MSG_SIZE, 1);
/* Tot bytes left to receive, received up to now, and received at a
* specific call of recv()
ssize_t left, tot, cur;
while(1) {
/* Initialize left byte count
left = MSG_SIZE;
tot = 0;
while(left > 0) {
cur = recv(TCP_COMM_STREAM, buf + tot, left, 0);
/* fprintf(stderr, "[DEBUG] Received %zd bytes", cur);
if(cur < 0) {
fprintf(stderr, "Error receiving data: %s\n", strerror(errno));
free(buf);
return NULL;
}
if(cur == 0) {
fprintf(stderr, "Connection closed by peer\n");
free(buf);
return NULL;
}
left -= cur;
tot += cur;
/* fprintf(stderr, ", %zd left, %zd tot\n", left, tot);
}
/* Done receiving data, add packet to list
incoming_packet_add(*(uint16_t*)buf,
buf + sizeof(uint16_t));
}
}
void buzz_stream_send_tcp() {
/* Tot bytes left to send, sent up to now, and sent at a specific
* call of send()
ssize_t left, tot, cur;
/* Initialize left byte count
left = MSG_SIZE;
tot = 0;
while(left > 0) {
cur = send(TCP_COMM_STREAM, STREAM_SEND_BUF + tot, left, 0);
/* fprintf(stderr, "[DEBUG] Sent %zd bytes", cur);
if(cur < 0) {
fprintf(stderr, "Error receiving data: %s\n", strerror(errno));
exit(1);
}
if(cur == 0) {
fprintf(stderr, "Connection closed by peer\n");
exit(1);
}
left -= cur;
tot += cur;
/* fprintf(stderr, ", %zd left, %zd tot\n", left, tot);
}
}
/****************************************/
/****************************************/
/*
int buzz_listen_tcp() {
/* Used to store the return value of the network function calls
int retval;
/* Get information on the available interfaces
struct addrinfo hints, *ifaceinfo;
memset(&hints, 0, sizeof(hints));
hints.ai_family = AF_INET; /* Only IPv4 is accepted
hints.ai_socktype = SOCK_STREAM; /* TCP socket
hints.ai_flags = AI_PASSIVE; /* Necessary for bind() later on
retval = getaddrinfo(NULL,
TCP_LIST_STREAM_PORT,
&hints,
&ifaceinfo);
if(retval != 0) {
fprintf(stderr, "Error getting local address information: %s\n",
gai_strerror(retval));
return 0;
}
/* Bind on the first interface available
TCP_LIST_STREAM = -1;
struct addrinfo* iface = NULL;
for(iface = ifaceinfo;
(iface != NULL) && (TCP_LIST_STREAM == -1);
iface = iface->ai_next) {
TCP_LIST_STREAM = socket(iface->ai_family,
iface->ai_socktype,
iface->ai_protocol);
if(TCP_LIST_STREAM > 0) {
int true = 1;
if((setsockopt(TCP_LIST_STREAM,
SOL_SOCKET,
SO_REUSEADDR,
&true,
sizeof(true)) != -1)
&&
(bind(TCP_LIST_STREAM,
iface->ai_addr,
iface->ai_addrlen) == -1)) {
close(TCP_LIST_STREAM);
TCP_LIST_STREAM = -1;
}
}
}
freeaddrinfo(ifaceinfo);
if(TCP_LIST_STREAM == -1) {
fprintf(stderr, "Can't bind socket to any interface\n");
return 0;
}
/* Listen on the socket
fprintf(stdout, "Listening on port " TCP_LIST_STREAM_PORT "...\n");
if(listen(TCP_LIST_STREAM, 1) == -1) {
close(TCP_LIST_STREAM);
TCP_LIST_STREAM = -1;
fprintf(stderr, "Can't listen on the socket: %s\n",
strerror(errno));
return 0;
}
/* Accept incoming connection
struct sockaddr addr;
socklen_t addrlen = sizeof(addr);
TCP_COMM_STREAM = accept(TCP_LIST_STREAM, &addr, &addrlen);
if(TCP_COMM_STREAM == -1) {
close(TCP_LIST_STREAM);
TCP_LIST_STREAM = -1;
fprintf(stderr, "Error accepting connection: %s\n",
strerror(errno));
return 0;
}
fprintf(stdout, "Accepted connection from %s\n",
inet_ntoa(((struct sockaddr_in*)(&addr))->sin_addr));
/* Ready to communicate through TCP
STREAM_SEND = buzz_stream_send_tcp;
STREAM_SEND_BUF = (uint8_t*)malloc(MSG_SIZE);
if(pthread_create(&INCOMING_MSG_THREAD, NULL, &buzz_stream_incoming_thread_tcp, NULL) != 0) {
fprintf(stderr, "Can't create thread: %s\n", strerror(errno));
close(TCP_COMM_STREAM);
TCP_COMM_STREAM = -1;
return 0;
}
return 1;
}*/
int buzz_listen_bt() {
return 0;
}
int buzz_listen(const char* type,
int msg_size) {
/* Set the message size
MSG_SIZE = msg_size;
/* Create the mutex
if(pthread_mutex_init(&INCOMING_PACKET_MUTEX, NULL) != 0) {
fprintf(stderr, "Error initializing the incoming packet mutex: %s\n",
strerror(errno));
return 0;
}
/* Listen to connections
if(strcmp(type, "tcp") == 0)
return buzz_listen_tcp();
else if(strcmp(type, "bt") == 0)
return buzz_listen_bt();
return 0;*/
}
/****************************************/
/****************************************/ /****************************************/
static const char* buzz_error_info() { static const char* buzz_error_info() {
@ -308,6 +155,7 @@ static int buzz_register_hooks() {
} }
/****************************************/ /****************************************/
/****************************************/ /****************************************/
int buzz_script_set(const char* bo_filename, int buzz_script_set(const char* bo_filename,
@ -415,61 +263,6 @@ void check_swarm_members(const void* key, void* data, void* params) {
void buzz_script_step() { void buzz_script_step() {
/*
* Process incoming messages
*/
/* Reset neighbor information */
/* Lock mutex */
/* fprintf(stderr, "[DEBUG] Processing incoming packets...\n"); */
//pthread_mutex_lock(&INCOMING_PACKET_MUTEX);
/* Go through messages and add them to the FIFO */
//struct incoming_packet_s* n;
//while(PACKETS_FIRST) {
/* Save next packet */
// n = PACKETS_FIRST->next;
/* Update Buzz neighbors information */
// buzzneighbors_add(VM, PACKETS_FIRST->id, 0.0, 0.0, 0.0);
/* Go through the payload and extract the messages */
// uint8_t* pl = PACKETS_FIRST->payload;
//size_t tot = 0;
uint16_t msgsz;
/* fprintf(stderr, "[DEBUG] Processing packet %p from %d\n", */
/* PACKETS_FIRST, */
/* PACKETS_FIRST->id); */
/* fprintf(stderr, "[DEBUG] recv sz = %u\n", */
/* *(uint16_t*)pl); */
//do {
/* Get payload size */
//msgsz = *(uint16_t*)(pl + tot);
//tot += sizeof(uint16_t);
/* fprintf(stderr, "[DEBUG] msg size = %u, tot = %zu\n", msgsz, tot); */
/* Make sure the message payload can be read */
// if(msgsz > 0 && msgsz <= MSG_SIZE - tot) {
/* Append message to the Buzz input message queue */
// buzzinmsg_queue_append(
// VM->inmsgs,
// buzzmsg_payload_frombuffer(pl + tot, msgsz));
// tot += msgsz;
/* fprintf(stderr, "[DEBUG] appended message, tot = %zu\n", tot); */
// }
//}
//while(MSG_SIZE - tot > sizeof(uint16_t) && msgsz > 0);
/* Erase packet */
/* fprintf(stderr, "[DEBUG] Done processing packet %p from %d\n", */
/* PACKETS_FIRST, */
/* PACKETS_FIRST->id); */
//free(PACKETS_FIRST->payload);
//free(PACKETS_FIRST);
/* Go to next packet */
// PACKETS_FIRST = n;
//}
/* The packet list is now empty */
//PACKETS_LAST = NULL;
/* Unlock mutex */
//pthread_mutex_unlock(&INCOMING_PACKET_MUTEX);
/* fprintf(stderr, "[DEBUG] Done processing incoming packets.\n"); */
/* Process messages */
//buzzvm_process_inmsgs(VM); //buzzvm_process_inmsgs(VM);
/* /*
* Update sensors * Update sensors
@ -485,48 +278,6 @@ void buzz_script_step() {
buzz_error_info()); buzz_error_info());
buzzvm_dump(VM); buzzvm_dump(VM);
} }
/*
* Broadcast messages
*/
/* Prepare buffer */
// memset(STREAM_SEND_BUF, 0, MSG_SIZE);
// *(uint16_t*)STREAM_SEND_BUF = VM->robot;
// ssize_t tot = sizeof(uint16_t);
//do {
/* Are there more messages? */
// if(buzzoutmsg_queue_isempty(VM->outmsgs)) break;
/* Get first message */
// buzzmsg_payload_t m = buzzoutmsg_queue_first(VM->outmsgs);
/* Make sure it fits the data buffer */
// if(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)
// >
// MSG_SIZE) {
// buzzmsg_payload_destroy(&m);
// break;
// }
/* Add message length to data buffer */
/* fprintf(stderr, "[DEBUG] send before sz = %u\n", */
/* *(uint16_t*)(STREAM_SEND_BUF + 2)); */
// *(uint16_t*)(STREAM_SEND_BUF + tot) = (uint16_t)buzzmsg_payload_size(m);
// tot += sizeof(uint16_t);
/* fprintf(stderr, "[DEBUG] send after sz = %u\n", */
/* *(uint16_t*)(STREAM_SEND_BUF + 2)); */
/* Add payload to data buffer */
// memcpy(STREAM_SEND_BUF + tot, m->data, buzzmsg_payload_size(m));
// tot += buzzmsg_payload_size(m);
/* Get rid of message */
// buzzoutmsg_queue_next(VM->outmsgs);
// buzzmsg_payload_destroy(&m);
// } while(1);
/* fprintf(stderr, "[DEBUG] send id = %u, sz = %u\n", */
/* *(uint16_t*)STREAM_SEND_BUF, */
/* *(uint16_t*)(STREAM_SEND_BUF + 2)); */
/* Send messages */
// buzzvm_process_outmsgs(VM);
// STREAM_SEND();
/* Sleep */
// usleep(100000);
/* Print swarm */ /* Print swarm */
buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot); buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
/* Check swarm state */ /* Check swarm state */
@ -544,11 +295,9 @@ void buzz_script_step() {
/****************************************/ /****************************************/
void buzz_script_destroy() { void buzz_script_destroy() {
/* Cancel thread */
pthread_cancel(INCOMING_MSG_THREAD);
pthread_join(INCOMING_MSG_THREAD, NULL);
/* Get rid of stream buffer */ /* Get rid of stream buffer */
free(STREAM_SEND_BUF); //free(STREAM_SEND_BUF);
/* Get rid of virtual machine */ /* Get rid of virtual machine */
if(VM) { if(VM) {
if(VM->state != BUZZVM_STATE_READY) { if(VM->state != BUZZVM_STATE_READY) {

5
src/buzz_utility.h
View File

@ -3,11 +3,12 @@
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[], double negighbour[]);
unsigned long int* 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);
extern void neighbour_location_handler(double distance, double azimuth, double elevation, int robot_id);
extern void buzz_script_step(); extern void buzz_script_step();
extern void buzz_script_destroy(); extern void buzz_script_destroy();

32
src/buzzuav_closures.cpp
View File

@ -3,12 +3,10 @@
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "uav_utility.h" #include "uav_utility.h"
#include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandCode.h"
// %Tag(ROS_HEADER)%
#include "ros/ros.h" #include "ros/ros.h"
// %EndTag(ROS_HEADER)%
double goto_pos[3]; double goto_pos[3];
float batt[3];
int cur_cmd; int cur_cmd;
/****************************************/ /****************************************/
/****************************************/ /****************************************/
@ -21,31 +19,29 @@ int buzzros_print(buzzvm_t vm) {
buzzvm_pop(vm); buzzvm_pop(vm);
switch(o->o.type) { switch(o->o.type) {
case BUZZTYPE_NIL: case BUZZTYPE_NIL:
//fprintf(stdout, "[nil]");
ROS_INFO("BUZZ - [nil]"); ROS_INFO("BUZZ - [nil]");
break; break;
case BUZZTYPE_INT: case BUZZTYPE_INT:
fprintf(stdout, "%d", o->i.value); ROS_INFO("%d", o->i.value);
//fprintf(stdout, "%d", o->i.value);
break; break;
case BUZZTYPE_FLOAT: case BUZZTYPE_FLOAT:
fprintf(stdout, "%f", o->f.value); ROS_INFO("%f", o->f.value);
break; break;
case BUZZTYPE_TABLE: case BUZZTYPE_TABLE:
//fprintf(stdout, "[table with %d elems]", (buzzdict_size(o->t.value))); ROS_INFO("[table with %d elems]", (buzzdict_size(o->t.value)));
break; break;
case BUZZTYPE_CLOSURE: case BUZZTYPE_CLOSURE:
if(o->c.value.isnative) if(o->c.value.isnative)
ROS_INFO("BUZZ - [nil]"); ROS_INFO("[n-closure @%d]", o->c.value.ref);
//fprintf(stdout, "[n-closure @%d]", o->c.value.ref);
else else
ROS_INFO("BUZZ - [nil]"); ROS_INFO("[c-closure @%d]", o->c.value.ref);
//fprintf(stdout, "[c-closure @%d]", o->c.value.ref);
break; break;
case BUZZTYPE_STRING: case BUZZTYPE_STRING:
fprintf(stdout, "%s", o->s.value.str); ROS_INFO("%s", o->s.value.str);
break; break;
case BUZZTYPE_USERDATA: case BUZZTYPE_USERDATA:
//fprintf(stdout, "[userdata @%p]", o->u.value); ROS_INFO("[userdata @%p]", o->u.value);
break; break;
default: default:
break; break;
@ -66,16 +62,18 @@ int buzzuav_goto(buzzvm_t vm) {
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT); buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
goto_pos[0]=buzzvm_stack_at(vm, 1)->f.value * 10.0f; goto_pos[2]=buzzvm_stack_at(vm, 1)->f.value * 10.0f;
goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f; goto_pos[1]=buzzvm_stack_at(vm, 2)->f.value * 10.0f;
goto_pos[2]=buzzvm_stack_at(vm, 3)->f.value * 10.0f; goto_pos[0]=buzzvm_stack_at(vm, 3)->f.value * 10.0f;
return buzzvm_ret0(vm); return buzzvm_ret0(vm);
} }
/******************************/
double* getgoto(){ double* getgoto(){
return goto_pos; return goto_pos;
} }
/******************************/
int getcmd(){ int getcmd(){
return cur_cmd; return cur_cmd;
} }
@ -99,7 +97,6 @@ int buzzuav_gohome(buzzvm_t vm) {
} }
/****************************************/ /****************************************/
float batt[3];
void set_battery(float voltage,float current,float remaining){ void set_battery(float voltage,float current,float remaining){
batt[0]=voltage; batt[0]=voltage;
batt[1]=current; batt[1]=current;
@ -109,7 +106,6 @@ batt[2]=remaining;
int buzzuav_update_battery(buzzvm_t vm) { int buzzuav_update_battery(buzzvm_t vm) {
static char BATTERY_BUF[256]; static char BATTERY_BUF[256];
// kh4_battery_status(BATTERY_BUF, DSPIC);
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1)); buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm); buzzvm_pusht(vm);
buzzvm_dup(vm); buzzvm_dup(vm);

26
src/buzzuav_closures.h
View File

@ -5,37 +5,33 @@
/* /*
* prextern int() function in Buzz * prextern int() function in Buzz
* This function is used to print data from buzz
* The command to use in Buzz is buzzros_print takes any available datatype in Buzz
*/ */
int buzzros_print(buzzvm_t vm); int buzzros_print(buzzvm_t vm);
/* /*
* set_wheels(ls,rs) function in Buzz * buzzuav_goto(latitude,longitude,altitude) function in Buzz
* Sets the wheel speeds to ls (left) and rs (right) * commands the UAV to go to a position supplied
* speeds are expressed in cm/s
*/ */
int buzzuav_goto(buzzvm_t vm); int buzzuav_goto(buzzvm_t vm);
/* Returns the current command from local variable*/
int getcmd(); int getcmd();
/* sets the battery state to the local variable
*/
void set_battery(float voltage,float current,float remaining); void set_battery(float voltage,float current,float remaining);
/*retuns the current go to position from local variable*/
double* getgoto(); double* getgoto();
/* /*
* set_leds(r,g,b) function in Buzz * Commands the UAV to takeoff
* Sets the color of the 3 leds to (r,g,b)
* speeds are expressed in cm/s
*/ */
int buzzuav_takeoff(buzzvm_t vm); int buzzuav_takeoff(buzzvm_t vm);
/* /* Commands the UAV to land
* set_leds(r,g,b) function in Buzz
* Sets the color of the 3 leds to (r,g,b)
* speeds are expressed in cm/s
*/ */
int buzzuav_land(buzzvm_t vm); int buzzuav_land(buzzvm_t vm);
/* /* Command the UAV to go to home location
* set_leds(r,g,b) function in Buzz
* Sets the color of the 3 leds to (r,g,b)
* speeds are expressed in cm/s
*/ */
int buzzuav_gohome(buzzvm_t vm); int buzzuav_gohome(buzzvm_t vm);

162
src/rosbuzz.cpp
View File

@ -3,47 +3,36 @@
* Header * Header
*/ */
// %Tag(FULLTEXT)%
// %Tag(ROS_HEADER)%
#include "ros/ros.h" #include "ros/ros.h"
// %EndTag(ROS_HEADER)%
#include "sensor_msgs/NavSatFix.h" #include "sensor_msgs/NavSatFix.h"
#include "mavros_msgs/GlobalPositionTarget.h" #include "mavros_msgs/GlobalPositionTarget.h"
#include "mavros_msgs/CommandCode.h" #include "mavros_msgs/CommandCode.h"
#include "mavros_msgs/State.h" #include "mavros_msgs/State.h"
#include "mavros_msgs/BatteryStatus.h" #include "mavros_msgs/BatteryStatus.h"
// %Tag(MSG_HEADER)% #include "mavros_msgs/Mavlink.h"
#include "std_msgs/String.h"
// %EndTag(MSG_HEADER)%
#include "std_msgs/Float64.h"
#include <sstream> #include <sstream>
#include <buzz/buzzasm.h> #include <buzz/buzzasm.h>
#include "buzzuav_closures.h" #include "buzzuav_closures.h"
#include "buzz_utility.h" #include "buzz_utility.h"
//extern "C" {
#include "uav_utility.h" #include "uav_utility.h"
//}
#include <stdlib.h> #include <stdlib.h>
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#include <signal.h> #include <signal.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/wait.h>
static int done = 0; static int done = 0;
static double cur_pos [3]; static double cur_pos[3];
double distance; double neighbor_pos[4];
double azimuth; uint64_t payload;
double elevation;
/** /**
* This tutorial demonstrates simple sending of messages over the ROS system. * This program implements Buzz node in ros.
*/ */
/* /*
* Print usage information * Print usage information
*/ */
void usage(const char* path, int status) { void usage(const char* path, int status) {
@ -56,6 +45,7 @@ void usage(const char* path, int status) {
fprintf(stderr, " file.bdbg The Buzz debug file\n\n"); fprintf(stderr, " file.bdbg The Buzz debug file\n\n");
exit(status); exit(status);
} }
/*Set the current position of the robot callback*/
void set_cur_pos(double latitude, void set_cur_pos(double latitude,
double longitude, double longitude,
double altitude){ double altitude){
@ -63,44 +53,57 @@ void set_cur_pos(double latitude,
cur_pos [0] =latitude; cur_pos [0] =latitude;
cur_pos [1] =longitude; cur_pos [1] =longitude;
cur_pos [2] =altitude; cur_pos [2] =altitude;
} }
/*convert from catresian to spherical coordinate system callback */
void cvt_spherical_coordinates(double latitude, void cvt_spherical_coordinates(double latitude,
double longitude, double longitude,
double altitude){ double altitude){
/** convert the current position coordination system from cartetion system (gps) to sperical system (Buzz) type **/ /** convert the current position coordination system from cartetion system (gps) to sperical system (Buzz) type **/
distance = sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
elevation = atan(longitude/latitude); neighbor_pos[0] = 01;
azimuth = atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude); neighbor_pos[1] = sqrt(pow(latitude,2.0)+pow(longitude,2.0)+pow(altitude,2.0));
fprintf(stdout, "%.15f :distance value\n", distance); neighbor_pos[2] = atan(longitude/latitude);
fprintf(stdout, "%.15f :elevation\n", elevation); neighbor_pos[3] = atan((sqrt(pow(latitude,2.0)+pow(longitude,2.0)))/altitude);
fprintf(stdout, "%.15f :azimuth\n", azimuth); 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]);
} }
// %Tag(CALLBACK)% /*battery status callback*/
void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg) void battery(const mavros_msgs::BatteryStatus::ConstPtr& msg)
{ {
set_battery(msg->voltage,msg->current,msg->remaining); set_battery(msg->voltage,msg->current,msg->remaining);
} }
/*current position callback*/
void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) void current_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
{ {
/*ROS_INFO("I heard current latitude: [%15f]", msg->latitude);
ROS_INFO("I heard current longitude: [%15f]", msg->longitude);
ROS_INFO("I heard current altitude: [%15f]", msg->altitude);*/
/*obtain the current posituion of the robot*/
//sperical* = cvt_spherical_coordinates(msg->latitude,msg->longitude,msg->altitude);
set_cur_pos(msg->latitude,msg->longitude,msg->altitude); set_cur_pos(msg->latitude,msg->longitude,msg->altitude);
} }
// %EndTag(CALLBACK)%
// %Tag(CALLBACK)% /*payload callback*/
void payload_obt(const mavros_msgs::Mavlink::ConstPtr& msg)
{
long unsigned int 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;
i++;
}
in_msg_process(message_obt, neighbor_pos);
}
/*neighbours position call back */
void neighbour_pos(const sensor_msgs::NavSatFix::ConstPtr& msg) void neighbour_pos(const sensor_msgs::NavSatFix::ConstPtr& msg)
{ {
/*obtain the current position of the robot*/ /*obtain the neigbours position*/
double latitude =(msg->latitude-cur_pos[0]); double latitude =(msg->latitude-cur_pos[0]);
double longitude =(msg->longitude-cur_pos[1]); double longitude =(msg->longitude-cur_pos[1]);
@ -111,92 +114,61 @@ double altitude =(msg->altitude-cur_pos[2]);
ROS_INFO("I heard neighbour altitude: [%15f]", altitude); ROS_INFO("I heard neighbour altitude: [%15f]", altitude);
cvt_spherical_coordinates(latitude,longitude,altitude); cvt_spherical_coordinates(latitude,longitude,altitude);
neighbour_location_handler( distance, azimuth, elevation, 01); //neighbour_location_handler( distance, azimuth, elevation, 01);
} }
// %EndTag(CALLBACK)%
/*controlc handler callback*/
static void ctrlc_handler(int sig) { static void ctrlc_handler(int sig) {
done = 1; done = 1;
} }
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
/*Compile the buzz code .bzz to .bo*/
system("bzzparse /home/vivek/catkin_ws/src/rosbuzz/src/test.bzz /home/vivek/catkin_ws/src/rosbuzz/src/out.basm"); system("bzzparse /home/vivek/catkin_ws/src/rosbuzz/src/test.bzz /home/vivek/catkin_ws/src/rosbuzz/src/out.basm");
system("bzzasm /home/vivek/catkin_ws/src/rosbuzz/src/out.basm /home/vivek/catkin_ws/src/rosbuzz/src/out.bo /home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"); system("bzzasm /home/vivek/catkin_ws/src/rosbuzz/src/out.basm /home/vivek/catkin_ws/src/rosbuzz/src/out.bo /home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg");
// %Tag(INIT)% /*initiate rosBuzz*/
ros::init(argc, argv, "rosBuzz"); ros::init(argc, argv, "rosBuzz");
// %EndTag(INIT)%
// %Tag(NODEHANDLE)%
/*Create node Handler*/
ros::NodeHandle n_c; ros::NodeHandle n_c;
// ros::NodeHandle n_n;
// %EndTag(NODEHANDLE)%
// %Tag(SUBSCRIBER)% /*subscribers*/
ros::Subscriber current_position = n_c.subscribe("current_pos", 1000, current_pos); ros::Subscriber current_position_sub = n_c.subscribe("current_pos", 1000, current_pos);
ros::Subscriber neighbour_position = n_c.subscribe("neighbour_pos", 1000, neighbour_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 battery_sub = n_c.subscribe("battery_state", 1000, battery);
// %EndTag(SUBSCRIBER)%
// %Tag(PUBLISHER)% ros::Subscriber payload_sub = n_c.subscribe("pay_load_in", 1000, payload_obt);
/*publishers*/
ros::Publisher goto_pub = n_c.advertise<mavros_msgs::GlobalPositionTarget>("go_to", 1000); 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 cmds_pub = n_c.advertise<mavros_msgs::State>("newstate", 1000);
// %EndTag(PUBLISHER)%
// %Tag(LOOP_RATE)% ros::Publisher payload_pub = n_c.advertise<mavros_msgs::Mavlink>("pay_load_out", 1000);
/*loop rate of ros*/
ros::Rate loop_rate(1); ros::Rate loop_rate(1);
// %EndTag(LOOP_RATE)%
// Buzz stuff
//
//
/* Parse command line */
/* if(argc != 5) usage(argv[0], 0); */
/* The stream type */
/* char* stream = argv[1];
if(strcmp(stream, "tcp") != 0 &&
strcmp(stream, "bt") != 0) {
fprintf(stderr, "%s: unknown stream type '%s'\n", argv[0], stream);
usage(argv[0], 0);
}
/* The message size */
/*char* endptr;
int msg_sz = strtol(argv[2], &endptr, 10);
if(endptr == argv[2] || *endptr != '\0') {
fprintf(stderr, "%s: can't parse '%s' into a number\n", argv[0], argv[2]);
return 0;
}
if(msg_sz <= 0) {
fprintf(stderr, "%s: invalid value %d for message size\n", argv[0], msg_sz);
return 0;
} */
/* The bytecode filename */ /* The bytecode filename */
char* bcfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bo"; //argv[1]; char* bcfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bo"; //argv[1];
/* The debugging information file name */ /* The debugging information file name */
char* dbgfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2]; char* dbgfname = "/home/vivek/catkin_ws/src/rosbuzz/src/out.bdbg"; //argv[2];
/* Wait for connection */
//if(!buzz_listen(stream, msg_sz)) return 1;
/* Set CTRL-C handler */ /* Set CTRL-C handler */
signal(SIGTERM, ctrlc_handler); signal(SIGTERM, ctrlc_handler);
signal(SIGINT, ctrlc_handler); signal(SIGINT, ctrlc_handler);
/* Initialize the robot */
//kh4_setup();
/* Set the Buzz bytecode */ /* Set the Buzz bytecode */
if(buzz_script_set(bcfname, dbgfname)) { if(buzz_script_set(bcfname, dbgfname)) {
@ -210,7 +182,6 @@ int main(int argc, char **argv)
// while(!done && !buzz_script_done())
/* Main loop */ /* Main loop */
buzz_script_step(); buzz_script_step();
@ -218,36 +189,34 @@ int main(int argc, char **argv)
// buzz_script_destroy(); // buzz_script_destroy();
// %Tag(FILL_MESSAGE)% /*prepare the goto publish message */
mavros_msgs::GlobalPositionTarget goto_set; mavros_msgs::GlobalPositionTarget goto_set;
double* goto_pos = getgoto(); double* goto_pos = getgoto();
goto_set.latitude = goto_pos[0]; goto_set.latitude = goto_pos[0];
goto_set.longitude=goto_pos[1]; goto_set.longitude=goto_pos[1];
goto_set.altitude=goto_pos[2]; goto_set.altitude=goto_pos[2];
/*prepare commands for takeoff,land and gohome*/
mavros_msgs::State cmds_set; mavros_msgs::State cmds_set;
char tmp[20]; char tmp[20];
sprintf(tmp,"%i",getcmd()); sprintf(tmp,"%i",getcmd());
cmds_set.mode = tmp; cmds_set.mode = tmp;
/*Prepare Pay load to be sent*/
// %EndTag(FILL_MESSAGE)% unsigned long int* payload_out_ptr= out_msg_process();
mavros_msgs::Mavlink payload_out;
payload_out.payload64.push_back(*payload_out_ptr);
// %Tag(PUBLISH)% /*publish prepared messages in respective topic*/
goto_pub.publish(goto_set); goto_pub.publish(goto_set);
cmds_pub.publish(cmds_set); cmds_pub.publish(cmds_set);
// %EndTag(PUBLISH)% payload_pub.publish(payload_out);
/*run once*/
// %Tag(SPINONCE)%
ros::spinOnce(); ros::spinOnce();
// %EndTag(SPINONCE)% /*sleep for the mentioned loop rate*/
// %Tag(RATE_SLEEP)%
loop_rate.sleep(); loop_rate.sleep();
// %EndTag(RATE_SLEEP)%
++count; ++count;
} }
/* Stop the robot */ /* Stop the robot */
uav_done(); uav_done();
@ -256,6 +225,5 @@ cmds_set.mode = tmp;
return 0; return 0;
} }
// %EndTag(FULLTEXT)%