From 367ed26ca8fb31b74ff0b8d263d37d7ae778fe36 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Thu, 5 Dec 2024 20:55:50 +0000 Subject: [PATCH] Satisfy clang even more --- .../admittance_controller/admittance_rule.hpp | 9 ++--- .../admittance_rule_impl.hpp | 35 +++++++++++-------- .../src/admittance_controller.cpp | 4 +-- .../test/test_joint_state_broadcaster.cpp | 4 +-- .../tolerances.hpp | 2 +- .../trajectory.hpp | 4 +-- .../src/joint_trajectory_controller.cpp | 4 +-- .../src/trajectory.cpp | 4 +-- .../test/test_trajectory_controller_utils.hpp | 3 +- pid_controller/src/pid_controller.cpp | 2 +- .../src/steering_odometry.cpp | 5 ++- 11 files changed, 43 insertions(+), 33 deletions(-) diff --git a/admittance_controller/include/admittance_controller/admittance_rule.hpp b/admittance_controller/include/admittance_controller/admittance_rule.hpp index a326b663d0..bfb3a612c7 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule.hpp @@ -63,10 +63,11 @@ struct AdmittanceState mass_inv.setZero(); stiffness.setZero(); selected_axes.setZero(); - current_joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_pos = Eigen::VectorXd::Zero(num_joints); - joint_vel = Eigen::VectorXd::Zero(num_joints); - joint_acc = Eigen::VectorXd::Zero(num_joints); + auto idx = static_cast(num_joints); + current_joint_pos = Eigen::VectorXd::Zero(idx); + joint_pos = Eigen::VectorXd::Zero(idx); + joint_vel = Eigen::VectorXd::Zero(idx); + joint_acc = Eigen::VectorXd::Zero(idx); } Eigen::VectorXd current_joint_pos; diff --git a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp index 13d4e67fbc..859c7e3336 100644 --- a/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp +++ b/admittance_controller/include/admittance_controller/admittance_rule_impl.hpp @@ -136,9 +136,11 @@ void AdmittanceRule::apply_parameters_update() for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { - admittance_state_.mass_inv[i] = 1.0 / parameters_.admittance.mass[i]; - admittance_state_.damping[i] = parameters_.admittance.damping_ratio[i] * 2 * - sqrt(admittance_state_.mass[i] * admittance_state_.stiffness[i]); + auto idx = static_cast(i); + admittance_state_.mass_inv[idx] = 1.0 / parameters_.admittance.mass[i]; + admittance_state_.damping[idx] = + parameters_.admittance.damping_ratio[i] * 2 * + sqrt(admittance_state_.mass[idx] * admittance_state_.stiffness[idx]); } } @@ -216,12 +218,13 @@ controller_interface::return_type AdmittanceRule::update( // update joint desired joint state for (size_t i = 0; i < num_joints_; ++i) { + auto idx = static_cast(i); desired_joint_state.positions[i] = - reference_joint_state.positions[i] + admittance_state_.joint_pos[i]; + reference_joint_state.positions[i] + admittance_state_.joint_pos[idx]; desired_joint_state.velocities[i] = - reference_joint_state.velocities[i] + admittance_state_.joint_vel[i]; + reference_joint_state.velocities[i] + admittance_state_.joint_vel[idx]; desired_joint_state.accelerations[i] = - reference_joint_state.accelerations[i] + admittance_state_.joint_acc[i]; + reference_joint_state.accelerations[i] + admittance_state_.joint_acc[idx]; } return controller_interface::return_type::OK; @@ -334,7 +337,7 @@ void AdmittanceRule::process_wrench_measurements( new_wrench_base.block<3, 1>(0, 1) -= (cog_world_rot * cog_pos_).cross(end_effector_weight_); // apply smoothing filter - for (size_t i = 0; i < 6; ++i) + for (Eigen::Index i = 0; i < 6; ++i) { wrench_world_(i) = filters::exponentialSmoothing( new_wrench_base(i), wrench_world_(i), parameters_.ft_sensor.filter_coefficient); @@ -345,18 +348,20 @@ const control_msgs::msg::AdmittanceControllerState & AdmittanceRule::get_control { for (size_t i = 0; i < NUM_CARTESIAN_DOF; ++i) { - state_message_.stiffness.data[i] = admittance_state_.stiffness[i]; - state_message_.damping.data[i] = admittance_state_.damping[i]; - state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[i]); - state_message_.mass.data[i] = admittance_state_.mass[i]; + auto idx = static_cast(i); + state_message_.stiffness.data[i] = admittance_state_.stiffness[idx]; + state_message_.damping.data[i] = admittance_state_.damping[idx]; + state_message_.selected_axes.data[i] = static_cast(admittance_state_.selected_axes[idx]); + state_message_.mass.data[i] = admittance_state_.mass[idx]; } for (size_t i = 0; i < parameters_.joints.size(); ++i) { + auto idx = static_cast(i); state_message_.joint_state.name[i] = parameters_.joints[i]; - state_message_.joint_state.position[i] = admittance_state_.joint_pos[i]; - state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[i]; - state_message_.joint_state.effort[i] = admittance_state_.joint_acc[i]; + state_message_.joint_state.position[i] = admittance_state_.joint_pos[idx]; + state_message_.joint_state.velocity[i] = admittance_state_.joint_vel[idx]; + state_message_.joint_state.effort[i] = admittance_state_.joint_acc[idx]; } state_message_.wrench_base.wrench.force.x = admittance_state_.wrench_base[0]; @@ -411,7 +416,7 @@ void AdmittanceRule::vec_to_eigen(const std::vector & data, T2 & matrix) { for (auto row = 0; row < matrix.rows(); row++) { - matrix(row, col) = data[row + col * matrix.rows()]; + matrix(row, col) = data[static_cast(row + col * matrix.rows())]; } } } diff --git a/admittance_controller/src/admittance_controller.cpp b/admittance_controller/src/admittance_controller.cpp index 6e0e38140a..333d6ffded 100644 --- a/admittance_controller/src/admittance_controller.cpp +++ b/admittance_controller/src/admittance_controller.cpp @@ -304,7 +304,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, admittance_->parameters_.joints, interface, joint_state_interface_[index])) @@ -319,7 +319,7 @@ controller_interface::CallbackReturn AdmittanceController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { diff --git a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp index de534c00b6..3ec7ff4ac2 100644 --- a/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp +++ b/joint_state_broadcaster/test/test_joint_state_broadcaster.cpp @@ -1091,10 +1091,10 @@ void JointStateBroadcasterTest::test_published_dynamic_joint_state_message( ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].interface_names, ElementsAreArray(INTERFACE_NAMES)); - const auto index_in_original_order = std::distance( + const auto index_in_original_order = static_cast(std::distance( joint_names_.cbegin(), std::find( - joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i])); + joint_names_.cbegin(), joint_names_.cend(), dynamic_joint_state_msg->joint_names[i]))); ASSERT_THAT( dynamic_joint_state_msg->interface_values[i].values, Each(joint_values_[index_in_original_order])); diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp index 1583faf107..b33a2bf5c5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp @@ -199,7 +199,7 @@ SegmentTolerances get_segment_tolerances( .c_str()); return default_tolerances; } - auto i = std::distance(joints.cbegin(), it); + auto i = static_cast(std::distance(joints.cbegin(), it)); std::string interface = ""; try { diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index d650c369d0..74d4e28f3a 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -206,8 +206,8 @@ inline std::vector mapping(const T & t1, const T & t2) } else { - const size_t t1_dist = std::distance(t1.begin(), t1_it); - const size_t t2_dist = std::distance(t2.begin(), t2_it); + const size_t t1_dist = static_cast(std::distance(t1.begin(), t1_it)); + const size_t t2_dist = static_cast(std::distance(t2.begin(), t2_it)); mapping_vector[t1_dist] = t2_dist; } } diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 9096e78bf1..365ce993d0 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -907,7 +907,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { @@ -921,7 +921,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { auto it = std::find(allowed_interface_types_.begin(), allowed_interface_types_.end(), interface); - auto index = std::distance(allowed_interface_types_.begin(), it); + auto index = static_cast(std::distance(allowed_interface_types_.begin(), it)); if (!controller_interface::get_ordered_interfaces( state_interfaces_, params_.joints, interface, joint_state_interface_[index])) { diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 1c50d26c62..f91eb3e9d9 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -175,8 +175,8 @@ bool Trajectory::sample( interpolate_between_points(t0, point, t1, next_point, sample_time, output_state); } - start_segment_itr = begin() + i; - end_segment_itr = begin() + (i + 1); + start_segment_itr = begin() + static_cast(i); + end_segment_itr = begin() + static_cast(i + 1); if (search_monotonically_increasing) { last_sample_idx_ = i; diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 7dff81f08f..656bed11d5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -467,7 +467,8 @@ class TrajectoryControllerTest : public ::testing::Test if (joint_names.empty()) { traj_msg.joint_names = { - joint_names_.begin(), joint_names_.begin() + points_positions[0].size()}; + joint_names_.begin(), + joint_names_.begin() + static_cast(points_positions[0].size())}; } else { diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index 032dc1d666..f3b2ba33a0 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -320,7 +320,7 @@ void PidController::reference_callback(const std::shared_ptrdof_names.begin(), found_it); + auto position = static_cast(std::distance(ref_msg->dof_names.begin(), found_it)); ref_msg->values[position] = msg->values[i]; ref_msg->values_dot[position] = msg->values_dot[i]; } diff --git a/steering_controllers_library/src/steering_odometry.cpp b/steering_controllers_library/src/steering_odometry.cpp index dbe210ed41..8b8bb8bfd8 100644 --- a/steering_controllers_library/src/steering_odometry.cpp +++ b/steering_controllers_library/src/steering_odometry.cpp @@ -204,7 +204,10 @@ void SteeringOdometry::set_velocity_rolling_window_size(size_t velocity_rolling_ reset_accumulators(); } -void SteeringOdometry::set_odometry_type(const unsigned int type) { config_type_ = type; } +void SteeringOdometry::set_odometry_type(const unsigned int type) +{ + config_type_ = static_cast(type); +} double SteeringOdometry::convert_twist_to_steering_angle(double v_bx, double omega_bz) {