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

Fixed the update crash caused by BVM state publisher.

This commit is contained in:
vivek-shankar 2018-06-06 22:40:26 -04:00
parent 3ad277bd5e
commit 58c09093a7
10 changed files with 82 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
buzz_scripts/include/update.bzz
View File

@ -4,6 +4,6 @@
####################################################################################################
updated="update_ack"
update_no=0
function updated_neigh(){
function updated_no_bct(){
neighbors.broadcast(updated, update_no)
}

1
buzz_scripts/minimal.bzz
View File

@ -51,7 +51,6 @@ function step() {
statef=action
statef()
log("Current state: ", BVMSTATE)
}

48
buzz_scripts/stand_by.bzz
View File

@ -1,35 +1,41 @@
include "act/states.bzz"
include "vstigenv.bzz"
updated="update_ack"
update_no=0
function init(){
barrier = stigmergy.create(101)
barrier.put(id,1)
barrier_val=0
barrier.onconflict(function (k,l,r) {
if(r. data < l. data or (r. data == l. data )) return l
else return r
})
BVMSTATE = "UPDATESTANDBY"
s = swarm.create(1)
s.join()
# Executed once at init time.
function init(){
barrier = stigmergy.create(101)
barrier.put(id,1)
barrier_val=0
barrier.onconflict(function (k,l,r) {
if(r. data < l. data or (r. data == l. data )) return l
else return r
})
BVMSTATE = "UPDATESTANDBY"
init_swarm()
# start the swarm command listener
nei_cmd_listen()
}
function stand_by(){
barrier.get(id)
barrier_val = barrier.size()
barrier.get(id)
barrier_val = barrier.size()
neighbors.listen(updated,
function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " )
if(value==update_no) barrier.put(rid,1)
}
neighbors.listen(updated,
function(vid, value, rid) {
print(" got from",vid," ", " value = ",value," ","rid"," " )
if(value==update_no) barrier.put(rid,1)
}
)
}
# Executed at each time step.
function step() {
stand_by()
stand_by()
}

36
include/buzz_update.h
View File

@ -1,7 +1,5 @@
#ifndef BUZZ_UPDATE_H
#define BUZZ_UPDATE_H
/*Simulation or robot check*/
//#define SIMULATION 1 // set in CMAKELIST
#include <stdlib.h>
#include <stdio.h>
@ -112,31 +110,30 @@ void code_message_inqueue_append(uint8_t* msg, uint16_t size);
void code_message_inqueue_process();
/*****************************************************/
/* obtains messages from out msgs queue of the updater*/
/*Obtains messages from out msgs queue of the updater*/
/*******************************************************/
uint8_t* getupdater_out_msg();
/******************************************************/
/*obtains out msg queue size*/
/*Obtains out msg queue size*/
/*****************************************************/
uint8_t* getupdate_out_msg_size();
/**************************************************/
/*destroys the out msg queue*/
/*Destroys the out msg queue*/
/*************************************************/
void destroy_out_msg_queue();
/***************************************************/
/*obatins updater state*/
/***************************************************/
// int get_update_mode();
// buzz_updater_elem_t get_updater();
/***************************************************/
/*sets bzz file name*/
/*Sets bzz file name*/
/***************************************************/
void set_bzz_file(const char* in_bzz_file);
/****************************************************/
/*Tests the code from a buffer*/
/***************************************************/
int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
/****************************************************/
@ -145,17 +142,22 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size);
void destroy_updater();
/****************************************************/
/*Checks for updater message*/
/***************************************************/
int is_msg_present();
int get_update_status();
void set_read_update_status();
/****************************************************/
/*Compiles a bzz script to bo and bdbg*/
/***************************************************/
int compile_bzz(std::string bzz_file);
/****************************************************/
/*Set number of robots in the updater*/
/***************************************************/
void updates_set_robots(int robots);
// void set_packet_id(int packet_id);
// void collect_data(std::ofstream& logger);
#endif

2
include/buzz_utility.h
View File

@ -77,4 +77,6 @@ buzzvm_t get_vm();
void set_robot_var(int ROBOTS);
int get_inmsg_size();
std::string getuavstate();
}

4
include/buzzuav_closures.h
View File

@ -85,10 +85,6 @@ double* getgoto();
* returns the current grid
*/
std::map<int, std::map<int,int>> getgrid();
/*
* returns the current Buzz state
*/
std::string getuavstate();
/*
* returns the gimbal commands
*/

27
src/buzz_update.cpp
View File

