Adapt python scripts to ROS2 API changes

Author: rhaschke

core/python/CMakeLists.txt

@@ -27,10 +27,8 @@ add_subdirectory(bindings)
 
 if(BUILD_TESTING)
 	find_package(ament_cmake_pytest REQUIRED)
-	set(_pytest_tests
-		test/test_mtc.py
-		test/rostest_mtc.py
-	)
+	find_package(launch_testing_ament_cmake)
+	set(_pytest_tests test/test_mtc.py)
 	foreach(_test_path ${_pytest_tests})
 		get_filename_component(_test_name ${_test_path} NAME_WE)
 		ament_add_pytest_test(${_test_name} ${_test_path}
@@ -39,4 +37,8 @@ if(BUILD_TESTING)
 			WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}
 		)
 	endforeach()
+
+	# add_launch_test(test/test.launch.py TARGET rostest_mtc ARGS "exe:=${CMAKE_CURRENT_SOURCE_DIR}/test/rostest_mtc.py")
+	# add_launch_test(test/test.launch.py TARGET rostest_mps ARGS "exe:=${CMAKE_CURRENT_SOURCE_DIR}/test/rostest_mps.py")
+	# add_launch_test(test/test.launch.py TARGET rostest_trampoline ARGS "exe:=${CMAKE_CURRENT_SOURCE_DIR}/test/rostest_trampoline.py")
 endif()

core/python/test/rostest_mps.py

@@ -1,15 +1,13 @@
 #! /usr/bin/env python3
 
 import unittest
-import rostest
-from py_binding_tools import roscpp_init
-from moveit_commander import PlanningSceneInterface
+import rclcpp
 from moveit.task_constructor import core, stages
 from geometry_msgs.msg import PoseStamped
 
 
 def setUpModule():
-    roscpp_init("test_mtc")
+    rclcpp.init()
 
 
 def make_pose(x, y, z):
@@ -31,7 +29,9 @@ def setUp(self):
         self.make_box = self.psi._PlanningSceneInterface__make_box
         # 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])
+        self.node = rclcpp.Node("test_mtc")
         self.task = task = core.Task()
+        self.task.loadRobotModel(self.node)
         task.add(stages.CurrentState("current"))
 
     def insertMove(self, position=-1):
@@ -116,4 +116,4 @@ def test_bw_remove_object(self):
 
 
 if __name__ == "__main__":
-    rostest.rosrun("mtc", "mps", TestModifyPlanningScene)
+    unittest.main()

core/python/test/rostest_mtc.py

@@ -1,21 +1,23 @@
 #! /usr/bin/env python3
 
 import unittest
-import rostest
-from py_binding_tools import roscpp_init
+import rclcpp
 from moveit.task_constructor import core, stages
 from geometry_msgs.msg import PoseStamped
 from geometry_msgs.msg import Vector3Stamped, Vector3
 from std_msgs.msg import Header
 
 
 def setUpModule():
-    roscpp_init("test_mtc")
+    rclcpp.init()
 
 
 class Test(unittest.TestCase):
     PLANNING_GROUP = "manipulator"
 
+    def setUp(self):
+        self.node = rclcpp.Node("test_mtc")
+
     def test_MoveAndExecute(self):
         moveRel = stages.MoveRelative("moveRel", core.JointInterpolationPlanner())
         moveRel.group = self.PLANNING_GROUP
@@ -26,6 +28,8 @@ def test_MoveAndExecute(self):
         moveTo.setGoal("all-zeros")
 
         task = core.Task()
+        task.loadRobotModel(self.node)
+
         task.add(stages.CurrentState("current"), moveRel, moveTo)
 
         self.assertTrue(task.plan())
@@ -45,6 +49,8 @@ def createDisplacement(group, displacement):
             return move
 
         task = core.Task()
+        task.loadRobotModel(self.node)
+
         task.add(stages.CurrentState("current"))
         merger = core.Merger("merger")
         merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
@@ -57,4 +63,4 @@ def createDisplacement(group, displacement):
 
 
 if __name__ == "__main__":
-    rostest.rosrun("mtc", "base", Test)
+    unittest.main()

core/python/test/rostest_trampoline.py

