mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
TestModifyPlanningScene
This commit is contained in:
parent
b318c3cae9
commit
45ca1a67a9
67
core/python/test/rostest_mps.py
Executable file
67
core/python/test/rostest_mps.py
Executable file
@ -0,0 +1,67 @@
|
||||
#! /usr/bin/env python
|
||||
|
||||
from __future__ import print_function
|
||||
import unittest
|
||||
import rostest
|
||||
from moveit_commander.roscpp_initializer import roscpp_initialize
|
||||
from moveit_commander import PlanningSceneInterface
|
||||
from moveit.task_constructor import core, stages
|
||||
from geometry_msgs.msg import PoseStamped
|
||||
|
||||
|
||||
def make_pose(x, y, z):
|
||||
pose = PoseStamped()
|
||||
pose.header.frame_id = "base_link"
|
||||
pose.pose.position.x = x
|
||||
pose.pose.position.y = y
|
||||
pose.pose.position.z = z
|
||||
pose.pose.orientation.w = 1.0
|
||||
return pose
|
||||
|
||||
|
||||
class TestModifyPlanningScene(unittest.TestCase):
|
||||
PLANNING_GROUP = "manipulator"
|
||||
|
||||
def setUp(self):
|
||||
super(TestModifyPlanningScene, self).setUp()
|
||||
self.psi = PlanningSceneInterface(synchronous=True)
|
||||
# insert a box to collide with
|
||||
self.psi.add_box("box", make_pose(0.8, 0.25, 1.25), [0.2, 0.2, 0.2])
|
||||
|
||||
# colliding motion
|
||||
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
|
||||
move.group = self.PLANNING_GROUP
|
||||
move.setDirection({"joint_1": 0.3})
|
||||
|
||||
self.task = task = core.Task()
|
||||
task.add(stages.CurrentState("current"), move)
|
||||
|
||||
def test_collision(self):
|
||||
self.assertFalse(self.task.plan())
|
||||
|
||||
def test_allow_collision_list(self):
|
||||
mps = stages.ModifyPlanningScene("mps")
|
||||
mps.allowCollisions("box", ["link_4", "link_5", "link_6"], True)
|
||||
self.task.insert(mps, 1)
|
||||
self.assertTrue(self.task.plan())
|
||||
|
||||
def test_allow_collision_all(self):
|
||||
# insert an extra collision object that is unknown to ACM
|
||||
self.psi.add_box("block", make_pose(0.8, 0.55, 1.25), [0.2, 0.2, 0.2])
|
||||
# attach box to end effector
|
||||
self.psi.attach_box("link_6", "box")
|
||||
mps = stages.ModifyPlanningScene("mps")
|
||||
self.assertFalse(self.task.plan())
|
||||
|
||||
# allow all collisions for attached "box" object
|
||||
mps.allowCollisions("box", True)
|
||||
self.task.insert(mps, 1)
|
||||
self.assertTrue(self.task.plan())
|
||||
# restore original state
|
||||
self.psi.remove_attached_object("link_6", "box")
|
||||
self.psi.remove_world_object("block")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
roscpp_initialize("test_mtc")
|
||||
rostest.rosrun("mtc", "mps", TestModifyPlanningScene)
|
||||
@ -14,14 +14,6 @@ import rospy
|
||||
class Test(unittest.TestCase):
|
||||
PLANNING_GROUP = "manipulator"
|
||||
|
||||
@classmethod
|
||||
def setUpClass(self):
|
||||
pass
|
||||
|
||||
@classmethod
|
||||
def tearDown(self):
|
||||
pass
|
||||
|
||||
def test_MoveAndExecute(self):
|
||||
moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner())
|
||||
moveRel.group = self.PLANNING_GROUP
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
<launch>
|
||||
<include file="$(find moveit_resources_fanuc_moveit_config)/launch/test_environment.launch" />
|
||||
<test pkg="moveit_task_constructor_core" type="rostest_mtc.py" test-name="rostest_mtc" time-limit="60" args="" />
|
||||
<test pkg="moveit_task_constructor_core" type="rostest_mps.py" test-name="rostest_mps" time-limit="60" args="" />
|
||||
<test pkg="moveit_task_constructor_core" type="rostest_trampoline.py" test-name="rostest_trampoline" time-limit="60" args="" />
|
||||
</launch>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user