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")