Pre-commit

PickNikRobotics · Oct 8, 2024 · cc59e67 · cc59e67
1 parent ffa421f
commit cc59e67
Showing 1 changed file with 12 additions and 11 deletions.
diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp
@@ -297,9 +297,9 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
   std::vector<std::vector<double>> target_joint_commands;
   if (initial_cmd_reached_ == false)
   {
-    double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(), joint_commands_[POSITION_INTERFACE_INDEX].end(), 0, [](double sum, double val) {
-        return sum + std::abs(val);
-    });
+    double abs_sum = std::accumulate(joint_commands_[POSITION_INTERFACE_INDEX].begin(),
+                                     joint_commands_[POSITION_INTERFACE_INDEX].end(), 0,
+                                     [](double sum, double val) { return sum + std::abs(val); });
     if (abs_sum >= trigger_joint_command_threshold_)
     {
       initial_cmd_reached_ = true;
@@ -321,7 +321,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
       [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus<double>{});
 
   bool exist_velocity_command = false;
-  static bool exist_velocity_command_old = false; // Use old state to publish zero velocities
+  static bool exist_velocity_command_old = false;  // Use old state to publish zero velocities
   for (std::size_t i = 0; i < info_.joints.size(); ++i)
   {
     if (fabs(target_joint_commands[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_)
@@ -330,11 +330,12 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
     }
   }
 
-  if (diff <= trigger_joint_command_threshold_ && (exist_velocity_command == false && exist_velocity_command_old == false))
+  if (diff <= trigger_joint_command_threshold_ &&
+      (exist_velocity_command == false && exist_velocity_command_old == false))
   {
     return hardware_interface::return_type::OK;
   }
-  
+
   exist_velocity_command_old = exist_velocity_command;
 
   // For Position Joint
@@ -365,7 +366,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
             if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
             {
               joint_state.name.push_back(mimic_joint.joint_name);
-              joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]);          
+              joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]);
             }
           }
         }
@@ -377,7 +378,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
       topic_based_joint_commands_publisher_->publish(joint_state);
     }
   }
-  
+
   // For Velocity Joint
   {
     sensor_msgs::msg::JointState joint_state;
@@ -406,7 +407,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
             if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
             {
               joint_state.name.push_back(mimic_joint.joint_name);
-              joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]);          
+              joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]);
             }
           }
         }
@@ -418,7 +419,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
       topic_based_joint_commands_publisher_->publish(joint_state);
     }
   }
-  
+
   // For Effort Joint
   {
     sensor_msgs::msg::JointState joint_state;
@@ -447,7 +448,7 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti
             if (joint_state.name[index] == mimic_joint.mimicked_joint_name)
             {
               joint_state.name.push_back(mimic_joint.joint_name);
-              joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]);          
+              joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]);
             }
           }
         }