Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Add hardcoded QoS for the /tf_static topic for parameter bridge #304

Closed
wants to merge 3 commits into from
Closed

Add hardcoded QoS for the /tf_static topic for parameter bridge #304

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
ca6f59b
Add API to create bidirectional bridge with rclcpp QoS
lFatality Dec 27, 2020
da90da9
Add hardcoded QoS for /tf_static topic in parameter bridge
lFatality Dec 27, 2020
1fc58b9
Let ROS1 pub be latching if bridging /tf_static topic
lFatality Mar 27, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 13 additions & 0 deletions include/ros1_bridge/bridge.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -104,6 +104,7 @@ create_bridge_from_2_to_1(
const std::string & ros1_type_name,
const std::string & ros1_topic_name,
size_t publisher_queue_size,
bool publisher_latching = false,
rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr);

Bridge2to1Handles
Expand All @@ -116,6 +117,7 @@ create_bridge_from_2_to_1(
const std::string & ros1_type_name,
const std::string & ros1_topic_name,
size_t publisher_queue_size,
bool publisher_latching = false,
rclcpp::PublisherBase::SharedPtr ros2_pub = nullptr);

BridgeHandles
Expand All @@ -127,6 +129,17 @@ 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,
bool ros1_publisher_latching,
const rclcpp::QoS & ros2_publisher_qos);

} // namespace ros1_bridge

#endif // ROS1_BRIDGE__BRIDGE_HPP_
33 changes: 31 additions & 2 deletions src/bridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,7 @@ create_bridge_from_2_to_1(
const std::string & ros1_type_name,
const std::string & ros1_topic_name,
size_t publisher_queue_size,
bool publisher_latching,
rclcpp::PublisherBase::SharedPtr ros2_pub)
{
auto subscriber_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(subscriber_queue_size));
Expand All @@ -88,6 +89,7 @@ create_bridge_from_2_to_1(
ros1_type_name,
ros1_topic_name,
publisher_queue_size,
publisher_latching,
ros2_pub);
}

Expand All @@ -101,11 +103,12 @@ create_bridge_from_2_to_1(
const std::string & ros1_type_name,
const std::string & ros1_topic_name,
size_t publisher_queue_size,
bool publisher_latching,
rclcpp::PublisherBase::SharedPtr ros2_pub)
{
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, publisher_latching);

auto ros2_sub = factory->create_ros2_subscriber(
ros2_node, ros2_topic_name, subscriber_qos, ros1_pub, ros2_pub);
Expand Down Expand Up @@ -135,7 +138,33 @@ create_bidirectional_bridge(
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);
false, handles.bridge1to2.ros2_publisher);
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,
bool ros1_publisher_latching,
const rclcpp::QoS & ros2_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, ros2_publisher_qos.get_rmw_qos_profile().depth, ros2_type_name,
topic_name, ros2_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,
ros1_publisher_latching, handles.bridge1to2.ros2_publisher);
return handles;
}

Expand Down
5 changes: 4 additions & 1 deletion src/dynamic_bridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -254,11 +254,14 @@ void update_bridge(
bridge.ros1_type_name = ros1_type_name;
bridge.ros2_type_name = ros2_type_name;

bool ros1_pub_latching = (topic_name == "/tf_static");
Copy link

@mechwiz mechwiz Dec 1, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
bool ros1_pub_latching = (topic_name == "/tf_static");
auto ros2_subscriber_qos = rclcpp::QoS(rclcpp::KeepLast(10));
bool ros1_pub_latching = false;
if (topic_name == "/tf_static") {
ros1_pub_latching = true;
ros2_subscriber_qos.keep_all();
ros2_subscriber_qos.transient_local();
}

I think there should also be ros2_subscriber_qos set here to transient_local (don't know if we need keep_all but put it there for now) since the ros2 subscriber might not come alive until after the /tf_static topic has been puslihed to and so will miss the message. This fixes it.


try {
bridge.bridge_handles = ros1_bridge::create_bridge_from_2_to_1(
ros2_node, ros1_node,
bridge.ros2_type_name, topic_name, 10,
Copy link

@mechwiz mechwiz Dec 1, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
bridge.ros2_type_name, topic_name, 10,
bridge.ros2_type_name, topic_name, ros2_subscriber_qos,

This should be updated based on my previous comment

bridge.ros1_type_name, topic_name, 10);
bridge.ros1_type_name, topic_name, 10,
ros1_pub_latching);
} catch (std::runtime_error & e) {
fprintf(
stderr,
Expand Down
11 changes: 10 additions & 1 deletion src/parameter_bridge.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -86,9 +86,18 @@ int main(int argc, char * argv[])
"with ROS 2 type '%s'\n",
topic_name.c_str(), type_name.c_str());

auto ros2_publisher_qos = rclcpp::QoS(rclcpp::KeepLast(queue_size));
bool ros1_publisher_latching = false;
if (topic_name == "/tf_static") {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I see how this is an improvement, thought it seems a little fragile. It won't set the QoS settings if the /tf_static has been remapped.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I share your concern about the tf_static being hardcoded but currently I also do not have a better solution. Maybe this could be solved with the approaches that were discussed in the issues that mentioned this PR?

ros1_publisher_latching = true;
ros2_publisher_qos.keep_all();
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this should set keep_all. Upstream uses a queue depth of 1 for broadcasters, or 100 for listeners. It seems reasonable to keep using KeepLast(queue_size).

https://github.com/ros2/geometry2/blob/4a6526b3e861f22639460750cd197b59ac1a100b/tf2_ros/include/tf2_ros/qos.hpp#L56

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think the bridge's ROS 2 subscriber would need to use transient_local() too, otherwise I think it won't get any old samples. Similarly the ROS 1 publisher would need to use latching.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hi there,
the settings were adopted from those used in #282 so they are equal to the dynamic bridge.
I assumed the history policy was set to keep_all as the messages on the tf_static topic are often only published once as far as I know so any late-joining subscriber would need to get all the messages transported on the topic. But I could be wrong here. If I change the setting, should I also change it in the dynamic bridge?

If I understand the QoS documentation (https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html) correctly I think changing the ROS2 subscriber to use transient_local would have the effect that the sub would only accept messages from publishers that are publishing with a transient_local policy, too. It would ignore volatile publishers. So in order for the sub to receive old samples, I don't think it needs to use transient_local. Instead the ROS2 publishers that publish the static TFs would need to be transient_local. Correct me if I'm mistaken here, the QoS settings are still pretty new to me.

I agree that the ROS1 publisher would need to use latching. This is also not done in the dynamic bridge at the moment. I added a commit that activates ROS1 pub latching if the topic to bridge is tf_static (for both the dynamic & static bridge).

ros2_publisher_qos.transient_local();
}

try {
ros1_bridge::BridgeHandles handles = ros1_bridge::create_bidirectional_bridge(
ros1_node, ros2_node, "", type_name, topic_name, queue_size);
ros1_node, ros2_node, "", type_name, topic_name, queue_size, ros1_publisher_latching,
ros2_publisher_qos);
all_handles.push_back(handles);
} catch (std::runtime_error & e) {
fprintf(
Expand Down