[ros2] Add sim_time to gazebo_ros_init #794

kev-the-dev · 2018-08-07T23:34:14Z

Add a Throttler class to utils, something that can be reused in a few places
Add publishing sim time to /clock in gazebo_ros_init
- The time of a simulation starts at 0.
- Subscribers that have transient local durability specified in their QoS settings will be able to receive the previously-published time message(s) even if the simulation is paused (up to the queue depth of 10).

Usage:

$ ros2 launch gazebo_ros empty_world.launch.py

$ ros2 topic echo /clock
clock:
  sec: 7
  nanosec: 507000000

Wiki: https://github.com/ros-simulation/gazebo_ros_pkgs/wiki/ROS-2-Migration:-ROS-Clocks-and-sim-time

chapulina · 2018-08-09T17:51:09Z

gazebo_ros/include/gazebo_ros/gazebo_ros_clock.hpp

+  /// Called by Gazebo to load plugin.
+  /// \param[in] argc Argument count.
+  /// \param[in] argv Argument values.
+  void Load(int argc, char ** argv);


We could add override

chapulina · 2018-08-09T17:51:33Z

gazebo_ros/src/gazebo_ros_clock.cpp

+  static const gazebo::common::Time DEFAULT_PUBLISH_PERIOD;
+
+  void OnWorldCreated(std::string _world_name);
+  void PublishSimTime();


Missing documentation

chapulina · 2018-08-09T17:52:49Z

gazebo_ros/src/gazebo_ros_clock.cpp

