[Issue-879] Add const specifier to moveit_core (#3202)

* Added const specifier to two methods of the distance field class.

* Added const specifier to member functions of locally defined structs in cartesian_interpolator class.

* Added const specifier to some methods of the Trajectory class.

* Formatting

moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp

@@ -166,7 +166,8 @@ class DistanceField
    * @param [in] pose The pose of the shape.
    * @param [out] points The points determined for this shape.
    */
-  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose, EigenSTL::vector_Vector3d* points);
+  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose,
+                      EigenSTL::vector_Vector3d* points) const;
 
   /**
    * \brief Adds the set of points corresponding to the shape at the
@@ -583,7 +584,7 @@ class DistanceField
    * @param [in] octree The octree to find points for.
    * @param [out] points The points determined for this octree.
    */
-  void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points);
+  void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points) const;
 
   /**
    * \brief Helper function that sets the point value and color given

moveit_core/distance_field/src/distance_field.cpp

@@ -206,7 +206,7 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance,
 }
 
 bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose,
-                                   EigenSTL::vector_Vector3d* points)
+                                   EigenSTL::vector_Vector3d* points) const
 {
   if (shape->type == shapes::OCTREE)
   {
@@ -237,7 +237,7 @@ void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Iso
   addPointsToField(point_vec);
 }
 
-void DistanceField::getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points)
+void DistanceField::getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points) const
 {
   // lower extent
   double min_x, min_y, min_z;

moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp

@@ -115,15 +115,15 @@ class CartesianInterpolator
       if (value < 0.0 || value > 1.0)
         throw std::runtime_error("Percentage values must be between 0 and 1, inclusive");
     }
-    operator double()
+    operator double() const
     {
       return value;
     }
-    double operator*()
+    double operator*() const
     {
       return value;
     }
-    Percentage operator*(const Percentage& p)
+    Percentage operator*(const Percentage& p) const
     {
       Percentage res(value * p.value);
       return res;
@@ -136,15 +136,15 @@ class CartesianInterpolator
     Distance(double meters) : meters(meters)
     {
     }
-    operator double()
+    operator double() const
     {
       return meters;
     }
-    double operator*()
+    double operator*() const
     {
       return meters;
     }
-    Distance operator*(const Percentage& p)
+    Distance operator*(const Percentage& p) const
     {
       Distance res(meters * p.value);
       return res;

moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp

@@ -158,20 +158,20 @@ class Trajectory
   };
 
   bool getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
-                             double& after_acceleration);
+                             double& after_acceleration) const;
   bool getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
-                                         double& before_acceleration, double& after_acceleration);
+                                         double& before_acceleration, double& after_acceleration) const;
   bool getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
-                                     double& after_acceleration);
+                                     double& after_acceleration) const;
   bool integrateForward(std::list<TrajectoryStep>& trajectory, double acceleration);
   void integrateBackward(std::list<TrajectoryStep>& start_trajectory, double path_pos, double path_vel,
                          double acceleration);
-  double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max);
-  double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max);
+  double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max) const;
+  double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max) const;
   double getAccelerationMaxPathVelocity(double path_pos) const;
   double getVelocityMaxPathVelocity(double path_pos) const;
-  double getAccelerationMaxPathVelocityDeriv(double path_pos);
-  double getVelocityMaxPathVelocityDeriv(double path_pos);
+  double getAccelerationMaxPathVelocityDeriv(double path_pos) const;
+  double getVelocityMaxPathVelocityDeriv(double path_pos) const;
 
   std::list<TrajectoryStep>::const_iterator getTrajectorySegment(double time) const;
 

moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp

@@ -419,7 +419,7 @@ Trajectory::Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, co
 
 // Returns true if end of path is reached.
 bool Trajectory::getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
-                                       double& before_acceleration, double& after_acceleration)
+                                       double& before_acceleration, double& after_acceleration) const
 {
   TrajectoryStep acceleration_switching_point(path_pos, 0.0);
   double acceleration_before_acceleration, acceleration_after_acceleration;
@@ -466,7 +466,7 @@ bool Trajectory::getNextSwitchingPoint(double path_pos, TrajectoryStep& next_swi
 }
 
 bool Trajectory::getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
-                                                   double& before_acceleration, double& after_acceleration)
+                                                   double& before_acceleration, double& after_acceleration) const
 {
   double switching_path_pos = path_pos;
   double switching_path_vel;
@@ -516,7 +516,7 @@ bool Trajectory::getNextAccelerationSwitchingPoint(double path_pos, TrajectorySt
 }
 
 bool Trajectory::getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
-                                               double& before_acceleration, double& after_acceleration)
+                                               double& before_acceleration, double& after_acceleration) const
 {
   bool start = false;
   path_pos -= DEFAULT_TIMESTEP;
@@ -744,7 +744,7 @@ void Trajectory::integrateBackward(std::list<TrajectoryStep>& start_trajectory,
   end_trajectory_ = trajectory;
 }
 
-double Trajectory::getMinMaxPathAcceleration(double path_pos, double path_vel, bool max)
+double Trajectory::getMinMaxPathAcceleration(double path_pos, double path_vel, bool max) const
 {
   Eigen::VectorXd config_deriv = path_.getTangent(path_pos);
   Eigen::VectorXd config_deriv2 = path_.getCurvature(path_pos);
@@ -762,7 +762,7 @@ double Trajectory::getMinMaxPathAcceleration(double path_pos, double path_vel, b
   return factor * max_path_acceleration;
 }
 
-double Trajectory::getMinMaxPhaseSlope(double path_pos, double path_vel, bool max)
+double Trajectory::getMinMaxPhaseSlope(double path_pos, double path_vel, bool max) const
 {
   return getMinMaxPathAcceleration(path_pos, path_vel, max) / path_vel;
 }
@@ -809,13 +809,13 @@ double Trajectory::getVelocityMaxPathVelocity(double path_pos) const
   return max_path_velocity;
 }
 
-double Trajectory::getAccelerationMaxPathVelocityDeriv(double path_pos)
+double Trajectory::getAccelerationMaxPathVelocityDeriv(double path_pos) const
 {
   return (getAccelerationMaxPathVelocity(path_pos + EPS) - getAccelerationMaxPathVelocity(path_pos - EPS)) /
          (2.0 * EPS);
 }
 
-double Trajectory::getVelocityMaxPathVelocityDeriv(double path_pos)
+double Trajectory::getVelocityMaxPathVelocityDeriv(double path_pos) const
 {
   const Eigen::VectorXd tangent = path_.getTangent(path_pos);
   double max_path_velocity = std::numeric_limits<double>::max();