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 MoveGroupInterface
10 
11 
12 class RobotStateUpdateTest(unittest.TestCase):
13  PLANNING_GROUP = "manipulator"
14 
15  @classmethod
16  def setUpClass(self):
17  self.group = MoveGroupInterface(self.PLANNING_GROUP, "robot_description", rospy.get_namespace())
18 
19  @classmethod
20  def tearDown(self):
21  pass
22 
23  def plan(self, target):
24  self.group.set_joint_value_target(target)
25  return self.group.compute_plan()
26 
27  def test(self):
28  current = np.asarray(self.group.get_current_joint_values())
29  for i in range(30):
30  target = current + np.random.uniform(-0.5, 0.5, size = current.shape)
31  # if plan was successfully executed, current state should be reported at target
32  if self.group.execute(self.plan(target)):
33  actual = np.asarray(self.group.get_current_joint_values())
34  self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0))
35  # otherwise current state should be still the same
36  else:
37  actual = np.asarray(self.group.get_current_joint_values())
38  self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0))
39 
40 
41 if __name__ == '__main__':
42  PKGNAME = 'moveit_ros_planning_interface'
43  NODENAME = 'moveit_test_robot_state_update'
44  rospy.init_node(NODENAME)
45  rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest)
46 
47  # suppress cleanup segfault in ROS < Kinetic
48  os._exit(0)


planning_interface
Author(s): Ioan Sucan
autogenerated on Sun Oct 18 2020 13:18:50