ros2 / rmw_fastrtps

Implementation of the ROS Middleware (rmw) Interface using eProsima's Fast RTPS.
Apache License 2.0
154 stars 117 forks source link

[ros2] QoS parameters can't be configured by parsing DEFAULT_FASTRTPS_PROFILES.xml #500

Closed yangli-flexiv closed 3 years ago

yangli-flexiv commented 3 years ago

QoS_github.zip

Bug report

Required Info:

Steps to reproduce issue

I have attached all the relevant files (well organized as a workspace) as a zip.

First, I created an xml file, DEFAULT_FASTRTPS_PROFILES.xml, to configure the QoS parameters for a publisher and subscriber node

<?xml version="1.0" encoding="UTF-8"?>
<dds xmlns="http://www.eprosima.com/XMLSchemas/fastRTPS_Profiles">
  <profiles>

    <publisher profile_name="publisher_profile" is_default_profile="true">
      <topic>
        <historyQos>
          <kind>KEEP_LAST</kind>
          <depth>1</depth>
        </historyQos>
      </topic>

      <qos> <!-- dataWriterQosPoliciesType -->
        <durability>
          <kind>VOLATILE</kind>
        </durability>
        <reliability>
          <kind>BEST_EFFORT</kind>
          <max_blocking_time>
            <sec>0</sec>
            <nanosec>1000000</nanosec>
          </max_blocking_time>
        </reliability>
      </qos>
      <historyMemoryPolicy>DYNAMIC</historyMemoryPolicy>
    </publisher>

    <subscriber profile_name="subscriber_profile" is_default_profile="true">
      <topic>
        <historyQos>
          <kind>KEEP_LAST</kind>
          <depth>1</depth>
        </historyQos>
      </topic>

      <qos> <!-- dataReaderQosPoliciesType -->
        <durability>
          <kind>VOLATILE</kind>
        </durability>
        <reliability>
          <kind>BEST_EFFORT</kind>
          <max_blocking_time>
            <sec>0</sec>
            <nanosec>1000000</nanosec>
          </max_blocking_time>
        </reliability>
      </qos>
      <historyMemoryPolicy>DYNAMIC</historyMemoryPolicy>
    </subscriber>

  </profiles>
</dds>

And I create a node of publisher, which inherits from rclcpp::Node, this is the same as the demo as in the official website:

https://index.ros.org/doc/ros2/Tutorials/Writing-A-Simple-Cpp-Publisher-And-Subscriber/

Then I want to print out the QoS parameters of this publisher to see whether the settings are the same as what I set in the above xml file. So I added a few lines of code to extract the information of QoS and print out. Below is the .cpp file:

#include <chrono>
#include <functional>
#include <memory>
#include <string>

#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"

using namespace std::chrono_literals;

/* This example creates a subclass of Node and uses std::bind() to register a
 * member function as a callback from the timer. */

class MinimalPublisher : public rclcpp::Node
{
public:
  MinimalPublisher()
      : Node("minimal_publisher"), count_(0)
  {
    publisher_ = this->create_publisher<std_msgs::msg::String>("topic", 10);
    timer_ = this->create_wall_timer(
        500ms, std::bind(&MinimalPublisher::timer_callback, this));

    rmw_qos_profile_t qos;
    auto rcl_publisher = publisher_->get_publisher_handle();
    const rmw_publisher_t *rmw_publisher = rcl_publisher_get_rmw_handle(rcl_publisher);
    rmw_publisher_get_actual_qos(rmw_publisher, &qos);

    std::cout << "qos.history = " << qos.history << std::endl;
    std::cout << "qos.depth = " << qos.depth << std::endl;
    std::cout << "qos.reliability = " << qos.reliability << std::endl;
    std::cout << "qos.durability = " << qos.durability << std::endl;
  }

private:
  void timer_callback()
  {
    auto message = std_msgs::msg::String();
    message.data = "Hello, world! " + std::to_string(count_++);
    RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str());
    publisher_->publish(message);
  }
  rclcpp::TimerBase::SharedPtr timer_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
  size_t count_;
};

int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  rclcpp::spin(std::make_shared<MinimalPublisher>());
  rclcpp::shutdown();
  return 0;
}

