def init_robot(self):
"""Make sure the robot is booted and ready to receive commands."""
mode_client = actionlib.SimpleActionClient("/ur_hardware_interface/set_mode", SetModeAction)
timeout = rospy.Duration(30)
try:
mode_client.wait_for_server(timeout)
except rospy.exceptions.ROSException as err:
self.fail(
"Could not reach set_mode action. Make sure that the driver is actually running."
" Msg: {}".format(err)
)
goal = SetModeGoal()
goal.target_robot_mode = RobotMode.RUNNING
goal.play_program = False # we use headless mode during tests
mode_client.send_goal(goal)
mode_client.wait_for_result()
self.assertTrue(mode_client.get_result().success)
send_program_srv = rospy.ServiceProxy(
"/ur_hardware_interface/resend_robot_program", Trigger
)
send_program_srv.call()
rospy.sleep(5)
def set_robot_to_mode(self, target_mode):
goal = SetModeGoal()
goal.target_robot_mode = target_mode
goal.play_program = False # we use headless mode during tests
# This might be a bug to hunt down. We have to reset the program before calling `resend_robot_program`
goal.stop_program = True
self.set_mode_client.send_goal(goal)
self.set_mode_client.wait_for_result()
return self.set_mode_client.get_result().success
def test_switch_on(self):
"""Test to set an IO and check whether it has been set."""
goal = SetModeGoal()
goal.target_robot_mode = RobotMode.RUNNING
goal.play_program = False # we use headless mode during tests
self.client.send_goal(goal)
self.client.wait_for_result()
self.assertTrue(self.client.get_result().success)
def test_switch_on_and_off(self):
"""Do full integration test of robot driver"""
#### Switch on test ####
goal = SetModeGoal()
goal.target_robot_mode = RobotMode.RUNNING
goal.play_program = False # we use headless mode during tests
self.set_mode_client.send_goal(goal)
self.set_mode_client.wait_for_result()
self.assertTrue(self.set_mode_client.get_result().success)
def trajectory_test(self):
"""Test robot movement"""
#### Make sure the controller is up and running ####
goal = SetModeGoal()
goal.target_robot_mode = RobotMode.RUNNING
goal.play_program = False # we use headless mode during tests
self.set_mode_client.send_goal(goal)
self.set_mode_client.wait_for_result()
rospy.sleep(2) # TODO properly wait until the robot is running
send_program_srv = rospy.ServiceProxy(
"/ur_hardware_interface/resend_robot_program", Trigger)
send_program_srv.call()
rospy.sleep(2) # TODO properly wait until the controller is running
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
position_list.append([-1.0 for i in range(6)])
duration_list = [6.0, 9.0, 12.0]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending simple goal")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
"""Test trajectory server. This is more of a validation test that the testing suite does the
right thing."""
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-0.5 for i in range(6)])
# Create illegal goal by making the second point come earlier than the first
duration_list = [6.0, 3.0]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending illegal goal")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
# As timings are illegal, we expect the result to be INVALID_GOAL
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.INVALID_GOAL)
rospy.loginfo("Received result INVALID_GOAL")
"""Test robot movement"""
goal = FollowJointTrajectoryGoal()
goal.trajectory.joint_names = [
"elbow_joint", "shoulder_lift_joint", "shoulder_pan_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
position_list = [[0.0 for i in range(6)]]
position_list.append([-1.0 for i in range(6)])
duration_list = [6.0, 6.5]
for i, position in enumerate(position_list):
point = JointTrajectoryPoint()
point.positions = position
point.time_from_start = rospy.Duration(duration_list[i])
goal.trajectory.points.append(point)
rospy.loginfo("Sending scaled goal without time restrictions")
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.SUCCESSFUL)
rospy.loginfo("Received result SUCCESSFUL")
# Now do the same again, but with a goal time constraint
rospy.loginfo("Sending scaled goal with time restrictions")
goal.goal_time_tolerance = rospy.Duration(0.01)
self.trajectory_client.send_goal(goal)
self.trajectory_client.wait_for_result()
self.assertEqual(self.trajectory_client.get_result().error_code,
FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED)
rospy.loginfo("Received result GOAL_TOLERANCE_VIOLATED")