Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion moveit_kinematics/test/test_kinematics_plugin.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>

Expand Down Expand Up @@ -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();
}
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand All @@ -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
Expand Down
8 changes: 8 additions & 0 deletions moveit_ros/hybrid_planning/test/test_basic_integration.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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_);

Expand Down
10 changes: 10 additions & 0 deletions moveit_ros/move_group/src/move_group.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
7 changes: 7 additions & 0 deletions moveit_ros/moveit_servo/demos/cpp_interface/demo_pose.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
7 changes: 7 additions & 0 deletions moveit_ros/moveit_servo/demos/cpp_interface/demo_twist.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
5 changes: 4 additions & 1 deletion moveit_ros/moveit_servo/demos/servo_keyboard_input.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@
#include <control_msgs/msg/joint_jog.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <moveit_msgs/srv/servo_command_type.hpp>
#include <rclcpp/executors/single_threaded_executor.hpp>
#include <rclcpp/rclcpp.hpp>
#include <signal.h>
#include <stdio.h>
Expand Down Expand Up @@ -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();
}
}

Expand Down
10 changes: 9 additions & 1 deletion moveit_ros/moveit_servo/include/moveit_servo/servo.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,9 @@

#pragma once

#include <rclcpp/logger.hpp>
#include <rclcpp/version.h>

#include <moveit_servo/collision_monitor.hpp>
#include <moveit_servo/moveit_servo_lib_parameters.hpp>
#include <moveit_servo/utils/command.hpp>
Expand All @@ -51,9 +54,14 @@
#include <pluginlib/class_loader.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <tf2_eigen/tf2_eigen.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 <variant>
#include <rclcpp/logger.hpp>
#include <queue>

namespace moveit_servo
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,14 @@
#include <moveit_msgs/srv/save_map.hpp>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/version.h>
// 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 <functional>
#include <memory>
Expand Down
18 changes: 18 additions & 0 deletions moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -34,10 +34,18 @@

/* Author: Jon Binney, Ioan Sucan */

#include <rclcpp/version.h>

#include <moveit/occupancy_map_monitor/occupancy_map_monitor.hpp>

#include <octomap_msgs/conversions.h>
// 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 <rclcpp/clock.hpp>
#include <rclcpp/executors.hpp>
#include <rclcpp/experimental/buffers/intra_process_buffer.hpp>
Expand Down Expand Up @@ -99,7 +107,17 @@ int main(int argc, char** argv)
node->create_publisher<octomap_msgs::msg::Octomap>("octomap_binary", RMW_QOS_POLICY_HISTORY_KEEP_LAST);
auto clock_ptr = std::make_shared<rclcpp::Clock>(RCL_ROS_TIME);
std::shared_ptr<tf2_ros::Buffer> buffer = std::make_shared<tf2_ros::Buffer>(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); });
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,14 @@

#include <gmock/gmock.h>
#include <gtest/gtest.h>
#include <rclcpp/version.h>
// 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 <memory>
#include <string>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,7 +37,14 @@
#pragma once

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/version.h>
// 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/occupancy_map_monitor/occupancy_map_updater.hpp>
#include <moveit/mesh_filter/mesh_filter.hpp>
#include <moveit/mesh_filter/stereo_camera_model.hpp>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@
#include <moveit/depth_image_octomap_updater/depth_image_octomap_updater.hpp>
#include <moveit/occupancy_map_monitor/occupancy_map_monitor.hpp>
#include <cmath>
#include <rclcpp/qos.hpp>
#include <rclcpp/version.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
// TODO: Remove conditional includes when released to all active distros.
#if __has_include(<tf2/LinearMath/Vector3.hpp>)
Expand Down Expand Up @@ -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()
Expand Down
10 changes: 9 additions & 1 deletion moveit_ros/perception/mesh_filter/src/transform_provider.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,16 @@
/* Author: Suat Gedikli */

#include <moveit/mesh_filter/transform_provider.hpp>
#include <tf2_ros/transform_listener.h>
#include <rclcpp/version.h>
// 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
#include <tf2_eigen/tf2_eigen.hpp>

TransformProvider::TransformProvider(double update_rate) : stop_(true), update_rate_(update_rate)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -39,8 +39,15 @@
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/callback_group.hpp>
#include <rclcpp/version.h>
#include <tf2_ros/transform_listener.h>
// For Rolling, Kilted, and newer
#if RCLCPP_VERSION_GTE(29, 6, 0)
#include <tf2_ros/message_filter.hpp>
#include <tf2_ros/transform_listener.hpp>
// For Jazzy and older
#else
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>
#endif
#if RCLCPP_VERSION_GTE(28, 3, 3) // Rolling
#include <message_filters/subscriber.hpp>
#else
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,8 +50,15 @@
#include <tf2/LinearMath/Transform.h>
#endif
#include <sensor_msgs/point_cloud2_iterator.hpp>
// For Rolling, Kilted, and newer
#if RCLCPP_VERSION_GTE(29, 6, 0)
#include <tf2_ros/create_timer_interface.hpp>
#include <tf2_ros/create_timer_ros.hpp>
// For Jazzy and older
#else
#include <tf2_ros/create_timer_interface.h>
#include <tf2_ros/create_timer_ros.h>
#endif
#include <moveit/utils/logger.hpp>
#include <rclcpp/version.h>

Expand Down Expand Up @@ -151,8 +158,19 @@ void PointCloudOctomapUpdater::start()
new message_filters::Subscriber<sensor_msgs::msg::PointCloud2>(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<sensor_msgs::msg::PointCloud2>;

MessageFilterPointCloud2::RequiredInterfaces required_interfaces{ node_->get_node_logging_interface(),
node_->get_node_clock_interface() };

point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(
*point_cloud_subscriber_, *tf_buffer_, monitor_->getMapFrame(), 5, std::move(required_interfaces));
#else
point_cloud_filter_ = new tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>(
*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(),
Expand Down
Loading
Loading