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 9, 2022
1 parent b10353f commit 34ab66b
Show file tree
Hide file tree
Showing 3 changed files with 22 additions and 22 deletions.
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
10 changes: 5 additions & 5 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 @@ -99,25 +99,25 @@ 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))
{
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))
{
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 34ab66b

Please sign in to comment.