test_check_state_validity_in_empty_scene.py

Go to the documentation of this file.

#!/usr/bin/env python

import sys
import rospy
import unittest
from actionlib import SimpleActionClient
import moveit_commander
from moveit_msgs.msg import Constraints
from moveit_msgs.srv import GetStateValidity


class TestCheckStateValidityInEmptyScene(unittest.TestCase):

    def setUp(self):
        moveit_commander.roscpp_initialize(sys.argv)
        self.robot_commander = moveit_commander.RobotCommander()
        self.group_name = self.robot_commander.get_group_names()[0]
        self.group_commander = moveit_commander.MoveGroupCommander(self.group_name)

        self._check_state_validity = rospy.ServiceProxy('/check_state_validity', GetStateValidity)

    def test_check_collision_free_state_validity_in_empty_scene(self):
        current_robot_state = self.robot_commander.get_current_state()

        validity_report = self._check_state_validity(current_robot_state, self.group_name, Constraints())
        self.assertListEqual(validity_report.contacts, [], "In the default_robot_state, the robot should not collide with itself")


    def test_check_colliding_state_validity_in_empty_scene(self):
        current_robot_state = self.robot_commander.get_current_state()

        # force a colliding state with the Fanuc M-10iA
        current_robot_state.joint_state.position = list(current_robot_state.joint_state.position)
        current_robot_state.joint_state.position[current_robot_state.joint_state.name.index("joint_3")] = -2

        validity_report = self._check_state_validity(current_robot_state, self.group_name, Constraints())

        self.assertNotEqual(len(validity_report.contacts), 0, "When the robot collides with itself, it should have some contacts (with itself)")


if __name__ == '__main__':
    import rostest
    rospy.init_node('check_state_validity_in_empty_scene')
    rostest.rosrun('moveit_ros_move_group', 'test_check_state_validity_in_empty_scene', TestCheckStateValidityInEmptyScene)