diff --git a/README.md b/README.md index f772b08c..72fa3b44 100644 --- a/README.md +++ b/README.md @@ -14,6 +14,9 @@ Note that before `rostopic echo` would work with bridged topics, a subscriber mu You can use the `--bridge-all-2to1-topics` option to bridge all ROS 2 topics to ROS 1 so that tools such as `rostopic list` and `rqt` will see the topics even if there are no matching ROS 1 subscribers. Run `ros2 run ros1_bridge dynamic_bridge -- --help` for more options. +If you want to specify messages/services you want to bridge, run `ros2 run ros1_bridge parameter_bridge`. +See [here](doc/parameter_bridge.rst) for more information. + ## Prerequisites In order to run the bridge you need to either: diff --git a/doc/index.rst b/doc/index.rst index 27ed3d5a..1f879291 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -1,3 +1,4 @@ +.. _index: ros1_bridge =========== diff --git a/doc/parameter_bridge.rst b/doc/parameter_bridge.rst new file mode 100644 index 00000000..9be9319c --- /dev/null +++ b/doc/parameter_bridge.rst @@ -0,0 +1,92 @@ + +parameter_bridge configuration +============================== + +You can launch parameter_bridge with the following command:: + + ros2 run ros1_bridge parameter_bridge [ [ []]] + +Parameter bridge reads ``topics``, ``services_1_to_2``, and ``services_2_to_1`` parameters on ROS1 as default to make your bridge. +You can specify the names of those parameters as arguments of the command. +Here is an example of configuration:: + + + topics: + - + topic: /clock + type: rosgraph_msgs/msg/Clock + queue_size: 100 + direction: 1to2 + - + topic: /tf + type: tf2_msgs/msg/TFMessage + queue_size: 10 + direction: bidirectional + reliable: true + - + topic: /tf_static + type: tf2_msgs/msg/TFMessage + queue_size: 10 + direction: 1to2 + transient_local: true + reliable: true + - + topic: /scan + type: sensor_msgs/msg/LaserScan + queue_size: 10 + direction: 1to2 + - + topic: /cmd_vel + type: geometry_msgs/msg/Twist + queue_size: 10 + direction: 2to1 + - + topic: /map + type: nav_msgs/msg/OccupancyGrid + queue_size: 1 + direction: 2to1 + transient_local: true + reliable: true + latch: true + + services_1_to_2: + - + service: /gazebo/pause_physics + type: std_srvs/srv/Empty + + services_2_to_1: + - + service: /reinitialize_global_localization + type: std_srvs/srv/Empty + + +Configuration Syntax +-------------------- + +- ``topics``: + + - *topic*: + + .. csv-table:: + :header: "key", "required", "default", "description" + :widths: 20, 10, 10, 50 + + ``topic``, YES, N/A, Name of the topic to bridge which is used in both ROS1/2 + ``type``, YES, N/A, Message type in ROS2 (see `here <./index.rst#how-are-ros-1-and-2-messages-associated-with-each-other>`_ how to map ROS1 and ROS2 messages) + ``queue_size``, NO, 100, Queue size used in both ROS1/2 + ``direction``, NO, bidirectionl, ``bidirectional`` or ``1to2`` or ``2to1`` + ``transient_local``, NO, false, Set ROS2 QoS durability profile transient_local (used both for 1to2 and 2to1) + ``reliable``, NO, false, Set ROS2 QoS durability profile reliable (used both for 1to2 and 2to1) + ``latch``, NO, false, Publish ROS1 message with latch option (used only for 2to1) + +- ``services_1_to_2``: , ROS1 services will be available in ROS2 +- ``services_2_to_1``: , ROS2 services will be available in ROS1 + + - *service*: + + .. csv-table:: + :header: "key", "required", "description" + :widths: 20, 10, 60 + + ``service``, YES, Name of the service to bridge which is used in both ROS1/2 + ``type``, YES, Service type in ROS2 (see `here <./index.rst#how-are-ros-1-and-2-services-associated-with-each-other>`_ how to map ROS1 and ROS2 services) diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index eb2fccff..5f785db4 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -72,6 +72,9 @@ get_factory( std::unique_ptr get_service_factory(const std::string &, const std::string &, const std::string &); +rmw_qos_profile_t +get_qos(size_t queue_size, bool transient_local, bool reliable); + Bridge1to2Handles create_bridge_from_1_to_2( ros::NodeHandle ros1_node, @@ -81,7 +84,9 @@ create_bridge_from_1_to_2( size_t subscriber_queue_size, const std::string & ros2_type_name, const std::string & ros2_topic_name, - size_t publisher_queue_size); + size_t publisher_queue_size, + bool transient_local = false, + bool reliable = false); Bridge2to1Handles create_bridge_from_2_to_1( @@ -93,7 +98,10 @@ create_bridge_from_2_to_1( const std::string & ros1_type_name, const std::string & ros1_topic_name, size_t publisher_queue_size, - rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr); + rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr, + bool transient_local = false, + bool reliable = false, + bool latch = false); BridgeHandles create_bidirectional_bridge( @@ -102,7 +110,10 @@ create_bidirectional_bridge( const std::string & ros1_type_name, const std::string & ros2_type_name, const std::string & topic_name, - size_t queue_size = 10); + size_t queue_size = 10, + bool transient_local = false, + bool reliable = false, + bool latch = false); } // namespace ros1_bridge diff --git a/src/bridge.cpp b/src/bridge.cpp index 2e154b24..7c097ac8 100755 --- a/src/bridge.cpp +++ b/src/bridge.cpp @@ -19,6 +19,19 @@ namespace ros1_bridge { +rmw_qos_profile_t +get_qos( + size_t queue_size, + bool transient_local, + bool reliable) +{ + auto qos = rclcpp::QoS(rclcpp::KeepLast(queue_size)); + + if (transient_local) {qos.transient_local();} + if (reliable) {qos.reliable();} + + return qos.get_rmw_qos_profile(); +} Bridge1to2Handles create_bridge_from_1_to_2( @@ -29,11 +42,14 @@ create_bridge_from_1_to_2( size_t subscriber_queue_size, const std::string & ros2_type_name, const std::string & ros2_topic_name, - size_t publisher_queue_size) + size_t publisher_queue_size, + bool transient_local, + bool reliable) { auto factory = get_factory(ros1_type_name, ros2_type_name); + auto qos = get_qos(publisher_queue_size, transient_local, reliable); auto ros2_pub = factory->create_ros2_publisher( - ros2_node, ros2_topic_name, publisher_queue_size); + ros2_node, ros2_topic_name, qos); auto ros1_sub = factory->create_ros1_subscriber( ros1_node, ros1_topic_name, subscriber_queue_size, ros2_pub, ros2_node->get_logger()); @@ -54,14 +70,18 @@ create_bridge_from_2_to_1( const std::string & ros1_type_name, const std::string & ros1_topic_name, size_t publisher_queue_size, - rclcpp::PublisherBase::SharedPtr ros2_pub) + rclcpp::PublisherBase::SharedPtr ros2_pub, + bool transient_local, + bool reliable, + bool latch) { auto factory = get_factory(ros1_type_name, ros2_type_name); auto ros1_pub = factory->create_ros1_publisher( - ros1_node, ros1_topic_name, publisher_queue_size); + ros1_node, ros1_topic_name, publisher_queue_size, latch); + auto qos = get_qos(subscriber_queue_size, transient_local, reliable); auto ros2_sub = factory->create_ros2_subscriber( - ros2_node, ros2_topic_name, subscriber_queue_size, ros1_pub, ros2_pub); + ros2_node, ros2_topic_name, qos, ros1_pub, ros2_pub); Bridge2to1Handles handles; handles.ros2_subscriber = ros2_sub; @@ -76,17 +96,21 @@ create_bidirectional_bridge( const std::string & ros1_type_name, const std::string & ros2_type_name, const std::string & topic_name, - size_t queue_size) + size_t queue_size, + bool transient_local, + bool reliable, + bool latch) { RCLCPP_INFO(ros2_node->get_logger(), "create bidirectional bridge for topic " + topic_name); BridgeHandles handles; handles.bridge1to2 = create_bridge_from_1_to_2( ros1_node, ros2_node, - ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, queue_size); + ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, queue_size, + transient_local, reliable); handles.bridge2to1 = create_bridge_from_2_to_1( ros2_node, ros1_node, ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size, - handles.bridge1to2.ros2_publisher); + handles.bridge1to2.ros2_publisher, transient_local, reliable, latch); return handles; } diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index d6fcd7b8..234fea25 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -42,6 +42,8 @@ int main(int argc, char * argv[]) auto ros2_node = rclcpp::Node::make_shared("ros_bridge"); std::list all_handles; + std::list all_1to2_handles; + std::list all_2to1_handles; std::list service_bridges_1_to_2; std::list service_bridges_2_to_1; @@ -78,24 +80,49 @@ int main(int argc, char * argv[]) std::string topic_name = static_cast(topics[i]["topic"]); std::string type_name = static_cast(topics[i]["type"]); size_t queue_size = static_cast(topics[i]["queue_size"]); + std::string direction = static_cast(topics[i]["direction"]); + bool transient_local = static_cast(topics[i]["transient_local"]); + bool reliable = static_cast(topics[i]["reliable"]); + bool latch = static_cast(topics[i]["latch"]); + if (!queue_size) { queue_size = 100; } + if (direction == "") { + direction = "bidirectional"; + } + printf( - "Trying to create bidirectional bridge for topic '%s' " - "with ROS 2 type '%s'\n", - topic_name.c_str(), type_name.c_str()); + "Trying to create %s bridge for topic '%s' " + "with ROS 2 type '%s' (transient_local=%s reliable=%s latch=%s)\n", + direction.c_str(), topic_name.c_str(), type_name.c_str(), + transient_local ? "true" : "false", reliable ? "true" : "false", latch ? "true" : "false"); try { - ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( - ros1_node, ros2_node, "", type_name, topic_name, queue_size); - all_handles.push_back(handles); + if (direction == "bidirectional") { + ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + ros1_node, ros2_node, "", type_name, topic_name, queue_size, transient_local, + reliable, latch); + all_handles.push_back(handles); + } else if (direction == "1to2") { + ros1_bridge::Bridge1to2Handles handles = ros1_bridge::create_bridge_from_1_to_2( + ros1_node, ros2_node, "", topic_name, queue_size, type_name, topic_name, queue_size, + transient_local, reliable); + all_1to2_handles.push_back(handles); + } else if (direction == "2to1") { + ros1_bridge::Bridge2to1Handles handles = ros1_bridge::create_bridge_from_2_to_1( + ros2_node, ros1_node, type_name, topic_name, queue_size, "", topic_name, queue_size, + nullptr, transient_local, reliable, latch); + all_2to1_handles.push_back(handles); + } } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create bidirectional bridge for topic '%s' " - "with ROS 2 type '%s': %s\n", - topic_name.c_str(), type_name.c_str(), e.what()); + "failed to create %s bridge for topic '%s' " + "with ROS 2 type '%s' (transient_local=%s reliable=%s latch=%s): %s\n", + direction.c_str(), topic_name.c_str(), type_name.c_str(), + transient_local ? "true" : "false", reliable ? "true" : "false", latch ? "true" : "false", + e.what()); } } } else {