diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index f2c072a94da..16e712e2e07 100644 --- a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp +++ b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp @@ -81,7 +81,7 @@ bool checkGoalState(const ompl::base::ProblemDefinitionPtr& prob_def, } std::vector> createOMPLStates(const std::vector& joint_states, - ompl::base::SpaceInformationPtr si) + const ompl::base::SpaceInformationPtr& si) { std::vector> states; states.reserve(joint_states.size()); @@ -98,7 +98,7 @@ std::vector> createOMPLStates(const std::vector plan(ompl::geometric::SimpleSetupPtr simple_setup, +std::shared_ptr plan(const ompl::geometric::SimpleSetupPtr& simple_setup, const OMPLPlannerParameters& params, const unsigned n_output_states) { @@ -220,7 +220,7 @@ std::shared_ptr 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(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 @@ -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(i - 1); + const auto composite_idx = static_cast(i - 1); auto& move_instruction = output.at(composite_idx).as(); move_instruction.getWaypoint().as().position = trajectory.row(i); } @@ -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(); + const auto& pi = request.instructions.getStartInstruction().as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); @@ -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().back().as(); - const StateWaypoint& sw = mi.getWaypoint().as(); + const auto& mi = response.results.back().as().back().as(); + const auto& sw = mi.getWaypoint().as(); start_states.push_back(sw.position); } @@ -365,12 +365,12 @@ tesseract_common::StatusCode OMPLMotionPlanner::solve(const PlannerRequest& requ // Add the goal waypoint(s) { - const PlanInstruction& pi = request.instructions[i].as(); + const auto& pi = request.instructions[i].as(); const std::string profile_name = getProfileString(name_, pi.getProfile(), request.composite_profile_remapping); WaypointProfile::ConstPtr p = request.profiles->getWaypointProfile(name_, profile_name); - std::vector states = std::any_cast>(p->create(pi, *request.env)); + auto states = std::any_cast>(p->create(pi, *request.env)); auto ompl_states = createOMPLStates(states, simple_setup->getSpaceInformation()); auto goal_states = std::make_shared(simple_setup->getSpaceInformation()); @@ -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; }