From ba6e72cb72330c1400a61e45878c921b25871e44 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 14 Oct 2021 09:48:49 +0200 Subject: [PATCH 01/14] Extend create_bidirectional_bridge to take qos param for ROS2 publisher Signed-off-by: Loy van Beek --- include/ros1_bridge/bridge.hpp | 10 ++++++++++ src/bridge.cpp | 24 ++++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index c0485df9..e1db7744 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -127,6 +127,16 @@ create_bidirectional_bridge( const std::string & topic_name, size_t queue_size = 10); +BridgeHandles +create_bidirectional_bridge( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string &ros1_type_name, + const std::string &ros2_type_name, + const std::string &topic_name, + size_t queue_size, + const rclcpp::QoS &publisher_qos); + } // namespace ros1_bridge #endif // ROS1_BRIDGE__BRIDGE_HPP_ diff --git a/src/bridge.cpp b/src/bridge.cpp index b8f0fa83..069b9431 100755 --- a/src/bridge.cpp +++ b/src/bridge.cpp @@ -139,4 +139,28 @@ create_bidirectional_bridge( return handles; } +BridgeHandles +create_bidirectional_bridge( + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string &ros1_type_name, + const std::string &ros2_type_name, + const std::string &topic_name, + size_t queue_size, + const rclcpp::QoS &publisher_qos) +{ + RCLCPP_INFO( + ros2_node->get_logger(), "create bidirectional bridge for topic %s", + topic_name.c_str()); + 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, publisher_qos); + 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); + return handles; +} + } // namespace ros1_bridge From 7dd23c3fa336fbca7677d4d7e11f7cbca7181c7d Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 14 Oct 2021 10:16:52 +0200 Subject: [PATCH 02/14] Busy setting up a way to read QoS parameters from ROS1 params Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index f4f4bd76..7ba2cc2d 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -240,3 +240,18 @@ int main(int argc, char * argv[]) return 0; } + +rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) +{ + auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); + if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) + { + } + else + { + fprintf( + stderr, + "QoS parameters could not be read\n"); // TODO: clearer message + } + return ros2_publisher_qos; +} \ No newline at end of file From f9dcb472797075b26c67da4bf29c35a52d61c47e Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 14 Oct 2021 10:45:39 +0200 Subject: [PATCH 03/14] Parse history qos params Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 28 ++++++++++++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 7ba2cc2d..0f49b7ea 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -246,6 +246,34 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) { + if(qos_params.hasMember("history")) + { + auto history = static_cast(qos_params["history"]); + if(history == "keep_all") + { + ros2_publisher_qos.keep_all(); + } + else if (history == "keep_last") + { + if(qos_params.hasMember("depth")) + { + auto depth = static_cast(qos_params["depth"]); + ros2_publisher_qos.keep_last(depth); + } + else + { + fprintf( + stderr, + "history: keep_last requires that also a depth is set\n"); + } + } + else + { + fprintf( + stderr, + "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also required 'depth' to be set)\n", history.c_str()); + } + } } else { From 59094800ed57389b549556ada265fc7e56dae62e Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 14 Oct 2021 10:56:06 +0200 Subject: [PATCH 04/14] Call qos_from_params when setting up topics Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 102 +++++++++++++++++++++------------------ 1 file changed, 56 insertions(+), 46 deletions(-) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 0f49b7ea..e1b45cd5 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -30,6 +30,48 @@ #include "ros1_bridge/bridge.hpp" +rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) +{ + auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); + if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) + { + if (qos_params.hasMember("history")) + { + auto history = static_cast(qos_params["history"]); + if (history == "keep_all") + { + ros2_publisher_qos.keep_all(); + } + else if (history == "keep_last") + { + if (qos_params.hasMember("depth")) + { + auto depth = static_cast(qos_params["depth"]); + ros2_publisher_qos.keep_last(depth); + } + else + { + fprintf( + stderr, + "history: keep_last requires that also a depth is set\n"); + } + } + else + { + fprintf( + stderr, + "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also required 'depth' to be set)\n", history.c_str()); + } + } + } + else + { + fprintf( + stderr, + "QoS parameters could not be read\n"); // TODO: clearer message + } + return ros2_publisher_qos; +} int main(int argc, char * argv[]) { @@ -87,9 +129,20 @@ int main(int argc, char * argv[]) topic_name.c_str(), type_name.c_str()); 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 (topics[i].hasMember("qos")) + { + auto qos_settings = qos_from_params(topics[i]["qos"]); + ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); + all_handles.push_back(handles); + } + else + { + ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( + ros1_node, ros2_node, "", type_name, topic_name, queue_size); + all_handles.push_back(handles); + } + } catch (std::runtime_error & e) { fprintf( stderr, @@ -239,47 +292,4 @@ int main(int argc, char * argv[]) } return 0; -} - -rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) -{ - auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); - if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) - { - if(qos_params.hasMember("history")) - { - auto history = static_cast(qos_params["history"]); - if(history == "keep_all") - { - ros2_publisher_qos.keep_all(); - } - else if (history == "keep_last") - { - if(qos_params.hasMember("depth")) - { - auto depth = static_cast(qos_params["depth"]); - ros2_publisher_qos.keep_last(depth); - } - else - { - fprintf( - stderr, - "history: keep_last requires that also a depth is set\n"); - } - } - else - { - fprintf( - stderr, - "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also required 'depth' to be set)\n", history.c_str()); - } - } - } - else - { - fprintf( - stderr, - "QoS parameters could not be read\n"); // TODO: clearer message - } - return ros2_publisher_qos; } \ No newline at end of file From 0846b9fea5c434ed494b504d8c315f164d2b2ac5 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 14 Oct 2021 12:02:02 +0200 Subject: [PATCH 05/14] Configure deadline, lifespan, liveliness_lease_durations Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 96 +++++++++++++++++++++++++++++++++++++++- 1 file changed, 95 insertions(+), 1 deletion(-) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index e1b45cd5..c4972db8 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -33,6 +33,7 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); + if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) { if (qos_params.hasMember("history")) @@ -60,7 +61,100 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { fprintf( stderr, - "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also required 'depth' to be set)\n", history.c_str()); + "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also requires 'depth' to be set)\n", history.c_str()); + } + } + + if (qos_params.hasMember("reliability")) + { + auto reliability = static_cast(qos_params["reliability"]); + if (reliability == "best_effort") + { + ros2_publisher_qos.best_effort(); + } + else if (reliability == "reliable") + { + ros2_publisher_qos.reliable(); + } + else + { + fprintf( + stderr, + "invalid value for 'reliability': '%s', allowed values are 'best_effort', 'reliable'\n", reliability.c_str()); + } + } + + if (qos_params.hasMember("durability")) + { + auto durability = static_cast(qos_params["durability"]); + if (durability == "transient_local") + { + ros2_publisher_qos.transient_local(); + } + else if (durability == "volatile") + { + ros2_publisher_qos.durability_volatile(); + } + else + { + fprintf( + stderr, + "invalid value for 'durability': '%s', allowed values are 'best_effort', 'volatile'\n", durability.c_str()); + } + } + + if (qos_params.hasMember("deadline")) + { + try + { + ros2_publisher_qos.deadline( + rclcpp::Duration( + static_cast(qos_params["deadline"]["secs"]), + static_cast(qos_params["deadline"]["nsecs"])) + ); + } + catch (std::runtime_error &e) + { + fprintf( + stderr, + "failed to create parametrize deadline: '%s'\n", + e.what()); + } + } + + if (qos_params.hasMember("lifespan")) + { + try + { + ros2_publisher_qos.lifespan( + rclcpp::Duration( + static_cast(qos_params["lifespan"]["secs"]), + static_cast(qos_params["lifespan"]["nsecs"]))); + } + catch (std::runtime_error &e) + { + fprintf( + stderr, + "failed to create parametrize lifespan: '%s'\n", + e.what()); + } + } + + if (qos_params.hasMember("liveliness_lease_duration")) + { + try + { + ros2_publisher_qos.liveliness_lease_duration( + rclcpp::Duration( + static_cast(qos_params["liveliness_lease_duration"]["secs"]), + static_cast(qos_params["liveliness_lease_duration"]["nsecs"]))); + } + catch (std::runtime_error &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness_lease_duration: '%s'\n", + e.what()); } } } From d408ac0dcda6f9ab4d6b5679450954293508a4aa Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Thu, 14 Oct 2021 12:09:08 +0200 Subject: [PATCH 06/14] Configure liveliness Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index c4972db8..67b77dde 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -140,6 +140,22 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } } + if (qos_params.hasMember("liveliness")) + { + try + { + auto liveliness = static_cast(qos_params["liveliness"]); + ros2_publisher_qos.liveliness(static_cast(liveliness)); + } + catch (std::runtime_error &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.what()); + } + } + if (qos_params.hasMember("liveliness_lease_duration")) { try From 4dd0ef022ec44e49ba79116a3f0676ecd679092d Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Oct 2021 11:46:07 +0200 Subject: [PATCH 07/14] Add some basic debug text Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 67b77dde..64a453bf 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -241,6 +241,7 @@ int main(int argc, char * argv[]) try { if (topics[i].hasMember("qos")) { + printf("Setting up QoS for '%s'\n", topic_name.c_str()); auto qos_settings = qos_from_params(topics[i]["qos"]); ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); From 148e26148c4b2883d0ca3cc0568d37effd1d1861 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Oct 2021 12:20:02 +0200 Subject: [PATCH 08/14] Print the QoS settings to stdout when setting them up Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 45 +++++++++++++++++++++++++++------------- 1 file changed, 31 insertions(+), 14 deletions(-) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 64a453bf..55d74bc0 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -33,15 +33,19 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); + + printf("Qos("); if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) { if (qos_params.hasMember("history")) { auto history = static_cast(qos_params["history"]); + printf("history: "); if (history == "keep_all") { ros2_publisher_qos.keep_all(); + printf("keep_all, "); } else if (history == "keep_last") { @@ -49,6 +53,7 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { auto depth = static_cast(qos_params["depth"]); ros2_publisher_qos.keep_last(depth); + printf("keep_last(%i), ", depth); } else { @@ -68,13 +73,16 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) if (qos_params.hasMember("reliability")) { auto reliability = static_cast(qos_params["reliability"]); + printf("reliability: "); if (reliability == "best_effort") { ros2_publisher_qos.best_effort(); + printf("best_effort, "); } else if (reliability == "reliable") { ros2_publisher_qos.reliable(); + printf("reliable, "); } else { @@ -87,13 +95,16 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) if (qos_params.hasMember("durability")) { auto durability = static_cast(qos_params["durability"]); + printf("durability: "); if (durability == "transient_local") { ros2_publisher_qos.transient_local(); + printf("transient_local, "); } else if (durability == "volatile") { ros2_publisher_qos.durability_volatile(); + printf("volatile, "); } else { @@ -107,11 +118,11 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { try { - ros2_publisher_qos.deadline( - rclcpp::Duration( - static_cast(qos_params["deadline"]["secs"]), - static_cast(qos_params["deadline"]["nsecs"])) - ); + rclcpp::Duration dur = rclcpp::Duration( + static_cast(qos_params["deadline"]["secs"]), + static_cast(qos_params["deadline"]["nsecs"])); + ros2_publisher_qos.deadline(dur); + printf("deadline: Duration(nsecs: %ld), ", dur.nanoseconds()); } catch (std::runtime_error &e) { @@ -126,10 +137,11 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { try { - ros2_publisher_qos.lifespan( - rclcpp::Duration( - static_cast(qos_params["lifespan"]["secs"]), - static_cast(qos_params["lifespan"]["nsecs"]))); + rclcpp::Duration dur = rclcpp::Duration( + static_cast(qos_params["lifespan"]["secs"]), + static_cast(qos_params["lifespan"]["nsecs"])); + ros2_publisher_qos.lifespan(dur); + printf("lifespan: Duration(nsecs: %ld), ", dur.nanoseconds()); } catch (std::runtime_error &e) { @@ -146,6 +158,7 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { auto liveliness = static_cast(qos_params["liveliness"]); ros2_publisher_qos.liveliness(static_cast(liveliness)); + printf("liveliness: %i, ", static_cast(liveliness)); } catch (std::runtime_error &e) { @@ -160,10 +173,11 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) { try { - ros2_publisher_qos.liveliness_lease_duration( - rclcpp::Duration( - static_cast(qos_params["liveliness_lease_duration"]["secs"]), - static_cast(qos_params["liveliness_lease_duration"]["nsecs"]))); + rclcpp::Duration dur = rclcpp::Duration( + static_cast(qos_params["liveliness_lease_duration"]["secs"]), + static_cast(qos_params["liveliness_lease_duration"]["nsecs"])); + ros2_publisher_qos.liveliness_lease_duration(dur); + printf("liveliness_lease_duration: Duration(nsecs: %ld), ", dur.nanoseconds()); } catch (std::runtime_error &e) { @@ -180,6 +194,8 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) stderr, "QoS parameters could not be read\n"); // TODO: clearer message } + + printf(")"); return ros2_publisher_qos; } @@ -241,8 +257,9 @@ int main(int argc, char * argv[]) try { if (topics[i].hasMember("qos")) { - printf("Setting up QoS for '%s'\n", topic_name.c_str()); + printf("Setting up QoS for '%s': ", topic_name.c_str()); auto qos_settings = qos_from_params(topics[i]["qos"]); + printf("\n"); ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); all_handles.push_back(handles); From 6f13f2c5829b275f381f28fcc4ad33c0c04d4aa9 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Oct 2021 13:33:22 +0200 Subject: [PATCH 09/14] Catch XmlRpc::XmlRpcExceptions when constructing QoS from parameters Signed-off-by: Loy van Beek --- CMakeLists.txt | 1 + package.xml | 1 + src/parameter_bridge.cpp | 29 +++++++++++++++++++++++++++++ 3 files changed, 31 insertions(+) diff --git a/CMakeLists.txt b/CMakeLists.txt index c8487612..f7f5af88 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -16,6 +16,7 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rmw_implementation_cmake REQUIRED) find_package(std_msgs REQUIRED) +find_package(xmlrpcpp REQUIRED) # find ROS 1 packages set(cmake_extras_files cmake/find_ros1_package.cmake cmake/find_ros1_interface_packages.cmake) diff --git a/package.xml b/package.xml index 995a45aa..59381ffd 100644 --- a/package.xml +++ b/package.xml @@ -25,6 +25,7 @@ rcutils rmw_implementation_cmake std_msgs + xmlrpcpp pkg-config diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 55d74bc0..a16244e1 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -27,6 +27,7 @@ // include ROS 2 #include "rclcpp/rclcpp.hpp" +#include #include "ros1_bridge/bridge.hpp" @@ -131,6 +132,13 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) "failed to create parametrize deadline: '%s'\n", e.what()); } + catch (XmlRpc::XmlRpcException &e) + { + fprintf( + stderr, + "failed to create parametrize deadline: '%s'\n", + e.getMessage().c_str()); + } } if (qos_params.hasMember("lifespan")) @@ -150,6 +158,13 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) "failed to create parametrize lifespan: '%s'\n", e.what()); } + catch (XmlRpc::XmlRpcException &e) + { + fprintf( + stderr, + "failed to create parametrize lifespan: '%s'\n", + e.getMessage().c_str()); + } } if (qos_params.hasMember("liveliness")) @@ -167,6 +182,13 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) "failed to create parametrize liveliness: '%s'\n", e.what()); } + catch (XmlRpc::XmlRpcException &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.getMessage().c_str()); + } } if (qos_params.hasMember("liveliness_lease_duration")) @@ -186,6 +208,13 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) "failed to create parametrize liveliness_lease_duration: '%s'\n", e.what()); } + catch (XmlRpc::XmlRpcException &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness_lease_duration: '%s'\n", + e.getMessage().c_str()); + } } } else From b777cbf56d67d667463c1646a76852835115ff0d Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 18 Oct 2021 16:52:32 +0200 Subject: [PATCH 10/14] Parse liveliness as either int enum value or upper/lower case string representation of liveliness enum values Signed-off-by: Loy van Beek --- README.md | 44 ++++++++++++++++++++++ src/parameter_bridge.cpp | 79 +++++++++++++++++++++++++++++++++------- 2 files changed, 110 insertions(+), 13 deletions(-) diff --git a/README.md b/README.md index 112df87f..e740418a 100644 --- a/README.md +++ b/README.md @@ -419,3 +419,47 @@ This should start printing text like `I heard: [hello world ...]` with a timesta ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 1, b: 2}" ``` If all is well, the output should contain `example_interfaces.srv.AddTwoInts_Response(sum=3)` + +### Parametrizing Quality of Service +An advantage or ROS 2 over ROS 1 is the possibility to define different Quality of Service settings per topic. +The parameter bridge optionally allows for this as well: +For some topics, like `/tf_static` this is actually required, as this is a latching topic in ROS 1. +In ROS 2 with the `parameter_bridge`, this requires that topic to be configured as such: + +```yaml +topics: + - + topic: /tf_static + type: tf2_msgs/msg/TFMessage + queue_size: 1 + qos: + history: keep_all + durability: transient_local +``` + +All other QoS options (as documented here in https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html) are available: + +```yaml +topics: + - + topic: /some_ros1_topic + type: std_msgs/msg/String + queue_size: 1 + qos: + history: keep_last # OR keep_last, then you can omit `depth` below + depth: 10 # Only required when history == keep_last + reliability: reliable # OR best_effort + durability: transient_local # OR volatile + deadline: + secs: 10 + nsecs: 2345 + lifespan: + secs: 20 + nsecs: 3456 + liveliness: liveliness_system_default # Values from https://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html, eg. LIVELINESS_AUTOMATIC + liveliness_lease_duration: + secs: 40 + nsecs: 5678 +``` + +Note that the `qos` section can be omitted entirely and options not set are left default. diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index a16244e1..74e6564e 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -169,25 +169,78 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) if (qos_params.hasMember("liveliness")) { - try + if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) { - auto liveliness = static_cast(qos_params["liveliness"]); - ros2_publisher_qos.liveliness(static_cast(liveliness)); - printf("liveliness: %i, ", static_cast(liveliness)); + printf("liviness is an int\n"); + try + { + auto liveliness = static_cast(qos_params["liveliness"]); + ros2_publisher_qos.liveliness(static_cast(liveliness)); + printf("liveliness: %i, ", static_cast(liveliness)); + } + catch (std::runtime_error &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.what()); + } + catch (XmlRpc::XmlRpcException &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.getMessage().c_str()); + } } - catch (std::runtime_error &e) + else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString) { - fprintf( - stderr, - "failed to create parametrize liveliness: '%s'\n", - e.what()); + try + { + rmw_qos_liveliness_policy_t liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; + auto liveliness_str = static_cast(qos_params["liveliness"]); + if (liveliness_str == "LIVELINESS_SYSTEM_DEFAULT" || liveliness_str == "liveliness_system_default") + { + liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; + } + else if (liveliness_str == "LIVELINESS_AUTOMATIC" || liveliness_str == "liveliness_automatic") + { + liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; + } + else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || liveliness_str == "liveliness_manual_by_topic") + { + liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; + } + else + { + fprintf( + stderr, + "invalid value for 'liveliness': '%s', allowed values are LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n", liveliness_str.c_str()); + } + + ros2_publisher_qos.liveliness(liveliness); + printf("liveliness: %s, ", liveliness_str.c_str()); + } + catch (std::runtime_error &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.what()); + } + catch (XmlRpc::XmlRpcException &e) + { + fprintf( + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.getMessage().c_str()); + } } - catch (XmlRpc::XmlRpcException &e) + else { fprintf( - stderr, - "failed to create parametrize liveliness: '%s'\n", - e.getMessage().c_str()); + stderr, + "failed to create parametrize liveliness, parameter was not a string or int \n"); } } From 774330e1b769bd6e89a9c33e079b25c611f1797a Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 19 Oct 2021 09:21:13 +0200 Subject: [PATCH 11/14] Fix formatting with uncrustify Signed-off-by: Loy van Beek --- include/ros1_bridge/bridge.hpp | 14 +- src/bridge.cpp | 28 ++-- src/parameter_bridge.cpp | 269 +++++++++++++-------------------- 3 files changed, 127 insertions(+), 184 deletions(-) diff --git a/include/ros1_bridge/bridge.hpp b/include/ros1_bridge/bridge.hpp index e1db7744..c4d79dfc 100755 --- a/include/ros1_bridge/bridge.hpp +++ b/include/ros1_bridge/bridge.hpp @@ -129,13 +129,13 @@ create_bidirectional_bridge( BridgeHandles create_bidirectional_bridge( - ros::NodeHandle ros1_node, - rclcpp::Node::SharedPtr ros2_node, - const std::string &ros1_type_name, - const std::string &ros2_type_name, - const std::string &topic_name, - size_t queue_size, - const rclcpp::QoS &publisher_qos); + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string & ros1_type_name, + const std::string & ros2_type_name, + const std::string & topic_name, + size_t queue_size, + const rclcpp::QoS & publisher_qos); } // namespace ros1_bridge diff --git a/src/bridge.cpp b/src/bridge.cpp index 069b9431..def61466 100755 --- a/src/bridge.cpp +++ b/src/bridge.cpp @@ -141,25 +141,25 @@ create_bidirectional_bridge( BridgeHandles create_bidirectional_bridge( - ros::NodeHandle ros1_node, - rclcpp::Node::SharedPtr ros2_node, - const std::string &ros1_type_name, - const std::string &ros2_type_name, - const std::string &topic_name, - size_t queue_size, - const rclcpp::QoS &publisher_qos) + ros::NodeHandle ros1_node, + rclcpp::Node::SharedPtr ros2_node, + const std::string & ros1_type_name, + const std::string & ros2_type_name, + const std::string & topic_name, + size_t queue_size, + const rclcpp::QoS & publisher_qos) { RCLCPP_INFO( - ros2_node->get_logger(), "create bidirectional bridge for topic %s", - topic_name.c_str()); + ros2_node->get_logger(), "create bidirectional bridge for topic %s", + topic_name.c_str()); 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, publisher_qos); + ros1_node, ros2_node, + ros1_type_name, topic_name, queue_size, ros2_type_name, topic_name, publisher_qos); 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); + ros2_node, ros1_node, + ros2_type_name, topic_name, queue_size, ros1_type_name, topic_name, queue_size, + handles.bridge1to2.ros2_publisher); return handles; } diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 74e6564e..a2996e66 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -36,245 +36,191 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(10)); printf("Qos("); - - if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) - { - if (qos_params.hasMember("history")) - { + + if (qos_params.getType() == XmlRpc::XmlRpcValue::TypeStruct) { + if (qos_params.hasMember("history")) { auto history = static_cast(qos_params["history"]); printf("history: "); - if (history == "keep_all") - { + if (history == "keep_all") { ros2_publisher_qos.keep_all(); printf("keep_all, "); - } - else if (history == "keep_last") - { - if (qos_params.hasMember("depth")) - { + } else if (history == "keep_last") { + if (qos_params.hasMember("depth")) { auto depth = static_cast(qos_params["depth"]); ros2_publisher_qos.keep_last(depth); printf("keep_last(%i), ", depth); - } - else - { + } else { fprintf( - stderr, - "history: keep_last requires that also a depth is set\n"); + stderr, + "history: keep_last requires that also a depth is set\n"); } - } - else - { + } else { fprintf( - stderr, - "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also requires 'depth' to be set)\n", history.c_str()); + stderr, + "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also requires 'depth' to be set)\n", + history.c_str()); } } - if (qos_params.hasMember("reliability")) - { + if (qos_params.hasMember("reliability")) { auto reliability = static_cast(qos_params["reliability"]); printf("reliability: "); - if (reliability == "best_effort") - { + if (reliability == "best_effort") { ros2_publisher_qos.best_effort(); printf("best_effort, "); - } - else if (reliability == "reliable") - { + } else if (reliability == "reliable") { ros2_publisher_qos.reliable(); printf("reliable, "); - } - else - { + } else { fprintf( - stderr, - "invalid value for 'reliability': '%s', allowed values are 'best_effort', 'reliable'\n", reliability.c_str()); + stderr, + "invalid value for 'reliability': '%s', allowed values are 'best_effort', 'reliable'\n", + reliability.c_str()); } } - if (qos_params.hasMember("durability")) - { + if (qos_params.hasMember("durability")) { auto durability = static_cast(qos_params["durability"]); printf("durability: "); - if (durability == "transient_local") - { + if (durability == "transient_local") { ros2_publisher_qos.transient_local(); printf("transient_local, "); - } - else if (durability == "volatile") - { + } else if (durability == "volatile") { ros2_publisher_qos.durability_volatile(); printf("volatile, "); - } - else - { + } else { fprintf( - stderr, - "invalid value for 'durability': '%s', allowed values are 'best_effort', 'volatile'\n", durability.c_str()); + stderr, + "invalid value for 'durability': '%s', allowed values are 'best_effort', 'volatile'\n", + durability.c_str()); } } - if (qos_params.hasMember("deadline")) - { - try - { + if (qos_params.hasMember("deadline")) { + try { rclcpp::Duration dur = rclcpp::Duration( - static_cast(qos_params["deadline"]["secs"]), - static_cast(qos_params["deadline"]["nsecs"])); + static_cast(qos_params["deadline"]["secs"]), + static_cast(qos_params["deadline"]["nsecs"])); ros2_publisher_qos.deadline(dur); printf("deadline: Duration(nsecs: %ld), ", dur.nanoseconds()); - } - catch (std::runtime_error &e) - { + } catch (std::runtime_error & e) { fprintf( - stderr, - "failed to create parametrize deadline: '%s'\n", - e.what()); - } - catch (XmlRpc::XmlRpcException &e) - { + stderr, + "failed to create parametrize deadline: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { fprintf( - stderr, - "failed to create parametrize deadline: '%s'\n", - e.getMessage().c_str()); + stderr, + "failed to create parametrize deadline: '%s'\n", + e.getMessage().c_str()); } } - if (qos_params.hasMember("lifespan")) - { - try - { + if (qos_params.hasMember("lifespan")) { + try { rclcpp::Duration dur = rclcpp::Duration( - static_cast(qos_params["lifespan"]["secs"]), - static_cast(qos_params["lifespan"]["nsecs"])); - ros2_publisher_qos.lifespan(dur); + static_cast(qos_params["lifespan"]["secs"]), + static_cast(qos_params["lifespan"]["nsecs"])); + ros2_publisher_qos.lifespan(dur); printf("lifespan: Duration(nsecs: %ld), ", dur.nanoseconds()); - } - catch (std::runtime_error &e) - { + } catch (std::runtime_error & e) { fprintf( - stderr, - "failed to create parametrize lifespan: '%s'\n", - e.what()); - } - catch (XmlRpc::XmlRpcException &e) - { + stderr, + "failed to create parametrize lifespan: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { fprintf( - stderr, - "failed to create parametrize lifespan: '%s'\n", - e.getMessage().c_str()); + stderr, + "failed to create parametrize lifespan: '%s'\n", + e.getMessage().c_str()); } } - if (qos_params.hasMember("liveliness")) - { - if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) - { + if (qos_params.hasMember("liveliness")) { + if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) { printf("liviness is an int\n"); - try - { + try { auto liveliness = static_cast(qos_params["liveliness"]); ros2_publisher_qos.liveliness(static_cast(liveliness)); printf("liveliness: %i, ", static_cast(liveliness)); - } - catch (std::runtime_error &e) - { + } catch (std::runtime_error & e) { fprintf( - stderr, - "failed to create parametrize liveliness: '%s'\n", - e.what()); - } - catch (XmlRpc::XmlRpcException &e) - { + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { fprintf( - stderr, - "failed to create parametrize liveliness: '%s'\n", - e.getMessage().c_str()); + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.getMessage().c_str()); } - } - else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString) - { - try - { - rmw_qos_liveliness_policy_t liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; + } else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString) { + try { + rmw_qos_liveliness_policy_t liveliness = + rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; auto liveliness_str = static_cast(qos_params["liveliness"]); - if (liveliness_str == "LIVELINESS_SYSTEM_DEFAULT" || liveliness_str == "liveliness_system_default") + if (liveliness_str == "LIVELINESS_SYSTEM_DEFAULT" || + liveliness_str == "liveliness_system_default") { liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; - } - else if (liveliness_str == "LIVELINESS_AUTOMATIC" || liveliness_str == "liveliness_automatic") + } else if (liveliness_str == "LIVELINESS_AUTOMATIC" || + liveliness_str == "liveliness_automatic") { liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - } - else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || liveliness_str == "liveliness_manual_by_topic") + } else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || + liveliness_str == "liveliness_manual_by_topic") { liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; - } - else - { + } else { fprintf( - stderr, - "invalid value for 'liveliness': '%s', allowed values are LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n", liveliness_str.c_str()); + stderr, + "invalid value for 'liveliness': '%s', allowed values are LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n", + liveliness_str.c_str()); } ros2_publisher_qos.liveliness(liveliness); printf("liveliness: %s, ", liveliness_str.c_str()); - } - catch (std::runtime_error &e) - { + } catch (std::runtime_error & e) { fprintf( - stderr, - "failed to create parametrize liveliness: '%s'\n", - e.what()); - } - catch (XmlRpc::XmlRpcException &e) - { + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { fprintf( - stderr, - "failed to create parametrize liveliness: '%s'\n", - e.getMessage().c_str()); + stderr, + "failed to create parametrize liveliness: '%s'\n", + e.getMessage().c_str()); } - } - else - { + } else { fprintf( stderr, "failed to create parametrize liveliness, parameter was not a string or int \n"); } } - if (qos_params.hasMember("liveliness_lease_duration")) - { - try - { + if (qos_params.hasMember("liveliness_lease_duration")) { + try { rclcpp::Duration dur = rclcpp::Duration( - static_cast(qos_params["liveliness_lease_duration"]["secs"]), - static_cast(qos_params["liveliness_lease_duration"]["nsecs"])); - ros2_publisher_qos.liveliness_lease_duration(dur); + static_cast(qos_params["liveliness_lease_duration"]["secs"]), + static_cast(qos_params["liveliness_lease_duration"]["nsecs"])); + ros2_publisher_qos.liveliness_lease_duration(dur); printf("liveliness_lease_duration: Duration(nsecs: %ld), ", dur.nanoseconds()); - } - catch (std::runtime_error &e) - { + } catch (std::runtime_error & e) { fprintf( - stderr, - "failed to create parametrize liveliness_lease_duration: '%s'\n", - e.what()); - } - catch (XmlRpc::XmlRpcException &e) - { + stderr, + "failed to create parametrize liveliness_lease_duration: '%s'\n", + e.what()); + } catch (XmlRpc::XmlRpcException & e) { fprintf( - stderr, - "failed to create parametrize liveliness_lease_duration: '%s'\n", - e.getMessage().c_str()); + stderr, + "failed to create parametrize liveliness_lease_duration: '%s'\n", + e.getMessage().c_str()); } } - } - else - { + } else { fprintf( - stderr, - "QoS parameters could not be read\n"); // TODO: clearer message + stderr, + "QoS parameters could not be read\n"); // TODO: clearer message } printf(")"); @@ -337,22 +283,19 @@ int main(int argc, char * argv[]) topic_name.c_str(), type_name.c_str()); try { - if (topics[i].hasMember("qos")) - { + if (topics[i].hasMember("qos")) { printf("Setting up QoS for '%s': ", topic_name.c_str()); auto qos_settings = qos_from_params(topics[i]["qos"]); printf("\n"); ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( ros1_node, ros2_node, "", type_name, topic_name, queue_size, qos_settings); all_handles.push_back(handles); - } - else - { + } else { ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge( ros1_node, ros2_node, "", type_name, topic_name, queue_size); all_handles.push_back(handles); } - + } catch (std::runtime_error & e) { fprintf( stderr, @@ -502,4 +445,4 @@ int main(int argc, char * argv[]) } return 0; -} \ No newline at end of file +} From 4796dcfb9a15f35682b59d68656b791c45642c54 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Tue, 19 Oct 2021 09:34:56 +0200 Subject: [PATCH 12/14] Fix cpplint formatting In 2 cases the formatting conflicts with what uncrustify wants; // NOLINT got rid of the complaints of cpplint, favouring uncrustify Signed-off-by: Loy van Beek --- src/parameter_bridge.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index a2996e66..2eb33b2f 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -12,6 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include + #include #include @@ -27,7 +29,6 @@ // include ROS 2 #include "rclcpp/rclcpp.hpp" -#include #include "ros1_bridge/bridge.hpp" @@ -57,7 +58,8 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } else { fprintf( stderr, - "invalid value for 'history': '%s', allowed values are 'keep_all', 'keep_last' (also requires 'depth' to be set)\n", + "invalid value for 'history': '%s', allowed values are 'keep_all'," + "'keep_last' (also requires 'depth' to be set)\n", history.c_str()); } } @@ -163,18 +165,19 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) liveliness_str == "liveliness_system_default") { liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT; - } else if (liveliness_str == "LIVELINESS_AUTOMATIC" || + } else if (liveliness_str == "LIVELINESS_AUTOMATIC" || // NOLINT liveliness_str == "liveliness_automatic") { liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC; - } else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || + } else if (liveliness_str == "LIVELINESS_MANUAL_BY_TOPIC" || // NOLINT liveliness_str == "liveliness_manual_by_topic") { liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC; } else { fprintf( stderr, - "invalid value for 'liveliness': '%s', allowed values are LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n", + "invalid value for 'liveliness': '%s', allowed values are " + "LIVELINESS_{SYSTEM_DEFAULT, AUTOMATIC, MANUAL_BY_TOPIC}, upper or lower case\n", liveliness_str.c_str()); } @@ -220,7 +223,7 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } else { fprintf( stderr, - "QoS parameters could not be read\n"); // TODO: clearer message + "QoS parameters could not be read\n"); } printf(")"); @@ -295,7 +298,6 @@ int main(int argc, char * argv[]) ros1_node, ros2_node, "", type_name, topic_name, queue_size); all_handles.push_back(handles); } - } catch (std::runtime_error & e) { fprintf( stderr, From c078d34d3f06ced55795296790a7905879b2109e Mon Sep 17 00:00:00 2001 From: Loy Date: Mon, 22 Nov 2021 08:59:22 +0100 Subject: [PATCH 13/14] Clearer logging as suggested by code review Co-authored-by: Geoffrey Biggs Signed-off-by: Loy van Beek --- README.md | 4 ++-- src/parameter_bridge.cpp | 23 +++++++++++------------ 2 files changed, 13 insertions(+), 14 deletions(-) diff --git a/README.md b/README.md index e740418a..26398076 100644 --- a/README.md +++ b/README.md @@ -421,8 +421,8 @@ ros2 service call /add_two_ints example_interfaces/srv/AddTwoInts "{a: 1, b: 2}" If all is well, the output should contain `example_interfaces.srv.AddTwoInts_Response(sum=3)` ### Parametrizing Quality of Service -An advantage or ROS 2 over ROS 1 is the possibility to define different Quality of Service settings per topic. -The parameter bridge optionally allows for this as well: +An advantage of ROS 2 over ROS 1 is the possibility to define different Quality of Service settings per topic. +The parameter bridge optionally allows for this as well. For some topics, like `/tf_static` this is actually required, as this is a latching topic in ROS 1. In ROS 2 with the `parameter_bridge`, this requires that topic to be configured as such: diff --git a/src/parameter_bridge.cpp b/src/parameter_bridge.cpp index 2eb33b2f..ed95aac5 100644 --- a/src/parameter_bridge.cpp +++ b/src/parameter_bridge.cpp @@ -108,12 +108,12 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create parametrize deadline: '%s'\n", + "failed to parse deadline: '%s'\n", e.what()); } catch (XmlRpc::XmlRpcException & e) { fprintf( stderr, - "failed to create parametrize deadline: '%s'\n", + "failed to parse deadline: '%s'\n", e.getMessage().c_str()); } } @@ -128,19 +128,18 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create parametrize lifespan: '%s'\n", + "failed to parse lifespan: '%s'\n", e.what()); } catch (XmlRpc::XmlRpcException & e) { fprintf( stderr, - "failed to create parametrize lifespan: '%s'\n", + "failed to parse lifespan: '%s'\n", e.getMessage().c_str()); } } if (qos_params.hasMember("liveliness")) { if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) { - printf("liviness is an int\n"); try { auto liveliness = static_cast(qos_params["liveliness"]); ros2_publisher_qos.liveliness(static_cast(liveliness)); @@ -148,12 +147,12 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create parametrize liveliness: '%s'\n", + "failed to parse liveliness: '%s'\n", e.what()); } catch (XmlRpc::XmlRpcException & e) { fprintf( stderr, - "failed to create parametrize liveliness: '%s'\n", + "failed to parse liveliness: '%s'\n", e.getMessage().c_str()); } } else if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeString) { @@ -186,18 +185,18 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create parametrize liveliness: '%s'\n", + "failed to parse liveliness: '%s'\n", e.what()); } catch (XmlRpc::XmlRpcException & e) { fprintf( stderr, - "failed to create parametrize liveliness: '%s'\n", + "failed to parse liveliness: '%s'\n", e.getMessage().c_str()); } } else { fprintf( stderr, - "failed to create parametrize liveliness, parameter was not a string or int \n"); + "failed to parse liveliness, parameter was not a string or int \n"); } } @@ -211,12 +210,12 @@ rclcpp::QoS qos_from_params(XmlRpc::XmlRpcValue qos_params) } catch (std::runtime_error & e) { fprintf( stderr, - "failed to create parametrize liveliness_lease_duration: '%s'\n", + "failed to parse liveliness_lease_duration: '%s'\n", e.what()); } catch (XmlRpc::XmlRpcException & e) { fprintf( stderr, - "failed to create parametrize liveliness_lease_duration: '%s'\n", + "failed to parse liveliness_lease_duration: '%s'\n", e.getMessage().c_str()); } } From ec447709ef100efd7b434ec3e1be352b91738d39 Mon Sep 17 00:00:00 2001 From: Loy van Beek Date: Mon, 22 Nov 2021 09:04:33 +0100 Subject: [PATCH 14/14] Clarify keep_last vs keep_all setting for history Signed-off-by: Loy van Beek --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 26398076..9c0bf433 100644 --- a/README.md +++ b/README.md @@ -446,7 +446,7 @@ topics: type: std_msgs/msg/String queue_size: 1 qos: - history: keep_last # OR keep_last, then you can omit `depth` below + history: keep_last # OR keep_all, then you can omit `depth` parameter below depth: 10 # Only required when history == keep_last reliability: reliable # OR best_effort durability: transient_local # OR volatile