Add gazebo_ros::QoS class

[jacobperron]

Connects to #1079

This PR adds a new class that parses SDF for ROS 2 QoS settings that plugins can use when creating publishers and subscriptions.

Here's an example of valid SDF (from one of the tests):

<?xml version='1.0' ?>
<sdf version='1.6'>
  <world name='default'>
    <plugin name='node_1' filename='libnode_name.so'>
      <ros>
        <qos>
          <topic name="foo">
            <publisher>
              <reliability>best_effort</reliability>
              <durability>transient_local</durability>
              <history>keep_all</history>
              <deadline>123</deadline> <!-- milliseconds -->
              <lifespan>42</lifespan> <!-- milliseconds -->
              <liveliness>manual_by_topic</liveliness>
              <liveliness_lease_duration>1000</liveliness_lease_duration> <!-- milliseconds -->
            </publisher>
            <subscription>
              <reliability>reliable</reliability>
              <durability>volatile</durability>
              <history depth="3">keep_last</history>
            </subscription>
          </topic>
        </qos>
      </ros>
    </plugin>
  </world>
</sdf>

The new <qos> tag can contain zero or more <topic> tags. Each topic tag must have a name attribute.

The implementation will look for optional <publisher> and <subscription> tags, which can contain any number of tags corresponding to all of the QoS settings supported by ROS 2.

A description of QoS settings can be found here:

• http://design.ros2.org/articles/qos.html
• http://design.ros2.org/articles/qos_deadline_liveliness_lifespan.html

One limitation of the current approach is that all subscriptions (or publishers) for a given topic will use the same QoS overrides. I don't think there's a way to differentiate them in ROS 2, but I also don't think this will be an issue for any of the plugins in this repository.

The choice to use milliseconds for all duration values is arbitrary. If there is a more preferable unit, it's easy to change.

[jacobperron]

Here is a follow-up PR that makes use of the new gazebo_ros::QoS object in gazebo_plugins: #1092

[ivanpauno]

Great work @jacobperron !!

I've left some minor comments

[jacobperron]

I don't think the current implementation will work with remapped topic names. I'm going to look into it and add some tests.

[ivanpauno]

I don't think the current implementation will work with remapped topic names. I'm going to look into it and add some tests.

Great point.
I was going to ask that question in my first review, and then I completely forgot 😂.

IMO, remapping should be done before querying the qos profile.
i.e. I expect a SDF with both remapping rules and QoS settings to look like:

<ros>
     <remapping>topic:=my_topic</remapping>
</ros>
 <qos>
    <topic name='my_topic'>
        ...
    </topic>
</qos>

For that, I think we need to apply remapping rules/topic name expansion in get_*_qos().

P.S.: Shouldn't qos tag be a child of ros tag?

[jacobperron]

I agree with your assessment. I'll take a look at implementing it now.

P.S.: Shouldn't qos tag be a child of ros tag?

Yes, when the SDF is parsed here in gazebo_ros::Node:

gazebo_ros_pkgs/gazebo_ros/src/node.cpp

Line 105 in 728bb0e

node->qos_ = gazebo_ros::QoS(sdf);

The sdf is a <ros> element.

I'll update the example in this PR description.

For unit testing gazebo_ros::QoS, it's not strictly required.

[jacobperron]

@ivanpauno Here's my attempt at supporting remapping: 9e753f5

I don't believe there's a way to get the topic remappings from rclcpp, so I'm making the necessary rcl function calls.

[ivanpauno]

@jacobperron By mistake I did my comments in commit 9e753f5, if you open it you can see the comments in their context.

It would be nice to add a test were a namespace is specified in the sdf.
So, it's clear how both expansion and remapping interact with this new feature.

[ivanpauno]

LGTM!!

[scpeters]

@chapulina I'm not sure if we're using the PIMPL pattern in gazebo_ros_pkgs yet, but I'm wondering if this new class is a good candidate for it, since it has lots of private member variables

[chapulina]

Thank you for catching this, @scpeters . Indeed we're using PIMPL for the ROS 2 version of gazebo_ros_pkgs, as described on the Contribution guide.

[jacobperron]

It would be nice to add a test were a namespace is specified in the sdf.

I've updated the test to include a node namespace: 19469da

[ivanpauno]

@jacobperron @scpeters is this ready to go in?

[jacobperron]

