Set controller names in Demo pick and place task

.github/workflows/ci.yaml

@@ -24,21 +24,21 @@ jobs:
             TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Debug -DCMAKE_CXX_FLAGS="--coverage"
           - IMAGE: rolling-source
             CLANG_TIDY: pedantic
-          - IMAGE: rolling-source
+          - IMAGE: jazzy-source
             NAME: asan
             # Add fast_unwind_on_malloc=0 to fix stacktraces being too short or do not make sense
             # see https://github.com/google/sanitizers/wiki/AddressSanitizer
             # Disable alloc/dealloc mismatch warnings: https://github.com/ros2/rclcpp/pull/1324
             DOCKER_RUN_OPTS: >-
-              -e PRELOAD=libasan.so.5
+              -e PRELOAD=libasan.so.8
               -e LSAN_OPTIONS="suppressions=$PWD/.github/workflows/lsan.suppressions,fast_unwind_on_malloc=0"
               -e ASAN_OPTIONS="new_delete_type_mismatch=0,alloc_dealloc_mismatch=0"
             TARGET_CMAKE_ARGS: -DCMAKE_CXX_FLAGS="-fsanitize=address -fno-omit-frame-pointer -O1 -g"
 
     env:
       CLANG_TIDY_ARGS: -quiet -export-fixes ${{ github.workspace }}/.work/clang-tidy-fixes.yaml
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
-      UNDERLAY: /root/ws_moveit/install
+      UNDERLAY: ${{ endsWith(matrix.env.IMAGE, '-source') && '/root/ws_moveit/install' || ''}}
       # TODO: Port to ROS2
       # DOWNSTREAM_WORKSPACE: "github:ubi-agni/mtc_demos#master github:TAMS-Group/mtc_pour#master"
       TARGET_CMAKE_ARGS: -DCMAKE_BUILD_TYPE=Release

core/package.xml

@@ -33,7 +33,7 @@
 	<!-- test_depend>launch_testing</test_depend -->
 	<test_depend>launch_testing_ament_cmake</test_depend>
 	<!-- test_depend>launch_testing_ros</test_depend -->
-	<!-- test_depend>moveit_resources_fanuc_moveit_config</test_depend -->
+	<test_depend>moveit_resources_fanuc_moveit_config</test_depend>
 	<test_depend>moveit_planners</test_depend>
 
 	<export>

core/src/stages/connect.cpp

@@ -57,7 +57,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) :
 
 	auto& p = properties();
 	p.declare<MergeMode>("merge_mode", WAYPOINTS, "merge mode");
-	p.declare<double>("max_distance", 1e-4, "maximally accepted distance between end and goal sate");
+	p.declare<double>("max_distance", 1e-2, "maximally accepted distance between end and goal sate");
 	p.declare<moveit_msgs::msg::Constraints>("path_constraints", moveit_msgs::msg::Constraints(),
 	                                         "constraints to maintain during trajectory");
 	properties().declare<TimeParameterizationPtr>("merge_time_parameterization",

core/test/CMakeLists.txt

@@ -54,8 +54,8 @@ if (BUILD_TESTING)
 	mtc_add_gmock(test_cost_queue.cpp)
 	mtc_add_gmock(test_interface_state.cpp)
 
-	mtc_add_gtest(test_move_to.cpp move_to.launch.py)
-	mtc_add_gtest(test_move_relative.cpp move_to.launch.py)
+	mtc_add_gtest(test_move_to.cpp test.launch.py)
+	mtc_add_gtest(test_move_relative.cpp test.launch.py)
 	mtc_add_gtest(test_pipeline_planner.cpp)
 
 	# building these integration tests works without moveit config packages

core/test/models.cpp

@@ -17,8 +17,3 @@ RobotModelPtr getModel() {
 	builder.addEndEffector("eef", "link2", "group", "eef_group");
 	return builder.build();
 }
-
-moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node) {
-	robot_model_loader::RobotModelLoader loader(node);
-	return loader.getModel();
-}

core/test/models.h

@@ -11,6 +11,3 @@ MOVEIT_CLASS_FORWARD(RobotModel);
 
 // get a hard-coded model
 moveit::core::RobotModelPtr getModel();
-
-// load a model from robot_description
-moveit::core::RobotModelPtr loadModel(const rclcpp::Node::SharedPtr& node);

core/test/test_move_relative.cpp

@@ -26,18 +26,18 @@ struct PandaMoveRelative : public testing::Test
 	Task t;
 	stages::MoveRelative* move;
 	PlanningScenePtr scene;
