format

ros-controls · Apr 22, 2023 · a7588fa · a7588fa
1 parent 3fb8ccf
commit a7588fa
Show file tree

Hide file tree

Showing 4 changed files with 37 additions and 38 deletions.
diff --git a/hardware_interface/include/hardware_interface/async_components.hpp b/hardware_interface/include/hardware_interface/async_components.hpp
@@ -33,23 +33,22 @@ namespace hardware_interface
 class AsyncComponentThread
 {
 public:
-
   explicit AsyncComponentThread(
-    Actuator* component, unsigned int update_rate,
+    Actuator * component, unsigned int update_rate,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
   : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
   {
   }
 
   explicit AsyncComponentThread(
-    System* component, unsigned int update_rate,
+    System * component, unsigned int update_rate,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
   : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
   {
   }
 
   explicit AsyncComponentThread(
-    Sensor* component, unsigned int update_rate,
+    Sensor * component, unsigned int update_rate,
     rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
   : hardware_component_(component), cm_update_rate_(update_rate), clock_interface_(clock_interface)
   {
@@ -73,29 +72,30 @@ class AsyncComponentThread
   {
     using TimePoint = std::chrono::system_clock::time_point;
 
-     std::visit(
-        [this](auto & component)
-        {
-      auto previous_time = clock_interface_->get_clock()->now();
-      while (!terminated_.load(std::memory_order_relaxed))
+    std::visit(
+      [this](auto & component)
       {
-        auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_);
-        TimePoint next_iteration_time =
-          TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds()));
-
-        if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+        auto previous_time = clock_interface_->get_clock()->now();
+        while (!terminated_.load(std::memory_order_relaxed))
         {
-          auto current_time = clock_interface_->get_clock()->now();
-          auto measured_period = current_time - previous_time;
-          previous_time = current_time;
-
-          // write
-          // read
+          auto const period = std::chrono::nanoseconds(1'000'000'000 / cm_update_rate_);
+          TimePoint next_iteration_time =
+            TimePoint(std::chrono::nanoseconds(clock_interface_->get_clock()->now().nanoseconds()));
+
+          if (component->get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE)
+          {
+            auto current_time = clock_interface_->get_clock()->now();
+            auto measured_period = current_time - previous_time;
+            previous_time = current_time;
+
+            // write
+            // read
+          }
+          next_iteration_time += period;
+          std::this_thread::sleep_until(next_iteration_time);
         }
-        next_iteration_time += period;
-        std::this_thread::sleep_until(next_iteration_time);
-      }
-    }, hardware_component_);
+      },
+      hardware_component_);
   }
 
 private:

diff --git a/hardware_interface/include/hardware_interface/resource_manager.hpp b/hardware_interface/include/hardware_interface/resource_manager.hpp
@@ -50,8 +50,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
 public:
   /// Default constructor for the Resource Manager.
   ResourceManager(
-    unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr
-    );
+    unsigned int update_rate = 100,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   /// Constructor for the Resource Manager.
   /**
@@ -71,7 +71,8 @@ class HARDWARE_INTERFACE_PUBLIC ResourceManager
    */
   explicit ResourceManager(
     const std::string & urdf, bool validate_interfaces = true, bool activate_all = false,
-    unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
+    unsigned int update_rate = 100,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr);
 
   ResourceManager(const ResourceManager &) = delete;
 

diff --git a/hardware_interface/include/hardware_interface/sensor.hpp b/hardware_interface/include/hardware_interface/sensor.hpp
@@ -78,7 +78,7 @@ class Sensor final
   return_type read(const rclcpp::Time & time, const rclcpp::Duration & period);
 
   HARDWARE_INTERFACE_PUBLIC
-  return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; } ;
+  return_type write(const rclcpp::Time &, const rclcpp::Duration &) { return return_type::OK; };
 
 private:
   std::unique_ptr<SensorInterface> impl_;

diff --git a/hardware_interface/src/resource_manager.cpp b/hardware_interface/src/resource_manager.cpp
@@ -79,13 +79,14 @@ class ResourceStorage
   // TODO(VX792): Change this when HW ifs get their own update rate,
   // because the ResourceStorage really shouldn't know about the cm's parameters
   ResourceStorage(
-    unsigned int update_rate = 100, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr
-    )
+    unsigned int update_rate = 100,
+    rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface = nullptr)
   : actuator_loader_(pkg_name, actuator_interface_name),
     sensor_loader_(pkg_name, sensor_interface_name),
     system_loader_(pkg_name, system_interface_name),
     cm_update_rate_(update_rate),
-    clock_interface_(clock_interface)  {
+    clock_interface_(clock_interface)
+  {
   }
 
   template <class HardwareT, class HardwareInterfaceT>
@@ -636,7 +637,6 @@ class ResourceStorage
   pluginlib::ClassLoader<SensorInterface> sensor_loader_;
   pluginlib::ClassLoader<SystemInterface> system_loader_;
 
-
   std::vector<Actuator> actuators_;
   std::vector<Sensor> sensors_;
   std::vector<System> systems_;
@@ -666,13 +666,11 @@ class ResourceStorage
   std::unordered_map<std::string, bool> claimed_command_interface_map_;
 
   /// List of async components by type
-  std::unordered_map<std::string, AsyncComponentThread>
-    async_component_threads_;
-
+  std::unordered_map<std::string, AsyncComponentThread> async_component_threads_;
+
   // Update rate of the controller manager, and the clock interface of its node - used by async components.
   unsigned int cm_update_rate_;
   rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface_;
-
 };
 
 ResourceManager::ResourceManager(
@@ -684,8 +682,8 @@ ResourceManager::ResourceManager(
 ResourceManager::~ResourceManager() = default;
 
 ResourceManager::ResourceManager(
-  const std::string & urdf, bool validate_interfaces, bool activate_all,
-   unsigned int update_rate, rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
+  const std::string & urdf, bool validate_interfaces, bool activate_all, unsigned int update_rate,
+  rclcpp::node_interfaces::NodeClockInterface::SharedPtr clock_interface)
 : resource_storage_(std::make_unique<ResourceStorage>(update_rate, clock_interface))
 {
   load_urdf(urdf, validate_interfaces);