[openrtm-commit:00791] r2355 - trunk/OpenRTM-aist/src/lib/rtm
openrtm ＠ openrtm.org
openrtm ＠ openrtm.org
2012年 5月 7日 (月) 11:08:36 JST
Author: n-ando
Date: 2012-05-07 11:08:36 +0900 (Mon, 07 May 2012)
New Revision: 2355
Modified:
trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.cpp
trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.h
Log:
Now RTObjectStateMachine object calls RTC's callbacks via servant if it is available. refs #2418
Modified: trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.cpp
===================================================================
--- trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.cpp 2012-05-07 00:47:08 UTC (rev 2354)
+++ trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.cpp 2012-05-07 02:08:36 UTC (rev 2355)
@@ -17,6 +17,8 @@
*/
#include <rtm/RTObjectStateMachine.h>
+#include <rtm/Manager.h>
+#include <rtm/RTObject.h>
#include <iostream>
#include <stdio.h>
@@ -27,7 +29,8 @@
: m_id(id),
m_rtobj(RTC::LightweightRTObject::_duplicate(comp)),
m_sm(NUM_OF_LIFECYCLESTATE),
- m_ca(false), m_dfc(false), m_fsm(false), m_mode(false)
+ m_ca(false), m_dfc(false), m_fsm(false), m_mode(false),
+ m_rtobjPtr(NULL), m_measure(false)
{
m_caVar = RTC::ComponentAction::_nil();
m_dfcVar = RTC::DataFlowComponentAction::_nil();
@@ -155,7 +158,11 @@
setComponentAction(const RTC::LightweightRTObject_ptr comp)
{
m_caVar = RTC::ComponentAction::_narrow(comp);
- if (!CORBA::is_nil(m_caVar)) { m_ca = true; }
+ if (CORBA::is_nil(m_caVar)) { return; }
+ m_ca = true;
+ PortableServer::POA_ptr poa = RTC::Manager::instance().getPOA();
+ m_rtobjPtr =
+ dynamic_cast<RTC::RTObject_impl*>(poa->reference_to_servant(comp));
}
void RTObjectStateMachine::
@@ -182,16 +189,40 @@
// RTC::ComponentAction operations
void RTObjectStateMachine::onStartup(void)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ m_rtobjPtr->on_startup(m_id);
+ return;
+ }
+ // call Object reference
if (!m_ca) { return; }
m_caVar->on_startup(m_id);
}
void RTObjectStateMachine::onShutdown(void)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ m_rtobjPtr->on_shutdown(m_id);
+ return;
+ }
+ // call Object reference
if (!m_ca) { return; }
m_caVar->on_shutdown(m_id);
}
void RTObjectStateMachine::onActivated(const ExecContextStates& st)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ if (m_rtobjPtr->on_activated(m_id) != RTC::RTC_OK)
+ {
+ m_sm.goTo(RTC::ERROR_STATE);
+ }
+ return;
+ }
+ // call Object reference
if (!m_ca) { return; }
if (m_caVar->on_activated(m_id) != RTC::RTC_OK)
{
@@ -203,24 +234,55 @@
void RTObjectStateMachine::onDeactivated(const ExecContextStates& st)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ m_rtobjPtr->on_deactivated(m_id);
+ return;
+ }
+ // call Object reference
if (!m_ca) { return; }
m_caVar->on_deactivated(m_id);
}
void RTObjectStateMachine::onAborting(const ExecContextStates& st)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ m_rtobjPtr->on_aborting(m_id);
+ return;
+ }
+ // call Object reference
if (!m_ca) { return; }
- m_caVar->on_error(m_id);
+ m_caVar->on_aborting(m_id);
}
void RTObjectStateMachine::onError(const ExecContextStates& st)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ m_rtobjPtr->on_error(m_id);
+ return;
+ }
+ // call Object reference
if (!m_ca) { return; }
m_caVar->on_error(m_id);
}
void RTObjectStateMachine::onReset(const ExecContextStates& st)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ if (m_rtobjPtr->on_reset(m_id) != RTC::RTC_OK)
+ {
+ m_sm.goTo(RTC::ERROR_STATE);
+ }
+ return;
+ }
+ // call Object reference
if (!m_ca) { return; }
if (m_caVar->on_reset(m_id) != RTC::RTC_OK)
{
@@ -233,9 +295,58 @@
// RTC::DataflowComponentAction
void RTObjectStateMachine::onExecute(const ExecContextStates& st)
{
+ static int count;
+ double max_interval, min_interval, mean_interval, stddev;
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ if (m_measure) { m_svtMeasure.tick(); }
+ if (m_rtobjPtr->on_execute(m_id) != RTC::RTC_OK)
+ {
+ m_sm.goTo(RTC::ERROR_STATE);
+ }
+ if (m_measure)
+ {
+ m_svtMeasure.tack();
+ if (count > 1000)
+ {
+ count = 0;
+ m_svtMeasure.getStatistics(max_interval, min_interval,
+ mean_interval, stddev);
+ std::cout << "[servant] ";
+ std::cout << " max: " << max_interval;
+ std::cout << " min: " << min_interval;
+ std::cout << " mean: " << mean_interval;
+ std::cout << " stddev: " << stddev;
+ std::cout << std::endl;
+ }
+ ++count;
+ }
+ return;
+ }
+ // call Object reference
if (!m_dfc) { return; }
- if (m_dfcVar->on_execute(m_id) != RTC::RTC_OK)
+ if (m_measure) { m_refMeasure.tick(); }
+ RTC::ReturnCode_t ret = m_dfcVar->on_execute(m_id);
+ if (m_measure)
{
+ m_refMeasure.tack();
+ if (count > 1000)
+ {
+ count = 0;
+ m_refMeasure.getStatistics(max_interval, min_interval,
+ mean_interval, stddev);
+ std::cout << "[objref] ";
+ std::cout << " max: " << max_interval;
+ std::cout << " min: " << min_interval;
+ std::cout << " mean: " << mean_interval;
+ std::cout << " stddev: " << stddev;
+ std::cout << std::endl;
+ }
+ ++count;
+ }
+ if (ret != RTC::RTC_OK)
+ {
m_sm.goTo(RTC::ERROR_STATE);
return;
}
@@ -244,6 +355,16 @@
void RTObjectStateMachine::onStateUpdate(const ExecContextStates& st)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ if (m_rtobjPtr->on_state_update(m_id) != RTC::RTC_OK)
+ {
+ m_sm.goTo(RTC::ERROR_STATE);
+ }
+ return;
+ }
+ // call Object reference
if (!m_dfc) { return; }
if (m_dfcVar->on_state_update(m_id) != RTC::RTC_OK)
{
@@ -255,6 +376,16 @@
void RTObjectStateMachine::onRateChanged(void)
{
+ // call Servant
+ if (m_rtobjPtr != NULL)
+ {
+ if (m_rtobjPtr->on_rate_changed(m_id) != RTC::RTC_OK)
+ {
+ m_sm.goTo(RTC::ERROR_STATE);
+ }
+ return;
+ }
+ // call Object reference
if (!m_dfc) { return; }
if (m_dfcVar->on_rate_changed(m_id) != RTC::RTC_OK)
{
Modified: trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.h
===================================================================
--- trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.h 2012-05-07 00:47:08 UTC (rev 2354)
+++ trunk/OpenRTM-aist/src/lib/rtm/RTObjectStateMachine.h 2012-05-07 02:08:36 UTC (rev 2355)
@@ -22,6 +22,7 @@
#include <stdlib.h>
#include <rtm/SystemLogger.h>
#include <coil/NonCopyable.h>
+#include <coil/TimeMeasure.h>
#include <rtm/idl/RTCSkel.h>
#include <rtm/StateMachine.h>
#include <assert.h>
@@ -32,7 +33,10 @@
#ifdef WIN32
#pragma warning( disable : 4290 )
#endif
-
+namespace RTC
+{
+ class RTObject_impl;
+}
namespace RTC_impl
{
typedef RTC::ExecutionContextHandle_t Ecid;
@@ -82,7 +86,7 @@
bool isCurrentState(ExecContextState state);
bool isNextState(ExecContextState state);
void goTo(ExecContextState state);
-
+
// Workers
void workerPreDo(void);
void workerDo(void);
@@ -111,10 +115,13 @@
RTC::DataFlowComponentAction_var m_dfcVar;
RTC::FsmParticipantAction_var m_fsmVar;
RTC::MultiModeComponentAction_var m_modeVar;
+ RTC::RTObject_impl* m_rtobjPtr;
+ bool m_measure;
// char dara[1000];
// Component action invoker
-
+ coil::TimeMeasure m_svtMeasure;
+ coil::TimeMeasure m_refMeasure;
};
}; // namespace RTC
openrtm-commit メーリングリストの案内