Simulation time issue (Fast-RTPS middleware)

[shumov-ag]

Bug report

Hi. There is a problem with simulation time since #1556 was merged.
I can see the problem when I use rmw_fastrtps_cpp as rmw_implementation and rclcpp from galactic branch.

The problem is that two different executors (main and local) are working with the same instance of guard condition simultaneously. But rmw_fastrtps_cpp does not support this. Only one of these executors will be process rcl_trigger_guard_condition() call, and second executor will not.

This leads to that TimeSource::clock_cb() is never been called in some scenarios (hardly reproduced on small examples)

Required Info:

• Operating System:
  • Ubuntu 20.04
• Installation type:
  • from source
• Version or commit hash:
  • galactic
• DDS implementation:
  • Fast-RTPS
• Client library (if applicable):
  • rclcpp

Steps to reproduce issue

1. Use rclcpp from galactic branch
2. Use rmw_fastrtps from galactic branch
3. Use a small patch for rmw_fastrtps from this commit shumov-ag/rmw_fastrtps@1f35b10
4. Use RMW_IMPLEMENTATION=rmw_fastrtps_cpp

#include <chrono>
#include <rclcpp/rclcpp.hpp>
#include <rosgraph_msgs/msg/clock.hpp>

using namespace std::chrono_literals;

class ClockNode : public rclcpp::Node
{
public:
  ClockNode()
  : Node("ClockNode")
  {
    publisher_ = this->create_publisher<rosgraph_msgs::msg::Clock>(
      "/clock", rclcpp::QoS(rclcpp::KeepLast(1)).best_effort());

    timer_ = this->create_wall_timer(1s, [this] {
      RCLCPP_INFO_STREAM(this->get_logger(), "tick");
      rosgraph_msgs::msg::Clock clock;
      clock.clock = rclcpp::Time(ctr_, 0);
      publisher_->publish(clock);
      ++ctr_;
    });
  }

private:
  rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr publisher_;
  rclcpp::TimerBase::SharedPtr timer_;
  size_t ctr_ = 0;
};

class CallbackNode : public rclcpp::Node
{
public:
  CallbackNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
  : Node("CallbackNode", options)
  {
    this->set_parameter(rclcpp::Parameter("use_sim_time", true));
    timer_ = rclcpp::create_timer(this, get_clock(), 1s, [this] {
      RCLCPP_INFO_STREAM(this->get_logger(), "tock");
    });
  }

private:
  rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
  rclcpp::init(argc, argv);
  rclcpp::executors::SingleThreadedExecutor executor;

  rclcpp::NodeOptions no;
  //no.use_clock_thread(false);   // good
  no.use_clock_thread(true);      // bad

  auto clock_node = std::make_shared<ClockNode>();
  auto callback_node = std::make_shared<CallbackNode>(no);

  executor.add_node(clock_node);
  executor.add_node(callback_node);
  executor.spin();

  rclcpp::shutdown();
  return 0;
}

Expected behavior

[INFO] [1643138932.701344523] [ClockNode]: tick
[INFO] [1643138933.701343771] [ClockNode]: tick
[INFO] [1643138934.701368588] [ClockNode]: tick
[INFO] [1643138934.701599376] [CallbackNode]: tock
[INFO] [1643138935.701367510] [ClockNode]: tick
[INFO] [1643138935.701575949] [CallbackNode]: tock
[INFO] [1643138936.701381387] [ClockNode]: tick
[INFO] [1643138936.701601377] [CallbackNode]: tock

Actual behavior

terminate called after throwing an instance of 'std::runtime_error'
  what():  The guard condition had already been attached
Aborted

Additional information

[fujitatomoya]

@shumov-ag thanks for the contribution.

let me confirm this problem. I think this is bug in rmw_fastrtps, if i am not mistaken. i think rmw_fastrtps should check and see if it needs to reattach the condition variable in the implementation.

[fujitatomoya]

CC: @MiguelCompany

[shumov-ag]

Should be fixed by ros2/rmw_fastrtps#619