robofsm: Prepare for logging state names

diff --git a/src/robofsm/robot.cc b/src/robofsm/robot.cc
index 6f132231d4a6ec20c99325fa90186cffc6b62a85..48b60aefecc615392405c4d7d455519ddafec7c9 100644 (file)
--- a/src/robofsm/robot.cc
+++ b/src/robofsm/robot.cc
@@ -273,4 +273,9 @@ float Robot::current_time() {
        clock_gettime(CLOCK_MONOTONIC, &now);
        timespec_subtract(diff, now, start_local);
        return diff.tv_sec + diff.tv_nsec/1000000000.0;
-}
\ No newline at end of file
+}
+
+void Robot::set_state_name(const char* name)
+{
+       sched.getActualHandle()->state_name = name;
+}

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 0973fbf2a7acf87101d80d25da624692d1596cde..f38c7f523ef2f327ee92a392ad85975b49a3c3c5 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -144,6 +144,7 @@ class Robot {
        void get_est_pos_trans(double &x, double &y, double &phi);
        void get_est_pos(double &x, double &y, double &phi);
        float current_time();
+       void set_state_name(const char* name);
 };
 
 const unsigned THREAD_PRIO_TRAJ_FOLLOWER = 90; /* As high as possible */

diff --git a/src/robofsm/scheduler.hpp b/src/robofsm/scheduler.hpp
index 8911924e09f4d406f4dfd98d8d40ff9572f3d173..3e1c47ba2fcbbacbcd6f8c4a1b2911482aceb91e 100644 (file)
--- a/src/robofsm/scheduler.hpp
+++ b/src/robofsm/scheduler.hpp
@@ -62,6 +62,7 @@ public:
                Processor *p =  new Processor( processor_context(*this, (my_processor)) );
                my_processor->processor_ = p;
                processor_queue_list.push_back(my_processor);
+               actual_processor = processor_queue_list.back();
                return handle;
        }
 
@@ -109,7 +110,9 @@ public:
                        else {
                                for(unsigned i = 0; i<processor_queue_list.size();i++) {
                                        if(processor_queue_list[i]->event_queue_.size()>0) {
+                                               actual_processor = processor_queue_list[i];
                                                processor_queue_list[i]->processor_->process_event(*(processor_queue_list[i]->event_queue_.front()));
+                                               printf("stav: %s\n",actual_processor->state_name);
                                                Guard g(queue_lock);
                                                processor_queue_list[i]->event_queue_.pop_front();
                                        }
@@ -122,6 +125,9 @@ public:
                        delete processor_queue_list[i];
                }
        }
+       processor_handle getActualHandle() {
+               return actual_processor;
+       }
 
     private:
        bool check_timer() {

diff --git a/src/robofsm/timedFSM.h b/src/robofsm/timedFSM.h
index 15a8bdf289b17233dbf91bf18c43b101157844b1..787bf0632d5d10368a70074b0a3839a85c161691 100644 (file)
--- a/src/robofsm/timedFSM.h
+++ b/src/robofsm/timedFSM.h
@@ -5,7 +5,8 @@
 #include <iostream>
 #include <events.h>
 #include <scheduler.hpp>
-
+#include <robot.h>
+#include <cxxabi.h>
 
 namespace sc = boost::statechart;
 namespace mpl = boost::mpl;
@@ -19,11 +20,13 @@ struct TimedState : boost::statechart::state<MostDerived, Context, Inner>
        std::list<Timer *> active_timers;
        typedef typename boost::statechart::state<MostDerived, Context, Inner> base_type; 
        Scheduler::processor_handle handle_;
+       int status;
     protected:
        typedef TimedState base_state;
     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);
        }
        ~TimedState() {
                if(handle_->timer_list_.timeouts.size()>0 && active_timers.size()>0)
@@ -52,21 +55,28 @@ struct TimedSimpleState : boost::statechart::simple_state<MostDerived, Context,
        std::list<Timer *> active_timers;
        typedef typename boost::statechart::simple_state<MostDerived, Context, Inner> base_type; 
        Scheduler::processor_handle handle_;
+       bool handle_owned;
+       int status;
     protected:
        typedef TimedSimpleState base_state;
     public:
-       TimedSimpleState() {
-               handle_ = base_type::outermost_context().my_handle();
+       TimedSimpleState() : handle_owned(false) {
+               robot.set_state_name(abi::__cxa_demangle(typeid(MostDerived).name(),0,0,&status));
        }
        ~TimedSimpleState() {
-               if(handle_->timer_list_.timeouts.size()>0 && active_timers.size()>0)
-               {
-                       std::list<Timer *>::iterator it;
-                       for(it = active_timers.begin(); it!= active_timers.end(); it++)
-                               handle_->timer_list_.timeouts.remove(*it);
-               }
+               if(handle_owned)
+                       if(handle_->timer_list_.timeouts.size()>0 && active_timers.size()>0)
+                       {
+                               std::list<Timer *>::iterator it;
+                               for(it = active_timers.begin(); it!= active_timers.end(); it++)
+                                       handle_->timer_list_.timeouts.remove(*it);
+                       }
        }
        void runTimer(Timer &t, long int milisec, const  boost::intrusive_ptr< boost::statechart::event_base > &e) {
+               if(!handle_owned) {
+                       handle_ = base_type::outermost_context().my_handle();
+                       handle_owned = true;
+               }
                handle_->timer_list_.addTimer(t, milisec, e);
                active_timers.push_back(&t);
        }