diff --git a/.gitignore b/.gitignore
index 9154f7f3d..9e6f5e458 100644
--- a/.gitignore
+++ b/.gitignore
@@ -36,3 +36,6 @@ msg/_.*\.py$
 
 # Catkin custom files
 CATKIN_IGNORE
+
+# gedit
+*~
diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt
index 66848dd00..dac4a819d 100644
--- a/hrpsys_ros_bridge/CMakeLists.txt
+++ b/hrpsys_ros_bridge/CMakeLists.txt
@@ -105,6 +105,7 @@ string(RANDOM _random_string)
 rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp)
 rtmbuild_add_executable(ImageSensorROSBridge src/ImageSensorROSBridge.cpp src/ImageSensorROSBridgeComp.cpp)
 rtmbuild_add_executable(RangeSensorROSBridge src/RangeSensorROSBridge.cpp src/RangeSensorROSBridgeComp.cpp)
+rtmbuild_add_executable(MasterSlaveROSBridge src/MasterSlaveROSBridge.cpp src/MasterSlaveROSBridgeComp.cpp)
 rtmbuild_add_executable(PointCloudROSBridge src/PointCloudROSBridge.cpp src/PointCloudROSBridgeComp.cpp)
 rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp)
 
diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
index 69e25c91f..68c8bad6b 100644
--- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
+++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
@@ -32,6 +32,7 @@
   <arg name="USE_HRPSYS_PROFILE" default="true" />
   <arg name="USE_VELOCITY_OUTPUT" default="false" />
   <arg name="PUBLISH_SENSOR_TF" default="true" />
+  <arg name="IS_MASTER_SIDE_OF_MASTERSLAVE" default="false" />
   <!--
       roslaunch hrpsys_ros_bridge sample.launch nameserver:=192.168.1.1
   -->
@@ -97,7 +98,7 @@
   <env name="push_policy" value="$(arg push_policy)" />
   <env name="push_rate" value="$(arg push_rate)" />
   <env name="buffer_length" value="$(arg buffer_length)" />
-  <node name="rtmlaunch_hrpsys_ros_bridge" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.launch USE_COMMON=$(arg USE_COMMON) USE_WALKING=$(arg USE_WALKING) USE_COLLISIONCHECK=$(arg USE_COLLISIONCHECK) USE_IMPEDANCECONTROLLER=$(arg USE_IMPEDANCECONTROLLER) USE_SOFTERRORLIMIT=$(arg USE_SOFTERRORLIMIT) USE_IMAGESENSOR=$(arg USE_IMAGESENSOR) USE_ROBOTHARDWARE=$(arg USE_ROBOTHARDWARE) USE_GRASPCONTROLLER=$(arg USE_GRASPCONTROLLER) USE_SERVOCONTROLLER=$(arg USE_SERVOCONTROLLER) USE_TORQUECONTROLLER=$(arg USE_TORQUECONTROLLER) USE_TORQUEFILTER=$(arg USE_TORQUEFILTER) USE_EMERGENCYSTOPPER=$(arg USE_EMERGENCYSTOPPER) USE_REFERENCEFORCEUPDATER=$(arg USE_REFERENCEFORCEUPDATER) USE_OBJECTCONTACTTURNAROUNDDETECTOR=$(arg USE_OBJECTCONTACTTURNAROUNDDETECTOR) USE_VELOCITY_OUTPUT=$(arg USE_VELOCITY_OUTPUT)" output="$(arg OUTPUT)"/>
+  <node name="rtmlaunch_hrpsys_ros_bridge" pkg="openrtm_tools" type="rtmlaunch.py" args="$(find hrpsys_ros_bridge)/launch/hrpsys_ros_bridge.launch USE_COMMON=$(arg USE_COMMON) USE_WALKING=$(arg USE_WALKING) USE_COLLISIONCHECK=$(arg USE_COLLISIONCHECK) USE_IMPEDANCECONTROLLER=$(arg USE_IMPEDANCECONTROLLER) USE_SOFTERRORLIMIT=$(arg USE_SOFTERRORLIMIT) USE_IMAGESENSOR=$(arg USE_IMAGESENSOR) USE_ROBOTHARDWARE=$(arg USE_ROBOTHARDWARE) USE_GRASPCONTROLLER=$(arg USE_GRASPCONTROLLER) USE_SERVOCONTROLLER=$(arg USE_SERVOCONTROLLER) USE_TORQUECONTROLLER=$(arg USE_TORQUECONTROLLER) USE_TORQUEFILTER=$(arg USE_TORQUEFILTER) USE_EMERGENCYSTOPPER=$(arg USE_EMERGENCYSTOPPER) USE_REFERENCEFORCEUPDATER=$(arg USE_REFERENCEFORCEUPDATER) USE_OBJECTCONTACTTURNAROUNDDETECTOR=$(arg USE_OBJECTCONTACTTURNAROUNDDETECTOR) USE_VELOCITY_OUTPUT=$(arg USE_VELOCITY_OUTPUT) IS_MASTER_SIDE_OF_MASTERSLAVE=$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" output="$(arg OUTPUT)"/>
 
 
   <!-- All Robots -->
@@ -146,7 +147,7 @@
     <rtactivate component="RobotHardwareServiceROSBridge.rtc" />
   </group>
 
-  <!-- USE_WALING
+  <!-- USE_WALKING
       AutoBalancer
       Stabilizer
   -->
@@ -173,6 +174,76 @@
     <rtactivate component="StabilizerServiceROSBridge.rtc" />
     <rtactivate component="KalmanFilterServiceROSBridge.rtc" />
   </group>
