diff --git a/moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp b/moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp index 535918c92d..9af20e1745 100644 --- a/moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp +++ b/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 diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index 6e4d44bebd..6c1e972d1e 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/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; diff --git a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp b/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp index f7843c8996..ccf24235ee 100644 --- a/moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp +++ b/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; diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp index cba0df41ef..0622b2dd55 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp +++ b/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& trajectory, double acceleration); void integrateBackward(std::list& 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::const_iterator getTrajectorySegment(double time) const; diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index 091ba1ab42..192a1c395d 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/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& 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::max();