moveit2
The MoveIt Motion Planning Framework for ROS 2.
robot_state_update.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import unittest
4 import numpy as np
5 import rospy
6 import rostest
7 import os
8 
9 from moveit_ros_planning_interface._moveit_move_group_interface import (
10  MoveGroupInterface,
11 )
12 from moveit_msgs.msg import MoveItErrorCodes
13 
14 
15 class RobotStateUpdateTest(unittest.TestCase):
16  PLANNING_GROUP = "manipulator"
17 
18  @classmethod
19  def setUpClass(self):
20  self.groupgroup = MoveGroupInterface(
21  self.PLANNING_GROUPPLANNING_GROUP, "robot_description", rospy.get_namespace()
22  )
23 
24  @classmethod
25  def tearDown(self):
26  pass
27 
28  def plan(self, target):
29  self.group.set_joint_value_target(target)
30  return self.group.plan()
31 
32  def test(self):
33  current = np.asarray(self.groupgroup.get_current_joint_values())
34  for i in range(30):
35  target = current + np.random.uniform(-0.5, 0.5, size=current.shape)
36  # if plan was successfully executed, current state should be reported at target
37  error_code1, plan, time = self.planplan(target)
38  error_code = MoveItErrorCodes()
39  error_code.deserialize(error_code1)
40  if (error_code.val == MoveItErrorCodes.SUCCESS) and self.groupgroup.execute(
41  plan
42  ):
43  actual = np.asarray(self.groupgroup.get_current_joint_values())
44  self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0))
45  # otherwise current state should be still the same
46  else:
47  actual = np.asarray(self.groupgroup.get_current_joint_values())
48  self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0))
49 
50 
51 if __name__ == "__main__":
52  PKGNAME = "moveit_ros_planning_interface"
53  NODENAME = "moveit_test_robot_state_update"
54  rospy.init_node(NODENAME)
55  rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest)
56 
57  # suppress cleanup segfault in ROS < Kinetic
58  os._exit(0)
Definition: plan.py:1