diff --git a/mmros/include/mmros/node/camera_lidar_node.hpp b/mmros/include/mmros/node/camera_lidar_node.hpp index 5a6c24c..a16ab34 100644 --- a/mmros/include/mmros/node/camera_lidar_node.hpp +++ b/mmros/include/mmros/node/camera_lidar_node.hpp @@ -43,6 +43,7 @@ class CameraLidarNode : public rclcpp::Node */ CameraLidarNode(const std::string & name, const rclcpp::NodeOptions & options); +protected: /** * @brief Connects the node to the pointcloud and image topics. * @@ -51,11 +52,10 @@ class CameraLidarNode : public rclcpp::Node * @param image_callback Callback function for image messages. * @param use_raw Whether to use raw image data. */ - void onConnect( + void on_connect( const PointCloudCallback & pointcloud_callback, const std::vector & image_topics, const ImageCallback & image_callback, bool use_raw); -protected: rclcpp::TimerBase::SharedPtr connection_timer_; //!< Topic connection timer. private: @@ -65,7 +65,7 @@ class CameraLidarNode : public rclcpp::Node * @param callback Callback function to be called when a new pointcloud is received. * @return True if the connection was successful, false otherwise. */ - bool onConnectLidar(const PointCloudCallback & pointcloud_callback); + bool on_connect_lidar(const PointCloudCallback & pointcloud_callback); /** * @brief Connect to a single image topic. @@ -76,7 +76,7 @@ class CameraLidarNode : public rclcpp::Node * @param use_raw Whether to use raw images or not. * @return True if the connection was successful, false otherwise. */ - bool onConnectForSingleCamera( + bool on_connect_for_single_camera( size_t camera_id, const std::string & image_topic, const ImageCallback & image_callback, bool use_raw); diff --git a/mmros/include/mmros/node/detection2d_node.hpp b/mmros/include/mmros/node/detection2d_node.hpp index 5fa1485..9b7505d 100644 --- a/mmros/include/mmros/node/detection2d_node.hpp +++ b/mmros/include/mmros/node/detection2d_node.hpp @@ -38,14 +38,14 @@ class Detection2dNode : public SingleCameraNode */ explicit Detection2dNode(const rclcpp::NodeOptions & options); +private: /** * @brief Main callback for input image. * * @param msg Input image message. */ - virtual void onImage(sensor_msgs::msg::Image::ConstSharedPtr msg); + void callback(sensor_msgs::msg::Image::ConstSharedPtr msg); -private: std::unique_ptr detector_; //!< TensorRT detector. rclcpp::Publisher::SharedPtr pub_; //!< Output publisher. }; diff --git a/mmros/include/mmros/node/instance_segmentation2d_node.hpp b/mmros/include/mmros/node/instance_segmentation2d_node.hpp index cb05f36..29b369b 100644 --- a/mmros/include/mmros/node/instance_segmentation2d_node.hpp +++ b/mmros/include/mmros/node/instance_segmentation2d_node.hpp @@ -35,14 +35,14 @@ class InstanceSegmentation2dNode : public SingleCameraNode */ explicit InstanceSegmentation2dNode(const rclcpp::NodeOptions & options); +private: /** * @brief Main callback for input image. * * @param msg Input image message. */ - virtual void onImage(sensor_msgs::msg::Image::ConstSharedPtr msg); + void callback(sensor_msgs::msg::Image::ConstSharedPtr msg); -private: std::unique_ptr detector_; //!< TensorRT detector. rclcpp::Publisher::SharedPtr pub_segment_; //!< Output segment publisher. diff --git a/mmros/include/mmros/node/lidar_node.hpp b/mmros/include/mmros/node/lidar_node.hpp index ccbc668..ab016f3 100644 --- a/mmros/include/mmros/node/lidar_node.hpp +++ b/mmros/include/mmros/node/lidar_node.hpp @@ -40,14 +40,14 @@ class LidarNode : public rclcpp::Node */ LidarNode(const std::string & name, const rclcpp::NodeOptions & options); +protected: /** * @brief Check node connection and start subscribing. * * @param callback Callback function. */ - void onConnect(const Callback & callback); + void on_connect(const Callback & callback); -protected: rclcpp::TimerBase::SharedPtr connection_timer_; //!< Topic connection timer. private: diff --git a/mmros/include/mmros/node/multi_camera_node.hpp b/mmros/include/mmros/node/multi_camera_node.hpp index 9553d45..c4f5283 100644 --- a/mmros/include/mmros/node/multi_camera_node.hpp +++ b/mmros/include/mmros/node/multi_camera_node.hpp @@ -42,6 +42,7 @@ class MultiCameraNode : public rclcpp::Node */ MultiCameraNode(const std::string & name, const rclcpp::NodeOptions & options); +protected: /** * @brief Connect to multiple image topics. * @@ -49,10 +50,9 @@ class MultiCameraNode : public rclcpp::Node * @param callback Callback function to be called when a new image is received. * @param use_raw Whether to use raw images or not. */ - void onConnect( + void on_connect( const std::vector & image_topics, const Callback & callback, bool use_raw); -protected: rclcpp::TimerBase::SharedPtr connection_timer_; //!< Topic connection timer. private: @@ -65,7 +65,7 @@ class MultiCameraNode : public rclcpp::Node * @param use_raw Whether to use raw images or not. * @return True if the connection was successful, false otherwise. */ - bool onConnectForSingleCamera( + bool on_connect_for_single_camera( size_t camera_id, const std::string & image_topic, const Callback & callback, bool use_raw); std::vector subscriptions_; //!< Subscribers for each camera topic. diff --git a/mmros/include/mmros/node/panoptic_segmentation2d_node.hpp b/mmros/include/mmros/node/panoptic_segmentation2d_node.hpp index 93c8d50..93e7202 100644 --- a/mmros/include/mmros/node/panoptic_segmentation2d_node.hpp +++ b/mmros/include/mmros/node/panoptic_segmentation2d_node.hpp @@ -40,14 +40,14 @@ class PanopticSegmentation2dNode : public SingleCameraNode */ explicit PanopticSegmentation2dNode(const rclcpp::NodeOptions & options); +private: /** * @brief Main callback for input image. * * @param msg Input image message. */ - virtual void onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg); + void callback(sensor_msgs::msg::Image::ConstSharedPtr msg); -private: std::unique_ptr detector_; //!< TensorRT detector. rclcpp::Publisher::SharedPtr pub_box_; //!< Output box publisher. rclcpp::Publisher::SharedPtr pub_mask_; //!< Output mask publisher. diff --git a/mmros/include/mmros/node/semantic_segmentation2d_node.hpp b/mmros/include/mmros/node/semantic_segmentation2d_node.hpp index 22cb98f..83b1e06 100644 --- a/mmros/include/mmros/node/semantic_segmentation2d_node.hpp +++ b/mmros/include/mmros/node/semantic_segmentation2d_node.hpp @@ -37,14 +37,14 @@ class SemanticSegmentation2dNode : public SingleCameraNode */ explicit SemanticSegmentation2dNode(const rclcpp::NodeOptions & options); +private: /** * @brief Main callback for input image. * * @param msg Input image message. */ - virtual void onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg); + void callback(sensor_msgs::msg::Image::ConstSharedPtr msg); -private: std::unique_ptr detector_; //!< TensorRT detector. rclcpp::Publisher::SharedPtr pub_; //!< Output publisher. }; diff --git a/mmros/include/mmros/node/single_camera_node.hpp b/mmros/include/mmros/node/single_camera_node.hpp index 5b73d46..ffee6d9 100644 --- a/mmros/include/mmros/node/single_camera_node.hpp +++ b/mmros/include/mmros/node/single_camera_node.hpp @@ -40,15 +40,15 @@ class SingleCameraNode : public rclcpp::Node */ SingleCameraNode(const std::string & name, const rclcpp::NodeOptions & options); +protected: /** * @brief Check node connection and start subscribing. * * @param callback Callback function. * @param use_raw Indicates whether to use raw image. */ - void onConnect(const Callback & callback, bool use_raw); + void on_connect(const Callback & callback, bool use_raw); -protected: rclcpp::TimerBase::SharedPtr connection_timer_; //!< Topic connection timer. private: diff --git a/mmros/include/mmros/node/utility.hpp b/mmros/include/mmros/node/utility.hpp index 4f851b7..1fabbc1 100644 --- a/mmros/include/mmros/node/utility.hpp +++ b/mmros/include/mmros/node/utility.hpp @@ -30,7 +30,7 @@ namespace mmros::node * @return std::optional The QoS profile of the topic, or std::nullopt if the topic is * not published. */ -inline std::optional getTopicQos(const rclcpp::Node * node, const std::string & topic) +inline std::optional to_topic_qos(const rclcpp::Node * node, const std::string & topic) { const auto publisher_info = node->get_publishers_info_by_topic(topic); return publisher_info.size() != 1 ? std::nullopt @@ -44,7 +44,7 @@ inline std::optional getTopicQos(const rclcpp::Node * node, const s * @param query The topic name to resolve. * @return std::string The resolved topic name. */ -inline std::string resolveTopicName(rclcpp::Node * node, const std::string & query) +inline std::string resolve_topic_name(rclcpp::Node * node, const std::string & query) { return node->get_node_topics_interface()->resolve_topic_name(query); } diff --git a/mmros/src/node/camera_lidar_node.cpp b/mmros/src/node/camera_lidar_node.cpp index 00e0de8..8f42b39 100644 --- a/mmros/src/node/camera_lidar_node.cpp +++ b/mmros/src/node/camera_lidar_node.cpp @@ -34,18 +34,18 @@ CameraLidarNode::CameraLidarNode(const std::string & name, const rclcpp::NodeOpt google::InstallFailureSignalHandler(); } -void CameraLidarNode::onConnect( +void CameraLidarNode::on_connect( const PointCloudCallback & pointcloud_callback, const std::vector & image_topics, const ImageCallback & image_callback, bool use_raw) { image_subscriptions_.clear(); bool success = - onConnectLidar(pointcloud_callback) && + on_connect_lidar(pointcloud_callback) && std::all_of(image_topics.begin(), image_topics.end(), [&](const auto & image_topic) { auto camera_id = std::distance( image_topics.begin(), std::find(image_topics.begin(), image_topics.end(), image_topic)); - return onConnectForSingleCamera(camera_id, image_topic, image_callback, use_raw); + return on_connect_for_single_camera(camera_id, image_topic, image_callback, use_raw); }); if (success && connection_timer_) { @@ -55,11 +55,11 @@ void CameraLidarNode::onConnect( } } -bool CameraLidarNode::onConnectLidar(const PointCloudCallback & pointcloud_callback) +bool CameraLidarNode::on_connect_lidar(const PointCloudCallback & pointcloud_callback) { - const auto pointcloud_topic = resolveTopicName(this, "~/input/pointcloud"); + const auto pointcloud_topic = resolve_topic_name(this, "~/input/pointcloud"); - const auto pointcloud_qos = getTopicQos(this, pointcloud_topic); + const auto pointcloud_qos = to_topic_qos(this, pointcloud_topic); if (pointcloud_qos) { pointcloud_subscription_ = create_subscription( pointcloud_topic, *pointcloud_qos, pointcloud_callback); @@ -71,11 +71,11 @@ bool CameraLidarNode::onConnectLidar(const PointCloudCallback & pointcloud_callb } } -bool CameraLidarNode::onConnectForSingleCamera( +bool CameraLidarNode::on_connect_for_single_camera( size_t camera_id, const std::string & image_topic, const ImageCallback & image_callback, bool use_raw) { - const auto image_qos = getTopicQos(this, use_raw ? image_topic : image_topic + "/compressed"); + const auto image_qos = to_topic_qos(this, use_raw ? image_topic : image_topic + "/compressed"); if (image_qos) { rclcpp::SubscriptionOptions options; options.callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); diff --git a/mmros/src/node/detection2d_node.cpp b/mmros/src/node/detection2d_node.cpp index 1b72dd6..cc4cba7 100644 --- a/mmros/src/node/detection2d_node.cpp +++ b/mmros/src/node/detection2d_node.cpp @@ -59,11 +59,11 @@ Detection2dNode::Detection2dNode(const rclcpp::NodeOptions & options) { using std::chrono_literals::operator""ms; - using std::placeholders::_1; bool use_raw = declare_parameter("use_raw"); connection_timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this, use_raw]() { - this->onConnect(std::bind(&Detection2dNode::onImage, this, _1), use_raw); + this->on_connect( + [this](sensor_msgs::msg::Image::ConstSharedPtr msg) { this->callback(msg); }, use_raw); }); pub_ = create_publisher("~/output/boxes", 1); @@ -75,7 +75,7 @@ Detection2dNode::Detection2dNode(const rclcpp::NodeOptions & options) } } -void Detection2dNode::onImage(sensor_msgs::msg::Image::ConstSharedPtr msg) +void Detection2dNode::callback(sensor_msgs::msg::Image::ConstSharedPtr msg) { cv_bridge::CvImagePtr in_image_ptr; try { diff --git a/mmros/src/node/instance_segmentation2d_node.cpp b/mmros/src/node/instance_segmentation2d_node.cpp index 408b2b3..ee6a007 100644 --- a/mmros/src/node/instance_segmentation2d_node.cpp +++ b/mmros/src/node/instance_segmentation2d_node.cpp @@ -60,11 +60,11 @@ InstanceSegmentation2dNode::InstanceSegmentation2dNode(const rclcpp::NodeOptions { using std::chrono_literals::operator""ms; - using std::placeholders::_1; bool use_raw = declare_parameter("use_raw"); connection_timer_ = create_timer(this, get_clock(), 100ms, [this, use_raw]() { - this->onConnect(std::bind(&InstanceSegmentation2dNode::onImage, this, _1), use_raw); + this->on_connect( + [this](sensor_msgs::msg::Image::ConstSharedPtr msg) { this->callback(msg); }, use_raw); }); pub_segment_ = @@ -77,7 +77,7 @@ InstanceSegmentation2dNode::InstanceSegmentation2dNode(const rclcpp::NodeOptions } } -void InstanceSegmentation2dNode::onImage(sensor_msgs::msg::Image::ConstSharedPtr msg) +void InstanceSegmentation2dNode::callback(sensor_msgs::msg::Image::ConstSharedPtr msg) { cv_bridge::CvImagePtr in_image_ptr; try { diff --git a/mmros/src/node/lidar_node.cpp b/mmros/src/node/lidar_node.cpp index 84b215d..c04ccc0 100644 --- a/mmros/src/node/lidar_node.cpp +++ b/mmros/src/node/lidar_node.cpp @@ -29,11 +29,11 @@ LidarNode::LidarNode(const std::string & name, const rclcpp::NodeOptions & optio google::InstallFailureSignalHandler(); } -void LidarNode::onConnect(const Callback & callback) +void LidarNode::on_connect(const Callback & callback) { - const auto pointcloud_topic = resolveTopicName(this, "~/input/pointcloud"); + const auto pointcloud_topic = resolve_topic_name(this, "~/input/pointcloud"); - const auto pointcloud_qos = getTopicQos(this, pointcloud_topic); + const auto pointcloud_qos = to_topic_qos(this, pointcloud_topic); if (pointcloud_qos) { subscription_ = create_subscription( pointcloud_topic, *pointcloud_qos, callback); diff --git a/mmros/src/node/multi_camera_node.cpp b/mmros/src/node/multi_camera_node.cpp index b10cdde..1e9ea1b 100644 --- a/mmros/src/node/multi_camera_node.cpp +++ b/mmros/src/node/multi_camera_node.cpp @@ -35,7 +35,7 @@ MultiCameraNode::MultiCameraNode(const std::string & name, const rclcpp::NodeOpt google::InstallFailureSignalHandler(); } -void MultiCameraNode::onConnect( +void MultiCameraNode::on_connect( const std::vector & image_topics, const Callback & callback, bool use_raw) { subscriptions_.clear(); @@ -44,7 +44,7 @@ void MultiCameraNode::onConnect( std::all_of(image_topics.begin(), image_topics.end(), [&](const auto & image_topic) { auto camera_id = std::distance( image_topics.begin(), std::find(image_topics.begin(), image_topics.end(), image_topic)); - return onConnectForSingleCamera(camera_id, image_topic, callback, use_raw); + return on_connect_for_single_camera(camera_id, image_topic, callback, use_raw); }); if (success && connection_timer_) { @@ -53,10 +53,10 @@ void MultiCameraNode::onConnect( } } -bool MultiCameraNode::onConnectForSingleCamera( +bool MultiCameraNode::on_connect_for_single_camera( size_t camera_id, const std::string & image_topic, const Callback & callback, bool use_raw) { - const auto image_qos = getTopicQos(this, use_raw ? image_topic : image_topic + "/compressed"); + const auto image_qos = to_topic_qos(this, use_raw ? image_topic : image_topic + "/compressed"); if (image_qos) { rclcpp::SubscriptionOptions options; options.callback_group = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); diff --git a/mmros/src/node/panoptic_segmentation2d_node.cpp b/mmros/src/node/panoptic_segmentation2d_node.cpp index e51941f..6c2e75a 100644 --- a/mmros/src/node/panoptic_segmentation2d_node.cpp +++ b/mmros/src/node/panoptic_segmentation2d_node.cpp @@ -55,11 +55,11 @@ PanopticSegmentation2dNode::PanopticSegmentation2dNode(const rclcpp::NodeOptions { using std::chrono_literals::operator""ms; - using std::placeholders::_1; bool use_raw = declare_parameter("use_raw"); connection_timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this, use_raw]() { - this->onConnect(std::bind(&PanopticSegmentation2dNode::onImage, this, _1), use_raw); + this->on_connect( + [this](sensor_msgs::msg::Image::ConstSharedPtr msg) { this->callback(msg); }, use_raw); }); pub_box_ = create_publisher("~/output/boxes", 1); @@ -72,7 +72,7 @@ PanopticSegmentation2dNode::PanopticSegmentation2dNode(const rclcpp::NodeOptions } } -void PanopticSegmentation2dNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) +void PanopticSegmentation2dNode::callback(sensor_msgs::msg::Image::ConstSharedPtr msg) { cv_bridge::CvImagePtr in_image_ptr; try { diff --git a/mmros/src/node/semantic_segmentation2d_node.cpp b/mmros/src/node/semantic_segmentation2d_node.cpp index ba22e44..e124bdc 100644 --- a/mmros/src/node/semantic_segmentation2d_node.cpp +++ b/mmros/src/node/semantic_segmentation2d_node.cpp @@ -50,11 +50,11 @@ SemanticSegmentation2dNode::SemanticSegmentation2dNode(const rclcpp::NodeOptions { using std::chrono_literals::operator""ms; - using std::placeholders::_1; bool use_raw = declare_parameter("use_raw"); connection_timer_ = rclcpp::create_timer(this, get_clock(), 100ms, [this, use_raw]() { - this->onConnect(std::bind(&SemanticSegmentation2dNode::onImage, this, _1), use_raw); + this->on_connect( + [this](sensor_msgs::msg::Image::ConstSharedPtr msg) { this->callback(msg); }, use_raw); }); pub_ = create_publisher("~/output/mask", 1); @@ -66,7 +66,7 @@ SemanticSegmentation2dNode::SemanticSegmentation2dNode(const rclcpp::NodeOptions } } -void SemanticSegmentation2dNode::onImage(const sensor_msgs::msg::Image::ConstSharedPtr msg) +void SemanticSegmentation2dNode::callback(const sensor_msgs::msg::Image::ConstSharedPtr msg) { cv_bridge::CvImagePtr in_image_ptr; try { diff --git a/mmros/src/node/single_camera_node.cpp b/mmros/src/node/single_camera_node.cpp index 3cccb4e..cf41159 100644 --- a/mmros/src/node/single_camera_node.cpp +++ b/mmros/src/node/single_camera_node.cpp @@ -31,11 +31,11 @@ SingleCameraNode::SingleCameraNode(const std::string & name, const rclcpp::NodeO google::InstallFailureSignalHandler(); } -void SingleCameraNode::onConnect(const Callback & callback, bool use_raw) +void SingleCameraNode::on_connect(const Callback & callback, bool use_raw) { - const auto image_topic = resolveTopicName(this, "~/input/image"); + const auto image_topic = resolve_topic_name(this, "~/input/image"); - const auto image_qos = getTopicQos(this, use_raw ? image_topic : image_topic + "/compressed"); + const auto image_qos = to_topic_qos(this, use_raw ? image_topic : image_topic + "/compressed"); if (image_qos) { subscription_ = image_transport::create_subscription( this, image_topic, callback, use_raw ? "raw" : "compressed",