@ -45,7 +45,7 @@ void init_update_monitor(const char* bo_filename, const char* stand_by_script, c
{
perror("inotify_init error");
}
/*If simulation set the file monitor only for robot 1*/
/*If simulation set the file monitor only for robot 0*/
if (SIMULATION == 1 && robot_id == 0)
{
/* watch /.bzz file for any activity and report it back to update */
@ -321,6 +321,7 @@ void code_message_inqueue_process()
// fprintf(stdout,"[debug]Inside inmsg code running");
if (*(uint8_t*)(updater->inmsg_queue->queue) == SENT_CODE)
{
updated = 1;
size += sizeof(uint8_t);
if (*(uint16_t*)(updater->inmsg_queue->queue + size) > *(uint16_t*)(updater->update_no))
{
@ -413,9 +414,6 @@ void create_update_patch()
fprintf(stdout, "Launching bsdiff command: %s \n", genpatch.str().c_str());
system(genpatch.str().c_str());
// BETTER: CALL THE FUNCTION IN BSDIFF.CPP
// bsdiff_do(path + name1 + ".bo", path + name2 + ".bo", path + "diff.bo");
/* delete old files & rename new files */
remove((path + name1 + ".bo").c_str());
@ -460,7 +458,7 @@ void update_routine()
// fprintf(stdout,"[Debug : ]updater value = %i \n",updater->mode);
if (*(int*)updater->mode == CODE_RUNNING)
{
buzzvm_function_call(VM, "updated_neigh", 0);
buzzvm_function_call(VM, "updated_no_bct", 0);
// Check for update
if (check_update())
{
@ -479,7 +477,7 @@ void update_routine()
name = name.substr(0, name.find_last_of("."));
bzzfile_in_compile << path << name << "-update.bo";
uint8_t* BO_BUF = 0;
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb"); // to change file edit
FILE* fp = fopen(bzzfile_in_compile.str().c_str(), "rb");
if (!fp)
{
perror(bzzfile_in_compile.str().c_str());
@ -528,7 +526,6 @@ void update_routine()
// collect_data();
buzz_utility::buzz_update_set((updater)->bcode, (char*)dbgf_name, *(size_t*)(updater->bcode_size));
// buzzvm_function_call(m_tBuzzVM, "updated", 0);
updated = 1;
update_try_counter = 0;
timer_steps = 0;
}
@ -538,7 +535,6 @@ void update_routine()
/*Time out hit deceided to roll back*/
*(int*)(updater->mode) = CODE_RUNNING;
buzz_utility::buzz_script_set(old_bcfname, dbgf_name, (int)VM->robot);
updated = 1;
update_try_counter = 0;
timer_steps = 0;
}
@ -580,6 +576,9 @@ int test_set_code(uint8_t* BO_BUF, const char* dbgfname, size_t bcode_size)
buzzvm_pushs(VM, buzzvm_string_register(VM, "ROBOTS", 1));
buzzvm_pushi(VM, no_of_robot);
buzzvm_gstore(VM);
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_pushs(VM, buzzvm_string_register(VM, "UPDATE", 1));
buzzvm_gstore(VM);
return 1;
}
/*Unable to step something wrong*/
@ -634,18 +633,6 @@ void destroy_out_msg_queue()
delete_p(updater->outmsg_queue);
updater_msg_ready = 0;
}
int get_update_status()
{
return updated;
}
void set_read_update_status()
{
updated = 0;
}
/*int get_update_mode()
{
return (int)*(int*)(updater->mode);
}*/
int is_msg_present()
{

23
src/buzz_utility.cpp
View File

@ -351,17 +351,17 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
// Set byte code
if (buzzvm_set_bcode(VM, UP_BO_BUF, bcode_size) != BUZZVM_STATE_READY)
{
ROS_ERROR("[%i] %s: Error loading Buzz bytecode, VM state : %i", Robot_id, VM->state);
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)
{
ROS_ERROR("[%i] Error registering hooks, VM state : %i", Robot_id, VM->state);
buzzvm_destroy(&VM);
buzzdebug_destroy(&DBG_INFO);
ROS_ERROR("[%i] Error registering hooks (update)", Robot_id);
return 0;
}
@ -379,7 +379,7 @@ int buzz_update_set(uint8_t* UP_BO_BUF, const char* bdbg_filename, size_t bcode_
// 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);
ROS_ERROR("Error in calling init, VM state : %i", VM->state);
return 0;
}
// All OK
@ -564,4 +564,21 @@ int get_inmsg_size()
{
return IN_MSG.size();
}
string getuavstate()
/*
/ return current BVM state
---------------------------------------*/
{
std::string uav_state = "Unknown";
if(VM && VM->strings){
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM);
buzzobj_t obj = buzzvm_stack_at(VM, 1);
uav_state = string(obj->s.value.str);
buzzvm_pop(VM);
}
return uav_state;
}
}

13
src/buzzuav_closures.cpp
View File

@ -479,19 +479,6 @@ float* getgimbal()
return rc_gimbal;
}
string getuavstate()
/*
/ return current BVM state
---------------------------------------*/
{
static buzzvm_t VM = buzz_utility::get_vm();
buzzvm_pushs(VM, buzzvm_string_register(VM, "BVMSTATE", 1));
buzzvm_gload(VM);
buzzobj_t uav_state = buzzvm_stack_at(VM, 1);
buzzvm_pop(VM);
return uav_state->s.value.str;
}
int getcmd()
/*
/ return current mavros command to the FC

11
src/roscontroller.cpp
View File

@ -120,6 +120,7 @@ void roscontroller::RosControllerRun()
send_MPpayload();
// Check updater state and step code
update_routine();
ROS_WARN("OUT OF UPDATE ROUTINE");
if (time_step == BUZZRATE)
{
time_step = 0;
@ -144,10 +145,10 @@ void roscontroller::RosControllerRun()
// Call the flight controler service
flight_controller_service_call();
// Set multi message available after update
if (get_update_status())
{
set_read_update_status();
}
//if (get_update_status())
//{
// set_read_update_status();
//}
// Set ROBOTS variable (swarm size)
get_number_of_robots();
buzz_utility::set_robot_var(no_of_robots);
@ -455,7 +456,7 @@ void roscontroller::uavstate_publisher()
/----------------------------------------------------*/
{
std_msgs::String uavstate_msg;
uavstate_msg.data = buzzuav_closures::getuavstate();
uavstate_msg.data = buzz_utility::getuavstate();
uavstate_pub.publish(uavstate_msg);
}