From 48617b712a7296e3bb44433f7ec9a751b76073a0 Mon Sep 17 00:00:00 2001
From: Petr Silhavik <silhavik.p@gmail.com>
Date: Sun, 27 Jan 2013 22:05:50 +0100
Subject: [PATCH] robofsm: Logging state names

State names are published to orte from now.
---
 src/robofsm/robot.cc   | 13 +++++++++++++
 src/robofsm/robot.h    |  1 +
 src/robofsm/timedFSM.h |  9 +++++++--
 3 files changed, 21 insertions(+), 2 deletions(-)

diff --git a/src/robofsm/robot.cc b/src/robofsm/robot.cc
index 48b60aef..4ccdd3b0 100644
--- 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 f38c7f52..ce8aa4ce 100644
--- 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 787bf063..770d26b9 100644
--- 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)
-- 
2.39.2