After I build the code, and source both setup.bash files of standard ros2 and my repo, I typed in the command in my terminal:

FASTRTPS_DEFAULT_PROFILES_FILE=DEFAULT_FASTRTPS_PROFILES.xml RMW_FASTRTPS_USE_QOS_FROM_XML=1 RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 run cpp_pubsub talker

The app can be executed, and I got the following prints:

qos.history = 1
qos.depth = 10
qos.reliability = 1
qos.durability = 2
...

Obviously, the depth (10) is different from what I set in the xml (1).

I tried to modify the label keyword (such as giving it garbage name ), running the binary will report the error, saying the file can't be parsed because of unknown keywords. Also, when I tried to change the name of the xml but keep the command in the terminal the same, when execute, it also report no such file can be found and parsed.

2021-01-12 12:07:28.552 [XMLPARSER Error] Error opening 'DEFAULT_FASTRTPS_=PROFILES.xml' -> Function loadXML
2021-01-12 12:07:28.552 [XMLPARSER Error] Error parsing 'DEFAULT_FASTRTPS_=PROFILES.xml' -> Function loadXMLFile

So I think the original xml file is correctly parsed.

I have tested my code in both eloquent and foxy of ros2 on Ubuntu 18.04 and Ubuntu 20.04, but the same error happens.

I don't know why the error happens. One thing I guess is I do the setting in the eprosima environment, as DEFAULT_FASTRTPS_PROFILES.xml is proposed by eprosima to configure QoS for publisher and subscriber, etc.; while I use the QoS API's in ros2 environment to print out. I am not sure whether information in DEFAULT_FASTRTPS_PROFILES.xml is correctly set to my publisher, though I believe it has been parsed.

Can anyway help me on this? Usually, what is the correct way to print the QoS information after one programmed DEFAULT_FASTRTPS_PROFILES.xml, to verify the values are correctly set to QoS parameters?

Thanks!

Expected behavior

Actual behavior

Additional information


Feature request

Feature description

Implementation considerations

EduPonz commented 3 years ago

Hi @yangli-flexiv ,

I'm afraid this is not a bug but rather a design characteristic. As stated in the README.md:

QoS set by rclcpp/rclpy are always honored. This means that setting any of them in the XML files has no effect, since they do not override what was used to create the publisher, subscription, service, or client.

In other words, the QoS setting that can be set using rclcpp/rclpy cannot be overridden with XML configuration. The affected QoS are (see rmw_qos_profile_t):

yangli-flexiv commented 3 years ago

Hi @EduPonz,

Thank you very much for your reply. If the XML can't override what the settings of QoS of publisher/subscriber, what is the point that we add an XML configuration file for QoS?

In the Example Section of README.md of the rmw_fastrtps github webpage,

https://github.com/ros2/rmw_fastrtps/tree/master

it does use an XML file to configure QoS properties of a publisher, and the command typed in the terminal

FASTRTPS_DEFAULT_PROFILES_FILE= RMW_FASTRTPS_USE_QOS_FROM_XML=1 RMW_IMPLEMENTATION=rmw_fastrtps_cpp ros2 run demo_nodes_cpp talker

also seems trying to change the QoS settings.

In the Section Full QoS configuration of README.md of the webpage https://github.com/ros2/rmw_fastrtps/tree/master , it is clearly stated the followings:

In order to modify the history memory policy or publication mode using XML files, environment variable RMW_FASTRTPS_USE_QOS_FROM_XML must be set to 1 (it is set to 0 by default).

So I think when I set RMW_FASTRTPS_USE_QOS_FROM_XML to be 1, QoS will be configured by the XML, right?

Can you explain more on this?

Thanks!

EduPonz commented 3 years ago

Hi @yangli-flexiv ,

Sorry if my previous answer was not clear enough.

If the XML can't override what the settings of QoS of publisher/subscriber, what is the point that we add an XML configuration file for QoS?

XML configuration can be used to override some QoS settings but not all of them. Specifically, those contained in rmw_qos_profile_t, which I listed above, cannot be overridden. However, those are just some of the QoS that can be set in Fast DDS. The rest of the QoS not contained in rmw_qos_profile_t can be configured using XML. I'd like to point you to Fast DDS' XML documentation for a complete a thorough description of everything that can be configured with XML files.

