Merge branch 'moveit:main' into feature/free-path-generator

Author: AimanHaidar

moveit_kinematics/test/test_kinematics_plugin.cpp

@@ -39,6 +39,7 @@
 #include <memory>
 #include <functional>
 #include <pluginlib/class_loader.hpp>
+#include <rclcpp/executors/single_threaded_executor.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <tf2_eigen/tf2_eigen.hpp>
 
@@ -403,7 +404,9 @@ TEST_F(KinematicsTest, randomWalkIK)
     auto pub = node_->create_publisher<moveit_msgs::msg::DisplayTrajectory>("display_random_walk", 1);
     traj.getRobotTrajectoryMsg(msg.trajectory[0]);
     pub->publish(msg);
-    rclcpp::spin_some(node_);
+    rclcpp::executors::SingleThreadedExecutor executor;
+    executor.add_node(node_);
+    executor.spin_some();
   }
 }
 

moveit_planners/ompl/CMakeLists.txt

@@ -27,7 +27,7 @@ if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
 endif()
 
 include_directories(ompl_interface/include)
-include_directories(SYSTEM ${Boost_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS})
+include_directories(SYSTEM ${Boost_INCLUDE_DIRS})
 
 add_subdirectory(ompl_interface)
 

moveit_planners/ompl/ompl_interface/CMakeLists.txt

