#include <ompl_interface.h>
Public Member Functions | |
constraint_samplers::ConstraintSamplerManager & | getConstraintSamplerManager () |
const constraint_samplers::ConstraintSamplerManager & | getConstraintSamplerManager () const |
ConstraintsLibrary & | getConstraintsLibrary () |
const ConstraintsLibrary & | getConstraintsLibrary () const |
ModelBasedPlanningContextPtr | getLastPlanningContext () const |
const planning_interface::PlannerConfigurationMap & | getPlannerConfigurations () const |
Get the configurations for the planners that are already loaded. | |
ModelBasedPlanningContextPtr | getPlanningContext (const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req) const |
ModelBasedPlanningContextPtr | getPlanningContext (const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, moveit_msgs::MoveItErrorCodes &error_code) const |
ModelBasedPlanningContextPtr | getPlanningContext (const std::string &config, const std::string &factory_type="") const |
const PlanningContextManager & | getPlanningContextManager () const |
PlanningContextManager & | getPlanningContextManager () |
bool | isUsingConstraintsApproximations () const |
void | loadConstraintApproximations (const std::string &path) |
bool | loadConstraintApproximations () |
Look up param server 'constraint_approximations' and use its value as the path to load constraint approximations to. | |
OMPLInterface (const robot_model::RobotModelConstPtr &kmodel, const ros::NodeHandle &nh=ros::NodeHandle("~")) | |
Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified NodeHandle. | |
OMPLInterface (const robot_model::RobotModelConstPtr &kmodel, const planning_interface::PlannerConfigurationMap &pconfig, const ros::NodeHandle &nh=ros::NodeHandle("~")) | |
Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified NodeHandle. However, planner configurations are used as specified in pconfig instead of reading them from the ROS parameter server. | |
void | printStatus () |
Print the status of this node. | |
void | saveConstraintApproximations (const std::string &path) |
bool | saveConstraintApproximations () |
Look up param server 'constraint_approximations' and use its value as the path to save constraint approximations to. | |
void | setPlannerConfigurations (const planning_interface::PlannerConfigurationMap &pconfig) |
Specify configurations for the planners. | |
bool | simplifySolutions () const |
void | simplifySolutions (bool flag) |
bool | solve (const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanResponse &res) const |
Solve the planning problem. | |
bool | solve (const planning_scene::PlanningSceneConstPtr &planning_scene, const planning_interface::MotionPlanRequest &req, planning_interface::MotionPlanDetailedResponse &res) const |
Solve the planning problem but give a more detailed response. | |
void | useConstraintsApproximations (bool flag) |
virtual | ~OMPLInterface () |
Protected Member Functions | |
void | configureContext (const ModelBasedPlanningContextPtr &context) const |
void | loadConstraintSamplers () |
Load the additional plugins for sampling constraints. | |
void | loadPlannerConfigurations () |
Configure the planners. | |
ModelBasedPlanningContextPtr | prepareForSolve (const planning_interface::MotionPlanRequest &req, const planning_scene::PlanningSceneConstPtr &planning_scene, moveit_msgs::MoveItErrorCodes *error_code, unsigned int *attempts, double *timeout) const |
Configure the OMPL planning context for a new planning request. | |
Protected Attributes | |
constraint_samplers::ConstraintSamplerManagerPtr | constraint_sampler_manager_ |
ConstraintsLibraryPtr | constraints_library_ |
PlanningContextManager | context_manager_ |
robot_model::RobotModelConstPtr | kmodel_ |
The ROS node handle. | |
ros::NodeHandle | nh_ |
bool | simplify_solutions_ |
bool | use_constraints_approximations_ |
Private Attributes | |
constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr | constraint_sampler_manager_loader_ |
This class defines the interface to the motion planners in OMPL
Definition at line 57 of file ompl_interface.h.
ompl_interface::OMPLInterface::OMPLInterface | ( | const robot_model::RobotModelConstPtr & | kmodel, |
const ros::NodeHandle & | nh = ros::NodeHandle("~") |
||
) |
Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified NodeHandle.
Definition at line 44 of file ompl_interface.cpp.
ompl_interface::OMPLInterface::OMPLInterface | ( | const robot_model::RobotModelConstPtr & | kmodel, |
const planning_interface::PlannerConfigurationMap & | pconfig, | ||
const ros::NodeHandle & | nh = ros::NodeHandle("~") |
||
) |
Initialize OMPL-based planning for a particular robot model. ROS configuration is read from the specified NodeHandle. However, planner configurations are used as specified in pconfig instead of reading them from the ROS parameter server.
Definition at line 59 of file ompl_interface.cpp.
ompl_interface::OMPLInterface::~OMPLInterface | ( | ) | [virtual] |
Definition at line 74 of file ompl_interface.cpp.
void ompl_interface::OMPLInterface::configureContext | ( | const ModelBasedPlanningContextPtr & | context | ) | const [protected] |
Definition at line 122 of file ompl_interface.cpp.
constraint_samplers::ConstraintSamplerManager& ompl_interface::OMPLInterface::getConstraintSamplerManager | ( | ) | [inline] |
Definition at line 122 of file ompl_interface.h.
const constraint_samplers::ConstraintSamplerManager& ompl_interface::OMPLInterface::getConstraintSamplerManager | ( | ) | const [inline] |
Definition at line 127 of file ompl_interface.h.
Definition at line 112 of file ompl_interface.h.
const ConstraintsLibrary& ompl_interface::OMPLInterface::getConstraintsLibrary | ( | ) | const [inline] |
Definition at line 117 of file ompl_interface.h.
ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getLastPlanningContext | ( | ) | const [inline] |
Definition at line 97 of file ompl_interface.h.
const planning_interface::PlannerConfigurationMap& ompl_interface::OMPLInterface::getPlannerConfigurations | ( | ) | const [inline] |
Get the configurations for the planners that are already loaded.
pconfig | Configurations for the different planners |
Definition at line 76 of file ompl_interface.h.
ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene, |
const planning_interface::MotionPlanRequest & | req | ||
) | const |
Definition at line 97 of file ompl_interface.cpp.
ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene, |
const planning_interface::MotionPlanRequest & | req, | ||
moveit_msgs::MoveItErrorCodes & | error_code | ||
) | const |
Definition at line 104 of file ompl_interface.cpp.
ompl_interface::ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::getPlanningContext | ( | const std::string & | config, |
const std::string & | factory_type = "" |
||
) | const |
Definition at line 114 of file ompl_interface.cpp.
const PlanningContextManager& ompl_interface::OMPLInterface::getPlanningContextManager | ( | ) | const [inline] |
Definition at line 102 of file ompl_interface.h.
Definition at line 107 of file ompl_interface.h.
bool ompl_interface::OMPLInterface::isUsingConstraintsApproximations | ( | ) | const [inline] |
Definition at line 137 of file ompl_interface.h.
void ompl_interface::OMPLInterface::loadConstraintApproximations | ( | const std::string & | path | ) |
Definition at line 163 of file ompl_interface.cpp.
Look up param server 'constraint_approximations' and use its value as the path to load constraint approximations to.
Definition at line 188 of file ompl_interface.cpp.
void ompl_interface::OMPLInterface::loadConstraintSamplers | ( | ) | [protected] |
Load the additional plugins for sampling constraints.
Definition at line 199 of file ompl_interface.cpp.
void ompl_interface::OMPLInterface::loadPlannerConfigurations | ( | ) | [protected] |
Configure the planners.
Definition at line 204 of file ompl_interface.cpp.
ModelBasedPlanningContextPtr ompl_interface::OMPLInterface::prepareForSolve | ( | const planning_interface::MotionPlanRequest & | req, |
const planning_scene::PlanningSceneConstPtr & | planning_scene, | ||
moveit_msgs::MoveItErrorCodes * | error_code, | ||
unsigned int * | attempts, | ||
double * | timeout | ||
) | const [protected] |
Configure the OMPL planning context for a new planning request.
Print the status of this node.
Definition at line 322 of file ompl_interface.cpp.
void ompl_interface::OMPLInterface::saveConstraintApproximations | ( | const std::string & | path | ) |
Definition at line 171 of file ompl_interface.cpp.
Look up param server 'constraint_approximations' and use its value as the path to save constraint approximations to.
Definition at line 176 of file ompl_interface.cpp.
void ompl_interface::OMPLInterface::setPlannerConfigurations | ( | const planning_interface::PlannerConfigurationMap & | pconfig | ) |
Specify configurations for the planners.
pconfig | Configurations for the different planners |
Definition at line 78 of file ompl_interface.cpp.
bool ompl_interface::OMPLInterface::simplifySolutions | ( | ) | const [inline] |
Definition at line 146 of file ompl_interface.h.
void ompl_interface::OMPLInterface::simplifySolutions | ( | bool | flag | ) | [inline] |
Definition at line 151 of file ompl_interface.h.
bool ompl_interface::OMPLInterface::solve | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene, |
const planning_interface::MotionPlanRequest & | req, | ||
planning_interface::MotionPlanResponse & | res | ||
) | const |
Solve the planning problem.
Definition at line 131 of file ompl_interface.cpp.
bool ompl_interface::OMPLInterface::solve | ( | const planning_scene::PlanningSceneConstPtr & | planning_scene, |
const planning_interface::MotionPlanRequest & | req, | ||
planning_interface::MotionPlanDetailedResponse & | res | ||
) | const |
Solve the planning problem but give a more detailed response.
Definition at line 147 of file ompl_interface.cpp.
void ompl_interface::OMPLInterface::useConstraintsApproximations | ( | bool | flag | ) | [inline] |
Definition at line 132 of file ompl_interface.h.
constraint_samplers::ConstraintSamplerManagerPtr ompl_interface::OMPLInterface::constraint_sampler_manager_ [protected] |
Definition at line 187 of file ompl_interface.h.
constraint_sampler_manager_loader::ConstraintSamplerManagerLoaderPtr ompl_interface::OMPLInterface::constraint_sampler_manager_loader_ [private] |
Definition at line 198 of file ompl_interface.h.
Definition at line 191 of file ompl_interface.h.
Definition at line 189 of file ompl_interface.h.
The ROS node handle.
The kinematic model for which motion plans are computed
Definition at line 185 of file ompl_interface.h.
ros::NodeHandle ompl_interface::OMPLInterface::nh_ [protected] |
Definition at line 182 of file ompl_interface.h.
bool ompl_interface::OMPLInterface::simplify_solutions_ [protected] |
Definition at line 194 of file ompl_interface.h.
bool ompl_interface::OMPLInterface::use_constraints_approximations_ [protected] |
Definition at line 192 of file ompl_interface.h.