Merge pull request #254 from Naoki-Hiraoka/revert-default-value

[hrpsys_gazebo_general] revert default value of robot_hrpsys_bringup.launch and add test

hrpsys_gazebo_general/CMakeLists.txt

@@ -2,7 +2,7 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(hrpsys_gazebo_general)
 
-find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge hrpsys_gazebo_msgs)
+find_package(catkin REQUIRED COMPONENTS hrpsys_ros_bridge hrpsys_gazebo_msgs rostest)
 
 find_package(PkgConfig)
 pkg_check_modules(openrtm_aist openrtm-aist REQUIRED)
@@ -100,3 +100,12 @@ add_custom_target(all_robots_compile ALL DEPENDS ${compile_urdf_robots})
 ## install
 install(DIRECTORY launch scripts worlds config robot_models DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} USE_SOURCE_PERMISSIONS)
 install(PROGRAMS setup.sh DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+
+## Test
+if ("hydro" STREQUAL "$ENV{ROS_DISTRO}")
+elseif ("indigo" STREQUAL "$ENV{ROS_DISTRO}")
+  #add_rostest(test/test-samplerobot-indigo.test)
+else()
+  add_rostest(test/test-samplerobot.test)
+endif()

hrpsys_gazebo_general/launch/robot_hrpsys_bringup.launch

@@ -27,7 +27,7 @@
   <!-- hrpsys_gazebo settings -->
   <arg name="SYNCHRONIZED" default="false" />
   <arg name="IOB_SUBSTEPS" default="5" />
-  <arg name="HRPSYS_RATE"  default="500" />
+  <arg name="HRPSYS_RATE"  default="200" />
 
   <env if="$(arg SYNCHRONIZED)"
        name="HRPSYS_GAZEBO_IOB_SYNCHRONIZED" value="1" />

hrpsys_gazebo_general/launch/samplerobot_hrpsys_bringup.launch

@@ -26,5 +26,6 @@
     <arg name="HRPSYS_PY_NAME" value="samplerobot_hrpsys_config.py" if="$(arg USE_UNSTABLE_RTC)"/>
     <arg name="KINEMATICS_MODE" value="$(arg KINEMATICS_MODE)"/>
     <arg name="BASE_LINK" value="WAIST_LINK0" />
+    <arg name="HRPSYS_RATE"  value="500" />
   </include>
 </launch>

hrpsys_gazebo_general/package.xml

@@ -21,6 +21,7 @@
   <build_depend>gazebo_ros</build_depend>
   <build_depend>hrpsys_gazebo_msgs</build_depend>
   <build_depend>hrpsys_ros_bridge</build_depend>
+  <build_depend>rostest</build_depend>
 
   <run_depend>collada_urdf_jsk_patch</run_depend>
   <run_depend>gazebo_ros</run_depend>

hrpsys_gazebo_general/scripts/gazebo

@@ -4,5 +4,6 @@ rospack find drcsim_gazebo 2> /dev/null
 if [ $? -eq 0 ]; then
     rosrun drcsim_gazebo run_gazebo $@
 else
+    pkill gzserver
     rosrun gazebo_ros gazebo $@
 fi

hrpsys_gazebo_general/test/launch_hrpsys.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+
+sleep 10 #wait for /clock
+rtmlaunch hrpsys_gazebo_general samplerobot_hrpsys_bringup.launch 2>/dev/null #to avoid exceeding the maximum log length

hrpsys_gazebo_general/test/launch_hrpsys_indigo.sh

@@ -0,0 +1,4 @@
+#!/bin/bash
+
+sleep 10 #wait for /clock
+rtmlaunch hrpsys_gazebo_general samplerobot_hrpsys_bringup_indigo.launch 2>/dev/null #to avoid exceeding the maximum log length

hrpsys_gazebo_general/test/test-samplerobot-indigo.test

@@ -0,0 +1,5 @@
+<launch>
+  <include file="$(find hrpsys_gazebo_general)/test/test-samplerobot.test" >
+    <arg name="INDIGO" value="true"/>
+  </include>
+</launch>

hrpsys_gazebo_general/test/test-samplerobot.py

@@ -0,0 +1,192 @@
+#!/usr/bin/env python
+
+PKG = 'hrpsys_gazebo_general'
+NAME = 'test_samplerobot'
+
+import argparse,unittest,rostest, time, sys, math, os
+from copy import deepcopy
+from numpy import *
+
+import rospy,rospkg, tf
+from geometry_msgs.msg import WrenchStamped
+from nav_msgs.msg import Odometry
+from sensor_msgs.msg import JointState
+from gazebo_msgs.msg import ModelStates
+from hrpsys_ros_bridge.srv import *
+
+import actionlib
+
+from trajectory_msgs.msg import JointTrajectoryPoint
+
+class TestSampleRobot(unittest.TestCase):
+    listener = None
+    lfsensor = None
+    rfsensor = None
+    odom = None
+    joint_states = None
+    model_states = None
+    feedback = None
+
+    def lfsensor_cb(self, sensor):
+        self.lfsensor = sensor
+
+    def rfsensor_cb(self, sensor):
+        self.rfsensor = sensor
+    def odom_cb(self, odom):
+        self.odom = odom
+    def jointstate_cb(self, joint_states):
+        self.joint_states = joint_states
+    def modelstate_cb(self, model_states):
+        self.model_states = model_states
+
+    def setUp(self):
+        rospy.init_node('TestSampleRobot')
+        rospy.Subscriber('/lfsensor', WrenchStamped, self.lfsensor_cb)
+        rospy.Subscriber('/rfsensor', WrenchStamped, self.rfsensor_cb)
+        rospy.Subscriber('/odom', Odometry, self.odom_cb)
+        rospy.Subscriber('/joint_states', JointState, self.jointstate_cb)
+        rospy.Subscriber('/gazebo/model_states', ModelStates, self.modelstate_cb)
+        self.listener = tf.TransformListener()
+
+    def test_odom(self): # need to check if map/ is published?
+        # wait odom topic
+        start_time = rospy.Time.now()
+        r = rospy.Rate(1)
+        while not rospy.is_shutdown() and (rospy.Time.now() - start_time).to_sec() < 120:
+            if self.odom:
+                break
+            rospy.loginfo("waiting for /odom")
+            r.sleep()
+        if not self.odom:
+            self.assertTrue(False, "no odom topic is available")
+        else:
+            self.assertTrue(True,"ok")
+
+    def test_odom_tf(self): # echeck if tf/odom is published
+        # wait odom topic
+        try:
+            rospy.loginfo("waiting for /WAIST_LINK0 to /odom")
+            self.listener.waitForTransform('/WAIST_LINK0', '/odom', rospy.Time(), rospy.Duration(120))
+        except tf.Exception:
+            self.assertTrue(None, "could not found tf from /WAIST_LINK0 to /odom")
+        (trans1,rot1) = self.listener.lookupTransform('/WAIST_LINK0', '/odom', rospy.Time(0))
+
+        self.assertTrue(True,"ok")
+
+    def test_force_sensor(self):
+        while self.lfsensor == None or self.rfsensor == None:
+            time.sleep(1)
+            rospy.logwarn("wait for sensor...")
+        rospy.logwarn("sensor = %r %r"%(self.lfsensor, self.rfsensor))
+        self.assertAlmostEqual(self.lfsensor.wrench.force.z+self.rfsensor.wrench.force.z, 1300, delta=200)
+
+    def impl_test_joint_angles(self, fullbody, goal):
+        fullbody.wait_for_server()
+
+        goal.trajectory.header.stamp = rospy.get_rostime()
+        goal.trajectory.joint_names.append("RLEG_HIP_R")
+        goal.trajectory.joint_names.append("RLEG_HIP_P")
+        goal.trajectory.joint_names.append("RLEG_HIP_Y")
+        goal.trajectory.joint_names.append("RLEG_KNEE")
+        goal.trajectory.joint_names.append("RLEG_ANKLE_P")
+        goal.trajectory.joint_names.append("RLEG_ANKLE_R")
+        goal.trajectory.joint_names.append("RARM_SHOULDER_P")
+        goal.trajectory.joint_names.append("RARM_SHOULDER_R")
+        goal.trajectory.joint_names.append("RARM_SHOULDER_Y")
+        goal.trajectory.joint_names.append("RARM_ELBOW")
+        goal.trajectory.joint_names.append("RARM_WRIST_Y")
+        goal.trajectory.joint_names.append("RARM_WRIST_P")
+        goal.trajectory.joint_names.append("RARM_WRIST_R")
+        goal.trajectory.joint_names.append("LLEG_HIP_R")
+        goal.trajectory.joint_names.append("LLEG_HIP_P")
+        goal.trajectory.joint_names.append("LLEG_HIP_Y")
+        goal.trajectory.joint_names.append("LLEG_KNEE")
+        goal.trajectory.joint_names.append("LLEG_ANKLE_P")
+        goal.trajectory.joint_names.append("LLEG_ANKLE_R")
+        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")
+        goal.trajectory.joint_names.append("LARM_WRIST_R")
+        goal.trajectory.joint_names.append("WAIST_P")
+        goal.trajectory.joint_names.append("WAIST_R")
+        goal.trajectory.joint_names.append("CHEST")
+
+        point = JointTrajectoryPoint()
+        target_positions = [-0.004457,-21.6929,-0.01202,47.6723,-25.93,0.014025,17.8356,-9.13759,-6.61188,-36.456,0.0,0.0,0.0,-0.004457,-21.6929,-0.01202,47.6723,-25.93,0.014025,17.8356,9.13759,6.61188,-36.456,0.0,0.0,0.0,0.0,0.0,0.0] # reset-pose
+        point.positions = [ x * math.pi / 180.0 for x in target_positions ]
+        point.time_from_start = rospy.Duration(5.0)
+        goal.trajectory.points.append(point)
+        fullbody.send_goal(goal)
+        fullbody.wait_for_result()
+
+        ## wait for update joint_states
+        while self.joint_states == None:
+            time.sleep(1)
+            rospy.logwarn("wait for joint_states..")
+        rospy.sleep(1)
+        current_positions = dict(zip(self.joint_states.name, self.joint_states.position))
+        goal_angles = [ 180.0 / math.pi * current_positions[x] for x in goal.trajectory.joint_names]
+        rospy.logwarn(goal_angles)
+        rospy.logwarn("difference between two angles %r %r"%(array(target_positions)-array(goal_angles),linalg.norm(array(target_positions)-array(goal_angles))))
+        self.assertAlmostEqual(linalg.norm(array(target_positions)-array(goal_angles)), 0, delta=0.1 * 180.0 / math.pi)
+
+    # send joint angles
+    def test_joint_angles(self):
+        # for < kinetic
+        if os.environ['ROS_DISTRO'] >= 'kinetic' :
+            return True
+        from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal
+        fullbody = actionlib.SimpleActionClient("/fullbody_controller/joint_trajectory_action", JointTrajectoryAction)
+        self.impl_test_joint_angles(fullbody, JointTrajectoryGoal())
+
+    def test_follow_joint_angles(self):
+        # for >= kinetic
+        if os.environ['ROS_DISTRO'] < 'kinetic' :
+            return True
+        from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal
+        fullbody = actionlib.SimpleActionClient("/fullbody_controller/follow_joint_trajectory_action", FollowJointTrajectoryAction)
+        self.impl_test_joint_angles(fullbody, FollowJointTrajectoryGoal())
+
+    # send walk motion
+    # test_walk_motion should be executed after test_joint_angles and test_follow_joint_angles. The order is determined by sorting the test method names (https://docs.python.org/3/library/unittest.html).
+    def test_walk_motion(self):
+        while self.model_states == None:
+            time.sleep(1)
+            rospy.logwarn("wait for gazebo/model_states..")
+
+        rospy.wait_for_service('/AutoBalancerServiceROSBridge/startAutoBalancer')
+        startAutoBalancer = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/startAutoBalancer', OpenHRP_AutoBalancerService_startAutoBalancer)
+        startAutoBalancer(["rleg","lleg","rarm","larm"])
+        rospy.sleep(3.0)
+        current_poses = dict(zip(self.model_states.name, self.model_states.pose))
+        self.assertAlmostEqual(current_poses["SampleRobot"].position.z, 0.668, delta=0.1)
+        self.assertAlmostEqual(linalg.norm(array([current_poses["SampleRobot"].orientation.x,current_poses["SampleRobot"].orientation.y,current_poses["SampleRobot"].orientation.z,current_poses["SampleRobot"].orientation.w])-array([0.0,0.0,0.0,1.0])), 0, delta=0.1)
+
+        rospy.wait_for_service('/StabilizerServiceROSBridge/startStabilizer')
+        startStabilizer = rospy.ServiceProxy('/StabilizerServiceROSBridge/startStabilizer', OpenHRP_StabilizerService_startStabilizer)
+        startStabilizer()
+        rospy.sleep(3.0)
+        current_poses = dict(zip(self.model_states.name, self.model_states.pose))
+        self.assertAlmostEqual(current_poses["SampleRobot"].position.z, 0.668, delta=0.1)
+        self.assertAlmostEqual(linalg.norm(array([current_poses["SampleRobot"].orientation.x,current_poses["SampleRobot"].orientation.y,current_poses["SampleRobot"].orientation.z,current_poses["SampleRobot"].orientation.w])-array([0.0,0.0,0.0,1.0])), 0, delta=0.1)
+
+        rospy.wait_for_service('/AutoBalancerServiceROSBridge/goPos')
+        goPos = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/goPos', OpenHRP_AutoBalancerService_goPos)
+        goPos(1.0,0,0)
+        rospy.wait_for_service('/AutoBalancerServiceROSBridge/waitFootSteps')
+        waitFootSteps = rospy.ServiceProxy('/AutoBalancerServiceROSBridge/waitFootSteps', OpenHRP_AutoBalancerService_waitFootSteps)
+        waitFootSteps()
+        rospy.sleep(1.0)
+        current_poses = dict(zip(self.model_states.name, self.model_states.pose))
+        self.assertAlmostEqual(current_poses["SampleRobot"].position.z, 0.668, delta=0.1)
+        self.assertAlmostEqual(linalg.norm(array([current_poses["SampleRobot"].orientation.x,current_poses["SampleRobot"].orientation.y,current_poses["SampleRobot"].orientation.z,current_poses["SampleRobot"].orientation.w])-array([0.0,0.0,0.0,1.0])), 0, delta=0.1)
+        self.assertAlmostEqual(current_poses["SampleRobot"].position.x, 1.0, delta=0.1)
+        self.assertAlmostEqual(current_poses["SampleRobot"].position.y, 0.0, delta=0.1)
+
+#unittest.main()
+if __name__ == '__main__':
+    import rostest
+    rostest.run(PKG, NAME, TestSampleRobot, sys.argv)

hrpsys_gazebo_general/test/test-samplerobot.test

@@ -0,0 +1,51 @@
+<launch>
+  <arg name="INDIGO" default="false"/>
+
+  <group if="$(arg INDIGO)">
+    <include file="$(find hrpsys_gazebo_general)/launch/gazebo_samplerobot_no_controllers_indigo.launch" />
+    <node pkg="hrpsys_gazebo_general" type="launch_hrpsys_indigo.sh" name="launch_hrpsys_indigo"/>
+  </group>
+
+  <group unless="$(arg INDIGO)">
+    <include file="$(find hrpsys_gazebo_general)/launch/gazebo_samplerobot_no_controllers.launch" />
+    <node pkg="hrpsys_gazebo_general" type="launch_hrpsys.sh" name="launch_hrpsys"/>
+  </group>
+
+
+  <node name="hrpsys_seq_state_ros_bridge_tf_relay" pkg="hrpsys_ros_bridge" type="hrpsys_seq_state_ros_bridge_tf_relay_for_test.py"/>
+
+  <!-- check if /SampleRobot/robot_state is published -->
+  <param name="hztest_rs/topic" value="/SampleRobot/robot_state" />
+  <param name="hztest_rs/wait_time" value="100" />
+  <param name="hztest_rs/hz" value="500.0" />
+  <param name="hztest_rs/hzerror" value="200.0" />
+  <param name="hztest_rs/test_duration" value="5.0" />
+  <test test-name="hztest_rs" pkg="rostest" type="hztest" name="hztest_rs" retry="4" />
+
+  <!-- check if /SampleRobot/joint_command is published -->
+  <param name="hztest_jc/topic" value="/SampleRobot/joint_command" />
+  <param name="hztest_jc/wait_time" value="100" />
+  <param name="hztest_jc/hz" value="500.0" />
+  <param name="hztest_jc/hzerror" value="200.0" />
+  <param name="hztest_jc/test_duration" value="5.0" />
+  <test test-name="hztest_jc" pkg="rostest" type="hztest" name="hztest_jc" retry="4" />
+
+  <!-- check if joint_states is published -->
+  <param name="hztest_js/topic" value="/joint_states" />
+  <param name="hztest_js/wait_time" value="100" />
+  <param name="hztest_js/hz" value="500.0" />
+  <param name="hztest_js/hzerror" value="200.0" />
+  <param name="hztest_js/test_duration" value="5.0" />
+  <test test-name="hztest_js" pkg="rostest" type="hztest" name="hztest_js" retry="4" />
+
+  <!-- check if tf is published -->
+  <param name="hztest_tf/topic" value="/hrpsys_ros_bridge/tf" />
+  <param name="hztest_tf/wait_time" value="100" />
+  <param name="hztest_tf/hz" value="50.0" />
+  <param name="hztest_tf/hzerror" value="20.0" />
+  <param name="hztest_tf/test_duration" value="5.0" />
+  <test test-name="hztest_tf" pkg="rostest" type="hztest" name="hztest_tf" retry="4" />
+
+  <test test-name="samplerobot" pkg="hrpsys_gazebo_general" type="test-samplerobot.py" retry="4" time-limit="400"/>
+
+</launch>