790 lines
28 KiB
C++
790 lines
28 KiB
C++
/** @file buzz_utility.cpp
|
|
* @version 1.0
|
|
* @date 27.09.2016
|
|
* @brief Buzz Implementation as a node in ROS for Dji M100 Drone.
|
|
* @author Vivek Shankar Varadharajan
|
|
* @copyright 2016 MistLab. All rights reserved.
|
|
*/
|
|
|
|
#include "buzz_utility.h"
|
|
|
|
namespace buzz_utility{
|
|
|
|
/****************************************/
|
|
/****************************************/
|
|
|
|
static buzzvm_t VM = 0;
|
|
static char* BO_FNAME = 0;
|
|
static uint8_t* BO_BUF = 0;
|
|
static buzzdebug_t DBG_INFO = 0;
|
|
static uint32_t MSG_SIZE = 250; // Only 250 bytes of Buzz messages every step (limited to Xbee frame size)
|
|
static uint32_t MAX_MSG_SIZE = 10000; // Maximum Msg size for sending update packets
|
|
static uint8_t Robot_id = 0;
|
|
static std::vector<uint8_t*> IN_MSG;
|
|
std::map< int, Pos_struct> users_map;
|
|
|
|
/****************************************/
|
|
|
|
void add_user(int id, double latitude, double longitude, float altitude)
|
|
{
|
|
Pos_struct pos_arr;
|
|
pos_arr.x=latitude;
|
|
pos_arr.y=longitude;
|
|
pos_arr.z=altitude;
|
|
map< int, buzz_utility::Pos_struct >::iterator it = users_map.find(id);
|
|
if(it!=users_map.end())
|
|
users_map.erase(it);
|
|
users_map.insert(make_pair(id, pos_arr));
|
|
//ROS_INFO("Buzz_utility got updated/new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
|
|
}
|
|
|
|
void update_users(){
|
|
if(users_map.size()>0) {
|
|
// Reset users information
|
|
// buzzusers_reset();
|
|
// create_stig_tables();
|
|
// Get user id and update user information
|
|
map< int, Pos_struct >::iterator it;
|
|
for (it=users_map.begin(); it!=users_map.end(); ++it){
|
|
//ROS_INFO("Buzz_utility will save user %i.", it->first);
|
|
buzzusers_add(it->first,
|
|
(it->second).x,
|
|
(it->second).y,
|
|
(it->second).z);
|
|
}
|
|
}
|
|
}
|
|
|
|
/*int buzzusers_reset() {
|
|
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
|
//Make new table
|
|
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
|
|
//make_table(&t);
|
|
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
|
//Register table as global symbol
|
|
//buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
|
buzzvm_tget(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
|
|
buzzvm_push(VM, t);
|
|
buzzvm_gstore(VM);
|
|
//buzzvm_pushi(VM, 2);
|
|
//buzzvm_callc(VM);
|
|
return VM->state;
|
|
}*/
|
|
|
|
int buzzusers_add(int id, double latitude, double longitude, double altitude) {
|
|
if(VM->state != BUZZVM_STATE_READY) return VM->state;
|
|
// Get users "p" table
|
|
/*buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "get", 1));
|
|
buzzvm_tget(VM);*/
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
|
buzzvm_tget(VM);
|
|
if(buzzvm_stack_at(VM, 1)->o.type == BUZZTYPE_NIL) {
|
|
//ROS_INFO("Empty data, create a new table");
|
|
buzzvm_pop(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
|
buzzvm_pusht(VM);
|
|
buzzobj_t data = buzzvm_stack_at(VM, 1);
|
|
buzzvm_tput(VM);
|
|
buzzvm_push(VM, data);
|
|
}
|
|
// When we get here, the "data" table is on top of the stack
|
|
// Push user id
|
|
buzzvm_pushi(VM, id);
|
|
// Create entry table
|
|
buzzobj_t entry = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
|
|
// Insert latitude
|
|
buzzvm_push(VM, entry);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "la", 1));
|
|
buzzvm_pushf(VM, latitude);
|
|
buzzvm_tput(VM);
|
|
// Insert longitude
|
|
buzzvm_push(VM, entry);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "lo", 1));
|
|
buzzvm_pushf(VM, longitude);
|
|
buzzvm_tput(VM);
|
|
// Insert altitude
|
|
buzzvm_push(VM, entry);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "al", 1));
|
|
buzzvm_pushf(VM, altitude);
|
|
buzzvm_tput(VM);
|
|
// Save entry into data table
|
|
buzzvm_push(VM, entry);
|
|
buzzvm_tput(VM);
|
|
//ROS_INFO("Buzz_utility saved new user: %i (%f,%f,%f)", id, latitude, longitude, altitude);
|
|
// forcing the new table into the stigmergy....
|
|
/*buzzobj_t newt = buzzvm_stack_at(VM, 0);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
|
buzzvm_tget(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
|
buzzvm_push(VM, nbr);
|
|
buzzvm_pushi(VM, 2);
|
|
buzzvm_callc(VM);*/
|
|
//buzzvm_gstore(VM);
|
|
return VM->state;
|
|
}
|
|
/**************************************************************************/
|
|
/*Deserializes uint64_t into 4 uint16_t, freeing out is left to the user */
|
|
/**************************************************************************/
|
|
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;
|
|
}
|
|
|
|
int get_robotid() {
|
|
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){
|
|
|
|
/* 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*/
|
|
uint16_t size=data[0]*sizeof(uint64_t);
|
|
delete[] data;
|
|
uint8_t* pl =(uint8_t*)malloc(size);
|
|
/* Copy packet into temporary buffer */
|
|
memcpy(pl, payload ,size);
|
|
IN_MSG.push_back(pl);
|
|
|
|
}
|
|
|
|
void in_message_process(){
|
|
while(!IN_MSG.empty()){
|
|
/* 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*/
|
|
uint16_t size=(*(uint16_t*)first_INmsg)*sizeof(uint64_t);
|
|
tot += sizeof(uint16_t);
|
|
/*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 */
|
|
uint16_t unMsgSize=0;
|
|
/*Obtain Buzz messages push it into queue*/
|
|
do {
|
|
/* Get payload size */
|
|
unMsgSize = *(uint16_t*)(first_INmsg + tot);
|
|
tot += sizeof(uint16_t);
|
|
/* 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));
|
|
tot += unMsgSize;
|
|
}
|
|
}while(size - tot > sizeof(uint16_t) && unMsgSize > 0);
|
|
free(first_INmsg);
|
|
IN_MSG.erase(IN_MSG.begin());
|
|
}
|
|
/* Process messages VM call*/
|
|
buzzvm_process_inmsgs(VM);
|
|
}
|
|
|
|
/***************************************************/
|
|
/*Obtains messages from buzz out message Queue*/
|
|
/***************************************************/
|
|
uint64_t* obt_out_msg(){
|
|
/* 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*/
|
|
ssize_t tot = sizeof(uint16_t);
|
|
/* Send robot id */
|
|
*(uint16_t*)(buff_send+tot) = (uint16_t) VM->robot;
|
|
tot += sizeof(uint16_t);
|
|
/* Send messages from FIFO */
|
|
do {
|
|
/* Are there more messages? */
|
|
if(buzzoutmsg_queue_isempty(VM)) break;
|
|
/* 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 */
|
|
//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)) > 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);
|
|
buzzmsg_payload_destroy(&m);
|
|
} while(1);
|
|
|
|
uint16_t total_size =(ceil((float)tot/(float)sizeof(uint64_t)));
|
|
*(uint16_t*)buff_send = (uint16_t) total_size;
|
|
|
|
uint64_t* payload_64 = new uint64_t[total_size];
|
|
|
|
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 */
|
|
return payload_64;
|
|
}
|
|
|
|
/****************************************/
|
|
/*Buzz script not able to load*/
|
|
/****************************************/
|
|
|
|
static const char* buzz_error_info() {
|
|
buzzdebug_entry_t dbg = *buzzdebug_info_get_fromoffset(DBG_INFO, &VM->pc);
|
|
char* msg;
|
|
if(dbg != NULL) {
|
|
asprintf(&msg,
|
|
"%s: execution terminated abnormally at %s:%" PRIu64 ":%" PRIu64 " : %s\n\n",
|
|
BO_FNAME,
|
|
dbg->fname,
|
|
dbg->line,
|
|
dbg->col,
|
|
VM->errormsg);
|
|
}
|
|
else {
|
|
asprintf(&msg,
|
|
"%s: execution terminated abnormally at bytecode offset %d: %s\n\n",
|
|
BO_FNAME,
|
|
VM->pc,
|
|
VM->errormsg);
|
|
}
|
|
return msg;
|
|
}
|
|
|
|
/****************************************/
|
|
/*Buzz hooks that can be used inside .bzz file*/
|
|
/****************************************/
|
|
|
|
static int buzz_register_hooks() {
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_moveto));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_storegoal));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_arm));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_disarm));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_takeoff));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_gohome));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_land));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addtargetRB));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzuav_addNeiStatus));
|
|
buzzvm_gstore(VM);
|
|
|
|
return VM->state;
|
|
}
|
|
|
|
/**************************************************/
|
|
/*Register dummy Buzz hooks for test during update*/
|
|
/**************************************************/
|
|
|
|
static int testing_buzz_register_hooks() {
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "print", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "log", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "debug", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::buzzros_print));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_moveto", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_storegoal", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_arm", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_disarm", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_takeoff", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_gohome", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "uav_land", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_targetrb", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "add_neighborStatus", 1));
|
|
buzzvm_pushcc(VM, buzzvm_function_register(VM, buzzuav_closures::dummy_closure));
|
|
buzzvm_gstore(VM);
|
|
|
|
return VM->state;
|
|
}
|
|
|
|
int create_stig_tables() {
|
|
/*
|
|
// usersvstig = stigmergy.create(123)
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
|
// get the stigmergy table from the global scope
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "stigmergy", 1));
|
|
buzzvm_gload(VM);
|
|
// get the create method from the stigmergy table
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "create", 1));
|
|
buzzvm_tget(VM);
|
|
// value of the stigmergy id
|
|
buzzvm_pushi(VM, 5);
|
|
// call the stigmergy.create() method
|
|
buzzvm_dump(VM);
|
|
// buzzvm_closure_call(VM, 1);
|
|
buzzvm_pushi(VM, 1);
|
|
buzzvm_callc(VM);
|
|
buzzvm_gstore(VM);
|
|
buzzvm_dump(VM);
|
|
|
|
//buzzusers_reset();
|
|
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
|
|
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
|
buzzvm_tget(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "p", 1));
|
|
buzzvm_push(VM, t);
|
|
buzzvm_dump(VM);
|
|
// buzzvm_closure_call(VM, 2);
|
|
buzzvm_pushi(VM, 2);
|
|
buzzvm_callc(VM);
|
|
//buzzvm_gstore(VM);
|
|
//buzzvm_dump(VM);
|
|
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "vt", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "put", 1));
|
|
buzzvm_tget(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "u", 1));
|
|
buzzvm_pusht(VM);
|
|
buzzvm_pushi(VM, 2);
|
|
buzzvm_call(VM, 0);
|
|
buzzvm_gstore(VM);*/
|
|
|
|
buzzobj_t t = buzzheap_newobj(VM->heap, BUZZTYPE_TABLE);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
|
buzzvm_push(VM,t);
|
|
buzzvm_gstore(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataG", 1));
|
|
buzzvm_pusht(VM);
|
|
buzzobj_t data = buzzvm_stack_at(VM, 1);
|
|
buzzvm_tput(VM);
|
|
buzzvm_push(VM, data);
|
|
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "users", 1));
|
|
buzzvm_gload(VM);
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "dataL", 1));
|
|
buzzvm_pusht(VM);
|
|
data = buzzvm_stack_at(VM, 1);
|
|
buzzvm_tput(VM);
|
|
buzzvm_push(VM, data);
|
|
|
|
return VM->state;
|
|
}
|
|
/****************************************/
|
|
/*Sets the .bzz and .bdbg file*/
|
|
/****************************************/
|
|
int buzz_script_set(const char* bo_filename,
|
|
const char* bdbg_filename, int robot_id) {
|
|
ROS_INFO(" Robot ID: %i" , robot_id);
|
|
/* Reset the Buzz VM */
|
|
if(VM) buzzvm_destroy(&VM);
|
|
Robot_id = robot_id;
|
|
VM = buzzvm_new((int)robot_id);
|
|
/* Get rid of debug info */
|
|
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
|
DBG_INFO = buzzdebug_new();
|
|
/* Read bytecode and fill in data structure */
|
|
FILE* fd = fopen(bo_filename, "rb");
|
|
if(!fd) {
|
|
perror(bo_filename);
|
|
return 0;
|
|
}
|
|
fseek(fd, 0, SEEK_END);
|
|
size_t bcode_size = ftell(fd);
|
|
rewind(fd);
|
|
BO_BUF = (uint8_t*)malloc(bcode_size);
|
|
if(fread(BO_BUF, 1, bcode_size, fd) < bcode_size) {
|
|
perror(bo_filename);
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
fclose(fd);
|
|
return 0;
|
|
}
|
|
fclose(fd);
|
|
/* Read debug information */
|
|
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
perror(bdbg_filename);
|
|
return 0;
|
|
}
|
|
/* Set byte code */
|
|
if(buzzvm_set_bcode(VM, BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] %s: Error loading Buzz script", Robot_id, bo_filename);
|
|
return 0;
|
|
}
|
|
/* Register hook functions */
|
|
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] Error registering hooks", Robot_id);
|
|
return 0;
|
|
}
|
|
|
|
/* Create vstig tables
|
|
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
|
|
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
|
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
|
return 0;
|
|
}*/
|
|
|
|
/* Save bytecode file name */
|
|
BO_FNAME = strdup(bo_filename);
|
|
|
|
// Execute the global part of the script
|
|
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
|
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
|
|
return 0;
|
|
}
|
|
// Call the Init() function
|
|
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
|
|
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
|
|
return 0;
|
|
}
|
|
/* All OK */
|
|
|
|
ROS_INFO("[%i] INIT DONE!!!", Robot_id);
|
|
|
|
return 1;//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){
|
|
// Reset the Buzz VM
|
|
if(VM) buzzvm_destroy(&VM);
|
|
VM = buzzvm_new(Robot_id);
|
|
// Get rid of debug info
|
|
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
|
DBG_INFO = buzzdebug_new();
|
|
|
|
// Read debug information
|
|
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
perror(bdbg_filename);
|
|
return 0;
|
|
}
|
|
// Set byte code
|
|
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update)", Robot_id);
|
|
return 0;
|
|
}
|
|
// Register hook functions
|
|
if(buzz_register_hooks() != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
|
|
return 0;
|
|
}
|
|
/* Create vstig tables
|
|
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
|
|
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
|
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
|
return 0;
|
|
}*/
|
|
|
|
// Execute the global part of the script
|
|
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
|
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
|
|
return 0;
|
|
}
|
|
// Call the Init() function
|
|
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
|
|
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
|
|
return 0;
|
|
}
|
|
// All OK
|
|
return 1;
|
|
}
|
|
|
|
/****************************************/
|
|
/*Performs a initialization test */
|
|
/****************************************/
|
|
int buzz_update_init_test(uint8_t* UP_BO_BUF, const char* bdbg_filename,size_t bcode_size){
|
|
// Reset the Buzz VM
|
|
if(VM) buzzvm_destroy(&VM);
|
|
VM = buzzvm_new(Robot_id);
|
|
// Get rid of debug info
|
|
if(DBG_INFO) buzzdebug_destroy(&DBG_INFO);
|
|
DBG_INFO = buzzdebug_new();
|
|
|
|
// Read debug information
|
|
if(!buzzdebug_fromfile(DBG_INFO, bdbg_filename)) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
perror(bdbg_filename);
|
|
return 0;
|
|
}
|
|
// Set byte code
|
|
if(buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] %s: Error loading Buzz bytecode (update init)", Robot_id);
|
|
return 0;
|
|
}
|
|
// Register hook functions
|
|
if(testing_buzz_register_hooks() != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] Error registering hooks (update init)", Robot_id);
|
|
return 0;
|
|
}
|
|
/* Create vstig tables
|
|
if(create_stig_tables() != BUZZVM_STATE_READY) {
|
|
buzzvm_destroy(&VM);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
ROS_ERROR("[%i] Error creating stigmergy tables", Robot_id);
|
|
//cout << "ERROR!!!! ---------- " << VM->errormsg << endl;
|
|
//cout << "ERROR!!!! ---------- " << buzzvm_strerror(VM) << endl;
|
|
return 0;
|
|
}*/
|
|
// Execute the global part of the script
|
|
if(buzzvm_execute_script(VM)!= BUZZVM_STATE_DONE){
|
|
ROS_ERROR("Error executing global part, VM state : %i",VM->state);
|
|
return 0;
|
|
}
|
|
// Call the Init() function
|
|
if(buzzvm_function_call(VM, "init", 0) != BUZZVM_STATE_READY){
|
|
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
|
|
return 0;
|
|
}
|
|
// All OK
|
|
return 1;
|
|
}
|
|
|
|
/****************************************/
|
|
/*Swarm struct*/
|
|
/****************************************/
|
|
|
|
struct buzzswarm_elem_s {
|
|
buzzdarray_t swarms;
|
|
uint16_t age;
|
|
};
|
|
typedef struct buzzswarm_elem_s* buzzswarm_elem_t;
|
|
|
|
void check_swarm_members(const void* key, void* data, void* params) {
|
|
buzzswarm_elem_t e = *(buzzswarm_elem_t*)data;
|
|
int* status = (int*)params;
|
|
if(*status == 3) return;
|
|
fprintf(stderr, "CHECKING SWARM :%i, member: %i, age: %i \n",
|
|
buzzdarray_get(e->swarms, 0, uint16_t), *(uint16_t*)key, e->age);
|
|
if(buzzdarray_size(e->swarms) != 1) {
|
|
fprintf(stderr, "Swarm list size is not 1\n");
|
|
*status = 3;
|
|
}
|
|
else {
|
|
int sid = 1;
|
|
if(!buzzdict_isempty(VM->swarms)) {
|
|
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
|
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
|
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
|
sid,
|
|
buzzdarray_get(e->swarms, 0, uint16_t));
|
|
*status = 3;
|
|
return;
|
|
}
|
|
}
|
|
if(buzzdict_size(VM->swarms)>1) {
|
|
sid = 2;
|
|
if(*buzzdict_get(VM->swarms, &sid, uint8_t) &&
|
|
buzzdarray_get(e->swarms, 0, uint16_t) != sid) {
|
|
fprintf(stderr, "I am in swarm #%d and neighbor is in %d\n",
|
|
sid,
|
|
buzzdarray_get(e->swarms, 0, uint16_t));
|
|
*status = 3;
|
|
return;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
/*Step through the buzz script*/
|
|
void 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() {
|
|
/*Process available messages*/
|
|
in_message_process();
|
|
/*Update sensors*/
|
|
update_sensors();
|
|
/* 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());
|
|
buzzvm_dump(VM);
|
|
}
|
|
|
|
/*Print swarm*/
|
|
//buzzswarm_members_print(stdout, VM->swarmmembers, VM->robot);
|
|
//int SwarmSize = buzzdict_size(VM->swarmmembers)+1;
|
|
//fprintf(stderr, "Real Swarm Size: %i\n",SwarmSize);
|
|
|
|
|
|
/* Check swarm state -- Not crashing thanks to test added in check_swarm_members */
|
|
//int status = 1;
|
|
//buzzdict_foreach(VM->swarmmembers, check_swarm_members, &status);
|
|
}
|
|
|
|
/****************************************/
|
|
/*Destroy the bvm and other resorces*/
|
|
/****************************************/
|
|
|
|
void buzz_script_destroy() {
|
|
if(VM) {
|
|
if(VM->state != BUZZVM_STATE_READY) {
|
|
ROS_ERROR("%s: execution terminated abnormally: %s",
|
|
BO_FNAME,
|
|
buzz_error_info());
|
|
buzzvm_dump(VM);
|
|
}
|
|
buzzvm_function_call(VM, "destroy", 0);
|
|
buzzvm_destroy(&VM);
|
|
free(BO_FNAME);
|
|
buzzdebug_destroy(&DBG_INFO);
|
|
}
|
|
ROS_INFO("Script execution stopped.");
|
|
}
|
|
|
|
|
|
/****************************************/
|
|
/****************************************/
|
|
|
|
/****************************************/
|
|
/*Execution completed*/
|
|
/****************************************/
|
|
|
|
int buzz_script_done() {
|
|
return VM->state != BUZZVM_STATE_READY;
|
|
}
|
|
|
|
int update_step_test() {
|
|
/*Process available messages*/
|
|
in_message_process();
|
|
buzzuav_closures::buzzuav_update_battery(VM);
|
|
buzzuav_closures::buzzuav_update_prox(VM);
|
|
buzzuav_closures::buzzuav_update_currentpos(VM);
|
|
buzzuav_closures::update_neighbors(VM);
|
|
//update_users();
|
|
buzzuav_closures::buzzuav_update_flight_status(VM);
|
|
//set_robot_var(buzzdict_size(VM->swarmmembers)+1);
|
|
|
|
int a = buzzvm_function_call(VM, "step", 0);
|
|
|
|
if(a!= BUZZVM_STATE_READY) {
|
|
ROS_ERROR("%s: execution terminated abnormally: %s\n\n",
|
|
BO_FNAME,
|
|
buzz_error_info());
|
|
fprintf(stdout, "step test VM state %i\n",a);
|
|
}
|
|
|
|
return a == BUZZVM_STATE_READY;
|
|
}
|
|
|
|
buzzvm_t get_vm() {
|
|
return VM;
|
|
}
|
|
|
|
void set_robot_var(int ROBOTS){
|
|
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
|
|
buzzvm_pushi(VM, ROBOTS);
|
|
buzzvm_gstore(VM);
|
|
}
|
|
}
|