Apply clang-tidy fixes

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<std::string> World::getObjectIds() const
 {
   std::vector<std::string> ids;
+  ids.reserve(objects_.size());
   for (const auto& object : objects_)
     ids.push_back(object.first);
   return ids;

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

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,

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;
   }
 }
 

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
 

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<std::size_t>::max());
 
-  void swap(robot_trajectory::RobotTrajectory& other);
+  void swap(robot_trajectory::RobotTrajectory& other) noexcept;
 
   RobotTrajectory& clear()
   {

moveit_core/robot_trajectory/src/robot_trajectory.cpp

@@ -121,7 +121,7 @@ double RobotTrajectory::getAverageSegmentDuration() const
     return getDuration() / static_cast<double>(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<trajectory_msgs::msg::JointTrajectory> toJointTrajectory(const Rob
                                                                        const std::vector<std::string>& joint_filter)
 {
   const auto group = trajectory.getGroup();
-  const auto robot_model = trajectory.getRobotModel();
+  const auto& robot_model = trajectory.getRobotModel();
   const std::vector<const moveit::core::JointModel*>& jnts =
       group ? group->getActiveJointModels() : robot_model->getActiveJointModels();
 

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;
 }

moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp

@@ -118,26 +118,26 @@ std::map<std::string, JointLimit>::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)

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'

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<double>& 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
   });
 }
 

moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp

@@ -38,6 +38,7 @@
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/robot_trajectory/robot_trajectory.h>
 #include <geometric_shapes/check_isometry.h>
+#include <memory>
 #include <tf2_eigen/tf2_eigen.hpp>
 #include <moveit/utils/logger.hpp>
 
@@ -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<rclcpp::Node>("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<const moveit::core::JointModel*> 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;
         }
 

moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp

@@ -127,18 +127,17 @@ 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;
         }
       }
       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;

moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp

@@ -118,6 +118,7 @@ class SRDFConfig : public SetupConfig
   std::vector<std::string> getGroupNames() const
   {
     std::vector<std::string> group_names;
+    group_names.reserve(srdf_.groups_.size());
     for (const srdf::Model::Group& group : srdf_.groups_)
     {
       group_names.push_back(group.name_);

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<CollisionLinearModel*>(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();