Skip to content

Commit

Permalink
Parametrize Quality of Service in parameter_bridge. (ros2#331)
Browse files Browse the repository at this point in the history
* Extend create_bidirectional_bridge to take qos param for ROS2 publisher

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Busy setting up a way to read QoS parameters from ROS1 params

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Parse history qos params

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Call qos_from_params when setting up topics

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Configure deadline, lifespan, liveliness_lease_durations

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Configure liveliness

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Add some basic debug text

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Print the QoS settings to stdout when setting them up

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Catch XmlRpc::XmlRpcExceptions when constructing QoS from parameters

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Parse liveliness as either int enum value or upper/lower case string representation of liveliness enum values

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Fix formatting with uncrustify

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* 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 <loy.vanbeek@mojin-robotics.de>

* Clearer logging as suggested by code review

Co-authored-by: Geoffrey Biggs <gbiggs@killbots.net>
Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

* Clarify keep_last vs keep_all setting for history

Signed-off-by: Loy van Beek <loy.vanbeek@mojin-robotics.de>

Co-authored-by: Geoffrey Biggs <gbiggs@killbots.net>
  • Loading branch information
2 people authored and hsd-dev committed Dec 9, 2022
1 parent ae614dc commit b8f8a4f
Show file tree
Hide file tree
Showing 6 changed files with 290 additions and 3 deletions.
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
44 changes: 44 additions & 0 deletions README.md
Original file line number Diff line number Diff line change
Expand Up @@ -458,3 +458,47 @@ source ${ROS2_INSTALL_PATH}/setup.bash
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 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:

```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_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
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.
10 changes: 10 additions & 0 deletions include/ros1_bridge/bridge.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -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_
1 change: 1 addition & 0 deletions package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,7 @@
<build_depend>rcutils</build_depend>
<build_depend>rmw_implementation_cmake</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>xmlrpcpp</build_depend>

<buildtool_export_depend>pkg-config</buildtool_export_depend>

Expand Down
24 changes: 24 additions & 0 deletions src/bridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
213 changes: 210 additions & 3 deletions src/parameter_bridge.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#include <xmlrpcpp/XmlRpcException.h>

#include <list>
#include <string>

Expand All @@ -30,6 +32,202 @@

#include "ros1_bridge/bridge.hpp"

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<std::string>(qos_params["history"]);
printf("history: ");
if (history == "keep_all") {
ros2_publisher_qos.keep_all();
printf("keep_all, ");
} else if (history == "keep_last") {
if (qos_params.hasMember("depth")) {
auto depth = static_cast<int>(qos_params["depth"]);
ros2_publisher_qos.keep_last(depth);
printf("keep_last(%i), ", 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 requires 'depth' to be set)\n",
history.c_str());
}
}

if (qos_params.hasMember("reliability")) {
auto reliability = static_cast<std::string>(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 {
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<std::string>(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 {
fprintf(
stderr,
"invalid value for 'durability': '%s', allowed values are 'best_effort', 'volatile'\n",
durability.c_str());
}
}

if (qos_params.hasMember("deadline")) {
try {
rclcpp::Duration dur = rclcpp::Duration(
static_cast<int>(qos_params["deadline"]["secs"]),
static_cast<int>(qos_params["deadline"]["nsecs"]));
ros2_publisher_qos.deadline(dur);
printf("deadline: Duration(nsecs: %ld), ", dur.nanoseconds());
} catch (std::runtime_error & e) {
fprintf(
stderr,
"failed to parse deadline: '%s'\n",
e.what());
} catch (XmlRpc::XmlRpcException & e) {
fprintf(
stderr,
"failed to parse deadline: '%s'\n",
e.getMessage().c_str());
}
}

if (qos_params.hasMember("lifespan")) {
try {
rclcpp::Duration dur = rclcpp::Duration(
static_cast<int>(qos_params["lifespan"]["secs"]),
static_cast<int>(qos_params["lifespan"]["nsecs"]));
ros2_publisher_qos.lifespan(dur);
printf("lifespan: Duration(nsecs: %ld), ", dur.nanoseconds());
} catch (std::runtime_error & e) {
fprintf(
stderr,
"failed to parse lifespan: '%s'\n",
e.what());
} catch (XmlRpc::XmlRpcException & e) {
fprintf(
stderr,
"failed to parse lifespan: '%s'\n",
e.getMessage().c_str());
}
}

if (qos_params.hasMember("liveliness")) {
if (qos_params["liveliness"].getType() == XmlRpc::XmlRpcValue::TypeInt) {
try {
auto liveliness = static_cast<int>(qos_params["liveliness"]);
ros2_publisher_qos.liveliness(static_cast<rmw_qos_liveliness_policy_t>(liveliness));
printf("liveliness: %i, ", static_cast<int>(liveliness));
} catch (std::runtime_error & e) {
fprintf(
stderr,
"failed to parse liveliness: '%s'\n",
e.what());
} catch (XmlRpc::XmlRpcException & e) {
fprintf(
stderr,
"failed to parse 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;
auto liveliness_str = static_cast<std::string>(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" || // NOLINT
liveliness_str == "liveliness_automatic")
{
liveliness = rmw_qos_liveliness_policy_t::RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
} 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",
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 parse liveliness: '%s'\n",
e.what());
} catch (XmlRpc::XmlRpcException & e) {
fprintf(
stderr,
"failed to parse liveliness: '%s'\n",
e.getMessage().c_str());
}
} else {
fprintf(
stderr,
"failed to parse liveliness, parameter was not a string or int \n");
}
}

if (qos_params.hasMember("liveliness_lease_duration")) {
try {
rclcpp::Duration dur = rclcpp::Duration(
static_cast<int>(qos_params["liveliness_lease_duration"]["secs"]),
static_cast<int>(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) {
fprintf(
stderr,
"failed to parse liveliness_lease_duration: '%s'\n",
e.what());
} catch (XmlRpc::XmlRpcException & e) {
fprintf(
stderr,
"failed to parse liveliness_lease_duration: '%s'\n",
e.getMessage().c_str());
}
}
} else {
fprintf(
stderr,
"QoS parameters could not be read\n");
}

printf(")");
return ros2_publisher_qos;
}

int main(int argc, char * argv[])
{
Expand Down Expand Up @@ -89,9 +287,18 @@ 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")) {
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 {
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,
Expand Down

0 comments on commit b8f8a4f

Please sign in to comment.