Skip to content

Commit

Permalink
Clang tidy fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
@marip8
marip8 committed Jan 10, 2022
1 parent b10353f commit 82662fc
Show file tree
Hide file tree
Showing 4 changed files with 31 additions and 29 deletions.
2 changes: 1 addition & 1 deletion ...tion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@ namespace tesseract_planning
class OMPLWaypointProfile : public WaypointProfile
{
public:
virtual std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override;
std::any create(const Instruction& instruction, const tesseract_environment::Environment& env) const override;

private:
friend class boost::serialization::access;
Expand Down
28 changes: 14 additions & 14 deletions tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -81,7 +81,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def,
}

std::vector<ompl::base::ScopedState<>> createOMPLStates(const std::vector<Eigen::VectorXd>& joint_states,
ompl::base::SpaceInformationPtr si)
const ompl::base::SpaceInformationPtr& si)
{
std::vector<ompl::base::ScopedState<>> states;
states.reserve(joint_states.size());
Expand All @@ -98,7 +98,7 @@ std::vector<ompl::base::ScopedState<>> createOMPLStates(const std::vector<Eigen:
return states;
}

std::shared_ptr<ompl::base::PlannerData> plan(ompl::geometric::SimpleSetupPtr simple_setup,
std::shared_ptr<ompl::base::PlannerData> plan(const ompl::geometric::SimpleSetupPtr& simple_setup,
const OMPLPlannerParameters& params,
const unsigned n_output_states)
{
Expand Down Expand Up @@ -220,7 +220,7 @@ std::shared_ptr<ompl::base::PlannerData> plan(ompl::geometric::SimpleSetupPtr si
// space to be considered equivalent are not merged or connected. Also different planners add edges in different ways
// and set different vertex/edge properties, so it may not make sense to combine them
auto planner_data = std::make_shared<ompl::base::PlannerData>(simple_setup->getSpaceInformation());
for (auto planner : planners)
for (const ompl::base::PlannerPtr& planner : planners)
planner->getPlannerData(*planner_data);

// The planning data is actually owned by the planner instances themselves. Deep copy the planning information by
Expand All @@ -240,7 +240,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra
// In this case, we need to insert more states into the composite to cover the difference
// Remember the composite does not include the start state, so compare its size with one less than the size of the
// trajectory
if (seed.size() < trajectory.rows() - 1)
if (static_cast<Eigen::Index>(seed.size()) < trajectory.rows() - 1)
{
const std::size_t diff = static_cast<std::size_t>(trajectory.rows() - 1) - output.size();
output.insert(output.end(), diff, output.back());
Expand All @@ -259,7 +259,7 @@ CompositeInstruction buildTrajectoryInstruction(const tesseract_common::TrajArra
// Subsequent trajectory states go into the composite instruction
// The index into the composite of these states is one less than the index of the trajectory state since the first
// trajectory state was saved outside the composite
const std::size_t composite_idx = static_cast<std::size_t>(i - 1);
const auto composite_idx = static_cast<std::size_t>(i - 1);
auto& move_instruction = output.at(composite_idx).as<MoveInstruction>();
move_instruction.getWaypoint().as<StateWaypoint>().position = trajectory.row(i);
}
Expand Down Expand Up @@ -342,7 +342,7 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ
// Get the start waypoint profile and add the states to the SimpleSetup
if (i == 0)
{
const PlanInstruction& pi = request.instructions.getStartInstruction().as<PlanInstruction>();
const auto& pi = request.instructions.getStartInstruction().as<PlanInstruction>();
const std::string profile_name =
getProfileString(name_, pi.getProfile(), request.composite_profile_remapping);
WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name);
Expand All @@ -351,8 +351,8 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ
else
{
// Use the last state of the previous trajectory as the single start state for this plan
const MoveInstruction& mi = response.results.back().as<CompositeInstruction>().back().as<MoveInstruction>();
const StateWaypoint& sw = mi.getWaypoint().as<StateWaypoint>();
const auto& mi = response.results.back().as<CompositeInstruction>().back().as<MoveInstruction>();
const auto& sw = mi.getWaypoint().as<StateWaypoint>();
start_states.push_back(sw.position);
}

Expand All @@ -365,12 +365,12 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ

// Add the goal waypoint(s)
{
const PlanInstruction& pi = request.instructions[i].as<PlanInstruction>();
const auto& pi = request.instructions[i].as<PlanInstruction>();

const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping);
WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name);

std::vector<Eigen::VectorXd> states = std::any_cast<std::vector<Eigen::VectorXd>>(p->create(pi, *request.env));
auto states = std::any_cast<std::vector<Eigen::VectorXd>>(p->create(pi, *request.env));
auto ompl_states = createOMPLStates(states, simple_setup->getSpaceInformation());

auto goal_states = std::make_shared<ompl::base::GoalStates>(simple_setup->getSpaceInformation());
Expand Down Expand Up @@ -428,14 +428,14 @@ bool OMPLMotionPlanner::checkUserInput(const PlannerRequest& request)
{
// Check that parameters are valid
if (request.env == nullptr)
std::runtime_error("Environment is invalid (nullptr)");
throw std::runtime_error("Environment is invalid (nullptr)");

if (request.instructions.empty())
std::runtime_error("Request contains no instructions");
throw std::runtime_error("Request contains no instructions");

if (request.instructions.size() != request.seed.size())
std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) +
") does not match seed size (" + std::to_string(request.seed.size()) + ")");
throw std::runtime_error("Instruction size (" + std::to_string(request.instructions.size()) +
") does not match seed size (" + std::to_string(request.seed.size()) + ")");

return true;
}
Expand Down
6 changes: 3 additions & 3 deletions tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio
{
for (const Instruction& inst : instruction)
{
const PlanInstruction& goal_instruction = inst.as<PlanInstruction>();
const auto& goal_instruction = inst.as<PlanInstruction>();
if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE)
throw std::runtime_error("Only freespace plan instruction types are currently supported");
}
Expand Down Expand Up @@ -87,7 +87,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio
break;
}
default:
CONSOLE_BRIDGE_logDebug("DOING NOTHING HERE");
CONSOLE_BRIDGE_logDebug("No collision state validator added");
}
simple_setup->setStateValidityChecker(csvc);
}
Expand Down Expand Up @@ -124,7 +124,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio
break;
}
default:
CONSOLE_BRIDGE_logDebug("Nothing doing");
CONSOLE_BRIDGE_logDebug("No collision validator added");
}