-	rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_relative");
+	rclcpp::Node::SharedPtr node;
 
 	const JointModelGroup* group;
 
 	PandaMoveRelative() {
-		t.setRobotModel(loadModel(node));
+		t.loadRobotModel(rclcpp::Node::make_shared("panda_move_relative"));
 
 		group = t.getRobotModel()->getJointModelGroup("panda_arm");
 
 		scene = std::make_shared<PlanningScene>(t.getRobotModel());
 		scene->getCurrentStateNonConst().setToDefaultValues();
-		scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
+		scene->getCurrentStateNonConst().setToDefaultValues(group, "ready");
 		t.add(std::make_unique<stages::FixedState>("start", scene));
 
 		auto move_relative = std::make_unique<stages::MoveRelative>("move", std::make_shared<solvers::CartesianPath>());
@@ -133,5 +133,6 @@ int main(int argc, char** argv) {
 	testing::InitGoogleTest(&argc, argv);
 	rclcpp::init(argc, argv);
 
-	return RUN_ALL_TESTS();
+	// Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw
+	quick_exit(RUN_ALL_TESTS());
 }

core/test/test_move_to.cpp

@@ -34,20 +34,22 @@ struct PandaMoveTo : public testing::Test
 	Task t;
 	stages::MoveTo* move_to;
 	PlanningScenePtr scene;
-	rclcpp::Node::SharedPtr node = rclcpp::Node::make_shared("panda_move_to");
+	rclcpp::Node::SharedPtr node;
 
 	PandaMoveTo() {
+		node = rclcpp::Node::make_shared("panda_move_to");
 		t.loadRobotModel(node);
 
+		auto group = t.getRobotModel()->getJointModelGroup("panda_arm");
+
 		scene = std::make_shared<PlanningScene>(t.getRobotModel());
 		scene->getCurrentStateNonConst().setToDefaultValues();
-		scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"),
-		                                                    "extended");
+		scene->getCurrentStateNonConst().setToDefaultValues(group, "extended");
 		t.add(std::make_unique<stages::FixedState>("start", scene));
 
 		auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
 		move_to = move.get();
-		move_to->setGroup("panda_arm");
+		move_to->setGroup(group->getName());
 		t.add(std::move(move));
 	}
 };
@@ -162,7 +164,7 @@ TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
 // will strongly deviate from the joint-space goal.
 TEST(Panda, connectCartesianBranchesFails) {
 	Task t;
-	t.setRobotModel(loadModel(rclcpp::Node::make_shared("panda_move_to")));
+	t.loadRobotModel(rclcpp::Node::make_shared("panda_move_to"));
 	auto scene = std::make_shared<PlanningScene>(t.getRobotModel());
 	scene->getCurrentStateNonConst().setToDefaultValues();
 	scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");

visualization/motion_planning_tasks/test/CMakeLists.txt

@@ -19,6 +19,6 @@ if (BUILD_TESTING)
 	ament_add_gtest_executable(${PROJECT_NAME}-test-task_model test_task_model.cpp)
 	target_link_libraries(${PROJECT_NAME}-test-task_model
 		motion_planning_tasks_rviz_plugin)
-	add_launch_test(test_task_model.launch.py
-		ARGS "test_binary_dir:=$<TARGET_FILE_DIR:${PROJECT_NAME}-test-task_model>")
+	add_launch_test(test.launch.py
+		ARGS "test_binary:=$<TARGET_FILE:${PROJECT_NAME}-test-task_model>")
 endif()

visualization/motion_planning_tasks/test/test_task_model.launch.py → visualization/motion_planning_tasks/test/test.launch.py

@@ -13,36 +13,29 @@
 
 @pytest.mark.launch_test
 def generate_test_description():
-    test_task_model = GTest(
-        path=[
-            PathJoinSubstitution(
-                [
-                    LaunchConfiguration("test_binary_dir"),
-                    "moveit_task_constructor_visualization-test-task_model",
-                ]
-            )
-        ],
+    test_exec = GTest(
+        path=[LaunchConfiguration("test_binary")],
         output="screen",
     )
     return (
         LaunchDescription(
             [
                 DeclareLaunchArgument(
-                    name="test_binary_dir",
-                    description="Binary directory of package containing test executables",
+                    name="test_binary",
+                    description="Test executable",
                 ),
-                test_task_model,
+                test_exec,
                 KeepAliveProc(),
                 ReadyToTest(),
             ]
         ),
-        {"test_task_model": test_task_model},
+        {"test_exec": test_exec},
     )
 
 
 class TestTerminatingProcessStops(unittest.TestCase):
-    def test_gtest_run_complete(self, proc_info, test_task_model):
-        proc_info.assertWaitForShutdown(process=test_task_model, timeout=4000.0)
+    def test_gtest_run_complete(self, proc_info, test_exec):
+        proc_info.assertWaitForShutdown(process=test_exec, timeout=4000.0)
 
 
 @launch_testing.post_shutdown_test()

visualization/motion_planning_tasks/test/test_task_model.cpp

@@ -239,5 +239,6 @@ int main(int argc, char** argv) {
 		auto test_result = RUN_ALL_TESTS();
 		app.exit(test_result);
 	});
-	return app.exec();
+	// Using quick_exit to avoid calling cleanup functions, which cause a segfault in rmw
+	quick_exit(app.exec());
 }