+  
+
+  <!-- Haptic Coltroller HC - Master Slave WBMS -->
+  <arg name="openrtm_args_high" default='-o "corba.master_manager:$(arg nameserver):$(arg managerport)" -o "corba.nameservers:$(arg nameserver):$(arg corbaport)" -o "naming.formats:%n.rtc" -o "exec_cxt.periodic.type:PeriodicExecutionContext" -o "exec_cxt.periodic.rate:1000" -o "logger.file_name:/tmp/rtc%p.log"' />
+  <node pkg="hrpsys_ros_bridge" name="MasterSlaveROSBridge" type="MasterSlaveROSBridge" args="$(arg openrtm_args_high) -o model:'file://$(arg MODEL_FILE)' -o example.MasterSlaveROSBridge.config_file:$(arg CONF_FILE)" output="$(arg OUTPUT)">
+    <param name="is_master_side" type="bool" value="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" />
+  </node>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:teleopOdom"       to="MasterSlaveROSBridge0.rtc:teleopOdom"       subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lleg_pose" to="MasterSlaveROSBridge0.rtc:master_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rleg_pose" to="MasterSlaveROSBridge0.rtc:master_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_larm_pose" to="MasterSlaveROSBridge0.rtc:master_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rarm_pose" to="MasterSlaveROSBridge0.rtc:master_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_com_pose"  to="MasterSlaveROSBridge0.rtc:master_com_pose"  subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_head_pose" to="MasterSlaveROSBridge0.rtc:master_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lhand_pose" to="MasterSlaveROSBridge0.rtc:master_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rhand_pose" to="MasterSlaveROSBridge0.rtc:master_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lfloor_pose" to="MasterSlaveROSBridge0.rtc:master_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rfloor_pose" to="MasterSlaveROSBridge0.rtc:master_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_lleg_wrench" to="MasterSlaveROSBridge0.rtc:master_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rleg_wrench" to="MasterSlaveROSBridge0.rtc:master_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_larm_wrench" to="MasterSlaveROSBridge0.rtc:master_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:master_rarm_wrench" to="MasterSlaveROSBridge0.rtc:master_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="hc.rtc:delay_check_packet_outbound" to="MasterSlaveROSBridge0.rtc:delay_check_packet_outbound" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lleg_wrench" to="hc.rtc:slave_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rleg_wrench" to="hc.rtc:slave_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_larm_wrench" to="hc.rtc:slave_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rarm_wrench" to="hc.rtc:slave_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lleg_pose" to="hc.rtc:slave_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rleg_pose" to="hc.rtc:slave_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_larm_pose" to="hc.rtc:slave_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rarm_pose" to="hc.rtc:slave_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_com_pose" to="hc.rtc:slave_com_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_head_pose" to="hc.rtc:slave_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lhand_pose" to="hc.rtc:slave_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rhand_pose" to="hc.rtc:slave_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_lfloor_pose" to="hc.rtc:slave_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:slave_rfloor_pose" to="hc.rtc:slave_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect if="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:delay_check_packet_inbound"  to="hc.rtc:delay_check_packet_inbound"  subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lleg_pose" to="wbms.rtc:master_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rleg_pose" to="wbms.rtc:master_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_larm_pose" to="wbms.rtc:master_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rarm_pose" to="wbms.rtc:master_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_com_pose"  to="wbms.rtc:master_com_pose"  subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_head_pose" to="wbms.rtc:master_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lhand_pose" to="wbms.rtc:master_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rhand_pose" to="wbms.rtc:master_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lfloor_pose" to="wbms.rtc:master_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rfloor_pose" to="wbms.rtc:master_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_lleg_wrench" to="wbms.rtc:master_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rleg_wrench" to="wbms.rtc:master_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_larm_wrench" to="wbms.rtc:master_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:master_rarm_wrench" to="wbms.rtc:master_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="MasterSlaveROSBridge0.rtc:delay_check_packet_inbound" to="wbms.rtc:delay_check_packet_inbound" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lleg_wrench" to="MasterSlaveROSBridge0.rtc:slave_lleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rleg_wrench" to="MasterSlaveROSBridge0.rtc:slave_rleg_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_larm_wrench" to="MasterSlaveROSBridge0.rtc:slave_larm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rarm_wrench" to="MasterSlaveROSBridge0.rtc:slave_rarm_wrench" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lleg_pose" to="MasterSlaveROSBridge0.rtc:slave_lleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rleg_pose" to="MasterSlaveROSBridge0.rtc:slave_rleg_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_larm_pose" to="MasterSlaveROSBridge0.rtc:slave_larm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rarm_pose" to="MasterSlaveROSBridge0.rtc:slave_rarm_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_com_pose" to="MasterSlaveROSBridge0.rtc:slave_com_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_head_pose" to="MasterSlaveROSBridge0.rtc:slave_head_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lhand_pose" to="MasterSlaveROSBridge0.rtc:slave_lhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rhand_pose" to="MasterSlaveROSBridge0.rtc:slave_rhand_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_lfloor_pose" to="MasterSlaveROSBridge0.rtc:slave_lfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:slave_rfloor_pose" to="MasterSlaveROSBridge0.rtc:slave_rfloor_pose" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtconnect unless="$(arg IS_MASTER_SIDE_OF_MASTERSLAVE)" from="wbms.rtc:delay_check_packet_outbound" to="MasterSlaveROSBridge0.rtc:delay_check_packet_outbound" subscription_type="$(arg subscription_type)" push_policy="$(arg push_policy)"/>
+  <rtactivate component="MasterSlaveROSBridge0.rtc" />
+
 
   <!-- USE_COLLISIONCHECK
        CollisionDetector
