Skip to content

Commit

Permalink
Fix clang-tidy issues (#1706)
Browse files Browse the repository at this point in the history 
* Blindly apply automatic clang-tidy fixes

* Exemplarily cleanup a few automatic clang-tidy fixes

* Clang-tidy fixups

* Missed const-ref fixups

* Fix unsupported non-const -> const

* More fixes

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
  • Loading branch information
@rhaschke @henningkayser
rhaschke and henningkayser committed Nov 10, 2022
1 parent a2aa1a6 commit 132ad28
Show file tree
Hide file tree
Showing 101 changed files with 445 additions and 425 deletions.
2 changes: 1 addition & 1 deletion moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -351,7 +351,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase
virtual bool
searchPositionIK(const std::vector<geometry_msgs::msg::Pose>& ik_poses, const std::vector<double>& ik_seed_state,
double timeout, const std::vector<double>& consistency_limits, std::vector<double>& solution,
const IKCallbackFn& solution_callback, IKCostFn cost_function,
const IKCallbackFn& solution_callback, const IKCostFn& cost_function,
moveit_msgs::msg::MoveItErrorCodes& error_code,
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
const moveit::core::RobotState* context_state = nullptr) const
Expand Down
64 changes: 30 additions & 34 deletions moveit_core/robot_state/include/moveit/robot_state/cartesian_interpolator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -170,27 +170,25 @@ class CartesianInterpolator
the returned path is truncated up to just before the jump.

Kinematics solvers may use cost functions to prioritize certain solutions, which may be specified with \e cost_function. */
static Distance
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
static Distance computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
const LinkModel* link, const Eigen::Vector3d& translation, bool global_reference_frame,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief Compute the sequence of joint values that correspond to a straight Cartesian path, for a particular link.

In contrast to the previous function, the translation vector is specified as a (unit) direction vector and
a distance. */
static Distance
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn())
static Distance computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
const LinkModel* link, const Eigen::Vector3d& direction, bool global_reference_frame, double distance,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn())
{
return computeCartesianPath(start_state, group, traj, link, distance * direction, global_reference_frame, max_step,
jump_threshold, validCallback, options, cost_function);
Expand All @@ -203,31 +201,29 @@ class CartesianInterpolator
The target frame is assumed to be specified either w.r.t. to the global reference frame or the virtual link frame
(\e global_reference_frame is false). This function returns the percentage (0..1) of the path that was achieved.
All other comments from the previous function apply. */
static Percentage
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn(),
const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
static Percentage computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
const LinkModel* link, const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(),
const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());

/** \brief Compute the sequence of joint values that perform a general Cartesian path.

In contrast to the previous functions, the Cartesian path is specified as a set of \e waypoints to be sequentially
reached by the virtual frame attached to the robot \e link. The waypoints are transforms given either w.r.t. the global
reference frame or the virtual frame at the immediately preceding waypoint. The virtual frame needs
to move in a straight line between two consecutive waypoints. All other comments apply. */
static Percentage
computeCartesianPath(RobotState* start_state, const JointModelGroup* group,
std::vector<std::shared_ptr<RobotState>>& traj, const LinkModel* link,
const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn(),
const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());
static Percentage computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<std::shared_ptr<RobotState>>& traj,
const LinkModel* link, const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame,
const MaxEEFStep& max_step, const JumpThreshold& jump_threshold,
const GroupStateValidityCallbackFn& validCallback = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn(),
const Eigen::Isometry3d& link_offset = Eigen::Isometry3d::Identity());

