From 3c20a3684acf2b78f69d71b789718b1d4af27b6f Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 7 Apr 2022 18:10:24 +0300 Subject: [PATCH 1/4] Revert OMPL parameter loading --- .../ompl/ompl_interface/src/ompl_interface.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index f18477e3c3..b45b744ef0 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -197,7 +197,14 @@ void OMPLInterface::loadPlannerConfigurations() << rclcpp::to_string(parameter.get_type()) << "]"); continue; } - specific_group_params[name] = parameter.value_to_string(); + if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_STRING) + specific_group_params[name] = parameter.as_string(); + else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_DOUBLE) + specific_group_params[name] = moveit::core::toString(parameter.as_double()); + else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_INTEGER) + specific_group_params[name] = std::to_string(parameter.as_int()); + else if (parameter.get_type() == rclcpp::ParameterType::PARAMETER_BOOL) + specific_group_params[name] = std::to_string(parameter.as_bool()); } } From 78d7e5c32d4635a60abc14f6ef6750374a6f75a8 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 7 Apr 2022 18:10:53 +0300 Subject: [PATCH 2/4] Comment failing rdf integration test --- moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp index 8e665c1cdb..e9bc3414ea 100644 --- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp +++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp @@ -66,6 +66,9 @@ TEST(RDFIntegration, topic_based) EXPECT_EQ("gonzo", loader.getSRDF()->getName()); } +// Failing in Jammy disabling temporarily (TODO (vatanaksoytezer): Fix this and enable again) +// See https://github.com/ros-planning/moveit2/issues/1156 +/* TEST(RDFIntegration, executor) { // RDFLoader should successfully load URDF and SRDF strings from a ROS topic when the node that is @@ -89,6 +92,7 @@ TEST(RDFIntegration, executor) ASSERT_NE(nullptr, loader.getSRDF()); EXPECT_EQ("gonzo", loader.getSRDF()->getName()); } +*/ TEST(RDFIntegration, xacro_test) { From 7c0b270f5e877b14cc67eeb1311f256f3fd064f0 Mon Sep 17 00:00:00 2001 From: Vatan Aksoy Tezer Date: Thu, 7 Apr 2022 18:11:39 +0300 Subject: [PATCH 3/4] Rename panda controllers --- .../moveit_servo/launch/pose_tracking_example.launch.py | 2 +- moveit_ros/moveit_servo/launch/servo_example.launch.py | 2 +- .../moveit_servo/test/launch/servo_launch_test_common.py | 2 +- .../moveit_servo/test/launch/test_servo_pose_tracking.test.py | 2 +- .../test/launch/move_group_launch_test_common.py | 4 ++-- 5 files changed, 6 insertions(+), 6 deletions(-) diff --git a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py index 69fed7eb75..29143be647 100644 --- a/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py +++ b/moveit_ros/moveit_servo/launch/pose_tracking_example.launch.py @@ -69,7 +69,7 @@ def generate_launch_description(): ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros2_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", diff --git a/moveit_ros/moveit_servo/launch/servo_example.launch.py b/moveit_ros/moveit_servo/launch/servo_example.launch.py index 4bccca9ec1..1e699cf2a6 100644 --- a/moveit_ros/moveit_servo/launch/servo_example.launch.py +++ b/moveit_ros/moveit_servo/launch/servo_example.launch.py @@ -70,7 +70,7 @@ def generate_launch_description(): ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros2_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", diff --git a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py index ee5930d6fb..493c208140 100644 --- a/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py +++ b/moveit_ros/moveit_servo/test/launch/servo_launch_test_common.py @@ -47,7 +47,7 @@ def generate_servo_test_description( ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros2_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", diff --git a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py index cf151faae3..ce9e48bf42 100644 --- a/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py +++ b/moveit_ros/moveit_servo/test/launch/test_servo_pose_tracking.test.py @@ -34,7 +34,7 @@ def generate_servo_test_description(*args, gtest_name: SomeSubstitutionsType): ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros2_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", diff --git a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py index 860eeff78d..b7fbf86cda 100644 --- a/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py +++ b/moveit_ros/planning_interface/test/launch/move_group_launch_test_common.py @@ -14,7 +14,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .trajectory_execution(file_path="config/panda_gripper_controllers.yaml") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") .to_moveit_configs() ) @@ -55,7 +55,7 @@ def generate_move_group_test_description(*args, gtest_name: SomeSubstitutionsTyp ros2_controllers_path = os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), "config", - "panda_ros2_controllers.yaml", + "ros2_controllers.yaml", ) ros2_control_node = Node( package="controller_manager", From 0804c0d0b4820505beedca7f343f307dd79f083b Mon Sep 17 00:00:00 2001 From: JafarAbdi Date: Thu, 7 Apr 2022 16:59:34 +0000 Subject: [PATCH 4/4] Fix failing test --- .../rdf_loader/test/test_rdf_integration.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp index e9bc3414ea..9d813402b5 100644 --- a/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp +++ b/moveit_ros/planning/rdf_loader/test/test_rdf_integration.cpp @@ -66,9 +66,6 @@ TEST(RDFIntegration, topic_based) EXPECT_EQ("gonzo", loader.getSRDF()->getName()); } -// Failing in Jammy disabling temporarily (TODO (vatanaksoytezer): Fix this and enable again) -// See https://github.com/ros-planning/moveit2/issues/1156 -/* TEST(RDFIntegration, executor) { // RDFLoader should successfully load URDF and SRDF strings from a ROS topic when the node that is @@ -77,11 +74,9 @@ TEST(RDFIntegration, executor) rclcpp::Node::SharedPtr node = std::make_shared("executor"); // Create a thread to spin an Executor. - std::thread([node]() { - rclcpp::executors::SingleThreadedExecutor executor; - executor.add_node(node); - executor.spin(); - }).detach(); + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node); + std::thread spinning_thread([&executor] { executor.spin(); }); // WHEN the RDFLoader is created rdf_loader::RDFLoader loader(node, "topic_description"); @@ -91,8 +86,9 @@ TEST(RDFIntegration, executor) EXPECT_EQ("gonzo", loader.getURDF()->name_); ASSERT_NE(nullptr, loader.getSRDF()); EXPECT_EQ("gonzo", loader.getSRDF()->getName()); + executor.cancel(); + spinning_thread.join(); } -*/ TEST(RDFIntegration, xacro_test) {