Updates/tests from ros_ign_gazebo::Stopwatch PR

buoy_gazebo/CMakeLists.txt

@@ -102,6 +102,63 @@ install(DIRECTORY
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   ament_lint_auto_find_test_dependencies()
+
+  # GTest
+  find_package(ament_cmake_gtest REQUIRED)
+  ament_find_gtest()
+
+  # launch_testing
+  find_package(launch_testing_ament_cmake REQUIRED)
+
+  # Helper function to generate gtest
+  function(gz_add_gtest TEST_NAME)
+    set(
+      options
+        ROS
+        LAUNCH_TEST
+        GAZEBO
+    )
+    set(oneValueArgs)
+    set(multiValueArgs
+      EXTRA_ROS_PKGS)
+
+    cmake_parse_arguments(gz_add_test "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
+
+    ament_add_gtest_executable(${TEST_NAME}
+      test/${TEST_NAME}.cpp
+    )
+    if(gz_add_test_GAZEBO)
+      target_link_libraries(${TEST_NAME}
+        ignition-gazebo${IGN_GAZEBO_VER}::ignition-gazebo${IGN_GAZEBO_VER}
+      )
+    endif()
+    if(gz_add_test_ROS)
+      set(ROS_PKGS rclcpp ${gz_add_test_EXTRA_ROS_PKGS})
+      foreach(PKG ${ROS_PKGS})
+        find_package(${PKG} REQUIRED)
+      endforeach()
+      ament_target_dependencies(${TEST_NAME} ${ROS_PKGS})
+    endif()
+    include_directories(${CMAKE_CURRENT_SOURCE_DIR}/src)
+    target_link_libraries(${TEST_NAME}
+      Stopwatch
+    )
+    install(
+      TARGETS ${TEST_NAME}
+      DESTINATION lib/${PROJECT_NAME}
+    )
+    if(gz_add_test_LAUNCH_TEST)
+      add_launch_test(test/launch/${TEST_NAME}.launch.py
+        TIMEOUT 300
+      )
+    else()
+      ament_add_gtest_test(${TEST_NAME})
+    endif()
+  endfunction()
+
+  # Add gtests
+  gz_add_gtest(test_stopwatch ROS)
+
 endif()
 
 ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/hooks/${PROJECT_NAME}.dsv.in")

buoy_gazebo/src/buoy_utils/CMakeLists.txt

@@ -1,10 +1,10 @@
-add_library(StopwatchSimTime
+add_library(Stopwatch
   SHARED
-    StopwatchSimTime.cpp
+    Stopwatch.cpp
 )
-ament_target_dependencies(StopwatchSimTime PUBLIC rclcpp)
+ament_target_dependencies(Stopwatch PUBLIC rclcpp)
 
 install(
   TARGETS
-    StopwatchSimTime
+    Stopwatch
   DESTINATION lib)

buoy_gazebo/src/buoy_utils/StopwatchSimTime.cpp → buoy_gazebo/src/buoy_utils/Stopwatch.cpp

@@ -12,7 +12,7 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include "StopwatchSimTime.hpp"
+#include "Stopwatch.hpp"
 
 #include <rclcpp/clock.hpp>
 
@@ -25,24 +25,39 @@ namespace buoy_utils
 {
 
 const rclcpp::Duration duration_zero = rclcpp::Duration(0, 0U);
-const rclcpp::Time time_min = rclcpp::Time(0, 0U);
+rclcpp::Time time_min = rclcpp::Time(0, 0U);
 
 // Private data class
-class StopwatchSimTimePrivate
+class StopwatchPrivate
 {
 public:
   /// \brief Default constructor.
-  StopwatchSimTimePrivate() = default;
+  StopwatchPrivate() = default;
 
   /// \brief Copy constructor.
   /// \param[in] _watch Watch to copy.
-  explicit StopwatchSimTimePrivate(const StopwatchSimTimePrivate & _watch)
+  explicit StopwatchPrivate(const StopwatchPrivate & _watch)
   : running(_watch.running),
     startTime(_watch.startTime),
     stopTime(_watch.stopTime),
     stopDuration(_watch.stopDuration),
     runDuration(_watch.runDuration)
   {
+    SetClock(_watch.clock);
+  }
+
+  void SetClock(rclcpp::Clock::SharedPtr _clock)
+  {
+    clock = _clock;
+    if (startTime.get_clock_type() != clock->get_clock_type()) {
+      startTime = rclcpp::Time(startTime.nanoseconds(), clock->get_clock_type());
+    }
+    if (stopTime.get_clock_type() != clock->get_clock_type()) {
+      stopTime = rclcpp::Time(stopTime.nanoseconds(), clock->get_clock_type());
+    }
+    if (time_min.get_clock_type() != clock->get_clock_type()) {
+      time_min = rclcpp::Time(0U, clock->get_clock_type());
+    }
   }
 
   /// \brief True if the real time clock is running.
@@ -65,36 +80,36 @@ class StopwatchSimTimePrivate
 };
 
 //////////////////////////////////////////////////
-StopwatchSimTime::StopwatchSimTime()
-: dataPtr(new StopwatchSimTimePrivate)
+Stopwatch::Stopwatch()
+: dataPtr(new StopwatchPrivate)
 {
 }
 
 //////////////////////////////////////////////////
-StopwatchSimTime::StopwatchSimTime(const StopwatchSimTime & _watch)
-: dataPtr(new StopwatchSimTimePrivate(*_watch.dataPtr))
+Stopwatch::Stopwatch(const Stopwatch & _watch)
+: dataPtr(new StopwatchPrivate(*_watch.dataPtr))
 {
 }
 
 //////////////////////////////////////////////////
-StopwatchSimTime::StopwatchSimTime(StopwatchSimTime && _watch) noexcept
+Stopwatch::Stopwatch(Stopwatch && _watch) noexcept
 : dataPtr(std::move(_watch.dataPtr))
 {
 }
 
 //////////////////////////////////////////////////
-StopwatchSimTime::~StopwatchSimTime()
+Stopwatch::~Stopwatch()
 {
 }
 
 //////////////////////////////////////////////////
-void StopwatchSimTime::SetClock(rclcpp::Clock::SharedPtr _clock)
+void Stopwatch::SetClock(rclcpp::Clock::SharedPtr _clock)
 {
-  this->dataPtr->clock = _clock;
+  this->dataPtr->SetClock(_clock);
 }
 
 //////////////////////////////////////////////////
-bool StopwatchSimTime::Start(const bool _reset)
+bool Stopwatch::Start(const bool _reset)
 {
   if (!this->dataPtr->clock) {
     return false;
@@ -119,13 +134,13 @@ bool StopwatchSimTime::Start(const bool _reset)
 }
 
 //////////////////////////////////////////////////
-const rclcpp::Time & StopwatchSimTime::StartTime() const
+const rclcpp::Time & Stopwatch::StartTime() const
 {
   return this->dataPtr->startTime;
 }
 
 //////////////////////////////////////////////////
-bool StopwatchSimTime::Stop()
+bool Stopwatch::Stop()
 {
   if (!this->dataPtr->clock) {
     return false;
@@ -143,19 +158,19 @@ bool StopwatchSimTime::Stop()
 }
 
 //////////////////////////////////////////////////
-const rclcpp::Time & StopwatchSimTime::StopTime() const
+const rclcpp::Time & Stopwatch::StopTime() const
 {
   return this->dataPtr->stopTime;
 }
 
 //////////////////////////////////////////////////
-bool StopwatchSimTime::Running() const
+bool Stopwatch::Running() const
 {
   return this->dataPtr->running;
 }
 
 //////////////////////////////////////////////////
-void StopwatchSimTime::Reset()
+void Stopwatch::Reset()
 {
   this->dataPtr->running = false;
   this->dataPtr->startTime = time_min;
@@ -165,7 +180,7 @@ void StopwatchSimTime::Reset()
 }
 
 //////////////////////////////////////////////////
-rclcpp::Duration StopwatchSimTime::ElapsedRunTime() const
+rclcpp::Duration Stopwatch::ElapsedRunTime() const
 {
   if (!this->dataPtr->clock) {
     return duration_zero;
@@ -179,7 +194,7 @@ rclcpp::Duration StopwatchSimTime::ElapsedRunTime() const
 }
 
 //////////////////////////////////////////////////
-rclcpp::Duration StopwatchSimTime::ElapsedStopTime() const
+rclcpp::Duration Stopwatch::ElapsedStopTime() const
 {
   if (!this->dataPtr->clock) {
     return duration_zero;
@@ -199,7 +214,7 @@ rclcpp::Duration StopwatchSimTime::ElapsedStopTime() const
 }
 
 //////////////////////////////////////////////////
-bool StopwatchSimTime::operator==(const StopwatchSimTime & _watch) const
+bool Stopwatch::operator==(const Stopwatch & _watch) const
 {
   return this->dataPtr->running == _watch.dataPtr->running &&
          this->dataPtr->startTime == _watch.dataPtr->startTime &&
@@ -209,20 +224,20 @@ bool StopwatchSimTime::operator==(const StopwatchSimTime & _watch) const
 }
 
 //////////////////////////////////////////////////
-bool StopwatchSimTime::operator!=(const StopwatchSimTime & _watch) const
+bool Stopwatch::operator!=(const Stopwatch & _watch) const
 {
   return !(*this == _watch);
 }
 
 //////////////////////////////////////////////////
-StopwatchSimTime & StopwatchSimTime::operator=(const StopwatchSimTime & _watch)
+Stopwatch & Stopwatch::operator=(const Stopwatch & _watch)
 {
-  this->dataPtr.reset(new StopwatchSimTimePrivate(*_watch.dataPtr));
+  this->dataPtr.reset(new StopwatchPrivate(*_watch.dataPtr));
   return *this;
 }
 
 //////////////////////////////////////////////////
-StopwatchSimTime & StopwatchSimTime::operator=(StopwatchSimTime && _watch)
+Stopwatch & Stopwatch::operator=(Stopwatch && _watch)
 {
   this->dataPtr = std::move(_watch.dataPtr);
   return *this;

buoy_gazebo/src/buoy_utils/StopwatchSimTime.hpp → buoy_gazebo/src/buoy_utils/Stopwatch.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef BUOY_UTILS__STOPWATCHSIMTIME_HPP_
-#define BUOY_UTILS__STOPWATCHSIMTIME_HPP_
+#ifndef BUOY_UTILS__STOPWATCH_HPP_
+#define BUOY_UTILS__STOPWATCH_HPP_
 
 #include <rclcpp/clock.hpp>
 
@@ -24,7 +24,7 @@
 namespace buoy_utils
 {
 // Forward declarations.
-class StopwatchSimTimePrivate;
+class StopwatchPrivate;
 
 /// \brief The stopwatch accepts an rclcpp::Clock instance from a rclcpp::Node (allowing use of
 /// sim time if use_sim_time set in node). Keeps track of time spent in the run state, accessed
@@ -46,29 +46,29 @@ class StopwatchSimTimePrivate;
 ///   timeSys.ElapsedRunTime()).count() << " ms\n";
 /// watch.Stop();
 /// ```
-class StopwatchSimTime
+class Stopwatch
 {
   /// \brief Constructor.
 
 public:
-  StopwatchSimTime();
+  Stopwatch();
 
   /// \brief Copy constructor
   /// \param[in] _watch The stop watch to copy.
 
 public:
-  StopwatchSimTime(const StopwatchSimTime & _watch);
+  Stopwatch(const Stopwatch & _watch);
 
   /// \brief Move constructor
   /// \param[in] _watch The stop watch to move.
 
 public:
-  StopwatchSimTime(StopwatchSimTime && _watch) noexcept;
+  Stopwatch(Stopwatch && _watch) noexcept;
 
   /// \brief Destructor.
 
 public:
-  virtual ~StopwatchSimTime();
+  virtual ~Stopwatch();
 
 public:
   /// \brief Take a clock instance (e.g. get_clock() from rclcpp::Node).
@@ -141,28 +141,28 @@ class StopwatchSimTime
   /// \return True if this watch equals the provided watch.
 
 public:
-  bool operator==(const StopwatchSimTime & _watch) const;
+  bool operator==(const Stopwatch & _watch) const;
 
   /// \brief Inequality operator.
   /// \param[in] _watch The watch to compare.
   /// \return True if this watch does not equal the provided watch.
 
 public:
-  bool operator!=(const StopwatchSimTime & _watch) const;
+  bool operator!=(const Stopwatch & _watch) const;
 
   /// \brief Copy assignment operator
   /// \param[in] _watch The stop watch to copy.
   /// \return Reference to this.
 
 public:
-  StopwatchSimTime & operator=(const StopwatchSimTime & _watch);
+  Stopwatch & operator=(const Stopwatch & _watch);
 
   /// \brief Move assignment operator
   /// \param[in] _watch The stop watch to move.
   /// \return Reference to this.
 
 public:
-  StopwatchSimTime & operator=(StopwatchSimTime && _watch);
+  Stopwatch & operator=(Stopwatch && _watch);
 
 #ifdef _WIN32
 // Disable warning C4251 which is triggered by
@@ -173,10 +173,10 @@ class StopwatchSimTime
   /// \brief Private data pointer.
 
 private:
-  std::unique_ptr<StopwatchSimTimePrivate> dataPtr;
+  std::unique_ptr<StopwatchPrivate> dataPtr;
 #ifdef _WIN32
 #pragma warning(pop)
 #endif
 };
 }  // namespace buoy_utils
-#endif  // BUOY_UTILS__STOPWATCHSIMTIME_HPP_
+#endif  // BUOY_UTILS__STOPWATCH_HPP_

buoy_gazebo/src/controllers/PowerController/CMakeLists.txt

@@ -4,6 +4,6 @@ gz_add_plugin(PowerController
   INCLUDE_DIRS
     ../..
   PUBLIC_LINK_LIBS
-    StopwatchSimTime
+    Stopwatch
   ROS
 )