diff --git a/.clang-format b/.clang-format index 9a6ec509a4..13ed70e95b 100644 --- a/.clang-format +++ b/.clang-format @@ -59,6 +59,10 @@ SpaceBeforeAssignmentOperators: true # Configure each individual brace in BraceWrapping BreakBeforeBraces: Custom +# Qualifiers (const, volatile, static, etc) +QualifierAlignment: Custom +QualifierOrder: ['static', 'inline', 'constexpr', 'const', 'volatile', 'type'] + # Control of individual brace wrapping cases BraceWrapping: AfterCaseLabel: true diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index b2c9ad1043..034f3bf537 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -64,10 +64,10 @@ AllowedCollisionMatrix::AllowedCollisionMatrix(const srdf::Model& srdf) for (const std::string& name : srdf.getNoDefaultCollisionLinks()) setDefaultEntry(name, collision_detection::AllowedCollision::ALWAYS); // re-enable specific collision pairs - for (auto const& collision : srdf.getEnabledCollisionPairs()) + for (const auto& collision : srdf.getEnabledCollisionPairs()) setEntry(collision.link1_, collision.link2_, false); // *finally* disable selected collision pairs - for (auto const& collision : srdf.getDisabledCollisionPairs()) + for (const auto& collision : srdf.getDisabledCollisionPairs()) setEntry(collision.link1_, collision.link2_, true); } diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 6ceaa96ccd..f89864803d 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -43,7 +43,7 @@ #include #include -const static double EPSILON{ 0.0001 }; +static const double EPSILON{ 0.0001 }; namespace collision_detection { diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index 5c2641d49d..6a9f45b28c 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -156,7 +156,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase /** @brief Signature for a cost function used to evaluate IK solutions. */ using IKCostFn = std::function&)>; + const moveit::core::JointModelGroup*, const std::vector&)>; /** * @brief Given a desired pose of the end-effector, compute the joint angles to reach it 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 109a48863d..8ceb77a151 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 @@ -398,18 +398,18 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); /// active joint distances between the two states (L1 norm). /// \param[in] trajectory Given robot trajectory /// \return Length of the robot trajectory [rad] -[[nodiscard]] double path_length(RobotTrajectory const& trajectory); +[[nodiscard]] double path_length(const RobotTrajectory& trajectory); /// \brief Calculate the smoothness of a given trajectory /// \param[in] trajectory Given robot trajectory /// \return Smoothness of the given trajectory /// or nullopt if it is not possible to calculate the smoothness -[[nodiscard]] std::optional smoothness(RobotTrajectory const& trajectory); +[[nodiscard]] std::optional smoothness(const RobotTrajectory& trajectory); /// \brief Calculate the waypoint density of a trajectory /// \param[in] trajectory Given robot trajectory /// \return Waypoint density of the given trajectory /// or nullopt if it is not possible to calculate the density -[[nodiscard]] std::optional waypoint_density(RobotTrajectory const& trajectory); +[[nodiscard]] std::optional waypoint_density(const RobotTrajectory& trajectory); } // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index f2663deb50..4201a877a4 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -636,19 +636,19 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory) return out; } -double path_length(RobotTrajectory const& trajectory) +double path_length(const RobotTrajectory& trajectory) { auto trajectory_length = 0.0; for (std::size_t index = 1; index < trajectory.getWayPointCount(); ++index) { - auto const& first = trajectory.getWayPoint(index - 1); - auto const& second = trajectory.getWayPoint(index); + const auto& first = trajectory.getWayPoint(index - 1); + const auto& second = trajectory.getWayPoint(index); trajectory_length += first.distance(second); } return trajectory_length; } -std::optional smoothness(RobotTrajectory const& trajectory) +std::optional smoothness(const RobotTrajectory& trajectory) { if (trajectory.getWayPointCount() > 2) { @@ -686,13 +686,13 @@ std::optional smoothness(RobotTrajectory const& trajectory) return std::nullopt; } -std::optional waypoint_density(RobotTrajectory const& trajectory) +std::optional waypoint_density(const RobotTrajectory& trajectory) { // Only calculate density if more than one waypoint exists if (trajectory.getWayPointCount() > 1) { // Calculate path length - auto const length = path_length(trajectory); + const auto length = path_length(trajectory); if (length > 0.0) { auto density = static_cast(trajectory.getWayPointCount()) / length; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h index 8d333e4b98..26038d1bbc 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/ruckig_traj_smoothing.h @@ -108,7 +108,7 @@ class RuckigSmoothing */ [[nodiscard]] static bool getRobotModelBounds(const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, - moveit::core::JointModelGroup const* const group, + const moveit::core::JointModelGroup* const group, ruckig::InputParameter& ruckig_input); /** diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index be966d2677..dfd3b6d029 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -75,7 +75,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto } // Kinematic limits (vels/accels/jerks) from RobotModel - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); ruckig::InputParameter ruckig_input{ num_dof }; if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input)) @@ -109,7 +109,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto } // Set default kinematic limits (vels/accels/jerks) - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); ruckig::InputParameter ruckig_input{ num_dof }; if (!getRobotModelBounds(max_velocity_scaling_factor, max_acceleration_scaling_factor, group, ruckig_input)) @@ -176,7 +176,7 @@ bool RuckigSmoothing::applySmoothing(robot_trajectory::RobotTrajectory& trajecto bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& trajectory) { - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); if (!group) { RCLCPP_ERROR(LOGGER, "The planner did not set the group the plan was computed for"); @@ -187,7 +187,7 @@ bool RuckigSmoothing::validateGroup(const robot_trajectory::RobotTrajectory& tra bool RuckigSmoothing::getRobotModelBounds(const double max_velocity_scaling_factor, const double max_acceleration_scaling_factor, - moveit::core::JointModelGroup const* const group, + const moveit::core::JointModelGroup* const group, ruckig::InputParameter& ruckig_input) { const size_t num_dof = group->getVariableCount(); @@ -244,7 +244,7 @@ bool RuckigSmoothing::runRuckig(robot_trajectory::RobotTrajectory& trajectory, const bool mitigate_overshoot, const double overshoot_threshold) { const size_t num_waypoints = trajectory.getWayPointCount(); - moveit::core::JointModelGroup const* const group = trajectory.getGroup(); + const moveit::core::JointModelGroup* const group = trajectory.getGroup(); const size_t num_dof = group->getVariableCount(); ruckig::OutputParameter ruckig_output{ num_dof }; diff --git a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp index 3094e511a8..e7d2ef6737 100644 --- a/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/test/test_ruckig_traj_smoothing.cpp @@ -158,9 +158,9 @@ TEST_F(RuckigTests, single_waypoint) EXPECT_TRUE( smoother_.applySmoothing(*trajectory_, 1.0 /* max vel scaling factor */, 1.0 /* max accel scaling factor */)); // And the waypoint did not change - auto const new_first_waypoint = trajectory_->getFirstWayPointPtr(); - auto const& variable_names = new_first_waypoint->getVariableNames(); - for (std::string const& variable_name : variable_names) + const auto new_first_waypoint = trajectory_->getFirstWayPointPtr(); + const auto& variable_names = new_first_waypoint->getVariableNames(); + for (const std::string& variable_name : variable_names) { EXPECT_EQ(first_waypoint_input.getVariablePosition(variable_name), new_first_waypoint->getVariablePosition(variable_name)); diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index de8181fc9c..677a2adaa7 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -85,7 +85,7 @@ class SharedData int num_nearest_ik_tests_; bool publish_trajectory_; - SharedData(SharedData const&) = delete; // this is a singleton + SharedData(const SharedData&) = delete; // this is a singleton SharedData() { initialize(); diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h index 8dea8329f3..2f533778c2 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/constrained_goal_sampler.h @@ -57,7 +57,7 @@ class ConstrainedGoalSampler : public ompl::base::GoalLazySamples private: bool sampleUsingConstraintSampler(const ompl::base::GoalLazySamples* gls, ompl::base::State* new_goal); - bool stateValidityCallback(ompl::base::State* new_goal, moveit::core::RobotState const* state, + bool stateValidityCallback(ompl::base::State* new_goal, const moveit::core::RobotState* state, const moveit::core::JointModelGroup* /*jmg*/, const double* /*jpos*/, bool verbose = false) const; bool checkStateValidity(ompl::base::State* new_goal, const moveit::core::RobotState& state, diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index 86b4b2affe..bf4376da49 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -77,7 +77,7 @@ bool ompl_interface::ConstrainedGoalSampler::checkStateValidity(ob::State* new_g } bool ompl_interface::ConstrainedGoalSampler::stateValidityCallback(ob::State* new_goal, - moveit::core::RobotState const* state, + const moveit::core::RobotState* state, const moveit::core::JointModelGroup* jmg, const double* jpos, bool verbose) const { diff --git a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp index bcca9890e5..483d43a5ef 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_threadsafe_state_storage.cpp @@ -68,10 +68,10 @@ class TestThreadSafeStateStorage : public ompl_interface_testing::LoadTestRobot, auto robot_state_stored = tss.getStateStorage(); // Check if robot_state_stored's joint angles matches with what we set - for (auto const& joint_name : robot_state_->getVariableNames()) + for (const auto& joint_name : robot_state_->getVariableNames()) { - auto const expected_value = robot_state_->getVariablePosition(joint_name); - auto const actual_value = robot_state_stored->getVariablePosition(joint_name); + const auto expected_value = robot_state_->getVariablePosition(joint_name); + const auto actual_value = robot_state_stored->getVariablePosition(joint_name); EXPECT_EQ(actual_value, expected_value) << "Expecting joint value for " << joint_name << " to match."; } } diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 154eac5aa1..da92788709 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -154,7 +154,7 @@ void TrajectoryGenerator::checkJointGoalConstraint(const moveit_msgs::msg::Const const std::vector& expected_joint_names, const std::string& group_name) const { - for (auto const& joint_constraint : constraint.joint_constraints) + for (const auto& joint_constraint : constraint.joint_constraints) { const std::string& curr_joint_name{ joint_constraint.joint_name }; if (std::find(expected_joint_names.cbegin(), expected_joint_names.cend(), curr_joint_name) == diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index bb665bca53..5a1dada4c6 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -102,7 +102,7 @@ void TrajectoryGeneratorPTP::planPTP(const std::map& start_ // check if goal already reached bool goal_reached = true; - for (auto const& goal : goal_pos) + for (const auto& goal : goal_pos) { if (fabs(start_pos.at(goal.first) - goal.second) >= MIN_MOVEMENT) { 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 45bf3e64d0..f56b838741 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -72,7 +72,7 @@ static constexpr double DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE{ 1e-8 }; /** * @brief Convert degree to rad. */ -inline static constexpr double deg2Rad(double angle) +static inline constexpr double deg2Rad(double angle) { return (angle / 180.0) * M_PI; } @@ -88,7 +88,7 @@ inline std::string getJointName(size_t joint_number, const std::string& joint_pr */ pilz_industrial_motion_planner::JointLimitsContainer createFakeLimits(const std::vector& joint_names); -inline std::string demangle(char const* name) +inline std::string demangle(const char* name) { return boost::core::demangle(name); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp index cad029206a..110f67b418 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_get_solver_tip_frame.cpp @@ -59,7 +59,7 @@ class SolverMock class JointModelGroupMock { public: - MOCK_CONST_METHOD0(getSolverInstance, SolverMock const*()); + MOCK_CONST_METHOD0(getSolverInstance, const SolverMock*()); MOCK_CONST_METHOD0(getName, const std::string&()); }; diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h index c523df53d1..213c11a321 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/include/pilz_industrial_motion_planner_testutils/xml_testdata_loader.h @@ -173,7 +173,7 @@ class XmlTestdataLoader : public TestdataLoader /** * @brief Converts string vector to double vector. */ - inline static std::vector strVec2doubleVec(std::vector& strVec); + static inline std::vector strVec2doubleVec(std::vector& strVec); public: /** diff --git a/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp index 502f2394fc..8d96090a69 100644 --- a/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/filter_functions.hpp @@ -50,7 +50,7 @@ namespace stomp_moveit namespace filters { // \brief An empty placeholder filter that doesn't apply any updates to the trajectory. -const static FilterFn NO_FILTER = [](const Eigen::MatrixXd& /*values*/, Eigen::MatrixXd& /*filtered_values*/) { +static const FilterFn NO_FILTER = [](const Eigen::MatrixXd& /*values*/, Eigen::MatrixXd& /*filtered_values*/) { return true; }; diff --git a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp index da474c223e..e32528f2db 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp @@ -87,7 +87,7 @@ class StompPlannerManager : public PlannerManager return nullptr; } - auto const params = param_listener_->get_params(); + const auto params = param_listener_->get_params(); std::shared_ptr planning_context = std::make_shared("STOMP", req.group_name, params); diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index 7308854c6c..cdf471fb49 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -112,7 +112,7 @@ BenchmarkExecutor::~BenchmarkExecutor() return false; } - auto const& pipeline = moveit_cpp_->getPlanningPipelines().at(planning_pipeline_name); + const auto& pipeline = moveit_cpp_->getPlanningPipelines().at(planning_pipeline_name); // Verify the pipeline has successfully initialized a planner if (!pipeline->getPlannerManager()) { @@ -861,7 +861,7 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request std::chrono::system_clock::time_point start = std::chrono::system_clock::now(); // Planning pipeline benchmark - auto const response = planning_component->plan(plan_req_params, planning_scene_); + const auto response = planning_component->plan(plan_req_params, planning_scene_); solved[j] = bool(response.error_code); @@ -924,7 +924,7 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request // Create multi-pipeline request moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request; - for (auto const& pipeline_planner_id_pair : parallel_pipeline_entry.second) + for (const auto& pipeline_planner_id_pair : parallel_pipeline_entry.second) { moveit_cpp::PlanningComponent::PlanRequestParameters plan_req_params = { .planner_id = pipeline_planner_id_pair.second, @@ -959,11 +959,11 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request // Solve problem std::chrono::system_clock::time_point start = std::chrono::system_clock::now(); - auto const t1 = std::chrono::system_clock::now(); - auto const response = planning_component->plan(multi_pipeline_plan_request, + const auto t1 = std::chrono::system_clock::now(); + const auto response = planning_component->plan(multi_pipeline_plan_request, &moveit::planning_pipeline_interfaces::getShortestSolution, nullptr, planning_scene_); - auto const t2 = std::chrono::system_clock::now(); + const auto t2 = std::chrono::system_clock::now(); solved[j] = bool(response.error_code); diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 3e76ac485e..fba77abfc4 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -255,10 +255,10 @@ bool BenchmarkOptions::readPlannerConfigs(const rclcpp::Node::SharedPtr& node) parallel_planning_pipelines[parallel_pipeline] = pipeline_planner_id_pairs; - for (auto const& entry : parallel_planning_pipelines) + for (const auto& entry : parallel_planning_pipelines) { RCLCPP_INFO(LOGGER, "Parallel planning pipeline '%s'", entry.first.c_str()); - for (auto const& pair : entry.second) + for (const auto& pair : entry.second) { RCLCPP_INFO(LOGGER, " '%s': '%s'", pair.first.c_str(), pair.second.c_str()); } diff --git a/moveit_ros/benchmarks/src/RunBenchmark.cpp b/moveit_ros/benchmarks/src/RunBenchmark.cpp index c718622765..d12952b168 100644 --- a/moveit_ros/benchmarks/src/RunBenchmark.cpp +++ b/moveit_ros/benchmarks/src/RunBenchmark.cpp @@ -97,7 +97,7 @@ int main(int argc, char** argv) return 1; } // Running benchmarks - for (auto const& name : scene_names) + for (const auto& name : scene_names) { options.scene_name = name; if (!server.runBenchmarks(options)) diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index e42ed04d94..2176b9db7d 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -261,7 +261,7 @@ GLuint mesh_filter::GLRenderer::createShader(GLuint shaderType, const string& Sh GLuint shader_id = glCreateShader(shaderType); // Compile Shader - char const* source_pointer = ShaderCode.c_str(); + const char* source_pointer = ShaderCode.c_str(); glShaderSource(shader_id, 1, &source_pointer, nullptr); glCompileShader(shader_id); diff --git a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp index d7a498a280..0546581165 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp +++ b/moveit_ros/planning/kinematics_plugin_loader/src/kinematics_plugin_loader.cpp @@ -140,7 +140,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl // just to be sure, do not call the same pluginlib instance allocation function in parallel std::scoped_lock slock(lock_); - for (auto const& [group, solver] : possible_kinematics_solvers_) + for (const auto& [group, solver] : possible_kinematics_solvers_) { // Don't bother trying to load a solver for the wrong group if (group != jmg->getName()) @@ -206,7 +206,7 @@ class KinematicsPluginLoader::KinematicsLoaderImpl void status() const { - for (auto const& [group, solver] : possible_kinematics_solvers_) + for (const auto& [group, solver] : possible_kinematics_solvers_) { RCLCPP_INFO(LOGGER, "Solver for group '%s': '%s' (search resolution = %lf)", group.c_str(), solver.c_str(), search_res_.at(group)); diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index f7101404ba..d161be7f83 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -189,7 +189,7 @@ planning_interface::MotionPlanResponse PlanningComponent::plan( planning_scene->setCurrentState(request.start_state); } - auto const motion_plan_response_vector = moveit::planning_pipeline_interfaces::planWithParallelPipelines( + const auto motion_plan_response_vector = moveit::planning_pipeline_interfaces::planWithParallelPipelines( requests, planning_scene, moveit_cpp_->getPlanningPipelines(), stopping_criterion_callback, solution_selection_function); @@ -354,7 +354,7 @@ std::vector<::planning_interface::MotionPlanRequest> PlanningComponent::getMotio { std::vector<::planning_interface::MotionPlanRequest> motion_plan_requests; motion_plan_requests.reserve(multi_pipeline_plan_request_parameters.plan_request_parameter_vector.size()); - for (auto const& plan_request_parameters : multi_pipeline_plan_request_parameters.plan_request_parameter_vector) + for (const auto& plan_request_parameters : multi_pipeline_plan_request_parameters.plan_request_parameter_vector) { motion_plan_requests.push_back(getMotionPlanRequest(plan_request_parameters)); } diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index d689397f44..56ddb80c84 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -76,7 +76,7 @@ const std::vector<::planning_interface::MotionPlanResponse> planWithParallelPipe // Print a warning if more parallel planning problems than available concurrent threads are defined. If // std::thread::hardware_concurrency() is not defined, the command returns 0 so the check does not work - auto const hardware_concurrency = std::thread::hardware_concurrency(); + const auto hardware_concurrency = std::thread::hardware_concurrency(); if (motion_plan_requests.size() > hardware_concurrency && hardware_concurrency != 0) { RCLCPP_WARN(LOGGER, diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp index f3fc96de98..121e2b4c52 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/solution_selection_functions.cpp @@ -44,7 +44,7 @@ ::planning_interface::MotionPlanResponse getShortestSolution(const std::vector<::planning_interface::MotionPlanResponse>& solutions) { // Find trajectory with minimal path - auto const shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), + const auto shortest_trajectory = std::min_element(solutions.begin(), solutions.end(), [](const ::planning_interface::MotionPlanResponse& solution_a, const ::planning_interface::MotionPlanResponse& solution_b) { // If both solutions were successful, check which path is shorter diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp index f0bf424cee..8bfe9add7d 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/stopping_criterion_function.cpp @@ -44,7 +44,7 @@ bool stopAtFirstSolution(const PlanResponsesContainer& plan_responses_container, const std::vector<::planning_interface::MotionPlanRequest>& /*plan_requests*/) { // Stop at the first successful plan - for (auto const& solution : plan_responses_container.getSolutions()) + for (const auto& solution : plan_responses_container.getSolutions()) { // bool(solution) is shorthand to evaluate the error code of the solution, checking for SUCCESS if (bool(solution)) diff --git a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp index 189e3b6c4e..fa36d0b5ab 100644 --- a/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp +++ b/moveit_ros/planning/rdf_loader/src/synchronized_string_parameter.cpp @@ -120,8 +120,8 @@ bool SynchronizedStringParameter::shouldPublish() bool SynchronizedStringParameter::waitForMessage(const rclcpp::Duration& timeout) { - auto const nd_name = std::string(node_->get_name()).append("_ssp_").append(name_); - auto const temp_node = std::make_shared(nd_name); + const auto nd_name = std::string(node_->get_name()).append("_ssp_").append(name_); + const auto temp_node = std::make_shared(nd_name); string_subscriber_ = temp_node->create_subscription( name_, rclcpp::QoS(1).transient_local().reliable(), // "transient_local()" is required for supporting late subscriptions diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 3259fbb9be..87086fe6fe 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -1650,7 +1650,7 @@ void MoveGroupInterface::getJointValueTarget(std::vector& group_variable bool MoveGroupInterface::setJointValueTarget(const std::vector& joint_values) { - auto const n_group_joints = impl_->getJointModelGroup()->getVariableCount(); + const auto n_group_joints = impl_->getJointModelGroup()->getVariableCount(); if (joint_values.size() != n_group_joints) { RCLCPP_DEBUG_STREAM(LOGGER, "Provided joint value list has length " << joint_values.size() << " but group "