moveit2
The MoveIt Motion Planning Framework for ROS 2.
|
#include <model_based_planning_context.h>
Public Member Functions | |
ModelBasedPlanningContext (const std::string &name, const ModelBasedPlanningContextSpecification &spec) | |
~ModelBasedPlanningContext () override | |
bool | solve (planning_interface::MotionPlanResponse &res) override |
Solve the motion planning problem and store the result in res. This function should not clear data structures before computing. The constructor and clear() do that. More... | |
bool | solve (planning_interface::MotionPlanDetailedResponse &res) override |
Solve the motion planning problem and store the detailed result in res. This function should not clear data structures before computing. The constructor and clear() do that. More... | |
void | clear () override |
Clear the data structures used by the planner. More... | |
bool | terminate () override |
If solve() is running, terminate the computation. Return false if termination not possible. No-op if solve() is not running (returns true). More... | |
const ModelBasedPlanningContextSpecification & | getSpecification () const |
const std::map< std::string, std::string > & | getSpecificationConfig () const |
void | setSpecificationConfig (const std::map< std::string, std::string > &config) |
const moveit::core::RobotModelConstPtr & | getRobotModel () const |
const moveit::core::JointModelGroup * | getJointModelGroup () const |
const moveit::core::RobotState & | getCompleteInitialRobotState () const |
const ModelBasedStateSpacePtr & | getOMPLStateSpace () const |
const og::SimpleSetupPtr & | getOMPLSimpleSetup () const |
og::SimpleSetupPtr & | getOMPLSimpleSetup () |
const ot::Benchmark & | getOMPLBenchmark () const |
ot::Benchmark & | getOMPLBenchmark () |
const kinematic_constraints::KinematicConstraintSetPtr & | getPathConstraints () const |
unsigned int | getMaximumStateSamplingAttempts () const |
void | setMaximumStateSamplingAttempts (unsigned int max_state_sampling_attempts) |
unsigned int | getMaximumGoalSamplingAttempts () const |
void | setMaximumGoalSamplingAttempts (unsigned int max_goal_sampling_attempts) |
unsigned int | getMaximumGoalSamples () const |
void | setMaximumGoalSamples (unsigned int max_goal_samples) |
unsigned int | getMaximumPlanningThreads () const |
void | setMaximumPlanningThreads (unsigned int max_planning_threads) |
double | getMaximumSolutionSegmentLength () const |
void | setMaximumSolutionSegmentLength (double mssl) |
unsigned int | getMinimumWaypointCount () const |
void | setMinimumWaypointCount (unsigned int mwc) |
Get the minimum number of waypoints along the solution path. More... | |
const constraint_samplers::ConstraintSamplerManagerPtr & | getConstraintSamplerManager () |
void | setConstraintSamplerManager (const constraint_samplers::ConstraintSamplerManagerPtr &csm) |
void | setVerboseStateValidityChecks (bool flag) |
void | setProjectionEvaluator (const std::string &peval) |
void | setPlanningVolume (const moveit_msgs::msg::WorkspaceParameters &wparams) |
void | setCompleteInitialState (const moveit::core::RobotState &complete_initial_robot_state) |
bool | setGoalConstraints (const std::vector< moveit_msgs::msg::Constraints > &goal_constraints, const moveit_msgs::msg::Constraints &path_constraints, moveit_msgs::msg::MoveItErrorCodes *error) |
bool | setPathConstraints (const moveit_msgs::msg::Constraints &path_constraints, moveit_msgs::msg::MoveItErrorCodes *error) |
void | setConstraintsApproximations (const ConstraintsLibraryPtr &constraints_library) |
ConstraintsLibraryPtr | getConstraintsLibraryNonConst () |
const ConstraintsLibraryPtr & | getConstraintsLibrary () const |
bool | simplifySolutions () const |
void | simplifySolutions (bool flag) |
void | setInterpolation (bool flag) |
void | setHybridize (bool flag) |
const moveit_msgs::msg::MoveItErrorCodes | solve (double timeout, unsigned int count) |
bool | benchmark (double timeout, unsigned int count, const std::string &filename="") |
double | getLastPlanTime () const |
double | getLastSimplifyTime () const |
void | simplifySolution (double timeout) |
void | interpolateSolution () |
bool | getSolutionPath (robot_trajectory::RobotTrajectory &traj) const |
void | convertPath (const og::PathGeometric &pg, robot_trajectory::RobotTrajectory &traj) const |
bool | loadConstraintApproximations (const rclcpp::Node::SharedPtr &node) |
Look up param server 'constraint_approximations' and use its value as the path to load constraint approximations to. More... | |
bool | saveConstraintApproximations (const rclcpp::Node::SharedPtr &node) |
Look up param server 'constraint_approximations' and use its value as the path to save constraint approximations to. More... | |
virtual void | configure (const rclcpp::Node::SharedPtr &node, bool use_constraints_approximations) |
Configure ompl_simple_setup_ and optionally the constraints_library_. More... | |
Public Member Functions inherited from planning_interface::PlanningContext | |
PlanningContext (const std::string &name, const std::string &group) | |
Construct a planning context named name for the group group. More... | |
virtual | ~PlanningContext () |
const std::string & | getGroupName () const |
Get the name of the group this planning context is for. More... | |
const std::string & | getName () const |
Get the name of this planning context. More... | |
const planning_scene::PlanningSceneConstPtr & | getPlanningScene () const |
Get the planning scene associated to this planning context. More... | |
const MotionPlanRequest & | getMotionPlanRequest () const |
Get the motion plan request associated to this planning context. More... | |
void | setPlanningScene (const planning_scene::PlanningSceneConstPtr &planning_scene) |
Set the planning scene for this context. More... | |
void | setMotionPlanRequest (const MotionPlanRequest &request) |
Set the planning request for this context. More... | |
Protected Member Functions | |
void | preSolve () |
void | postSolve () |
void | startSampling () |
void | stopSampling () |
virtual ob::ProjectionEvaluatorPtr | getProjectionEvaluator (const std::string &peval) const |
virtual ob::StateSamplerPtr | allocPathConstrainedSampler (const ompl::base::StateSpace *ss) const |
virtual void | useConfig () |
virtual ob::GoalPtr | constructGoal () |
virtual ob::PlannerTerminationCondition | constructPlannerTerminationCondition (double timeout, const ompl::time::point &start) |
void | registerTerminationCondition (const ob::PlannerTerminationCondition &ptc) |
void | unregisterTerminationCondition () |
int32_t | logPlannerStatus (const og::SimpleSetupPtr &ompl_simple_setup) |
Convert OMPL PlannerStatus to moveit_msgs::msg::MoveItErrorCode. More... | |
Protected Attributes | |
ModelBasedPlanningContextSpecification | spec_ |
moveit::core::RobotState | complete_initial_robot_state_ |
og::SimpleSetupPtr | ompl_simple_setup_ |
the OMPL planning context; this contains the problem definition and the planner used More... | |
ot::Benchmark | ompl_benchmark_ |
the OMPL tool for benchmarking planners More... | |
ot::ParallelPlan | ompl_parallel_plan_ |
tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_simple_setup_ More... | |
std::vector< int > | space_signature_ |
kinematic_constraints::KinematicConstraintSetPtr | path_constraints_ |
moveit_msgs::msg::Constraints | path_constraints_msg_ |
std::vector< kinematic_constraints::KinematicConstraintSetPtr > | goal_constraints_ |
const ob::PlannerTerminationCondition * | ptc_ |
std::mutex | ptc_lock_ |
double | last_plan_time_ |
the time spent computing the last plan More... | |
double | last_simplify_time_ |
the time spent simplifying the last plan More... | |
unsigned int | max_goal_samples_ |
unsigned int | max_state_sampling_attempts_ |
unsigned int | max_goal_sampling_attempts_ |
maximum number of attempts to be made at sampling a goal states More... | |
unsigned int | max_planning_threads_ |
when planning in parallel, this is the maximum number of threads to use at one time More... | |
double | max_solution_segment_length_ |
unsigned int | minimum_waypoint_count_ |
bool | multi_query_planning_enabled_ |
when false, clears planners before running solve() More... | |
ConstraintsLibraryPtr | constraints_library_ |
bool | simplify_solutions_ |
bool | interpolate_ |
bool | hybridize_ |
Protected Attributes inherited from planning_interface::PlanningContext | |
std::string | name_ |
The name of this planning context. More... | |
std::string | group_ |
The group (as in the SRDF) this planning context is for. More... | |
planning_scene::PlanningSceneConstPtr | planning_scene_ |
The planning scene for this context. More... | |
MotionPlanRequest | request_ |
The planning request for this context. More... | |
Definition at line 88 of file model_based_planning_context.h.
ompl_interface::ModelBasedPlanningContext::ModelBasedPlanningContext | ( | const std::string & | name, |
const ModelBasedPlanningContextSpecification & | spec | ||
) |
Definition at line 75 of file model_based_planning_context.cpp.
|
inlineoverride |
Definition at line 93 of file model_based_planning_context.h.
|
protectedvirtual |
Definition at line 226 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::benchmark | ( | double | timeout, |
unsigned int | count, | ||
const std::string & | filename = "" |
||
) |
Definition at line 691 of file model_based_planning_context.cpp.
|
overridevirtual |
Clear the data structures used by the planner.
Implements planning_interface::PlanningContext.
Definition at line 618 of file model_based_planning_context.cpp.
|
virtual |
Configure ompl_simple_setup_ and optionally the constraints_library_.
ompl_simple_setup_ gets a start state, state sampler, and state validity checker.
node | ROS node used to load the constraint approximations. |
use_constraints_approximations | Set to true if we want to load the constraint approximation. |
Definition at line 103 of file model_based_planning_context.cpp.
|
protectedvirtual |
Definition at line 505 of file model_based_planning_context.cpp.
|
protectedvirtual |
Definition at line 539 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::convertPath | ( | const og::PathGeometric & | pg, |
robot_trajectory::RobotTrajectory & | traj | ||
) | const |
Definition at line 476 of file model_based_planning_context.cpp.
|
inline |
Definition at line 128 of file model_based_planning_context.h.
|
inline |
Definition at line 234 of file model_based_planning_context.h.
|
inline |
Definition at line 268 of file model_based_planning_context.h.
|
inline |
Definition at line 263 of file model_based_planning_context.h.
|
inline |
Definition at line 123 of file model_based_planning_context.h.
|
inline |
Definition at line 308 of file model_based_planning_context.h.
|
inline |
Definition at line 314 of file model_based_planning_context.h.
|
inline |
Definition at line 188 of file model_based_planning_context.h.
|
inline |
Definition at line 176 of file model_based_planning_context.h.
|
inline |
Definition at line 200 of file model_based_planning_context.h.
|
inline |
Definition at line 212 of file model_based_planning_context.h.
|
inline |
Definition at line 164 of file model_based_planning_context.h.
|
inline |
Definition at line 223 of file model_based_planning_context.h.
|
inline |
Definition at line 153 of file model_based_planning_context.h.
|
inline |
Definition at line 148 of file model_based_planning_context.h.
|
inline |
Definition at line 143 of file model_based_planning_context.h.
|
inline |
Definition at line 138 of file model_based_planning_context.h.
|
inline |
Definition at line 133 of file model_based_planning_context.h.
|
inline |
Definition at line 158 of file model_based_planning_context.h.
|
protectedvirtual |
Definition at line 162 of file model_based_planning_context.cpp.
|
inline |
Definition at line 118 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::getSolutionPath | ( | robot_trajectory::RobotTrajectory & | traj | ) | const |
Definition at line 487 of file model_based_planning_context.cpp.
|
inline |
Definition at line 103 of file model_based_planning_context.h.
|
inline |
Definition at line 108 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::interpolateSolution | ( | ) |
Definition at line 448 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::loadConstraintApproximations | ( | const rclcpp::Node::SharedPtr & | node | ) |
Look up param server 'constraint_approximations' and use its value as the path to load constraint approximations to.
Definition at line 1057 of file model_based_planning_context.cpp.
|
protected |
Convert OMPL PlannerStatus to moveit_msgs::msg::MoveItErrorCode.
Definition at line 975 of file model_based_planning_context.cpp.
|
protected |
Definition at line 750 of file model_based_planning_context.cpp.
|
protected |
Definition at line 737 of file model_based_planning_context.cpp.
|
protected |
Definition at line 963 of file model_based_planning_context.cpp.
bool ompl_interface::ModelBasedPlanningContext::saveConstraintApproximations | ( | const rclcpp::Node::SharedPtr & | node | ) |
Look up param server 'constraint_approximations' and use its value as the path to save constraint approximations to.
Definition at line 1045 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setCompleteInitialState | ( | const moveit::core::RobotState & | complete_initial_robot_state | ) |
Definition at line 611 of file model_based_planning_context.cpp.
|
inline |
Definition at line 239 of file model_based_planning_context.h.
|
inline |
Definition at line 258 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::setGoalConstraints | ( | const std::vector< moveit_msgs::msg::Constraints > & | goal_constraints, |
const moveit_msgs::msg::Constraints & | path_constraints, | ||
moveit_msgs::msg::MoveItErrorCodes * | error | ||
) |
Definition at line 658 of file model_based_planning_context.cpp.
|
inline |
Definition at line 288 of file model_based_planning_context.h.
|
inline |
Definition at line 283 of file model_based_planning_context.h.
|
inline |
Definition at line 194 of file model_based_planning_context.h.
|
inline |
Definition at line 182 of file model_based_planning_context.h.
|
inline |
Definition at line 206 of file model_based_planning_context.h.
|
inline |
Definition at line 218 of file model_based_planning_context.h.
|
inline |
Definition at line 170 of file model_based_planning_context.h.
|
inline |
Get the minimum number of waypoints along the solution path.
Definition at line 229 of file model_based_planning_context.h.
bool ompl_interface::ModelBasedPlanningContext::setPathConstraints | ( | const moveit_msgs::msg::Constraints & | path_constraints, |
moveit_msgs::msg::MoveItErrorCodes * | error | ||
) |
Definition at line 647 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setPlanningVolume | ( | const moveit_msgs::msg::WorkspaceParameters & | wparams | ) |
Definition at line 419 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::setProjectionEvaluator | ( | const std::string & | peval | ) |
Definition at line 149 of file model_based_planning_context.cpp.
|
inline |
Definition at line 113 of file model_based_planning_context.h.
void ompl_interface::ModelBasedPlanningContext::setVerboseStateValidityChecks | ( | bool | flag | ) |
Definition at line 497 of file model_based_planning_context.cpp.
void ompl_interface::ModelBasedPlanningContext::simplifySolution | ( | double | timeout | ) |
Definition at line 438 of file model_based_planning_context.cpp.
|
inline |
Definition at line 273 of file model_based_planning_context.h.
|
inline |
Definition at line 278 of file model_based_planning_context.h.
const moveit_msgs::msg::MoveItErrorCodes ompl_interface::ModelBasedPlanningContext::solve | ( | double | timeout, |
unsigned int | count | ||
) |
Definition at line 851 of file model_based_planning_context.cpp.
|
overridevirtual |
Solve the motion planning problem and store the detailed result in res. This function should not clear data structures before computing. The constructor and clear() do that.
Implements planning_interface::PlanningContext.
Definition at line 800 of file model_based_planning_context.cpp.
|
overridevirtual |
Solve the motion planning problem and store the result in res. This function should not clear data structures before computing. The constructor and clear() do that.
Implements planning_interface::PlanningContext.
Definition at line 767 of file model_based_planning_context.cpp.
|
protected |
Definition at line 709 of file model_based_planning_context.cpp.
|
protected |
Definition at line 723 of file model_based_planning_context.cpp.
|
overridevirtual |
If solve() is running, terminate the computation. Return false if termination not possible. No-op if solve() is not running (returns true).
Implements planning_interface::PlanningContext.
Definition at line 1035 of file model_based_planning_context.cpp.
|
protected |
Definition at line 969 of file model_based_planning_context.cpp.
|
protectedvirtual |
Definition at line 275 of file model_based_planning_context.cpp.
|
protected |
Definition at line 396 of file model_based_planning_context.h.
|
protected |
Definition at line 447 of file model_based_planning_context.h.
|
protected |
Definition at line 411 of file model_based_planning_context.h.
|
protected |
Definition at line 455 of file model_based_planning_context.h.
|
protected |
Definition at line 452 of file model_based_planning_context.h.
|
protected |
the time spent computing the last plan
Definition at line 417 of file model_based_planning_context.h.
|
protected |
the time spent simplifying the last plan
Definition at line 420 of file model_based_planning_context.h.
|
protected |
maximum number of valid states to store in the goal region for any planning request (when such sampling is possible)
Definition at line 424 of file model_based_planning_context.h.
|
protected |
maximum number of attempts to be made at sampling a goal states
Definition at line 431 of file model_based_planning_context.h.
|
protected |
when planning in parallel, this is the maximum number of threads to use at one time
Definition at line 434 of file model_based_planning_context.h.
|
protected |
the maximum length that is allowed for segments that make up the motion plan; by default this is 1% from the extent of the space
Definition at line 438 of file model_based_planning_context.h.
|
protected |
maximum number of attempts to be made at sampling a state when attempting to find valid states that satisfy some set of constraints
Definition at line 428 of file model_based_planning_context.h.
|
protected |
the minimum number of points to include on the solution path (interpolation is used to reach this number, if needed)
Definition at line 442 of file model_based_planning_context.h.
|
protected |
when false, clears planners before running solve()
Definition at line 445 of file model_based_planning_context.h.
|
protected |
the OMPL tool for benchmarking planners
Definition at line 402 of file model_based_planning_context.h.
|
protected |
tool used to compute multiple plans in parallel; this uses the problem definition maintained by ompl_simple_setup_
Definition at line 405 of file model_based_planning_context.h.
|
protected |
the OMPL planning context; this contains the problem definition and the planner used
Definition at line 399 of file model_based_planning_context.h.
|
protected |
Definition at line 409 of file model_based_planning_context.h.
|
protected |
Definition at line 410 of file model_based_planning_context.h.
|
protected |
Definition at line 413 of file model_based_planning_context.h.
|
protected |
Definition at line 414 of file model_based_planning_context.h.
|
protected |
Definition at line 449 of file model_based_planning_context.h.
|
protected |
Definition at line 407 of file model_based_planning_context.h.
|
protected |
Definition at line 394 of file model_based_planning_context.h.