diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index acd1c17b5..d1452d1e2 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/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) diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp index 5f350dc49..18d9d8fd1 100644 --- a/gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp +++ b/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 +#include + +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 impl_; }; + +} // namespace gazebo_ros #endif // GAZEBO_ROS__GAZEBO_ROS_INIT_HPP_ diff --git a/gazebo_ros/include/gazebo_ros/utils.hpp b/gazebo_ros/include/gazebo_ros/utils.hpp index 54627031a..cb6738f47 100644 --- a/gazebo_ros/include/gazebo_ros/utils.hpp +++ b/gazebo_ros/include/gazebo_ros/utils.hpp @@ -15,6 +15,7 @@ #ifndef GAZEBO_ROS__UTILS_HPP_ #define GAZEBO_ROS__UTILS_HPP_ +#include #include #include @@ -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, or it is the first time. + bool IsReady(const gazebo::common::Time & _time); + +private: + /// The time between calls to @IsReady returning true + double period_; + /// The last time @IsReady returned true + gazebo::common::Time last_time_; +}; + } // namespace gazebo_ros #endif // GAZEBO_ROS__UTILS_HPP_ diff --git a/gazebo_ros/package.xml b/gazebo_ros/package.xml index 31ce46fa8..540ce6e4d 100644 --- a/gazebo_ros/package.xml +++ b/gazebo_ros/package.xml @@ -23,6 +23,7 @@ rclcpp gazebo_dev + geometry_msgs rclcpp gazebo_dev @@ -30,7 +31,6 @@ ament_cmake_gtest ament_lint_auto ament_lint_common - geometry_msgs std_msgs diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp index a3f3c6771..254d770c7 100644 --- a/gazebo_ros/src/gazebo_ros_init.cpp +++ b/gazebo_ros/src/gazebo_ros_init.cpp @@ -14,10 +14,32 @@ #include "gazebo_ros/gazebo_ros_init.hpp" -#include #include +#include +#include +#include +#include + +#include + +namespace gazebo_ros +{ + +class GazeboRosInitPrivate +{ +public: + GazeboRosInitPrivate(); + gazebo_ros::Node::SharedPtr ros_node_; + rclcpp::Publisher::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()) { } @@ -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( + "/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(_info.simTime); + clock_pub_->publish(clock); } GZ_REGISTER_SYSTEM_PLUGIN(GazeboRosInit) + +} // namespace gazebo_ros diff --git a/gazebo_ros/src/utils.cpp b/gazebo_ros/src/utils.cpp index aef59b0ae..c1554a078 100644 --- a/gazebo_ros/src/utils.cpp +++ b/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_(1.0 / _hz), + last_time_(0, 0) +{ +} + +bool Throttler::IsReady(const gazebo::common::Time & _now) +{ + // If time went backwards, reset + if (_now < last_time_) { + last_time_ = _now; + return true; + } + + // If not enough time has passed, return false + if (period_ > 0 && (_now - last_time_).Double() < period_) { + return false; + } + + // Enough time has passed, set last time to now and return true + last_time_ = _now; + return true; +} + } // namespace gazebo_ros diff --git a/gazebo_ros/test/CMakeLists.txt b/gazebo_ros/test/CMakeLists.txt index f6a9db76d..35f4ec0a4 100644 --- a/gazebo_ros/test/CMakeLists.txt +++ b/gazebo_ros/test/CMakeLists.txt @@ -29,6 +29,7 @@ set(tests test_conversions test_utils test_plugins + test_sim_time ) foreach(test ${tests}) diff --git a/gazebo_ros/test/test_sim_time.cpp b/gazebo_ros/test/test_sim_time.cpp new file mode 100644 index 000000000..b54a5e493 --- /dev/null +++ b/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 +#include +#include +#include + +#include +#include +#include + +/// 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_process_; +}; + +void TestSimTime::SetUp() +{ + gazebo_process_ = std::make_unique(std::vector{"-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 args; + std::vector params = {rclcpp::Parameter("use_sim_time", true)}; + auto node = std::make_shared("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(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(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(); +} diff --git a/gazebo_ros/test/test_utils.cpp b/gazebo_ros/test/test_utils.cpp index dec42e7b9..726b6ce13 100644 --- a/gazebo_ros/test/test_utils.cpp +++ b/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);