mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Test Merger
This commit is contained in:
parent
745d6739bd
commit
49b2ff8ce5
@ -5,8 +5,9 @@ import unittest
|
|||||||
import rostest
|
import rostest
|
||||||
from moveit.python_tools import roscpp_init
|
from moveit.python_tools import roscpp_init
|
||||||
from moveit.task_constructor import core, stages
|
from moveit.task_constructor import core, stages
|
||||||
from geometry_msgs.msg import Pose
|
from geometry_msgs.msg import PoseStamped, Pose
|
||||||
from geometry_msgs.msg import PoseStamped
|
from geometry_msgs.msg import Vector3Stamped, Vector3
|
||||||
|
from std_msgs.msg import Header
|
||||||
import rospy
|
import rospy
|
||||||
|
|
||||||
|
|
||||||
@ -24,7 +25,7 @@ class Test(unittest.TestCase):
|
|||||||
def test_MoveRelative(self):
|
def test_MoveRelative(self):
|
||||||
task = core.Task()
|
task = core.Task()
|
||||||
task.add(stages.CurrentState("current"))
|
task.add(stages.CurrentState("current"))
|
||||||
move = stages.MoveRelative("move", core.PipelinePlanner())
|
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
|
||||||
move.group = self.PLANNING_GROUP
|
move.group = self.PLANNING_GROUP
|
||||||
move.setDirection({"joint_1" : 0.2, "joint_2" : 0.4})
|
move.setDirection({"joint_1" : 0.2, "joint_2" : 0.4})
|
||||||
task.add(move)
|
task.add(move)
|
||||||
@ -39,6 +40,34 @@ class Test(unittest.TestCase):
|
|||||||
s = task.solutions[0]
|
s = task.solutions[0]
|
||||||
task.execute(s)
|
task.execute(s)
|
||||||
|
|
||||||
|
def test_Merger(self):
|
||||||
|
cartesian = core.CartesianPath()
|
||||||
|
def createDisplacement(group, displacement):
|
||||||
|
move = stages.MoveRelative("displace", cartesian)
|
||||||
|
move.group = group
|
||||||
|
move.ik_frame = PoseStamped(header = Header(frame_id = "tool0"))
|
||||||
|
dir = Vector3Stamped(
|
||||||
|
header = Header(frame_id = "base_link"),
|
||||||
|
vector = Vector3(*displacement)
|
||||||
|
)
|
||||||
|
move.setDirection(dir)
|
||||||
|
move.restrictDirection(core.PropagationDirection.FORWARD)
|
||||||
|
return move
|
||||||
|
|
||||||
|
task = core.Task()
|
||||||
|
task.add(stages.CurrentState("current"))
|
||||||
|
merger = core.Merger("merger")
|
||||||
|
merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
|
||||||
|
merger.insert(createDisplacement(self.PLANNING_GROUP, [ 0.2, 0, 0]))
|
||||||
|
task.add(merger)
|
||||||
|
|
||||||
|
task.enableIntrospection()
|
||||||
|
task.init()
|
||||||
|
if task.plan():
|
||||||
|
task.publish(task.solutions[0])
|
||||||
|
|
||||||
|
rospy.sleep(100)
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == '__main__':
|
||||||
roscpp_init("test_mtc")
|
roscpp_init("test_mtc")
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user