From 567de39c47bffb8b3a0d2a8fec1323ffa91250ee Mon Sep 17 00:00:00 2001 From: Shobuj Paul Date: Sat, 14 Oct 2023 17:52:10 +0530 Subject: [PATCH] Removed unrequired const keyword --- .../src/collision_octomap_filter.cpp | 28 +++++++++---------- .../bullet_integration/basic_types.h | 4 +-- .../constraint_samplers/test/pr2_arm_ik.cpp | 2 +- .../constraint_samplers/test/pr2_arm_ik.h | 10 +++---- .../test/pr2_arm_kinematics_plugin.cpp | 8 +++--- .../test/pr2_arm_kinematics_plugin.h | 6 ++-- .../robot_trajectory/robot_trajectory.h | 2 +- .../robot_trajectory/src/robot_trajectory.cpp | 2 +- .../ikfast61_moveit_plugin_template.cpp | 4 +-- .../ompl_interface/detail/ompl_constraints.h | 2 +- .../trajectory_functions.h | 4 +-- .../trajectory_generator.h | 14 +++++----- .../trajectory_generator_circ.h | 2 +- .../trajectory_generator_lin.h | 2 +- .../trajectory_generator_ptp.h | 6 ++-- .../src/trajectory_functions.cpp | 6 ++-- .../src/trajectory_generator.cpp | 8 +++--- .../src/trajectory_generator_circ.cpp | 2 +- .../src/trajectory_generator_lin.cpp | 2 +- .../src/trajectory_generator_ptp.cpp | 6 ++-- .../test/test_utils.cpp | 6 ++-- .../test/test_utils.h | 8 +++--- .../src/unittest_trajectory_functions.cpp | 4 +-- .../prbt_manipulator_ikfast_moveit_plugin.cpp | 4 +-- .../trajopt/src/problem_description.cpp | 6 ++-- .../navigation_widget.hpp | 6 ++-- .../src/navigation_widget.cpp | 6 ++-- 27 files changed, 80 insertions(+), 80 deletions(-) diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index e187ba6af28..260a334efda 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -48,21 +48,21 @@ static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection.collision_octomap_filter"); // forward declarations -bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double spacing, const double iso_value, - const double r_multiple, const octomath::Vector3& contact_point, +bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value, + double r_multiple, const octomath::Vector3& contact_point, octomath::Vector3& normal, double& depth, bool estimate_depth); -bool findSurface(const octomap::point3d_list& cloud, const double spacing, const double iso_value, - const double r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point, +bool findSurface(const octomap::point3d_list& cloud, double spacing, double iso_value, + double r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point, octomath::Vector3& normal); -bool sampleCloud(const octomap::point3d_list& cloud, const double spacing, const double r_multiple, +bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_multiple, const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient); int collision_detection::refineContactNormals(const World::ObjectConstPtr& object, CollisionResult& res, - const double cell_bbx_search_distance, - const double allowed_angle_divergence, const bool estimate_depth, - const double iso_value, const double metaball_radius_multiple) + double cell_bbx_search_distance, + double allowed_angle_divergence, bool estimate_depth, + double iso_value, double metaball_radius_multiple) { if (!object) { @@ -170,9 +170,9 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec return modified; } -bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const double spacing, const double iso_value, - const double r_multiple, const octomath::Vector3& contact_point, - octomath::Vector3& normal, double& depth, const bool estimate_depth) +bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, double spacing, double iso_value, + double r_multiple, const octomath::Vector3& contact_point, + octomath::Vector3& normal, double& depth, bool estimate_depth) { if (estimate_depth) { @@ -207,8 +207,8 @@ bool getMetaballSurfaceProperties(const octomap::point3d_list& cloud, const doub // This algorithm is from Salisbury & Tarr's 1997 paper. It will find the // closest point on the surface starting from a seed point that is close by // following the direction of the field gradient. -bool findSurface(const octomap::point3d_list& cloud, const double spacing, const double iso_value, - const double r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point, +bool findSurface(const octomap::point3d_list& cloud, double spacing, double iso_value, + double r_multiple, const octomath::Vector3& seed, octomath::Vector3& surface_point, octomath::Vector3& normal) { octomath::Vector3 p = seed, dp, gs; @@ -233,7 +233,7 @@ bool findSurface(const octomap::point3d_list& cloud, const double spacing, const // return p; } -bool sampleCloud(const octomap::point3d_list& cloud, const double spacing, const double r_multiple, +bool sampleCloud(const octomap::point3d_list& cloud, double spacing, double r_multiple, const octomath::Vector3& position, double& intensity, octomath::Vector3& gradient) { intensity = 0.f; diff --git a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h index 283cfa628c2..073a29482eb 100644 --- a/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h +++ b/moveit_core/collision_detection_bullet/include/moveit/collision_detection_bullet/bullet_integration/basic_types.h @@ -59,7 +59,7 @@ struct ContactTestData { EIGEN_MAKE_ALIGNED_OPERATOR_NEW - ContactTestData(const std::vector& active, const double contact_distance, + ContactTestData(const std::vector& active, double contact_distance, collision_detection::CollisionResult& res, const collision_detection::CollisionRequest& req) : active(active), contact_distance(contact_distance), res(res), req(req), done(false), pair_done(false) { @@ -68,7 +68,7 @@ struct ContactTestData const std::vector& active; /** \brief If after a positive broadphase check the distance is below this threshold, a contact is added. */ - const double contact_distance; + double contact_distance; collision_detection::CollisionResult& res; const collision_detection::CollisionRequest& req; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index e3dd992f0ce..1df9d8c7af6 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -198,7 +198,7 @@ void PR2ArmIK::getSolverInfo(moveit_msgs::msg::KinematicSolverInfo& info) info = solver_info_; } -void PR2ArmIK::computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double t1_in, +void PR2ArmIK::computeIKShoulderPan(const Eigen::Isometry3f& g_in, double t1_in, std::vector >& solution) const { // t1 = shoulder/turret pan is specified diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.h b/moveit_core/constraint_samplers/test/pr2_arm_ik.h index 9bfe2ac80d3..a7633c6ef4f 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.h @@ -58,7 +58,7 @@ inline double distance(const urdf::Pose& transform) transform.position.z * transform.position.z); } -inline bool solveQuadratic(const double a, const double b, const double c, double* x1, double* x2) +inline bool solveQuadratic(double a, double b, double c, double* x1, double* x2) { double discriminant = b * b - 4 * a * c; if (fabs(a) < IK_EPS) @@ -88,7 +88,7 @@ inline bool solveQuadratic(const double a, const double b, const double c, doubl } } -inline bool solveCosineEqn(const double a, const double b, const double c, double& soln1, double& soln2) +inline bool solveCosineEqn(double a, double b, double c, double& soln1, double& soln2) { double theta1 = atan2(b, a); double denom = sqrt(a * a + b * b); @@ -140,7 +140,7 @@ class PR2ArmIK @param Input pose for end-effector @param Initial guess for shoulder pan angle */ - void computeIKShoulderPan(const Eigen::Isometry3f& g_in, const double shoulder_pan_initial_guess, + void computeIKShoulderPan(const Eigen::Isometry3f& g_in, double shoulder_pan_initial_guess, std::vector >& solution) const; /** @@ -148,7 +148,7 @@ class PR2ArmIK h @param Input pose for end-effector @param Initial guess for shoulder roll angle */ - void computeIKShoulderRoll(const Eigen::Isometry3f& g_in, const double shoulder_roll_initial_guess, + void computeIKShoulderRoll(const Eigen::Isometry3f& g_in, double shoulder_roll_initial_guess, std::vector >& solution) const; // std::vector > solution_ik_;/// a vector of ik solutions @@ -172,7 +172,7 @@ class PR2ArmIK bool checkJointLimits(const std::vector& joint_values) const; - bool checkJointLimits(const double joint_value, const int joint_num) const; + bool checkJointLimits(double joint_value, int joint_num) const; Eigen::Isometry3f grhs_, gf_, home_inv_; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index 1d3b8a282bc..b1dd1d2bbab 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -50,7 +50,7 @@ namespace pr2_arm_kinematics { static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin"); -bool PR2ArmIKSolver::getCount(int& count, const int max_count, const int min_count) +bool PR2ArmIKSolver::getCount(int& count, int max_count, int min_count) { if (count > 0) { @@ -87,8 +87,8 @@ bool PR2ArmIKSolver::getCount(int& count, const int max_count, const int min_cou } PR2ArmIKSolver::PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name, - const std::string& tip_frame_name, const double search_discretization_angle, - const int free_angle) + const std::string& tip_frame_name, double search_discretization_angle, + int free_angle) : ChainIkSolverPos() { search_discretization_angle_ = search_discretization_angle; @@ -160,7 +160,7 @@ int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_i } int PR2ArmIKSolver::cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, - const double timeout) + double timeout) { const bool verbose = false; KDL::JntArray q_init = q_in; diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h index d5e29fd1701..1d231fc9322 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h @@ -80,7 +80,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos * to return multiple solutions from an inverse kinematics computation. */ PR2ArmIKSolver(const urdf::ModelInterface& robot_model, const std::string& root_frame_name, - const std::string& tip_frame_name, const double search_discretization_angle, const int free_angle); + const std::string& tip_frame_name, double search_discretization_angle, int free_angle); ~PR2ArmIKSolver() override{}; @@ -98,7 +98,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos int CartToJnt(const KDL::JntArray& q_init, const KDL::Frame& p_in, KDL::JntArray& q_out) override; - int cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, const double timeout); + int cartToJntSearch(const KDL::JntArray& q_in, const KDL::Frame& p_in, KDL::JntArray& q_out, double timeout); void getSolverInfo(moveit_msgs::msg::KinematicSolverInfo& response) { @@ -106,7 +106,7 @@ class PR2ArmIKSolver : public KDL::ChainIkSolverPos } private: - bool getCount(int& count, const int max_count, const int min_count); + bool getCount(int& count, int max_count, int min_count); double search_discretization_angle_; 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 d5f24873668..26e7f9da4fb 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 @@ -300,7 +300,7 @@ class RobotTrajectory * @param The waypoint index after (or equal to) the supplied duration. * @param The progress (0 to 1) between the two waypoints, based on time (not based on joint distances). */ - void findWayPointIndicesForDurationAfterStart(const double duration, int& before, int& after, double& blend) const; + void findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const; // TODO support visitor function for interpolation, or at least different types. /** @brief Gets a robot state corresponding to a supplied duration from start for the trajectory, using linear time diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index 19eb32d2ec8..8709ed35e28 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -490,7 +490,7 @@ RobotTrajectory& RobotTrajectory::setRobotTrajectoryMsg(const moveit::core::Robo return setRobotTrajectoryMsg(st, trajectory); } -void RobotTrajectory::findWayPointIndicesForDurationAfterStart(const double duration, int& before, int& after, +void RobotTrajectory::findWayPointIndicesForDurationAfterStart(double duration, int& before, int& after, double& blend) const { if (duration < 0.0) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index a34c490e7f0..d0c71b113e5 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -353,7 +353,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase double enforceLimits(double val, double min, double max) const; void fillFreeParams(int count, int* array); - bool getCount(int& count, const int max_count, const int min_count) const; + bool getCount(int& count, int max_count, int min_count) const; /** * @brief samples the designated redundant joint using the chosen discretization method @@ -728,7 +728,7 @@ void IKFastKinematicsPlugin::fillFreeParams(int count, int* array) free_params_.push_back(array[i]); } -bool IKFastKinematicsPlugin::getCount(int& count, const int max_count, const int min_count) const +bool IKFastKinematicsPlugin::getCount(int& count, int max_count, int min_count) const { if (count > 0) { diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h index 5d77b02f4e4..93b2cc8bc22 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/ompl_constraints.h @@ -446,7 +446,7 @@ ompl::base::ConstraintPtr createOMPLConstraints(const moveit::core::RobotModelCo * https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2016/RD2016script.pdf * Eq. 2.107 * */ -inline Eigen::Matrix3d angularVelocityToAngleAxis(const double angle, const Eigen::Ref& axis) +inline Eigen::Matrix3d angularVelocityToAngleAxis(double angle, const Eigen::Ref& axis) { double t{ std::abs(angle) }; Eigen::Matrix3d r_skew; diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h index 1214f2d0cff..1dd242b703d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h @@ -126,7 +126,7 @@ bool verifySampleJointLimits(const std::map& position_last, bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr& scene, const JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, const double sampling_time, + const std::map& initial_joint_position, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision = false); @@ -200,7 +200,7 @@ bool linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Ve std::size_t& index); bool intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, - const double r); + double r); /** * @brief Checks if current robot state is in self collision. diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h index 1658448fd7d..27f976bf695 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator.h @@ -136,8 +136,8 @@ class TrajectoryGenerator * The trap profile returns uses the longer distance of translational and * rotational motion. */ - std::unique_ptr cartesianTrapVelocityProfile(const double max_velocity_scaling_factor, - const double max_acceleration_scaling_factor, + std::unique_ptr cartesianTrapVelocityProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, const std::unique_ptr& path) const; private: @@ -157,7 +157,7 @@ class TrajectoryGenerator virtual void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) = 0; private: /** @@ -247,9 +247,9 @@ class TrajectoryGenerator /** * @return True if scaling factor is valid, otherwise false. */ - static bool isScalingFactorValid(const double scaling_factor); - static void checkVelocityScaling(const double scaling_factor); - static void checkAccelerationScaling(const double scaling_factor); + static bool isScalingFactorValid(double scaling_factor); + static void checkVelocityScaling(double scaling_factor); + static void checkAccelerationScaling(double scaling_factor); /** * @return True if ONE position + ONE orientation constraint given, @@ -277,7 +277,7 @@ class TrajectoryGenerator const std::unique_ptr clock_; }; -inline bool TrajectoryGenerator::isScalingFactorValid(const double scaling_factor) +inline bool TrajectoryGenerator::isScalingFactorValid(double scaling_factor) { return (scaling_factor > MIN_SCALING_FACTOR && scaling_factor <= MAX_SCALING_FACTOR); } diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h index 952489ff020..2246b2c3d69 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_circ.h @@ -91,7 +91,7 @@ class TrajectoryGeneratorCIRC : public TrajectoryGenerator const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, const double sampling_time, + const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h index b1e2a502415..19e48dac38a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_lin.h @@ -74,7 +74,7 @@ class TrajectoryGeneratorLIN : public TrajectoryGenerator const planning_interface::MotionPlanRequest& req, MotionPlanInfo& info) const final; void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, const double sampling_time, + const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; /** diff --git a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h index d52398f1819..cc73ff4839f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h +++ b/moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_generator_ptp.h @@ -79,11 +79,11 @@ class TrajectoryGeneratorPTP : public TrajectoryGenerator * @param sampling_time */ void planPTP(const std::map& start_pos, const std::map& goal_pos, - trajectory_msgs::msg::JointTrajectory& joint_trajectory, const double velocity_scaling_factor, - const double acceleration_scaling_factor, const double sampling_time); + trajectory_msgs::msg::JointTrajectory& joint_trajectory, double velocity_scaling_factor, + double acceleration_scaling_factor, double sampling_time); void plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, - const MotionPlanInfo& plan_info, const double sampling_time, + const MotionPlanInfo& plan_info, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) override; private: diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp index 2df5e21911d..1c9cb805c62 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_functions.cpp @@ -201,7 +201,7 @@ bool pilz_industrial_motion_planner::generateJointTrajectory( const planning_scene::PlanningSceneConstPtr& scene, const pilz_industrial_motion_planner::JointLimitsContainer& joint_limits, const KDL::Trajectory& trajectory, const std::string& group_name, const std::string& link_name, - const std::map& initial_joint_position, const double sampling_time, + const std::map& initial_joint_position, double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory, moveit_msgs::msg::MoveItErrorCodes& error_code, bool check_self_collision) { @@ -526,7 +526,7 @@ bool pilz_industrial_motion_planner::isRobotStateStationary(const moveit::core:: bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::string& link_name, const Eigen::Vector3d& center_position, - const double r, + double r, const robot_trajectory::RobotTrajectoryPtr& traj, bool inverseOrder, std::size_t& index) { @@ -564,7 +564,7 @@ bool pilz_industrial_motion_planner::linearSearchIntersectionPoint(const std::st bool pilz_industrial_motion_planner::intersectionFound(const Eigen::Vector3d& p_center, const Eigen::Vector3d& p_current, const Eigen::Vector3d& p_next, - const double r) + double r) { return ((p_current - p_center).norm() <= r) && ((p_next - p_center).norm() >= r); } 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 4d8b3af5a55..75bb14d7ace 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -79,7 +79,7 @@ void TrajectoryGenerator::cmdSpecificRequestValidation(const planning_interface: // to provide a command specific request validation. } -void TrajectoryGenerator::checkVelocityScaling(const double scaling_factor) +void TrajectoryGenerator::checkVelocityScaling(double scaling_factor) { if (!isScalingFactorValid(scaling_factor)) { @@ -90,7 +90,7 @@ void TrajectoryGenerator::checkVelocityScaling(const double scaling_factor) } } -void TrajectoryGenerator::checkAccelerationScaling(const double scaling_factor) +void TrajectoryGenerator::checkAccelerationScaling(double scaling_factor) { if (!isScalingFactorValid(scaling_factor)) { @@ -281,8 +281,8 @@ void TrajectoryGenerator::setFailureResponse(const rclcpp::Time& planning_start, } std::unique_ptr -TrajectoryGenerator::cartesianTrapVelocityProfile(const double max_velocity_scaling_factor, - const double max_acceleration_scaling_factor, +TrajectoryGenerator::cartesianTrapVelocityProfile(double max_velocity_scaling_factor, + double max_acceleration_scaling_factor, const std::unique_ptr& path) const { std::unique_ptr vp_trans = std::make_unique( diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 344fc983ee2..2748e11fe62 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -188,7 +188,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni void TrajectoryGeneratorCIRC::plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { std::unique_ptr cart_path(setPathCIRC(plan_info)); std::unique_ptr vel_profile( diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 5c26be41740..50e25a8f7ee 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -145,7 +145,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // create Cartesian path for lin std::unique_ptr path(setPathLIN(plan_info.start_pose, plan_info.goal_pose)); 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 5c7798a822f..95623762d14 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 @@ -91,8 +91,8 @@ TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelCon void TrajectoryGeneratorPTP::planPTP(const std::map& start_pos, const std::map& goal_pos, trajectory_msgs::msg::JointTrajectory& joint_trajectory, - const double velocity_scaling_factor, const double acceleration_scaling_factor, - const double sampling_time) + double velocity_scaling_factor, double acceleration_scaling_factor, + double sampling_time) { // initialize joint names for (const auto& item : goal_pos) @@ -245,7 +245,7 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin void TrajectoryGeneratorPTP::plan(const planning_scene::PlanningSceneConstPtr& /*scene*/, const planning_interface::MotionPlanRequest& req, const MotionPlanInfo& plan_info, - const double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) + double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory) { // plan the ptp trajectory planPTP(plan_info.start_joint_position, plan_info.goal_joint_position, joint_trajectory, 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 8958ae2b28d..b3b98569e8e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -926,7 +926,7 @@ bool testutils::checkBlendingCartSpaceContinuity(const pilz_industrial_motion_pl return true; } -bool testutils::checkThatPointsInRadius(const std::string& link_name, const double r, Eigen::Isometry3d& circ_pose, +bool testutils::checkThatPointsInRadius(const std::string& link_name, double r, Eigen::Isometry3d& circ_pose, const pilz_industrial_motion_planner::TrajectoryBlendResponse& res) { bool result = true; @@ -1052,8 +1052,8 @@ bool testutils::getBlendTestData(const rclcpp::Node::SharedPtr& node, const size bool testutils::generateTrajFromBlendTestData( const planning_scene::PlanningSceneConstPtr& scene, const std::shared_ptr& tg, const std::string& group_name, - const std::string& link_name, const testutils::BlendTestData& data, const double sampling_time_1, - const double sampling_time_2, planning_interface::MotionPlanResponse& res_1, + const std::string& link_name, const testutils::BlendTestData& data, double sampling_time_1, + double sampling_time_2, planning_interface::MotionPlanResponse& res_1, planning_interface::MotionPlanResponse& res_2, double& dis_1, double& dis_2) { const moveit::core::RobotModelConstPtr robot_model = scene->getRobotModel(); 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 61bbfc28b3a..7dfe10dd597 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -344,7 +344,7 @@ bool checkBlendingCartSpaceContinuity(const pilz_industrial_motion_planner::Traj * @brief Checks if all points of the blending trajectory lie within the * blending radius. */ -bool checkThatPointsInRadius(const std::string& link_name, const double r, Eigen::Isometry3d& circ_pose, +bool checkThatPointsInRadius(const std::string& link_name, double r, Eigen::Isometry3d& circ_pose, const pilz_industrial_motion_planner::TrajectoryBlendResponse& res); /** @@ -434,8 +434,8 @@ bool checkBlendResult(const pilz_industrial_motion_planner::TrajectoryBlendReque bool generateTrajFromBlendTestData(const planning_scene::PlanningSceneConstPtr& scene, const std::shared_ptr& tg, const std::string& group_name, const std::string& link_name, - const BlendTestData& data, const double sampling_time_1, - const double sampling_time_2, planning_interface::MotionPlanResponse& res_lin_1, + const BlendTestData& data, double sampling_time_1, + double sampling_time_2, planning_interface::MotionPlanResponse& res_lin_1, planning_interface::MotionPlanResponse& res_lin_2, double& dis_lin_1, double& dis_lin_2); @@ -474,7 +474,7 @@ checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& tr const double rot_axis_tol = DEFAULT_ROTATION_AXIS_EQUALITY_TOLERANCE, const double acc_tol = DEFAULT_ACCELERATION_EQUALITY_TOLERANCE); -inline bool isMonotonouslyDecreasing(const std::vector& vec, const double tol) +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 diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp index 726ff0a03cb..18a1bac7c08 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp @@ -132,7 +132,7 @@ class TrajectoryFunctionsTestBase : public testing::Test * @param epsilon * @return */ - bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, const double epsilon); + bool tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, double epsilon); protected: // ros stuff @@ -153,7 +153,7 @@ class TrajectoryFunctionsTestBase : public testing::Test }; bool TrajectoryFunctionsTestBase::tfNear(const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2, - const double epsilon) + double epsilon) { for (std::size_t i = 0; i < 3; ++i) { diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp index ce6647269a5..d035de8c15e 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/src/prbt_manipulator_ikfast_moveit_plugin.cpp @@ -358,7 +358,7 @@ class IKFastKinematicsPlugin : public kinematics::KinematicsBase double enforceLimits(double val, double min, double max) const; void fillFreeParams(int count, int* array); - bool getCount(int& count, const int max_count, const int min_count) const; + bool getCount(int& count, int max_count, int min_count) const; /** * @brief samples the designated redundant joint using the chosen discretization method @@ -732,7 +732,7 @@ void IKFastKinematicsPlugin::fillFreeParams(int count, int* array) free_params_.push_back(array[i]); } -bool IKFastKinematicsPlugin::getCount(int& count, const int max_count, const int min_count) const +bool IKFastKinematicsPlugin::getCount(int& count, int max_count, int min_count) const { if (count > 0) { diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp index 2c9db59a705..9fd3803ad01 100644 --- a/moveit_planners/trajopt/src/problem_description.cpp +++ b/moveit_planners/trajopt/src/problem_description.cpp @@ -57,8 +57,8 @@ * @param name The name to use when printing an error or warning * @param apply_first If true and only one value is given, broadcast value to length of expected_size */ -void checkParameterSize(trajopt::DblVec& parameter, const unsigned int& expected_size, const std::string& name, - const bool apply_first = true) +void checkParameterSize(trajopt::DblVec& parameter, unsigned int expected_size, const std::string& name, + bool apply_first = true) { if (apply_first == true && parameter.size() == 1) { @@ -242,7 +242,7 @@ TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) // Apply constraint to each fixed dof to its initial value for all timesteps (freeze that column) if (!bi.dofs_fixed.empty()) { - for (const int dof_ind : bi.dofs_fixed) + for (int dof_ind : bi.dofs_fixed) { for (int i = 1; i < prob->GetNumSteps(); ++i) { diff --git a/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp index 35b0c121a06..d6195a6e4b7 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp +++ b/moveit_setup_assistant/moveit_setup_assistant/include/moveit_setup_assistant/navigation_widget.hpp @@ -58,10 +58,10 @@ class NavigationWidget : public QListView explicit NavigationWidget(QWidget* parent = nullptr); void setNavs(const QList& navs); - void setEnabled(const int index, bool enabled); - void setSelected(const int index); + void setEnabled(int index, bool enabled); + void setSelected(int index); - bool isEnabled(const int index) const; + bool isEnabled(int index) const; private: QStandardItemModel* model_; diff --git a/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp b/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp index 879f12e5b2b..4c2bbe5e936 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp +++ b/moveit_setup_assistant/moveit_setup_assistant/src/navigation_widget.cpp @@ -90,7 +90,7 @@ void NavigationWidget::setNavs(const QList& navs) setModel(model_); } -void NavigationWidget::setEnabled(const int index, bool enabled) +void NavigationWidget::setEnabled(int index, bool enabled) { if (enabled) { @@ -103,7 +103,7 @@ void NavigationWidget::setEnabled(const int index, bool enabled) } } -void NavigationWidget::setSelected(const int index) +void NavigationWidget::setSelected(int index) { // First make sure item is enabled setEnabled(index, true); @@ -117,7 +117,7 @@ void NavigationWidget::setSelected(const int index) selectionModel()->select(selection, QItemSelectionModel::Select); } -bool NavigationWidget::isEnabled(const int index) const +bool NavigationWidget::isEnabled(int index) const { return model_->item(index)->flags() > Qt::NoItemFlags; }