diff --git a/depth_image_proc/src/convert_metric.cpp b/depth_image_proc/src/convert_metric.cpp index e77215abe..e526ad2da 100644 --- a/depth_image_proc/src/convert_metric.cpp +++ b/depth_image_proc/src/convert_metric.cpp @@ -67,15 +67,12 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options) // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo&) + [this](rclcpp::MatchedInfo &) { std::lock_guard lock(connect_mutex_); - if (pub_depth_.getNumSubscribers() == 0) - { + if (pub_depth_.getNumSubscribers() == 0) { sub_raw_.shutdown(); - } - else if (!sub_raw_) - { + } else if (!sub_raw_) { image_transport::TransportHints hints(this, "raw"); sub_raw_ = image_transport::create_subscription( this, "image_raw", @@ -83,7 +80,8 @@ ConvertMetricNode::ConvertMetricNode(const rclcpp::NodeOptions & options) hints.getTransport()); } }; - pub_depth_ = image_transport::create_publisher(this, "image", rmw_qos_profile_default, pub_options); + pub_depth_ = + image_transport::create_publisher(this, "image", rmw_qos_profile_default, pub_options); } void ConvertMetricNode::depthCb(const sensor_msgs::msg::Image::ConstSharedPtr & raw_msg) diff --git a/depth_image_proc/src/point_cloud_xyz.cpp b/depth_image_proc/src/point_cloud_xyz.cpp index ac3ea9a79..f2435f672 100644 --- a/depth_image_proc/src/point_cloud_xyz.cpp +++ b/depth_image_proc/src/point_cloud_xyz.cpp @@ -57,22 +57,21 @@ PointCloudXyzNode::PointCloudXyzNode(const rclcpp::NodeOptions & options) // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo& s) + [this](rclcpp::MatchedInfo & s) { std::lock_guard lock(connect_mutex_); - if (s.current_count == 0) - { + if (s.current_count == 0) { sub_depth_.shutdown(); - } - else if (!sub_depth_) - { + } else if (!sub_depth_) { auto custom_qos = rmw_qos_profile_system_default; custom_qos.depth = queue_size_; sub_depth_ = image_transport::create_camera_subscription( this, "image_rect", - std::bind(&PointCloudXyzNode::depthCb, this, std::placeholders::_1, std::placeholders::_2), + std::bind( + &PointCloudXyzNode::depthCb, this, std::placeholders::_1, + std::placeholders::_2), "raw", custom_qos); } diff --git a/depth_image_proc/src/point_cloud_xyz_radial.cpp b/depth_image_proc/src/point_cloud_xyz_radial.cpp index e60fb532b..b2f288fd2 100644 --- a/depth_image_proc/src/point_cloud_xyz_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyz_radial.cpp @@ -56,15 +56,12 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo& s) + [this](rclcpp::MatchedInfo & s) { std::lock_guard lock(connect_mutex_); - if (s.current_count == 0) - { + if (s.current_count == 0) { sub_depth_.shutdown(); - } - else if (!sub_depth_) - { + } else if (!sub_depth_) { auto custom_qos = rmw_qos_profile_system_default; custom_qos.depth = queue_size_; @@ -78,7 +75,8 @@ PointCloudXyzRadialNode::PointCloudXyzRadialNode(const rclcpp::NodeOptions & opt custom_qos); } }; - pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); + pub_point_cloud_ = create_publisher( + "points", rclcpp::SensorDataQoS(), pub_options); } void PointCloudXyzRadialNode::depthCb( diff --git a/depth_image_proc/src/point_cloud_xyzi.cpp b/depth_image_proc/src/point_cloud_xyzi.cpp index 7a28f025b..51f1bec68 100644 --- a/depth_image_proc/src/point_cloud_xyzi.cpp +++ b/depth_image_proc/src/point_cloud_xyzi.cpp @@ -74,17 +74,14 @@ PointCloudXyziNode::PointCloudXyziNode(const rclcpp::NodeOptions & options) // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo& s) + [this](rclcpp::MatchedInfo & s) { std::lock_guard lock(connect_mutex_); - if (s.current_count == 0) - { + if (s.current_count == 0) { sub_depth_.unsubscribe(); sub_intensity_.unsubscribe(); sub_info_.unsubscribe(); - } - else if (!sub_depth_.getSubscriber()) - { + } else if (!sub_depth_.getSubscriber()) { // parameter for depth_image_transport hint std::string depth_image_transport_param = "depth_image_transport"; diff --git a/depth_image_proc/src/point_cloud_xyzi_radial.cpp b/depth_image_proc/src/point_cloud_xyzi_radial.cpp index f201231a3..a838bc58d 100644 --- a/depth_image_proc/src/point_cloud_xyzi_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzi_radial.cpp @@ -72,17 +72,14 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo& s) + [this](rclcpp::MatchedInfo & s) { std::lock_guard lock(connect_mutex_); - if (s.current_count == 0) - { + if (s.current_count == 0) { sub_depth_.unsubscribe(); sub_intensity_.unsubscribe(); sub_info_.unsubscribe(); - } - else if (!sub_depth_.getSubscriber()) - { + } else if (!sub_depth_.getSubscriber()) { // parameter for depth_image_transport hint std::string depth_image_transport_param = "depth_image_transport"; @@ -96,7 +93,8 @@ PointCloudXyziRadialNode::PointCloudXyziRadialNode(const rclcpp::NodeOptions & o sub_info_.subscribe(this, "intensity/camera_info"); } }; - pub_point_cloud_ = create_publisher("points", rclcpp::SensorDataQoS(), pub_options); + pub_point_cloud_ = create_publisher( + "points", rclcpp::SensorDataQoS(), pub_options); } void PointCloudXyziRadialNode::imageCb( diff --git a/depth_image_proc/src/point_cloud_xyzrgb.cpp b/depth_image_proc/src/point_cloud_xyzrgb.cpp index d6341e46d..2bd99f3d3 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb.cpp @@ -83,17 +83,14 @@ PointCloudXyzrgbNode::PointCloudXyzrgbNode(const rclcpp::NodeOptions & options) // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo& s) + [this](rclcpp::MatchedInfo & s) { std::lock_guard lock(connect_mutex_); - if (s.current_count == 0) - { + if (s.current_count == 0) { sub_depth_.unsubscribe(); sub_rgb_.unsubscribe(); sub_info_.unsubscribe(); - } - else if (!sub_depth_.getSubscriber()) - { + } else if (!sub_depth_.getSubscriber()) { // parameter for depth_image_transport hint std::string depth_image_transport_param = "depth_image_transport"; image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); diff --git a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp index de53e17c1..0ba393829 100644 --- a/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp +++ b/depth_image_proc/src/point_cloud_xyzrgb_radial.cpp @@ -87,17 +87,14 @@ PointCloudXyzrgbRadialNode::PointCloudXyzrgbRadialNode(const rclcpp::NodeOptions // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo& s) + [this](rclcpp::MatchedInfo & s) { std::lock_guard lock(connect_mutex_); - if (s.current_count == 0) - { + if (s.current_count == 0) { sub_depth_.unsubscribe(); sub_rgb_.unsubscribe(); sub_info_.unsubscribe(); - } - else if (!sub_depth_.getSubscriber()) - { + } else if (!sub_depth_.getSubscriber()) { // parameter for depth_image_transport hint std::string depth_image_transport_param = "depth_image_transport"; image_transport::TransportHints depth_hints(this, "raw", depth_image_transport_param); diff --git a/image_proc/src/crop_decimate.cpp b/image_proc/src/crop_decimate.cpp index bc678427c..0cf0f4b44 100644 --- a/image_proc/src/crop_decimate.cpp +++ b/image_proc/src/crop_decimate.cpp @@ -130,17 +130,14 @@ CropDecimateNode::CropDecimateNode(const rclcpp::NodeOptions & options) // Create image pub with connection callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo&) + [this](rclcpp::MatchedInfo &) { - if (pub_.getNumSubscribers() == 0) - { + if (pub_.getNumSubscribers() == 0) { sub_.shutdown(); - } - else if (!sub_) - { + } else if (!sub_) { auto qos_profile = getTopicQosProfile(this, "in/image_raw"); sub_ = image_transport::create_camera_subscription( - this, "in/image_raw", std::bind( + this, "in/image_raw", std::bind( &CropDecimateNode::imageCb, this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile); } diff --git a/image_proc/src/debayer.cpp b/image_proc/src/debayer.cpp index 52640fb2f..b784198f3 100644 --- a/image_proc/src/debayer.cpp +++ b/image_proc/src/debayer.cpp @@ -56,20 +56,17 @@ DebayerNode::DebayerNode(const rclcpp::NodeOptions & options) // Create publisher options to setup callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo&) + [this](rclcpp::MatchedInfo &) { - if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) - { + if (pub_mono_.getNumSubscribers() == 0 && pub_color_.getNumSubscribers() == 0) { sub_raw_.shutdown(); - } - else if (!sub_raw_) - { + } else if (!sub_raw_) { auto qos_profile = getTopicQosProfile(this, "image_raw"); sub_raw_ = image_transport::create_subscription( - this, "image_raw", - std::bind( - &DebayerNode::imageCb, this, - std::placeholders::_1), "raw", qos_profile); + this, "image_raw", + std::bind( + &DebayerNode::imageCb, this, + std::placeholders::_1), "raw", qos_profile); } }; pub_mono_ = image_transport::create_publisher(this, "image_mono", qos_profile, pub_options); diff --git a/image_proc/src/rectify.cpp b/image_proc/src/rectify.cpp index 92d60a270..d814a3062 100644 --- a/image_proc/src/rectify.cpp +++ b/image_proc/src/rectify.cpp @@ -58,19 +58,16 @@ RectifyNode::RectifyNode(const rclcpp::NodeOptions & options) // Create publisher with connect callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo&) + [this](rclcpp::MatchedInfo &) { - if (pub_rect_.getNumSubscribers() == 0) - { + if (pub_rect_.getNumSubscribers() == 0) { sub_camera_.shutdown(); - } - else if (!sub_camera_) - { + } else if (!sub_camera_) { auto qos_profile = getTopicQosProfile(this, "image"); sub_camera_ = image_transport::create_camera_subscription( this, "image", std::bind( - &RectifyNode::imageCb, - this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile); + &RectifyNode::imageCb, + this, std::placeholders::_1, std::placeholders::_2), "raw", qos_profile); } }; pub_rect_ = image_transport::create_publisher(this, "image_rect", qos_profile, pub_options); diff --git a/image_proc/src/resize.cpp b/image_proc/src/resize.cpp index f47b748de..8795360d6 100644 --- a/image_proc/src/resize.cpp +++ b/image_proc/src/resize.cpp @@ -55,14 +55,11 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options) // Create image pub with connection callback rclcpp::PublisherOptions pub_options; pub_options.event_callbacks.matched_callback = - [this](rclcpp::MatchedInfo&) + [this](rclcpp::MatchedInfo &) { - if (pub_image_.getNumSubscribers() == 0) - { + if (pub_image_.getNumSubscribers() == 0) { sub_image_.shutdown(); - } - else if (!sub_image_) - { + } else if (!sub_image_) { auto qos_profile = getTopicQosProfile(this, "image/image_raw"); sub_image_ = image_transport::create_camera_subscription( this, "image/image_raw", @@ -72,7 +69,8 @@ ResizeNode::ResizeNode(const rclcpp::NodeOptions & options) std::placeholders::_2), "raw", qos_profile); } }; - pub_image_ = image_transport::create_camera_publisher(this, "resize/image_raw", qos_profile, pub_options); + pub_image_ = image_transport::create_camera_publisher( + this, "resize/image_raw", qos_profile, pub_options); interpolation_ = this->declare_parameter("interpolation", 1); use_scale_ = this->declare_parameter("use_scale", true);