Source code for robots.pr2.torso_ros

#! /usr/bin/env python
"""
This script tests the PR2 torso armature joint
"""

from morse.testing.ros import RosTestCase

# Include this import to be able to use your test file as a regular 
# builder script, ie, usable with: 'morse [run|exec] base_testing.py
try:
    from morse.builder import *
except ImportError:
    pass

import sys
import time

import rospy
from control_msgs.msg import SingleJointPositionAction, SingleJointPositionGoal
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from sensor_msgs.msg import JointState
import actionlib

[docs]def getjoint(name): data = rospy.wait_for_message("/pr2/joint_states", JointState) idx = data.name.index(name) return data.position[idx]
[docs]class PR2TorsoTest(RosTestCase):
[docs] def setUpEnv(self): print("Adding a PR2 robot...") pr2 = BasePR2() pr2.add_interface('ros') env = Environment('empty', fastmode=True) env.set_camera_rotation([1.0470, 0, 0.7854])
[docs] def test_controller(self): topic = "/pr2/torso_controller/command" rospy.init_node('test_pr2_torso', disable_signals=True) rospy.loginfo("Preparing to publish on %s" % topic) ctl = rospy.Publisher(topic, JointTrajectory) self.assertEquals(getjoint("torso_lift_joint"), 0.0) duration = 0.1 traj = JointTrajectory() traj.joint_name = "torso_lift_joint" initialpoint = JointTrajectoryPoint() initialpoint.positions = 0.0 initialpoint.velocities = 0.0 initialpoint.time_from_start = rospy.Duration(0.0) finalpoint = JointTrajectoryPoint() finalpoint.positions = 0.195 finalpoint.velocities = 0.0 finalpoint.time_from_start = rospy.Duration(duration) # First, move up traj.points = [initialpoint, finalpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.195) # Go back to initial position finalpoint.time_from_start = rospy.Duration(0.0) initialpoint.time_from_start = rospy.Duration(duration) traj.points = [finalpoint, initialpoint] ctl.publish(traj) time.sleep(duration + 0.1) self.assertEquals(getjoint("torso_lift_joint"), 0.0)
[docs] def test_action(self): rospy.loginfo("Trying to move PR2 torso at action level.") rospy.init_node('test_pr2_torso', disable_signals=True) client = actionlib.SimpleActionClient('torso_controller/position_joint_action', SingleJointPositionAction) self.assertTrue(client.wait_for_server(rospy.Duration(5))) self.assertEquals(getjoint("torso_lift_joint"), 0.0) up = SingleJointPositionGoal up.position = 0.195 up.min_duration = rospy.Duration(2.0) up.max_velocity = 1.0 print("Sending a 'up' goal to the torso...(timeout=5sec)") status = client.send_goal_and_wait(up, rospy.Duration(5)) print("Got this status: " + str(status)) self.assertEqual(status, actionlib.GoalStatus.SUCCEEDED) self.assertEquals(getjoint("torso_lift_joint"), 0.195) down = SingleJointPositionGoal down.position = 0.0 down.min_duration = rospy.Duration(2.0) down.max_velocity = 1.0 print("Sending a 'down' goal to the torso...(timeout=5sec)") status = client.send_goal_and_wait(down, rospy.Duration(5)) print("Got this status: " + str(status)) self.assertEqual(status, actionlib.GoalStatus.SUCCEEDED) self.assertEquals(getjoint("torso_lift_joint"), 0.0)
########################## Run these tests ########################## if __name__ == "__main__": from morse.testing.testing import main main(PR2TorsoTest)