Skip to content

Commit

Permalink
Made changes as per comments
Browse files Browse the repository at this point in the history
  • Loading branch information
Shobuj-Paul committed Oct 17, 2023
1 parent 9937ad9 commit db0b936
Show file tree
Hide file tree
Showing 8 changed files with 20 additions and 21 deletions.
Expand Up @@ -59,7 +59,7 @@ struct ContactTestData
{
EIGEN_MAKE_ALIGNED_OPERATOR_NEW

ContactTestData(const std::vector<std::string>& active, double contact_distance,
ContactTestData(const std::vector<std::string>& active, const 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)
{
Expand All @@ -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. */
double contact_distance;
const double& contact_distance;

collision_detection::CollisionResult& res;
const collision_detection::CollisionRequest& req;
Expand Down
2 changes: 1 addition & 1 deletion moveit_core/kinematics_base/src/kinematics_base.cpp
Expand Up @@ -95,7 +95,7 @@ bool KinematicsBase::initialize(const rclcpp::Node::SharedPtr& /*node*/,

bool KinematicsBase::setRedundantJoints(const std::vector<unsigned int>& redundant_joint_indices)
{
for (const unsigned int& redundant_joint_index : redundant_joint_indices)
for (unsigned int redundant_joint_index : redundant_joint_indices)
{
if (redundant_joint_index >= getJointNames().size())
{
Expand Down
Expand Up @@ -64,9 +64,9 @@ std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& tra
* \param [in] min_angle_change The minimum angle change to use for time parameterization (default: 0.001).
* \return True if time parameterization was successful, false otherwise.
*/
bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
const double& acceleration_scaling_factor, const double path_tolerance = 0.1,
const double resample_dt = 0.1, const double min_angle_change = 0.001);
bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor,
double acceleration_scaling_factor, double path_tolerance = 0.1,
double resample_dt = 0.1, double min_angle_change = 0.001);
/**
* \brief Applies Ruckig smoothing to a robot trajectory.
* \param [in,out] trajectory The robot trajectory to be smoothed.
Expand All @@ -76,7 +76,7 @@ bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory
* \param [in] overshoot_threshold The maximum allowed overshoot during smoothing (default: 0.01).
* \return True if smoothing was successful, false otherwise.
*/
bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
const double& acceleration_scaling_factor, const bool mitigate_overshoot = false,
const double overshoot_threshold = 0.01);
bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor,
double acceleration_scaling_factor, bool mitigate_overshoot = false,
double overshoot_threshold = 0.01);
} // namespace trajectory_processing
11 changes: 5 additions & 6 deletions moveit_core/trajectory_processing/src/trajectory_tools.cpp
Expand Up @@ -48,16 +48,15 @@ std::size_t trajectoryWaypointCount(const moveit_msgs::msg::RobotTrajectory& tra
{
return std::max(trajectory.joint_trajectory.points.size(), trajectory.multi_dof_joint_trajectory.points.size());
}
bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
const double& acceleration_scaling_factor, const double path_tolerance,
const double resample_dt, const double min_angle_change)
bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor,
double acceleration_scaling_factor, double path_tolerance, double resample_dt,
double min_angle_change)
{
TimeOptimalTrajectoryGeneration totg(path_tolerance, resample_dt, min_angle_change);
return totg.computeTimeStamps(trajectory, velocity_scaling_factor, acceleration_scaling_factor);
}
bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, const double& velocity_scaling_factor,
const double& acceleration_scaling_factor, const bool mitigate_overshoot,
const double overshoot_threshold)
bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor,
double acceleration_scaling_factor, bool mitigate_overshoot, double overshoot_threshold)
{
RuckigSmoothing time_param;
return time_param.applySmoothing(trajectory, velocity_scaling_factor, acceleration_scaling_factor, mitigate_overshoot,
Expand Down
Expand Up @@ -154,7 +154,7 @@ bool SrvKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& re

bool SrvKinematicsPlugin::isRedundantJoint(unsigned int index) const
{
for (const unsigned int& redundant_joint_indice : redundant_joint_indices_)
for (unsigned int redundant_joint_indice : redundant_joint_indices_)
{
if (redundant_joint_indice == index)
return true;
Expand Down
Expand Up @@ -56,7 +56,7 @@ namespace visualization

namespace
{
const auto GREEN = [](const double& a) {
const auto GREEN = [](double a) {
std_msgs::msg::ColorRGBA color;
color.r = 0.1;
color.g = 0.8;
Expand Down
Expand Up @@ -150,14 +150,14 @@ class GLRenderer
* \author Suat Gedikli (gedikli@willowgarage.com)
* \return distance of near clipping plane in meters
*/
double getNearClippingDistance() const;
const double& getNearClippingDistance() const;

/**
* \brief returns the distance of the far clipping plane in meters
* \author Suat Gedikli (gedikli@willowgarage.com)
* \return distance of the far clipping plane in meters
*/
double getFarClippingDistance() const;
const double& getFarClippingDistance() const;

/**
* \brief returns the width of the frame buffer objectsin pixels
Expand Down
4 changes: 2 additions & 2 deletions moveit_ros/perception/mesh_filter/src/gl_renderer.cpp
Expand Up @@ -246,12 +246,12 @@ const GLuint& mesh_filter::GLRenderer::getProgramID() const
return program_;
}

double mesh_filter::GLRenderer::getNearClippingDistance() const
const double& mesh_filter::GLRenderer::getNearClippingDistance() const
{
return near_;
}

double mesh_filter::GLRenderer::getFarClippingDistance() const
const double& mesh_filter::GLRenderer::getFarClippingDistance() const
{
return far_;
}
Expand Down

0 comments on commit db0b936

Please sign in to comment.