simple_setup->getSpaceInformation()->setMotionValidator(mv);
Expand Down
24 changes: 13 additions & 11 deletions tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,7 +5,7 @@ namespace tesseract_planning
{
void checkCollision(const Eigen::VectorXd& state,
const tesseract_environment::Environment& env,
tesseract_kinematics::JointGroup::ConstPtr manip)
const tesseract_kinematics::JointGroup::ConstPtr& manip)
{
tesseract_collision::DiscreteContactManager::Ptr contact_checker = env.getDiscreteContactManager();
tesseract_common::TransformMap link_transforms = manip->calcFwdKin(state);
Expand All @@ -28,7 +28,7 @@ void checkCollision(const Eigen::VectorXd& state,
}
}

Eigen::VectorXd updateLimits(Eigen::Ref<const Eigen::VectorXd> joint_waypoint, tesseract_common::KinematicLimits limits)
Eigen::VectorXd updateLimits(const Eigen::Ref<const Eigen::VectorXd>& joint_waypoint, const tesseract_common::KinematicLimits& limits)
{
if (!tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits))
throw std::runtime_error("State violates joint limits");
Expand Down Expand Up @@ -65,12 +65,12 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c
tesseract_common::KinematicLimits limits = manip->getLimits();
tesseract_kinematics::IKSolutions valid_solutions;
valid_solutions.reserve(joint_solutions.size());
for (std::size_t i = 0; i < joint_solutions.size(); ++i)
for (const Eigen::VectorXd& js : joint_solutions)
{
try
{
// Update the joint solution based on the kinematic limits
Eigen::VectorXd solution = updateLimits(joint_solutions[i], limits);
Eigen::VectorXd solution = updateLimits(js, limits);

// Ensure the solution is collision-free
checkCollision(solution, env, manip);
Expand Down Expand Up @@ -99,25 +99,27 @@ tesseract_kinematics::IKSolutions getValidIKSolutions(const Eigen::Isometry3d& c
std::any OMPLWaypointProfile::create(const Instruction& instruction,
const tesseract_environment::Environment& env) const
{
PlanInstruction plan_instruction = instruction.as<PlanInstruction>();
tesseract_common::ManipulatorInfo mi = plan_instruction.getManipulatorInfo();
const auto& plan_instruction = instruction.as<PlanInstruction>();
const tesseract_common::ManipulatorInfo& mi = plan_instruction.getManipulatorInfo();
const Waypoint& waypoint = plan_instruction.getWaypoint();

if (isCartesianWaypoint(waypoint))
{
const CartesianWaypoint& cw = waypoint.as<CartesianWaypoint>();
const auto& cw = waypoint.as<CartesianWaypoint>();
return getValidIKSolutions(cw, mi, env);
}
else if (isJointWaypoint(waypoint))

if (isJointWaypoint(waypoint))
{
const JointWaypoint& jw = waypoint.as<JointWaypoint>();
const auto& jw = waypoint.as<JointWaypoint>();
const Eigen::VectorXd updated_state = updateLimits(jw, env.getJointGroup(mi.manipulator)->getLimits());
checkCollision(updated_state, env, env.getJointGroup(mi.manipulator));
return std::vector<Eigen::VectorXd>{ updated_state };
}
else if (isStateWaypoint(waypoint))

if (isStateWaypoint(waypoint))
{
const StateWaypoint& sw = waypoint.as<StateWaypoint>();
const auto& sw = waypoint.as<StateWaypoint>();
Eigen::Map<const Eigen::VectorXd> state(sw.position.data(), sw.position.size());
const Eigen::VectorXd updated_state = updateLimits(state, env.getJointGroup(mi.manipulator)->getLimits());
checkCollision(updated_state, env, env.getJointGroup(mi.manipulator));
Expand Down

0 comments on commit 82662fc

Please sign in to comment.