Removed unrequired const keyword

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;

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<std::string>& active, const double contact_distance,
+  ContactTestData(const std::vector<std::string>& 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<std::string>& 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;

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<std::vector<double> >& solution) const
 {
   // t1 = shoulder/turret pan is specified

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,15 +140,15 @@ 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<std::vector<double> >& solution) const;
 
   /**
      @brief compute IK based on an initial guess for the shoulder roll angle.
      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<std::vector<double> >& solution) const;
 
   //  std::vector<std::vector<double> > solution_ik_;/// a vector of ik solutions
@@ -172,7 +172,7 @@ class PR2ArmIK
 
   bool checkJointLimits(const std::vector<double>& 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_;
 

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;

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,15 +98,15 @@ 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)
   {
     pr2_arm_ik_.getSolverInfo(response);
   }
 
 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_;
 

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

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)

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)
   {

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<const Eigen::Vector3d>& axis)
+inline Eigen::Matrix3d angularVelocityToAngleAxis(double angle, const Eigen::Ref<const Eigen::Vector3d>& axis)
 {
   double t{ std::abs(angle) };
   Eigen::Matrix3d r_skew;

moveit_planners/pilz_industrial_motion_planner/include/pilz_industrial_motion_planner/trajectory_functions.h

@@ -126,7 +126,7 @@ bool verifySampleJointLimits(const std::map<std::string, double>& 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<std::string, double>& initial_joint_position, const double sampling_time,
+                             const std::map<std::string, double>& 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.

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<KDL::VelocityProfile> cartesianTrapVelocityProfile(const double max_velocity_scaling_factor,
-                                                                     const double max_acceleration_scaling_factor,
+  std::unique_ptr<KDL::VelocityProfile> cartesianTrapVelocityProfile(double max_velocity_scaling_factor,
+                                                                     double max_acceleration_scaling_factor,
                                                                      const std::unique_ptr<KDL::Path>& 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<rclcpp::Clock> 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);
 }

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;
 
   /**

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;
 
   /**

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<std::string, double>& start_pos, const std::map<std::string, double>& 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:

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<std::string, double>& initial_joint_position, const double sampling_time,
+    const std::map<std::string, double>& 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);
 }