diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index 54a082b329..fd39bab192 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -39,6 +39,7 @@ #include #include #include +#include #include #include @@ -403,7 +404,9 @@ TEST_F(KinematicsTest, randomWalkIK) auto pub = node_->create_publisher("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(); } } diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp index 164cd231a7..0687c05240 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.hpp @@ -40,6 +40,7 @@ #pragma once #include +#include #include #include @@ -58,8 +59,15 @@ #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +#include +// For Jazzy and older +#else #include #include +#endif // Forward declaration of parameter class allows users to implement custom parameters namespace local_planner_parameters diff --git a/moveit_ros/hybrid_planning/test/test_basic_integration.cpp b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp index 97399f272a..859d1792b0 100644 --- a/moveit_ros/hybrid_planning/test/test_basic_integration.cpp +++ b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp @@ -41,8 +41,16 @@ #include #include #include +#include #include + +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) #include +// For Jazzy and older +#else +#include +#endif #include #include diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index f6035e52ca..d17693c548 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -35,10 +35,19 @@ /* Author: Jonas Tietz */ #include "tf_publisher_capability.hpp" + +#include + #include #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include #include @@ -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_); diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 2389100756..e071d7648c 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -34,9 +34,19 @@ /* Author: Ioan Sucan */ +#include + #include #include +// For Rolling, L-turtle, and newer + +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include #include diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp index 4c713fd8b6..f7e96bc31b 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp @@ -44,8 +44,15 @@ #include #include #include +#include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include using namespace moveit_servo; diff --git a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp index 8c92fa4cfc..c2f4f8d236 100644 --- a/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp +++ b/moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp @@ -43,8 +43,15 @@ #include #include #include +#include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include using namespace moveit_servo; diff --git a/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp index 9c5d0805a6..b7e45bc227 100644 --- a/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp +++ b/moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp @@ -42,6 +42,7 @@ #include #include #include +#include #include #include #include @@ -188,9 +189,11 @@ int main(int argc, char** argv) void KeyboardServo::spin() { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(nh_); while (rclcpp::ok()) { - rclcpp::spin_some(nh_); + executor.spin_some(); } } diff --git a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp index 88a8f91665..667c64d332 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/servo.hpp @@ -41,6 +41,9 @@ #pragma once +#include +#include + #include #include #include @@ -51,9 +54,14 @@ #include #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include -#include #include namespace moveit_servo diff --git a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp index 7f58505143..d30741a6cb 100644 --- a/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp +++ b/moveit_ros/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.hpp @@ -42,7 +42,14 @@ #include #include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index 7cd0dbb9bc..4976eb473e 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -34,10 +34,18 @@ /* Author: Jon Binney, Ioan Sucan */ +#include + #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include #include @@ -99,7 +107,17 @@ int main(int argc, char** argv) node->create_publisher("octomap_binary", RMW_QOS_POLICY_HISTORY_KEEP_LAST); auto clock_ptr = std::make_shared(RCL_ROS_TIME); std::shared_ptr buffer = std::make_shared(clock_ptr, tf2::durationFromSec(5.0)); +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + tf2_ros::TransformListener listener(*buffer, + tf2_ros::TransformListener::RequiredInterfaces{ + node->get_node_base_interface(), node->get_node_logging_interface(), + node->get_node_parameters_interface(), node->get_node_topics_interface() }, + false // spin_thread + ); +#else tf2_ros::TransformListener listener(*buffer, node, false /* spin_thread - disables executor */); +#endif occupancy_map_monitor::OccupancyMapMonitor server(node, buffer); server.setUpdateCallback( [&octree_binary_pub, &server, logger = node->get_logger()] { return publishOctomap(octree_binary_pub, server); }); diff --git a/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp b/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp index 207480e531..f4328ad974 100644 --- a/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp +++ b/moveit_ros/occupancy_map_monitor/test/occupancy_map_monitor_tests.cpp @@ -38,7 +38,14 @@ #include #include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include diff --git a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp index 13d22bcfc9..dbbfd2d431 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp +++ b/moveit_ros/perception/depth_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp @@ -37,7 +37,14 @@ #pragma once #include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include #include diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index be57f380b0..b3f4cdae0f 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -37,6 +37,8 @@ #include #include #include +#include +#include #include // TODO: Remove conditional includes when released to all active distros. #if __has_include() @@ -158,13 +160,26 @@ void DepthImageOctomapUpdater::start() pub_filtered_label_image_ = filtered_label_transport_->advertiseCamera(prefix + "filtered_label", 1); - sub_depth_image_ = image_transport::create_camera_subscription( - node_.get(), image_topic_, - [this](const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) { - return depthImageCallback(depth_msg, info_msg); - }, - "raw", rmw_qos_profile_sensor_data); + sub_depth_image_ = +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + image_transport::create_camera_subscription( + *node_, image_topic_, + [this](const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) { + return depthImageCallback(depth_msg, info_msg); + }, + "raw", rclcpp::SensorDataQoS()); +// For Kilted and older +#else + image_transport::create_camera_subscription( + node_.get(), image_topic_, + [this](const sensor_msgs::msg::Image::ConstSharedPtr& depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr& info_msg) { + return depthImageCallback(depth_msg, info_msg); + }, + "raw", rmw_qos_profile_sensor_data); +#endif } void DepthImageOctomapUpdater::stop() diff --git a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp index a21695ed70..18a9cd898a 100644 --- a/moveit_ros/perception/mesh_filter/src/transform_provider.cpp +++ b/moveit_ros/perception/mesh_filter/src/transform_provider.cpp @@ -35,8 +35,16 @@ /* Author: Suat Gedikli */ #include -#include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +#include +// For Jazzy and older +#else #include +#include +#endif #include TransformProvider::TransformProvider(double update_rate) : stop_(true), update_rate_(update_rate) diff --git a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp index cfe085e8c0..65c7f090b9 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.hpp @@ -39,8 +39,15 @@ #include #include #include -#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +#include +// For Jazzy and older +#else #include +#include +#endif #if RCLCPP_VERSION_GTE(28, 3, 3) // Rolling #include #else diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 87e539eac9..7ab437a75a 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -50,8 +50,15 @@ #include #endif #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +#include +// For Jazzy and older +#else #include #include +#endif #include #include @@ -151,8 +158,19 @@ void PointCloudOctomapUpdater::start() new message_filters::Subscriber(node_, point_cloud_topic_, qos_profile, options); if (tf_listener_ && tf_buffer_ && !monitor_->getMapFrame().empty()) { +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + using MessageFilterPointCloud2 = tf2_ros::MessageFilter; + + MessageFilterPointCloud2::RequiredInterfaces required_interfaces{ node_->get_node_logging_interface(), + node_->get_node_clock_interface() }; + + point_cloud_filter_ = new tf2_ros::MessageFilter( + *point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, std::move(required_interfaces)); +#else point_cloud_filter_ = new tf2_ros::MessageFilter( *point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, node_); +#endif point_cloud_filter_->registerCallback( [this](const sensor_msgs::msg::PointCloud2::ConstSharedPtr& cloud) { cloudMsgCallback(cloud); }); RCLCPP_INFO(logger_, "Listening to '%s' using message filter with target frame '%s'", point_cloud_topic_.c_str(), diff --git a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.hpp b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.hpp index eeaf4ab415..245c9c1354 100644 --- a/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.hpp +++ b/moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.hpp @@ -40,12 +40,19 @@ #pragma once #include +#include #include #include #include #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif namespace moveit_cpp { diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.hpp index 273d4b1049..19b56f0b26 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.hpp +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.hpp @@ -45,10 +45,17 @@ #include #include +#include #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include diff --git a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp index fc965680e5..8ed8bd8fb8 100644 --- a/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp +++ b/moveit_ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.hpp @@ -37,8 +37,16 @@ #pragma once #include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +#include +// For Jazzy and older +#else #include #include +#endif #include #include #include diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp index 33f18180de..5ce64bfb47 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor_middleware_handle.cpp @@ -50,7 +50,6 @@ #include #include #include -#include #include diff --git a/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp b/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp index 2977b0f76c..affffce9d4 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/current_state_monitor_tests.cpp @@ -43,7 +43,14 @@ #include #include #include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif struct MockMiddlewareHandle : public planning_scene_monitor::CurrentStateMonitor::MiddlewareHandle { diff --git a/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp b/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp index 0b957fdf43..427ea1a919 100644 --- a/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp +++ b/moveit_ros/planning/planning_scene_monitor/test/trajectory_monitor_tests.cpp @@ -44,7 +44,14 @@ #include #include #include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif using namespace std::chrono_literals; diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.hpp b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.hpp index 0295e3216b..28534fc72c 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.hpp +++ b/moveit_ros/planning_interface/common_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.hpp @@ -37,7 +37,14 @@ #pragma once #include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp index 7692dc4f6a..c68b02bb53 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.hpp @@ -48,6 +48,7 @@ #include #include #include +#include #include #include @@ -56,7 +57,13 @@ #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 37c2471c2a..40e3fbdeed 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -39,6 +39,10 @@ #include #include #include + +#include +#include + #include #include #include @@ -68,9 +72,13 @@ #include #endif #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include -#include -#include +#endif namespace moveit { diff --git a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.hpp b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.hpp index 1221ffb3d4..227f135542 100644 --- a/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.hpp +++ b/moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.hpp @@ -36,13 +36,19 @@ #pragma once +#include #include #include #include -//#include #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif namespace robot_interaction { diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp index f754754b04..1ab1b6786a 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp @@ -22,12 +22,20 @@ #include #include +#include + #include #include #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include diff --git a/moveit_ros/trajectory_cache/src/utils/utils.cpp b/moveit_ros/trajectory_cache/src/utils/utils.cpp index 5d3a293408..bb9895995d 100644 --- a/moveit_ros/trajectory_cache/src/utils/utils.cpp +++ b/moveit_ros/trajectory_cache/src/utils/utils.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include #include #include @@ -28,7 +30,13 @@ #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include diff --git a/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.cpp b/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.cpp index f9a6882d1b..fe6c40fdde 100644 --- a/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.cpp +++ b/moveit_ros/trajectory_cache/test/fixtures/move_group_fixture.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "move_group_fixture.hpp" @@ -60,8 +61,10 @@ void MoveGroupFixture::TearDown() void MoveGroupFixture::spinNode() { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_); while (is_spinning_ && rclcpp::ok()) { - rclcpp::spin_some(node_); + executor.spin_some(); } } diff --git a/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.cpp b/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.cpp index 3d362b0106..f1ae851836 100644 --- a/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.cpp +++ b/moveit_ros/trajectory_cache/test/fixtures/warehouse_fixture.cpp @@ -19,6 +19,7 @@ #include #include +#include #include #include @@ -57,8 +58,10 @@ void WarehouseFixture::TearDown() void WarehouseFixture::spinNode() { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(node_); while (is_spinning_ && rclcpp::ok()) { - rclcpp::spin_some(node_); + executor.spin_some(); } } diff --git a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp index 69131bc52a..5b2c27393c 100644 --- a/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/test/test_trajectory_cache.cpp @@ -16,6 +16,7 @@ * @author methylDragon */ +#include #include #include @@ -1044,10 +1045,12 @@ int main(int argc, char** argv) std::atomic running = true; std::thread spin_thread([&]() { + rclcpp::executors::SingleThreadedExecutor executor; + executor.add_node(test_node); + executor.add_node(move_group_node); while (rclcpp::ok() && running) { - rclcpp::spin_some(test_node); - rclcpp::spin_some(move_group_node); + executor.spin_some(); } }); diff --git a/moveit_ros/trajectory_cache/test/utils/test_utils.cpp b/moveit_ros/trajectory_cache/test/utils/test_utils.cpp index 835b1de2af..44698b49dd 100644 --- a/moveit_ros/trajectory_cache/test/utils/test_utils.cpp +++ b/moveit_ros/trajectory_cache/test/utils/test_utils.cpp @@ -25,7 +25,13 @@ #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include "../fixtures/warehouse_fixture.hpp" diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp index 4f5a2b9242..deed6a6727 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_display.hpp @@ -36,6 +36,9 @@ #pragma once +#include +#include + #include #include #include @@ -50,12 +53,11 @@ #include #include -#include - #include #include #endif +#include #include namespace Ogre @@ -106,7 +108,16 @@ class MotionPlanningDisplay : public PlanningSceneDisplay void load(const rviz_common::Config& config) override; void save(rviz_common::Config config) const override; +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + using rviz_common::Display::update; + // `using` handles update(float, float) deprecation warning and redirect + void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override; +// For Kilted and older +#else void update(float wall_dt, float ros_dt) override; +#endif + void reset() override; moveit::core::RobotStateConstPtr getQueryStartState() const @@ -213,7 +224,15 @@ private Q_SLOTS: void onRobotModelLoaded() override; void onNewPlanningSceneState() override; void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type) override; + void updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override; +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + [[deprecated("Use updateInternal(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] void + updateInternal(double wall_dt, double ros_dt) override; +// For Kilted and older +#else void updateInternal(double wall_dt, double ros_dt) override; +#endif void renderWorkspaceBox(); void updateLinkColors(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp index 67e44bb40a..e591cc96f7 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/include/moveit/motion_planning_rviz_plugin/motion_planning_frame.hpp @@ -36,6 +36,8 @@ #pragma once +#include + #include #include #include @@ -122,7 +124,15 @@ class MotionPlanningFrame : public QWidget void initFromMoveGroupNS(); void constructPlanningRequest(moveit_msgs::msg::MotionPlanRequest& mreq); + void updateSceneMarkers(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt); +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + [[deprecated("Use updateSceneMarkers(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] void + updateSceneMarkers(double wall_dt, double ros_dt); +// For Kilted and older +#else void updateSceneMarkers(double wall_dt, double ros_dt); +#endif void updateExternalCommunication(); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index e279a23e5d..dfad31931d 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1337,6 +1337,19 @@ void MotionPlanningDisplay::onDisable() // ****************************************************************************************** // Update // ****************************************************************************************** +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) +void MotionPlanningDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) +{ + if (int_marker_display_) + int_marker_display_->update(wall_dt, ros_dt); + if (frame_) + frame_->updateSceneMarkers(wall_dt, ros_dt); + + PlanningSceneDisplay::update(wall_dt, ros_dt); +} +// For Kilted and older +#else void MotionPlanningDisplay::update(float wall_dt, float ros_dt) { if (int_marker_display_) @@ -1346,8 +1359,9 @@ void MotionPlanningDisplay::update(float wall_dt, float ros_dt) PlanningSceneDisplay::update(wall_dt, ros_dt); } +#endif -void MotionPlanningDisplay::updateInternal(double wall_dt, double ros_dt) +void MotionPlanningDisplay::updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) { PlanningSceneDisplay::updateInternal(wall_dt, ros_dt); @@ -1357,6 +1371,11 @@ void MotionPlanningDisplay::updateInternal(double wall_dt, double ros_dt) renderWorkspaceBox(); } +void MotionPlanningDisplay::updateInternal(double wall_dt, double ros_dt) +{ + updateInternal(std::chrono::nanoseconds(std::lround(wall_dt)), std::chrono::nanoseconds(std::lround(ros_dt))); +} + void MotionPlanningDisplay::load(const rviz_common::Config& config) { PlanningSceneDisplay::load(config); diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 074545f324..7e2bb78b6f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -36,6 +36,8 @@ #include +#include + #include #include #include @@ -47,7 +49,13 @@ #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include @@ -59,6 +67,8 @@ #include "ui_motion_planning_rviz_plugin_frame.h" +#include + namespace moveit_rviz_plugin { @@ -678,12 +688,17 @@ void MotionPlanningFrame::tabChanged(int index) } } -void MotionPlanningFrame::updateSceneMarkers(double /*wall_dt*/, double /*ros_dt*/) +void MotionPlanningFrame::updateSceneMarkers(std::chrono::nanoseconds /*wall_dt*/, std::chrono::nanoseconds /*ros_dt*/) { if (scene_marker_) scene_marker_->update(); } +void MotionPlanningFrame::updateSceneMarkers(double wall_dt, double ros_dt) +{ + updateSceneMarkers(std::chrono::nanoseconds(std::lround(wall_dt)), std::chrono::nanoseconds(std::lround(ros_dt))); +} + void MotionPlanningFrame::updateExternalCommunication() { if (ui_->allow_external_program->isChecked()) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp index 1c0aa57c77..9778246fb8 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/include/moveit/planning_scene_rviz_plugin/planning_scene_display.hpp @@ -36,6 +36,8 @@ #pragma once +#include + #include #include #include @@ -49,6 +51,8 @@ #include +#include + namespace Ogre { class SceneNode; @@ -79,7 +83,16 @@ class MOVEIT_PLANNING_SCENE_RVIZ_PLUGIN_CORE_EXPORT PlanningSceneDisplay : publi void load(const rviz_common::Config& config) override; void save(rviz_common::Config config) const override; + // For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + using rviz_common::Display::update; + // `using` handles update(float, float) deprecation warning and redirect + void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override; +// For Kilted and older +#else void update(float wall_dt, float ros_dt) override; +#endif + void reset() override; void setLinkColor(const std::string& link_name, const QColor& color); @@ -178,7 +191,15 @@ protected Q_SLOTS: void fixedFrameChanged() override; // new virtual functions added by this plugin + virtual void updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt); +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + [[deprecated("Use updateInternal(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] virtual void + updateInternal(double wall_dt, double ros_dt); +// For Kilted and older +#else virtual void updateInternal(double wall_dt, double ros_dt); +#endif virtual void onSceneMonitorReceivedUpdate(planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType update_type); planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 4bce580a25..074df1accb 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -50,7 +50,13 @@ #include #include #include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else #include +#endif #include #include @@ -58,6 +64,8 @@ #include #include +#include + namespace moveit_rviz_plugin { // ****************************************************************************************** @@ -661,6 +669,21 @@ void PlanningSceneDisplay::queueRenderSceneGeometry() planning_scene_needs_render_ = true; } +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) +void PlanningSceneDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) +{ + Display::update(wall_dt, ros_dt); + + executeMainLoopJobs(); + + calculateOffsetPosition(); + + if (planning_scene_monitor_) + updateInternal(wall_dt, ros_dt); +} +// For Kilted and older +#else void PlanningSceneDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); @@ -672,10 +695,11 @@ void PlanningSceneDisplay::update(float wall_dt, float ros_dt) if (planning_scene_monitor_) updateInternal(wall_dt, ros_dt); } +#endif -void PlanningSceneDisplay::updateInternal(double wall_dt, double /*ros_dt*/) +void PlanningSceneDisplay::updateInternal(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds /*ros_dt*/) { - current_scene_time_ += wall_dt; + current_scene_time_ += std::chrono::duration_cast>(wall_dt).count(); if (planning_scene_render_ && ((current_scene_time_ > scene_display_time_property_->getFloat() && robot_state_needs_render_) || planning_scene_needs_render_)) @@ -687,6 +711,11 @@ void PlanningSceneDisplay::updateInternal(double wall_dt, double /*ros_dt*/) } } +void PlanningSceneDisplay::updateInternal(double wall_dt, double ros_dt) +{ + updateInternal(std::chrono::nanoseconds(std::lround(wall_dt)), std::chrono::nanoseconds(std::lround(ros_dt))); +} + void PlanningSceneDisplay::load(const rviz_common::Config& config) { Display::load(config); diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp index 19e4c48920..c55a7cc009 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/include/moveit/robot_state_rviz_plugin/robot_state_display.hpp @@ -36,6 +36,8 @@ #pragma once +#include + #include #ifndef Q_MOC_RUN @@ -45,6 +47,8 @@ #include #endif +#include + namespace rviz_common { namespace properties @@ -71,7 +75,17 @@ class RobotStateDisplay : public rviz_common::Display ~RobotStateDisplay() override; void load(const rviz_common::Config& config) override; + + // For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + using rviz_common::Display::update; + // `using` handles update(float, float) deprecation warning and redirect + void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override; +// For Kilted and older +#else void update(float wall_dt, float ros_dt) override; +#endif + void reset() override; const moveit::core::RobotModelConstPtr& getRobotModel() const diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp index 960215e33a..483b10d9ed 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp +++ b/moveit_ros/visualization/robot_state_rviz_plugin/src/robot_state_display.cpp @@ -459,6 +459,21 @@ void RobotStateDisplay::onDisable() Display::onDisable(); } +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) +void RobotStateDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) +{ + Display::update(wall_dt, ros_dt); + calculateOffsetPosition(); + if (robot_ && update_state_ && robot_state_) + { + update_state_ = false; + robot_state_->update(); + robot_->update(robot_state_); + } +} +// For Kilted and older +#else void RobotStateDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); @@ -470,6 +485,7 @@ void RobotStateDisplay::update(float wall_dt, float ros_dt) robot_->update(robot_state_); } } +#endif // ****************************************************************************************** // Calculate Offset Position diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp index 9a52e96e65..1831a0a2b5 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.hpp @@ -36,6 +36,8 @@ #pragma once +#include + #include #include #include @@ -88,7 +90,15 @@ class TrajectoryVisualization : public QObject ~TrajectoryVisualization() override; - virtual void update(double wall_dt, double sim_dt); + virtual void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds sim_dt); +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + [[deprecated("Use update(std::chrono::nanoseconds, std::chrono::nanoseconds) instead")]] virtual void + update(double wall_dt, double ros_dt); +// For Kilted and older +#else + virtual void update(double wall_dt, double ros_dt); +#endif virtual void reset(); void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node, diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 2876aed004..05fabc60fe 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -58,6 +58,7 @@ #include +#include #include using namespace std::placeholders; @@ -419,7 +420,7 @@ void TrajectoryVisualization::dropTrajectory() drop_displaying_trajectory_ = true; } -void TrajectoryVisualization::update(double wall_dt, double sim_dt) +void TrajectoryVisualization::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds sim_dt) { if (drop_displaying_trajectory_) { @@ -475,11 +476,11 @@ void TrajectoryVisualization::update(double wall_dt, double sim_dt) int waypoint_count = displaying_trajectory_message_->getWayPointCount(); if (use_sim_time_property_->getBool()) { - current_state_time_ += sim_dt; + current_state_time_ += std::chrono::duration_cast>(sim_dt).count(); } else { - current_state_time_ += wall_dt; + current_state_time_ += std::chrono::duration_cast>(wall_dt).count(); } double tm = getStateDisplayTime(); @@ -549,6 +550,11 @@ void TrajectoryVisualization::update(double wall_dt, double sim_dt) display_path_robot_->updateAttachedObjectColors(default_attached_object_color_); } +void TrajectoryVisualization::update(double wall_dt, double sim_dt) +{ + update(std::chrono::nanoseconds(std::lround(wall_dt)), std::chrono::nanoseconds(std::lround(sim_dt))); +} + void TrajectoryVisualization::incomingDisplayTrajectory(const moveit_msgs::msg::DisplayTrajectory::ConstSharedPtr& msg) { // Error check diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.hpp b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.hpp index 3949bbb084..83f40524d5 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.hpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/include/moveit/trajectory_rviz_plugin/trajectory_display.hpp @@ -38,6 +38,8 @@ #pragma once +#include + #include #include #include @@ -48,6 +50,8 @@ #include #endif +#include + namespace rviz_common { class DisplayContext; @@ -68,7 +72,17 @@ class TrajectoryDisplay : public rviz_common::Display void loadRobotModel(); void load(const rviz_common::Config& config) override; + + // For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) + using rviz_common::Display::update; + // `using` handles update(float, float) deprecation warning and redirect + void update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) override; +// For Kilted and older +#else void update(float wall_dt, float ros_dt) override; +#endif + void reset() override; // overrides from Display diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index 78a13c718f..a66e5d965c 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -129,11 +129,21 @@ void TrajectoryDisplay::onDisable() trajectory_visual_->onDisable(); } +// For Rolling, L-turtle, and newer +#if RCLCPP_VERSION_GTE(30, 0, 0) +void TrajectoryDisplay::update(std::chrono::nanoseconds wall_dt, std::chrono::nanoseconds ros_dt) +{ + Display::update(wall_dt, ros_dt); + trajectory_visual_->update(wall_dt, ros_dt); +} +// For Kilted and older +#else void TrajectoryDisplay::update(float wall_dt, float ros_dt) { Display::update(wall_dt, ros_dt); trajectory_visual_->update(wall_dt, ros_dt); } +#endif void TrajectoryDisplay::changedRobotDescription() { diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index b837cec649..c54f86e385 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -34,17 +34,6 @@ /* Author: Ioan Sucan */ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include #include @@ -60,6 +49,24 @@ #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +// For Rolling, Kilted, and newer +#if RCLCPP_VERSION_GTE(29, 6, 0) +#include +// For Jazzy and older +#else +#include +#endif + rclcpp::Logger getLogger() { return moveit::getLogger("moveit.ros.save_to_warehouse");