From d194dd6eef3029c8755481ea27461881aa05e22e Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 5 Dec 2022 11:09:29 -0700 Subject: [PATCH 1/4] Add QoS profile parameter to image bridge Signed-off-by: Sebastian Castro --- ros_gz_image/src/image_bridge.cpp | 29 +++++++++++++++++++++++------ 1 file changed, 23 insertions(+), 6 deletions(-) diff --git a/ros_gz_image/src/image_bridge.cpp b/ros_gz_image/src/image_bridge.cpp index f72160f2..109d1da2 100644 --- a/ros_gz_image/src/image_bridge.cpp +++ b/ros_gz_image/src/image_bridge.cpp @@ -22,22 +22,39 @@ #include #include #include +#include ////////////////////////////////////////////////// -/// \brief Bridges one topic +/// \brief Bridges one image topic class Handler { public: /// \brief Constructor /// \param[in] _topic Image base topic - /// \param[in] _it_node Pointer to image transport node + /// \param[in] _node Pointer to ROS node /// \param[in] _gz_node Pointer to Gazebo node Handler( const std::string & _topic, - std::shared_ptr _it_node, + std::shared_ptr _node, std::shared_ptr _gz_node) { - this->ros_pub = _it_node->advertise(_topic, 1); + // Get QoS profile from parameter + rmw_qos_profile_t qos_profile = rmw_qos_profile_default; + const auto qos_str = + _node->get_parameter("qos").get_parameter_value().get(); + if (qos_str == "system_default") { + qos_profile = rmw_qos_profile_system_default; + } else if (qos_str == "sensor_data") { + qos_profile = rmw_qos_profile_sensor_data; + } else if (qos_str != "default") { + RCLCPP_ERROR(_node->get_logger(), + "Invalid QoS profile %s specified. Using default profile.", + qos_str.c_str()); + } + + // Create publishers and subscribers + this->ros_pub = image_transport::create_publisher( + _node.get(), _topic, qos_profile); _gz_node->Subscribe(_topic, &Handler::OnImage, this); } @@ -77,7 +94,7 @@ int main(int argc, char * argv[]) // ROS node auto node_ = rclcpp::Node::make_shared("ros_gz_image"); - auto it_node = std::make_shared(node_); + node_->declare_parameter("qos", "default"); // Gazebo node auto gz_node = std::make_shared(); @@ -91,7 +108,7 @@ int main(int argc, char * argv[]) // Create publishers and subscribers for (auto topic : args) { - handlers.push_back(std::make_shared(topic, it_node, gz_node)); + handlers.push_back(std::make_shared(topic, node_, gz_node)); } // Spin ROS and Gz until shutdown From 72c2a038dacc6fc9862e4f729cb854a9145a769a Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 5 Dec 2022 12:59:13 -0700 Subject: [PATCH 2/4] Style fixes Signed-off-by: Sebastian Castro --- ros_gz_image/src/image_bridge.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ros_gz_image/src/image_bridge.cpp b/ros_gz_image/src/image_bridge.cpp index 109d1da2..777734d5 100644 --- a/ros_gz_image/src/image_bridge.cpp +++ b/ros_gz_image/src/image_bridge.cpp @@ -18,11 +18,11 @@ #include #include +#include #include #include #include #include -#include ////////////////////////////////////////////////// /// \brief Bridges one image topic @@ -47,7 +47,8 @@ class Handler } else if (qos_str == "sensor_data") { qos_profile = rmw_qos_profile_sensor_data; } else if (qos_str != "default") { - RCLCPP_ERROR(_node->get_logger(), + RCLCPP_ERROR( + _node->get_logger(), "Invalid QoS profile %s specified. Using default profile.", qos_str.c_str()); } From 4d7ecf8cdda5fb51aa1ccf26f7ce35ee622b0ad9 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Tue, 6 Dec 2022 07:37:02 -0700 Subject: [PATCH 3/4] Reorder headers to make cpplint happy Signed-off-by: Sebastian Castro --- .vscode/settings.json | 3 +++ ros_gz_image/src/image_bridge.cpp | 4 ++-- 2 files changed, 5 insertions(+), 2 deletions(-) create mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..46d9a84f --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "ros.distro": "humble" +} \ No newline at end of file diff --git a/ros_gz_image/src/image_bridge.cpp b/ros_gz_image/src/image_bridge.cpp index 777734d5..2b2fd52d 100644 --- a/ros_gz_image/src/image_bridge.cpp +++ b/ros_gz_image/src/image_bridge.cpp @@ -12,14 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include #include -#include -#include #include #include #include From 0e4f2efd534be3effb07e44a379b2b4063fc6a62 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Tue, 6 Dec 2022 07:43:19 -0700 Subject: [PATCH 4/4] Add information to README.md Signed-off-by: Sebastian Castro --- .vscode/settings.json | 3 --- ros_gz_image/README.md | 11 +++++++++++ 2 files changed, 11 insertions(+), 3 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 46d9a84f..00000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,3 +0,0 @@ -{ - "ros.distro": "humble" -} \ No newline at end of file diff --git a/ros_gz_image/README.md b/ros_gz_image/README.md index b7b27e1f..99db4da1 100644 --- a/ros_gz_image/README.md +++ b/ros_gz_image/README.md @@ -9,3 +9,14 @@ For compressed images, install and the bridge will publish `/compressed` images. The same goes for other `image_transport` plugins. +To run the bridge from the command line: + +```shell +ros2 run ros_gz_image image_bridge /topic1 /topic2 +``` + +You can also modify the [Quality of Service (QoS) policy](https://docs.ros.org/en/rolling/Concepts/About-Quality-of-Service-Settings.html#qos-policies) used to publish images using an additional `qos` ROS parameter. For example: + +```shell +ros2 run ros_gz_image image_bridge /topic1 /topic2 --ros-args qos:=sensor_data +```