add source files of EmergencyStopper rtc

doc/CMakeLists.txt

@@ -11,6 +11,7 @@ set(input_files package.h utilities.h
  ../rtc/Joystick2PanTiltAngles/Joystick2PanTiltAngles.txt
  ../rtc/JpegDecoder/JpegDecoder.txt
  ../rtc/NullComponent/NullComponent.txt
+ ../rtc/EmergencyStopper/EmergencyStopper.txt
  ../rtc/OccupancyGridMap3D/OccupancyGridMap3D.txt
  ../rtc/OGMap3DViewer/OGMap3DViewer.txt
  ../rtc/Range2PointCloud/Range2PointCloud.txt

doc/Doxyfile.in

@@ -490,6 +490,7 @@ INPUT = @CMAKE_CURRENT_SOURCE_DIR@/package.h \
  @CMAKE_CURRENT_SOURCE_DIR@/../rtc/KalmanFilter \
  @CMAKE_CURRENT_SOURCE_DIR@/../rtc/MLSFilter \
  @CMAKE_CURRENT_SOURCE_DIR@/../rtc/NullComponent \
+ @CMAKE_CURRENT_SOURCE_DIR@/../rtc/EmergencyStopper \
  @CMAKE_CURRENT_SOURCE_DIR@/../rtc/OGMap3DViewer \
  @CMAKE_CURRENT_SOURCE_DIR@/../rtc/OccupancyGridMap3D \
  @CMAKE_CURRENT_SOURCE_DIR@/../rtc/PCDLoader \

doc/package.h

@@ -42,6 +42,7 @@ To use python scripts to create RT components, connect ports and get/set propert
  <li>\ref KalmanFilter</li>
  <li>\ref MLSFilter</li>
  <li>\ref NullComponent</li>
+ <li>\ref EmergencyStopper</li>
  <li>\ref OGMap3DViewer</li>
  <li>\ref OccupancyGridMap3D</li>
  <li>\ref PCDLoader</li>

idl/CMakeLists.txt

@@ -8,6 +8,7 @@ set(idl_files
  ExecutionProfileService.idl
  OGMap3DService.idl
  NullService.idl
+ EmergencyStopperService.idl
  TimeKeeperService.idl
  Img.idl
  HRPDataTypes.idl

idl/EmergencyStopperService.idl

@@ -0,0 +1,12 @@
+#ifndef __EMERGENCYSTOPPER_SERVICE_IDL__
+#define __EMERGENCYSTOPPER_SERVICE_IDL__
+
+module OpenHRP
+{
+ interface EmergencyStopperService
+ {
+ void echo(in string msg);
+ };
+};
+
+#endif

launch/samplerobot.launch

@@ -51,6 +51,7 @@
  -o "example.RemoveForceSensorLinkOffset.config_file:$(arg CONF_FILE)"
  -o "example.KalmanFilter.config_file:$(arg CONF_FILE)"
  -o "example.Stabilizer.config_file:$(arg CONF_FILE)"
+ -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)"
  -o "example.CollisionDetector.config_file:$(arg CONF_FILE)"
  -o "example.SoftErrorLimiter.config_file:$(arg CONF_FILE)"
  -o "example.$(arg JOINT_CONTROLLER).config_file:$(arg CONF_FILE)"

python/hrpsys_config.py

@@ -201,6 +201,11 @@ class HrpsysConfigurator:
  abc_version = None
  st_version = None
 
+ # CollisionDetector
+ es = None
+ es_svc = None
+ es_version = None
+
  # CollisionDetector
  co = None
  co_svc = None
@@ -641,6 +646,7 @@ def getRTCListUnstable(self):
  ['ic', "ImpedanceController"],
  ['abc', "AutoBalancer"],
  ['st', "Stabilizer"],
+ ['es', "EmergencyStopper"],
  ['co', "CollisionDetector"],
  ['tc', "TorqueController"],
  # ['te', "ThermoEstimator"],

rtc/CMakeLists.txt

@@ -32,6 +32,7 @@ add_subdirectory(ExtractCameraImage)
 add_subdirectory(CaptureController)
 add_subdirectory(RangeNoiseMixer)
 add_subdirectory(AverageFilter)
+add_subdirectory(EmergencyStopper)
 if (USE_HRPSYSUTIL)
  add_subdirectory(Viewer)
  add_subdirectory(CameraImageViewer)

rtc/EmergencyStopper/CMakeLists.txt

@@ -0,0 +1,15 @@
+set(comp_sources EmergencyStopper.cpp EmergencyStopperService_impl.cpp)
+set(libs hrpsysBaseStub)
+add_library(EmergencyStopper SHARED ${comp_sources})
+target_link_libraries(EmergencyStopper ${libs})
+set_target_properties(EmergencyStopper PROPERTIES PREFIX "")
+
+add_executable(EmergencyStopperComp EmergencyStopperComp.cpp ${comp_sources})
+target_link_libraries(EmergencyStopperComp ${libs})
+
+set(target EmergencyStopper EmergencyStopperComp)
+
+install(TARGETS ${target}
+ RUNTIME DESTINATION bin CONFIGURATIONS Release Debug
+ LIBRARY DESTINATION lib CONFIGURATIONS Release Debug
+)

rtc/EmergencyStopper/EmergencyStopper.cpp

@@ -0,0 +1,197 @@
+// -*- C++ -*-
+/*!
+ * @file EmergencyStopper.cpp
+ * @brief emergency stopper
+ * $Date$
+ *
+ * $Id$
+ */
+
+#include "EmergencyStopper.h"
+#include "util/VectorConvert.h"
+
+// Module specification
+// <rtc-template block="module_spec">
+static const char* emergencystopper_spec[] =
+ {
+ "implementation_id", "EmergencyStopper",
+ "type_name", "EmergencyStopper",
+ "description", "emergency stopper",
+ "version", HRPSYS_PACKAGE_VERSION,
+ "vendor", "AIST",
+ "category", "example",
+ "activity_type", "DataFlowComponent",
+ "max_instance", "10",
+ "language", "C++",
+ "lang_type", "compile",
+ // Configuration variables
+ "conf.default.string", "test",
+ "conf.default.intvec", "1,2,3",
+ "conf.default.double", "1.234",
+
+ ""
+ };
+// </rtc-template>
+
+EmergencyStopper::EmergencyStopper(RTC::Manager* manager)
+ : RTC::DataFlowComponentBase(manager),
+ // <rtc-template block="initializer">
+ m_dataIn("dataIn", m_data),
+ m_dataOut("dataOut", m_data),
+ m_EmergencyStopperServicePort("EmergencyStopperService"),
+ // </rtc-template>
+ dummy(0)
+{
+ std::cout << "EmergencyStopper::EmergencyStopper()" << std::endl;
+ m_data.data = 0;
+}
+
+EmergencyStopper::~EmergencyStopper()
+{
+ std::cout << "EmergencyStopper::~EmergencyStopper()" << std::endl;
+}
+
+
+
+RTC::ReturnCode_t EmergencyStopper::onInitialize()
+{
+ std::cout << m_profile.instance_name << ": onInitialize()" << std::endl;
+ // <rtc-template block="bind_config">
+ // Bind variables and configuration variable
+ bindParameter("string", confstring, "testtest");
+ bindParameter("intvec", confintvec, "4,5,6,7");
+ bindParameter("double", confdouble, "4.567");
+
+ // </rtc-template>
+
+ // Registration: InPort/OutPort/Service
+ // <rtc-template block="registration">
+ // Set InPort buffers
+ addInPort("dataIn", m_dataIn);
+
+ // Set OutPort buffer
+ addOutPort("dataOut", m_dataOut);
+
+ // Set service provider to Ports
+ m_EmergencyStopperServicePort.registerProvider("service0", "EmergencyStopperService", m_EmergencyStopperService);
+
+ // Set service consumers to Ports
+
+ // Set CORBA Service Ports
+ addPort(m_EmergencyStopperServicePort);
+
+ // </rtc-template>
+
+ RTC::Properties& prop = getProperties();
+ std::cout << "prop[\"testconf\"] = " << prop["testconf"] << std::endl;
+
+ return RTC::RTC_OK;
+}
+
+
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onFinalize()
+{
+ return RTC::RTC_OK;
+}
+*/
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onStartup(RTC::UniqueId ec_id)
+{
+ return RTC::RTC_OK;
+}
+*/
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onShutdown(RTC::UniqueId ec_id)
+{
+ return RTC::RTC_OK;
+}
+*/
+
+RTC::ReturnCode_t EmergencyStopper::onActivated(RTC::UniqueId ec_id)
+{
+ std::cout << m_profile.instance_name<< ": onActivated(" << ec_id << ")" << std::endl;
+ return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t EmergencyStopper::onDeactivated(RTC::UniqueId ec_id)
+{
+ std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl;
+ return RTC::RTC_OK;
+}
+
+RTC::ReturnCode_t EmergencyStopper::onExecute(RTC::UniqueId ec_id)
+{
+ // std::cout << m_profile.instance_name<< ": onExecute(" << ec_id << "), data = " << m_data.data << std::endl;
+ // std::cout << "confstring = " << confstring << std::endl;
+ // std::cout << "confintvec = ";
+ // for (unsigned int i=0; i<confintvec.size(); i++){
+ // std::cout << confintvec[i] << " ";
+ // }
+ // std::cout << std::endl;
+ // std::cout << "confdouble = " << confdouble << std::endl;
+
+ while (m_dataIn.isNew()){
+ m_dataIn.read();
+ std::cout << m_profile.instance_name << ": read(), data = " << m_data.data << std::endl;
+ }
+ m_data.data += 1;
+
+ m_dataOut.write();
+ return RTC::RTC_OK;
+}
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onAborting(RTC::UniqueId ec_id)
+{
+ return RTC::RTC_OK;
+}
+*/
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onError(RTC::UniqueId ec_id)
+{
+ return RTC::RTC_OK;
+}
+*/
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onReset(RTC::UniqueId ec_id)
+{
+ return RTC::RTC_OK;
+}
+*/
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onStateUpdate(RTC::UniqueId ec_id)
+{
+ return RTC::RTC_OK;
+}
+*/
+
+/*
+RTC::ReturnCode_t EmergencyStopper::onRateChanged(RTC::UniqueId ec_id)
+{
+ return RTC::RTC_OK;
+}
+*/
+
+
+
+extern "C"
+{
+
+ void EmergencyStopperInit(RTC::Manager* manager)
+ {
+ RTC::Properties profile(emergencystopper_spec);
+ manager->registerFactory(profile,
+ RTC::Create<EmergencyStopper>,
+ RTC::Delete<EmergencyStopper>);
+ }
+
+};
+
+