In the Section Full QoS configuration of README.md of the webpage https://github.com/ros2/rmw_fastrtps/tree/master , it is clearly stated the followings:

In order to modify the history memory policy or publication mode using XML files, environment variable RMW_FASTRTPS_USE_QOS_FROM_XML must be set to 1 (it is set to 0 by default).

So I think when I set RMW_FASTRTPS_USE_QOS_FROM_XML to be 1, QoS will be configured by the XML, right?

I think the misunderstanding comes from the name of the environment variable. Setting RMW_FASTRTPS_USE_QOS_FROM_XML=1 tells rmw_fastrtps that it should not override neither History Memory Policy nor Publication Mode. Instead, the value of those two QoS is left as stated in the XML, or as the default Fast DDS value in case the XML does not specify anything for them. However, RMW_FASTRTPS_USE_QOS_FROM_XML only affects those two QoS, so it cannot be used to override the setting from rmw_qos_profile_t. Actually, if you carefully look at the XML snippet, you'll see that the QoS specified there are just History Memory Policy and Publication Mode.

Re-reading the README.md, I think that what I explained is precisely what it's written. That being said, I wrote most of it; and I know about the actual behaviour, so I'm probably a little bias. In any case, if you think the README.md could be improved upon, or that some clarifications are needed, please do submit a PR to improve it. Sometimes these things are more clearly stated by people outside the project.

I hope my answer was satisfactory. If that is not the case, please write back and I'll be happy to help!

yangli-flexiv commented 3 years ago

Hi @EduPonz ,

Thank you very much! Sure, I am glad to contribute and will submit a MR for this repo.

Btw, for the link you shared with me Fast DDS' XML documentation, in the example section,

https://fast-dds.docs.eprosima.com/en/v2.1.0/fastdds/xml_configuration/example.html,

it has the options of historyQos (kind and depth), durability, and reliability in <data_writer profile_name="datawriter_profile_example">. Here data_writer is the same as publisher. These above QoS parameters are also in the element of rmw_qos_profile_t. As you said in the last reply, they can't be modified via XML. Then I get confused, if they can't be set in XML, why DEFAULT_FASTRTPS_PROFILES.xml provides these options?

The relevant pieces of code in the example XML is cropped below:

        <data_writer profile_name="datawriter_profile_example">
            <topic>
                <historyQos>
                    <kind>KEEP_LAST</kind>
                    <depth>20</depth>
                </historyQos>
            </topic>

            <qos> <!-- dataWriterQosPoliciesType -->
                <durability>
                    <kind>VOLATILE</kind>
                </durability>

                <reliability>
                    <kind>BEST_EFFORT</kind>
                    <max_blocking_time>
                        <sec>1</sec>
                        <nanosec>856000</nanosec>
                    </max_blocking_time>
                </reliability>

            </qos>

        </data_writer>
EduPonz commented 3 years ago

Hi @yangli-flexiv ,

Please bear in mind that Fast DDS is a library independent from ROS 2. In that sense, rmw_fastrtps is a wrapper around Fast DDS to enable the use of the library in the ROS 2 framework. In fact, Fast DDS is created and maintained by eProsima, whereas rmw_fastrtps is an Open Robotics project (although here at eProsima we make a significant contribution effort).

As said in Fast DDS documentation, XML files can be used to configured every QoS available, including those in rmw_qos_profile_t. However, rmw_fastrtps overrides whatever configuration specified in the XML for the QoS in rmw_qos_profile_t. This is because rmw_fastrtps (ROS 2) wants to always honor the QoS received from rcl.

To summarize:

You may want to take a look at #503 , where @briansoe66 attemps to clarify README.md.

hidmic commented 3 years ago

@EduPonz hmm, looking at fill_entity_qos_from_profile() implementation in rmw_fastrtps_shared_cpp makes me think that any QoS set via XML will be taken into account if a *_SYSTEM_DEFAULT policy is used. Or at least that's the intent.

EduPonz commented 3 years ago

Yes, you're right! Then it goes like:

clalancette commented 3 years ago

I think #506 clarifies this, so I'm going to close this issue out. Feel free to reopen if you feel there is more to do here.