$search
Namespaces | |
namespace | msg |
Classes | |
class | KinematicStateToOmplStateMapping |
struct | OmplPlannerDiagnostics_ |
class | OmplRos |
Initializes a bunch of planners for different groups (collections of joints and links, e.g. a robot arm). This class gets all its parameters from the ROS parameter server. After initializing, just call run on the class to start it running. More... | |
class | OmplRosIKSampleableRegion |
Inherits from ompl::base::GoalSampleableRegion and can be used to sample goals using IK. More... | |
class | OmplRosIKSampler |
class | OmplRosIKStateTransformer |
A state trasformer that uses forward and inverse kinematics to convert to and from ompl and physical robot states. More... | |
class | OmplRosJointPlanner |
A joint planner - this is the planner that most applications will use. More... | |
class | OmplRosJointStateValidityChecker |
This class implements a state validity checker in joint space. More... | |
class | OmplRosPlanningGroup |
An instantiation of a particular planner for a specific group. More... | |
class | OmplRosProjectionEvaluator |
A projection evaluator specifically designed for the ROS interface to OMPL while using a compound state space. More... | |
class | OmplRosRPYIKStateTransformer |
class | OmplRosRPYIKTaskSpacePlanner |
class | OmplRosStateTransformer |
class | OmplRosStateValidityChecker |
A ROS wrapper around a ompl::base::StateValidityChecker object. More... | |
class | OmplRosTaskSpacePlanner |
A generic task space planner. More... | |
class | OmplRosTaskSpaceValidityChecker |
This class implements a state validity checker in task space. More... | |
class | OmplStateToKinematicStateMapping |
class | OmplStateToRobotStateMapping |
class | PlannerConfig |
A class to define a planner configuration. More... | |
class | PlannerConfigMap |
A map from group name to planner configuration names. This class is used internally and is not intended for external use. More... | |
class | RobotStateToKinematicStateMapping |
class | RobotStateToOmplStateMapping |
Typedefs | |
typedef ::ompl_ros_interface::OmplPlannerDiagnostics_ < std::allocator< void > > | OmplPlannerDiagnostics |
typedef boost::shared_ptr < ::ompl_ros_interface::OmplPlannerDiagnostics const > | OmplPlannerDiagnosticsConstPtr |
typedef boost::shared_ptr < ::ompl_ros_interface::OmplPlannerDiagnostics > | OmplPlannerDiagnosticsPtr |
typedef boost::shared_ptr < ompl_ros_interface::OmplRosStateValidityChecker > | OmplRosStateValidityCheckerPtr |
Enumerations | |
enum | MAPPING_TYPE { REAL_VECTOR, SO2, SO3, SE2, SE3, COMPOUND, UNKNOWN } |
enumeration of different mapping types for ompl More... | |
Functions | |
bool | addToOmplStateSpace (const planning_models::KinematicModel *kinematic_model, const std::string &joint_name, ompl::base::StateSpacePtr &ompl_state_space) |
Add a state to the ompl state space. | |
bool | constraintsToOmplState (const arm_navigation_msgs::Constraints &constraints, ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_scoped_state, const bool &fail_if_match_not_found=true) |
Convert a set of constraints to an ompl scoped state. | |
bool | getJointStateGroupToOmplStateMapping (planning_models::KinematicState::JointStateGroup *joint_state_group, const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, ompl_ros_interface::KinematicStateToOmplStateMapping &mapping) |
Create a mapping from the kinematic state to an ompl state. | |
bool | getJointStateToOmplStateMapping (const sensor_msgs::JointState &joint_state, const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, ompl_ros_interface::RobotStateToOmplStateMapping &mapping, const bool &fail_if_match_not_found=true) |
Create a mapping from the ROS message joint_state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
ompl_ros_interface::MAPPING_TYPE | getMappingType (const ompl::base::StateSpacePtr &state_space) |
ompl_ros_interface::MAPPING_TYPE | getMappingType (const ompl::base::StateSpace *state_space) |
ompl_ros_interface::MAPPING_TYPE | getMappingType (const planning_models::KinematicModel::JointModel *joint_model) |
bool | getOmplStateToJointStateGroupMapping (const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, planning_models::KinematicState::JointStateGroup *joint_state_group, ompl_ros_interface::OmplStateToKinematicStateMapping &mapping) |
Create a mapping from the ompl state to a kinematic state. | |
bool | getOmplStateToJointStateMapping (const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, const sensor_msgs::JointState &joint_state, ompl_ros_interface::OmplStateToRobotStateMapping &mapping, const bool &fail_if_match_not_found=true) |
Create a mapping from an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
bool | getOmplStateToJointTrajectoryMapping (const ompl::base::StateSpacePtr &state_space, const trajectory_msgs::JointTrajectory &joint_trajectory, ompl_ros_interface::OmplStateToRobotStateMapping &mapping) |
Get the mapping from an ompl state space to a JointTrajectory message. | |
bool | getOmplStateToRobotStateMapping (const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, const arm_navigation_msgs::RobotState &robot_state, ompl_ros_interface::OmplStateToRobotStateMapping &mapping, const bool &fail_if_match_not_found=true) |
Create a mapping from an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
bool | getOmplStateToRobotTrajectoryMapping (const ompl::base::StateSpacePtr &state_space, const arm_navigation_msgs::RobotTrajectory &robot_trajectory, ompl_ros_interface::OmplStateToRobotStateMapping &mapping) |
Get the mapping from an ompl state space to a RobotTrajectory message. | |
bool | getRobotStateToJointModelGroupMapping (const arm_navigation_msgs::RobotState &robot_state, const planning_models::KinematicModel::JointModelGroup *joint_model_group, ompl_ros_interface::RobotStateToKinematicStateMapping &mapping) |
Get the mapping from a RobotState message to a JointModelGroup. | |
bool | getRobotStateToOmplStateMapping (const arm_navigation_msgs::RobotState &robot_state, const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, ompl_ros_interface::RobotStateToOmplStateMapping &mapping, const bool &fail_if_match_not_found=true) |
Create a mapping from the ROS message robot_state to an ompl state. This function is intended to help in efficient conversion between ROS messages and an ompl state object by providing a mapping that can be cached and used frequently. | |
bool | jointConstraintsToOmplState (const std::vector< arm_navigation_msgs::JointConstraint > &joint_constraints, ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_scoped_state) |
Convert a set of joint constraints to an ompl scoped state. | |
ompl::base::StateSpacePtr | jointGroupToOmplStateSpacePtr (const planning_models::KinematicModel::JointModelGroup *joint_group, ompl_ros_interface::OmplStateToKinematicStateMapping &ompl_kinematic_mapping, ompl_ros_interface::KinematicStateToOmplStateMapping &kinematic_ompl_mapping) |
create a object of type ompl::base::CompoundStateSpace from an object of type planning_models::KinematicModel::JointGroup | |
bool | jointStateGroupToRobotTrajectory (planning_models::KinematicState::JointStateGroup *joint_state_group, arm_navigation_msgs::RobotTrajectory &robot_trajectory) |
Create a RobotTrajectory message from a joint state group. | |
bool | jointStateToRealVectorState (const sensor_msgs::JointState &joint_state, const ompl_ros_interface::RobotStateToOmplStateMapping &mapping, ompl::base::RealVectorStateSpace::StateType &real_vector_state, const bool &fail_if_match_not_found=true) |
Convert a ROS message joint state to an ompl real vector state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
bool | kinematicStateGroupToOmplState (const planning_models::KinematicState::JointStateGroup *joint_state_group, const ompl_ros_interface::KinematicStateToOmplStateMapping &mapping, ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state) |
Convert a kinematic state to an ompl scoped state given the appropriate mapping. | |
bool | omplPathGeometricToRobotTrajectory (const ompl::geometric::PathGeometric &path, const ompl_ros_interface::OmplStateToRobotStateMapping &mapping, arm_navigation_msgs::RobotTrajectory &robot_trajectory) |
Convert an ompl path to a RobotTrajectory message. | |
bool | omplPathGeometricToRobotTrajectory (const ompl::geometric::PathGeometric &path, const ompl::base::StateSpacePtr &state_space, arm_navigation_msgs::RobotTrajectory &robot_trajectory) |
Convert an ompl path to a RobotTrajectory message. | |
bool | omplRealVectorStateToJointState (const ompl::base::RealVectorStateSpace::StateType &ompl_state, const ompl_ros_interface::OmplStateToRobotStateMapping &mapping, sensor_msgs::JointState &joint_state) |
Convert an ompl state to the ROS message joint_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
bool | omplStateToKinematicStateGroup (const ompl::base::State *ompl_state, const ompl_ros_interface::OmplStateToKinematicStateMapping &mapping, planning_models::KinematicState::JointStateGroup *joint_state_group) |
Convert an ompl state to a kinematic state given the appropriate mapping. | |
bool | omplStateToKinematicStateGroup (const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, const ompl_ros_interface::OmplStateToKinematicStateMapping &mapping, planning_models::KinematicState::JointStateGroup *joint_state_group) |
Convert an ompl scoped state to a kinematic state given the appropriate mapping. | |
bool | omplStateToRobotState (const ompl::base::State &ompl_state, const ompl_ros_interface::OmplStateToRobotStateMapping &mapping, arm_navigation_msgs::RobotState &robot_state) |
Convert an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
bool | omplStateToRobotState (const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, const ompl_ros_interface::OmplStateToRobotStateMapping &mapping, arm_navigation_msgs::RobotState &robot_state) |
Convert an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
template<typename ContainerAllocator > | |
std::ostream & | operator<< (std::ostream &s, const ::ompl_ros_interface::OmplPlannerDiagnostics_< ContainerAllocator > &v) |
void | poseMsgToSE3StateSpace (const geometry_msgs::Pose &pose_msg, ompl::base::SE3StateSpace::StateType &pose) |
Convert a pose message to a SE3StateSpace::StateType. | |
void | quaternionMsgToSO3StateSpace (const geometry_msgs::Quaternion &quaternion_msg, ompl::base::SO3StateSpace::StateType &quaternion) |
Convert a quaternion message to a SO3StateSpace::StateType. | |
bool | robotStateToJointStateGroup (const arm_navigation_msgs::RobotState &robot_state, const ompl_ros_interface::RobotStateToKinematicStateMapping &robot_state_to_joint_state_group_mapping, planning_models::KinematicState::JointStateGroup *joint_state_group) |
Convert from a RobotState to a JointStateGroup. | |
bool | robotStateToOmplState (const arm_navigation_msgs::RobotState &robot_state, ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_scoped_state, const bool &fail_if_match_not_found=true) |
Convert a ROS message robot state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
bool | robotStateToOmplState (const arm_navigation_msgs::RobotState &robot_state, const ompl_ros_interface::RobotStateToOmplStateMapping &mapping, ompl::base::State *ompl_state, const bool &fail_if_match_not_found=true) |
Convert a ROS message robot state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
bool | robotStateToOmplState (const arm_navigation_msgs::RobotState &robot_state, const ompl_ros_interface::RobotStateToOmplStateMapping &mapping, ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, const bool &fail_if_match_not_found=true) |
Convert a ROS message robot state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently. | |
void | SE2StateSpaceToPoseMsg (const ompl::base::SE2StateSpace::StateType &pose, geometry_msgs::Pose &pose_msg) |
Convert a SE2StateSpace::StateType to a pose message. | |
void | SE3StateSpaceToPoseMsg (const ompl::base::SE3StateSpace::StateType &pose, geometry_msgs::Pose &pose_msg) |
Convert a SE3StateSpace::StateType to a pose message. | |
void | SO3StateSpaceToPoseMsg (const ompl::base::SO3StateSpace::StateType &quaternion, geometry_msgs::Pose &pose_msg) |
Convert a SO3StateSpace::StateType to a pose message. | |
void | SO3StateSpaceToQuaternionMsg (const ompl::base::SO3StateSpace::StateType &quaternion, geometry_msgs::Quaternion &quaternion_msg) |
Convert a SO3StateSpace::StateType to a quaternion message. |
typedef ::ompl_ros_interface::OmplPlannerDiagnostics_<std::allocator<void> > ompl_ros_interface::OmplPlannerDiagnostics |
Definition at line 149 of file OmplPlannerDiagnostics.h.
typedef boost::shared_ptr< ::ompl_ros_interface::OmplPlannerDiagnostics const> ompl_ros_interface::OmplPlannerDiagnosticsConstPtr |
Definition at line 152 of file OmplPlannerDiagnostics.h.
typedef boost::shared_ptr< ::ompl_ros_interface::OmplPlannerDiagnostics> ompl_ros_interface::OmplPlannerDiagnosticsPtr |
Definition at line 151 of file OmplPlannerDiagnostics.h.
typedef boost::shared_ptr<ompl_ros_interface::OmplRosStateValidityChecker> ompl_ros_interface::OmplRosStateValidityCheckerPtr |
Definition at line 115 of file ompl_ros_state_validity_checker.h.
enumeration of different mapping types for ompl
Definition at line 68 of file ompl_ros_conversions.h.
bool ompl_ros_interface::addToOmplStateSpace | ( | const planning_models::KinematicModel * | kinematic_model, | |
const std::string & | joint_name, | |||
ompl::base::StateSpacePtr & | ompl_state_space | |||
) |
Add a state to the ompl state space.
kinematic_model | - the kinematic model to use for getting properties of the particular joint | |
joint_name | - The joint name to add | |
ompl_state_space | - The state space to add joints to |
Definition at line 141 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::constraintsToOmplState | ( | const arm_navigation_msgs::Constraints & | constraints, | |
ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_scoped_state, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Convert a set of constraints to an ompl scoped state.
constraints | The input joint constraints | |
ompl_scoped_state | The output scoped state | |
fail_if_match_not_found | - if true, fail if no matching joint was found for any of the joints specified in the constraints |
Definition at line 1123 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getJointStateGroupToOmplStateMapping | ( | planning_models::KinematicState::JointStateGroup * | joint_state_group, | |
const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |||
ompl_ros_interface::KinematicStateToOmplStateMapping & | mapping | |||
) |
Create a mapping from the kinematic state to an ompl state.
kinematic_state | The kinematic state to create the mapping from | |
ompl_state | The ompl state to create the mapping to | |
mapping | The resultant mapping |
Definition at line 269 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getJointStateToOmplStateMapping | ( | const sensor_msgs::JointState & | joint_state, | |
const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |||
ompl_ros_interface::RobotStateToOmplStateMapping & | mapping, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Create a mapping from the ROS message joint_state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
robot_state | The joint state message to create the mapping from | |
ompl_state | The ompl state to create the mapping to | |
mapping | The resultant mapping |
Definition at line 428 of file ompl_ros_conversions.cpp.
ompl_ros_interface::MAPPING_TYPE ompl_ros_interface::getMappingType | ( | const ompl::base::StateSpacePtr & | state_space | ) |
Definition at line 263 of file ompl_ros_conversions.cpp.
ompl_ros_interface::MAPPING_TYPE ompl_ros_interface::getMappingType | ( | const ompl::base::StateSpace * | state_space | ) |
Definition at line 233 of file ompl_ros_conversions.cpp.
ompl_ros_interface::MAPPING_TYPE ompl_ros_interface::getMappingType | ( | const planning_models::KinematicModel::JointModel * | joint_model | ) |
Definition at line 211 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getOmplStateToJointStateGroupMapping | ( | const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |
planning_models::KinematicState::JointStateGroup * | joint_state_group, | |||
ompl_ros_interface::OmplStateToKinematicStateMapping & | mapping | |||
) |
Create a mapping from the ompl state to a kinematic state.
ompl_state | The ompl state to create the mapping from | |
joint_state_group | The kinematic state to create the mapping to | |
mapping | The resultant mapping |
Definition at line 333 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getOmplStateToJointStateMapping | ( | const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |
const sensor_msgs::JointState & | joint_state, | |||
ompl_ros_interface::OmplStateToRobotStateMapping & | mapping, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Create a mapping from an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
ompl_state | The ompl state to create the mapping from | |
joint_state | The joint state message to create the mapping to | |
mapping | The resultant mapping |
Definition at line 480 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getOmplStateToJointTrajectoryMapping | ( | const ompl::base::StateSpacePtr & | state_space, | |
const trajectory_msgs::JointTrajectory & | joint_trajectory, | |||
ompl_ros_interface::OmplStateToRobotStateMapping & | mapping | |||
) |
Get the mapping from an ompl state space to a JointTrajectory message.
state_space | The state space | |
joint_trajectory | The JointTrajectory message to find the mapping for | |
mapping | The mapping from the state_space to the robot trajectory |
Definition at line 1031 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getOmplStateToRobotStateMapping | ( | const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |
const arm_navigation_msgs::RobotState & | robot_state, | |||
ompl_ros_interface::OmplStateToRobotStateMapping & | mapping, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Create a mapping from an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
ompl_state | The ompl state to create the mapping from | |
robot_state | The joint state message to create the mapping to | |
mapping | The resultant mapping |
Definition at line 549 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getOmplStateToRobotTrajectoryMapping | ( | const ompl::base::StateSpacePtr & | state_space, | |
const arm_navigation_msgs::RobotTrajectory & | robot_trajectory, | |||
ompl_ros_interface::OmplStateToRobotStateMapping & | mapping | |||
) |
Get the mapping from an ompl state space to a RobotTrajectory message.
state_space | The state space | |
robot_trajectory | The RobotTrajectory message to find the mapping for | |
mapping | The mapping from the state_space to the robot trajectory |
Definition at line 997 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getRobotStateToJointModelGroupMapping | ( | const arm_navigation_msgs::RobotState & | robot_state, | |
const planning_models::KinematicModel::JointModelGroup * | joint_model_group, | |||
ompl_ros_interface::RobotStateToKinematicStateMapping & | mapping | |||
) |
Get the mapping from a RobotState message to a JointModelGroup.
robot_state | - The input RobotState | |
joint_model_group | - The resultant joint model group | |
mapping | - The mapping from the robot state to the joint model |
Definition at line 1142 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::getRobotStateToOmplStateMapping | ( | const arm_navigation_msgs::RobotState & | robot_state, | |
const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |||
ompl_ros_interface::RobotStateToOmplStateMapping & | mapping, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Create a mapping from the ROS message robot_state to an ompl state. This function is intended to help in efficient conversion between ROS messages and an ompl state object by providing a mapping that can be cached and used frequently.
robot_state | The robot state to create the mapping from | |
ompl_state | The ompl state to create the mapping to | |
mapping | The resultant mapping |
Definition at line 392 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::jointConstraintsToOmplState | ( | const std::vector< arm_navigation_msgs::JointConstraint > & | joint_constraints, | |
ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_scoped_state | |||
) |
Convert a set of joint constraints to an ompl scoped state.
joint_constraints | The input joint constraints | |
ompl_scoped_state | The output scoped state |
Definition at line 1106 of file ompl_ros_conversions.cpp.
ompl::base::StateSpacePtr ompl_ros_interface::jointGroupToOmplStateSpacePtr | ( | const planning_models::KinematicModel::JointModelGroup * | joint_group, | |
ompl_ros_interface::OmplStateToKinematicStateMapping & | ompl_kinematic_mapping, | |||
ompl_ros_interface::KinematicStateToOmplStateMapping & | kinematic_ompl_mapping | |||
) |
create a object of type ompl::base::CompoundStateSpace from an object of type planning_models::KinematicModel::JointGroup
joint_group | The joint group to construct this from | |
state_space | The output state space | |
ompl_kinematic_mapping | Mapping from the ompl state to the corresponding kinematic state | |
kinematic_ompl_mapping | Mapping from the kinematic state to the corresponding ompl state |
Definition at line 41 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::jointStateGroupToRobotTrajectory | ( | planning_models::KinematicState::JointStateGroup * | joint_state_group, | |
arm_navigation_msgs::RobotTrajectory & | robot_trajectory | |||
) |
Create a RobotTrajectory message from a joint state group.
Create a robot trajectory message for a joint state group.
joint_state_group | The kinematic state to convert from | |
robot_trajectory | The robot trajectory that was created |
joint_state_group | The input group | |
robot_trajectory | The output robot trajectory |
Definition at line 892 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::jointStateToRealVectorState | ( | const sensor_msgs::JointState & | joint_state, | |
const ompl_ros_interface::RobotStateToOmplStateMapping & | mapping, | |||
ompl::base::RealVectorStateSpace::StateType & | real_vector_state, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Convert a ROS message joint state to an ompl real vector state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
joint_state | The joint state message to create the mapping to | |
real_vector_state | The ompl state to create the mapping from | |
mapping | The mapping to use for this conversion |
Definition at line 657 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::kinematicStateGroupToOmplState | ( | const planning_models::KinematicState::JointStateGroup * | joint_state_group, | |
const ompl_ros_interface::KinematicStateToOmplStateMapping & | mapping, | |||
ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_scoped_state | |||
) |
Convert a kinematic state to an ompl scoped state given the appropriate mapping.
Convert a kinematic state to an ompl state given the appropriate mapping.
joint_state_group | The kinematic state to convert from | |
mapping | The given mapping | |
ompl_state | The ompl scoped state to convert to | |
joint_state_group | The kinematic state to convert from | |
mapping | The given mapping | |
ompl_scoped_state | The ompl state to convert to |
Definition at line 731 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::omplPathGeometricToRobotTrajectory | ( | const ompl::geometric::PathGeometric & | path, | |
const ompl_ros_interface::OmplStateToRobotStateMapping & | mapping, | |||
arm_navigation_msgs::RobotTrajectory & | robot_trajectory | |||
) |
Convert an ompl path to a RobotTrajectory message.
path | The ompl path | |
mapping | The mapping from the path to the robot trajectory | |
robot_trajectory | The RobotTrajectory message to convert the path to |
Definition at line 939 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::omplPathGeometricToRobotTrajectory | ( | const ompl::geometric::PathGeometric & | path, | |
const ompl::base::StateSpacePtr & | state_space, | |||
arm_navigation_msgs::RobotTrajectory & | robot_trajectory | |||
) |
Convert an ompl path to a RobotTrajectory message.
path | The ompl path | |
state_space | The state space corresponding to the path | |
robot_trajectory | The RobotTrajectory message to convert the path to |
Definition at line 922 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::omplRealVectorStateToJointState | ( | const ompl::base::RealVectorStateSpace::StateType & | ompl_state, | |
const ompl_ros_interface::OmplStateToRobotStateMapping & | mapping, | |||
sensor_msgs::JointState & | joint_state | |||
) |
Convert an ompl state to the ROS message joint_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
ompl_state | The ompl state to create the mapping from | |
mapping | The mapping to use for this conversion | |
joint_state | The joint state message to create the mapping to |
Definition at line 599 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::omplStateToKinematicStateGroup | ( | const ompl::base::State * | ompl_state, | |
const ompl_ros_interface::OmplStateToKinematicStateMapping & | mapping, | |||
planning_models::KinematicState::JointStateGroup * | joint_state_group | |||
) |
Convert an ompl state to a kinematic state given the appropriate mapping.
ompl_state | The ompl state to convert to | |
mapping | The given mapping | |
joint_state_group | The kinematic state to convert from |
Definition at line 837 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::omplStateToKinematicStateGroup | ( | const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_scoped_state, | |
const ompl_ros_interface::OmplStateToKinematicStateMapping & | mapping, | |||
planning_models::KinematicState::JointStateGroup * | joint_state_group | |||
) |
Convert an ompl scoped state to a kinematic state given the appropriate mapping.
Convert an ompl state to a kinematic state given the appropriate mapping.
ompl_state | The ompl scoped state to convert from | |
mapping | The given mapping | |
joint_state_group | The kinematic state to convert to | |
ompl_scoped_state | The ompl state to convert to | |
mapping | The given mapping | |
joint_state_group | The kinematic state to convert from |
Definition at line 780 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::omplStateToRobotState | ( | const ompl::base::State & | ompl_state, | |
const ompl_ros_interface::OmplStateToRobotStateMapping & | mapping, | |||
arm_navigation_msgs::RobotState & | robot_state | |||
) |
Convert an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
ompl_state | The ompl state to create the mapping from | |
mapping | The mapping to use for this conversion | |
robot_state | The robot state message to create the mapping to |
bool ompl_ros_interface::omplStateToRobotState | ( | const ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |
const ompl_ros_interface::OmplStateToRobotStateMapping & | mapping, | |||
arm_navigation_msgs::RobotState & | robot_state | |||
) |
Convert an ompl state to the ROS message robot_state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
ompl_state | The ompl state to create the mapping from | |
mapping | The mapping to use for this conversion | |
robot_state | The robot state message to create the mapping to |
Definition at line 579 of file ompl_ros_conversions.cpp.
std::ostream& ompl_ros_interface::operator<< | ( | std::ostream & | s, | |
const ::ompl_ros_interface::OmplPlannerDiagnostics_< ContainerAllocator > & | v | |||
) | [inline] |
Definition at line 156 of file OmplPlannerDiagnostics.h.
void ompl_ros_interface::poseMsgToSE3StateSpace | ( | const geometry_msgs::Pose & | pose_msg, | |
ompl::base::SE3StateSpace::StateType & | pose | |||
) |
Convert a pose message to a SE3StateSpace::StateType.
pose_msg | - the input pose message | |
pose | - the resultant object of type SE3StateSpace::StateType |
Definition at line 707 of file ompl_ros_conversions.cpp.
void ompl_ros_interface::quaternionMsgToSO3StateSpace | ( | const geometry_msgs::Quaternion & | quaternion_msg, | |
ompl::base::SO3StateSpace::StateType & | quaternion | |||
) |
Convert a quaternion message to a SO3StateSpace::StateType.
pose_msg | - the resultant quaternion message | |
pose | - an object of type SO3StateSpace::StateType |
Definition at line 716 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::robotStateToJointStateGroup | ( | const arm_navigation_msgs::RobotState & | robot_state, | |
const ompl_ros_interface::RobotStateToKinematicStateMapping & | robot_state_to_joint_state_group_mapping, | |||
planning_models::KinematicState::JointStateGroup * | joint_state_group | |||
) |
Convert from a RobotState to a JointStateGroup.
robot_state | - The input RobotState | |
robot_state_to_joint_state_group_mapping | - The mapping from the robot state to the joint state group | |
joint_state_group | - The resultant joint state group |
Definition at line 1185 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::robotStateToOmplState | ( | const arm_navigation_msgs::RobotState & | robot_state, | |
ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_scoped_state, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Convert a ROS message robot state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
robot_state | The robot state message to create the mapping to | |
ompl_state | The ompl state to create the mapping from |
Definition at line 647 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::robotStateToOmplState | ( | const arm_navigation_msgs::RobotState & | robot_state, | |
const ompl_ros_interface::RobotStateToOmplStateMapping & | mapping, | |||
ompl::base::State * | ompl_state, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Convert a ROS message robot state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
robot_state | The robot state message to create the mapping to | |
mapping | The mapping to use for this conversion | |
ompl_state | The ompl state to create the mapping from |
Definition at line 618 of file ompl_ros_conversions.cpp.
bool ompl_ros_interface::robotStateToOmplState | ( | const arm_navigation_msgs::RobotState & | robot_state, | |
const ompl_ros_interface::RobotStateToOmplStateMapping & | mapping, | |||
ompl::base::ScopedState< ompl::base::CompoundStateSpace > & | ompl_state, | |||
const bool & | fail_if_match_not_found = true | |||
) |
Convert a ROS message robot state to an ompl state. This function is intended to help in efficient conversion between ROS messages and OMPL state objects by providing a mapping that can be cached and used frequently.
robot_state | The robot state message to create the mapping to | |
mapping | The mapping to use for this conversion | |
ompl_state | The ompl state to create the mapping from |
Definition at line 610 of file ompl_ros_conversions.cpp.
void ompl_ros_interface::SE2StateSpaceToPoseMsg | ( | const ompl::base::SE2StateSpace::StateType & | pose, | |
geometry_msgs::Pose & | pose_msg | |||
) |
Convert a SE2StateSpace::StateType to a pose message.
pose | - an object of type SE2StateSpace::StateType | |
pose_msg | - the resultant pose message |
Definition at line 689 of file ompl_ros_conversions.cpp.
void ompl_ros_interface::SE3StateSpaceToPoseMsg | ( | const ompl::base::SE3StateSpace::StateType & | pose, | |
geometry_msgs::Pose & | pose_msg | |||
) |
Convert a SE3StateSpace::StateType to a pose message.
pose | - an object of type SE3StateSpace::StateType | |
pose_msg | - the resultant pose message |
Definition at line 668 of file ompl_ros_conversions.cpp.
void ompl_ros_interface::SO3StateSpaceToPoseMsg | ( | const ompl::base::SO3StateSpace::StateType & | quaternion, | |
geometry_msgs::Pose & | pose_msg | |||
) |
Convert a SO3StateSpace::StateType to a pose message.
pose | - an object of type SO3StateSpace::StateType | |
pose_msg | - the resultant pose message |
Definition at line 683 of file ompl_ros_conversions.cpp.
void ompl_ros_interface::SO3StateSpaceToQuaternionMsg | ( | const ompl::base::SO3StateSpace::StateType & | quaternion, | |
geometry_msgs::Quaternion & | quaternion_msg | |||
) |
Convert a SO3StateSpace::StateType to a quaternion message.
pose | - an object of type SO3StateSpace::StateType | |
quaternion_msg | - the resultant quaternion message |
Definition at line 698 of file ompl_ros_conversions.cpp.