[ros2] Add sim_time to gazebo_ros_init

gazebo_ros/CMakeLists.txt

@@ -15,6 +15,7 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(gazebo_dev REQUIRED)
+find_package(geometry_msgs REQUIRED)
 
 include_directories(
   include
@@ -31,23 +32,28 @@ ament_target_dependencies(gazebo_ros_node
 )
 ament_export_libraries(gazebo_ros_node)
 
-add_library(gazebo_ros_init SHARED
-  src/gazebo_ros_init.cpp
+add_library(gazebo_ros_utils SHARED
+  src/utils.cpp
 )
-ament_target_dependencies(gazebo_ros_init
+ament_target_dependencies(gazebo_ros_utils
   "rclcpp"
   "gazebo_dev"
 )
-ament_export_libraries(gazebo_ros_init)
+ament_export_libraries(gazebo_ros_utils)
 
-add_library(gazebo_ros_utils SHARED
-  src/utils.cpp
+add_library(gazebo_ros_init SHARED
+  src/gazebo_ros_init.cpp
 )
-ament_target_dependencies(gazebo_ros_utils
+ament_target_dependencies(gazebo_ros_init
   "rclcpp"
   "gazebo_dev"
+  "geometry_msgs"
 )
-ament_export_libraries(gazebo_ros_utils)
+target_link_libraries(gazebo_ros_init
+  gazebo_ros_node
+  gazebo_ros_utils
+)
+ament_export_libraries(gazebo_ros_init)
 
 ament_export_include_directories(include)
 ament_export_dependencies(rclcpp)

gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp

@@ -15,10 +15,17 @@
 #ifndef GAZEBO_ROS__GAZEBO_ROS_INIT_HPP_
 #define GAZEBO_ROS__GAZEBO_ROS_INIT_HPP_
 
-
 #include <gazebo/common/Plugin.hh>
 
+#include <memory>
+
+namespace gazebo_ros
+{
+
+class GazeboRosInitPrivate;
+
 /// Initializes ROS with the system arguments passed to Gazebo (i.e. calls rclcpp::init).
+/// Also publishes the latest simtime to /clock
 class GazeboRosInit : public gazebo::SystemPlugin
 {
 public:
@@ -28,9 +35,12 @@ class GazeboRosInit : public gazebo::SystemPlugin
   /// Destructor
   virtual ~GazeboRosInit();
 
-  /// Called by Gazebo to load plugin.
-  /// \param[in] argc Argument count.
-  /// \param[in] argv Argument values.
-  void Load(int argc, char ** argv);
+  // Documentation inherited
+  void Load(int argc, char ** argv) override;
+
+private:
+  std::unique_ptr<GazeboRosInitPrivate> impl_;
 };
+
+}  // namespace gazebo_ros
 #endif  // GAZEBO_ROS__GAZEBO_ROS_INIT_HPP_

gazebo_ros/include/gazebo_ros/utils.hpp

@@ -17,6 +17,7 @@
 
 #include <gazebo/sensors/Noise.hh>
 #include <gazebo/sensors/Sensor.hh>
+#include <gazebo/common/Time.hh>
 
 #include <string>
 
@@ -55,5 +56,25 @@ std::string ScopedNameBase(const std::string & str);
 /// \return The string representing the tf frame of the sensor
 std::string SensorFrameID(const gazebo::sensors::Sensor & _sensor, const sdf::Element & _sdf);
 
+/// Helper class used to throttle something to a given rate
+class Throttler
+{
+public:
+  /// Create a throttler with a frequency
+  /// \param[in] _hz Frequency at which IsReady will return true, in hertz
+  explicit Throttler(const double _hz);
+  /// Check if the enough time has elapsed since the last time IsReady returned true
+  /// \param[in] _time The current time.
+  /// \return false if not enough time has elapsed from the last time IsReady was true
+  /// \return true if enough time has elapsed since the last success
+  bool IsReady(const gazebo::common::Time & _time);
+
+private:
+  /// The time between calls to @IsReady returning true
+  gazebo::common::Time period_;
+  /// The last time @IsReady returned true
+  gazebo::common::Time last_time_;
+};
+
 }  // namespace gazebo_ros
 #endif  // GAZEBO_ROS__UTILS_HPP_

gazebo_ros/package.xml

@@ -23,14 +23,14 @@
 
   <build_depend>rclcpp</build_depend>
   <build_depend>gazebo_dev</build_depend>
+  <build_depend>geometry_msgs</build_depend>
 
   <exec_depend>rclcpp</exec_depend>
   <exec_depend>gazebo_dev</exec_depend>
 
   <test_depend>ament_cmake_gtest</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
-  <build_depend>geometry_msgs</build_depend>
   <test_depend>std_msgs</test_depend>
 
   <export>

gazebo_ros/src/gazebo_ros_init.cpp

@@ -14,10 +14,32 @@
 
 #include "gazebo_ros/gazebo_ros_init.hpp"
 
-#include <rclcpp/rclcpp.hpp>
+#include <gazebo_ros/node.hpp>
+#include <gazebo_ros/utils.hpp>
+#include <gazebo_ros/conversions.hpp>
+#include <rosgraph_msgs/msg/clock.hpp>
 #include <gazebo/common/Plugin.hh>
 
+#include <memory>
+
+namespace gazebo_ros
+{
+
+class GazeboRosInitPrivate
+{
+public:
+  GazeboRosInitPrivate();
+  gazebo_ros::Node::SharedPtr ros_node_;
+  rclcpp::Publisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_pub_;
+  gazebo::event::ConnectionPtr world_update_event_;
+  gazebo_ros::Throttler throttler_;
+  static constexpr double DEFAULT_PUBLISH_FREQUENCY = 10.;
+
+  void PublishSimTime(const gazebo::common::UpdateInfo & _info);
+};
+
 GazeboRosInit::GazeboRosInit()
+: impl_(std::make_unique<GazeboRosInitPrivate>())
 {
 }
 
@@ -29,7 +51,50 @@ void GazeboRosInit::Load(int argc, char ** argv)
 {
   // Initialize ROS with arguments
   rclcpp::init(argc, argv);
-  RCLCPP_INFO(rclcpp::get_logger("gazebo_ros_init"), "ROS has been initialized.");
+
+  impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_clock");
+
+  // Offer transient local durability on the clock topic so that if publishing is infrequent (e.g.
+  // the simulation is paused), late subscribers can receive the previously published message(s).
+  rmw_qos_profile_t qos_profile = rmw_qos_profile_default;
+  qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  impl_->clock_pub_ = impl_->ros_node_->create_publisher<rosgraph_msgs::msg::Clock>(
+    "/clock",
+    qos_profile);
+
+  // Get publish rate from parameter if set
+  rclcpp::Parameter rate_param;
+  if (impl_->ros_node_->get_parameter("publish_rate", rate_param)) {
+    if (rclcpp::ParameterType::PARAMETER_DOUBLE == rate_param.get_type()) {
+      impl_->throttler_ = Throttler(rate_param.as_double());
+    } else if (rclcpp::ParameterType::PARAMETER_INTEGER == rate_param.get_type()) {
+      impl_->throttler_ = Throttler(rate_param.as_int());
+    } else {
+      RCLCPP_WARN(impl_->ros_node_->get_logger(),
+        "Could not read value of param publish_rate [%s] as double/int, using default %ghz.",
+        rate_param.value_to_string().c_str(),
+        GazeboRosInitPrivate::DEFAULT_PUBLISH_FREQUENCY);
+    }
+  }
+
+  impl_->world_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin(
+    std::bind(&GazeboRosInitPrivate::PublishSimTime, impl_.get(), std::placeholders::_1));
+}
+
+GazeboRosInitPrivate::GazeboRosInitPrivate()
+: throttler_(DEFAULT_PUBLISH_FREQUENCY)
+{
+}
+
+void GazeboRosInitPrivate::PublishSimTime(const gazebo::common::UpdateInfo & _info)
+{
+  if (!throttler_.IsReady(_info.realTime)) {return;}
+
+  rosgraph_msgs::msg::Clock clock;
+  clock.clock = gazebo_ros::Convert<builtin_interfaces::msg::Time>(_info.simTime);
+  clock_pub_->publish(clock);
 }
 
 GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosInit)
+
+}  // namespace gazebo_ros

gazebo_ros/src/utils.cpp

@@ -60,4 +60,28 @@ std::string SensorFrameID(const gazebo::sensors::Sensor & _sensor, const sdf::El
   return gazebo_ros::ScopedNameBase(_sensor.ParentName());
 }
 
+Throttler::Throttler(const double _hz)
+: period_(gazebo::common::Time(1.0 / _hz)),
+  last_time_(0, 0)
+{
+}
+
+bool Throttler::IsReady(const gazebo::common::Time & _now)
+{
+  // If time went backwords, reset
+  if (_now < last_time_) {
+    last_time_ = _now;
+    return true;
+  }
+
+  // If not enough time has passed, return false
+  if (_now - last_time_ < period_) {
+    return false;
+  }
+
+  // Enough time has passed, set last time to now and return true
+  last_time_ = _now;
+  return true;
+}
+
 }  // namespace gazebo_ros

gazebo_ros/test/CMakeLists.txt

@@ -29,6 +29,7 @@ set(tests
   test_conversions
   test_utils
   test_plugins
+  test_sim_time
 )
 
 foreach(test ${tests})

gazebo_ros/test/test_sim_time.cpp

@@ -0,0 +1,84 @@
+// Copyright 2018 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <gazebo_ros/testing_utils.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <rosgraph_msgs/msg/clock.hpp>
+
+#include <memory>
+#include <string>
+#include <vector>
+
+/// Tests the simtime published to /clock by gazebo_ros_init
+class TestSimTime : public ::testing::Test
+{
+public:
+  TestSimTime() {}
+  void SetUp() override;
+  void TearDown() override;
+
+protected:
+  std::unique_ptr<gazebo_ros::GazeboProcess> gazebo_process_;
+};
+
+void TestSimTime::SetUp()
+{
+  gazebo_process_ = std::make_unique<gazebo_ros::GazeboProcess>(std::vector<const char *>{"-s",
+        "libgazebo_ros_init.so"});
+  ASSERT_GT(gazebo_process_->Run(), 0);
+}
+
+void TestSimTime::TearDown()
+{
+  ASSERT_GE(gazebo_process_->Terminate(), 0);
+  gazebo_process_.reset();
+}
+
+TEST_F(TestSimTime, TestClock)
+{
+  // Create a node which will use sim time
+  auto context = rclcpp::contexts::default_context::get_global_default_context();
+  std::vector<std::string> args;
+  std::vector<rclcpp::Parameter> params = {rclcpp::Parameter("use_sim_time", true)};
+  auto node = std::make_shared<rclcpp::Node>("test_sim_time", "", context, args, params);
+
+  using ClockMsg = rosgraph_msgs::msg::Clock;
+
+  // Get two clock messages, cachine the ROS time once each is received
+  auto first_msg =
+    gazebo_ros::get_message_or_timeout<ClockMsg>(node, "/clock", rclcpp::Duration(5, 0));
+  ASSERT_NE(first_msg, nullptr);
+  auto first_msg_time = node->now();
+  auto second_msg =
+    gazebo_ros::get_message_or_timeout<ClockMsg>(node, "/clock", rclcpp::Duration(1, 0));
+  ASSERT_NE(second_msg, nullptr);
+  auto second_msg_time = node->now();
+
+  // The SIM time should be close to zero (start of simulation)
+  EXPECT_LT(rclcpp::Time(first_msg->clock), rclcpp::Time(1, 0, RCL_ROS_TIME));
+  EXPECT_LT(rclcpp::Time(second_msg->clock), rclcpp::Time(1, 0, RCL_ROS_TIME));
+  // 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));
+  EXPECT_EQ(second_msg_time, rclcpp::Time(second_msg->clock));
+}
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

gazebo_ros/test/test_utils.cpp

@@ -92,6 +92,30 @@ TEST(TestUtils, SensorFrameID)
   }
 }
 
+TEST(TestUtils, Throttler)
+{
+  using Time = gazebo::common::Time;
+  using Throttler = gazebo_ros::Throttler;
+  {
+    // Test negative period (always ready)
+    Throttler t(-1);
+    EXPECT_TRUE(t.IsReady(Time()));
+    EXPECT_TRUE(t.IsReady(Time(50, 0)));
+    EXPECT_TRUE(t.IsReady(Time(20, 0)));
+    EXPECT_TRUE(t.IsReady(Time(1E3, 0)));
+  }
+  {
+    // Test frequency
+    Throttler t(2.0);
+    EXPECT_TRUE(t.IsReady(Time(0, 6E8)));
+    EXPECT_FALSE(t.IsReady(Time(0, 7E8)));
+    EXPECT_FALSE(t.IsReady(Time(0, 8E8)));
+    EXPECT_TRUE(t.IsReady(Time(1E4, 0)));
+    EXPECT_TRUE(t.IsReady(Time(1E4, 6E8)));
+    EXPECT_FALSE(t.IsReady(Time(1E4, 9E8)));
+  }
+}
+
 int main(int argc, char ** argv)
 {
   ::testing::InitGoogleTest(&argc, argv);