Add test for joint trajectory action cancel and overwrite

[pazeshun]

For k-okada#10 (comment)
Test part of #1071
Travis test of this PR should be failed.

[pazeshun]

This PR tests the following three points against PA10 and samplerobot:

• Whether actionlib.SimpleActionClient.cancel_goal() (publishing to /<CONTROLLER_NAME>/follow_joint_trajectory_action/cancel) can be used to stop the robot motion.
  • As different hrpsys functions are used for between the trajectory having one point and the trajectory having multiple points, canceling one point trajectory and canceling two points trajectory are tested.
• Whether the robot smoothly changes its joint trajectory when a new goal comes (actionlib.SimpleActionClient.send_goal() is called before the robot finishes the previous goal and stops).
• Whether that new goal is accomplished in time.
  • PA 10 test :
    All transition types (one point trajectory -> one point trajectory, one point trajectory -> two points trajectory, two points trajectory -> one point trajectory, two points trajectory -> two points trajectory) are tested.
  • samplerobot test :
    This test should also include all transition types, but even with use setJointAnglesOfGroup() and playPattern(), use clearJointAngles() with test #1071 , two points trajectory is not overwritable because fix setJointAnglesSequenceOfGroup, it does not take current joint angle fkanehiro/hrpsys-base#1237 is not released (cf. Command overwritting does not work on angle-vector-sequence rtmros_hironx#534 (comment)). So currently, tests using two points trajectory are commented out.

Test failures are fixed with the commits of #765.
So I created #1071, which also includes k-okada#10 fixing an issue specifically critical for HIRONX.

[pazeshun]

Travis test correctly failed:

  <testcase classname="__main__.TestPA10Robot" name="test_jta_cancel_goal" time="10.9525">
    <failure type="AssertionError">0.36908914153100636 != 0 within 0.1 delta
  File "/usr/lib/python2.7/unittest/case.py", line 327, in run
    testMethod()
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-pa10.py", line 163, in test_jta_cancel_goal
    self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-pa10.py", line 187, in impl_test_jta_cancel_goal
    self.jta_cancel_goal_template(larm, goal, [point1])
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-pa10.py", line 209, in jta_cancel_goal_template
    self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
  File "/usr/lib/python2.7/unittest/case.py", line 559, in assertAlmostEqual
    raise self.failureException(msg)
    </failure>
  </testcase>
  <testcase classname="__main__.TestPA10Robot" name="test_jta_overwrite_goal" time="18.5075">
    <failure type="AssertionError">0.71282626947711192 != 0 within 0.1 delta
  File "/usr/lib/python2.7/unittest/case.py", line 327, in run
    testMethod()
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-pa10.py", line 218, in test_jta_overwrite_goal
    self.impl_test_jta_overwrite_goal(larm, JointTrajectoryGoal())
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-pa10.py", line 266, in impl_test_jta_overwrite_goal
    self.jta_overwrite_goal_template(larm, goal, trans1, [point1], [point2, point3])
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-pa10.py", line 325, in jta_overwrite_goal_template
    self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
  File "/usr/lib/python2.7/unittest/case.py", line 559, in assertAlmostEqual
    raise self.failureException(msg)
    </failure>
  </testcase>

  <testcase classname="__main__.TestSampleRobot" name="test_jta_cancel_goal" time="10.0140">
    <failure type="AssertionError">0.12036482272553561 != 0 within 0.1 delta
  File "/usr/lib/python2.7/unittest/case.py", line 327, in run
    testMethod()
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-samplerobot.py", line 258, in test_jta_cancel_goal
    self.impl_test_jta_cancel_goal(larm, JointTrajectoryGoal())
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-samplerobot.py", line 291, in impl_test_jta_cancel_goal
    self.jta_cancel_goal_template(larm, goal, [point1])
  File "/workspace/ros/ws_rtmros_common/src/rtmros_common/hrpsys_ros_bridge/test/test-samplerobot.py", line 313, in jta_cancel_goal_template
    self.assertAlmostEqual(linalg.norm(array(trans1)-array(trans2)), 0, delta=0.1)
  File "/usr/lib/python2.7/unittest/case.py", line 559, in assertAlmostEqual
    raise self.failureException(msg)
    </failure>
  </testcase>

(from USE_JENKINS=true ROS_DISTRO=hydro USE_DEB=false NOT_TEST_INSTALL=true)