Fix logging rates

buoy_gazebo/src/IncidentWaves/IncidentWaves.cpp

@@ -74,7 +74,6 @@ void IncidentWaves::Configure(
   gz::sim::EntityComponentManager & _ecm,
   gz::sim::EventManager & /*_eventMgr*/)
 {
-
   this->dataPtr->model = gz::sim::Model(_entity);
   if (!this->dataPtr->model.Valid(_ecm)) {
     ignerr << "IncidentWaves plugin should be attached to a model entity. " <<

buoy_gazebo/src/buoy_utils/Rate.hpp

@@ -0,0 +1,111 @@
+// Copyright 2022 Open Source Robotics Foundation, Inc. and Monterey Bay Aquarium Research Institute
+//
+// 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.
+
+#ifndef BUOY_UTILS__RATE_HPP_
+#define BUOY_UTILS__RATE_HPP_
+
+
+#include <chrono>
+
+#include <rclcpp/macros.hpp>
+#include <rclcpp/rate.hpp>
+
+
+namespace buoy_utils
+{
+
+using std::chrono::duration;
+using std::chrono::duration_cast;
+using std::chrono::nanoseconds;
+
+class SimRate : public rclcpp::RateBase
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(SimRate);
+
+  explicit SimRate(const double & rate, rclcpp::Clock::SharedPtr _clock)
+  : SimRate(
+      duration_cast<nanoseconds>(duration<double>(1.0 / rate)),
+      _clock
+  )
+  {}
+
+  explicit SimRate(
+    const std::chrono::nanoseconds & period,
+    rclcpp::Clock::SharedPtr _clock)
+  : clock(_clock),
+    period_(period),
+    last_interval_(
+      rclcpp::Time(_clock->now(),
+      _clock->get_clock_type())
+    )
+  {}
+
+  virtual bool sleep()
+  {
+    // Time coming into sleep
+    auto now = clock->now();
+    // Time of next interval
+    auto next_interval = last_interval_ + period_;
+    // Detect backwards time flow
+    if (now < last_interval_) {
+      // Best thing to do is to set the next_interval to now + period
+      next_interval = now + period_;
+    }
+    // Calculate the time to sleep
+    auto time_to_sleep = next_interval - now;
+    // Update the interval
+    last_interval_ += period_;
+    // If the time_to_sleep is negative or zero, don't sleep
+    if (time_to_sleep <= rclcpp::Duration(0, 0U)) {
+      // If an entire cycle was missed then reset next interval.
+      // This might happen if the loop took more than a cycle.
+      // Or if time jumps forward.
+      if (now > next_interval + period_) {
+        last_interval_ = now + period_;
+      }
+      // Either way do not sleep and return false
+      return false;
+    }
+    // Sleep (will get interrupted by ctrl-c, may not sleep full time)
+    clock->sleep_for(time_to_sleep);
+    return true;
+  }
+
+  virtual bool is_steady() const
+  {
+    return false;
+  }
+
+  virtual void reset()
+  {
+    last_interval_ = clock->now();
+  }
+
+  rclcpp::Duration period() const
+  {
+    return period_;
+  }
+
+private:
+  RCLCPP_DISABLE_COPY(SimRate)
+
+  rclcpp::Clock::SharedPtr clock = nullptr;
+  rclcpp::Duration period_;
+  rclcpp::Time last_interval_;
+};
+
+}  // namespace buoy_utils
+
+#endif  // BUOY_UTILS__RATE_HPP_

buoy_gazebo/src/controllers/BatteryController/BatteryController.cpp

@@ -33,9 +33,11 @@
 
 #include <buoy_interfaces/msg/bc_record.hpp>
 
+#include "buoy_utils/Rate.hpp"
 #include "ElectroHydraulicPTO/BatteryState.hpp"
 #include "BatteryController.hpp"
 
+
 using namespace std::chrono_literals;
 
 namespace buoy_gazebo
@@ -48,7 +50,7 @@ struct BatteryControllerROS2
   bool use_sim_time_{true};
 
   rclcpp::Publisher<buoy_interfaces::msg::BCRecord>::SharedPtr bc_pub_{nullptr};
-  std::unique_ptr<rclcpp::Rate> pub_rate_{nullptr};
+  std::unique_ptr<buoy_utils::SimRate> pub_rate_{nullptr};
   buoy_interfaces::msg::BCRecord bc_record_;
   double pub_rate_hz_{10.0};
 };