@@ -2,8 +2,7 @@
 # -*- coding: utf-8 -*-
 
 import unittest
-import rostest
-from py_binding_tools import roscpp_init
+import rclcpp
 from moveit.task_constructor import core, stages
 from moveit.core.planning_scene import PlanningScene
 from geometry_msgs.msg import Vector3Stamped, Vector3, PoseStamped
@@ -13,7 +12,7 @@
 
 
 def setUpModule():
-    roscpp_init("test_mtc")
+    rclcpp.init()
 
 
 def pybind11_versions():
@@ -100,9 +99,11 @@ class TestTrampolines(unittest.TestCase):
     def setUp(self):
         self.cartesian = core.CartesianPath()
         self.jointspace = core.JointInterpolationPlanner()
+        self.node = rclcpp.Node("test_mtc")
 
     def create(self, *stages):
         task = core.Task()
+        task.loadRobotModel(self.node)
         task.enableIntrospection()
         for stage in stages:
             task.add(stage)
@@ -140,4 +141,4 @@ def test_propagator(self):
 
 
 if __name__ == "__main__":
-    rostest.rosrun("mtc", "trampoline", TestTrampolines)
+    unittest.main()

core/python/test/test.launch.py

@@ -0,0 +1,51 @@
+import unittest
+
+import launch_testing
+import pytest
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_testing.actions import ReadyToTest
+from launch_testing.util import KeepAliveProc
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+@pytest.mark.launch_test
+def generate_test_description():
+    moveit_config = MoveItConfigsBuilder("moveit_resources_fanuc").to_moveit_configs()
+
+    test_exec = Node(
+        executable=[LaunchConfiguration("exe")],
+        output="screen",
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    return (
+        LaunchDescription(
+            [
+                DeclareLaunchArgument(name="exe"),
+                test_exec,
+                KeepAliveProc(),
+                ReadyToTest(),
+            ]
+        ),
+        {"test_exec": test_exec},
+    )
+
+
+class TestTerminatingProcessStops(unittest.TestCase):
+    def test_gtest_run_complete(self, proc_info, test_exec):
+        proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0)
+
+
+@launch_testing.post_shutdown_test()
+class TaskModelTestAfterShutdown(unittest.TestCase):
+    def test_exit_code(self, proc_info):
+        # Check that all processes in the launch exit with code 0
+        launch_testing.asserts.assertExitCodes(proc_info)

core/python/test/test_mtc.py

@@ -3,13 +3,19 @@
 
 import sys
 import unittest
+import rclcpp
 from geometry_msgs.msg import Pose, PoseStamped, PointStamped, TwistStamped, Vector3Stamped
 from moveit_msgs.msg import RobotState, Constraints, MotionPlanRequest
 from moveit.task_constructor import core, stages
 
 
+def setUpModule():
+    rclcpp.init()
+
+
 class TestPropertyMap(unittest.TestCase):
     def setUp(self):
+        self.node = rclcpp.Node("test_mtc_props")
         self.props = core.PropertyMap()
 
     def _check(self, name, value):
@@ -28,7 +34,7 @@ def test_assign(self):
         self.assertRaises(TypeError, self._check, "request", MotionPlanRequest())
 
     def test_assign_in_reference(self):
-        planner = core.PipelinePlanner()
+        planner = core.PipelinePlanner(self.node)
         props = planner.properties
 
         props["goal_joint_tolerance"] = 3.14
@@ -40,21 +46,15 @@ def test_assign_in_reference(self):
 
         props["planner"] = "planner"
         self.assertEqual(props["planner"], "planner")
-        self.assertEqual(planner.planner, "planner")
 
         props["double"] = 3.14
         a = props
         props["double"] = 2.71
         self.assertEqual(a["double"], 2.71)
 
-        planner.planner = "other"
-        self.assertEqual(props["planner"], "other")
-        self.assertEqual(planner.planner, "other")
-
         del planner
         # We can still access props, because actual destruction of planner is delayed
         self.assertEqual(props["goal_joint_tolerance"], 2.71)
-        self.assertEqual(props["planner"], "other")
 
     def test_iter(self):
         # assign values so we can iterate over them
@@ -116,7 +116,8 @@ def test_allow_collisions(self):
 
 class TestStages(unittest.TestCase):
     def setUp(self):
