From 2aa76e3b3c0156ba93af97f06af629860481db92 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sat, 8 Jun 2024 17:02:13 +0200 Subject: [PATCH] Apply clang-tidy fixes --- moveit_core/collision_detection/src/world.cpp | 1 + .../constraint_samplers/test/pr2_arm_ik.cpp | 2 +- .../robot_model/src/prismatic_joint_model.cpp | 2 +- .../robot_model/src/revolute_joint_model.cpp | 2 +- .../robot_state/test/robot_state_test.cpp | 2 +- .../moveit/robot_trajectory/robot_trajectory.h | 2 +- .../robot_trajectory/src/robot_trajectory.cpp | 4 ++-- moveit_core/version/version.cpp | 2 +- .../src/joint_limits_container.cpp | 16 ++++++++-------- .../test/test_utils.cpp | 6 +++--- .../test/test_utils.h | 2 +- .../src/trajectory_execution_manager.cpp | 9 +++++---- .../src/planning_scene_interface.cpp | 9 ++++----- .../moveit_setup_framework/data/srdf_config.hpp | 1 + .../src/collision_linear_model.cpp | 4 ++-- 15 files changed, 33 insertions(+), 31 deletions(-) diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index b27a2b5b15..eae07cf01c 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -109,6 +109,7 @@ void World::addToObject(const std::string& object_id, const Eigen::Isometry3d& p std::vector World::getObjectIds() const { std::vector ids; + ids.reserve(objects_.size()); for (const auto& object : objects_) ids.push_back(object.first); return ids; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 7b97b1cbad..b35496a1a4 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -804,6 +804,6 @@ bool PR2ArmIK::checkJointLimits(const double joint_value, const int joint_num) c jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]); } - return !(jv < min_angles_[joint_num] || jv > max_angles_[joint_num]); + return jv >= min_angles_[joint_num] && jv <= max_angles_[joint_num]; } } // namespace pr2_arm_kinematics diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp index e229910d2f..81e9128053 100644 --- a/moveit_core/robot_model/src/prismatic_joint_model.cpp +++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp @@ -79,7 +79,7 @@ void PrismaticJointModel::getVariableDefaultPositions(double* values, const Boun bool PrismaticJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const { - return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin); + return values[0] >= bounds[0].min_position_ - margin && values[0] <= bounds[0].max_position_ + margin; } void PrismaticJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 1b80b4e2e7..524632159c 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -189,7 +189,7 @@ bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bou } else { - return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin); + return values[0] >= bounds[0].min_position_ - margin && values[0] <= bounds[0].max_position_ + margin; } } diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 8fdce99e73..cafb5bf01f 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -95,7 +95,7 @@ void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointMod EXPECT_NEAR(angle, 0.0, 1e-05) << "Angle between Cartesian velocity and Cartesian displacement larger than expected. " "Angle: " << angle << ". displacement: " << displacement.transpose() - << ". Cartesian velocity: " << cartesian_velocity.head<3>().transpose() << std::endl; + << ". Cartesian velocity: " << cartesian_velocity.head<3>().transpose() << '\n'; } } // namespace diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 5ca5e86aae..1f77946234 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -242,7 +242,7 @@ class RobotTrajectory RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0, size_t end_index = std::numeric_limits::max()); - void swap(robot_trajectory::RobotTrajectory& other); + void swap(robot_trajectory::RobotTrajectory& other) noexcept; RobotTrajectory& clear() { diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 93d3537a41..8e0eed38be 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -121,7 +121,7 @@ double RobotTrajectory::getAverageSegmentDuration() const return getDuration() / static_cast(duration_from_previous_.size()); } -void RobotTrajectory::swap(RobotTrajectory& other) +void RobotTrajectory::swap(RobotTrajectory& other) noexcept { robot_model_.swap(other.robot_model_); std::swap(group_, other.group_); @@ -719,7 +719,7 @@ std::optional toJointTrajectory(const Rob const std::vector& joint_filter) { const auto group = trajectory.getGroup(); - const auto robot_model = trajectory.getRobotModel(); + const auto& robot_model = trajectory.getRobotModel(); const std::vector& jnts = group ? group->getActiveJointModels() : robot_model->getActiveJointModels(); diff --git a/moveit_core/version/version.cpp b/moveit_core/version/version.cpp index 533b482cda..498c61cd4c 100644 --- a/moveit_core/version/version.cpp +++ b/moveit_core/version/version.cpp @@ -47,6 +47,6 @@ int main(int /*argc*/, char** /*argv*/) if (strlen(MOVEIT_GIT_NAME)) std::cout << " (" << MOVEIT_GIT_NAME << ")"; } - std::cout << std::endl; + std::cout << '\n'; return 0; } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index 2247d9f62d..55127d0334 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -118,26 +118,26 @@ std::map::const_iterator JointLimitsContainer::end() co bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, double joint_position) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_position_limits && - (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position))); + return !hasLimit(joint_name) || !getLimit(joint_name).has_position_limits || + (joint_position >= getLimit(joint_name).min_position && joint_position <= getLimit(joint_name).max_position); } bool JointLimitsContainer::verifyVelocityLimit(const std::string& joint_name, double joint_velocity) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_velocity_limits && - fabs(joint_velocity) > getLimit(joint_name).max_velocity)); + return !hasLimit(joint_name) || !getLimit(joint_name).has_velocity_limits || + fabs(joint_velocity) <= getLimit(joint_name).max_velocity; } bool JointLimitsContainer::verifyAccelerationLimit(const std::string& joint_name, double joint_acceleration) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_acceleration_limits && - fabs(joint_acceleration) > getLimit(joint_name).max_acceleration)); + return !hasLimit(joint_name) || !getLimit(joint_name).has_acceleration_limits || + fabs(joint_acceleration) <= getLimit(joint_name).max_acceleration; } bool JointLimitsContainer::verifyDecelerationLimit(const std::string& joint_name, double joint_acceleration) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_deceleration_limits && - fabs(joint_acceleration) > -1.0 * getLimit(joint_name).max_deceleration)); + return !hasLimit(joint_name) || !getLimit(joint_name).has_deceleration_limits || + fabs(joint_acceleration) <= -1.0 * getLimit(joint_name).max_deceleration; } void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 844fe72de7..5dcfad0cde 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -301,9 +301,9 @@ bool testutils::checkSLERP(const Eigen::Isometry3d& start_pose, const Eigen::Iso // parallel rotation axis // it is possible the axis opposite is // if the angle is zero, axis is arbitrary - if (!(((start_goal_aa.axis() - start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || - ((start_goal_aa.axis() + start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || - (fabs(start_wp_aa.angle()) < fabs(rot_angle_tolerance)))) + if (((start_goal_aa.axis() - start_wp_aa.axis()).norm() >= fabs(rot_axis_norm_tolerance)) && + ((start_goal_aa.axis() + start_wp_aa.axis()).norm() >= fabs(rot_axis_norm_tolerance)) && + (fabs(start_wp_aa.angle()) >= fabs(rot_angle_tolerance))) { std::cout << "Rotational linearity is violated. \n" << '\n' diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h index 625f70fd23..758c5ecada 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -477,7 +477,7 @@ checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& tr inline bool isMonotonouslyDecreasing(const std::vector& vec, double tol) { return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) { - return !(std::abs(a - b) < tol || a < b); // true -> a is ordered before b -> list is not sorted + return std::abs(a - b) >= tol && a >= b; // true -> a is ordered before b -> list is not sorted }); } diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index da7b7b0537..8a863ef80d 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -38,6 +38,7 @@ #include #include #include +#include #include #include @@ -166,7 +167,7 @@ void TrajectoryExecutionManager::initialize() rclcpp::NodeOptions opt; opt.allow_undeclared_parameters(true); opt.automatically_declare_parameters_from_overrides(true); - controller_mgr_node_.reset(new rclcpp::Node("moveit_simple_controller_manager", opt)); + controller_mgr_node_ = std::make_shared("moveit_simple_controller_manager", opt); auto all_params = node_->get_node_parameters_interface()->get_parameter_overrides(); for (const auto& param : all_params) @@ -939,12 +940,12 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont } std::set joints; - for (std::size_t i = 0, end = joint_names.size(); i < end; ++i) + for (const auto& joint_name : joint_names) { - const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_names[i]); + const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name); if (!jm) { - RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_names[i]); + RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_name); return false; } diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 2e589cf982..a4277f0731 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -127,8 +127,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl bool good = true; for (const geometry_msgs::msg::Pose& mesh_pose : collision_object.mesh_poses) { - if (!(mesh_pose.position.x >= minx && mesh_pose.position.x <= maxx && mesh_pose.position.y >= miny && - mesh_pose.position.y <= maxy && mesh_pose.position.z >= minz && mesh_pose.position.z <= maxz)) + if (mesh_pose.position.x < minx || mesh_pose.position.x > maxx || mesh_pose.position.y < miny || + mesh_pose.position.y > maxy || mesh_pose.position.z < minz || mesh_pose.position.z > maxz) { good = false; break; @@ -136,9 +136,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl } for (const geometry_msgs::msg::Pose& primitive_pose : collision_object.primitive_poses) { - if (!(primitive_pose.position.x >= minx && primitive_pose.position.x <= maxx && - primitive_pose.position.y >= miny && primitive_pose.position.y <= maxy && - primitive_pose.position.z >= minz && primitive_pose.position.z <= maxz)) + if (primitive_pose.position.x < minx || primitive_pose.position.x > maxx || primitive_pose.position.y < miny || + primitive_pose.position.y > maxy || primitive_pose.position.z < minz || primitive_pose.position.z > maxz) { good = false; break; diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp index 4b3fae3440..c5ad29cbf0 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp @@ -118,6 +118,7 @@ class SRDFConfig : public SetupConfig std::vector getGroupNames() const { std::vector group_names; + group_names.reserve(srdf_.groups_.size()); for (const srdf::Model::Group& group : srdf_.groups_) { group_names.push_back(group.name_); diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp index e6564a876c..9fb5b65d1b 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp @@ -260,8 +260,8 @@ void SortFilterProxyModel::setShowAll(bool show_all) bool SortFilterProxyModel::filterAcceptsRow(int source_row, const QModelIndex& source_parent) const { CollisionLinearModel* m = qobject_cast(sourceModel()); - if (!(show_all_ || m->reason(source_row) <= ALWAYS || - m->data(m->index(source_row, 2), Qt::CheckStateRole) == Qt::Checked)) + if (!show_all_ && m->reason(source_row) > ALWAYS && + m->data(m->index(source_row, 2), Qt::CheckStateRole) != Qt::Checked) return false; // not accepted due to check state const QRegExp regexp = filterRegExp();