diff --git a/hrpsys_ros_bridge/src/MasterSlaveROSBridge.cpp b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.cpp
new file mode 100644
index 000000000..4be587daf
--- /dev/null
+++ b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.cpp
@@ -0,0 +1,361 @@
+#include "MasterSlaveROSBridge.h"
+
+static const char* masterslaverosbridge_spec[] =
+  {
+    "implementation_id", "MasterSlaveROSBridge",
+    "type_name",         "MasterSlaveROSBridge",
+    "description",       "rtm data - ros bridge",
+    "version",           "1.0",
+    "vendor",            "JSK",
+    "category",          "example",
+    "activity_type",     "SPORADIC",
+    "kind",              "DataFlowComponent",
+    "max_instance",      "10",
+    "language",          "C++",
+    "lang_type",         "compile",
+    ""
+  };
+
+MasterSlaveROSBridge::MasterSlaveROSBridge(RTC::Manager* manager) : RTC::DataFlowComponentBase(manager),
+    m_delayCheckPacketOutboundIn("delay_check_packet_outbound", m_delayCheckPacketOutbound),
+    m_delayCheckPacketInboundOut("delay_check_packet_inbound", m_delayCheckPacketInbound),
+    m_exDataOut("exData", m_exData),
+    m_exDataIndexOut("exDataIndex", m_exDataIndex)
+{}
+
+MasterSlaveROSBridge::~MasterSlaveROSBridge(){}
+
+RTC::ReturnCode_t MasterSlaveROSBridge::onInitialize(){
+    addInPort("delay_check_packet_outbound", m_delayCheckPacketOutboundIn);
+    addOutPort("delay_check_packet_inbound", m_delayCheckPacketInboundOut);
+    addOutPort("exData", m_exDataOut);
+    addOutPort("exDataIndex", m_exDataIndexOut);
+
+    ee_names.push_back("lleg");
+    ee_names.push_back("rleg");
+    ee_names.push_back("larm");
+    ee_names.push_back("rarm");
+
+    tgt_names = ee_names;
+    tgt_names.push_back("com");
+    tgt_names.push_back("head");
+    tgt_names.push_back("rhand");
+    tgt_names.push_back("lhand");
+    tgt_names.push_back("rfloor");
+    tgt_names.push_back("lfloor");
+
+    ros::param::param<bool>("~is_master_side", is_master_side);
+    ros::param::get("~is_master_side", is_master_side);
+
+    if(is_master_side){
+        RTC_INFO_STREAM("Set up ports for MASTER side connection (is_master_side = "<<is_master_side<<")");
+        for ( int i=0; i<ee_names.size(); i++) { // read ROS data from Network
+            std::string n = "slave_"+ee_names[i]+"_wrench";
+            slaveEEWrenches_sub[ee_names[i]] = nh.subscribe<geometry_msgs::WrenchStamped>(n, 1,
+                boost::bind(&MasterSlaveROSBridge::onSlaveEEWrenchCB, this, _1, ee_names[i]), ros::VoidConstPtr(),
+                ros::TransportHints().unreliable().reliable().tcpNoDelay());
+            RTC_INFO_STREAM("register ROS Subscriber " << n );
+        }
+        for ( int i=0; i<ee_names.size(); i++) { // write RTM data to HC
+            std::string n = "slave_"+ee_names[i]+"_wrench";
+            m_slaveEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
+            registerOutPort(n.c_str(), *m_slaveEEWrenchesOut[ee_names[i]]);
+            RTC_INFO_STREAM("register RTC OutPort " << n );
+        }
+        for ( int i=0; i<tgt_names.size(); i++) { // read RTM data from HC
+            std::string n = "master_"+tgt_names[i]+"_pose";
+            m_masterTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
+            registerInPort(n.c_str(), *m_masterTgtPosesIn[tgt_names[i]]);
+            RTC_INFO_STREAM("register RTC InPort " << n );
+        }
+        for ( int i=0; i<tgt_names.size(); i++) { // write ROS data to Network
+            std::string n = "master_"+tgt_names[i]+"_pose";
+            masterTgtPoses_pub[tgt_names[i]] = nh.advertise<geometry_msgs::PoseStamped>(n, 1);
+            RTC_INFO_STREAM("register ROS Publisher " << n );
+        }
+        //////////////////
+        for ( int i=0; i<tgt_names.size(); i++) { // read ROS data from Network
+            std::string n = "slave_"+tgt_names[i]+"_pose";
+            slaveTgtPoses_sub[tgt_names[i]] = nh.subscribe<geometry_msgs::PoseStamped>(n, 1,
+                boost::bind(&MasterSlaveROSBridge::onSlaveTgtPoseCB, this, _1, tgt_names[i]), ros::VoidConstPtr(),
+                ros::TransportHints().unreliable().reliable().tcpNoDelay());
+            RTC_INFO_STREAM("register ROS Subscriber " << n );
+        }
+        for ( int i=0; i<tgt_names.size(); i++) { // write RTM data to HC
+            std::string n = "slave_"+tgt_names[i]+"_pose";
+            m_slaveTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]]));
+            registerOutPort(n.c_str(), *m_slaveTgtPosesOut[tgt_names[i]]);
+            RTC_INFO_STREAM("register RTC OutPort " << n );
+        }
+        for ( int i=0; i<ee_names.size(); i++) { // read RTM data from HC
+            std::string n = "master_"+ee_names[i]+"_wrench";
+            m_masterEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
+            registerInPort(n.c_str(), *m_masterEEWrenchesIn[ee_names[i]]);
+            RTC_INFO_STREAM("register RTC InPort " << n );
+        }
+        for ( int i=0; i<ee_names.size(); i++) { // write ROS data to Network
+            std::string n = "master_"+ee_names[i]+"_wrench";
+            masterEEWrenches_pub[ee_names[i]] = nh.advertise<geometry_msgs::WrenchStamped>(n, 1);
+            RTC_INFO_STREAM("register ROS Publisher " << n );
+        }
+        ///////////////////
+        {// teleop Odom TF
+            std::string n = "teleopOdom";
+            m_teleopOdomIn = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_teleopOdom));
+            registerInPort(n.c_str(), *m_teleopOdomIn);
+            RTC_INFO_STREAM("register RTC InPort  " << n );
+        }
+
+    }else{
+        RTC_INFO_STREAM("Set up ports for SLAVE side connection (is_master_side = "<<is_master_side<<")");
+        for ( int i=0; i<tgt_names.size(); i++) { // to read ROS data from Network
+            std::string n = "master_"+tgt_names[i]+"_pose";
+            masterTgtPoses_sub[tgt_names[i]] = nh.subscribe<geometry_msgs::PoseStamped>(n, 1,
+                boost::bind(&MasterSlaveROSBridge::onMasterTgtPoseCB, this, _1, tgt_names[i]), ros::VoidConstPtr(),
+                ros::TransportHints().unreliable().reliable().tcpNoDelay());
+            RTC_INFO_STREAM("register ROS Subscriber " << n );
+        }
+        for ( int i=0; i<tgt_names.size(); i++) { // to write RTM data to WBMS
+            std::string n = "master_"+tgt_names[i]+"_pose";
+            m_masterTgtPosesOut[tgt_names[i]] = OTP3_Ptr(new RTC::OutPort<RTC::TimedPose3D>(n.c_str(), m_masterTgtPoses[tgt_names[i]]));
+            registerOutPort(n.c_str(), *m_masterTgtPosesOut[tgt_names[i]]);
+            RTC_INFO_STREAM("register RTC OutPort " << n );
+        }
+        for ( int i=0; i<ee_names.size(); i++) { // to read RTM data from WBMS
+            std::string n = "slave_"+ee_names[i]+"_wrench";
+            m_slaveEEWrenchesIn[ee_names[i]] = ITDS_Ptr(new RTC::InPort<RTC::TimedDoubleSeq>(n.c_str(), m_slaveEEWrenches[ee_names[i]]));
+            registerInPort(n.c_str(), *m_slaveEEWrenchesIn[ee_names[i]]);
+            RTC_INFO_STREAM("register RTC InPort " << n );
+        }
+        for ( int i=0; i<ee_names.size(); i++) { // to write ROS data to Network
+            std::string n = "slave_"+ee_names[i]+"_wrench";
+            slaveEEWrenches_pub[ee_names[i]] = nh.advertise<geometry_msgs::WrenchStamped>(n, 1);
+            RTC_INFO_STREAM("register ROS Publisher " << n );
+        }
+        /////////
+        for ( int i=0; i<ee_names.size(); i++) { // to read ROS data from Network
+            std::string n = "master_"+ee_names[i]+"_wrench";
+            masterEEWrenches_sub[ee_names[i]] = nh.subscribe<geometry_msgs::WrenchStamped>(n, 1,
+                boost::bind(&MasterSlaveROSBridge::onMasterEEWrenchCB, this, _1, ee_names[i]), ros::VoidConstPtr(),
+                ros::TransportHints().unreliable().reliable().tcpNoDelay());
+            RTC_INFO_STREAM("register ROS Subscriber " << n );
+        }
+        for ( int i=0; i<ee_names.size(); i++) { // to write RTM data to WBMS
+            std::string n = "master_"+ee_names[i]+"_wrench";
+            m_masterEEWrenchesOut[ee_names[i]] = OTDS_Ptr(new RTC::OutPort<RTC::TimedDoubleSeq>(n.c_str(), m_masterEEWrenches[ee_names[i]]));
+            registerOutPort(n.c_str(), *m_masterEEWrenchesOut[ee_names[i]]);
+            RTC_INFO_STREAM("register RTC OutPort " << n );
+        }
+        for ( int i=0; i<tgt_names.size(); i++) { // to read RTM data from WBMS
+            std::string n = "slave_"+tgt_names[i]+"_pose";
+            m_slaveTgtPosesIn[tgt_names[i]] = ITP3_Ptr(new RTC::InPort<RTC::TimedPose3D>(n.c_str(), m_slaveTgtPoses[tgt_names[i]]));
+            registerInPort(n.c_str(), *m_slaveTgtPosesIn[tgt_names[i]]);
+            RTC_INFO_STREAM("register RTC InPort " << n );
+        }
+        for ( int i=0; i<tgt_names.size(); i++) { // to write ROS data to Network
+            std::string n = "slave_"+tgt_names[i]+"_pose";
+            slaveTgtPoses_pub[tgt_names[i]] = nh.advertise<geometry_msgs::PoseStamped>(n, 1);
+            RTC_INFO_STREAM("register ROS Publisher " << n );
+        }
+    }
+
+    // both
+    delay_check_packet_sub = nh.subscribe("delay_check_packet_inbound", 1, &MasterSlaveROSBridge::ondelayCheckPacketCB, this, ros::TransportHints().unreliable().reliable().tcpNoDelay());
+    delay_check_packet_pub = nh.advertise<std_msgs::Time>("delay_check_packet_outbound", 1);
+
+    ROS_INFO_STREAM("[MasterSlaveROSBridge] @Initilize name : " << getInstanceName());
+
+    loop = 0;
+    tm.tick();
+    return RTC::RTC_OK;
+}
+
+void MasterSlaveROSBridge::ondelayCheckPacketCB(const std_msgs::Time::ConstPtr& msg){
+    m_delayCheckPacketInbound.sec = msg->data.sec;
+    m_delayCheckPacketInbound.nsec = msg->data.nsec;
+    m_delayCheckPacketInboundOut.write();
+}
+
+void MasterSlaveROSBridge::onMasterEEWrenchCB(const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key){
+    m_masterEEWrenches[key].data.length(6);
+    m_masterEEWrenches[key].data[0] = msg->wrench.force.x;
+    m_masterEEWrenches[key].data[1] = msg->wrench.force.y;
+    m_masterEEWrenches[key].data[2] = msg->wrench.force.z;
+    m_masterEEWrenches[key].data[3] = msg->wrench.torque.x;
+    m_masterEEWrenches[key].data[4] = msg->wrench.torque.y;
+    m_masterEEWrenches[key].data[5] = msg->wrench.torque.z;
+    m_masterEEWrenchesOut[key]->write();
+}
+
+void MasterSlaveROSBridge::onSlaveEEWrenchCB(const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key){
+    m_slaveEEWrenches[key].data.length(6);
+    m_slaveEEWrenches[key].data[0] = msg->wrench.force.x;
+    m_slaveEEWrenches[key].data[1] = msg->wrench.force.y;
+    m_slaveEEWrenches[key].data[2] = msg->wrench.force.z;
+    m_slaveEEWrenches[key].data[3] = msg->wrench.torque.x;
+    m_slaveEEWrenches[key].data[4] = msg->wrench.torque.y;
+    m_slaveEEWrenches[key].data[5] = msg->wrench.torque.z;
+    m_slaveEEWrenchesOut[key]->write();
+}
+
+void MasterSlaveROSBridge::onMasterTgtPoseCB(const geometry_msgs::PoseStamped::ConstPtr& msg, std::string& key){
+    tf::Quaternion quat(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w);
+    if(quat.length() != 0.0){
+        tf::Matrix3x3(quat.normalized()).getRPY(m_masterTgtPoses[key].data.orientation.r, m_masterTgtPoses[key].data.orientation.p, m_masterTgtPoses[key].data.orientation.y);
+    }else{
+        m_masterTgtPoses[key].data.orientation = (RTC::Orientation3D){0,0,0};
+    }
+    m_masterTgtPoses[key].data.position = (RTC::Point3D){msg->pose.position.x, msg->pose.position.y, msg->pose.position.z};
+    m_masterTgtPosesOut[key]->write();
+}
+
+void MasterSlaveROSBridge::onSlaveTgtPoseCB(const geometry_msgs::PoseStamped::ConstPtr& msg, std::string& key){
+    tf::Quaternion quat(msg->pose.orientation.x,msg->pose.orientation.y,msg->pose.orientation.z,msg->pose.orientation.w);
+    if(quat.length() != 0.0){
+        tf::Matrix3x3(quat.normalized()).getRPY(m_slaveTgtPoses[key].data.orientation.r, m_slaveTgtPoses[key].data.orientation.p, m_slaveTgtPoses[key].data.orientation.y);
+    }else{
+        m_slaveTgtPoses[key].data.orientation = (RTC::Orientation3D){0,0,0};
+    }
+    m_slaveTgtPoses[key].data.position = (RTC::Point3D){msg->pose.position.x, msg->pose.position.y, msg->pose.position.z};
+    m_slaveTgtPosesOut[key]->write();
+}
+
+
+RTC::ReturnCode_t MasterSlaveROSBridge::onExecute(RTC::UniqueId ec_id){
+    ros::Time tm_on_execute = ros::Time::now();
+
+
+    if(is_master_side){ //master
+        for(int i=0; i<tgt_names.size(); i++){
+            if(m_masterTgtPosesIn[tgt_names[i]]->isNew()){
+                m_masterTgtPosesIn[tgt_names[i]]->read();
+                geometry_msgs::PoseStamped tmp;
+                tmp.header.stamp = tm_on_execute;
+                tmp.header.frame_id = "teleop_odom";
+                tmp.pose.position.x = m_masterTgtPoses[tgt_names[i]].data.position.x;
+                tmp.pose.position.y = m_masterTgtPoses[tgt_names[i]].data.position.y;
+                tmp.pose.position.z = m_masterTgtPoses[tgt_names[i]].data.position.z;
+                tf::Quaternion quat = tf::createQuaternionFromRPY(
+                    m_masterTgtPoses[tgt_names[i]].data.orientation.r,
+                    m_masterTgtPoses[tgt_names[i]].data.orientation.p,
+                    m_masterTgtPoses[tgt_names[i]].data.orientation.y);
+                tmp.pose.orientation.x = quat.getX();
+                tmp.pose.orientation.y = quat.getY();
+                tmp.pose.orientation.z = quat.getZ();
+                tmp.pose.orientation.w = quat.getW();
+                masterTgtPoses_pub[tgt_names[i]].publish(tmp);
+            }
+        }
+        for(int i=0; i<ee_names.size(); i++){
+            if(m_masterEEWrenchesIn[ee_names[i]]->isNew()){
+                m_masterEEWrenchesIn[ee_names[i]]->read();
+                geometry_msgs::WrenchStamped tmp;
+                tmp.header.stamp = tm_on_execute;
+                tmp.header.frame_id = "master_"+ee_names[i]+"_aligned_to_world";
+                tmp.wrench.force.x  = m_masterEEWrenches[ee_names[i]].data[0];
+                tmp.wrench.force.y  = m_masterEEWrenches[ee_names[i]].data[1];
+                tmp.wrench.force.z  = m_masterEEWrenches[ee_names[i]].data[2];
+                tmp.wrench.torque.x = m_masterEEWrenches[ee_names[i]].data[3];
+                tmp.wrench.torque.y = m_masterEEWrenches[ee_names[i]].data[4];
+                tmp.wrench.torque.z = m_masterEEWrenches[ee_names[i]].data[5];
+                masterEEWrenches_pub[ee_names[i]].publish(tmp);
+            }
+        }
+        if (m_teleopOdomIn->isNew()) {
+            m_teleopOdomIn->read();
+            geometry_msgs::TransformStamped tf_tmp;
+            tf_tmp.header.stamp = tm_on_execute;
+            tf_tmp.header.frame_id = "teleop_odom";
+            tf_tmp.child_frame_id = "BASE_LINK";
+            tf::Quaternion quat = tf::createQuaternionFromRPY(m_teleopOdom.data.orientation.r, m_teleopOdom.data.orientation.p, m_teleopOdom.data.orientation.y);
+            tf_tmp.transform.translation.x = m_teleopOdom.data.position.x;
+            tf_tmp.transform.translation.y = m_teleopOdom.data.position.y;
+            tf_tmp.transform.translation.z = m_teleopOdom.data.position.z;
+            tf_tmp.transform.rotation.x = quat.getX();
+            tf_tmp.transform.rotation.y = quat.getY();
+            tf_tmp.transform.rotation.z = quat.getZ();
+            tf_tmp.transform.rotation.w = quat.getW();
+            if(loop%10==0){ // will be 100Hz of 1000Hz
+                std::vector<geometry_msgs::TransformStamped> tf_transforms;
+                tf_transforms.push_back(tf_tmp);
+                br.sendTransform(tf_transforms);
+            }
+        }
+
+    }else{ // slave
+        for(int i=0; i<ee_names.size(); i++){
+            if(m_slaveEEWrenchesIn[ee_names[i]]->isNew()){
+                m_slaveEEWrenchesIn[ee_names[i]]->read();
+                geometry_msgs::WrenchStamped tmp;
+                tmp.header.stamp = tm_on_execute;
+                tmp.header.frame_id = "slave_"+ee_names[i]+"_aligned_to_world";
+                tmp.wrench.force.x  = m_slaveEEWrenches[ee_names[i]].data[0];
+                tmp.wrench.force.y  = m_slaveEEWrenches[ee_names[i]].data[1];
+                tmp.wrench.force.z  = m_slaveEEWrenches[ee_names[i]].data[2];
+                tmp.wrench.torque.x = m_slaveEEWrenches[ee_names[i]].data[3];
+                tmp.wrench.torque.y = m_slaveEEWrenches[ee_names[i]].data[4];
+                tmp.wrench.torque.z = m_slaveEEWrenches[ee_names[i]].data[5];
+                slaveEEWrenches_pub[ee_names[i]].publish(tmp);
+            }
+        }
+        for(int i=0; i<tgt_names.size(); i++){
+            if(m_slaveTgtPosesIn[tgt_names[i]]->isNew()){
+                m_slaveTgtPosesIn[tgt_names[i]]->read();
+                geometry_msgs::PoseStamped tmp;
+                tmp.header.stamp = tm_on_execute;
+                tmp.header.frame_id = "slave_teleop_odom";
+                tmp.pose.position.x = m_slaveTgtPoses[tgt_names[i]].data.position.x;
+                tmp.pose.position.y = m_slaveTgtPoses[tgt_names[i]].data.position.y;
+                tmp.pose.position.z = m_slaveTgtPoses[tgt_names[i]].data.position.z;
+                tf::Quaternion quat = tf::createQuaternionFromRPY(
+                    m_slaveTgtPoses[tgt_names[i]].data.orientation.r,
+                    m_slaveTgtPoses[tgt_names[i]].data.orientation.p,
+                    m_slaveTgtPoses[tgt_names[i]].data.orientation.y);
+                tmp.pose.orientation.x = quat.getX();
+                tmp.pose.orientation.y = quat.getY();
+                tmp.pose.orientation.z = quat.getZ();
+                tmp.pose.orientation.w = quat.getW();
+                slaveTgtPoses_pub[tgt_names[i]].publish(tmp);
+            }
+        }
+    }
+
+
+    // both
+    if(m_delayCheckPacketOutboundIn.isNew()){
+        m_delayCheckPacketOutboundIn.read();
+        std_msgs::Time tmp;
+        tmp.data.sec = m_delayCheckPacketOutbound.sec;
+        tmp.data.nsec = m_delayCheckPacketOutbound.nsec;
+        delay_check_packet_pub.publish(tmp);
+    }
+
+
+    static int count = 0;
+    tm.tack();
+    if ( tm.interval() > 1 ) {
+        ROS_INFO_STREAM("[" << getInstanceName() << "] @onExecutece " << ec_id << " is working at " << count << "[Hz]");
+        tm.tick();
+        count = 0;
+    }
+    count ++;
+//    } else {  // m_range
+//    double interval = 5;
+//    tm.tack();
+//    if ( tm.interval() > interval ) {
+//    ROS_WARN_STREAM("[" << getInstanceName() << "] @onExecutece " << ec_id << " is not executed last " << interval << "[sec]");
+//    tm.tick();
+//    }
+//    }
+
+    loop++;
+    ros::spinOnce();
+    return RTC::RTC_OK;
+}
+
+extern "C"{
+    void MasterSlaveROSBridgeInit(RTC::Manager* manager)  {
+        coil::Properties profile(masterslaverosbridge_spec);
+        manager->registerFactory(profile, RTC::Create<MasterSlaveROSBridge>, RTC::Delete<MasterSlaveROSBridge>);
+    }
+};
diff --git a/hrpsys_ros_bridge/src/MasterSlaveROSBridge.h b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.h
new file mode 100644
index 000000000..18b03183d
--- /dev/null
+++ b/hrpsys_ros_bridge/src/MasterSlaveROSBridge.h
@@ -0,0 +1,108 @@
+#ifndef MASTERSLAVEROSBRIDGE_H
+#define MASTERSLAVEROSBRIDGE_H
+
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/InterfaceDataTypes.hh>
+#include <rtm/Manager.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+#include <rtm/DataInPort.h>
+#include <rtm/DataOutPort.h>
+// ros
+#include "ros/ros.h"
+#include "std_msgs/Time.h"
+#include "geometry_msgs/PoseStamped.h"
+#include "geometry_msgs/WrenchStamped.h"
+//#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+
+using namespace RTC;
+
+class MasterSlaveROSBridge  : public RTC::DataFlowComponentBase
+{
+    public:
+        MasterSlaveROSBridge(RTC::Manager* manager);
+        ~MasterSlaveROSBridge();
+        virtual RTC::ReturnCode_t onInitialize();
+        virtual RTC::ReturnCode_t onExecute(RTC::UniqueId ec_id);
+        void updateOdometryTF(const ros::Time &stamp);
+        void onMasterTgtPoseCB  (const geometry_msgs::PoseStamped::ConstPtr& msg,   std::string& key);
+        void onSlaveTgtPoseCB   (const geometry_msgs::PoseStamped::ConstPtr& msg,   std::string& key);
+        void onMasterEEWrenchCB (const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key);
+        void onSlaveEEWrenchCB  (const geometry_msgs::WrenchStamped::ConstPtr& msg, std::string& key);
+        void ondelayCheckPacketCB(const std_msgs::Time::ConstPtr& msg);
+
+    protected:
+        // used in both case
+        bool is_master_side;
+        ros::NodeHandle nh;
+        coil::TimeMeasure tm;
+        unsigned long loop;
+        std::vector<std::string> ee_names, tgt_names;
+        typedef boost::shared_ptr<RTC::InPort   <RTC::TimedPose3D>      > ITP3_Ptr;
+        typedef boost::shared_ptr<RTC::InPort   <RTC::TimedDoubleSeq>   > ITDS_Ptr;
+        typedef boost::shared_ptr<RTC::OutPort  <RTC::TimedPose3D>      > OTP3_Ptr;
+        typedef boost::shared_ptr<RTC::OutPort  <RTC::TimedDoubleSeq>   > OTDS_Ptr;
+        std::map<std::string, RTC::TimedPose3D>     m_masterTgtPoses;
+        std::map<std::string, RTC::TimedPose3D>     m_slaveTgtPoses;
+        std::map<std::string, RTC::TimedDoubleSeq>  m_masterEEWrenches;
+        std::map<std::string, RTC::TimedDoubleSeq>  m_slaveEEWrenches;
+
+        // master side
+        std::map<std::string, ITP3_Ptr>         m_masterTgtPosesIn;
+        std::map<std::string, OTDS_Ptr>         m_slaveEEWrenchesOut;
+        std::map<std::string, ros::Publisher>   masterTgtPoses_pub;
+        std::map<std::string, ros::Subscriber>  slaveEEWrenches_sub;
+        // sub usage
+        std::map<std::string, ITDS_Ptr>         m_masterEEWrenchesIn;
+        std::map<std::string, OTP3_Ptr>         m_slaveTgtPosesOut;
+        std::map<std::string, ros::Publisher>   masterEEWrenches_pub;
+        std::map<std::string, ros::Subscriber>  slaveTgtPoses_sub;
+        RTC::TimedPose3D m_teleopOdom;
+        ITP3_Ptr m_teleopOdomIn;
+        tf::TransformBroadcaster br;
+
+        // slave side
+        std::map<std::string, ITDS_Ptr>         m_slaveEEWrenchesIn;
+        std::map<std::string, OTP3_Ptr>         m_masterTgtPosesOut;
+        std::map<std::string, ros::Publisher>   slaveEEWrenches_pub;
+        std::map<std::string, ros::Subscriber>  masterTgtPoses_sub;
+        // sub usage
+        std::map<std::string, ITP3_Ptr>         m_slaveTgtPosesIn;
+        std::map<std::string, OTDS_Ptr>         m_masterEEWrenchesOut;
+        std::map<std::string, ros::Publisher>   slaveTgtPoses_pub;
+        std::map<std::string, ros::Subscriber>  masterEEWrenches_sub;
+
+        // delay calc
+        ros::Subscriber delay_check_packet_sub;
+        ros::Publisher delay_check_packet_pub;
+        RTC::Time m_delayCheckPacketInbound, m_delayCheckPacketOutbound;
+        RTC::OutPort<RTC::Time> m_delayCheckPacketInboundOut;
+        RTC::InPort<RTC::Time> m_delayCheckPacketOutboundIn;
+
+        RTC::TimedDoubleSeq m_exData;
+        RTC::TimedStringSeq m_exDataIndex;
+        RTC::OutPort<RTC::TimedDoubleSeq> m_exDataOut;
+        RTC::OutPort<RTC::TimedStringSeq> m_exDataIndexOut;
+};
+
+
+////// copy
+#define dbg(var) std::cout<<#var"= "<<(var)<<std::endl
+#define dbgn(var) std::cout<<#var"= "<<std::endl<<(var)<<std::endl
+#define dbgv(var) std::cout<<#var"= "<<(var.transpose())<<std::endl
+#define RTC_INFO_STREAM(var) std::cout << "[" << m_profile.instance_name << "] "<< var << std::endl;
+#define RTC_WARN_STREAM(var) std::cerr << "\x1b[31m[" << m_profile.instance_name << "] " << var << "\x1b[39m" << std::endl;
+
+#define eps_eq(a, b, c)             (fabs((a)-(b)) <= c)
+#define LIMIT_NORM(x,max)           (x= ( x<(-max) ? -max : (x>max ? max : x)))
+#define LIMIT_MIN(x,min)            (x= ( x<min ? min : x ))
+#define LIMIT_MAX(x,max)            (x= ( x>max ? max : x ))
+#define LIMIT_MINMAX(x,min,max)     (x= ( x<min ? min : ( x>max ? max : x )))
+#define LIMIT_NORM_V(v,max)         if(v.norm()>max){v=v.normalized()*max;}
+#define LIMIT_MIN_V(v,minv)         (v= v.cwiseMax(minv))
+#define LIMIT_MAX_V(v,maxv)         (v= v.cwiseMin(maxv))
+#define LIMIT_MINMAX_V(v,minv,maxv) (v= v.cwiseMin(minv).cwiseMax(maxv))
+
+extern "C"{  DLL_EXPORT void MasterSlaveROSBridgeInit(RTC::Manager* manager);};
+#endif // MASTERSLAVEROSBRIDGE_H
diff --git a/hrpsys_ros_bridge/src/MasterSlaveROSBridgeComp.cpp b/hrpsys_ros_bridge/src/MasterSlaveROSBridgeComp.cpp
new file mode 100644
index 000000000..5b410000b
--- /dev/null
+++ b/hrpsys_ros_bridge/src/MasterSlaveROSBridgeComp.cpp
@@ -0,0 +1,50 @@
+// -*- C++ -*-
+/*!
+ * @file MasterSlaveROSBridgeComp.cpp
+ * @brief Standalone component
+ * @date $Date$ 
+ *
+ * $Id$ 
+ */
+#include <rtm/Manager.h>
+#include <iostream>
+#include <string>
+#include "MasterSlaveROSBridge.h"
+
+void MyModuleInit(RTC::Manager* manager)
+{
+  MasterSlaveROSBridgeInit(manager);
+  RTC::RtcBase* comp;
+
+  // Create a component
+  comp = manager->createComponent("MasterSlaveROSBridge");
+
+  return;
+}
+
+int main (int argc, char** argv)
+{
+  RTC::Manager* manager;
+  manager = RTC::Manager::init(argc, argv);
+  ros::init(argc, argv, "MasterSlaveROSBridgeComp", ros::init_options::NoSigintHandler);
+
+  // Initialize manager
+  manager->init(argc, argv);
+
+  // Set module initialization proceduer
+  // This procedure will be invoked in activateManager() function.
+  manager->setModuleInitProc(MyModuleInit);
+
+  // Activate manager and register to naming service
+  manager->activateManager();
+
+  // run the manager in blocking mode
+  // runManager(false) is the default.
+  manager->runManager();
+
+  // If you want to run the manager in non-blocking mode, do like this
+  // manager->runManager(true);
+
+  return 0;
+}
+
diff --git a/hrpsys_tools/launch/hrpsys.launch b/hrpsys_tools/launch/hrpsys.launch
index bda5a38c1..7bac8c9dd 100644
--- a/hrpsys_tools/launch/hrpsys.launch
+++ b/hrpsys_tools/launch/hrpsys.launch
@@ -76,6 +76,8 @@
 -o "example.EmergencyStopper.config_file:$(arg CONF_FILE)"
 -o "example.ReferenceForceUpdater.config_file:$(arg CONF_FILE)"
 -o "example.ObjectContactTurnaroundDetector.config_file:$(arg CONF_FILE)"
