robofsm: Logging state names

State names are published to orte from now.

diff --git a/src/robofsm/robot.cc b/src/robofsm/robot.cc
index 48b60aefecc615392405c4d7d455519ddafec7c9..4ccdd3b0f3bc32c24dc2c8cef2596b2afa120834 100644 (file)
--- a/src/robofsm/robot.cc
+++ b/src/robofsm/robot.cc
@@ -278,4 +278,17 @@ float Robot::current_time() {
 void Robot::set_state_name(const char* name)
 {
        sched.getActualHandle()->state_name = name;
+       trans_callback();
 }
+
+void Robot::trans_callback()
+{
+       if (MAIN == sched.getActualHandle()) {
+               strncpy(robot.orte.fsm_main.state_name, MAIN->state_name, sizeof(robot.orte.fsm_main.state_name));
+               ORTEPublicationSend(robot.orte.publication_fsm_main);
+       } else if (MOTION == sched.getActualHandle()) {
+               strncpy(robot.orte.fsm_motion.state_name, MOTION->state_name, sizeof(robot.orte.fsm_motion.state_name));
+               ORTEPublicationSend(robot.orte.publication_fsm_motion);
+       }
+}
+

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index f38c7f523ef2f327ee92a392ad85975b49a3c3c5..ce8aa4ce06fc79db9d3ddd2b4a50e7c239f334fa 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -53,6 +53,7 @@ enum robot_component {
 class Robot {
        static void int_handler(int sig);
        void block_signals();
+       void trans_callback();
        //static void trans_callback(struct robo_fsm *fsm) publication stringnames of states
     public:
        pthread_mutex_t lock;

diff --git a/src/robofsm/timedFSM.h b/src/robofsm/timedFSM.h
index 787bf0632d5d10368a70074b0a3839a85c161691..770d26b9cc2bebe7f502958375593dde7763b8c8 100644 (file)
--- a/src/robofsm/timedFSM.h
+++ b/src/robofsm/timedFSM.h
@@ -5,9 +5,10 @@
 #include <iostream>
 #include <events.h>
 #include <scheduler.hpp>
+#ifndef __LOCAL 
 #include <robot.h>
 #include <cxxabi.h>
-
+#endif
 namespace sc = boost::statechart;
 namespace mpl = boost::mpl;
 
@@ -26,7 +27,9 @@ struct TimedState : boost::statechart::state<MostDerived, Context, Inner>
     public:
        TimedState(typename base_type::my_context ctx ) : base_type( ctx ) {
                handle_ = base_type::outermost_context().my_handle();
-               handle_->state_name = abi::__cxa_demangle(typeid(MostDerived).name(),0,0,&status);
+               #ifndef __LOCAL 
+               robot.set_state_name(abi::__cxa_demangle(typeid(MostDerived).name(),0,0,&status));
+               #endif
        }
        ~TimedState() {
                if(handle_->timer_list_.timeouts.size()>0 && active_timers.size()>0)
@@ -61,7 +64,9 @@ struct TimedSimpleState : boost::statechart::simple_state<MostDerived, Context,
        typedef TimedSimpleState base_state;
     public:
        TimedSimpleState() : handle_owned(false) {
+               #ifndef __LOCAL 
                robot.set_state_name(abi::__cxa_demangle(typeid(MostDerived).name(),0,0,&status));
+               #endif
        }
        ~TimedSimpleState() {
                if(handle_owned)