-        self.planner = core.PipelinePlanner()
+        self.node = rclcpp.Node("test_mtc_stages")
+        self.planner = core.PipelinePlanner(self.node)
 
     def _check(self, stage, name, value):
         self._check_assign(stage, name, value)
@@ -198,8 +199,7 @@ def test_MoveRelative(self):
         stage.setDirection({"joint": 0.1})
 
     def test_Connect(self):
-        planner = core.PipelinePlanner()
-        stage = stages.Connect("connect", [("group1", planner), ("group2", planner)])
+        stage = stages.Connect("connect", [("group1", self.planner), ("group2", self.planner)])
 
     def test_FixCollisionObjects(self):
         stage = stages.FixCollisionObjects("collision")

demo/scripts/alternatives.py

@@ -2,17 +2,19 @@
 # -*- coding: utf-8 -*-
 
 from moveit.task_constructor import core, stages
-from py_binding_tools import roscpp_init
+import rclcpp
 import time
 
-roscpp_init("mtc_tutorial")
+rclcpp.init()
+node = rclcpp.Node("mtc_tutorial")
 
 # Use the joint interpolation planner
 jointPlanner = core.JointInterpolationPlanner()
 
 # Create a task
 task = core.Task()
 task.name = "alternatives"
+task.loadRobotModel(node)
 
 # Start from current robot state
 currentState = stages.CurrentState("current state")

demo/scripts/cartesian.py

@@ -1,15 +1,17 @@
 #! /usr/bin/env python3
 # -*- coding: utf-8 -*-
 
+from math import pi
+import time
+
 from std_msgs.msg import Header
 from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3
 from moveit.task_constructor import core, stages
-from math import pi
-import time
 
-from py_binding_tools import roscpp_init
+import rclcpp
 
-roscpp_init("mtc_tutorial")
+rclcpp.init()
+node = rclcpp.Node("mtc_tutorial")
 
 # [cartesianTut1]
 group = "panda_arm"
@@ -24,6 +26,7 @@
 # [cartesianTut3]
 task = core.Task()
 task.name = "cartesian"
+task.loadRobotModel(node)
 
 # start from current robot state
 task.add(stages.CurrentState("current state"))
@@ -34,23 +37,25 @@
 move = stages.MoveRelative("x +0.2", cartesian)
 move.group = group
 header = Header(frame_id="world")
-move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
+move.setDirection(Vector3Stamped(header=header, vector=Vector3(x=0.2, y=0.0, z=0.0)))
 task.add(move)
 # [initAndConfigMoveRelative]
 
 # [cartesianTut4]
 # move along y
 move = stages.MoveRelative("y -0.3", cartesian)
 move.group = group
-move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0)))
+move.setDirection(Vector3Stamped(header=header, vector=Vector3(x=0.0, y=-0.3, z=0.0)))
 task.add(move)
 # [cartesianTut4]
 
 # [cartesianTut5]
 # rotate about z
 move = stages.MoveRelative("rz +45°", cartesian)
 move.group = group
-move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0))))
+move.setDirection(
+    TwistStamped(header=header, twist=Twist(angular=Vector3(x=0.0, y=0.0, z=pi / 4.0)))
+)
 task.add(move)
 # [cartesianTut5]
 

demo/scripts/compute_ik.py

@@ -6,22 +6,24 @@
 from std_msgs.msg import Header
 import time
 
-from py_binding_tools import roscpp_init
+import rclcpp
 
-roscpp_init("mtc_tutorial")
+rclcpp.init()
+node = rclcpp.Node("mtc_tutorial")
 
 # Specify the planning group
 group = "panda_arm"
 
 # Create a task
 task = core.Task()
 task.name = "compute IK"
+task.loadRobotModel(node)
 
 # Add a stage to retrieve the current state
 task.add(stages.CurrentState("current state"))
 
 # Add a planning stage connecting the generator stages
-planner = core.PipelinePlanner()  # create default planning pipeline
+planner = core.PipelinePlanner(node)  # create default planning pipeline
 task.add(stages.Connect("connect", [(group, planner)]))  # operate on group
 del planner  # Delete PipelinePlanner when not explicitly needed anymore