+void GazeboRosClock::Load(int argc, char ** argv)
+{
+  (void) argc;
+  (void) argv;


Should we try to pass these to rclcpp::init in case it hasn't been initialized yet?

chapulina · 2018-08-09T17:53:17Z

gazebo_ros/include/gazebo_ros/gazebo_ros_clock.hpp

+
+class GazeboRosClockPrivate;
+
+/// Initializes ROS with the system arguments passed to Gazebo (i.e. calls rclcpp::init).


Documentation from a different plugin

chapulina · 2018-08-09T17:55:01Z

gazebo_ros/src/gazebo_ros_clock.cpp

+  gazebo::common::Time publish_period_;
+  static const gazebo::common::Time DEFAULT_PUBLISH_PERIOD;
+
+  void OnWorldCreated(std::string _world_name);


Make input a const reference

chapulina · 2018-08-09T17:59:59Z

gazebo_ros/src/gazebo_ros_clock.cpp

+
+  clock_pub_ = rosnode_->create_publisher<rosgraph_msgs::msg::Clock>("/clock");
+  world_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(
+    std::bind(&GazeboRosClockPrivate::PublishSimTime, this));


You could connect to a callback that receives gazebo::common::UpdateInfo, so you can get the times from there and don't need to keep a pointer to the world. This should save the whole logic of waiting for a world to be created.

See an example here.

dhood · 2018-08-09T20:14:34Z

I don't know that it makes sense to have such a standalone plugin for a ROS clock, I would think that it goes well with gazebo_ros_init. I have seen that both approaches have been mentioned on #779 but not a discussion of pros/cons.

I'd say it makes sense that gazebo_ros_init sets up a reasonable amount of the default Gazebo-ROS integration, which includes clock publishing. No need to separate them if they will always be used together (I've never heard of anyone not wanting the clock to be published, and even if there's a use case for that, it might not warrant two separate plugins). What's the motivation for keeping them separate?

dhood

exciting we will have sim time in ROS 2 soon!

dhood · 2018-08-10T01:15:03Z

gazebo_ros/test/test_sim_time.cpp

+TEST_F(TestSimTime, TestClock)
+{
+  auto node = std::make_shared<rclcpp::Node>("my_node");
+  rclcpp::Clock clock;


don't think this is what you want here (an rclcpp::Clock might not be using sim time). You'd be wanting to check node->now() (which will use ROS time if the use_sim_time parameter has been set). (When you do that, please set the use_sim_time parameter explicitly on the node and don't rely on ros2/rclcpp#514)

dhood · 2018-08-10T01:16:17Z

gazebo_ros/test/test_sim_time.cpp

+  auto second_msg =
+    gazebo_ros::get_message_or_timeout<ClockMsg>(node, "/clock", rclcpp::Duration(5, 0));
+  ASSERT_NE(second_msg, nullptr);
+  auto second_msg_time = clock.now();


I imagine you also wanted to check that the time roughly matches what was published on the clock topic

dhood · 2018-08-10T01:19:26Z

gazebo_ros/src/gazebo_ros_init.cpp

+  if (impl_->rosnode_->get_parameter("publish_rate", rate_param)) {
+    if (rclcpp::ParameterType::PARAMETER_DOUBLE == rate_param.get_type()) {
+      impl_->throttler_ = Throttler(gazebo::common::Time(1.0 / rate_param.as_double()));
+    } else if (rclcpp::ParameterType::PARAMETER_INTEGER != rate_param.get_type()) {


should this be ==?

dhood · 2018-08-10T01:22:07Z

gazebo_ros/src/gazebo_ros_init.cpp

+{
+public:
+  GazeboRosInitPrivate();
+  gazebo_ros::Node::SharedPtr rosnode_;


recommend ros_node_ to match e.g.

gazebo_ros_pkgs/gazebo_plugins/src/gazebo_ros_joint_state_publisher.cpp

Line 58 in 4fe1967

gazebo_ros::Node::SharedPtr ros_node_;

dhood · 2018-08-10T01:26:32Z

gazebo_ros/src/utils.cpp

@@ -60,4 +60,30 @@ std::string SensorFrameID(const gazebo::sensors::Sensor & _sensor, const sdf::El
  return gazebo_ros::ScopedNameBase(_sensor.ParentName());
 }

+Throttler::Throttler(const gazebo::common::Time & _period)


I don't think Times should be use to represent durations (at least in ROS we have different types; Time is meant for time stamps). Just the rate constructor seems like it'd be fine

I agree the other constructor may be confusing. I'll continue to use time to store the period internally because Gazebo does not have a duration type

I would suggest instead to store the period as a double and compare doubles with doubles e.g. https://github.com/ros-simulation/gazebo_ros_pkgs/pull/793/files#diff-b9fc9fd135967672730a2b1c536338b7L71

dhood · 2018-08-10T01:46:11Z

gazebo_ros/src/gazebo_ros_init.cpp

+
+void GazeboRosInitPrivate::PublishSimTime(const gazebo::common::UpdateInfo & _info)
+{
+  if (!throttler_.IsReady(_info.realTime)) {return;}


@tfoote do you agree that specifying the rate of publishing to /clock wrt to real time is appropriate here? seems reasonable since then even if RTF is really low the clock will still be published regularly

There's not a real standard, but I believe that the publish rate wrt to realtime makes sense. It's mostly limited by the clock communication bandwidth requirments which are hardware based and thus connect to realtime.

dhood · 2018-08-10T01:49:13Z

gazebo_ros/test/test_utils.cpp

+  {
+    // Test frequency
+    Throttler t(2.0);
+    EXPECT_FALSE(t.IsReady(Time(0, 4E8)));


for the usual usage (with time stamps as an input), the first call to IsReady will be true because of the jump from 0. This isn't necessarily an issue, but want to check it's what we want since it's contrary to this specific test. If so would be good to document (and maybe make that behaviour consistent in all cases).

dhood · 2018-08-10T01:53:59Z

gazebo_ros/src/gazebo_ros_init.cpp

-  RCLCPP_INFO(rclcpp::get_logger("gazebo_ros_init"), "ROS has been initialized.");
+
+  impl_->rosnode_ = gazebo_ros::Node::Create("gazebo_ros_clock");
+  impl_->clock_pub_ = impl_->rosnode_->create_publisher<rosgraph_msgs::msg::Clock>("/clock");


@tfoote I think setting durability on the /clock publisher's QoS settings would be appropriate for late-subscribers when the simulation's paused. Would you agree?

Yes, adding durability would make sense so that a late starting node would get their time initialized.

added in 165cb30

interestingly, in ROS 1 it isn't published as latched:

gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.cpp

Line 221 in 6a82024

pub_clock_ = nh_->advertise<rosgraph_msgs::Clock>("/clock",10);

dhood · 2018-08-10T02:01:38Z

gazebo_ros/src/gazebo_ros_init.cpp

+}
+
+// By default publish at 10 HZ (100 millisecond period)
+const gazebo::common::Time GazeboRosInitPrivate::DEFAULT_PUBLISH_PERIOD = gazebo::common::Time(0,


(genuine question): how come we don't just initialise this above?

dhood · 2018-08-10T02:02:52Z

gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp

-  /// Called by Gazebo to load plugin.
-  /// \param[in] argc Argument count.
-  /// \param[in] argv Argument values.
+  // Documentation inherited
  void Load(int argc, char ** argv);


could add override (attribution: think this might have been mentioned before)

kev-the-dev · 2018-08-11T00:06:45Z

Rewrote test to actually test a node's sim time and sanity check on the /clock output.

dhood

looks pretty good to me now!

dhood · 2018-08-11T01:10:12Z

gazebo_ros/test/test_sim_time.cpp

+  // Time should go forward
+  EXPECT_GT(second_msg, first_msg);
+  // The message from the clock should be the node's time
+  EXPECT_EQ(first_msg_time, rclcpp::Time(first_msg->clock));


Seems like this is subject to a race condition (additional clock message may have been received by the node after first_msg_time was stored), so we will likely benefit from a tolerance here and below

I take back what I said, since spinning only happens in get_message_or_timeout. It's not possible that additional messages are received between storing first_msg and first_msg_time.

mikaelarguedas · 2018-08-13T20:16:23Z

@ros-pull-request-builder retest this please

noticed collapsed unresolved comments

dhood · 2018-08-21T01:24:14Z

I added example usage and a wiki page to the original post

j-rivero mentioned this pull request Aug 9, 2018

Port gazebo_ros_pkgs to ROS 2.0 #512

Closed

chapulina reviewed Aug 9, 2018

View reviewed changes

dhood self-requested a review August 9, 2018 18:33

kev-the-dev force-pushed the ros2-clock branch from 2e03079 to 6baf251 Compare August 10, 2018 01:03

kev-the-dev changed the base branch from ros2-utils to ros2 August 10, 2018 01:04

kev-the-dev changed the title ~~[ros2] Add gazebo_ros_clock~~ [ros2] Add sim_time to gazebo_ros_init Aug 10, 2018

dhood reviewed Aug 10, 2018

View reviewed changes

dhood reviewed Aug 11, 2018

View reviewed changes

dhood added the ros2 label Aug 17, 2018

Kevin Allen and others added 5 commits August 17, 2018 17:00

Add Throttler to gazebo_ros utilities

d5a6752

Add sim time to gazebo_ros_init

c317d18

Remove period constructor from Throttler

0104814

Improve sim time test

b5e75a3

Fix compilation in isolation for gazebo_ros_init

1959361

dhood force-pushed the ros2-clock branch from 2b1f8f1 to 1959361 Compare August 18, 2018 00:05

dhood added 2 commits August 17, 2018 17:18

Transient local durability for clock publisher

165cb30

Linter fixup

4f2ff6d

dhood previously approved these changes Aug 20, 2018

View reviewed changes

Document Throttler will return true on first call

bc1f710

dhood added 2 commits August 20, 2018 15:07

Store rate as double not Time

e87b52e

Import order improvements

7acdeb8

dhood force-pushed the ros2-clock branch from ae43d6d to 7acdeb8 Compare August 20, 2018 22:58

dhood approved these changes Aug 21, 2018

View reviewed changes

dhood merged commit fa8d749 into ros2 Aug 21, 2018

dhood deleted the ros2-clock branch August 21, 2018 01:26

chapulina mentioned this pull request Aug 21, 2018

[ros2] gazebo_ros_api_plugin split #779

Open

chapulina mentioned this pull request Aug 31, 2019

[ros2] Remove ported / deprecated gazebo_ros #989

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[ros2] Add sim_time to gazebo_ros_init #794

[ros2] Add sim_time to gazebo_ros_init #794

kev-the-dev commented Aug 7, 2018 •

edited by dhood

Loading

chapulina Aug 9, 2018

chapulina Aug 9, 2018

chapulina Aug 9, 2018

chapulina Aug 9, 2018

chapulina Aug 9, 2018

chapulina Aug 9, 2018

dhood commented Aug 9, 2018

dhood left a comment

dhood Aug 10, 2018

dhood Aug 10, 2018

dhood Aug 10, 2018

dhood Aug 10, 2018

dhood Aug 10, 2018

kev-the-dev Aug 10, 2018

dhood Aug 10, 2018 •

edited

Loading

dhood Aug 10, 2018

tfoote Aug 13, 2018

dhood Aug 10, 2018

dhood Aug 10, 2018

tfoote Aug 13, 2018

dhood Aug 20, 2018

dhood Aug 21, 2018 •

edited

Loading

dhood Aug 10, 2018

dhood Aug 10, 2018

kev-the-dev commented Aug 11, 2018

dhood left a comment

dhood Aug 11, 2018

dhood Aug 20, 2018

mikaelarguedas commented Aug 13, 2018

dhood commented Aug 21, 2018


		class GazeboRosClockPrivate;

		/// Initializes ROS with the system arguments passed to Gazebo (i.e. calls rclcpp::init).

[ros2] Add sim_time to gazebo_ros_init #794

[ros2] Add sim_time to gazebo_ros_init #794

Conversation

kev-the-dev commented Aug 7, 2018 • edited by dhood Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dhood commented Aug 9, 2018

dhood left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dhood Aug 10, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

dhood Aug 21, 2018 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kev-the-dev commented Aug 11, 2018

dhood left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mikaelarguedas commented Aug 13, 2018

dhood commented Aug 21, 2018

kev-the-dev commented Aug 7, 2018 •

edited by dhood

Loading

dhood Aug 10, 2018 •

edited

Loading

dhood Aug 21, 2018 •

edited

Loading