add out port for emergency_mode

snozawa · Jul 3, 2015 · 9cec4b7 · 9cec4b7
1 parent 4b408f4
commit 9cec4b7
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 0 deletions.
diff --git a/rtc/EmergencyStopper/EmergencyStopper.cpp b/rtc/EmergencyStopper/EmergencyStopper.cpp
@@ -41,6 +41,7 @@ EmergencyStopper::EmergencyStopper(RTC::Manager* manager)
  m_qRefIn("qRef", m_qRef),
  m_emergencySignalIn("emergencySignal", m_emergencySignal),
  m_qOut("q", m_q),
+ m_emergencyModeOut("emergencyMode", m_emergencyMode),
  m_EmergencyStopperServicePort("EmergencyStopperService"),
  // </rtc-template>
  m_robot(hrp::BodyPtr()),
@@ -72,6 +73,7 @@ RTC::ReturnCode_t EmergencyStopper::onInitialize()
 
  // Set OutPort buffer
  addOutPort("q", m_qOut);
+ addOutPort("emergencyMode", m_emergencyModeOut);
 
  // Set service provider to Ports
  m_EmergencyStopperServicePort.registerProvider("service0", "EmergencyStopperService", m_service0);
@@ -244,6 +246,11 @@ RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id)
  std::cerr << std::endl;
  }
  m_qOut.write();
+
+ m_emergencyMode.data = is_stop_mode;
+ m_emergencyMode.tm = m_qRef.tm;
+ m_emergencyModeOut.write();
+
  prev_is_stop_mode = is_stop_mode;
  return RTC::RTC_OK;
 }

diff --git a/rtc/EmergencyStopper/EmergencyStopper.h b/rtc/EmergencyStopper/EmergencyStopper.h
@@ -112,6 +112,7 @@ class EmergencyStopper
  TimedDoubleSeq m_qRef;
  TimedDoubleSeq m_q;
  TimedLong m_emergencySignal;
+ TimedLong m_emergencyMode;
 
  // DataInPort declaration
  // <rtc-template block="inport_declare">
@@ -123,6 +124,7 @@ class EmergencyStopper
  // DataOutPort declaration
  // <rtc-template block="outport_declare">
  OutPort<TimedDoubleSeq> m_qOut;
+ OutPort<TimedLong> m_emergencyModeOut;
 
  // </rtc-template>