#include <ros/ros.h>#include <iostream>#include <cmath>#include <stdexcept>#include "duration.h"#include <sys/time.h>#include "ros/time.h"#include <cstdio>#include <sstream>#include <log4cxx/logger.h>#include "ros/console.h"#include <boost/static_assert.hpp>#include <cassert>#include <stdint.h>#include <assert.h>#include <stddef.h>#include <string>#include "ros/assert.h"#include <vector>#include <map>#include <set>#include <list>#include <boost/shared_ptr.hpp>#include <boost/weak_ptr.hpp>#include <boost/function.hpp>#include "exceptions.h"#include <boost/shared_array.hpp>#include "ros/types.h"#include "ros/forwards.h"#include "ros/common.h"#include "ros/macros.h"#include <string.h>#include <boost/array.hpp>#include "serialized_message.h"#include "message_forward.h"#include <boost/utility/enable_if.hpp>#include <boost/type_traits/remove_const.hpp>#include <boost/type_traits/remove_reference.hpp>#include "message_traits.h"#include "ros/exception.h"#include <boost/call_traits.hpp>#include <boost/mpl/and.hpp>#include <boost/mpl/or.hpp>#include <boost/mpl/not.hpp>#include <cstring>#include <boost/bind.hpp>#include <typeinfo>#include <ros/message.h>#include <boost/type_traits/is_void.hpp>#include <boost/type_traits/is_base_of.hpp>#include <boost/type_traits/is_const.hpp>#include <boost/type_traits/add_const.hpp>#include <boost/make_shared.hpp>#include <ros/static_assert.h>#include "ros/message_traits.h"#include "ros/builtin_message_traits.h"#include "ros/serialization.h"#include "ros/message_event.h"#include "forwards.h"#include "timer_options.h"#include "wall_timer_options.h"#include "ros/service_traits.h"#include <boost/lexical_cast.hpp>#include "subscription_callback_helper.h"#include "ros/spinner.h"#include <time.h>#include "ros/publisher.h"#include <boost/utility.hpp>#include "ros/service_server.h"#include "ros/subscriber.h"#include "ros/node_handle.h"#include "ros/init.h"#include "XmlRpcValue.h"#include "node_handle.h"#include "ros/names.h"#include <algorithm>#include <stdio.h>#include <boost/concept_check.hpp>#include "ompl/base/State.h"#include <boost/random/mersenne_twister.hpp>#include <boost/random/uniform_real.hpp>#include <boost/random/normal_distribution.hpp>#include <boost/random/variate_generator.hpp>#include <boost/noncopyable.hpp>#include "ompl/util/ClassForward.h"#include <cstdarg>#include <valarray>#include "ompl/util/Console.h"#include "ompl/base/StateSpace.h"#include "ompl/base/spaces/SO2StateSpace.h"#include "ompl/base/spaces/RealVectorStateSpace.h"#include "ompl/base/spaces/SO3StateSpace.h"#include <utility>#include <cstdlib>#include "ompl/base/SpaceInformation.h"#include <ctype.h>#include <stdlib.h>#include "tinystr.h"#include "link.h"#include "btMatrix3x3.h"#include <boost/thread/shared_mutex.hpp>#include <boost/bimap.hpp>#include "kinematic_model.h"#include <ros/ros.h>#include <iomanip>#include <boost/thread/mutex.hpp>#include <ostream>#include "ros/message_operations.h"#include "std_msgs/Header.h"#include "geometry_msgs/Vector3.h"#include "geometry_msgs/Quaternion.h"#include "geometry_msgs/Point.h"#include "LinearMath/btTransform.h"#include "tf/exceptions.h"#include <boost/signals.hpp>#include "geometry_msgs/Pose.h"#include <trajectory_msgs/JointTrajectory.h>#include <sensor_msgs/JointState.h>#include "geometric_shapes_msgs/Shape.h"#include "geometry_msgs/PoseStamped.h"#include "motion_planning_msgs/RobotState.h"#include "motion_planning_msgs/JointConstraint.h"#include "motion_planning_msgs/PositionConstraint.h"#include "motion_planning_msgs/OrientationConstraint.h"#include "geometry_msgs/PointStamped.h"#include "motion_planning_msgs/Constraints.h"#include "motion_planning_msgs/RobotTrajectory.h"#include "motion_planning_msgs/ArmNavigationErrorCodes.h"
Go to the source code of this file.
Classes | |
| class | ompl_ros_interface::KinematicStateToOmplStateMapping |
| class | ompl_ros_interface::OmplStateToKinematicStateMapping |
| class | ompl_ros_interface::OmplStateToRobotStateMapping |
| class | ompl_ros_interface::RobotStateToKinematicStateMapping |
| class | ompl_ros_interface::RobotStateToOmplStateMapping |
Namespaces | |
| namespace | ompl_ros_interface |
Enumerations | |
| enum | ompl_ros_interface::MAPPING_TYPE { ompl_ros_interface::REAL_VECTOR, ompl_ros_interface::SO2, ompl_ros_interface::SO3, ompl_ros_interface::SE2, ompl_ros_interface::SE3, ompl_ros_interface::COMPOUND, ompl_ros_interface::UNKNOWN } |
enumeration of different mapping types for ompl More... | |
Functions | |
| bool | ompl_ros_interface::addToOmplStateSpace (boost::shared_ptr< 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 | ompl_ros_interface::constraintsToOmplState (const motion_planning_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 | 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. | |
| 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. | |
| ompl_ros_interface::MAPPING_TYPE | ompl_ros_interface::getMappingType (const ompl::base::StateSpacePtr &state_space) |
| ompl_ros_interface::MAPPING_TYPE | ompl_ros_interface::getMappingType (const ompl::base::StateSpace *state_space) |
| ompl_ros_interface::MAPPING_TYPE | ompl_ros_interface::getMappingType (const planning_models::KinematicModel::JointModel *joint_model) |
| 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. | |
| 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. | |
| 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. | |
| bool | ompl_ros_interface::getOmplStateToRobotStateMapping (const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, const motion_planning_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 | ompl_ros_interface::getOmplStateToRobotTrajectoryMapping (const ompl::base::StateSpacePtr &state_space, const motion_planning_msgs::RobotTrajectory &robot_trajectory, ompl_ros_interface::OmplStateToRobotStateMapping &mapping) |
| Get the mapping from an ompl state space to a RobotTrajectory message. | |
| bool | ompl_ros_interface::getRobotStateToJointModelGroupMapping (const motion_planning_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 | ompl_ros_interface::getRobotStateToOmplStateMapping (const motion_planning_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 | ompl_ros_interface::jointConstraintsToOmplState (const std::vector< motion_planning_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 | 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 | |
| bool | ompl_ros_interface::jointStateGroupToRobotTrajectory (planning_models::KinematicState::JointStateGroup *joint_state_group, motion_planning_msgs::RobotTrajectory &robot_trajectory) |
| Create a RobotTrajectory message from a joint state group. | |
| 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. | |
| 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_state) |
| Convert a kinematic state to an ompl scoped state given the appropriate mapping. | |
| bool | ompl_ros_interface::omplPathGeometricToRobotTrajectory (const ompl::geometric::PathGeometric &path, const ompl_ros_interface::OmplStateToRobotStateMapping &mapping, motion_planning_msgs::RobotTrajectory &robot_trajectory) |
| Convert an ompl path to a RobotTrajectory message. | |
| bool | ompl_ros_interface::omplPathGeometricToRobotTrajectory (const ompl::geometric::PathGeometric &path, const ompl::base::StateSpacePtr &state_space, motion_planning_msgs::RobotTrajectory &robot_trajectory) |
| Convert an ompl path to a RobotTrajectory message. | |
| 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. | |
| 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. | |
| bool | ompl_ros_interface::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 | ompl_ros_interface::omplStateToRobotState (const ompl::base::State &ompl_state, const ompl_ros_interface::OmplStateToRobotStateMapping &mapping, motion_planning_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 | ompl_ros_interface::omplStateToRobotState (const ompl::base::ScopedState< ompl::base::CompoundStateSpace > &ompl_state, const ompl_ros_interface::OmplStateToRobotStateMapping &mapping, motion_planning_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. | |
| void | ompl_ros_interface::poseMsgToSE3StateSpace (const geometry_msgs::Pose &pose_msg, ompl::base::SE3StateSpace::StateType &pose) |
| Convert a pose message to a SE3StateSpace::StateType. | |
| void | ompl_ros_interface::quaternionMsgToSO3StateSpace (const geometry_msgs::Quaternion &quaternion_msg, ompl::base::SO3StateSpace::StateType &quaternion) |
| Convert a quaternion message to a SO3StateSpace::StateType. | |
| bool | ompl_ros_interface::robotStateToJointStateGroup (const motion_planning_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 | ompl_ros_interface::robotStateToOmplState (const motion_planning_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 | ompl_ros_interface::robotStateToOmplState (const motion_planning_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 | ompl_ros_interface::robotStateToOmplState (const motion_planning_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 | ompl_ros_interface::SE2StateSpaceToPoseMsg (const ompl::base::SE2StateSpace::StateType &pose, geometry_msgs::Pose &pose_msg) |
| Convert a SE2StateSpace::StateType to a pose message. | |
| void | ompl_ros_interface::SE3StateSpaceToPoseMsg (const ompl::base::SE3StateSpace::StateType &pose, geometry_msgs::Pose &pose_msg) |
| Convert a SE3StateSpace::StateType to a pose message. | |
| void | ompl_ros_interface::SO3StateSpaceToPoseMsg (const ompl::base::SO3StateSpace::StateType &quaternion, geometry_msgs::Pose &pose_msg) |
| Convert a SO3StateSpace::StateType to a pose message. | |
| void | ompl_ros_interface::SO3StateSpaceToQuaternionMsg (const ompl::base::SO3StateSpace::StateType &quaternion, geometry_msgs::Quaternion &quaternion_msg) |
| Convert a SO3StateSpace::StateType to a quaternion message. | |