+-o "example.WholeBodyMasterSlave.config_file:$(arg CONF_FILE)"
+-o "example.HapticController.config_file:$(arg CONF_FILE)"
 -o "example.RobotHardware.config_file:$(arg RobotHardware_conf)"
     '/>
   <arg name="hrpsys_opt_rtc_config_args" default=''/>
diff --git a/rtmbuild/cmake/rtmbuild.cmake b/rtmbuild/cmake/rtmbuild.cmake
index b6c983974..14e86e2e9 100644
--- a/rtmbuild/cmake/rtmbuild.cmake
+++ b/rtmbuild/cmake/rtmbuild.cmake
@@ -137,7 +137,8 @@ macro(rtmbuild_init)
     rosbuild_gensrv()
   endif()
 
-  include_directories(${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS})
+  #include_directories(${catkin_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${openhrp3_INCLUDE_DIRS})
+  include_directories( ${openhrp3_INCLUDE_DIRS} ${openrtm_aist_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS})
   # since catkin > 0.7.0, the CPATH is no longer being set by catkin, so rtmbuild manually add them
   set(_cmake_prefix_path_tmp $ENV{CMAKE_PREFIX_PATH})
   string(REPLACE ":" ";" _cmake_prefix_path_tmp ${_cmake_prefix_path_tmp})
