From d5a6752dbf5fcf5c2716dc84d3c5dd4069a21ebc Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Thu, 9 Aug 2018 18:00:06 -0700 Subject: [PATCH 01/10] Add Throttler to gazebo_ros utilities --- gazebo_ros/include/gazebo_ros/utils.hpp | 24 +++++++++++++++++ gazebo_ros/src/utils.cpp | 26 +++++++++++++++++++ gazebo_ros/test/test_utils.cpp | 34 +++++++++++++++++++++++++ 3 files changed, 84 insertions(+) diff --git a/gazebo_ros/include/gazebo_ros/utils.hpp b/gazebo_ros/include/gazebo_ros/utils.hpp index 54627031a..0c8a04d19 100644 --- a/gazebo_ros/include/gazebo_ros/utils.hpp +++ b/gazebo_ros/include/gazebo_ros/utils.hpp @@ -17,6 +17,7 @@ #include #include +#include #include @@ -55,5 +56,28 @@ 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 period + /// \param[in] _period Period at which IsReady will return true + explicit Throttler(const gazebo::common::Time & _period); + /// 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_ diff --git a/gazebo_ros/src/utils.cpp b/gazebo_ros/src/utils.cpp index aef59b0ae..340c0207d 100644 --- a/gazebo_ros/src/utils.cpp +++ b/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) +: period_(_period), + last_time_(0, 0) +{ +} + +Throttler::Throttler(const double _hz) +: Throttler(gazebo::common::Time(1.0 / _hz)) +{ +} + +bool Throttler::IsReady(const gazebo::common::Time & _now) +{ + if (_now < last_time_) { + last_time_ = _now; + return true; + } + + if (_now - last_time_ < period_) { + return false; + } + + last_time_ = _now; + return true; +} + } // namespace gazebo_ros diff --git a/gazebo_ros/test/test_utils.cpp b/gazebo_ros/test/test_utils.cpp index dec42e7b9..2a105a446 100644 --- a/gazebo_ros/test/test_utils.cpp +++ b/gazebo_ros/test/test_utils.cpp @@ -92,6 +92,40 @@ 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_FALSE(t.IsReady(Time(0, 4E8))); + EXPECT_TRUE(t.IsReady(Time(0, 6E8))); + EXPECT_TRUE(t.IsReady(Time(1E4, 0))); + EXPECT_FALSE(t.IsReady(Time(1E4, 4E8))); + EXPECT_TRUE(t.IsReady(Time(1E4, 6E8))); + } + { + // Test period + Time period(1, 1E3); + Throttler t(period); + EXPECT_FALSE(t.IsReady(Time())); + EXPECT_FALSE(t.IsReady(period - Time(0, 1))); + Time new_time = period; + EXPECT_TRUE(t.IsReady(new_time)); + EXPECT_FALSE(t.IsReady(new_time + period - Time(0, 1))); + EXPECT_TRUE(t.IsReady(new_time + period)); + } +} + int main(int argc, char ** argv) { ::testing::InitGoogleTest(&argc, argv); From c317d18b6de5f20f955945608014bef7652ef259 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Thu, 9 Aug 2018 18:03:38 -0700 Subject: [PATCH 02/10] Add sim time to gazebo_ros_init --- gazebo_ros/CMakeLists.txt | 20 +++-- .../include/gazebo_ros/gazebo_ros_init.hpp | 18 ++++- gazebo_ros/src/gazebo_ros_init.cpp | 66 +++++++++++++++- gazebo_ros/test/CMakeLists.txt | 1 + gazebo_ros/test/test_sim_time.cpp | 75 +++++++++++++++++++ 5 files changed, 166 insertions(+), 14 deletions(-) create mode 100644 gazebo_ros/test/test_sim_time.cpp diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index acd1c17b5..89f283ba7 100644 --- a/gazebo_ros/CMakeLists.txt +++ b/gazebo_ros/CMakeLists.txt @@ -31,23 +31,27 @@ 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" ) -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..af3c864a1 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. + // Documentation inherited void Load(int argc, char ** argv); + +private: + std::unique_ptr impl_; }; + +} // namespace gazebo_ros #endif // GAZEBO_ROS__GAZEBO_ROS_INIT_HPP_ diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp index a3f3c6771..e9e53fa62 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 rosnode_; + rclcpp::Publisher::SharedPtr clock_pub_; + gazebo::event::ConnectionPtr world_update_event_; + gazebo_ros::Throttler throttler_; + static const gazebo::common::Time DEFAULT_PUBLISH_PERIOD; + + void PublishSimTime(const gazebo::common::UpdateInfo & _info); +}; + GazeboRosInit::GazeboRosInit() +: impl_(std::make_unique()) { } @@ -29,7 +51,47 @@ 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_->rosnode_ = gazebo_ros::Node::Create("gazebo_ros_clock"); + impl_->clock_pub_ = impl_->rosnode_->create_publisher("/clock"); + + // Get publish rate from parameter if set + rclcpp::Parameter rate_param; + 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()) { + impl_->throttler_ = Throttler(gazebo::common::Time(1.0 / rate_param.as_int())); + } else { + RCLCPP_WARN(impl_->rosnode_->get_logger(), + "Could not read value of param publish_rate [%s] as double/int, using default %ghz.", + rate_param.value_to_string().c_str(), + 1.0 / GazeboRosInitPrivate::DEFAULT_PUBLISH_PERIOD.Double()); + } + } + + impl_->world_update_event_ = gazebo::event::Events::ConnectWorldUpdateBegin( + std::bind(&GazeboRosInitPrivate::PublishSimTime, impl_.get(), std::placeholders::_1)); +} + +// By default publish at 10 HZ (100 millisecond period) +const gazebo::common::Time GazeboRosInitPrivate::DEFAULT_PUBLISH_PERIOD = gazebo::common::Time(0, + 1E8); + +GazeboRosInitPrivate::GazeboRosInitPrivate() +: throttler_(DEFAULT_PUBLISH_PERIOD) +{ +} + +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/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..aba6a03ea --- /dev/null +++ b/gazebo_ros/test/test_sim_time.cpp @@ -0,0 +1,75 @@ +// 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 +#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) +{ + auto node = std::make_shared("my_node"); + rclcpp::Clock clock; + + using ClockMsg = rosgraph_msgs::msg::Clock; + + auto first_msg = + gazebo_ros::get_message_or_timeout(node, "/clock", rclcpp::Duration(5, 0)); + ASSERT_NE(first_msg, nullptr); + auto first_msg_time = clock.now(); + + auto second_msg = + gazebo_ros::get_message_or_timeout(node, "/clock", rclcpp::Duration(5, 0)); + ASSERT_NE(second_msg, nullptr); + auto second_msg_time = clock.now(); + + ASSERT_GT(second_msg, first_msg); +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From 0104814c0ded8265150dcf1ccfa3c745cccc1db5 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Fri, 10 Aug 2018 12:15:12 -0700 Subject: [PATCH 03/10] Remove period constructor from Throttler --- gazebo_ros/include/gazebo_ros/utils.hpp | 3 --- gazebo_ros/src/utils.cpp | 12 +++++------- gazebo_ros/test/test_utils.cpp | 16 +++------------- 3 files changed, 8 insertions(+), 23 deletions(-) diff --git a/gazebo_ros/include/gazebo_ros/utils.hpp b/gazebo_ros/include/gazebo_ros/utils.hpp index 0c8a04d19..926cd7a26 100644 --- a/gazebo_ros/include/gazebo_ros/utils.hpp +++ b/gazebo_ros/include/gazebo_ros/utils.hpp @@ -60,9 +60,6 @@ std::string SensorFrameID(const gazebo::sensors::Sensor & _sensor, const sdf::El class Throttler { public: - /// Create a throttler with a period - /// \param[in] _period Period at which IsReady will return true - explicit Throttler(const gazebo::common::Time & _period); /// Create a throttler with a frequency /// \param[in] _hz Frequency at which IsReady will return true, in hertz explicit Throttler(const double _hz); diff --git a/gazebo_ros/src/utils.cpp b/gazebo_ros/src/utils.cpp index 340c0207d..04691f4c4 100644 --- a/gazebo_ros/src/utils.cpp +++ b/gazebo_ros/src/utils.cpp @@ -60,28 +60,26 @@ std::string SensorFrameID(const gazebo::sensors::Sensor & _sensor, const sdf::El return gazebo_ros::ScopedNameBase(_sensor.ParentName()); } -Throttler::Throttler(const gazebo::common::Time & _period) -: period_(_period), - last_time_(0, 0) -{ -} - Throttler::Throttler(const double _hz) -: Throttler(gazebo::common::Time(1.0 / _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; } diff --git a/gazebo_ros/test/test_utils.cpp b/gazebo_ros/test/test_utils.cpp index 2a105a446..726b6ce13 100644 --- a/gazebo_ros/test/test_utils.cpp +++ b/gazebo_ros/test/test_utils.cpp @@ -107,22 +107,12 @@ TEST(TestUtils, Throttler) { // Test frequency Throttler t(2.0); - EXPECT_FALSE(t.IsReady(Time(0, 4E8))); 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_FALSE(t.IsReady(Time(1E4, 4E8))); EXPECT_TRUE(t.IsReady(Time(1E4, 6E8))); - } - { - // Test period - Time period(1, 1E3); - Throttler t(period); - EXPECT_FALSE(t.IsReady(Time())); - EXPECT_FALSE(t.IsReady(period - Time(0, 1))); - Time new_time = period; - EXPECT_TRUE(t.IsReady(new_time)); - EXPECT_FALSE(t.IsReady(new_time + period - Time(0, 1))); - EXPECT_TRUE(t.IsReady(new_time + period)); + EXPECT_FALSE(t.IsReady(Time(1E4, 9E8))); } } From b5e75a36b85c6d4ba50611ed389d989b2e0727c7 Mon Sep 17 00:00:00 2001 From: Kevin Allen Date: Fri, 10 Aug 2018 12:16:12 -0700 Subject: [PATCH 04/10] Improve sim time test --- .../include/gazebo_ros/gazebo_ros_init.hpp | 2 +- gazebo_ros/src/gazebo_ros_init.cpp | 26 ++++++++----------- gazebo_ros/test/test_sim_time.cpp | 25 ++++++++++++------ 3 files changed, 29 insertions(+), 24 deletions(-) diff --git a/gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp b/gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp index af3c864a1..18d9d8fd1 100644 --- a/gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp +++ b/gazebo_ros/include/gazebo_ros/gazebo_ros_init.hpp @@ -36,7 +36,7 @@ class GazeboRosInit : public gazebo::SystemPlugin virtual ~GazeboRosInit(); // Documentation inherited - void Load(int argc, char ** argv); + void Load(int argc, char ** argv) override; private: std::unique_ptr impl_; diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp index e9e53fa62..95c099942 100644 --- a/gazebo_ros/src/gazebo_ros_init.cpp +++ b/gazebo_ros/src/gazebo_ros_init.cpp @@ -29,11 +29,11 @@ class GazeboRosInitPrivate { public: GazeboRosInitPrivate(); - gazebo_ros::Node::SharedPtr rosnode_; + gazebo_ros::Node::SharedPtr ros_node_; rclcpp::Publisher::SharedPtr clock_pub_; gazebo::event::ConnectionPtr world_update_event_; gazebo_ros::Throttler throttler_; - static const gazebo::common::Time DEFAULT_PUBLISH_PERIOD; + static constexpr double DEFAULT_PUBLISH_FREQUENCY = 10.; void PublishSimTime(const gazebo::common::UpdateInfo & _info); }; @@ -52,21 +52,21 @@ void GazeboRosInit::Load(int argc, char ** argv) // Initialize ROS with arguments rclcpp::init(argc, argv); - impl_->rosnode_ = gazebo_ros::Node::Create("gazebo_ros_clock"); - impl_->clock_pub_ = impl_->rosnode_->create_publisher("/clock"); + impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_clock"); + impl_->clock_pub_ = impl_->ros_node_->create_publisher("/clock"); // Get publish rate from parameter if set rclcpp::Parameter rate_param; - if (impl_->rosnode_->get_parameter("publish_rate", rate_param)) { + if (impl_->ros_node_->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()) { - impl_->throttler_ = Throttler(gazebo::common::Time(1.0 / rate_param.as_int())); + 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_->rosnode_->get_logger(), + 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(), - 1.0 / GazeboRosInitPrivate::DEFAULT_PUBLISH_PERIOD.Double()); + GazeboRosInitPrivate::DEFAULT_PUBLISH_FREQUENCY); } } @@ -74,12 +74,8 @@ void GazeboRosInit::Load(int argc, char ** argv) std::bind(&GazeboRosInitPrivate::PublishSimTime, impl_.get(), std::placeholders::_1)); } -// By default publish at 10 HZ (100 millisecond period) -const gazebo::common::Time GazeboRosInitPrivate::DEFAULT_PUBLISH_PERIOD = gazebo::common::Time(0, - 1E8); - GazeboRosInitPrivate::GazeboRosInitPrivate() -: throttler_(DEFAULT_PUBLISH_PERIOD) +: throttler_(DEFAULT_PUBLISH_FREQUENCY) { } diff --git a/gazebo_ros/test/test_sim_time.cpp b/gazebo_ros/test/test_sim_time.cpp index aba6a03ea..b54a5e493 100644 --- a/gazebo_ros/test/test_sim_time.cpp +++ b/gazebo_ros/test/test_sim_time.cpp @@ -19,7 +19,6 @@ #include #include -#include #include /// Tests the simtime published to /clock by gazebo_ros_init @@ -49,22 +48,32 @@ void TestSimTime::TearDown() TEST_F(TestSimTime, TestClock) { - auto node = std::make_shared("my_node"); - rclcpp::Clock clock; + // 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 = clock.now(); - + auto first_msg_time = node->now(); auto second_msg = - gazebo_ros::get_message_or_timeout(node, "/clock", rclcpp::Duration(5, 0)); + gazebo_ros::get_message_or_timeout(node, "/clock", rclcpp::Duration(1, 0)); ASSERT_NE(second_msg, nullptr); - auto second_msg_time = clock.now(); + auto second_msg_time = node->now(); - ASSERT_GT(second_msg, first_msg); + // 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) From 1959361a4cf68a7498599fece940f4a7658208c9 Mon Sep 17 00:00:00 2001 From: dhood Date: Fri, 17 Aug 2018 17:05:09 -0700 Subject: [PATCH 05/10] Fix compilation in isolation for gazebo_ros_init --- gazebo_ros/CMakeLists.txt | 2 ++ gazebo_ros/package.xml | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/gazebo_ros/CMakeLists.txt b/gazebo_ros/CMakeLists.txt index 89f283ba7..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 @@ -46,6 +47,7 @@ add_library(gazebo_ros_init SHARED ament_target_dependencies(gazebo_ros_init "rclcpp" "gazebo_dev" + "geometry_msgs" ) target_link_libraries(gazebo_ros_init gazebo_ros_node 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 From 165cb300a794e4adb5bce40f99375ea9118987ce Mon Sep 17 00:00:00 2001 From: dhood Date: Fri, 17 Aug 2018 17:18:43 -0700 Subject: [PATCH 06/10] Transient local durability for clock publisher --- gazebo_ros/src/gazebo_ros_init.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp index 95c099942..77ef9a73c 100644 --- a/gazebo_ros/src/gazebo_ros_init.cpp +++ b/gazebo_ros/src/gazebo_ros_init.cpp @@ -53,7 +53,13 @@ void GazeboRosInit::Load(int argc, char ** argv) rclcpp::init(argc, argv); impl_->ros_node_ = gazebo_ros::Node::Create("gazebo_ros_clock"); - impl_->clock_pub_ = impl_->ros_node_->create_publisher("/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; From 4f2ff6d00ee09c5780c8e195d1b7d55cb09d3429 Mon Sep 17 00:00:00 2001 From: dhood Date: Mon, 20 Aug 2018 14:46:21 -0700 Subject: [PATCH 07/10] Linter fixup --- gazebo_ros/src/gazebo_ros_init.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp index 77ef9a73c..d8183095c 100644 --- a/gazebo_ros/src/gazebo_ros_init.cpp +++ b/gazebo_ros/src/gazebo_ros_init.cpp @@ -58,7 +58,8 @@ void GazeboRosInit::Load(int argc, char ** argv) // 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", + impl_->clock_pub_ = impl_->ros_node_->create_publisher( + "/clock", qos_profile); // Get publish rate from parameter if set From bc1f7102d87a8da816eee9fc4d936242f6659697 Mon Sep 17 00:00:00 2001 From: dhood Date: Mon, 20 Aug 2018 14:58:14 -0700 Subject: [PATCH 08/10] Document Throttler will return true on first call --- gazebo_ros/include/gazebo_ros/utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gazebo_ros/include/gazebo_ros/utils.hpp b/gazebo_ros/include/gazebo_ros/utils.hpp index 926cd7a26..a6b022ecd 100644 --- a/gazebo_ros/include/gazebo_ros/utils.hpp +++ b/gazebo_ros/include/gazebo_ros/utils.hpp @@ -66,7 +66,7 @@ class Throttler /// 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 + /// \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: From e87b52edfba80a9407faa2683951135a62a1b66d Mon Sep 17 00:00:00 2001 From: dhood Date: Mon, 20 Aug 2018 15:07:09 -0700 Subject: [PATCH 09/10] Store rate as double not Time --- gazebo_ros/include/gazebo_ros/utils.hpp | 2 +- gazebo_ros/src/utils.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/gazebo_ros/include/gazebo_ros/utils.hpp b/gazebo_ros/include/gazebo_ros/utils.hpp index a6b022ecd..7d0fd2da0 100644 --- a/gazebo_ros/include/gazebo_ros/utils.hpp +++ b/gazebo_ros/include/gazebo_ros/utils.hpp @@ -71,7 +71,7 @@ class Throttler private: /// The time between calls to @IsReady returning true - gazebo::common::Time period_; + double period_; /// The last time @IsReady returned true gazebo::common::Time last_time_; }; diff --git a/gazebo_ros/src/utils.cpp b/gazebo_ros/src/utils.cpp index 04691f4c4..c1554a078 100644 --- a/gazebo_ros/src/utils.cpp +++ b/gazebo_ros/src/utils.cpp @@ -61,21 +61,21 @@ std::string SensorFrameID(const gazebo::sensors::Sensor & _sensor, const sdf::El } Throttler::Throttler(const double _hz) -: period_(gazebo::common::Time(1.0 / _hz)), +: period_(1.0 / _hz), last_time_(0, 0) { } bool Throttler::IsReady(const gazebo::common::Time & _now) { - // If time went backwords, reset + // If time went backwards, reset if (_now < last_time_) { last_time_ = _now; return true; } // If not enough time has passed, return false - if (_now - last_time_ < period_) { + if (period_ > 0 && (_now - last_time_).Double() < period_) { return false; } From 7acdeb8271be9dbd6251088d39ed3ee811852ef8 Mon Sep 17 00:00:00 2001 From: dhood Date: Mon, 20 Aug 2018 15:55:08 -0700 Subject: [PATCH 10/10] Import order improvements --- gazebo_ros/include/gazebo_ros/utils.hpp | 2 +- gazebo_ros/src/gazebo_ros_init.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/gazebo_ros/include/gazebo_ros/utils.hpp b/gazebo_ros/include/gazebo_ros/utils.hpp index 7d0fd2da0..cb6738f47 100644 --- a/gazebo_ros/include/gazebo_ros/utils.hpp +++ b/gazebo_ros/include/gazebo_ros/utils.hpp @@ -15,9 +15,9 @@ #ifndef GAZEBO_ROS__UTILS_HPP_ #define GAZEBO_ROS__UTILS_HPP_ +#include #include #include -#include #include diff --git a/gazebo_ros/src/gazebo_ros_init.cpp b/gazebo_ros/src/gazebo_ros_init.cpp index d8183095c..254d770c7 100644 --- a/gazebo_ros/src/gazebo_ros_init.cpp +++ b/gazebo_ros/src/gazebo_ros_init.cpp @@ -14,11 +14,11 @@ #include "gazebo_ros/gazebo_ros_init.hpp" +#include +#include #include #include -#include #include -#include #include