Merge pull request start-jsk#763 from mmurooka/pub-emergency-mode

[hrpsys_ros_bridge_tutorials] publish emergency_mode from EmergencyStopper rtc
YutaKojio · Jul 3, 2015 · 10b2bbe · 10b2bbe
2 parents e1ba621 + 11f8ed2
commit 10b2bbe
Show file tree

Hide file tree

Showing 5 changed files with 22 additions and 1 deletion.
diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
@@ -227,6 +227,7 @@
           output="screen" args ="$(arg openrtm_args)" />
     <node pkg="hrpsys_ros_bridge" name="HardEmergencyStopperServiceROSBridge" type="EmergencyStopperServiceROSBridgeComp"
           output="screen" args ="$(arg openrtm_args)" />
+    <rtconnect from="es.rtc:emergencyMode" to="HrpsysSeqStateROSBridge0.rtc:emergencyMode" subscription_type="new"/>
     <rtconnect from="EmergencyStopperServiceROSBridge.rtc:EmergencyStopperService" to="es.rtc:EmergencyStopperService"  subscription_type="new"/>
     <rtconnect from="HardEmergencyStopperServiceROSBridge.rtc:EmergencyStopperService" to="hes.rtc:EmergencyStopperService"  subscription_type="new"/>
     <rtactivate component="EmergencyStopperServiceROSBridge.rtc" />

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -138,6 +138,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onInitialize() {
     tmpname.erase(0,4); // Remove "ref_"
     cop_pub[i] = nh.advertise<geometry_msgs::PointStamped>(tmpname+"_cop", 10);
   }
+  em_mode_pub = nh.advertise<std_msgs::Int32>("emergency_mode", 10);
 
   return RTC::RTC_OK;
 }
@@ -786,6 +787,20 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
       }
   }
 
+  if ( m_emergencyModeIn.isNew() ) {
+    try {
+      m_emergencyModeIn.read();
+      //ROS_DEBUG_STREAM("[" << getInstanceName() << "] @onExecute" << "  emergencyMode: " << m_emergencyMode.data);
+      std_msgs::Int32 em_mode;
+      em_mode.data = m_emergencyMode.data;
+      em_mode_pub.publish(em_mode);
+    }
+    catch(const std::runtime_error &e)
+      {
+        ROS_ERROR_STREAM("[" << getInstanceName() << "] " << e.what());
+      }
+  }
+
   //
   return RTC::RTC_OK;
 }

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -15,6 +15,7 @@
 // ros
 #include "ros/ros.h"
 #include "rosgraph_msgs/Clock.h"
+#include "std_msgs/Int32.h"
 #include "sensor_msgs/JointState.h"
 #include "nav_msgs/Odometry.h"
 #include "geometry_msgs/WrenchStamped.h"
@@ -52,7 +53,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
                                hrpsys_ros_bridge::SetSensorTransformation::Response& res);
  private:
   ros::NodeHandle nh;
-  ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, odom_pub, imu_pub;
+  ros::Publisher joint_state_pub, joint_controller_state_pub, mot_states_pub, diagnostics_pub, clock_pub, zmp_pub, odom_pub, imu_pub, em_mode_pub;
   ros::Subscriber trajectory_command_sub;
   std::vector<ros::Publisher> fsensor_pub, cop_pub;
   actionlib::SimpleActionServer<pr2_controllers_msgs::JointTrajectoryAction> joint_trajectory_server;

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
@@ -37,6 +37,7 @@ HrpsysSeqStateROSBridgeImpl::HrpsysSeqStateROSBridgeImpl(RTC::Manager* manager)
     m_servoStateIn("servoState", m_servoState),
     m_rszmpIn("rszmp", m_rszmp),
     m_rsCOPInfoIn("rsCOPInfo", m_rsCOPInfo),
+    m_emergencyModeIn("emergencyMode", m_emergencyMode),
     m_mctorqueOut("mctorque", m_mctorque),
     m_SequencePlayerServicePort("SequencePlayerService")
 
@@ -62,6 +63,7 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onInitialize()
   addInPort("rszmp", m_rszmpIn);
   addInPort("servoState", m_servoStateIn);
   addInPort("rsCOPInfo", m_rsCOPInfoIn);
+  addInPort("emergencyMode", m_emergencyModeIn);
 
   // Set OutPort buffer
   addOutPort("mctorque", m_mctorqueOut);

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
@@ -136,6 +136,8 @@ class HrpsysSeqStateROSBridgeImpl  : public RTC::DataFlowComponentBase
   InPort<TimedPoint3D> m_rszmpIn;
   TimedDoubleSeq m_rsCOPInfo;
   InPort<TimedDoubleSeq> m_rsCOPInfoIn;
+  TimedLong m_emergencyMode;
+  InPort<TimedLong> m_emergencyModeIn;
 
   // </rtc-template>