Skip to content

Commit 8dd8376

Browse files
Feedback
Signed-off-by: ElSayed ElSheikh <[email protected]>
1 parent 0ab5957 commit 8dd8376

File tree

2 files changed

+26
-3
lines changed

2 files changed

+26
-3
lines changed

nav2_collision_monitor/src/pointcloud.cpp

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -61,9 +61,7 @@ void PointCloud::configure()
6161
getParameters(source_topic);
6262

6363
const point_cloud_transport::TransportHints hint(transport_type_);
64-
6564
pct_ = std::make_shared<point_cloud_transport::PointCloudTransport>(*node);
66-
6765
data_sub_ = pct_->subscribe(
6866
source_topic, nav2::qos::SensorDataQoS(),
6967
std::bind(&PointCloud::dataCallback, this, std::placeholders::_1),
@@ -136,6 +134,7 @@ void PointCloud::getParameters(std::string & source_topic)
136134
nav2::declare_parameter_if_not_declared(
137135
node, source_name_ + ".min_range", rclcpp::ParameterValue(0.0));
138136
min_range_ = node->get_parameter(source_name_ + ".min_range").as_double();
137+
nav2::declare_parameter_if_not_declared(
139138
node, source_name_ + ".transport_type", rclcpp::ParameterValue(std::string("raw")));
140139
transport_type_ = node->get_parameter(source_name_ + ".transport_type").as_string();
141140
}

nav2_costmap_2d/plugins/obstacle_layer.cpp

Lines changed: 25 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -289,8 +289,32 @@ void ObstacleLayer::onInitialize()
289289
tf_filter_tolerance));
290290

291291
} else {
292-
auto sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
292+
// For Kilted and Older Support from Message Filters API change
293+
#if RCLCPP_VERSION_GTE(29, 6, 0)
294+
std::shared_ptr<point_cloud_transport::SubscriberFilter> sub;
295+
#else
296+
std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
297+
rclcpp_lifecycle::LifecycleNode>> sub;
298+
#endif
299+
300+
// For Kilted compatibility in Message Filters API change
301+
#if RCLCPP_VERSION_GTE(29, 6, 0)
302+
sub = std::make_shared<point_cloud_transport::SubscriberFilter>(
293303
*node, topic, transport_type, custom_qos_profile, sub_opt);
304+
// For Jazzy compatibility in Message Filters API change
305+
#elif RCLCPP_VERSION_GTE(29, 0, 0)
306+
sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
307+
rclcpp_lifecycle::LifecycleNode>>(
308+
std::static_pointer_cast<rclcpp_lifecycle::LifecycleNode>(node),
309+
topic, custom_qos_profile, sub_opt);
310+
// For Humble and Older compatibility in Message Filters API change
311+
#else
312+
sub = std::make_shared<message_filters::Subscriber<sensor_msgs::msg::PointCloud2,
313+
rclcpp_lifecycle::LifecycleNode>>(
314+
std::static_pointer_cast<rclcpp_lifecycle::LifecycleNode>(node),
315+
topic, custom_qos_profile.get_rmw_qos_profile(), sub_opt);
316+
#endif
317+
294318
sub->unsubscribe();
295319

296320
if (inf_is_valid) {

0 commit comments

Comments
 (0)