/** \brief Tests joint space jumps of a trajectory.

Expand Down
14 changes: 7 additions & 7 deletions moveit_core/robot_state/include/moveit/robot_state/robot_state.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -952,7 +952,7 @@ class RobotState
bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
set by computing inverse kinematics.
Expand All @@ -964,7 +964,7 @@ class RobotState
bool setFromIK(const JointModelGroup* group, const geometry_msgs::msg::Pose& pose, const std::string& tip,
double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
set by computing inverse kinematics.
Expand All @@ -975,7 +975,7 @@ class RobotState
bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
set by computing inverse kinematics.
Expand All @@ -986,7 +986,7 @@ class RobotState
bool setFromIK(const JointModelGroup* group, const Eigen::Isometry3d& pose, const std::string& tip,
double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief If the group this state corresponds to is a chain and a solver is available, then the joint values can be
set by computing inverse kinematics.
Expand All @@ -1000,7 +1000,7 @@ class RobotState
const std::vector<double>& consistency_limits, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief Warning: This function inefficiently copies all transforms around.
If the group consists of a set of sub-groups that are each a chain and a solver
Expand All @@ -1015,7 +1015,7 @@ class RobotState
const std::vector<std::string>& tips, double timeout = 0.0,
const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/** \brief Warning: This function inefficiently copies all transforms around.
If the group consists of a set of sub-groups that are each a chain and a solver
Expand All @@ -1031,7 +1031,7 @@ class RobotState
const std::vector<std::string>& tips, const std::vector<std::vector<double>>& consistency_limits,
double timeout = 0.0, const GroupStateValidityCallbackFn& constraint = GroupStateValidityCallbackFn(),
const kinematics::KinematicsQueryOptions& options = kinematics::KinematicsQueryOptions(),
kinematics::KinematicsBase::IKCostFn cost_function = kinematics::KinematicsBase::IKCostFn());
const kinematics::KinematicsBase::IKCostFn& cost_function = kinematics::KinematicsBase::IKCostFn());

/**
\brief setFromIK for multiple poses and tips (end effectors) when no solver exists for the jmg that can solver for
Expand Down
6 changes: 3 additions & 3 deletions moveit_core/robot_state/src/cartesian_interpolator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ CartesianInterpolator::Distance CartesianInterpolator::computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
const Eigen::Vector3d& translation, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function)
{
const double distance = translation.norm();
// The target pose is obtained by adding the translation vector to the link's current pose
Expand All @@ -77,7 +77,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
const Eigen::Isometry3d& target, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function,
const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function,
const Eigen::Isometry3d& link_offset)
{
// check unsanitized inputs for non-isometry
Expand Down Expand Up @@ -177,7 +177,7 @@ CartesianInterpolator::Percentage CartesianInterpolator::computeCartesianPath(
RobotState* start_state, const JointModelGroup* group, std::vector<RobotStatePtr>& traj, const LinkModel* link,
const EigenSTL::vector_Isometry3d& waypoints, bool global_reference_frame, const MaxEEFStep& max_step,
const JumpThreshold& jump_threshold, const GroupStateValidityCallbackFn& validCallback,
const kinematics::KinematicsQueryOptions& options, kinematics::KinematicsBase::IKCostFn cost_function,
const kinematics::KinematicsQueryOptions& options, const kinematics::KinematicsBase::IKCostFn& cost_function,
const Eigen::Isometry3d& link_offset)
{
double percentage_solved = 0.0;
Expand Down
14 changes: 7 additions & 7 deletions moveit_core/robot_state/src/robot_state.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1461,7 +1461,7 @@ bool RobotState::integrateVariableVelocity(const JointModelGroup* jmg, const Eig
bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, double timeout,
const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options,
kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsBase::IKCostFn& cost_function)
{
const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
if (!solver)
Expand All @@ -1475,7 +1475,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg:
bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg::Pose& pose, const std::string& tip,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options,
kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsBase::IKCostFn& cost_function)
{
Eigen::Isometry3d mat;
tf2::fromMsg(pose, mat);
Expand All @@ -1486,7 +1486,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const geometry_msgs::msg:
bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose, double timeout,
const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options,
kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsBase::IKCostFn& cost_function)
{
const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
if (!solver)
Expand All @@ -1501,7 +1501,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d&
bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d& pose_in, const std::string& tip_in,
double timeout, const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options,
kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsBase::IKCostFn& cost_function)
{
static std::vector<double> consistency_limits;
return setFromIK(jmg, pose_in, tip_in, consistency_limits, timeout, constraint, options, cost_function);
Expand Down Expand Up @@ -1551,7 +1551,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const Eigen::Isometry3d&
const std::vector<double>& consistency_limits_in, double timeout,
const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options,
kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsBase::IKCostFn& cost_function)
{
// Convert from single pose and tip to vectors
EigenSTL::vector_Isometry3d poses;
Expand All @@ -1570,7 +1570,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
const std::vector<std::string>& tips_in, double timeout,
const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options,
kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsBase::IKCostFn& cost_function)
{
const std::vector<std::vector<double> > consistency_limits;
return setFromIK(jmg, poses_in, tips_in, consistency_limits, timeout, constraint, options, cost_function);
Expand All @@ -1581,7 +1581,7 @@ bool RobotState::setFromIK(const JointModelGroup* jmg, const EigenSTL::vector_Is
const std::vector<std::vector<double> >& consistency_limit_sets, double timeout,
const GroupStateValidityCallbackFn& constraint,
const kinematics::KinematicsQueryOptions& options,
kinematics::KinematicsBase::IKCostFn cost_function)
const kinematics::KinematicsBase::IKCostFn& cost_function)
{
// Error check
if (poses_in.size() != tips_in.size())
Expand Down
Loading

0 comments on commit 132ad28

Please sign in to comment.