@@ -24,28 +24,23 @@ set_target_properties(moveit_ompl_interface
 
 find_package(OpenMP REQUIRED)
 
-# Used to link in ODE, an OMPL dependency, on macOS
-if(APPLE)
-  target_link_directories(moveit_ompl_interface PUBLIC ${OMPL_LIBRARY_DIRS})
-endif()
-
 ament_target_dependencies(
   moveit_ompl_interface
+  PUBLIC
   moveit_core
   moveit_msgs
   moveit_ros_planning
   rclcpp
   pluginlib
   tf2_eigen
   tf2_ros
-  OMPL
   Boost)
 set_target_properties(
   moveit_ompl_interface PROPERTIES COMPILE_FLAGS
                                    "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
 set_target_properties(moveit_ompl_interface PROPERTIES LINK_FLAGS
                                                        "${OpenMP_CXX_FLAGS}")
-
+target_link_libraries(moveit_ompl_interface PUBLIC ompl::ompl)
 add_executable(moveit_generate_state_database
                scripts/generate_state_database.cpp)
 target_link_libraries(moveit_generate_state_database moveit_ompl_interface)
@@ -64,9 +59,9 @@ ament_target_dependencies(
   rclcpp
   pluginlib
   tf2_ros
-  OMPL
   Boost)
-target_link_libraries(moveit_ompl_planner_plugin moveit_ompl_interface)
+target_link_libraries(moveit_ompl_planner_plugin moveit_ompl_interface
+                      ompl::ompl)
 
 install(TARGETS moveit_generate_state_database
         RUNTIME DESTINATION lib/${PROJECT_NAME})
@@ -77,23 +72,24 @@ if(BUILD_TESTING)
   find_package(Eigen3 REQUIRED)
 
   ament_add_gtest(test_state_space test/test_state_space.cpp)
-  ament_target_dependencies(test_state_space moveit_core OMPL Boost Eigen3)
-  target_link_libraries(test_state_space moveit_ompl_interface)
+  ament_target_dependencies(test_state_space moveit_core Boost Eigen3)
+  target_link_libraries(test_state_space moveit_ompl_interface ompl::ompl)
   set_target_properties(test_state_space PROPERTIES LINK_FLAGS
                                                     "${OpenMP_CXX_FLAGS}")
 
   ament_add_gtest(test_state_validity_checker
                   test/test_state_validity_checker.cpp)
-  ament_target_dependencies(test_state_validity_checker moveit_core OMPL Boost
+  ament_target_dependencies(test_state_validity_checker moveit_core Boost
                             Eigen3)
-  target_link_libraries(test_state_validity_checker moveit_ompl_interface)
+  target_link_libraries(test_state_validity_checker moveit_ompl_interface
+                        ompl::ompl)
   set_target_properties(test_state_validity_checker
                         PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
 
   ament_add_gtest(test_planning_context_manager
                   test/test_planning_context_manager.cpp)
   ament_target_dependencies(test_planning_context_manager moveit_core tf2_eigen
-                            OMPL Boost Eigen3)
+                            Boost Eigen3)
   target_link_libraries(test_planning_context_manager moveit_ompl_interface)
 
   # Disabling flaky test TODO (vatanaksoytezer): Uncomment once this is fixed
@@ -104,25 +100,25 @@ if(BUILD_TESTING)
   ament_add_gtest(test_constrained_planning_state_space
                   test/test_constrained_planning_state_space.cpp)
   ament_target_dependencies(test_constrained_planning_state_space moveit_core
-                            OMPL Boost Eigen3)
+                            Boost Eigen3)
   target_link_libraries(test_constrained_planning_state_space
-                        moveit_ompl_interface)
+                        moveit_ompl_interface ompl::ompl)
   set_target_properties(test_constrained_planning_state_space
                         PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
 
   ament_add_gtest(test_constrained_state_validity_checker
                   test/test_constrained_state_validity_checker.cpp)
   ament_target_dependencies(test_constrained_state_validity_checker moveit_core
-                            OMPL Boost Eigen3)
+                            Boost Eigen3)
   target_link_libraries(test_constrained_state_validity_checker
-                        moveit_ompl_interface)
+                        moveit_ompl_interface ompl::ompl)
   set_target_properties(test_constrained_state_validity_checker
                         PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
 
   ament_add_gtest(test_threadsafe_state_storage
                   test/test_threadsafe_state_storage.cpp)
-  ament_target_dependencies(test_threadsafe_state_storage moveit_core OMPL
-                            Boost Eigen3)
+  ament_target_dependencies(test_threadsafe_state_storage moveit_core Boost
+                            Eigen3)
   target_link_libraries(test_threadsafe_state_storage moveit_ompl_interface)
   set_target_properties(test_threadsafe_state_storage
                         PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")

moveit_ros/benchmarks/src/BenchmarkExecutor.cpp

@@ -43,6 +43,7 @@
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <moveit/utils/logger.hpp>
 
+#include <regex>
 #include <boost/regex.hpp>
 
 #if __has_include(<boost/timer/progress_display.hpp>)

moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp

@@ -40,6 +40,7 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/version.h>
 #include <rclcpp_action/rclcpp_action.hpp>
 
 #include <pluginlib/class_loader.hpp>
@@ -58,8 +59,15 @@
 #include <moveit/local_planner/local_constraint_solver_interface.hpp>
 #include <moveit/local_planner/trajectory_operator_interface.hpp>
 
+// For Rolling, Kilted, and newer
+#if RCLCPP_VERSION_GTE(29, 6, 0)
+#include <tf2_ros/buffer.hpp>
+#include <tf2_ros/transform_listener.hpp>
+// For Jazzy and older
+#else
 #include <tf2_ros/buffer.h>
 #include <tf2_ros/transform_listener.h>
+#endif
 
 // Forward declaration of parameter class allows users to implement custom parameters
 namespace local_planner_parameters

moveit_ros/hybrid_planning/test/test_basic_integration.cpp

@@ -41,8 +41,16 @@
 #include <moveit/planning_scene_monitor/planning_scene_monitor.hpp>
 #include <moveit/robot_state/conversions.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/version.h>
 #include <rclcpp_action/rclcpp_action.hpp>
+
+// For Rolling, Kilted, and newer
+#if RCLCPP_VERSION_GTE(29, 6, 0)
 #include <tf2_ros/buffer.hpp>
+// For Jazzy and older
+#else
+#include <tf2_ros/buffer.h>
+#endif
 
 #include <moveit_msgs/action/hybrid_planner.hpp>
 #include <moveit_msgs/msg/display_robot_state.hpp>

moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp

@@ -35,10 +35,19 @@
 /* Author: Jonas Tietz */
 
 #include "tf_publisher_capability.hpp"
+
+#include <rclcpp/version.h>
+
 #include <moveit/moveit_cpp/moveit_cpp.hpp>
 #include <moveit/utils/message_checks.hpp>
 #include <moveit/move_group/capability_names.hpp>
+// For Rolling, Kilted, and newer
+#if RCLCPP_VERSION_GTE(29, 6, 0)
+#include <tf2_ros/transform_broadcaster.hpp>
+// For Jazzy and older
+#else
 #include <tf2_ros/transform_broadcaster.h>
+#endif
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <moveit/robot_state/robot_state.hpp>
 #include <moveit/robot_state/attached_body.hpp>
@@ -76,7 +85,14 @@ void publishSubframes(tf2_ros::TransformBroadcaster& broadcaster, const moveit::
 
 void TfPublisher::publishPlanningSceneFrames()
 {
+// For Rolling, L-turtle, and newer
+#if RCLCPP_VERSION_GTE(30, 0, 0)
+  tf2_ros::TransformBroadcaster broadcaster(tf2_ros::TransformBroadcaster::RequiredInterfaces{
+      context_->moveit_cpp_->getNode()->get_node_parameters_interface(),
+      context_->moveit_cpp_->getNode()->get_node_topics_interface() });
+#else
   tf2_ros::TransformBroadcaster broadcaster(context_->moveit_cpp_->getNode());
+#endif
   geometry_msgs::msg::TransformStamped transform;
   rclcpp::Rate rate(rate_);
 

moveit_ros/move_group/src/move_group.cpp

@@ -34,9 +34,19 @@
 
 /* Author: Ioan Sucan */
 
+#include <rclcpp/version.h>
+
 #include <moveit/moveit_cpp/moveit_cpp.hpp>
 #include <moveit/planning_scene_monitor/planning_scene_monitor.hpp>
+// For Rolling, L-turtle, and newer
+
+// For Rolling, Kilted, and newer
+#if RCLCPP_VERSION_GTE(29, 6, 0)
+#include <tf2_ros/transform_listener.hpp>
+// For Jazzy and older
+#else
 #include <tf2_ros/transform_listener.h>
+#endif
 #include <moveit/move_group/move_group_capability.hpp>
 #include <moveit/trajectory_execution_manager/trajectory_execution_manager.hpp>
 #include <boost/tokenizer.hpp>

moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp

@@ -44,8 +44,15 @@
 #include <moveit_servo/utils/common.hpp>
 #include <mutex>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/version.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// For Rolling, Kilted, and newer
+#if RCLCPP_VERSION_GTE(29, 6, 0)
+#include <tf2_ros/transform_listener.hpp>
+// For Jazzy and older
+#else
 #include <tf2_ros/transform_listener.h>
+#endif
 #include <moveit/utils/logger.hpp>
 
 using namespace moveit_servo;

moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp

@@ -43,8 +43,15 @@
 #include <moveit_servo/servo.hpp>
 #include <moveit_servo/utils/common.hpp>
 #include <rclcpp/rclcpp.hpp>
+#include <rclcpp/version.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// For Rolling, Kilted, and newer
+#if RCLCPP_VERSION_GTE(29, 6, 0)
+#include <tf2_ros/transform_listener.hpp>
+// For Jazzy and older
+#else
 #include <tf2_ros/transform_listener.h>
+#endif
 #include <moveit/utils/logger.hpp>
 
 using namespace moveit_servo;