@@ -145,7 +146,8 @@ macro(rtmbuild_init)
     include_directories(${_cmake_prefix_path}/include)
   endforeach()
 
-  link_directories(${catkin_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS})
+  #link_directories(${catkin_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${openhrp3_LIBRARY_DIRS})
+  link_directories(${openhrp3_LIBRARY_DIRS} ${openrtm_aist_LIBRARY_DIRS} ${catkin_LIBRARY_DIRS})
 
 endmacro(rtmbuild_init)
 
@@ -308,7 +310,8 @@ macro(rtmbuild_add_executable exe)
   set_target_properties(${exe} PROPERTIES COMPILE_FLAGS "-Wno-deprecated")
   ##
   add_dependencies(${exe} RTMBUILD_${PROJECT_NAME}_genbridge ${${_package}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS} )
-  target_link_libraries(${exe} ${catkin_LIBRARIES} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS} )
+  #target_link_libraries(${exe} ${catkin_LIBRARIES} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS} )
+  target_link_libraries(${exe} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS} ${openrtm_aist_LIBRARIES} ${catkin_LIBRARIES}  )
 endmacro(rtmbuild_add_executable)
 
 macro(rtmbuild_add_library lib)
@@ -318,6 +321,7 @@ macro(rtmbuild_add_library lib)
   else()
     rosbuild_add_library(${ARGV})
   endif()
-  target_link_libraries(${lib} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS})
+  #target_link_libraries(${lib} ${openrtm_aist_LIBRARIES} ${openhrp3_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS})
+  target_link_libraries(${lib}  ${openhrp3_LIBRARIES} ${openrtm_aist_LIBRARIES} ${${PROJECT_NAME}_IDLLIBRARY_DIRS})
 endmacro(rtmbuild_add_library)