From 747bb29c7a3868b109ec95c0d337cfdf9df8d101 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 26 May 2024 07:42:01 +0200 Subject: [PATCH 1/5] CI: add jazzy build --- .github/workflows/ci.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6134243c5..474751cca 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -24,13 +24,13 @@ 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" @@ -38,7 +38,7 @@ jobs: 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 From f79c6c537ef50c4fda8e75a2904583477164e650 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 26 May 2024 20:45:23 +0200 Subject: [PATCH 2/5] Connect: Relax validity check of reached end state (#542) Looks like Rolling's MoveIt uses a more relaxed goal constraint threshold than Humble. For this reason, all end states reached by Connect solutions of pick+place demo are rejected. This commit relaxes the max_distance threshold of Connect accordingly. --- core/src/stages/connect.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/src/stages/connect.cpp b/core/src/stages/connect.cpp index 823750dd1..8e17a58c9 100644 --- a/core/src/stages/connect.cpp +++ b/core/src/stages/connect.cpp @@ -57,7 +57,7 @@ Connect::Connect(const std::string& name, const GroupPlannerVector& planners) : auto& p = properties(); p.declare("merge_mode", WAYPOINTS, "merge mode"); - p.declare("max_distance", 1e-4, "maximally accepted distance between end and goal sate"); + p.declare("max_distance", 1e-2, "maximally accepted distance between end and goal sate"); p.declare("path_constraints", moveit_msgs::msg::Constraints(), "constraints to maintain during trajectory"); properties().declare("merge_time_parameterization", From a9ddbe19986ea2a5c8ad1459e424e66f67f4dfd4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Jafar=20Uru=C3=A7?= Date: Tue, 28 May 2024 06:26:10 +0100 Subject: [PATCH 3/5] Update authors and maintainers (#425) --- core/package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/core/package.xml b/core/package.xml index 3cb44239b..b10c3f67f 100644 --- a/core/package.xml +++ b/core/package.xml @@ -33,7 +33,7 @@ launch_testing_ament_cmake - + moveit_resources_fanuc_moveit_config moveit_planners From bb047c894cd2a395a672abdd47ec76d709d1cb1e Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 May 2024 09:04:42 +0200 Subject: [PATCH 4/5] Cleanup unit tests - Unify move_to.launch.py and test_task_model.launch.py - Rename them to test.launch.py as they are independent of the executable - Move Node creation to the fixture constructor - Replace Task::setRobotModel(loadModel()) with Task::loadRobotModel() --- core/test/CMakeLists.txt | 4 ++-- core/test/models.cpp | 5 ---- core/test/models.h | 3 --- .../{move_to.launch.py => test.launch.py} | 0 core/test/test_move_relative.cpp | 6 ++--- core/test/test_move_to.cpp | 12 ++++++---- .../motion_planning_tasks/test/CMakeLists.txt | 4 ++-- ...st_task_model.launch.py => test.launch.py} | 23 +++++++------------ 8 files changed, 22 insertions(+), 35 deletions(-) rename core/test/{move_to.launch.py => test.launch.py} (100%) rename visualization/motion_planning_tasks/test/{test_task_model.launch.py => test.launch.py} (60%) diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index afa8e24c8..5e4ce4b89 100644 --- a/core/test/CMakeLists.txt +++ b/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 diff --git a/core/test/models.cpp b/core/test/models.cpp index 3abe5ab12..8ee16f792 100644 --- a/core/test/models.cpp +++ b/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(); -} diff --git a/core/test/models.h b/core/test/models.h index 760235222..43fbd850a 100644 --- a/core/test/models.h +++ b/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); diff --git a/core/test/move_to.launch.py b/core/test/test.launch.py similarity index 100% rename from core/test/move_to.launch.py rename to core/test/test.launch.py diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 5a36457f9..482ee4e24 100644 --- a/core/test/test_move_relative.cpp +++ b/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(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); - scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); + scene->getCurrentStateNonConst().setToDefaultValues(group, "ready"); t.add(std::make_unique("start", scene)); auto move_relative = std::make_unique("move", std::make_shared()); diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp index 7bfc756bf..994b23602 100644 --- a/core/test/test_move_to.cpp +++ b/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(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); - scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), - "extended"); + scene->getCurrentStateNonConst().setToDefaultValues(group, "extended"); t.add(std::make_unique("start", scene)); auto move = std::make_unique("move", std::make_shared()); 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(t.getRobotModel()); scene->getCurrentStateNonConst().setToDefaultValues(); scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready"); diff --git a/visualization/motion_planning_tasks/test/CMakeLists.txt b/visualization/motion_planning_tasks/test/CMakeLists.txt index 97667690e..806a01ce0 100644 --- a/visualization/motion_planning_tasks/test/CMakeLists.txt +++ b/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:=$") + add_launch_test(test.launch.py + ARGS "test_binary:=$") endif() diff --git a/visualization/motion_planning_tasks/test/test_task_model.launch.py b/visualization/motion_planning_tasks/test/test.launch.py similarity index 60% rename from visualization/motion_planning_tasks/test/test_task_model.launch.py rename to visualization/motion_planning_tasks/test/test.launch.py index 40cf5cbca..3afb47b02 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.launch.py +++ b/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() From edf2605a1d165f0e8965cd1620ca32dabbe081a5 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Wed, 29 May 2024 11:33:40 +0200 Subject: [PATCH 5/5] TEMP: Workaround for unittests failing during shutdown Using quick_exit instead of return/exit avoids calling all cleanup functions, which fail due to a bug in rmw_fastrtps_cpp. --- core/test/test_move_relative.cpp | 3 ++- visualization/motion_planning_tasks/test/test_task_model.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/core/test/test_move_relative.cpp b/core/test/test_move_relative.cpp index 482ee4e24..04133fb25 100644 --- a/core/test/test_move_relative.cpp +++ b/core/test/test_move_relative.cpp @@ -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()); } diff --git a/visualization/motion_planning_tasks/test/test_task_model.cpp b/visualization/motion_planning_tasks/test/test_task_model.cpp index 2e0721609..9bd9d3305 100644 --- a/visualization/motion_planning_tasks/test/test_task_model.cpp +++ b/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()); }