@@ -122,7 +124,9 @@ struct BatteryControllerPrivate
             std::unique_lock next(next_access_mutex_);
             std::unique_lock data(data_mutex_);
             next.unlock();
-            ros_->pub_rate_ = std::make_unique<rclcpp::Rate>(ros_->pub_rate_hz_);
+            ros_->pub_rate_ = std::make_unique<buoy_utils::SimRate>(
+              ros_->pub_rate_hz_,
+              ros_->node_->get_clock());
             data.unlock();
           }
         }
@@ -155,6 +159,10 @@ BatteryController::BatteryController()
 BatteryController::~BatteryController()
 {
   // Stop ros2 threads
+  if (rclcpp::ok()) {
+    rclcpp::shutdown();
+  }
+
   this->dataPtr->stop_ = true;
   if (this->dataPtr->ros_->executor_) {
     this->dataPtr->ros_->executor_->cancel();

buoy_gazebo/src/controllers/PowerController/PowerController.cpp

@@ -34,12 +34,14 @@
 #include <ros_gz_sim/Stopwatch.hpp>
 
 #include <buoy_interfaces/msg/pc_record.hpp>
+#include <buoy_interfaces/srv/pc_pack_rate_command.hpp>
 #include <buoy_interfaces/srv/pc_wind_curr_command.hpp>
 #include <buoy_interfaces/srv/pc_scale_command.hpp>
 #include <buoy_interfaces/srv/pc_retract_command.hpp>
 #include <buoy_interfaces/srv/pc_bias_curr_command.hpp>
 #include <buoy_interfaces/msg/pb_command_response.hpp>
 
+#include "buoy_utils/Rate.hpp"
 #include "ElectroHydraulicPTO/ElectroHydraulicState.hpp"
 #include "PowerController.hpp"
 
@@ -56,7 +58,7 @@ struct PowerControllerROS2
   bool use_sim_time_{true};
 
   rclcpp::Publisher<buoy_interfaces::msg::PCRecord>::SharedPtr pc_pub_{nullptr};
-  std::unique_ptr<rclcpp::Rate> pub_rate_{nullptr};
+  std::unique_ptr<buoy_utils::SimRate> pub_rate_{nullptr};
   static const rcl_interfaces::msg::FloatingPointRange valid_pub_rate_range_;
   buoy_interfaces::msg::PCRecord pc_record_;
   double pub_rate_hz_{10.0};
@@ -68,6 +70,12 @@ const rcl_interfaces::msg::FloatingPointRange PowerControllerROS2::valid_pub_rat
 
 struct PowerControllerServices
 {
+  // PCPackRateCommand
+  rclcpp::Service<buoy_interfaces::srv::PCPackRateCommand>::SharedPtr packrate_command_service_{
+    nullptr};
+  std::function<void(std::shared_ptr<buoy_interfaces::srv::PCPackRateCommand::Request>,
+    std::shared_ptr<buoy_interfaces::srv::PCPackRateCommand::Response>)> packrate_command_handler_;
+
   // PCWindCurrCommand -- Winding Current (Torque)
   rclcpp::Service<buoy_interfaces::srv::PCWindCurrCommand>::SharedPtr torque_command_service_{
     nullptr};
@@ -200,7 +208,9 @@ struct PowerControllerPrivate
     std::unique_lock next(next_access_mutex_);
     std::unique_lock data(data_mutex_);
     next.unlock();
-    ros_->pub_rate_ = std::make_unique<rclcpp::Rate>(ros_->pub_rate_hz_);
+    ros_->pub_rate_ = std::make_unique<buoy_utils::SimRate>(
+      ros_->pub_rate_hz_,
+      ros_->node_->get_clock());
     data.unlock();
   }
 
@@ -290,6 +300,22 @@ struct PowerControllerPrivate
 
   void setupServices()
   {
+    // Pack Rate
+    services_->packrate_command_handler_ =
+      [this](const std::shared_ptr<buoy_interfaces::srv::PCPackRateCommand::Request> request,
+        std::shared_ptr<buoy_interfaces::srv::PCPackRateCommand::Response> response)
+      {
+        RCLCPP_DEBUG_STREAM(
+          ros_->node_->get_logger(),
+          "[ROS 2 Power Control] PCPackRateCommand Received [" << request->rate_hz << " Hz]");
+        rclcpp::Parameter parameter("publish_rate", static_cast<double>(request->rate_hz));
+        handle_publish_rate(parameter);
+      };
+    services_->packrate_command_service_ =
+      ros_->node_->create_service<buoy_interfaces::srv::PCPackRateCommand>(
+      "pc_pack_rate_command",
+      services_->packrate_command_handler_);
+
     // PCWindCurrCommand
     services_->torque_command_watch_.SetClock(ros_->node_->get_clock());
     services_->torque_command_handler_ =
@@ -580,6 +606,10 @@ PowerController::PowerController()
 PowerController::~PowerController()
 {
   // Stop ros2 threads
+  if (rclcpp::ok()) {
+    rclcpp::shutdown();
+  }
+
   this->dataPtr->stop_ = true;
   if (this->dataPtr->ros_->executor_) {
     this->dataPtr->ros_->executor_->cancel();

buoy_gazebo/src/controllers/SpringController/SpringController.cpp

@@ -36,10 +36,13 @@
 #include <buoy_interfaces/msg/sc_record.hpp>
 #include <buoy_interfaces/srv/valve_command.hpp>
 #include <buoy_interfaces/srv/pump_command.hpp>
+#include <buoy_interfaces/srv/sc_pack_rate_command.hpp>
 
+#include "buoy_utils/Rate.hpp"
 #include "PolytropicPneumaticSpring/SpringState.hpp"
 #include "SpringController.hpp"
 
+
 using namespace std::chrono_literals;
 
 namespace buoy_gazebo
@@ -52,13 +55,23 @@ struct SpringControllerROS2
   bool use_sim_time_{true};
 
   rclcpp::Publisher<buoy_interfaces::msg::SCRecord>::SharedPtr sc_pub_{nullptr};
-  std::unique_ptr<rclcpp::Rate> pub_rate_{nullptr};
+  std::unique_ptr<buoy_utils::SimRate> pub_rate_{nullptr};
+  static const rcl_interfaces::msg::FloatingPointRange valid_pub_rate_range_;
   buoy_interfaces::msg::SCRecord sc_record_;
   double pub_rate_hz_{10.0};
 };
+const rcl_interfaces::msg::FloatingPointRange SpringControllerROS2::valid_pub_rate_range_ =
+  rcl_interfaces::msg::FloatingPointRange()
+  .set__from_value(10.0F)
+  .set__to_value(50.0F);
 
 struct SpringControllerServices
 {
+  rclcpp::Service<buoy_interfaces::srv::SCPackRateCommand>::SharedPtr packrate_command_service_{
+    nullptr};
+  std::function<void(std::shared_ptr<buoy_interfaces::srv::SCPackRateCommand::Request>,
+    std::shared_ptr<buoy_interfaces::srv::SCPackRateCommand::Response>)> packrate_command_handler_;
+
   rclcpp::Service<buoy_interfaces::srv::ValveCommand>::SharedPtr valve_command_service_{nullptr};
   std::function<void(std::shared_ptr<buoy_interfaces::srv::ValveCommand::Request>,
     std::shared_ptr<buoy_interfaces::srv::ValveCommand::Response>)> valve_command_handler_;
@@ -100,6 +113,40 @@ struct SpringControllerPrivate
     return spring_data_valid_ && load_cell_data_valid_;
   }
 
+  void handle_publish_rate(const rclcpp::Parameter & parameter)
+  {
+    double rate_hz = parameter.as_double();
+    RCLCPP_INFO_STREAM(
+      ros_->node_->get_logger(),
+      "[ROS 2 Spring Control] setting publish_rate: " << rate_hz);
+
+    if (SpringControllerROS2::valid_pub_rate_range_.from_value > rate_hz ||
+      rate_hz > SpringControllerROS2::valid_pub_rate_range_.to_value)
+    {
+      RCLCPP_WARN_STREAM(
+        ros_->node_->get_logger(),
+        "[ROS 2 Spring Control] publish_rate out of bounds -- clipped between [" <<
+          SpringControllerROS2::valid_pub_rate_range_.from_value << ", " <<
+          SpringControllerROS2::valid_pub_rate_range_.to_value << "] Hz");
+    }
+
+    ros_->pub_rate_hz_ = std::min(
+      std::max(
+        rate_hz,
+        SpringControllerROS2::valid_pub_rate_range_.from_value),
+      SpringControllerROS2::valid_pub_rate_range_.to_value);
+
+    // low prio data access
+    std::unique_lock low(low_prio_mutex_);
+    std::unique_lock next(next_access_mutex_);
+    std::unique_lock data(data_mutex_);
+    next.unlock();
+    ros_->pub_rate_ = std::make_unique<buoy_utils::SimRate>(
+      ros_->pub_rate_hz_,
+      ros_->node_->get_clock());
+    data.unlock();
+  }
+
   void ros2_setup(const std::string & node_name, const std::string & ns)
   {
     if (!rclcpp::ok()) {
@@ -129,26 +176,7 @@ struct SpringControllerPrivate
             parameter.get_name() == "publish_rate" &&
             parameter.get_type() == rclcpp::PARAMETER_DOUBLE)
           {
-            double rate_hz = parameter.as_double();
-            RCLCPP_INFO_STREAM(
-              ros_->node_->get_logger(),
-              "[ROS 2 Spring Control] setting publish_rate: " << rate_hz);
-
-            if (rate_hz < 10.0 || rate_hz > 50.0) {
-              RCLCPP_WARN_STREAM(
-                ros_->node_->get_logger(),
-                "[ROS 2 Spring Control] publish_rate out of bounds -- clipped between [10,50]");
-            }
-
-            ros_->pub_rate_hz_ = std::min(std::max(rate_hz, 10.0), 50.0);
-
-            // low prio data access
-            std::unique_lock low(low_prio_mutex_);
-            std::unique_lock next(next_access_mutex_);
-            std::unique_lock data(data_mutex_);
-            next.unlock();
-            ros_->pub_rate_ = std::make_unique<rclcpp::Rate>(ros_->pub_rate_hz_);
-            data.unlock();
+            handle_publish_rate(parameter);
           }
         }
         return result;
@@ -171,6 +199,22 @@ struct SpringControllerPrivate
 
   void setup_services()
   {
+    // Pack Rate
+    services_->packrate_command_handler_ =
+      [this](const std::shared_ptr<buoy_interfaces::srv::SCPackRateCommand::Request> request,
+        std::shared_ptr<buoy_interfaces::srv::SCPackRateCommand::Response> response)
+      {
+        RCLCPP_DEBUG_STREAM(
+          ros_->node_->get_logger(),
+          "[ROS 2 Spring Control] SCPackRateCommand Received [" << request->rate_hz << " Hz]");
+        rclcpp::Parameter parameter("publish_rate", static_cast<double>(request->rate_hz));
+        handle_publish_rate(parameter);
+      };
+    services_->packrate_command_service_ =
+      ros_->node_->create_service<buoy_interfaces::srv::SCPackRateCommand>(
+      "sc_pack_rate_command",
+      services_->packrate_command_handler_);
+
     services_->command_watch_.SetClock(ros_->node_->get_clock());
 
     // ValveCommand
@@ -437,6 +481,10 @@ SpringController::SpringController()
 SpringController::~SpringController()
 {
   // Stop ros2 threads
+  if (rclcpp::ok()) {
+    rclcpp::shutdown();
+  }
+
   this->dataPtr->stop_ = true;
   if (this->dataPtr->ros_->executor_) {
     this->dataPtr->ros_->executor_->cancel();