diff --git a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h index ca8348368ca..73f56d768fb 100644 --- a/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h +++ b/tesseract_motion_planners/ompl/include/tesseract_motion_planners/ompl/profile/ompl_waypoint_profile.h @@ -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; diff --git a/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp b/tesseract_motion_planners/ompl/src/ompl_motion_planner.cpp index f2c072a94da..bd6cd12a92b 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 @@ -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(seed.size()) < trajectory.rows() - 1) { const std::size_t diff = static_cast(trajectory.rows() - 1) - output.size(); output.insert(output.end(), diff, output.back()); @@ -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; } diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp index b3b2352ccce..3a4d7e5c321 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_composite_profile_rvss.cpp @@ -21,7 +21,7 @@ std::any OMPLCompositeProfileRVSS::create(const CompositeInstruction& instructio { for (const Instruction& inst : instruction) { - const PlanInstruction& goal_instruction = inst.as(); + const auto& goal_instruction = inst.as(); if (goal_instruction.getPlanType() != PlanInstructionType::FREESPACE) throw std::runtime_error("Only freespace plan instruction types are currently supported"); } @@ -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); } @@ -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); diff --git a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp index 034738854ca..b51f752c1ae 100644 --- a/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp +++ b/tesseract_motion_planners/ompl/src/profile/ompl_waypoint_profile.cpp @@ -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); @@ -28,7 +28,7 @@ void checkCollision(const Eigen::VectorXd& state, } } -Eigen::VectorXd updateLimits(Eigen::Ref joint_waypoint, tesseract_common::KinematicLimits limits) +Eigen::VectorXd updateLimits(const Eigen::Ref& joint_waypoint, const tesseract_common::KinematicLimits& limits) { if (!tesseract_common::satisfiesPositionLimits(joint_waypoint, limits.joint_limits)) throw std::runtime_error("State violates joint limits"); @@ -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); @@ -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(); - tesseract_common::ManipulatorInfo mi = plan_instruction.getManipulatorInfo(); + const auto& plan_instruction = instruction.as(); + const tesseract_common::ManipulatorInfo& mi = plan_instruction.getManipulatorInfo(); const Waypoint& waypoint = plan_instruction.getWaypoint(); if (isCartesianWaypoint(waypoint)) { - const CartesianWaypoint& cw = waypoint.as(); + const auto& cw = waypoint.as(); return getValidIKSolutions(cw, mi, env); } - else if (isJointWaypoint(waypoint)) + + if (isJointWaypoint(waypoint)) { - const JointWaypoint& jw = waypoint.as(); + const auto& jw = waypoint.as(); const Eigen::VectorXd updated_state = updateLimits(jw, env.getJointGroup(mi.manipulator)->getLimits()); checkCollision(updated_state, env, env.getJointGroup(mi.manipulator)); return std::vector{ updated_state }; } - else if (isStateWaypoint(waypoint)) + + if (isStateWaypoint(waypoint)) { - const StateWaypoint& sw = waypoint.as(); + const auto& sw = waypoint.as(); Eigen::Map 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));