46 from moveit_ros_planning_interface._moveit_move_group_interface
import MoveGroupInterface
50 PLANNING_GROUP =
"manipulator" 51 PLANNING_NS =
"test_ns/" 64 self.group.set_joint_value_target(*args)
65 res = self.group.get_joint_value_target()
66 self.assertTrue(np.all(np.asarray(res) == np.asarray(expect)),
67 "Setting failed for %s, values: %s" % (type(args[0]), res))
70 n = self.group.get_variable_count()
78 self.group.set_joint_value_target(target)
79 return self.group.compute_plan()
82 current = np.asarray(self.group.get_current_joint_values())
84 plan1 = self.
plan(current + 0.2)
85 plan2 = self.
plan(current + 0.2)
88 self.assertTrue(self.group.execute(plan1))
91 self.assertFalse(self.group.execute(plan2))
94 plan3 = self.
plan(current)
95 self.assertTrue(self.group.execute(plan3))
98 if __name__ ==
'__main__':
99 PKGNAME =
'moveit_ros_planning_interface' 100 NODENAME =
'moveit_test_python_move_group' 101 rospy.init_node(NODENAME)
102 rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupNsTest)
def check_target_setting(self, expect, args)
def test_validation(self)
def test_target_setting(self)