From 84417af7689f91196880cb496a11520db70c165f Mon Sep 17 00:00:00 2001
From: Naoki-Hiraoka <hiraoka@jsk.imi.i.u-tokyo.ac.jp>
Date: Mon, 19 Aug 2019 19:34:09 +0900
Subject: [PATCH 01/17] [HrpsysSeqStateROSBridge] output joint_state/velocity

---
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 10 +++++++++-
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h   |  1 +
 2 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index 30bd3b7c7..15808d0bb 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -463,8 +463,16 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         joint_state.velocity.push_back(m_rsvel.data[i]);
       }
     } else {
-      joint_state.velocity.resize(joint_state.name.size());
+      double time_from_prev = (joint_state.header.stamp - prev_joint_state.header.stamp).toSec();
+      if(time_from_prev > 0 && prev_joint_state.position.size() == joint_state.position.size()) {
+        for (unsigned int i = 0; i < joint_state.position.size(); i++) {
+          joint_state.velocity.push_back((joint_state.position[i] - prev_joint_state.position[i]) / time_from_prev);
+        }
+      } else {
+        joint_state.velocity.resize(joint_state.name.size());
+      }
     }
+    prev_joint_state = joint_state;
     // set effort if m_rstorque is available
     if (m_rstorque.data.length() == body->joints().size()) {
       for ( unsigned int i = 0; i < body->joints().size() ; i++ ){
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
index bf615738c..3d4f10513 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -75,6 +75,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
 
   coil::Mutex m_mutex;
   coil::TimeMeasure tm;
+  sensor_msgs::JointState prev_joint_state;
 
   std::string nameserver;
   std::string rootlink_name;

From 4fc4f32a55047538e41035a418acc54f26366fd6 Mon Sep 17 00:00:00 2001
From: Naoki-Hiraoka <hiraoka@jsk.imi.i.u-tokyo.ac.jp>
Date: Tue, 20 Aug 2019 14:11:35 +0900
Subject: [PATCH 02/17] [hrpsys_ros_bridge.launch] add comment

---
 hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
index 69e25c91f..6963b96ac 100644
--- a/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
+++ b/hrpsys_ros_bridge/launch/hrpsys_ros_bridge.launch
@@ -30,7 +30,7 @@
   <arg name="USE_REFERENCEFORCEUPDATER" default="false" />
   <arg name="USE_OBJECTCONTACTTURNAROUNDDETECTOR" default="false" />
   <arg name="USE_HRPSYS_PROFILE" default="true" />
-  <arg name="USE_VELOCITY_OUTPUT" default="false" />
+  <arg name="USE_VELOCITY_OUTPUT" default="false" doc="true: output actual velocity / false: output differential value of actual postition as /joint_states/velocity"/>
   <arg name="PUBLISH_SENSOR_TF" default="true" />
   <!--
       roslaunch hrpsys_ros_bridge sample.launch nameserver:=192.168.1.1

From ed51fd2462122ef8dad45cc6fb37d3512238d15a Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Fri, 25 Oct 2019 15:21:08 +0900
Subject: [PATCH 03/17] Add test for joint trajectory action cancel with PA10

---
 hrpsys_ros_bridge/test/test-pa10.py | 49 +++++++++++++++++++++++++++++
 1 file changed, 49 insertions(+)

diff --git a/hrpsys_ros_bridge/test/test-pa10.py b/hrpsys_ros_bridge/test/test-pa10.py
index 0652dd292..55ffc109f 100755
--- a/hrpsys_ros_bridge/test/test-pa10.py
+++ b/hrpsys_ros_bridge/test/test-pa10.py
@@ -112,6 +112,55 @@ def test_joint_trajectory_command(self):
         rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
         self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
 
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-120208947
+    def test_jta_cancel_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_cancel_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_cancel_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        try:
+            self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("J1")
+        goal.trajectory.joint_names.append("J2")
+        goal.trajectory.joint_names.append("J3")
+        goal.trajectory.joint_names.append("J4")
+        goal.trajectory.joint_names.append("J5")
+        goal.trajectory.joint_names.append("J6")
+        goal.trajectory.joint_names.append("J7")
+
+        point = JointTrajectoryPoint()
+        point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
+        larm.cancel_goal()
+        rospy.sleep(2.5)
+        (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
 # unittest.main()
 if __name__ == '__main__':
     import rostest

From c9745eb3127f94d0175a653563533ba5e4cc9845 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Fri, 25 Oct 2019 18:25:10 +0900
Subject: [PATCH 04/17] Add test for joint trajectory action goal overwrite
 with PA10

---
 hrpsys_ros_bridge/test/test-pa10.py | 67 +++++++++++++++++++++++++++++
 1 file changed, 67 insertions(+)

diff --git a/hrpsys_ros_bridge/test/test-pa10.py b/hrpsys_ros_bridge/test/test-pa10.py
index 55ffc109f..ce835710c 100755
--- a/hrpsys_ros_bridge/test/test-pa10.py
+++ b/hrpsys_ros_bridge/test/test-pa10.py
@@ -18,6 +18,7 @@
 
 import actionlib
 
+from sensor_msgs.msg import JointState
 from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory
 
 class TestPA10Robot(unittest.TestCase):
@@ -161,6 +162,72 @@ def impl_test_jta_cancel_goal(self, larm, goal):
         rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
         self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
 
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-392741195
+    def test_jta_overwrite_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_overwrite_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_overwrite_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        try:
+            self.listener.waitForTransform('/BASE_LINK', '/J7_LINK', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /BASE_LINK to /J7_LINK")
+        (trans1,rot1) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans1,rot1))
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("J1")
+        goal.trajectory.joint_names.append("J2")
+        goal.trajectory.joint_names.append("J3")
+        goal.trajectory.joint_names.append("J4")
+        goal.trajectory.joint_names.append("J5")
+        goal.trajectory.joint_names.append("J6")
+        goal.trajectory.joint_names.append("J7")
+
+        point = JointTrajectoryPoint()
+        point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        # send first goal
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [0,0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(2.5)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        # send second goal
+        larm.send_goal(goal)
+        jt_pos1 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position just after sending second goal: %r"%(array(jt_pos1.position)))
+        rospy.sleep(0.5)
+        jt_pos2 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position 0.5 sec after sending second goal: %r"%(array(jt_pos2.position)))
+        rospy.sleep(2)
+        (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
+        rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two joint positions %r"%(array(jt_pos2.position)-array(jt_pos1.position)))
+        # if robot suddenly stops when goal is overwritten, joint position immediately starts to decrease (heading for new goal (zero)).
+        # joint_states includes unchanged hand joints, so using assertGreaterEqual instead of assertGreater is necessary.
+        for x in array(jt_pos2.position)-array(jt_pos1.position):
+            self.assertGreaterEqual(x, 0)
+        rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
 # unittest.main()
 if __name__ == '__main__':
     import rostest

From df15eb05c7b2a6982a7c1e5ba5ec7ba717f73d1c Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Fri, 25 Oct 2019 21:34:25 +0900
Subject: [PATCH 05/17] Add test for joint trajectory action cancel overwrite
 with samplerobot

---
 hrpsys_ros_bridge/test/test-samplerobot.py | 129 +++++++++++++++++++++
 1 file changed, 129 insertions(+)

diff --git a/hrpsys_ros_bridge/test/test-samplerobot.py b/hrpsys_ros_bridge/test/test-samplerobot.py
index 61fd1a162..bec22d91f 100755
--- a/hrpsys_ros_bridge/test/test-samplerobot.py
+++ b/hrpsys_ros_bridge/test/test-samplerobot.py
@@ -224,6 +224,135 @@ def test_joint_angles_duration_0(self): # https://github.com/start-jsk/rtmros_co
         rospy.logwarn("difference between two angles %r %r"%(array([30,30,30,-90,-40,-30])-array(goal_angles),linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles))))
         self.assertAlmostEqual(linalg.norm(array([30,30,30,-90,-40,-30])-array(goal_angles)), 0, delta=0.1)
 
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-120208947
+    def test_jta_cancel_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_cancel_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_cancel_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_cancel_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        # initialize
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
+        goal.trajectory.joint_names.append("LARM_ELBOW")
+        goal.trajectory.joint_names.append("LARM_WRIST_Y")
+        goal.trajectory.joint_names.append("LARM_WRIST_P")
+        point = JointTrajectoryPoint()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        larm.wait_for_result()
+
+        try:
+            self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
+        point.time_from_start = rospy.Duration(5.0)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
+        larm.cancel_goal()
+        rospy.sleep(2.5)
+        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
+    # https://github.com/start-jsk/rtmros_common/pull/765#issuecomment-392741195
+    def test_jta_overwrite_goal(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())
+
+    def test_fjta_overwrite_goal(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        larm = actionlib.SimpleActionClient("/larm_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_jta_overwrite_goal(larm, FollowJointTrajectoryGoal())
+
+    def impl_test_jta_overwrite_goal(self, larm, goal):
+        larm.wait_for_server()
+
+        # initialize
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("LARM_SHOULDER_P")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_R")
+        goal.trajectory.joint_names.append("LARM_SHOULDER_Y")
+        goal.trajectory.joint_names.append("LARM_ELBOW")
+        goal.trajectory.joint_names.append("LARM_WRIST_Y")
+        goal.trajectory.joint_names.append("LARM_WRIST_P")
+        point = JointTrajectoryPoint()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        larm.send_goal(goal)
+        larm.wait_for_result()
+
+        try:
+            self.listener.waitForTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /LARM_LINK7")
+        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans1,rot1))
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [ x * math.pi / 180.0 for x in [20,20,20,20,20,20] ]
+        point.time_from_start = rospy.Duration(5.0)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        # send first goal
+        larm.send_goal(goal)
+        rospy.sleep(2.5)
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point.positions = [0,0,0,0,0,0]
+        point.time_from_start = rospy.Duration(2.5)
+        del goal.trajectory.points[:]
+        goal.trajectory.points.append(point)
+        # send second goal
+        larm.send_goal(goal)
+        jt_pos1 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position just after sending second goal: %r"%(array(jt_pos1.position)))
+        rospy.sleep(0.5)
+        jt_pos2 = rospy.wait_for_message('/joint_states', JointState)
+        rospy.logwarn("joint position 0.5 sec after sending second goal: %r"%(array(jt_pos2.position)))
+        rospy.sleep(2)
+        (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
+        rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
+        larm.wait_for_result()
+        rospy.logwarn("difference between two joint positions %r"%(array(jt_pos2.position)-array(jt_pos1.position)))
+        # if robot suddenly stops when goal is overwritten, joint position immediately starts to decrease (heading for new goal (zero)).
+        # joint_states includes unchanged hand joints, so using assertGreaterEqual instead of assertGreater is necessary.
+        for x in array(jt_pos2.position)-array(jt_pos1.position):
+            self.assertGreaterEqual(x, 0)
+        rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
+        self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+
 
 #unittest.main()
 if __name__ == '__main__':

From 461a657b0ca5a8ba930606eac0904bab6509ed0d Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 10 Feb 2017 15:58:30 +0900
Subject: [PATCH 06/17] add code to get hrpsys rtc version, thakns to @n-ando

---
 hrpsys_ros_bridge/CMakeLists.txt              |  4 +-
 hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp | 70 +++++++++++++++++++
 hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h   | 31 ++++++++
 .../src/HrpsysSeqStateROSBridgeImpl.cpp       |  6 +-
 .../src/HrpsysSeqStateROSBridgeImpl.h         |  4 +-
 5 files changed, 110 insertions(+), 5 deletions(-)
 create mode 100644 hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp
 create mode 100644 hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h

diff --git a/hrpsys_ros_bridge/CMakeLists.txt b/hrpsys_ros_bridge/CMakeLists.txt
index 66848dd00..06c842ac1 100644
--- a/hrpsys_ros_bridge/CMakeLists.txt
+++ b/hrpsys_ros_bridge/CMakeLists.txt
@@ -102,11 +102,11 @@ rtmbuild_genbridge()
 # pr2_controller_msgs is not catkinized
 string(RANDOM _random_string)
 
-rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp)
+rtmbuild_add_executable(HrpsysSeqStateROSBridge src/HrpsysSeqStateROSBridgeImpl.cpp src/HrpsysSeqStateROSBridge.cpp src/HrpsysSeqStateROSBridgeComp.cpp src/HrpsysROSBridgeUtil.cpp)
 rtmbuild_add_executable(ImageSensorROSBridge src/ImageSensorROSBridge.cpp src/ImageSensorROSBridgeComp.cpp)
 rtmbuild_add_executable(RangeSensorROSBridge src/RangeSensorROSBridge.cpp src/RangeSensorROSBridgeComp.cpp)
 rtmbuild_add_executable(PointCloudROSBridge src/PointCloudROSBridge.cpp src/PointCloudROSBridgeComp.cpp)
-rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp)
+rtmbuild_add_executable(HrpsysJointTrajectoryBridge src/HrpsysJointTrajectoryBridge.cpp src/HrpsysJointTrajectoryBridgeComp.cpp src/HrpsysROSBridgeUtil.cpp)
 
 install(DIRECTORY launch euslisp srv idl scripts models test cmake
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
diff --git a/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp
new file mode 100644
index 000000000..6e69a8d6e
--- /dev/null
+++ b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.cpp
@@ -0,0 +1,70 @@
+// -*- C++ -*-
+/*!
+ * @file  HrpsysROSBridgeUtil.cpp * @brief hrpsys - ros bridge util files * @date  $Date$ 
+ *
+ * $Id$ 
+ */
+
+#include "HrpsysROSBridgeUtil.h"
+#include <ros/console.h>
+
+//
+void Parse(int result[3], const std::string& input)
+{
+  std::istringstream parser(input);
+  parser >> result[0];
+  for(int idx = 1; idx < 3; idx++)
+  {
+    parser.get(); //Skip period
+    parser >> result[idx];
+  }
+}
+
+bool LessThanVersion(const std::string& a,const std::string& b)
+{
+  int parsedA[3], parsedB[3];
+  try {
+    Parse(parsedA, a);
+    Parse(parsedB, b);
+    return std::lexicographical_compare(parsedA, parsedA + 4, parsedB, parsedB + 4);
+  } catch (...) {
+    ROS_ERROR_STREAM("LessThanVersion failed " << a << " < " << b <<", force return false");
+    return false;
+  }
+}
+
+//
+std::string GetHrpsysVersion(RTC::CorbaPort& m_ServicePort) {
+  std::string hrpsys_version;
+  try {
+    // copied from HrpsysSeqStateROSBridgeImpl.cpp
+    RTC::ConnectorProfileList* connector_profile_list = m_ServicePort.get_connector_profiles();
+    for (int i = 0; i < connector_profile_list->length(); i++ ) {
+      RTC::ConnectorProfile& connector_profile = (*connector_profile_list)[i];
+      RTC::PortServiceList ports_list = connector_profile.ports;
+      for (int j = 0; j < ports_list.length(); j++ ) {
+        RTC::PortService_var port = ports_list[j];
+        RTC::PortProfile* profile = port->get_port_profile();
+        if ( profile ) {
+          RTC::RTObject_var owner = profile->owner;
+          if ( owner ) {
+            RTC::ComponentProfile* component_profile = owner->get_component_profile();
+            if ( component_profile && std::string(component_profile->type_name) == std::string("SequencePlayer") ) {
+              ROS_WARN_STREAM(__PRETTY_FUNCTION__ << " Connected to " << component_profile->type_name  << " (" << component_profile->instance_name << ") version " << component_profile->version);
+              hrpsys_version = std::string(component_profile->version);
+            }
+          }
+        }
+      }
+    }
+    if ( hrpsys_version == "" ) {
+      ROS_ERROR_STREAM( __PRETTY_FUNCTION__ << " Could not get valid hrpsys_version");
+    }
+  } catch (...) {
+    ROS_ERROR_STREAM( __PRETTY_FUNCTION__ << " Could not get hrpsys_version");
+  }
+  return hrpsys_version;
+}
+
+
+
diff --git a/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h
new file mode 100644
index 000000000..219c09f2b
--- /dev/null
+++ b/hrpsys_ros_bridge/src/HrpsysROSBridgeUtil.h
@@ -0,0 +1,31 @@
+// -*- C++ -*-
+/*!
+ * @file  HrpsysROSBridgeUtil.h * @brief hrpsys - ros bridge util files * @date  $Date$ 
+ *
+ * $Id$ 
+ */
+#ifndef HRPSYSROSBRIDGEUTIL_H
+#define HRPSYSROSBRIDGEUTIL_H
+
+#include <string>
+// http://stackoverflow.com/questions/2941491/compare-versions-as-strings
+bool LessThanVersion(const std::string& a,const std::string& b);
+
+//
+#include <rtm/idl/BasicDataTypeSkel.h>
+#include <rtm/idl/ExtendedDataTypesSkel.h>
+#include <rtm/Manager.h>
+#include <rtm/CorbaNaming.h>
+#include <rtm/DataFlowComponentBase.h>
+#include <rtm/CorbaPort.h>
+
+#include <ros/ros.h>
+#include <ros/console.h>
+
+#include <string>
+#include <iostream>
+
+std::string GetHrpsysVersion(RTC::CorbaPort& m_ServicePort);
+
+#endif // HRPSYSROSBRIDGEUTIL_H
+
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
index c8d7596de..ac2a92496 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.cpp
@@ -375,12 +375,14 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onShutdown(RTC::UniqueId ec_id)
   return RTC::RTC_OK;
 }
 */
-/*
+
 RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onActivated(RTC::UniqueId ec_id)
 {
+  hrpsys_version = GetHrpsysVersion(m_SequencePlayerServicePort);
+
   return RTC::RTC_OK;
 }
-*/
+
 /*
 RTC::ReturnCode_t HrpsysSeqStateROSBridgeImpl::onDeactivated(RTC::UniqueId ec_id)
 {
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
index 5971d639b..a1fcca492 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
@@ -63,7 +63,7 @@ class HrpsysSeqStateROSBridgeImpl  : public RTC::DataFlowComponentBase
 
   // The activated action (Active state entry action)
   // former rtc_active_entry()
-  // virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
+  virtual RTC::ReturnCode_t onActivated(RTC::UniqueId ec_id);
 
   // The deactivated action (Active state exit action)
   // former rtc_active_exit()
@@ -199,6 +199,8 @@ class HrpsysSeqStateROSBridgeImpl  : public RTC::DataFlowComponentBase
 
   double dt;
 
+  std::string hrpsys_version;
+
  private:
 };
 

From f4d9c410ed524e850f23c9181898b1f4707e8958 Mon Sep 17 00:00:00 2001
From: Kei Okada <kei.okada@gmail.com>
Date: Fri, 10 Jul 2015 10:51:34 +0900
Subject: [PATCH 07/17] use setJointAnglesOfGroup() and playPattern(), use
 clearJointAngles()

---
 .../src/HrpsysJointTrajectoryBridge.cpp       | 64 +++++++++++++++++--
 .../src/HrpsysJointTrajectoryBridge.h         |  2 +
 .../src/HrpsysSeqStateROSBridge.cpp           | 21 +++++-
 .../src/HrpsysSeqStateROSBridgeImpl.h         |  3 +
 4 files changed, 83 insertions(+), 7 deletions(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 336917762..498e37e26 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -41,6 +41,9 @@
 #include "hrpsys_ros_bridge/idl/ExecutionProfileService.hh"
 #include "hrpsys_ros_bridge/idl/RobotHardwareService.hh"
 
+// Version
+#include "HrpsysROSBridgeUtil.h"
+
 // Module specification
 // <rtc-template block="module_spec">
 static const char* hrpsysjointtrajectorybridge_spec[] = {"implementation_id", "HrpsysJointTrajectoryBridge",
@@ -186,6 +189,8 @@ RTC::ReturnCode_t HrpsysJointTrajectoryBridge::onActivated(RTC::UniqueId ec_id)
     ROS_WARN_STREAM("param: " << config_name << ", param does not exist.");
   }
 
+  hrpsys_version = GetHrpsysVersion(m_SequencePlayerServicePort);
+
   return RTC::RTC_OK;
 }
 
@@ -493,13 +498,23 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
   {
     if (groupname.length() > 0)
     { // group
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternGroup: " << groupname);
-      parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesOfGroup: " << groupname);
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
+      } else {
+        parent->m_service0->setJointAnglesSequenceOfGroup(groupname.c_str(), angles, duration);
+      }
     }
     else
-    {
-      OpenHRP::dSequenceSequence rpy, zmp;
-      parent->m_service0->playPattern(angles, rpy, zmp, duration);
+    { // fullbody
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        OpenHRP::dSequenceSequence rpy, zmp;
+        parent->m_service0->playPattern(angles, rpy, zmp, duration);
+      } else {
+        parent->m_service0->setJointAnglesSequence(angles, duration);
+      }
     }
   }
 
@@ -527,6 +542,26 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionPreempt()");
   joint_trajectory_server->setPreempted();
+  if (groupname.length() > 0)
+  { // group
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+    } else {
+      parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+    }
+  }
+  else
+  { // fullbody
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      parent->m_service0->clear();
+    } else {
+      parent->m_service0->clearJointAngles();
+    }
+  }
 }
 #endif
 
@@ -534,6 +569,25 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionPreempt()");
   follow_joint_trajectory_server->setPreempted();
+
+  if (groupname.length() > 0)
+  { // group
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+    } else {
+      parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+    }
+  }
+  else
+  { // fullbody
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      parent->m_service0->clearJointAngles();
+    } else {
+      parent->m_service0->clear();
+    }
+  }
 }
 
 void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg)
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
index 4a4e3bd1a..58355ccd5 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
@@ -126,6 +126,8 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
   RTC::CorbaPort m_SequencePlayerServicePort;
   RTC::CorbaConsumer<OpenHRP::SequencePlayerService> m_service0;
 
+  std::string hrpsys_version;
+
 protected:
   hrp::BodyPtr body;
   OpenHRP::BodyInfo_var bodyinfo;
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index 30bd3b7c7..c27c5a6fc 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -215,8 +215,13 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
   if ( duration.length() == 1 ) {
     m_service0->setJointAngles(angles[0], duration[0]);
   } else {
-    OpenHRP::dSequenceSequence rpy, zmp;
-    m_service0->playPattern(angles, rpy, zmp, duration);
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      OpenHRP::dSequenceSequence rpy, zmp;
+      m_service0->playPattern(angles, rpy, zmp, duration);
+    } else {
+      m_service0->setJointAnglesSequence(angles, duration);
+    }
   }
 
   interpolationp = true;
@@ -238,11 +243,23 @@ void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
 #ifdef USE_PR2_CONTROLLERS_MSGS
 void HrpsysSeqStateROSBridge::onJointTrajectoryActionPreempt() {
   joint_trajectory_server.setPreempted();
+  // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+  if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+    m_service0->clear();
+  } else {
+    m_service0->clearJointAngles();
+  }
 }
 #endif
 
 void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionPreempt() {
   follow_joint_trajectory_server.setPreempted();
+  // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+  if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+    m_service0->clear();
+  } else {
+    m_service0->clearJointAngles();
+  }
 }
 
 void HrpsysSeqStateROSBridge::onTrajectoryCommandCB(const trajectory_msgs::JointTrajectoryConstPtr& msg) {
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
index a1fcca492..20c5dfb82 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridgeImpl.h
@@ -36,6 +36,9 @@
 
 // </rtc-template>
 
+// version
+#include "HrpsysROSBridgeUtil.h"
+
 #include "tf/transform_broadcaster.h"
 using namespace RTC;
 

From 1dc2fcd7b5919db0edbce08a46a2a45ee154d601 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 9 Feb 2018 14:37:41 +0900
Subject: [PATCH 08/17] add more debug info for #765

---
 .../src/HrpsysJointTrajectoryBridge.cpp       | 21 +++++++++++++------
 1 file changed, 15 insertions(+), 6 deletions(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 498e37e26..8a84699a3 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -467,8 +467,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       ss << " " << point.positions[j];
     }
     ROS_INFO_STREAM(
-        "[" << parent->getInstanceName() << "] @onJointTrajectoryAction (" << this->groupname << ") : time_from_start " << trajectory.points[i].time_from_start.toSec());
-    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] " << ss.str());
+        "[" << parent->getInstanceName() << "] i:" << i << " : time_from_start " << trajectory.points[i].time_from_start.toSec());
 
     if (i > 0)
     {
@@ -479,6 +478,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       duration[i] = trajectory.points[i].time_from_start.toSec();
       if ( std::abs(duration[i]) < 0.001 ) duration[i] = 0.001; // set magic delta ... https://github.com/start-jsk/rtmros_common/issues/1036
     }
+    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] [" << ss.str() << "] " << duration[i]);
   }
 
   parent->m_mutex.unlock();
@@ -491,6 +491,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     }
     else
     { // fullbody
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAngles");
       parent->m_service0->setJointAngles(angles[0], duration[0]);
     }
   }
@@ -498,11 +499,12 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
   {
     if (groupname.length() > 0)
     { // group
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesOfGroup: " << groupname);
       // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
       if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternOfGroup: " << groupname);
         parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
       } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesSequenceOfGroup: " << groupname);
         parent->m_service0->setJointAnglesSequenceOfGroup(groupname.c_str(), angles, duration);
       }
     }
@@ -510,9 +512,11 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     { // fullbody
       // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
       if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPattern");
         OpenHRP::dSequenceSequence rpy, zmp;
         parent->m_service0->playPattern(angles, rpy, zmp, duration);
       } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] setJointAnglesSequence");
         parent->m_service0->setJointAnglesSequence(angles, duration);
       }
     }
@@ -544,11 +548,12 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
   joint_trajectory_server->setPreempted();
   if (groupname.length() > 0)
   { // group
-    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
     // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
     if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
       parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
     } else {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
       parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
     }
   }
@@ -557,8 +562,10 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
     ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
     // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
     if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
       parent->m_service0->clear();
     } else {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
       parent->m_service0->clearJointAngles();
     }
   }
@@ -572,19 +579,21 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
 
   if (groupname.length() > 0)
   { // group
-    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
     if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
       parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
     } else {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
       parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
     }
   }
   else
   { // fullbody
-    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
     if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
       parent->m_service0->clearJointAngles();
     } else {
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
       parent->m_service0->clear();
     }
   }

From 2ba699fdac62de3ed88794eed575a789669fe4c4 Mon Sep 17 00:00:00 2001
From: Kei Okada <k-okada@jsk.t.u-tokyo.ac.jp>
Date: Fri, 9 Feb 2018 14:52:59 +0900
Subject: [PATCH 09/17] setJointANglesSequenceOfGroup has bugs as of 315.15.0,
 use old API intead

see https://github.com/fkanehiro/hrpsys-base/pull/1237
---
 hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 8a84699a3..115ee5a08 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -500,7 +500,8 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     if (groupname.length() > 0)
     { // group
       // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
-      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
         ROS_INFO_STREAM("[" << parent->getInstanceName() << "] playPatternOfGroup: " << groupname);
         parent->m_service0->playPatternOfGroup(groupname.c_str(), angles, duration);
       } else {
@@ -549,7 +550,8 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
   if (groupname.length() > 0)
   { // group
     // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
-    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+    // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
       ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
       parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
     } else {
@@ -579,7 +581,8 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
 
   if (groupname.length() > 0)
   { // group
-    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+    // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+    if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
       ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
       parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
     } else {

From fb33bf603d2e849428fbdafc7fb5ec1e4162652f Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Mon, 18 Jun 2018 13:56:27 +0900
Subject: [PATCH 10/17] Tell getting new goal from cancel

---
 .../src/HrpsysJointTrajectoryBridge.cpp       | 84 ++++++++++---------
 .../src/HrpsysSeqStateROSBridge.cpp           | 24 +++---
 2 files changed, 59 insertions(+), 49 deletions(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 115ee5a08..56d0bbfa0 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -547,28 +547,31 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
 {
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionPreempt()");
   joint_trajectory_server->setPreempted();
-  if (groupname.length() > 0)
-  { // group
-    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
-    // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
-    if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
-      parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
-    } else {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
-      parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+  if (!joint_trajectory_server->isNewGoalAvailable())
+  {
+    if (groupname.length() > 0)
+    { // group
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
+        parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+        parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+      }
     }
-  }
-  else
-  { // fullbody
-    ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
-    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
-    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
-      parent->m_service0->clear();
-    } else {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
-      parent->m_service0->clearJointAngles();
+    else
+    { // fullbody
+      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles ");
+      // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
+        parent->m_service0->clear();
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
+        parent->m_service0->clearJointAngles();
+      }
     }
   }
 }
@@ -579,25 +582,28 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onFollowJointTrajectoryActionPreempt()");
   follow_joint_trajectory_server->setPreempted();
 
-  if (groupname.length() > 0)
-  { // group
-    // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
-    if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
-      parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
-    } else {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
-      parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+  if (!follow_joint_trajectory_server->isNewGoalAvailable())
+  {
+    if (groupname.length() > 0)
+    { // group
+      // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.16.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearOfGroup: " << groupname);
+        parent->m_service0->clearOfGroup(groupname.c_str(), 0.05);
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAnglesOfGroup: " << groupname);
+        parent->m_service0->clearJointAnglesOfGroup(groupname.c_str());
+      }
     }
-  }
-  else
-  { // fullbody
-    if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
-      parent->m_service0->clearJointAngles();
-    } else {
-      ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
-      parent->m_service0->clear();
+    else
+    { // fullbody
+      if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
+        parent->m_service0->clearJointAngles();
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
+        parent->m_service0->clear();
+      }
     }
   }
 }
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index c27c5a6fc..03a33e299 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -243,22 +243,26 @@ void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
 #ifdef USE_PR2_CONTROLLERS_MSGS
 void HrpsysSeqStateROSBridge::onJointTrajectoryActionPreempt() {
   joint_trajectory_server.setPreempted();
-  // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
-  if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
-    m_service0->clear();
-  } else {
-    m_service0->clearJointAngles();
+  if (!joint_trajectory_server.isNewGoalAvailable()) {
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      m_service0->clear();
+    } else {
+      m_service0->clearJointAngles();
+    }
   }
 }
 #endif
 
 void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionPreempt() {
   follow_joint_trajectory_server.setPreempted();
-  // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
-  if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
-    m_service0->clear();
-  } else {
-    m_service0->clearJointAngles();
+  if (!follow_joint_trajectory_server.isNewGoalAvailable()) {
+    // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
+    if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
+      m_service0->clear();
+    } else {
+      m_service0->clearJointAngles();
+    }
   }
 }
 

From 70f0ca96531bc2b5b706c10b53831a2a8ca8130f Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Mon, 18 Jun 2018 16:31:41 +0900
Subject: [PATCH 11/17] Add comments to clarify when stopping motion is
 executed

---
 hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 6 ++++--
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp     | 4 ++++
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 56d0bbfa0..21dadfed7 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -548,7 +548,8 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectoryAct
   ROS_INFO_STREAM("[" << parent->getInstanceName() << "] @onJointTrajectoryActionPreempt()");
   joint_trajectory_server->setPreempted();
   if (!joint_trajectory_server->isNewGoalAvailable())
-  {
+  { // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes
     if (groupname.length() > 0)
     { // group
       // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
@@ -583,7 +584,8 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
   follow_joint_trajectory_server->setPreempted();
 
   if (!follow_joint_trajectory_server->isNewGoalAvailable())
-  {
+  { // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes
     if (groupname.length() > 0)
     { // group
       // hrpsys < 315.16.0 have bugs in setJointAnglesSequenceOfGroup https://github.com/fkanehiro/hrpsys-base/pull/1237
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index 03a33e299..38c47adb4 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -244,6 +244,8 @@ void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionGoal() {
 void HrpsysSeqStateROSBridge::onJointTrajectoryActionPreempt() {
   joint_trajectory_server.setPreempted();
   if (!joint_trajectory_server.isNewGoalAvailable()) {
+    // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes.
     // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
     if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
       m_service0->clear();
@@ -257,6 +259,8 @@ void HrpsysSeqStateROSBridge::onJointTrajectoryActionPreempt() {
 void HrpsysSeqStateROSBridge::onFollowJointTrajectoryActionPreempt() {
   follow_joint_trajectory_server.setPreempted();
   if (!follow_joint_trajectory_server.isNewGoalAvailable()) {
+    // Cancel request comes from client, so motion should be stopped immediately,
+    // while motion should be changed smoothly when new goal comes.
     // hrpsys < 315.5.0 does not have clearJointAngles, so need to use old API
     if (LessThanVersion(hrpsys_version, std::string("315.5.0"))) {
       m_service0->clear();

From f8bd4ec81cc06543857b1719beceadc86fcbf2bc Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sat, 26 Oct 2019 01:35:01 +0900
Subject: [PATCH 12/17] Fix swapping of clear and clearJointAngles

---
 hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 21dadfed7..1a1ca4a8c 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -600,11 +600,11 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onFollowJointTraject
     else
     { // fullbody
       if (LessThanVersion(parent->hrpsys_version, std::string("315.5.0"))) {
-        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
-        parent->m_service0->clearJointAngles();
-      } else {
         ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clear");
         parent->m_service0->clear();
+      } else {
+        ROS_INFO_STREAM("[" << parent->getInstanceName() << "] clearJointAngles");
+        parent->m_service0->clearJointAngles();
       }
     }
   }

From 7992d708ed43ad3149be851fa835b16932eb1a05 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sat, 26 Oct 2019 19:16:35 +0900
Subject: [PATCH 13/17] Add test of feedback from joint trajectory action with
 PA10

---
 hrpsys_ros_bridge/test/test-pa10.py | 26 +++++++++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge/test/test-pa10.py b/hrpsys_ros_bridge/test/test-pa10.py
index 0652dd292..a2a3672e4 100755
--- a/hrpsys_ros_bridge/test/test-pa10.py
+++ b/hrpsys_ros_bridge/test/test-pa10.py
@@ -10,6 +10,7 @@
     import roslib; roslib.load_manifest(PKG)
 
 import argparse,unittest,rostest, time, sys, math, os
+from copy import deepcopy
 from numpy import *
 
 import rospy,rospkg, tf
@@ -24,6 +25,7 @@ class TestPA10Robot(unittest.TestCase):
     listener = None
     lfsensor = None
     rfsensor = None
+    feedback = None
 
     def send_joint_angles(self,angles):
         pub = rospy.Publisher("/fullbody_controller/command", JointTrajectory)
@@ -68,6 +70,9 @@ def test_follow_joint_trajectory_action(self):
         larm = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
         self.impl_test_joint_trajectory_action(larm, FollowJointTrajectoryGoal())
 
+    def feedback_cb(self, msg):
+        self.feedback = msg
+
     def impl_test_joint_trajectory_action(self, larm, goal):
         larm.wait_for_server()
 
@@ -90,12 +95,31 @@ def impl_test_joint_trajectory_action(self, larm, goal):
         point.positions = [ x * math.pi / 180.0 for x in [10,20,30,40,50,60,70] ]
         point.time_from_start = rospy.Duration(5.0)
         goal.trajectory.points.append(point)
-        larm.send_goal(goal)
+        larm.send_goal(goal, feedback_cb = self.feedback_cb)
+        rospy.sleep(2.5)
+        mid_feedback = deepcopy(self.feedback)
         larm.wait_for_result()
         (trans2,rot2) = self.listener.lookupTransform('/BASE_LINK', '/J7_LINK', rospy.Time(0))
         rospy.logwarn("tf_echo /BASE_LINK /J7_LINK %r %r"%(trans2,rot2))
         rospy.logwarn("difference between two /J7_LINK %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
         self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+        # time_from_start in feedback should have a meaning
+        # https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-403416846
+        # JointTrajectoryAction in pr2_controllers_msgs has empty feedback
+        if hasattr(mid_feedback, 'desired'):
+            rospy.logwarn("time_from_start of desired of midpoint feedback: %r"%mid_feedback.desired.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.desired.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of actual of midpoint feedback: %r"%mid_feedback.actual.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.actual.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of error of midpoint feedback: %r"%mid_feedback.error.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.error.time_from_start.to_sec(), 2.5, delta=1)
+        # feedback shouldn't be published after motion finishes
+        # https://github.com/start-jsk/rtmros_common/pull/1049#issuecomment-403780615
+        fb_id1 = id(self.feedback)
+        rospy.sleep(0.5)
+        fb_id2 = id(self.feedback)
+        rospy.logwarn("object id of feedback after motion: %r, 0.5sec after: %r"%(fb_id1, fb_id2))
+        self.assertEqual(fb_id1, fb_id2)
 
     def test_joint_trajectory_command(self):
         try:

From 474813ca968cef76cd82524ac8a8f6cd91245a69 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Sat, 26 Oct 2019 19:36:21 +0900
Subject: [PATCH 14/17] Add test of feedback from joint trajectory action with
 samplerobot

---
 hrpsys_ros_bridge/test/test-samplerobot.py | 26 +++++++++++++++++++++-
 1 file changed, 25 insertions(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge/test/test-samplerobot.py b/hrpsys_ros_bridge/test/test-samplerobot.py
index 61fd1a162..707aa1e2a 100755
--- a/hrpsys_ros_bridge/test/test-samplerobot.py
+++ b/hrpsys_ros_bridge/test/test-samplerobot.py
@@ -4,6 +4,7 @@
 NAME = 'test_samplerobot'
 
 import argparse,unittest,rostest, time, sys, math, os
+from copy import deepcopy
 from numpy import *
 
 import rospy,rospkg, tf
@@ -22,6 +23,7 @@ class TestSampleRobot(unittest.TestCase):
     rfsensor = None
     odom = None
     joint_states = None
+    feedback = None
 
     def lfsensor_cb(self, sensor):
         self.lfsensor = sensor
@@ -90,6 +92,9 @@ def test_load_pattern(self):
             self.assertAlmostEqual(t2-t1, 11, delta=2)
             #self.assertNotAlmostEqual(trans[1],0,2)
 
+    def feedback_cb(self, msg):
+        self.feedback = msg
+
     def impl_test_joint_angles(self, larm, goal):
         larm.wait_for_server()
 
@@ -111,12 +116,31 @@ def impl_test_joint_angles(self, larm, goal):
         point.positions = [ x * math.pi / 180.0 for x in [30,30,30,-90,-40,-30] ]
         point.time_from_start = rospy.Duration(5.0)
         goal.trajectory.points.append(point)
-        larm.send_goal(goal)
+        larm.send_goal(goal, feedback_cb = self.feedback_cb)
+        rospy.sleep(2.5)
+        mid_feedback = deepcopy(self.feedback)
         larm.wait_for_result()
         (trans2,rot2) = self.listener.lookupTransform('/WAIST_LINK0', '/LARM_LINK7', rospy.Time(0))
         rospy.logwarn("tf_echo /WAIST_LINK0 /LARM_LINK7 %r %r"%(trans2,rot2))
         rospy.logwarn("difference between two /LARM_LINK7 %r %r"%(array(trans1)-array(trans2),linalg.norm(array(trans1)-array(trans2))))
         self.assertNotAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
+        # time_from_start in feedback should have a meaning
+        # https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/233#issuecomment-403416846
+        # JointTrajectoryAction in pr2_controllers_msgs has empty feedback
+        if hasattr(mid_feedback, 'desired'):
+            rospy.logwarn("time_from_start of desired of midpoint feedback: %r"%mid_feedback.desired.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.desired.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of actual of midpoint feedback: %r"%mid_feedback.actual.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.actual.time_from_start.to_sec(), 2.5, delta=1)
+            rospy.logwarn("time_from_start of error of midpoint feedback: %r"%mid_feedback.error.time_from_start.to_sec())
+            self.assertAlmostEqual(mid_feedback.error.time_from_start.to_sec(), 2.5, delta=1)
+        # feedback shouldn't be published after motion finishes
+        # https://github.com/start-jsk/rtmros_common/pull/1049#issuecomment-403780615
+        fb_id1 = id(self.feedback)
+        rospy.sleep(0.5)
+        fb_id2 = id(self.feedback)
+        rospy.logwarn("object id of feedback after motion: %r, 0.5sec after: %r"%(fb_id1, fb_id2))
+        self.assertEqual(fb_id1, fb_id2)
 
         ## move less than 1.0 sec
         goal.trajectory.header.stamp = rospy.get_rostime()

From dcfb14d5c5123c0f50a3dfda8446548becfdc6ac Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Mon, 9 Jul 2018 20:49:45 +0900
Subject: [PATCH 15/17] Set time_from_start to feedback of joint trajectory
 action

---
 hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 3 +++
 hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h   | 1 +
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp     | 4 ++++
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h       | 1 +
 4 files changed, 9 insertions(+)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index 336917762..fe766207e 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -330,6 +330,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
 
   // FIXME: need to set actual informatoin, currently we set dummy information
   trajectory_msgs::JointTrajectoryPoint commanded_joint_trajectory_point, error_joint_trajectory_point;
+  commanded_joint_trajectory_point.time_from_start = tm_on_execute - prev_traj_tm;
   commanded_joint_trajectory_point.positions.resize(joint_list.size());
   commanded_joint_trajectory_point.velocities.resize(joint_list.size());
   commanded_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -341,6 +342,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
       commanded_joint_trajectory_point.accelerations[j] = parent->body->link(joint_list[j])->ddq;
       commanded_joint_trajectory_point.effort[j]        = parent->body->link(joint_list[j])->u;
     }
+  error_joint_trajectory_point.time_from_start = tm_on_execute - prev_traj_tm;
   error_joint_trajectory_point.positions.resize(joint_list.size());
   error_joint_trajectory_point.velocities.resize(joint_list.size());
   error_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -502,6 +504,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       parent->m_service0->playPattern(angles, rpy, zmp, duration);
     }
   }
+  prev_traj_tm = ros::Time::now();
 
   interpolationp = true;
 }
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
index 4a4e3bd1a..605c8c3c4 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
@@ -102,6 +102,7 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
     std::string groupname;
     std::vector<std::string> joint_list;
     bool interpolationp;
+    ros::Time prev_traj_tm;
 
   public:
     typedef boost::shared_ptr<jointTrajectoryActionObj> Ptr;
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index 30bd3b7c7..e27b05c90 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -218,6 +218,7 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
     OpenHRP::dSequenceSequence rpy, zmp;
     m_service0->playPattern(angles, rpy, zmp, duration);
   }
+  prev_traj_tm = ros::Time::now();
 
   interpolationp = true;
 }
@@ -442,6 +443,9 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         //joint_state.velocity
         //joint_state.effort
         follow_joint_trajectory_feedback.joint_names.push_back(j->name);
+        follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - prev_traj_tm;
+        follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - prev_traj_tm;
+        follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - prev_traj_tm;
         follow_joint_trajectory_feedback.desired.positions.push_back(j->q);
         follow_joint_trajectory_feedback.actual.positions.push_back(j->q);
         follow_joint_trajectory_feedback.error.positions.push_back(0);
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
index bf615738c..00478cde1 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -87,6 +87,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
   void clock_cb(const rosgraph_msgs::ClockPtr& str) {};
 
   bool follow_action_initialized;
+  ros::Time prev_traj_tm;
 
   boost::mutex tf_mutex;
   double tf_rate;

From eb2b53ef14d84af38e39e774d2d7259efd02fc77 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Tue, 10 Jul 2018 19:32:13 +0900
Subject: [PATCH 16/17] Stop feedback when trajectory is completed

---
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index e27b05c90..16a2289b6 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -578,7 +578,8 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
     }
     if ( !follow_joint_trajectory_feedback.joint_names.empty() &&
          !follow_joint_trajectory_feedback.actual.positions.empty() &&
-         follow_action_initialized ) {
+         follow_action_initialized &&
+         follow_joint_trajectory_server.isActive() ) {
       follow_joint_trajectory_server.publishFeedback(follow_joint_trajectory_feedback);
     }
   } // end: m_mcangleIn

From 65a418b7672a8ae2cf1cdc0ca194c92d8cd07216 Mon Sep 17 00:00:00 2001
From: Shun Hasegawa <pazeshun3684@gmail.com>
Date: Wed, 11 Jul 2018 13:58:13 +0900
Subject: [PATCH 17/17] prev_traj_tm -> traj_start_tm

---
 hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp | 6 +++---
 hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h   | 2 +-
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp     | 8 ++++----
 hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h       | 2 +-
 4 files changed, 9 insertions(+), 9 deletions(-)

diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
index fe766207e..b94129315 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp
@@ -330,7 +330,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
 
   // FIXME: need to set actual informatoin, currently we set dummy information
   trajectory_msgs::JointTrajectoryPoint commanded_joint_trajectory_point, error_joint_trajectory_point;
-  commanded_joint_trajectory_point.time_from_start = tm_on_execute - prev_traj_tm;
+  commanded_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   commanded_joint_trajectory_point.positions.resize(joint_list.size());
   commanded_joint_trajectory_point.velocities.resize(joint_list.size());
   commanded_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -342,7 +342,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::proc()
       commanded_joint_trajectory_point.accelerations[j] = parent->body->link(joint_list[j])->ddq;
       commanded_joint_trajectory_point.effort[j]        = parent->body->link(joint_list[j])->u;
     }
-  error_joint_trajectory_point.time_from_start = tm_on_execute - prev_traj_tm;
+  error_joint_trajectory_point.time_from_start = tm_on_execute - traj_start_tm;
   error_joint_trajectory_point.positions.resize(joint_list.size());
   error_joint_trajectory_point.velocities.resize(joint_list.size());
   error_joint_trajectory_point.accelerations.resize(joint_list.size());
@@ -504,7 +504,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
       parent->m_service0->playPattern(angles, rpy, zmp, duration);
     }
   }
-  prev_traj_tm = ros::Time::now();
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }
diff --git a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
index 605c8c3c4..875745991 100644
--- a/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.h
@@ -102,7 +102,7 @@ class HrpsysJointTrajectoryBridge : public RTC::DataFlowComponentBase
     std::string groupname;
     std::vector<std::string> joint_list;
     bool interpolationp;
-    ros::Time prev_traj_tm;
+    ros::Time traj_start_tm;
 
   public:
     typedef boost::shared_ptr<jointTrajectoryActionObj> Ptr;
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
index 16a2289b6..03c8be9fa 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp
@@ -218,7 +218,7 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
     OpenHRP::dSequenceSequence rpy, zmp;
     m_service0->playPattern(angles, rpy, zmp, duration);
   }
-  prev_traj_tm = ros::Time::now();
+  traj_start_tm = ros::Time::now();
 
   interpolationp = true;
 }
@@ -443,9 +443,9 @@ RTC::ReturnCode_t HrpsysSeqStateROSBridge::onExecute(RTC::UniqueId ec_id)
         //joint_state.velocity
         //joint_state.effort
         follow_joint_trajectory_feedback.joint_names.push_back(j->name);
-        follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - prev_traj_tm;
-        follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - prev_traj_tm;
-        follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - prev_traj_tm;
+        follow_joint_trajectory_feedback.desired.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.actual.time_from_start = tm_on_execute - traj_start_tm;
+        follow_joint_trajectory_feedback.error.time_from_start = tm_on_execute - traj_start_tm;
         follow_joint_trajectory_feedback.desired.positions.push_back(j->q);
         follow_joint_trajectory_feedback.actual.positions.push_back(j->q);
         follow_joint_trajectory_feedback.error.positions.push_back(0);
diff --git a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
index 00478cde1..3e80a2891 100644
--- a/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
+++ b/hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.h
@@ -87,7 +87,7 @@ class HrpsysSeqStateROSBridge  : public HrpsysSeqStateROSBridgeImpl
   void clock_cb(const rosgraph_msgs::ClockPtr& str) {};
 
   bool follow_action_initialized;
-  ros::Time prev_traj_tm;
+  ros::Time traj_start_tm;
 
   boost::mutex tf_mutex;
   double tf_rate;