Merge branch 'master' into dev

This commit is contained in:
dave 2017-12-18 19:33:01 -05:00
commit a9204ad0b0
8 changed files with 1377 additions and 1265 deletions

View File

@ -15,7 +15,7 @@ function wmat(mat, row, col, val) {
# Read from matrix
function rmat(mat, row, col) {
#log("rmat ", mat, row, col)
index = (row-1)*mat.nb_col + (col - 1)
var index = (row-1)*mat.nb_col + (col - 1)
if (mat.mat[index] == nil) {
log("Wrong matrix read index: ", row, " ", col)
return -1
@ -41,7 +41,7 @@ function getvec(t,row){
function init_test_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}}
index = 0
var index = 0
while(index<len*len){
map.mat[index]=1.0
index = index + 1
@ -56,7 +56,7 @@ function init_test_map(len){
function init_map(len){
map = {.nb_col=len, .nb_row=len, .mat={}}
index = 0
var index = 0
while(index<len*len){
map.mat[index]=1.0
index = index + 1
@ -65,8 +65,8 @@ function init_map(len){
}
function add_obstacle(pos, off, inc_trust) {
xi = math.round(pos.x)
yi = math.round(pos.y)
var xi = math.round(pos.x)
var yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0) {
#log("Add obstacle in cell: ", xi, " ", yi)
@ -79,8 +79,8 @@ function add_obstacle(pos, off, inc_trust) {
}
function remove_obstacle(pos, off, dec_trust) {
xi = math.round(pos.x)
yi = math.round(pos.y)
var xi = math.round(pos.x)
var yi = math.round(pos.y)
if(xi < map.nb_col+1 and yi < map.nb_row+1 and xi > 0 and yi > 0){
#log("Remove obstacle in cell: ", xi, " ", yi)
@ -107,14 +107,14 @@ function table_copy(t) {
}
function print_pos(t) {
ir=1
var ir=1
while(ir<=t.nb_row){
log(ir, ": ", rmat(t,ir,1), " ", rmat(t,ir,2))
ir = ir + 1
}
}
function print_map(t) {
ir=t.nb_row
var ir=t.nb_row
log("Printing a ", t.nb_row, " by ", t.nb_col, " map")
while(ir>0){
logst=string.concat("\t", string.tostring(ir), "\t:")
@ -132,7 +132,7 @@ function print_map(t) {
}
function print_map_argos(t){
ir=t.nb_row
var ir=t.nb_row
msg = string.tostring(ir)
while(ir>0){
ic=t.nb_col
@ -147,5 +147,5 @@ function print_map_argos(t){
}
ir = ir - 1
}
set_argos_map(msg)
#set_argos_map(msg)
}

View File

@ -363,8 +363,8 @@ function doesItIntersect(point,vector) {
distance = math.vec2.length(dif)
if(distance==0.0){
# Find what block we're in right now
xi = math.round(point.x) #+1
yi = math.round(point.y) #+1
var xi = math.round(point.x) #+1
var yi = math.round(point.y) #+1
if(xi!=cur_cell.x and yi!=cur_cell.y){
if(rmat(map,xi,yi) > 0.5)
return 1
@ -382,8 +382,8 @@ function doesItIntersect(point,vector) {
pos_chk = math.vec2.sub(point,math.vec2.scale(vec,range));
# Find what block we're in right now
xi = math.round(pos_chk.x) #+1
yi = math.round(pos_chk.y) #+1
var xi = math.round(pos_chk.x) #+1
var yi = math.round(pos_chk.y) #+1
#log("Grid :", pos_chk.x, " ", pos_chk.y," ", xi," ", yi, " R: ", range)
if(xi!=cur_cell.x and yi!=cur_cell.y){

View File

@ -33,46 +33,95 @@ typedef enum {
*/
int buzzros_print(buzzvm_t vm);
void setWPlist(std::string path);
/*
* buzzuav_goto(latitude,longitude,altitude) function in Buzz
* commands the UAV to go to a position supplied
* closure to move following a vector
*/
int buzz_floor(buzzvm_t vm);
int buzzuav_moveto(buzzvm_t vm);
/*
* closure to store a new GPS goal
*/
int buzzuav_storegoal(buzzvm_t vm);
/*
* closure to control the gimbal
*/
int buzzuav_setgimbal(buzzvm_t vm);
/*
* parse a csv list of waypoints
*/
void parse_gpslist();
/*
* closure to take a picture
*/
int buzzuav_takepicture(buzzvm_t vm);
/* Returns the current command from local variable*/
/*
* Returns the current command from local variable
*/
int getcmd();
/*Sets goto position from rc client*/
/*
* Sets goto position from rc client
*/
void rc_set_goto(int id, double latitude, double longitude, double altitude);
/*Sets gimbal orientation from rc client*/
/*
*Sets gimbal orientation from rc client
*/
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t);
/*sets rc requested command */
/*
* sets rc requested command
*/
void rc_call(int rc_cmd);
/* sets the battery state */
/*
* sets the battery state
*/
void set_battery(float voltage, float current, float remaining);
/*
* sets the xbee network status
*/
void set_deque_full(bool state);
void set_rssi(float value);
void set_raw_packet_loss(float value);
void set_filtered_packet_loss(float value);
void set_api_rssi(float value);
/* sets current position */
/*
* sets current position
*/
void set_currentpos(double latitude, double longitude, double altitude);
/*retuns the current go to position */
/*
* returns the current go to position
*/
double* getgoto();
/*
* returns the current Buzz state
*/
std::string getuavstate();
/*
* returns the gimbal commands
*/
float* getgimbal();
/* updates flight status*/
/*
*updates flight status
*/
void flight_status_update(uint8_t state);
/* Update neighbors table */
/*
*Update neighbors table
*/
void neighbour_pos_callback(int id, float range, float bearing, float elevation);
/*
* update neighbors from in msgs
*/
void update_neighbors(buzzvm_t vm);
/*
* closure to add a neighbor status
*/
int buzzuav_addNeiStatus(buzzvm_t vm);
/*
* returns the current array of neighbors status
*/
mavros_msgs::Mavlink get_status();
/*Flight status*/
/*
*Flight status
*/
void set_obstacle_dist(float dist[]);
/*
@ -91,7 +140,8 @@ int buzzuav_disarm(buzzvm_t vm);
*/
int buzzuav_land(buzzvm_t vm);
/* Command the UAV to go to home location
/*
* Command the UAV to go to home location
*/
int buzzuav_gohome(buzzvm_t vm);
@ -107,7 +157,9 @@ int buzzuav_update_xbee_status(buzzvm_t vm);
* Updates current position in Buzz
*/
int buzzuav_update_currentpos(buzzvm_t vm);
int buzzuav_update_targets(buzzvm_t vm);
/*
* add new target in the BVM
*/
int buzzuav_addtargetRB(buzzvm_t vm);
/*
* Updates flight status and rc command in Buzz, put it in a tabel to acess it
@ -121,10 +173,10 @@ int buzzuav_update_flight_status(buzzvm_t vm);
* Proximity and ground sensors to do !!!!
*/
int buzzuav_update_prox(buzzvm_t vm);
/*
* returns the current FC command
*/
int bzz_cmd();
int dummy_closure(buzzvm_t vm);
//#endif
}

View File

@ -99,4 +99,4 @@ References
------
* ROS and Buzz : consensus-based behaviors for heterogeneous teams. Submitted to the Internaional Conference on Robotics and Automation (September 2017). 6pgs. St-Onge, D., Shankar Varadharajan, V., Li, G., Svogor, I. and Beltrame, G. arXiv : https://arxiv.org/abs/1710.08843
* Over-The-Air Updates for Robotic Swarms. Accepted by IEEE Software (September 2017 - pending minor revision). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.
* Over-The-Air Updates for Robotic Swarms. Submitted to IEEE Software (August 2017). 8pgs. Shankar Varadharajan, V., St-Onge, D., Guß, C. and Beltrame, G.

View File

@ -24,10 +24,10 @@ std::map<int, Pos_struct> users_map;
/****************************************/
/**************************************************************************/
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
/**************************************************************************/
uint16_t* u64_cvt_u16(uint64_t u64)
/*
/ Deserializes uint64_t into 4 uint16_t, freeing out is left to the user
------------------------------------------------------------------------*/
{
uint16_t* out = new uint16_t[4];
uint32_t int32_1 = u64 & 0xFFFFFFFF;
@ -36,61 +36,66 @@ uint16_t* u64_cvt_u16(uint64_t u64)
out[1] = (int32_1 & (0xFFFF0000)) >> 16;
out[2] = int32_2 & 0xFFFF;
out[3] = (int32_2 & (0xFFFF0000)) >> 16;
// DEBUG
// cout << " values " <<out[0] <<" "<<out[1] <<" "<<out[2] <<" "<<out[3] <<" ";
return out;
}
int get_robotid()
/*
/ return this robot ID
------------------------------------------------------------------------*/
{
return Robot_id;
}
/***************************************************/
/*Appends obtained messages to buzz in message Queue*/
/***************************************************/
/*******************************************************************************************************************/
/* Message format of payload (Each slot is uint64_t) */
/* _______________________________________________________________________________________________________________ */
/*| | |*/
/*|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|*/
/*|__________________________________________________________________________|____________________________________|*/
/*******************************************************************************************************************/
void in_msg_append(uint64_t* payload)
/*
/ Appends obtained messages to buzz in message Queue
---------------------------------------------------------------------
/ Message format of payload (Each slot is uint64_t)
/ _______________________________________________________________________________________________________________
/| | |
/|Size in Uint64_t(but size is Uint16_t)|robot_id|Update msg size|Update msg|Update msgs+Buzz_msgs with size.....|
/|__________________________________________________________________________|____________________________________|
---------------------------------------------------------------------------------------------------------------------*/
{
/* Go through messages and append them to the vector */
// Go through messages and append them to the vector
uint16_t* data = u64_cvt_u16((uint64_t)payload[0]);
/*Size is at first 2 bytes*/
// Size is at first 2 bytes
uint16_t size = data[0] * sizeof(uint64_t);
delete[] data;
uint8_t* pl = (uint8_t*)malloc(size);
/* Copy packet into temporary buffer */
// Copy packet into temporary buffer
memcpy(pl, payload, size);
IN_MSG.push_back(pl);
}
void in_message_process()
/*
/ Process msgs in
---------------------------------------------------------------------------------------------------------------------*/
{
while (!IN_MSG.empty())
{
/* Go through messages and append them to the FIFO */
// Go through messages and append them to the FIFO
uint8_t* first_INmsg = (uint8_t*)IN_MSG.front();
size_t tot = 0;
/*Size is at first 2 bytes*/
// Size is at first 2 bytes
uint16_t size = (*(uint16_t*)first_INmsg) * sizeof(uint64_t);
tot += sizeof(uint16_t);
/*Decode neighbor Id*/
// Decode neighbor Id
uint16_t neigh_id = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
/* Go through the messages until there's nothing else to read */
// Go through the messages until there's nothing else to read
uint16_t unMsgSize = 0;
/*Obtain Buzz messages push it into queue*/
// Obtain Buzz messages push it into queue
do
{
/* Get payload size */
// Get payload size
unMsgSize = *(uint16_t*)(first_INmsg + tot);
tot += sizeof(uint16_t);
/* Append message to the Buzz input message queue */
// Append message to the Buzz input message queue
if (unMsgSize > 0 && unMsgSize <= size - tot)
{
buzzinmsg_queue_append(VM, neigh_id, buzzmsg_payload_frombuffer(first_INmsg + tot, unMsgSize));
@ -100,33 +105,34 @@ void in_message_process()
free(first_INmsg);
IN_MSG.erase(IN_MSG.begin());
}
/* Process messages VM call*/
// Process messages VM call*
buzzvm_process_inmsgs(VM);
}
/***************************************************/
/*Obtains messages from buzz out message Queue*/
/***************************************************/
uint64_t* obt_out_msg()
/*
/ Obtains messages from buzz out message Queue
-------------------------------------------------*/
{
/* Process out messages */
// Process out messages
buzzvm_process_outmsgs(VM);
uint8_t* buff_send = (uint8_t*)malloc(MAX_MSG_SIZE);
memset(buff_send, 0, MAX_MSG_SIZE);
/*Taking into consideration the sizes included at the end*/
// Taking into consideration the sizes included at the end
ssize_t tot = sizeof(uint16_t);
/* Send robot id */
// Send robot id
*(uint16_t*)(buff_send + tot) = (uint16_t)VM->robot;
tot += sizeof(uint16_t);
/* Send messages from FIFO */
// Send messages from FIFO
do
{
/* Are there more messages? */
// Are there more messages?
if (buzzoutmsg_queue_isempty(VM))
break;
/* Get first message */
// Get first message
buzzmsg_payload_t m = buzzoutmsg_queue_first(VM);
/* Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes */
// Make sure the next message makes the data buffer with buzz messages to be less than MAX SIZE Bytes
// DEBUG
// ROS_INFO("read size : %i", (int)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)));
if ((uint32_t)(tot + buzzmsg_payload_size(m) + sizeof(uint16_t)) > MAX_MSG_SIZE)
{
@ -134,15 +140,15 @@ uint64_t* obt_out_msg()
break;
}
/* Add message length to data buffer */
// 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 */
// Add payload to data buffer
memcpy(buff_send + tot, m->data, buzzmsg_payload_size(m));
tot += buzzmsg_payload_size(m);
/* Get rid of message */
// Get rid of message
buzzoutmsg_queue_next(VM);
buzzmsg_payload_destroy(&m);
} while (1);
@ -154,18 +160,18 @@ uint64_t* obt_out_msg()
memcpy((void*)payload_64, (void*)buff_send, total_size * sizeof(uint64_t));
free(buff_send);
/*for(int i=0;i<total_size;i++){
cout<<" payload from out msg "<<*(payload_64+i)<<endl;
}*/
/* Send message */
// DEBUG
// for(int i=0;i<total_size;i++){
// cout<<" payload from out msg "<<*(payload_64+i)<<endl;
//}
// Send message
return payload_64;
}
/****************************************/
/*Buzz script not able to load*/
/****************************************/
static const char* buzz_error_info()
/*
/ Buzz script not able to load
---------------------------------*/
{
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
char* msg;
@ -181,11 +187,10 @@ static const char* buzz_error_info()
return msg;
}
/****************************************/
/*Buzz hooks that can be used inside .bzz file*/
/****************************************/
static int buzz_register_hooks()
/*
/ Buzz hooks that can be used inside .bzz file
------------------------------------------------*/
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
@ -233,11 +238,10 @@ static int buzz_register_hooks()
return VM->state;
}
/**************************************************/
/*Register dummy Buzz hooks for test during update*/
/**************************************************/
static int testing_buzz_register_hooks()
/*
/ Register dummy Buzz hooks for test during update
---------------------------------------------------*/
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
@ -285,14 +289,14 @@ static int testing_buzz_register_hooks()
return VM->state;
}
/****************************************/
/*Sets the .bzz and .bdbg file*/
/****************************************/
int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robot_id)
/*
/ Sets the .bzz and .bdbg file
---------------------------------*/
{
ROS_INFO(" Robot ID: %i", robot_id);
Robot_id = robot_id;
/* Read bytecode from file and fill the bo buffer*/
// Read bytecode from file and fill the bo buffer
FILE* fd = fopen(bo_filename, "rb");
if (!fd)
{
@ -313,16 +317,16 @@ int buzz_script_set(const char* bo_filename, const char* bdbg_filename, int robo
}
fclose(fd);
/* Save bytecode file name */
// Save bytecode file name
BO_FNAME = strdup(bo_filename);
return buzz_update_set(BO_BUF, bdbg_filename, bcode_size);
}
/****************************************/
/*Sets a new update */
/****************************************/
int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
/*
/ Sets a new update
-----------------------*/
{
// Reset the Buzz VM
if (VM)
@ -379,10 +383,10 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
return 1;
}
/****************************************/
/*Performs a initialization test */
/****************************************/
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_size)
/*
/ Performs a initialization test
-----------------------------------*/
{
// Reset the Buzz VM
if (VM)
@ -439,38 +443,40 @@ int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t
return 1;
}
/****************************************/
/*Swarm struct*/
/****************************************/
struct buzzswarm_elem_s
/*
/ Swarm struct
----------------*/
{
buzzdarray_t swarms;
uint16_t age;
};
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
/*Step through the buzz script*/
void update_sensors()
/*
/ Update from all external inputs
-------------------------------*/
{
/* Update sensors*/
// Update sensors
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_xbee_status(VM);
buzzuav_closures::buzzuav_update_prox(VM);
buzzuav_closures::buzzuav_update_currentpos(VM);
buzzuav_closures::update_neighbors(VM);
buzzuav_closures::buzzuav_update_targets(VM);
// update_users();
buzzuav_closures::buzzuav_update_flight_status(VM);
}
void buzz_script_step()
/*
/ Step through the buzz script
-------------------------------*/
{
/*Process available messages*/
// Process available messages
in_message_process();
/*Update sensors*/
// Update sensors
update_sensors();
/* Call Buzz step() function */
// Call Buzz step() function
if (buzzvm_function_call(VM, "step", 0) != BUZZVM_STATE_READY)
{
ROS_ERROR("%s: execution terminated abnormally: %s", BO_FNAME, buzz_error_info());
@ -478,11 +484,10 @@ void buzz_script_step()
}
}
/****************************************/
/*Destroy the bvm and other resorces*/
/****************************************/
void buzz_script_destroy()
/*
/ Destroy the bvm and other related ressources
-------------------------------------*/
{
if (VM)
{
@ -499,21 +504,20 @@ void buzz_script_destroy()
ROS_INFO("Script execution stopped.");
}
/****************************************/
/****************************************/
/****************************************/
/*Execution completed*/
/****************************************/
int buzz_script_done()
/*
/ Check if the BVM execution terminated
---------------------------------------*/
{
return VM->state != BUZZVM_STATE_READY;
}
int update_step_test()
/*
/ Step test for the update mechanism
------------------------------------*/
{
/*Process available messages*/
// Process available messages
in_message_process();
buzzuav_closures::buzzuav_update_battery(VM);
buzzuav_closures::buzzuav_update_prox(VM);
@ -533,11 +537,17 @@ int update_step_test()
}
buzzvm_t get_vm()
/*
/ return the BVM
----------------*/
{
return VM;
}
void set_robot_var(int ROBOTS)
/*
/ set swarm size in the BVM
-----------------------------*/
{
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, ROBOTS);
@ -545,6 +555,9 @@ void set_robot_var(int ROBOTS)
}
int get_inmsg_size()
/*
/ get the incoming msgs size
------------------------------*/
{
return IN_MSG.size();
}

View File

@ -13,8 +13,6 @@ namespace buzzuav_closures
{
// TODO: Minimize the required global variables and put them in the header
// static const rosbzz_node::roscontroller* roscontroller_ptr;
/*forward declaration of ros controller ptr storing function*/
// void set_ros_controller_ptr(const rosbzz_node::roscontroller* roscontroller_ptrin);
static double goto_pos[3];
static double rc_goto_pos[3];
static float rc_gimbal[4];
@ -43,6 +41,9 @@ std::map<int, buzz_utility::neighbors_status> neighbors_status_map;
/****************************************/
int buzzros_print(buzzvm_t vm)
/*
/ Buzz closure to print out
----------------------------------------------------------- */
{
std::ostringstream buffer(std::ostringstream::ate);
buffer << buzz_utility::get_robotid();
@ -90,11 +91,17 @@ int buzzros_print(buzzvm_t vm)
}
void setWPlist(string path)
/*
/ set the absolute path for a csv list of waypoints
----------------------------------------------------------- */
{
WPlistname = path + "include/graphs/waypointlist.csv";
}
float constrainAngle(float x)
/*
/ Wrap the angle between -pi, pi
----------------------------------------------------------- */
{
x = fmod(x, 2 * M_PI);
if (x < 0.0)
@ -102,10 +109,10 @@ float constrainAngle(float x)
return x;
}
/*----------------------------------------/
void rb_from_gps(double nei[], double out[], double cur[])
/*
/ Compute Range and Bearing from 2 GPS set of coordinates
/----------------------------------------*/
void rb_from_gps(double nei[], double out[], double cur[])
{
double d_lon = nei[1] - cur[1];
double d_lat = nei[0] - cur[0];
@ -117,6 +124,9 @@ void rb_from_gps(double nei[], double out[], double cur[])
}
void parse_gpslist()
/*
/ parse a csv of GPS targets
/----------------------------------------*/
{
// Open file:
ROS_INFO("WP list file: %s", WPlistname.c_str());
@ -147,6 +157,7 @@ void parse_gpslist()
lat = atof(strtok(NULL, DELIMS));
alt = atoi(strtok(NULL, DELIMS));
tilt = atoi(strtok(NULL, DELIMS));
// DEBUG
// ROS_INFO("%.6f, %.6f, %i %i %i",lat, lon, alt, tilt, tid);
RB_arr.latitude = lat;
RB_arr.longitude = lon;
@ -164,15 +175,15 @@ void parse_gpslist()
fin.close();
}
/*----------------------------------------/
int buzzuav_moveto(buzzvm_t vm)
/*
/ Buzz closure to move following a 2D vector
/----------------------------------------*/
int buzzuav_moveto(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); /* dx */
buzzvm_lload(vm, 2); /* dy */
buzzvm_lload(vm, 3); /* dheight */
buzzvm_lload(vm, 1); // dx
buzzvm_lload(vm, 2); // dy
buzzvm_lload(vm, 3); //* dheight
buzzvm_type_assert(vm, 3, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 2, BUZZTYPE_FLOAT);
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
@ -182,55 +193,17 @@ int buzzuav_moveto(buzzvm_t vm)
goto_pos[0] = dx;
goto_pos[1] = dy;
goto_pos[2] = height + dh;
// DEBUG
// ROS_WARN("[%i] Buzz requested Move To: x: %.7f , y: %.7f, z: %.7f", (int)buzz_utility::get_robotid(), goto_pos[0],
// goto_pos[1], goto_pos[2]);
buzz_cmd = COMMAND_MOVETO; // TO DO what should we use
buzz_cmd = COMMAND_MOVETO; // TODO: standard mavros?
return buzzvm_ret0(vm);
}
int buzzuav_update_targets(buzzvm_t vm)
{
if (vm->state != BUZZVM_STATE_READY)
return vm->state;
buzzvm_pushs(vm, buzzvm_string_register(vm, "targets", 1));
buzzvm_pusht(vm);
buzzobj_t targettbl = buzzvm_stack_at(vm, 1);
double rb[3], tmp[3];
map<int, buzz_utility::RB_struct>::iterator it;
for (it = targets_map.begin(); it != targets_map.end(); ++it)
{
tmp[0] = (it->second).latitude;
tmp[1] = (it->second).longitude;
tmp[2] = height;
rb_from_gps(tmp, rb, cur_pos);
// ROS_WARN("----------Pushing target id %i (%f,%f)", it->first, rb[0], rb[1]);
buzzvm_push(vm, targettbl);
// When we get here, the "targets" table is on top of the stack
// ROS_INFO("Buzz_utility will save user %i.", it->first);
// Push user id
buzzvm_pushi(vm, it->first);
// Create entry table
buzzobj_t entry = buzzheap_newobj(vm->heap, BUZZTYPE_TABLE);
// Insert range
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "range", 1));
buzzvm_pushf(vm, rb[0]);
buzzvm_tput(vm);
// Insert longitude
buzzvm_push(vm, entry);
buzzvm_pushs(vm, buzzvm_string_register(vm, "bearing", 1));
buzzvm_pushf(vm, rb[1]);
buzzvm_tput(vm);
// Save entry into data table
buzzvm_push(vm, entry);
buzzvm_tput(vm);
}
buzzvm_gstore(vm);
return vm->state;
}
int buzzuav_addtargetRB(buzzvm_t vm)
/*
/ Buzz closure to add a target (goal) GPS
/----------------------------------------*/
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // longitude
@ -249,7 +222,6 @@ int buzzuav_addtargetRB(buzzvm_t vm)
rb_from_gps(tmp, rb, cur_pos);
if (fabs(rb[0]) < 100.0)
{
// printf("\tGot new user from bzz stig: %i - %f, %f\n", uid, rb[0], rb[1]);
buzz_utility::RB_struct RB_arr;
RB_arr.latitude = tmp[0];
RB_arr.longitude = tmp[1];
@ -260,6 +232,7 @@ int buzzuav_addtargetRB(buzzvm_t vm)
if (it != targets_map.end())
targets_map.erase(it);
targets_map.insert(make_pair(uid, RB_arr));
// DEBUG
// ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
return vm->state;
}
@ -270,6 +243,9 @@ int buzzuav_addtargetRB(buzzvm_t vm)
}
int buzzuav_addNeiStatus(buzzvm_t vm)
/*
/ closure to add neighbors status to the BVM
/----------------------------------------*/
{
buzzvm_lnum_assert(vm, 5);
buzzvm_lload(vm, 1); // fc
@ -296,6 +272,9 @@ int buzzuav_addNeiStatus(buzzvm_t vm)
}
mavros_msgs::Mavlink get_status()
/*
/ return neighbors status from BVM
/----------------------------------------*/
{
mavros_msgs::Mavlink payload_out;
map<int, buzz_utility::neighbors_status>::iterator it;
@ -307,27 +286,25 @@ mavros_msgs::Mavlink get_status()
payload_out.payload64.push_back(it->second.xbee);
payload_out.payload64.push_back(it->second.flight_status);
}
/*Add Robot id and message number to the published message*/
// Add Robot id and message number to the published message
payload_out.sysid = (uint8_t)neighbors_status_map.size();
/*payload_out.msgid = (uint32_t)message_number;
message_number++;*/
return payload_out;
}
/*----------------------------------------/
int buzzuav_takepicture(buzzvm_t vm)
/*
/ Buzz closure to take a picture here.
/----------------------------------------*/
int buzzuav_takepicture(buzzvm_t vm)
{
// cur_cmd = mavros_msgs::CommandCode::CMD_DO_SET_CAM_TRIGG_DIST;
buzz_cmd = COMMAND_PICTURE;
return buzzvm_ret0(vm);
}
/*----------------------------------------/
int buzzuav_setgimbal(buzzvm_t vm)
/*
/ Buzz closure to change locally the gimbal orientation
/----------------------------------------*/
int buzzuav_setgimbal(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 4);
buzzvm_lload(vm, 1); // time
@ -343,15 +320,15 @@ int buzzuav_setgimbal(buzzvm_t vm)
rc_gimbal[2] = buzzvm_stack_at(vm, 2)->f.value;
rc_gimbal[3] = buzzvm_stack_at(vm, 1)->f.value;
ROS_WARN("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
ROS_INFO("Set RC_GIMBAL ---- %f %f %f (%f)", rc_gimbal[0], rc_gimbal[1], rc_gimbal[2], rc_gimbal[3]);
buzz_cmd = COMMAND_GIMBAL;
return buzzvm_ret0(vm);
}
/*----------------------------------------/
int buzzuav_storegoal(buzzvm_t vm)
/*
/ Buzz closure to store locally a GPS destination from the fleet
/----------------------------------------*/
int buzzuav_storegoal(buzzvm_t vm)
{
buzzvm_lnum_assert(vm, 3);
buzzvm_lload(vm, 1); // altitude
@ -380,21 +357,25 @@ int buzzuav_storegoal(buzzvm_t vm)
if (fabs(rb[0]) < 150.0)
rc_set_goto((int)buzz_utility::get_robotid(), goal[0], goal[1], goal[2]);
ROS_WARN("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
ROS_INFO("Set RC_GOTO ---- %f %f %f (%f %f, %f %f)", goal[0], goal[1], goal[2], cur_pos[0], cur_pos[1], rb[0], rb[1]);
return buzzvm_ret0(vm);
}
/*----------------------------------------/
/ Buzz closure to arm/disarm the drone, useful for field tests to ensure all systems are up and running
/----------------------------------------*/
int buzzuav_arm(buzzvm_t vm)
/*
/ Buzz closure to arm
/---------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM;
printf(" Buzz requested Arm \n");
buzz_cmd = COMMAND_ARM;
return buzzvm_ret0(vm);
}
int buzzuav_disarm(buzzvm_t vm)
/*
/ Buzz closure to disarm
/---------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::CMD_COMPONENT_ARM_DISARM + 1;
printf(" Buzz requested Disarm \n");
@ -402,10 +383,10 @@ int buzzuav_disarm(buzzvm_t vm)
return buzzvm_ret0(vm);
}
/*---------------------------------------/
/ Buzz closure for basic UAV commands
/---------------------------------------*/
int buzzuav_takeoff(buzzvm_t vm)
/*
/ Buzz closure to takeoff
/---------------------------------------*/
{
buzzvm_lnum_assert(vm, 1);
buzzvm_lload(vm, 1); /* Altitude */
@ -419,6 +400,9 @@ int buzzuav_takeoff(buzzvm_t vm)
}
int buzzuav_land(buzzvm_t vm)
/*
/ Buzz closure to land
/-------------------------------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::NAV_LAND;
printf(" Buzz requested Land !!! \n");
@ -427,6 +411,9 @@ int buzzuav_land(buzzvm_t vm)
}
int buzzuav_gohome(buzzvm_t vm)
/*
/ Buzz closure to return Home
/-------------------------------------------------------------*/
{
cur_cmd = mavros_msgs::CommandCode::NAV_RETURN_TO_LAUNCH;
printf(" Buzz requested gohome !!! \n");
@ -434,20 +421,26 @@ int buzzuav_gohome(buzzvm_t vm)
return buzzvm_ret0(vm);
}
/*-------------------------------
/ Get/Set to transfer variable from Roscontroller to buzzuav
/------------------------------------*/
double* getgoto()
/*
/ return the GPS goal
/-------------------------------------------------------------*/
{
return goto_pos;
}
float* getgimbal()
/*
/ return current gimbal commands
---------------------------------------*/
{
return rc_gimbal;
}
string getuavstate()
/*
/ return current BVM state
---------------------------------------*/
{
static buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "UAVSTATE", 1));
@ -458,11 +451,17 @@ string getuavstate()
}
int getcmd()
/*
/ return current mavros command to the FC
---------------------------------------*/
{
return cur_cmd;
}
int bzz_cmd()
/*
/ return and clean the custom command from Buzz to the FC
----------------------------------------------------------*/
{
int cmd = buzz_cmd;
buzz_cmd = 0;
@ -470,6 +469,9 @@ int bzz_cmd()
}
void rc_set_goto(int id, double latitude, double longitude, double altitude)
/*
/ update interface RC GPS goal input
-----------------------------------*/
{
rc_id = id;
rc_goto_pos[0] = latitude;
@ -478,6 +480,9 @@ void rc_set_goto(int id, double latitude, double longitude, double altitude)
}
void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
/*
/ update interface RC gimbal control input
-----------------------------------*/
{
rc_id = id;
rc_gimbal[0] = yaw;
@ -487,17 +492,26 @@ void rc_set_gimbal(int id, float yaw, float roll, float pitch, float t)
}
void rc_call(int rc_cmd_in)
/*
/ update interface RC command input
-----------------------------------*/
{
rc_cmd = rc_cmd_in;
}
void set_obstacle_dist(float dist[])
/*
/ update interface proximity value array
-----------------------------------*/
{
for (int i = 0; i < 5; i++)
obst[i] = dist[i];
}
void set_battery(float voltage, float current, float remaining)
/*
/ update interface battery value array
-----------------------------------*/
{
batt[0] = voltage;
batt[1] = current;
@ -505,6 +519,9 @@ void set_battery(float voltage, float current, float remaining)
}
int buzzuav_update_battery(buzzvm_t vm)
/*
/ update BVM battery table
-----------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "battery", 1));
buzzvm_pusht(vm);
@ -524,6 +541,9 @@ int buzzuav_update_battery(buzzvm_t vm)
return vm->state;
}
/*
/ Set of function to update interface variable of xbee network status
----------------------------------------------------------------------*/
void set_deque_full(bool state)
{
deque_full = state;
@ -550,6 +570,9 @@ void set_api_rssi(float value)
}
int buzzuav_update_xbee_status(buzzvm_t vm)
/*
/ update BVM xbee_status table
-----------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "xbee_status", 1));
buzzvm_pusht(vm);
@ -577,16 +600,16 @@ int buzzuav_update_xbee_status(buzzvm_t vm)
return vm->state;
}
/***************************************/
/*current pos update*/
/***************************************/
void set_currentpos(double latitude, double longitude, double altitude)
/*
/ update interface position array
-----------------------------------*/
{
cur_pos[0] = latitude;
cur_pos[1] = longitude;
cur_pos[2] = altitude;
}
/*adds neighbours position*/
// adds neighbours position
void neighbour_pos_callback(int id, float range, float bearing, float elevation)
{
buzz_utility::Pos_struct pos_arr;
@ -599,12 +622,12 @@ void neighbour_pos_callback(int id, float range, float bearing, float elevation)
neighbors_map.insert(make_pair(id, pos_arr));
}
/* update at each step the VM table */
// update at each step the VM table
void update_neighbors(buzzvm_t vm)
{
/* Reset neighbor information */
// Reset neighbor information
buzzneighbors_reset(vm);
/* Get robot id and update neighbor information */
// Get robot id and update neighbor information
map<int, buzz_utility::Pos_struct>::iterator it;
for (it = neighbors_map.begin(); it != neighbors_map.end(); ++it)
{
@ -612,8 +635,10 @@ void update_neighbors(buzzvm_t vm)
}
}
/****************************************/
int buzzuav_update_currentpos(buzzvm_t vm)
/*
/ Update the BVM position table
/------------------------------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "position", 1));
buzzvm_pusht(vm);
@ -634,16 +659,18 @@ int buzzuav_update_currentpos(buzzvm_t vm)
}
void flight_status_update(uint8_t state)
/*
/ Update the interface status variable
/------------------------------------------------------*/
{
status = state;
}
/*----------------------------------------------------
int buzzuav_update_flight_status(buzzvm_t vm)
/*
/ Create the generic robot table with status, remote controller current comand and destination
/ and current position of the robot
/ TODO: change global name for robot
/------------------------------------------------------*/
int buzzuav_update_flight_status(buzzvm_t vm)
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "flight", 1));
buzzvm_pusht(vm);
@ -657,7 +684,6 @@ int buzzuav_update_flight_status(buzzvm_t vm)
buzzvm_pushi(vm, status);
buzzvm_tput(vm);
buzzvm_gstore(vm);
// also set rc_controllers goto
buzzvm_pushs(vm, buzzvm_string_register(vm, "rc_goto", 1));
buzzvm_pusht(vm);
buzzvm_dup(vm);
@ -680,32 +706,31 @@ int buzzuav_update_flight_status(buzzvm_t vm)
return vm->state;
}
/******************************************************/
/*Create an obstacle Buzz table from proximity sensors*/
/* Acessing proximity in buzz script
proximity[0].angle and proximity[0].value - front
"" "" "" - right and back
proximity[3].angle and proximity[3].value - left
proximity[4].angle = -1 and proximity[4].value -bottom */
/******************************************************/
int buzzuav_update_prox(buzzvm_t vm)
/*
/ Create an obstacle Buzz table from proximity sensors
/ Acessing proximity in buzz script
/ proximity[0].angle and proximity[0].value - front
/ "" "" "" - right and back
/ proximity[3].angle and proximity[3].value - left
/ proximity[4].angle = -1 and proximity[4].value -bottom
-------------------------------------------------------------*/
{
buzzvm_pushs(vm, buzzvm_string_register(vm, "proximity", 1));
buzzvm_pusht(vm);
buzzobj_t tProxTable = buzzvm_stack_at(vm, 1);
buzzvm_gstore(vm);
/* Fill into the proximity table */
// Fill into the proximity table
buzzobj_t tProxRead;
float angle = 0;
for (size_t i = 0; i < 4; ++i)
{
/* Create table for i-th read */
// Create table for i-th read
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
/* Fill in the read */
// Fill in the read
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[i + 1]);
@ -714,19 +739,19 @@ int buzzuav_update_prox(buzzvm_t vm)
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
/* Store read table in the proximity table */
// Store read table in the proximity table
buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, i);
buzzvm_push(vm, tProxRead);
buzzvm_tput(vm);
angle += 1.5708;
}
/* Create table for bottom read */
// Create table for bottom read
angle = -1;
buzzvm_pusht(vm);
tProxRead = buzzvm_stack_at(vm, 1);
buzzvm_pop(vm);
/* Fill in the read */
// Fill in the read
buzzvm_push(vm, tProxRead);
buzzvm_pushs(vm, buzzvm_string_register(vm, "value", 0));
buzzvm_pushf(vm, obst[0]);
@ -735,7 +760,7 @@ int buzzuav_update_prox(buzzvm_t vm)
buzzvm_pushs(vm, buzzvm_string_register(vm, "angle", 0));
buzzvm_pushf(vm, angle);
buzzvm_tput(vm);
/*Store read table in the proximity table*/
// Store read table in the proximity table
buzzvm_push(vm, tProxTable);
buzzvm_pushi(vm, 4);
buzzvm_push(vm, tProxRead);
@ -743,11 +768,10 @@ int buzzuav_update_prox(buzzvm_t vm)
return vm->state;
}
/**********************************************/
/*Dummy closure for use during update testing */
/**********************************************/
int dummy_closure(buzzvm_t vm)
/*
/ Dummy closure for use during update testing
----------------------------------------------------*/
{
return buzzvm_ret0(vm);
}

View File

@ -8,19 +8,17 @@
#include "roscontroller.h"
/**
* This program implements Buzz node in ros using mavros_msgs.
*/
int main(int argc, char** argv)
/*
/ This program implements Buzz in a ROS node using mavros_msgs.
-----------------------------------------------------------------*/
{
/*Initialize rosBuzz node*/
// Initialize rosBuzz node
ros::init(argc, argv, "rosBuzz");
ros::NodeHandle nh_priv("~");
ros::NodeHandle nh;
rosbzz_node::roscontroller RosController(nh, nh_priv);
/*Register ros controller object inside buzz*/
// buzzuav_closures::set_ros_controller_ptr(&RosController);
RosController.RosControllerRun();
return 0;
}

File diff suppressed because it is too large Load Diff