@ivanpauno I think the point about considering to use a pimpl pattern is something we should consider (#1091 (comment)).

[jacobperron]

I rebased on ros2 to get changed needed for compilation on Ubuntu Focal (#1093).

[ivanpauno]

@scpeters @jacobperron I finally figured out the remaining build problems before closing.
I've introduced some linter failures that I will fix tomorrow, but you can already take a look to the last two commits (1eb881a, 3b8d0f5).

[jacobperron]

The latest changes LGTM 👍

[chapulina]

I haven't tried running it, but I skimmed through the code and it looks good to me.

Besides those 2 comments, I'd also ask if it would be possible to use the new <qos> elements in one of the example worlds here or here, or create a new example world to demonstrate the feature.

[chapulina]

Is this change needed?

[ivanpauno]

Valid namespaces should be always absolute (contrary to names, which can be relative/private/absolute).

I guess @jacobperron did that change because the namespace is being passed to the new QoS class, and some of the API it uses may be doing namespace validation.

@jacobperron can you confirm if that was the case?

[chapulina]

Got it. Since we started going down this road on this PR, do you think we should also do some checking on line 58 in case the user passes an invalid namespace? Or maybe we prepend the / for them in case it's missing?

[jacobperron]

@ivanpauno is exactly right. Without this change, I was seeing runtime errors from some rcl / rclcpp API calls (though I can't recall exactly which ones).

I guess if a user passes an invalid namespace, they will see similar runtime errors. I think the validation and error message from the underlying layers is enough (it also is nice to keep it encapsulated there in case the naming requirement changes for some reason).

[chapulina]

Got it, makes sense. I'm just wondering why we haven't seen problems so far with this default value, and whether this change is backwards compatible. But you peeps know best about these details 😉

[chapulina]

I think this may have messed something up 🤔 #1116

[ivanpauno]

Doesn't seem related to me.
That default value is being immediately overridden by the value in the SDF if it exists

gazebo_ros_pkgs/gazebo_ros/src/node.cpp

Lines 57 to 59 in 57732fe

	if (sdf->HasElement("namespace")) {
	ns = sdf->GetElement("namespace")->Get<std::string>();
	}

.

[ivanpauno]

Maybe, it's related to this

gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_factory.cpp

Lines 231 to 235 in 57732fe

	// Walk recursively through the entire file, and add <ros><namespace> tags to all plugins
	auto robot_namespace = req->robot_namespace;
	if (!robot_namespace.empty()) {
	AddNamespace(entity_elem, robot_namespace);
	}

gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_factory.cpp

Lines 354 to 355 in 57732fe

	// Set namespace
	ns_elem->Set<std::string>(_robot_namespace);

?

According to grep that's the only place where we set the namespace SDF tag.

[chapulina]

Nice catch! @ahcorde , are you spawning your robot through the factory? In my case I was.

[ahcorde]

#1117

[chapulina]

I know both <plugin><qos> and <plugin><ros><qos> are supported, but I guess the latter is the recommended one (based on documentation). How about using that on most tests? Just so people looking have a good example to follow.

[ivanpauno]

Done in 7a5188a

[ivanpauno]

Besides those 2 comments

I've addressed/replied to the commets.

I'd also ask if it would be possible to use the new elements in one of the example worlds here or here, or create a new example world to demonstrate the feature.

I would prefer to do that in a follow up if you don't mind.
Feel free to create a ticket and assign it to me.

[ivanpauno]

@chapulina is this ready to go in?

[chapulina]

@chapulina is this ready to go in?

I haven't tried it out but looking at the code I don't see anything weird. Could you peeps just run CI before merging? Instructions here: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-CI#next-turtle

[ivanpauno]

I haven't tried it out but looking at the code I don't see anything weird. Could you peeps just run CI before merging? Instructions here: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-CI#next-turtle

Thanks for the instructions! I didn't know how to run CI here.

CI:

• Linux

[chapulina]

Thanks for triggering CI. I see all QoS tests passed. I don't think the flake8 failures are this PR's fault.

All other tests failing with:

[Err] [Master.cc:95] EXCEPTION: Unable to start server[bind: Address already in use]. There is probably another Gazebo process running.

are being caused by some earlier test not being properly terminated and leaving Gazebo running. I think it's because of test_entity_spawner.test.py, see the testing adventure here: #1033 (comment). Again, not this PR's fault.

[ivanpauno]

Thanks for triggering CI. I see all QoS tests passed. I don't think the flake8 failures are this PR's fault.

I saw the flake8 failures locally, and they are also reproducible in ros2 branch.

All other tests failing with:

[Err] [Master.cc:95] EXCEPTION: Unable to start server[bind: Address already in use]. There is probably another Gazebo process running.

are being caused by some earlier test not being properly terminated and leaving Gazebo running. > I think it's because of test_entity_spawner.test.py, see the testing adventure here: #1033 (comment). Again, not this PR's fault.

I experienced that locally too 😂.
Killing the gazebo server running in the background before re-running the tests solved the issue (we should probably have a script in CI machines to kill it).

---

@chapulina I don't have write access here, please merge if it's ready.
Thanks!!

[jacobperron]

I can rebase the connected PR.

[chapulina]

we should probably have a script in CI machines to kill it

Yeah I think this could be built into the test cases themselves to kill any running gzservers in between tests. The downside is if someone is running another simulation on the same machine and that gets killed by mistake 😅

[srushtibobade]

@jacobperron
Thanks for all the good work!!
where should this XML file be located/ or added to the project?

[ivanpauno]

this can just be added in a normal gazebo sdf world file, the new qos parameters can be applied to gazebo_ros_pkgs plugins as shown in the first post #1091 (comment)