Merge pull request #36 from robotkernel/feat/state_err

Feat/state err

Author: robert-burger

include/robotkernel/module_base.h

@@ -230,22 +230,22 @@ class module_base :
          */
         virtual int set_state(module_state_t state);
 
-        //! State transition from PREOP to SAFEOP
+        //! State transition from OP to SAFEOP
         virtual void set_state_op_2_safeop() {}
 
-        //! State transition from PREOP to SAFEOP
+        //! State transition from SAFEOP to PREOP
         virtual void set_state_safeop_2_preop() {}
 
-        //! State transition from PREOP to SAFEOP
+        //! State transition from PREOP to INIT
         virtual void set_state_preop_2_init() {}
 
-        //! State transition from PREOP to SAFEOP
+        //! State transition from INIT to PREOP
         virtual void set_state_init_2_preop() {}
 
         //! State transition from PREOP to SAFEOP
         virtual void set_state_preop_2_safeop() {}
 
-        //! State transition from PREOP to SAFEOP
+        //! State transition from SAFEOP to OP
         virtual void set_state_safeop_2_op() {}
 
         //! Get module state machine state.

include/robotkernel/robotkernel.h

@@ -30,6 +30,9 @@
 #include "robotkernel/device.h"
 #include "robotkernel/device_listener.h"
 #include "robotkernel/service.h"
+#include "robotkernel/trigger.h"
+#include "robotkernel/process_data.h"
+#include "robotkernel/stream.h"
 #include "robotkernel/helpers.h"
 
 namespace robotkernel {
@@ -108,6 +111,7 @@ inline std::shared_ptr<T> get_device(const std::string& dev_name) {
 extern const std::string name(void);
 
 }; // namespace robotkernel;
+        
 
 #endif // ROBOTKERNEL_KERNEL_BASE_H
 

src/module_base.cpp

@@ -52,14 +52,22 @@ int module_base::set_state(module_state_t state) {
     // get transition
     uint32_t transition = GEN_STATE(this->state, state);
 
+#define try_set_state(transition) \
+    try { \
+        set_state_ ## transition(); \
+    } catch (std::exception& e) { \
+        log(error, "caught exception during " #transition ": %s\n", e.what()); \
+        return module_state_error; \
+    }
+
     switch (transition) {
         case op_2_safeop:
         case op_2_preop:
         case op_2_init:
         case op_2_boot: {
             // ====> stop sending commands
             std::unique_lock<std::mutex> lock(state_mtx);
-            set_state_op_2_safeop();
+            try_set_state(op_2_safeop);
             this->state = module_state_safeop;
 
             if (state == module_state_safeop)
@@ -70,7 +78,7 @@ int module_base::set_state(module_state_t state) {
         case safeop_2_boot: {
             // ====> stop receiving measurements
             std::unique_lock<std::mutex> lock(state_mtx);
-            set_state_safeop_2_preop();
+            try_set_state(safeop_2_preop);
             this->state = module_state_preop;
 
             if (state == module_state_preop)
@@ -80,7 +88,7 @@ int module_base::set_state(module_state_t state) {
         case preop_2_boot: {
             // ====> deinit devices
             std::unique_lock<std::mutex> lock(state_mtx);
-            set_state_preop_2_init();
+            try_set_state(preop_2_init);
             this->state = module_state_init;
         }
         case init_2_init:
@@ -99,7 +107,7 @@ int module_base::set_state(module_state_t state) {
         case init_2_preop: {
             // ====> init devices
             std::unique_lock<std::mutex> lock(state_mtx);
-            set_state_init_2_preop();
+            try_set_state(init_2_preop);
             this->state = module_state_preop;
 
             if (state == module_state_preop)
@@ -109,7 +117,7 @@ int module_base::set_state(module_state_t state) {
         case preop_2_safeop: {
             // ====> start receiving measurements
             std::unique_lock<std::mutex> lock(state_mtx);
-            set_state_preop_2_safeop();
+            try_set_state(preop_2_safeop);
             this->state = module_state_safeop;
 
             if (state == module_state_safeop)
@@ -118,7 +126,7 @@ int module_base::set_state(module_state_t state) {
         case safeop_2_op: {
             // ====> start sending commands
             std::unique_lock<std::mutex> lock(state_mtx);
-            set_state_safeop_2_op();
+            try_set_state(safeop_2_op);
             this->state = module_state_op;
             break;
         }