Merge pull request #1042 from YoheiKakiuchi/fix_less_than_one_sec

Fix less than one sec

Author: k-okada

hrpsys_ros_bridge/src/HrpsysJointTrajectoryBridge.cpp

@@ -472,7 +472,7 @@ void HrpsysJointTrajectoryBridge::jointTrajectoryActionObj::onJointTrajectory(
     else
     { // if i == 0
       duration[i] = trajectory.points[i].time_from_start.toSec();
-      if ( abs(duration[i]) < 0.001 ) duration[i] = 0.001; // set magic delta ... https://github.com/start-jsk/rtmros_common/issues/1036
+      if ( std::abs(duration[i]) < 0.001 ) duration[i] = 0.001; // set magic delta ... https://github.com/start-jsk/rtmros_common/issues/1036
     }
   }
 

hrpsys_ros_bridge/src/HrpsysSeqStateROSBridge.cpp

@@ -206,7 +206,7 @@ void HrpsysSeqStateROSBridge::onJointTrajectory(trajectory_msgs::JointTrajectory
       duration[i] =  trajectory.points[i].time_from_start.toSec() - trajectory.points[i-1].time_from_start.toSec();
     } else { // if i == 0
       duration[i] = trajectory.points[i].time_from_start.toSec();
-      if ( abs(duration[i]) < 0.001 ) duration[i] = 0.001; // set delta ... https://github.com/start-jsk/rtmros_common/issues/1036
+      if ( std::abs(duration[i]) < 0.001 ) duration[i] = 0.001; // set delta ... https://github.com/start-jsk/rtmros_common/issues/1036
     }
   }
 

hrpsys_ros_bridge/test/test-samplerobot.py

@@ -118,6 +118,19 @@ def impl_test_joint_angles(self, larm, goal):
         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)
 
+        ## move less than 1.0 sec
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        point = JointTrajectoryPoint()
+        point.positions = [ x * math.pi / 180.0 for x in [30,30,30, 0,-40,-30] ]
+        point.time_from_start = rospy.Duration(0.9)
+        goal.trajectory.points = [point]
+        larm.send_goal(goal)
+        start_time = rospy.get_rostime()
+        larm.wait_for_result()
+        stop_time  = rospy.get_rostime()
+        rospy.logwarn("working time: %f"%(stop_time - start_time).to_sec())
+        self.assertNotAlmostEqual((stop_time - start_time).to_sec(), 0, delta=0.1)
+
     # send joint angles
     def test_joint_angles(self):
         # for < kinetic