Fixed the update crash caused by BVM state publisher.

buzz_scripts/include/update.bzz

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

buzz_scripts/minimal.bzz

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

buzz_scripts/stand_by.bzz

@@ -1,24 +1,31 @@
+include "act/states.bzz"
+include "vstigenv.bzz"
+
 updated="update_ack"
 update_no=0
-function init(){
+BVMSTATE = "UPDATESTANDBY"
-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
-})
 
-s = swarm.create(1)
+# Executed once at init time.
-s.join()
+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)
+	neighbors.listen(updated,
-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)
@@ -27,9 +34,8 @@ neighbors.listen(updated,
 
 }
 
-
+# Executed at each time step.
 function step() {
-
+	stand_by()
-stand_by()
 
 }

include/buzz_update.h

@@ -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();
+/****************************************************/
-
+/*Compiles a bzz script to bo and bdbg*/
-void set_read_update_status();
+/***************************************************/
 
 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

include/buzz_utility.h

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

include/buzzuav_closures.h

@@ -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
  */

src/buzz_update.cpp

@@ -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()
 {

src/buzz_utility.cpp

@@ -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;
   }
 
@@ -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;
+}
+
 }

src/buzzuav_closures.cpp

@@ -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

src/roscontroller.cpp

@@ -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())
+      //if (get_update_status())
-      {
+      //{
-        set_read_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);
 }