Formatting as requested in this repo

Felix Exner · Felix Exner · commit f28b6a118344 · 2022-02-23T16:56:22.000+01:00
diff --git a/scaled_controllers/include/scaled_controllers/scaled_joint_trajectory_controller.hpp b/scaled_controllers/include/scaled_controllers/scaled_joint_trajectory_controller.hpp
@@ -26,32 +26,32 @@
 #include "angles/angles.h"
 #include "joint_trajectory_controller/joint_trajectory_controller.hpp"
 #include "joint_trajectory_controller/trajectory.hpp"
-#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "rclcpp/time.hpp"
 #include "rclcpp/duration.hpp"
+#include "rclcpp/time.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
 
 namespace scaled_controllers
 {
 using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
-class ScaledJointTrajectoryController : public joint_trajectory_controller::JointTrajectoryController
+class ScaledJointTrajectoryController
+: public joint_trajectory_controller::JointTrajectoryController
 {
 public:
   ScaledJointTrajectoryController() = default;
   ~ScaledJointTrajectoryController() override = default;
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
 
-  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
 protected:
   struct TimeData
   {
-    TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0)
-    {
-    }
+    TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) {}
     rclcpp::Time time;
     rclcpp::Duration period;
     rclcpp::Time uptime;
diff --git a/scaled_controllers/include/scaled_controllers/speed_scaling_state_broadcaster.hpp b/scaled_controllers/include/scaled_controllers/speed_scaling_state_broadcaster.hpp
@@ -29,10 +29,10 @@
 #include <vector>
 
 #include "controller_interface/controller_interface.hpp"
+#include "rclcpp/duration.hpp"
+#include "rclcpp/time.hpp"
 #include "rclcpp_lifecycle/lifecycle_publisher.hpp"
 #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
-#include "rclcpp/time.hpp"
-#include "rclcpp/duration.hpp"
 #include "std_msgs/msg/float64.hpp"
 
 namespace scaled_controllers
@@ -48,13 +48,14 @@ class SpeedScalingStateBroadcaster : public controller_interface::ControllerInte
 
   controller_interface::InterfaceConfiguration state_interface_configuration() const override;
 
-  controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override;
+  controller_interface::return_type update(
+    const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
-  CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
-  CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
-  CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
   CallbackReturn on_init() override;
 
diff --git a/scaled_controllers/src/scaled_joint_trajectory_controller.cpp b/scaled_controllers/src/scaled_joint_trajectory_controller.cpp
@@ -28,15 +28,16 @@
 
 namespace scaled_controllers
 {
-controller_interface::InterfaceConfiguration ScaledJointTrajectoryController::state_interface_configuration() const
+controller_interface::InterfaceConfiguration
+ScaledJointTrajectoryController::state_interface_configuration() const
 {
   controller_interface::InterfaceConfiguration conf;
   conf = JointTrajectoryController::state_interface_configuration();
   conf.names.push_back("speed_scaling/speed_scaling_factor");
   return conf;
 }
 
-CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State& state)
+CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecycle::State & state)
 {
   TimeData time_data;
   time_data.time = node_->now();
@@ -46,44 +47,58 @@ CallbackReturn ScaledJointTrajectoryController::on_activate(const rclcpp_lifecyc
   return JointTrajectoryController::on_activate(state);
 }
 
-controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time,
-                                                                          const rclcpp::Duration& /*period*/)
+controller_interface::return_type ScaledJointTrajectoryController::update(
+  const rclcpp::Time & time, const rclcpp::Duration & /*period*/)
 {
-  if (state_interfaces_.back().get_name() == "speed_scaling") {
+  if (state_interfaces_.back().get_name() == "speed_scaling")
+  {
     scaling_factor_ = state_interfaces_.back().get_value();
-  } else {
-    RCLCPP_ERROR(get_node()->get_logger(), "Speed scaling interface not found in hardware interface.");
+  }
+  else
+  {
+    RCLCPP_ERROR(
+      get_node()->get_logger(), "Speed scaling interface not found in hardware interface.");
   }
 
-  if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) {
+  if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE)
+  {
     return controller_interface::return_type::OK;
   }
 
-  auto resize_joint_trajectory_point = [&](trajectory_msgs::msg::JointTrajectoryPoint& point, size_t size) {
-    point.positions.resize(size);
-    if (has_velocity_state_interface_) {
-      point.velocities.resize(size);
-    }
-    if (has_acceleration_state_interface_) {
-      point.accelerations.resize(size);
-    }
-  };
-  auto compute_error_for_joint = [&](JointTrajectoryPoint& error, int index, const JointTrajectoryPoint& current,
-                                     const JointTrajectoryPoint& desired) {
+  auto resize_joint_trajectory_point =
+    [&](trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) {
+      point.positions.resize(size);
+      if (has_velocity_state_interface_)
+      {
+        point.velocities.resize(size);
+      }
+      if (has_acceleration_state_interface_)
+      {
+        point.accelerations.resize(size);
+      }
+    };
+  auto compute_error_for_joint = [&](
+                                   JointTrajectoryPoint & error, int index,
+                                   const JointTrajectoryPoint & current,
+                                   const JointTrajectoryPoint & desired) {
     // error defined as the difference between current and desired
-    error.positions[index] = angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
-    if (has_velocity_state_interface_ && has_velocity_command_interface_) {
+    error.positions[index] =
+      angles::shortest_angular_distance(current.positions[index], desired.positions[index]);
+    if (has_velocity_state_interface_ && has_velocity_command_interface_)
+    {
       error.velocities[index] = desired.velocities[index] - current.velocities[index];
     }
-    if (has_acceleration_state_interface_ && has_acceleration_command_interface_) {
+    if (has_acceleration_state_interface_ && has_acceleration_command_interface_)
+    {
       error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
     }
   };
 
   // Check if a new external message has been received from nonRT threads
   auto current_external_msg = traj_external_point_ptr_->get_trajectory_msg();
   auto new_external_msg = traj_msg_external_point_ptr_.readFromRT();
-  if (current_external_msg != *new_external_msg) {
+  if (current_external_msg != *new_external_msg)
+  {
     fill_partial_goal(*new_external_msg);
     sort_to_local_joint_order(*new_external_msg);
     traj_external_point_ptr_->update(*new_external_msg);
@@ -94,96 +109,120 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
   resize_joint_trajectory_point(state_current, joint_num);
 
   // current state update
-  auto assign_point_from_interface = [&, joint_num](std::vector<double>& trajectory_point_interface,
-                                                    const auto& joint_inteface) {
-    for (auto index = 0ul; index < joint_num; ++index) {
-      trajectory_point_interface[index] = joint_inteface[index].get().get_value();
-    }
-  };
-  auto assign_interface_from_point = [&, joint_num](auto& joint_inteface,
-                                                    const std::vector<double>& trajectory_point_interface) {
-    for (auto index = 0ul; index < joint_num; ++index) {
-      joint_inteface[index].get().set_value(trajectory_point_interface[index]);
-    }
-  };
+  auto assign_point_from_interface =
+    [&, joint_num](std::vector<double> & trajectory_point_interface, const auto & joint_inteface) {
+      for (auto index = 0ul; index < joint_num; ++index)
+      {
+        trajectory_point_interface[index] = joint_inteface[index].get().get_value();
+      }
+    };
+  auto assign_interface_from_point =
+    [&, joint_num](auto & joint_inteface, const std::vector<double> & trajectory_point_interface) {
+      for (auto index = 0ul; index < joint_num; ++index)
+      {
+        joint_inteface[index].get().set_value(trajectory_point_interface[index]);
+      }
+    };
 
   state_current.time_from_start.set__sec(0);
 
   // Assign values from the hardware
   // Position states always exist
   assign_point_from_interface(state_current.positions, joint_state_interface_[0]);
   // velocity and acceleration states are optional
-  if (has_velocity_state_interface_) {
+  if (has_velocity_state_interface_)
+  {
     assign_point_from_interface(state_current.velocities, joint_state_interface_[1]);
     // Acceleration is used only in combination with velocity
-    if (has_acceleration_state_interface_) {
+    if (has_acceleration_state_interface_)
+    {
       assign_point_from_interface(state_current.accelerations, joint_state_interface_[2]);
-    } else {
+    }
+    else
+    {
       // Make empty so the property is ignored during interpolation
       state_current.accelerations.clear();
     }
-  } else {
+  }
+  else
+  {
     // Make empty so the property is ignored during interpolation
     state_current.velocities.clear();
     state_current.accelerations.clear();
   }
 
   // currently carrying out a trajectory
-  if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg()) {
+  if (traj_point_active_ptr_ && (*traj_point_active_ptr_)->has_trajectory_msg())
+  {
     // Main Speed scaling difference...
     // Adjust time with scaling factor
     TimeData time_data;
     time_data.time = time;
     rcl_duration_value_t period = (time_data.time - time_data_.readFromRT()->time).nanoseconds();
     time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * period);
     time_data.uptime = time_data_.readFromRT()->uptime + time_data.period;
-    rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(period);
+    rclcpp::Time traj_time =
+      time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(period);
     time_data_.writeFromNonRT(time_data);
 
     // if sampling the first time, set the point before you sample
-    if (!(*traj_point_active_ptr_)->is_sampled_already()) {
+    if (!(*traj_point_active_ptr_)->is_sampled_already())
+    {
       (*traj_point_active_ptr_)->set_point_before_trajectory_msg(traj_time, state_current);
     }
     resize_joint_trajectory_point(state_error, joint_num);
 
     // find segment for current timestamp
     joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr;
     const bool valid_point =
-        (*traj_point_active_ptr_)->sample(traj_time, state_desired, start_segment_itr, end_segment_itr);
+      (*traj_point_active_ptr_)
+        ->sample(traj_time, state_desired, start_segment_itr, end_segment_itr);
 
-    if (valid_point) {
+    if (valid_point)
+    {
       bool abort = false;
       bool outside_goal_tolerance = false;
       const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end();
 
       // set values for next hardware write()
-      if (has_position_command_interface_) {
+      if (has_position_command_interface_)
+      {
         assign_interface_from_point(joint_command_interface_[0], state_desired.positions);
       }
-      if (has_velocity_command_interface_) {
+      if (has_velocity_command_interface_)
+      {
         assign_interface_from_point(joint_command_interface_[1], state_desired.velocities);
       }
-      if (has_acceleration_command_interface_) {
+      if (has_acceleration_command_interface_)
+      {
         assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations);
       }
 
-      for (size_t index = 0; index < joint_num; ++index) {
+      for (size_t index = 0; index < joint_num; ++index)
+      {
         // set values for next hardware write()
         compute_error_for_joint(state_error, index, state_current, state_desired);
 
-        if (before_last_point &&
-            !check_state_tolerance_per_joint(state_error, index, default_tolerances_.state_tolerance[index], true)) {
+        if (
+          before_last_point &&
+          !check_state_tolerance_per_joint(
+            state_error, index, default_tolerances_.state_tolerance[index], true))
+        {
           abort = true;
         }
         // past the final point, check that we end up inside goal tolerance
-        if (!before_last_point && !check_state_tolerance_per_joint(
-                                      state_error, index, default_tolerances_.goal_state_tolerance[index], true)) {
+        if (
+          !before_last_point &&
+          !check_state_tolerance_per_joint(
+            state_error, index, default_tolerances_.goal_state_tolerance[index], true))
+        {
           outside_goal_tolerance = true;
         }
       }
 
       const auto active_goal = *rt_active_goal_.readFromRT();
-      if (active_goal) {
+      if (active_goal)
+      {
         // send feedback
         auto feedback = std::make_shared<FollowJTrajAction::Feedback>();
         feedback->header.stamp = time;
@@ -195,13 +234,17 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
         active_goal->setFeedback(feedback);
 
         // check abort
-        if (abort || outside_goal_tolerance) {
+        if (abort || outside_goal_tolerance)
+        {
           auto result = std::make_shared<FollowJTrajAction::Result>();
 
-          if (abort) {
+          if (abort)
+          {
             RCLCPP_WARN(node_->get_logger(), "Aborted due to state tolerance violation");
             result->set__error_code(FollowJTrajAction::Result::PATH_TOLERANCE_VIOLATED);
-          } else if (outside_goal_tolerance) {
+          }
+          else if (outside_goal_tolerance)
+          {
             RCLCPP_WARN(node_->get_logger(), "Aborted due to goal tolerance violation");
             result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
           }
@@ -210,28 +253,35 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
         }
 
         // check goal tolerance
-        if (!before_last_point) {
-          if (!outside_goal_tolerance) {
+        if (!before_last_point)
+        {
+          if (!outside_goal_tolerance)
+          {
             auto res = std::make_shared<FollowJTrajAction::Result>();
             res->set__error_code(FollowJTrajAction::Result::SUCCESSFUL);
             active_goal->setSucceeded(res);
             rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
 
             RCLCPP_INFO(node_->get_logger(), "Goal reached, success!");
-          } else if (default_tolerances_.goal_time_tolerance != 0.0) {
+          }
+          else if (default_tolerances_.goal_time_tolerance != 0.0)
+          {
             // if we exceed goal_time_toleralance set it to aborted
             const rclcpp::Time traj_start = (*traj_point_active_ptr_)->get_trajectory_start_time();
             const rclcpp::Time traj_end = traj_start + start_segment_itr->time_from_start;
 
             // TODO(anyone): This will break in speed scaling we have to discuss how to handle the goal
             // time when the robot scales itself down.
             const double difference = time.seconds() - traj_end.seconds();
-            if (difference > default_tolerances_.goal_time_tolerance) {
+            if (difference > default_tolerances_.goal_time_tolerance)
+            {
               auto result = std::make_shared<FollowJTrajAction::Result>();
               result->set__error_code(FollowJTrajAction::Result::GOAL_TOLERANCE_VIOLATED);
               active_goal->setAborted(result);
               rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr());
-              RCLCPP_WARN(node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds", difference);
+              RCLCPP_WARN(
+                node_->get_logger(), "Aborted due goal_time_tolerance exceeding by %f seconds",
+                difference);
             }
           }
         }
@@ -246,4 +296,5 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const
 }  // namespace scaled_controllers
 
 #include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(scaled_controllers::ScaledJointTrajectoryController, controller_interface::ControllerInterface)
+PLUGINLIB_EXPORT_CLASS(
+  scaled_controllers::ScaledJointTrajectoryController, controller_interface::ControllerInterface)
diff --git a/scaled_controllers/src/speed_scaling_state_broadcaster.cpp b/scaled_controllers/src/speed_scaling_state_broadcaster.cpp
