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 +``` diff --git a/ros_gz_image/src/image_bridge.cpp b/ros_gz_image/src/image_bridge.cpp index f72160f2..2b2fd52d 100644 --- a/ros_gz_image/src/image_bridge.cpp +++ b/ros_gz_image/src/image_bridge.cpp @@ -12,32 +12,50 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include #include #include -#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 +95,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 +109,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