clang tidy fixes

evan-palmer · evan-palmer · commit 666dc35cfc72 · 2025-04-27T00:10:21.000-07:00
diff --git a/ik_solvers/include/ik_solvers/task_priority_solver.hpp b/ik_solvers/include/ik_solvers/task_priority_solver.hpp
@@ -43,11 +43,11 @@ class Constraint
 {
 public:
   /// Create a new constraint given the current primal value, constraint value, task priority and feedback gain.
-  Constraint(const Eigen::MatrixXd & primal, const Eigen::MatrixXd & constraint, int priority, double gain)
-  : primal_(primal),
-    constraint_(constraint),
+  Constraint(Eigen::MatrixXd primal, Eigen::MatrixXd constraint, int priority, double gain)
+  : primal_(std::move(primal)),
+    constraint_(std::move(constraint)),
     priority_(priority),
-    gain_(gain) {};
+    gain_(gain){};
 
   /// Destructor.
   virtual ~Constraint() = default;
@@ -167,7 +167,7 @@ class JointConstraint : public SetConstraint
 /// If the priorities are equal, the constraints are sorted based on their memory address.
 struct ConstraintCompare
 {
-  bool operator()(const std::shared_ptr<Constraint> & lhs, const std::shared_ptr<Constraint> & rhs) const
+  auto operator()(const std::shared_ptr<Constraint> & lhs, const std::shared_ptr<Constraint> & rhs) const -> bool
   {
     return lhs->priority() == rhs->priority() ? std::less<>{}(lhs, rhs) : lhs->priority() < rhs->priority();
   }
@@ -211,7 +211,7 @@ class TaskPriorityIKSolver : public IKSolver
 public:
   TaskPriorityIKSolver() = default;
 
-  virtual auto initialize(
+  auto initialize(
     const std::shared_ptr<rclcpp_lifecycle::LifecycleNode> & node,
     const std::shared_ptr<pinocchio::Model> & model,
     const std::shared_ptr<pinocchio::Data> & data,
diff --git a/ik_solvers/src/task_priority_solver.cpp b/ik_solvers/src/task_priority_solver.cpp
@@ -214,8 +214,8 @@ auto tpik(const hierarchy::ConstraintSet & tasks, size_t nv, double damping)
   Eigen::MatrixXd nullspace = Eigen::MatrixXd::Identity(nv, nv);
 
   for (const auto & task : tasks) {
-    const Eigen::MatrixXd J_inv = pinv::damped_least_squares(task->jacobian(), damping);
-    vel += nullspace * J_inv * (task->gain() * task->error());
+    const Eigen::MatrixXd jacobian_inv = pinv::damped_least_squares(task->jacobian(), damping);
+    vel += nullspace * jacobian_inv * (task->gain() * task->error());
 
     jacobians.push_back(task->jacobian());
     nullspace = compute_nullspace(construct_augmented_jacobian(jacobians));
@@ -227,15 +227,13 @@ auto tpik(const hierarchy::ConstraintSet & tasks, size_t nv, double damping)
 /// Check if the solution is feasible.
 auto is_feasible(const hierarchy::ConstraintSet & constraints, const Eigen::VectorXd & solution) -> bool
 {
-  for (const auto & constraint : constraints) {
+  return std::ranges::all_of(constraints, [&solution](const auto & constraint) {
     auto set_task = std::dynamic_pointer_cast<hierarchy::SetConstraint>(constraint);
     const double pred = (constraint->jacobian() * solution).value();
-    if (!((set_task->primal() > set_task->lower_threshold() && pred < 0) ||
-          (set_task->primal() < set_task->upper_threshold() && pred > 0))) {
-      return false;
-    }
-  }
-  return true;
+    return (
+      (set_task->primal() > set_task->lower_threshold() && pred < 0) ||
+      (set_task->primal() < set_task->upper_threshold() && pred > 0));
+  });
 }
 
 /// Search for the feasible solutions to the task hierarchy and return the one with the smallest norm.
@@ -316,7 +314,7 @@ auto TaskPriorityIKSolver::configure_parameters() -> void
 }
 
 // NOLINTNEXTLINE(readability-convert-member-functions-to-static)
-auto TaskPriorityIKSolver::solve_ik(const Eigen::Affine3d & target_pose, const Eigen::VectorXd & q)
+auto TaskPriorityIKSolver::solve_ik(const Eigen::Affine3d & goal, const Eigen::VectorXd & q)
   -> std::expected<Eigen::VectorXd, SolverError>
 {
   configure_parameters();
@@ -328,7 +326,7 @@ auto TaskPriorityIKSolver::solve_ik(const Eigen::Affine3d & target_pose, const E
 
   // insert the pose constraint
   const double gain = params_.end_effector_pose_task.gain;
-  hierarchy_.insert(std::make_shared<hierarchy::PoseConstraint>(model_, data_, ee_pose, target_pose, ee_frame_, gain));
+  hierarchy_.insert(std::make_shared<hierarchy::PoseConstraint>(model_, data_, ee_pose, goal, ee_frame_, gain));
 
   for (const auto & joint_name : params_.constrained_joints) {
     // get the joint index in the configuration vector
diff --git a/topic_sensors/src/odom_sensor.cpp b/topic_sensors/src/odom_sensor.cpp
@@ -57,7 +57,9 @@ auto OdomSensor::on_configure(const rclcpp_lifecycle::State & /*previous_state*/
   }
 
   state_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(
-    topic, rclcpp::SensorDataQoS(), [this, &transform_message](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {
+    topic,
+    rclcpp::SensorDataQoS(),
+    [this, &transform_message](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {  // NOLINT
       if (transform_message) {
         m2m::transform_message(*msg, "map_ned", "base_link_fsd");
       }
diff --git a/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp b/velocity_controllers/src/adaptive_integral_terminal_sliding_mode_controller.cpp
@@ -117,15 +117,19 @@ auto AdaptiveIntegralTerminalSlidingModeController::on_configure(const rclcpp_li
 
   // NOLINTNEXTLINE(performance-unnecessary-value-param)
   reference_sub_ = get_node()->create_subscription<geometry_msgs::msg::Twist>(
-    "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) {
+    "~/reference",
+    rclcpp::SystemDefaultsQoS(),
+    [this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) {  // NOLINT
       reference_.writeFromNonRT(*msg);
     });
 
   // use the tf interface when we aren't getting the state from a topic
   if (params_.use_external_measured_states) {
     RCLCPP_INFO(logger_, "Using external measured states");  // NOLINT
     system_state_sub_ = get_node()->create_subscription<nav_msgs::msg::Odometry>(
-      "~/system_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {
+      "~/system_state",
+      rclcpp::SystemDefaultsQoS(),
+      [this](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {  // NOLINT
         system_state_.writeFromNonRT(*msg);
       });
   } else {
diff --git a/velocity_controllers/src/integral_sliding_mode_controller.cpp b/velocity_controllers/src/integral_sliding_mode_controller.cpp
@@ -109,15 +109,19 @@ auto IntegralSlidingModeController::on_configure(const rclcpp_lifecycle::State &
   RCLCPP_INFO(logger_, "Commands won't be sent until both reference and state messages are received.");  // NOLINT
 
   reference_sub_ = get_node()->create_subscription<geometry_msgs::msg::Twist>(
-    "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) {
+    "~/reference",
+    rclcpp::SystemDefaultsQoS(),
+    [this](const std::shared_ptr<geometry_msgs::msg::Twist> msg) {  // NOLINT
       reference_.writeFromNonRT(*msg);
     });
 
   // use the tf interface when we aren't getting the state from a topic
   if (params_.use_external_measured_states) {
     RCLCPP_INFO(logger_, "Using external measured states");  // NOLINT
     system_state_sub_ = get_node()->create_subscription<nav_msgs::msg::Odometry>(
-      "~/system_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {
+      "~/system_state",
+      rclcpp::SystemDefaultsQoS(),
+      [this](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {  // NOLINT
         system_state_.writeFromNonRT(*msg);
       });
   } else {
diff --git a/whole_body_controllers/src/ik_controller.cpp b/whole_body_controllers/src/ik_controller.cpp
@@ -42,10 +42,10 @@ auto to_eigen(const std::vector<double> & vec) -> Eigen::Affine3d
   if (vec.size() != 7) {
     throw std::invalid_argument("Invalid size for pose vector");
   }
-  Eigen::Translation3d translation = {vec[0], vec[1], vec[2]};
+  const Eigen::Translation3d translation = {vec[0], vec[1], vec[2]};
   Eigen::Quaterniond rotation = {vec[6], vec[3], vec[4], vec[5]};
   rotation.normalize();
-  return Eigen::Affine3d(translation * rotation);
+  return {translation * rotation};
 }
 
 }  // namespace
@@ -161,15 +161,17 @@ auto IKController::on_configure(const rclcpp_lifecycle::State & /*previous_state
   velocity_state_values_.resize(velocity_interface_names_.size(), std::numeric_limits<double>::quiet_NaN());
 
   reference_sub_ = get_node()->create_subscription<geometry_msgs::msg::Pose>(
-    "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<geometry_msgs::msg::Pose> msg) {
+    "~/reference", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<geometry_msgs::msg::Pose> msg) {  // NOLINT
       m2m::transform_message(*msg);
       reference_.writeFromNonRT(*msg);
     });
 
   if (params_.use_external_measured_vehicle_states) {
     RCLCPP_INFO(logger_, "Using external measured vehicle states");  // NOLINT
     vehicle_state_sub_ = get_node()->create_subscription<nav_msgs::msg::Odometry>(
-      "~/vehicle_state", rclcpp::SystemDefaultsQoS(), [this](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {
+      "~/vehicle_state",
+      rclcpp::SystemDefaultsQoS(),
+      [this](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {  // NOLINT
         m2m::transform_message(*msg, "map", "base_link");
         vehicle_state_.writeFromNonRT(*msg);
       });
@@ -221,9 +223,8 @@ auto IKController::command_interface_configuration() const -> controller_interfa
       position_interface_names_, std::back_inserter(config.names), [this, &format_interface](const auto & name) {
         if (std::ranges::find(free_flyer_pos_dofs_, name) != free_flyer_pos_dofs_.end()) {
           return format_interface(name, hardware_interface::HW_IF_POSITION, params_.vehicle_reference_controller);
-        } else {
-          return format_interface(name, hardware_interface::HW_IF_POSITION, params_.manipulator_reference_controller);
         }
+        return format_interface(name, hardware_interface::HW_IF_POSITION, params_.manipulator_reference_controller);
       });
   }
 
@@ -232,9 +233,8 @@ auto IKController::command_interface_configuration() const -> controller_interfa
       velocity_interface_names_, std::back_inserter(config.names), [this, &format_interface](const auto & name) {
         if (std::ranges::find(free_flyer_vel_dofs_, name) != free_flyer_vel_dofs_.end()) {
           return format_interface(name, hardware_interface::HW_IF_VELOCITY, params_.vehicle_reference_controller);
-        } else {
-          return format_interface(name, hardware_interface::HW_IF_VELOCITY, params_.manipulator_reference_controller);
         }
+        return format_interface(name, hardware_interface::HW_IF_VELOCITY, params_.manipulator_reference_controller);
       });
   }
 
@@ -327,7 +327,7 @@ auto IKController::update_system_state_values() -> controller_interface::return_
     const auto position_interfaces = std::span(state_interfaces_.begin(), position_interfaces_end);
     const auto velocity_interfaces = std::span(velocity_interfaces_start, velocity_interfaces_end);
 
-    std::vector<double> position_states, velocity_states;
+    std::vector<double> position_states, velocity_states;  // NOLINT(readability-isolate-declaration)
     position_states.reserve(position_interfaces.size());
     velocity_states.reserve(velocity_interfaces.size());
 
@@ -411,11 +411,11 @@ auto IKController::update_and_write_commands(const rclcpp::Time & /*time*/, cons
   configure_parameters();
 
   const Eigen::VectorXd q = Eigen::VectorXd::Map(position_state_values_.data(), position_state_values_.size());
-  const Eigen::Affine3d target_pose = to_eigen(reference_interfaces_);
+  const Eigen::Affine3d goal = to_eigen(reference_interfaces_);
 
   // TODO(anyone): add solver support for velocity states
   // right now we only use the positions for the solver
-  const auto result = solver_->solve(period, target_pose, q);
+  const auto result = solver_->solve(period, goal, q);
 
   if (!result.has_value()) {
     const auto err = result.error();

Original file line number	Diff line number	Diff line change
`@@ -57,7 +57,9 @@ auto OdomSensor::on_configure(const rclcpp_lifecycle::State & /previous_state/`
`57`	`57`	`}`
`58`	`58`
`59`	`59`	`state_sub_ = node_->create_subscription<nav_msgs::msg::Odometry>(`
`60`		`- topic, rclcpp::SensorDataQoS(), [this, &transform_message](const std::shared_ptr<nav_msgs::msg::Odometry> msg) {`
	`60`	`+ topic,`
	`61`	`+ rclcpp::SensorDataQoS(),`
	`62`	`+ [this, &transform_message](const std::shared_ptr<nav_msgs::msg::Odometry> msg) { // NOLINT`
`61`	`63`	`if (transform_message) {`
`62`	`64`	`m2m::transform_message(*msg, "map_ned", "base_link_fsd");`
`63`	`65`	`}`