Use MoveIt's roscpp_initialize

This commit is contained in:
Robert Haschke 2023-02-15 14:32:20 +01:00
parent d95a2fc787
commit 885ac49ffb
15 changed files with 29 additions and 28 deletions

View File

@ -32,6 +32,7 @@
<test_depend>rostest</test_depend>
<test_depend>moveit_resources_fanuc_moveit_config</test_depend>
<test_depend>moveit_planners</test_depend>
<test_depend>moveit_commander</test_depend>
<export>
<moveit_task_constructor_core plugin="${prefix}/motion_planning_stages_plugin_description.xml" />

View File

@ -3,7 +3,7 @@
from __future__ import print_function
import unittest
import rostest
from moveit.tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped, Pose
from geometry_msgs.msg import Vector3Stamped, Vector3
@ -65,5 +65,5 @@ class Test(unittest.TestCase):
if __name__ == "__main__":
roscpp_init("test_mtc")
roscpp_initialize("test_mtc")
rostest.rosrun("mtc", "base", Test)

View File

@ -4,7 +4,7 @@
from __future__ import print_function
import unittest
import rostest
from moveit.tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.task_constructor import core, stages
from moveit.core.planning_scene import PlanningScene
from geometry_msgs.msg import Vector3Stamped, Vector3
@ -131,5 +131,5 @@ class TestTrampolines(unittest.TestCase):
if __name__ == "__main__":
roscpp_init("test_mtc")
roscpp_initialize("test_mtc")
rostest.rosrun("mtc", "trampoline", TestTrampolines)

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_alternatives")
roscpp_initialize("mtc_tutorial_alternatives")
# Use the joint interpolation planner
jointPlanner = core.JointInterpolationPlanner()

View File

@ -7,9 +7,9 @@ from moveit.task_constructor import core, stages
from math import pi
import time
from moveit.tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
roscpp_init("mtc_tutorial")
roscpp_initialize("mtc_tutorial")
# [cartesianTut1]
group = "panda_arm"

View File

@ -6,9 +6,9 @@ from geometry_msgs.msg import PoseStamped, Pose, Vector3
from std_msgs.msg import Header
import time
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
roscpp_init("mtc_tutorial_compute_ik")
roscpp_initialize("mtc_tutorial_compute_ik")
# Specify the planning group
group = "panda_arm"

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_initialize("mtc_tutorial_current_state")
# Create a task
task = core.Task()

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_fallbacks")
roscpp_initialize("mtc_tutorial_fallbacks")
# use cartesian and joint interpolation planners
cartesianPlanner = core.CartesianPath()

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_initialize("mtc_tutorial_current_state")
# Create a task
task = core.Task()

View File

@ -3,11 +3,11 @@
from moveit.core import planning_scene
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.core.planning_scene import PlanningScene
import time
roscpp_init("mtc_tutorial_current_state")
roscpp_initialize("mtc_tutorial_current_state")
# Create a task

View File

@ -3,10 +3,10 @@
from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_compute_ik")
roscpp_initialize("mtc_tutorial_compute_ik")
# Specify the planning group
group = "panda_arm"

View File

@ -2,10 +2,10 @@
# -*- coding: utf-8 -*-
from moveit.task_constructor import core, stages
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_merger")
roscpp_initialize("mtc_tutorial_merger")
# use the joint interpolation planner
planner = core.JointInterpolationPlanner()

View File

@ -5,10 +5,10 @@ from moveit.task_constructor import core, stages
from moveit_msgs.msg import CollisionObject
from shape_msgs.msg import SolidPrimitive
from geometry_msgs.msg import PoseStamped
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
import time
roscpp_init("mtc_tutorial_modify_planning_scene")
roscpp_initialize("mtc_tutorial_modify_planning_scene")
# Create a task
task = core.Task()

View File

@ -1,13 +1,13 @@
#! /usr/bin/env python
# -*- coding: utf-8 -*-
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
from moveit.task_constructor import core, stages
from moveit_commander import PlanningSceneInterface
from geometry_msgs.msg import PoseStamped, TwistStamped
import time
roscpp_init("pickplace")
roscpp_initialize("pickplace")
# [pickAndPlaceTut1]
# Specify robot parameters

View File

@ -5,9 +5,9 @@ from moveit.task_constructor import core, stages
from geometry_msgs.msg import PoseStamped
import time
from moveit.python_tools import roscpp_init
from moveit_commander.roscpp_initializer import roscpp_initialize
roscpp_init("mtc_tutorial")
roscpp_initialize("mtc_tutorial")
# Create a task container
task = core.Task()