├── ompl_global_planner_plugin.xml
├── README.md
├── package.xml
├── .travis.yml
├── CMakeLists.txt
├── include
    └── ompl_global_planner.h
└── src
    └── ompl_global_planner.cpp


/ompl_global_planner_plugin.xml:
--------------------------------------------------------------------------------
1 | <library path="lib/libompl_global_planner_lib">
2 |   <class name="ompl_global_planner/OmplGlobalPlanner" type="ompl_global_planner::OmplGlobalPlanner" base_class_type="nav_core::BaseGlobalPlanner">
3 |     <description>
4 |       OMPL global planner
5 |     </description>
6 |   </class>
7 | </library>
8 | 


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | move-base-ompl
 2 | ==============
 3 | 
 4 | ROS move-base planner plugin using ompl
 5 | 
 6 | [![Build Status](https://travis-ci.org/windelbouwman/move-base-ompl.svg)](https://travis-ci.org/windelbouwman/move-base-ompl)
 7 | 
 8 | About
 9 | -----
10 | 
11 | The move-base package for ROS contains several planners for 2D navigation
12 | of moving base. This package provides two plugins to expose the routing
13 | capabilities of the OMPL library.
14 | 
15 | Attention: This work is very experimental and pull-requests and suggestions are welcome!
16 | 
17 | Usage
18 | -----
19 | 
20 | To use this planner, check it out in your local catkin workspace:
21 | 
22 |     $ cd ~/catkin_ws/src
23 |     $ git clone https://github.com/windelbouwman/move-base-ompl.git
24 |     $ cd ..
25 |     $ catkin_make
26 | 
27 | Now the planner should be placed as shared object in the devel/lib folder. Now you can use in 
28 | together with the move_base node.
29 | 
30 | List the plugin with the command:
31 | 
32 |     $ rospack plugins --attrib=plugin nav_core
33 | 
34 | 
35 | Then use the plugin in your move_base_parameters.yaml file:
36 | 
37 |     base_global_planner: ompl_global_planner/OmplGlobalPlanner
38 | 
39 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <package>
 2 |   <name>move_base_ompl</name>
 3 |   <version>0.0.1</version>
 4 |   <description>
 5 |     ROS move-base planner plugin using ompl
 6 |   </description>
 7 |   <maintainer email="windel.bouwman@gmail.com">Windel Bouwman</maintainer>
 8 |   <license>BSD</license>
 9 | 
10 |   <author>Windel Bouwman</author>
11 | 
12 |   <buildtool_depend>catkin</buildtool_depend>
13 | 
14 |   <build_depend>costmap_2d</build_depend>
15 |   <build_depend>geometry_msgs</build_depend>
16 |   <build_depend>nav_core</build_depend>
17 |   <build_depend>nav_msgs</build_depend>
18 |   <build_depend>navfn</build_depend>
19 |   <build_depend>pluginlib</build_depend>
20 |   <build_depend>roscpp</build_depend>
21 |   <build_depend>tf</build_depend>
22 |   <build_depend>ompl</build_depend>
23 | 
24 |   <run_depend>costmap_2d</run_depend>
25 |   <run_depend>geometry_msgs</run_depend>
26 |   <run_depend>nav_core</run_depend>
27 |   <run_depend>nav_msgs</run_depend>
28 |   <run_depend>navfn</run_depend>
29 |   <run_depend>pluginlib</run_depend>
30 |   <run_depend>roscpp</run_depend>
31 |   <run_depend>tf</run_depend>
32 |   <run_depend>ompl</run_depend>
33 |   <export>
34 |       <nav_core plugin="${prefix}/ompl_global_planner_plugin.xml" />
35 |   </export>
36 | 
37 | </package>
38 | 


--------------------------------------------------------------------------------
/.travis.yml:
--------------------------------------------------------------------------------
 1 | 
 2 | # Use ubuntu trusty (14.04) with ros indigo on travis
 3 | 
 4 | dist: trusty
 5 | sudo: required
 6 | language:
 7 |   - generic
 8 | cache:
 9 |   - apt
10 | 
11 | env:
12 |   global:
13 |     - ROS_DISTRO=indigo
14 |     - CI_SOURCE_PATH=$(pwd)
15 | 
16 | before_install:
17 |   - lsb_release -a
18 |   - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list'
19 |   - sudo apt-key adv --keyserver hkp://ha.pool.sks-keyservers.net:80 --recv-key 421C365BD9FF1F717815A3895523BAEEB01FA116
20 |   - sudo apt-get update -qq
21 |   - sudo apt-get install ros-$ROS_DISTRO-ros-base
22 |   - sudo apt-get install ros-$ROS_DISTRO-move-base ros-$ROS_DISTRO-ompl
23 |   - sudo apt-get install python-catkin-pkg python-empy python-rospkg
24 |   - pip install catkin_pkg
25 |   - pip install rospkg
26 |   - sudo rosdep init
27 |   - rosdep update
28 | install:
29 |   # make a workspace and checkout the right repos:
30 |   - source /opt/ros/$ROS_DISTRO/setup.bash
31 |   - mkdir -p ~/catkin_ws/src
32 |   - cd ~/catkin_ws/src
33 |   - catkin_init_workspace
34 |   - cd ..
35 |   - catkin_make
36 |   - cd ~/catkin_ws/src
37 |   - ln -s $CI_SOURCE_PATH . # link repo under test.
38 | before_script:
39 |   - source ~/catkin_ws/devel/setup.bash
40 |   - cd ~/catkin_ws/src
41 | script:
42 |   - cd ~/catkin_ws
43 |   - catkin_make
44 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(move_base_ompl)
 3 | 
 4 | add_compile_options(-std=c++11)
 5 | 
 6 | find_package(catkin REQUIRED
 7 |   COMPONENTS
 8 |     costmap_2d
 9 |     geometry_msgs
10 |     nav_core
11 |     navfn
12 |     nav_msgs
13 |     pluginlib
14 |     roscpp
15 |     tf
16 |     base_local_planner
17 | )
18 | 
19 | find_package(OMPL REQUIRED)
20 | 
21 | catkin_package(
22 |   INCLUDE_DIRS include
23 |   LIBRARIES ompl_global_planner_lib
24 |   CATKIN_DEPENDS
25 |     costmap_2d
26 |     geometry_msgs
27 |     nav_core
28 |     navfn
29 |     nav_msgs
30 |     pluginlib
31 |     roscpp
32 |     tf
33 | )
34 | 
35 | include_directories(
36 |   include
37 |   ${catkin_INCLUDE_DIRS}
38 | )
39 | 
40 | add_library(ompl_global_planner_lib
41 |   src/ompl_global_planner.cpp
42 | )
43 | target_link_libraries(ompl_global_planner_lib ${catkin_LIBRARIES} ${OMPL_LIBRARIES})
44 | 
45 | add_dependencies(ompl_global_planner_lib ${PROJECT_NAME}_gencfg)
46 | 
47 | install(TARGETS ompl_global_planner_lib
48 |   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
49 |   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
50 |   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
51 | 
52 | install(DIRECTORY include/
53 |   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION})
54 | 
55 | install(FILES ompl_gp_plugin.xml
56 |   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
57 | 


--------------------------------------------------------------------------------
/include/ompl_global_planner.h:
--------------------------------------------------------------------------------
  1 | #ifndef _OMPL_GLOBAL_PLANNER_H
  2 | #define _OMPL_GLOBAL_PLANNER_H
  3 | /*********************************************************************
  4 |  *
  5 |  * Software License Agreement (BSD License)
  6 |  *
  7 |  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
  8 |  *  All rights reserved.
  9 |  *
 10 |  *  Redistribution and use in source and binary forms, with or without
 11 |  *  modification, are permitted provided that the following conditions
 12 |  *  are met:
 13 |  *
 14 |  *   * Redistributions of source code must retain the above copyright
 15 |  *     notice, this list of conditions and the following disclaimer.
 16 |  *   * Redistributions in binary form must reproduce the above
 17 |  *     copyright notice, this list of conditions and the following
 18 |  *     disclaimer in the documentation and/or other materials provided
 19 |  *     with the distribution.
 20 |  *   * Neither the name of Willow Garage, Inc. nor the names of its
 21 |  *     contributors may be used to endorse or promote products derived
 22 |  *     from this software without specific prior written permission.
 23 |  *
 24 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 25 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 26 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 27 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 28 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 29 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 30 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 31 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 32 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 33 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 34 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 35 |  *  POSSIBILITY OF SUCH DAMAGE.
 36 |  *
 37 |  * Author: Eitan Marder-Eppstein
 38 |  *         David V. Lu!!
 39 |  *********************************************************************/
 40 | 
 41 | /*
 42 |     Modified by: Windel Bouwman
 43 | */
 44 | 
 45 | #include <ros/ros.h>
 46 | #include <costmap_2d/costmap_2d.h>
 47 | #include <geometry_msgs/PoseStamped.h>
 48 | #include <geometry_msgs/Point.h>
 49 | #include <nav_msgs/Path.h>
 50 | #include <tf/transform_datatypes.h>
 51 | #include <nav_core/base_global_planner.h>
 52 | #include <nav_msgs/GetPlan.h>
 53 | #include <base_local_planner/costmap_model.h>
 54 | 
 55 | #include <ompl/base/spaces/SE2StateSpace.h>
 56 | #include <ompl/control/planners/rrt/RRT.h>
 57 | #include <ompl/geometric/planners/rrt/RRTstar.h>
 58 | #include <ompl/geometric/planners/prm/PRMstar.h>
 59 | #include <ompl/geometric/planners/prm/PRM.h>
 60 | #include <ompl/geometric/planners/rrt/RRTConnect.h>
 61 | #include <ompl/geometric/planners/rrt/TRRT.h>
 62 | #include <ompl/control/spaces/RealVectorControlSpace.h>
 63 | #include <ompl/control/SimpleSetup.h>
 64 | #include <ompl/base/objectives/StateCostIntegralObjective.h>
 65 | #include <ompl/base/objectives/MechanicalWorkOptimizationObjective.h>
 66 | #include <ompl/base/objectives/PathLengthOptimizationObjective.h>
 67 | 
 68 | namespace ob = ompl::base;
 69 | namespace oc = ompl::control;
 70 | namespace og = ompl::geometric;
 71 | 
 72 | namespace ompl_global_planner {
 73 | 
 74 | class OmplGlobalPlanner : public nav_core::BaseGlobalPlanner
 75 | {
 76 |     public:
 77 |         OmplGlobalPlanner();
 78 | 
 79 |         // Implemented functions for plugin:
 80 |         virtual void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
 81 |         virtual bool makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
 82 |                       std::vector<geometry_msgs::PoseStamped>& plan);
 83 | 
 84 |         void publishPlan(const std::vector<geometry_msgs::PoseStamped>& path);
 85 | 
 86 |         ~OmplGlobalPlanner() {
 87 |         }
 88 | 
 89 |         // Ompl related functions:
 90 |         void propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State *result);
 91 |         bool isStateValid(const oc::SpaceInformation *si, const ob::State *state);
 92 | 
 93 |         double calc_cost(const ob::State*);
 94 |         double motion_cost(const ob::State* s1, const ob::State* s2);
 95 |         void get_xy_theta_v(const ob::State* s, double& x, double& y, double& theta, double& velocity);
 96 |         void set_xy_theta_v(ob::State*, double x, double y, double theta, double velocity);
 97 | 
 98 |     private:
 99 |         costmap_2d::Costmap2DROS* _costmap_ros;
100 |         std::string _frame_id;
101 |         ros::Publisher _plan_pub;
102 |         bool _initialized;
103 |         bool _allow_unknown;
104 | 
105 |         std::string tf_prefix_;
106 |         boost::mutex _mutex;
107 |         base_local_planner::CostmapModel* _costmap_model;
108 | 
109 |         // State spaces:
110 |         ob::StateSpacePtr _se2_space;
111 |         ob::StateSpacePtr _velocity_space;
112 |         ob::StateSpacePtr _space;
113 | };
114 | 
115 | 
116 | class CostMapObjective : public ob::StateCostIntegralObjective
117 | {
118 |     public:
119 |     CostMapObjective(OmplGlobalPlanner& op, const ob::SpaceInformationPtr& si)
120 |         : ob::StateCostIntegralObjective(si, true),
121 |         _ompl_planner(op)
122 |     {
123 |     }
124 | 
125 |     virtual ob::Cost stateCost(const ob::State* s) const
126 |     {
127 |         return ob::Cost(_ompl_planner.calc_cost(s));
128 |     }
129 | 
130 | 
131 |     private:
132 |         OmplGlobalPlanner& _ompl_planner;
133 | };
134 | 
135 | 
136 | class CostMapWorkObjective : public ob::MechanicalWorkOptimizationObjective
137 | {
138 |     public:
139 |     CostMapWorkObjective(OmplGlobalPlanner& op, const ob::SpaceInformationPtr& si)
140 |         : ob::MechanicalWorkOptimizationObjective(si),
141 |         _ompl_planner(op)
142 |     {
143 |     }
144 | 
145 |     ob::Cost stateCost(const ob::State* s) const
146 |     {
147 |         return ob::Cost(_ompl_planner.calc_cost(s));
148 |     }
149 | 
150 |     /*
151 |     ob::Cost motionCost(const ob::State* s1, const ob::State* s2) const
152 |     {
153 |         return ob::Cost(_ompl_planner.motion_cost(s1, s2));
154 |     }
155 |     */
156 | 
157 |     private:
158 |         OmplGlobalPlanner& _ompl_planner;
159 | };
160 | 
161 | }
162 | 
163 | #endif
164 | 


--------------------------------------------------------------------------------
/src/ompl_global_planner.cpp:
--------------------------------------------------------------------------------
  1 | /*********************************************************************
  2 |  *
  3 |  * Software License Agreement (BSD License)
  4 |  *
  5 |  *  Copyright (c) 2008, 2013, Willow Garage, Inc.
  6 |  *  All rights reserved.
  7 |  *
  8 |  *  Redistribution and use in source and binary forms, with or without
  9 |  *  modification, are permitted provided that the following conditions
 10 |  *  are met:
 11 |  *
 12 |  *   * Redistributions of source code must retain the above copyright
 13 |  *     notice, this list of conditions and the following disclaimer.
 14 |  *   * Redistributions in binary form must reproduce the above
 15 |  *     copyright notice, this list of conditions and the following
 16 |  *     disclaimer in the documentation and/or other materials provided
 17 |  *     with the distribution.
 18 |  *   * Neither the name of Willow Garage, Inc. nor the names of its
 19 |  *     contributors may be used to endorse or promote products derived
 20 |  *     from this software without specific prior written permission.
 21 |  *
 22 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 23 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 24 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 25 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 26 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 27 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 28 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 29 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 30 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 31 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 32 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 33 |  *  POSSIBILITY OF SUCH DAMAGE.
 34 |  *
 35 |  * Author: Eitan Marder-Eppstein
 36 |  *         David V. Lu!!
 37 |  * Modificator: Windel Bouwman
 38 |  *********************************************************************/
 39 | 
 40 | /*
 41 | 
 42 |     Greatly copied from:
 43 | 
 44 |     http://ompl.kavrakilab.org/RigidBodyPlanningWithControls_8cpp_source.html
 45 | 
 46 | */
 47 | 
 48 | #include <ompl_global_planner.h>
 49 | #include <pluginlib/class_list_macros.h>
 50 | #include <tf/transform_listener.h>
 51 | 
 52 | 
 53 | //register this planner as a BaseGlobalPlanner plugin
 54 | PLUGINLIB_EXPORT_CLASS(ompl_global_planner::OmplGlobalPlanner, nav_core::BaseGlobalPlanner)
 55 | 
 56 | namespace ompl_global_planner
 57 | {
 58 | 
 59 | 
 60 | OmplGlobalPlanner::OmplGlobalPlanner() :
 61 |         _costmap_ros(NULL), _initialized(false), _allow_unknown(true),
 62 |         _se2_space(new ob::SE2StateSpace()),
 63 |         _velocity_space(new ob::RealVectorStateSpace(1)),
 64 |         _space(_se2_space + _velocity_space),
 65 |         _costmap_model(NULL)
 66 | {
 67 | }
 68 | 
 69 | void OmplGlobalPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
 70 | {
 71 |     if (!_initialized)
 72 |     {
 73 |         ros::NodeHandle private_nh("~/" + name);
 74 |         _costmap_ros = costmap_ros;
 75 |         _frame_id = "map";
 76 |         _costmap_model = new base_local_planner::CostmapModel(*_costmap_ros->getCostmap());
 77 | 
 78 |         _plan_pub = private_nh.advertise<nav_msgs::Path>("plan", 1);
 79 |         private_nh.param("allow_unknown", _allow_unknown, true);
 80 | 
 81 |         //get the tf prefix
 82 |         ros::NodeHandle prefix_nh;
 83 |         tf_prefix_ = tf::getPrefixParam(prefix_nh);
 84 | 
 85 |         _initialized = true;
 86 |         ROS_INFO("Ompl global planner initialized!");
 87 |     }
 88 |     else
 89 |     {
 90 |         ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing");
 91 |     }
 92 | 
 93 | }
 94 | 
 95 | void OmplGlobalPlanner::get_xy_theta_v(const ob::State *s, double& x, double& y, double& theta, double& v)
 96 | {
 97 |     const ob::CompoundStateSpace::StateType* compound_state = s->as<ob::CompoundStateSpace::StateType>();
 98 |     const ob::SE2StateSpace::StateType* se2state = compound_state->as<ob::SE2StateSpace::StateType>(0);
 99 |     const ob::RealVectorStateSpace::StateType* v_state = compound_state->as<ob::RealVectorStateSpace::StateType>(1);
100 | 
101 |     // Get the values:
102 |     x = se2state->getX();
103 |     y = se2state->getY();
104 |     theta = se2state->getYaw();
105 |     v = (*v_state)[0];
106 | }
107 | 
108 | // Store x,y and theta into state:
109 | void OmplGlobalPlanner::set_xy_theta_v(ob::State* rs, double x, double y, double theta, double v)
110 | {
111 |     ob::CompoundStateSpace::StateType* compound_state = rs->as<ob::CompoundStateSpace::StateType>();
112 |     ob::SE2StateSpace::StateType* se2state = compound_state->as<ob::SE2StateSpace::StateType>(0);
113 |     ob::RealVectorStateSpace::StateType* v_state = compound_state->as<ob::RealVectorStateSpace::StateType>(1);
114 | 
115 |     // Set values:
116 |     se2state->setX(x);
117 |     se2state->setY(y);
118 |     se2state->setYaw(theta);
119 |     (*v_state)[0] = v;
120 | 
121 |     // Make sure angle is (-pi,pi]:
122 |     const ob::SO2StateSpace* SO2 = _se2_space->as<ob::SE2StateSpace>()->as<ob::SO2StateSpace>(1);
123 |     ob::SO2StateSpace::StateType* so2 = se2state->as<ob::SO2StateSpace::StateType>(1);
124 |     SO2->enforceBounds(so2);
125 | }
126 | 
127 | double OmplGlobalPlanner::calc_cost(const ob::State *state)
128 | {
129 |     double x, y, theta, velocity;
130 |     get_xy_theta_v(state, x, y, theta, velocity);
131 | 
132 |     // Get the cost of the footprint at the current location:
133 |     double cost = _costmap_model->footprintCost(x, y, theta, _costmap_ros->getRobotFootprint());
134 | 
135 |     if (cost < 0)
136 |     {
137 |         // Unknown cell, assume zero cost here!
138 |         cost = 0;
139 |     }
140 | 
141 |     return cost;
142 | }
143 | 
144 | // Calculate the cost of a motion:
145 | double motion_cost(const ob::State* s1, const ob::State* s2)
146 | {
147 |     // int nd = validSegmentCount(s1, s2);
148 |     // TODO: interpolate?
149 |     double cst = 0;
150 | 
151 |     // cst = 
152 | 
153 |     return cst;
154 | }
155 | 
156 | // Check the current state:
157 | bool OmplGlobalPlanner::isStateValid(const oc::SpaceInformation *si, const ob::State *state)
158 | {
159 |     if (!si->satisfiesBounds(state))
160 |     {
161 |         return false;
162 |     }
163 | 
164 |     // Get the cost of the footprint at the current location:
165 |     double cost = calc_cost(state);
166 | 
167 |     // std::cout << cost << std::endl;
168 |     // Too high cost:
169 |     if (cost > 90)
170 |     {
171 |         return false;
172 |     }
173 | 
174 |     // Error? Unknown space?
175 |     if (cost < 0)
176 |     {
177 |     }
178 | 
179 |     return true;
180 | }
181 | 
182 | // Calculate vehicle dynamics, assume a velocity state, but steering to be instantly possibe.
183 | void OmplGlobalPlanner::propagate(const ob::State *start, const oc::Control *control, const double duration, ob::State* result)
184 | {
185 |     // Implement vehicle dynamics:
186 |     double x, y, theta, velocity;
187 |     get_xy_theta_v(start, x, y, theta, velocity);
188 | 
189 |     const double* ctrl = control->as<oc::RealVectorControlSpace::ControlType>()->values;
190 |     double acc = ctrl[0];
191 |     double steer_angle = ctrl[1];
192 | 
193 |     // Calculate vehicle dynamics:
194 |     double x_n, y_n, theta_n, velocity_n;
195 |     // TODO: elaborate further..
196 |     x_n = x + velocity * duration * cos(theta);
197 |     y_n = y + velocity * duration * sin(theta);
198 |     velocity_n = velocity + acc * duration;
199 | 
200 |     double vehicle_length = 0.4;
201 |     double lengthInv = 1 / vehicle_length;
202 |     double omega = velocity * lengthInv * tan(steer_angle);
203 |     theta_n = theta + omega * duration;
204 | 
205 |     // Store new state in result:
206 |     set_xy_theta_v(result, x_n, y_n, theta_n, velocity_n);
207 | }
208 | 
209 | double calcYaw(const geometry_msgs::Pose pose)
210 | {
211 |     double yaw, pitch, roll;
212 |     tf::Pose pose_tf;
213 |     tf::poseMsgToTF(pose, pose_tf);
214 |     pose_tf.getBasis().getEulerYPR(yaw, pitch, roll);
215 |     return yaw;
216 | }
217 | 
218 | void pose2state(const geometry_msgs::Pose& pose, ob::State* state)
219 | {
220 |     //ompl_start[0] = start.pose.position.x;
221 |     //ompl_start[1] = start.pose.position.y;
222 |     //ompl_start[2] = calcYaw(start.pose);
223 | }
224 | 
225 | bool OmplGlobalPlanner::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal,
226 |                            std::vector<geometry_msgs::PoseStamped>& plan)
227 | {
228 |     boost::mutex::scoped_lock lock(_mutex);
229 |     if (!_initialized)
230 |     {
231 |         ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use");
232 |         return false;
233 |     }
234 | 
235 |     //clear the plan, just in case
236 |     plan.clear();
237 | 
238 |     ros::NodeHandle n;
239 |     std::string global_frame = _frame_id;
240 | 
241 |     //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame
242 |     if (tf::resolve(tf_prefix_, goal.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
243 |         ROS_ERROR(
244 |                 "The goal pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, goal.header.frame_id).c_str());
245 |         return false;
246 |     }
247 | 
248 |     if (tf::resolve(tf_prefix_, start.header.frame_id) != tf::resolve(tf_prefix_, global_frame)) {
249 |         ROS_ERROR(
250 |                 "The start pose passed to this planner must be in the %s frame.  It is instead in the %s frame.", tf::resolve(tf_prefix_, global_frame).c_str(), tf::resolve(tf_prefix_, start.header.frame_id).c_str());
251 |         return false;
252 |     }
253 | 
254 |     double wx = start.pose.position.x;
255 |     double wy = start.pose.position.y;
256 | 
257 |     //clear the starting cell within the costmap because we know it can't be an obstacle
258 |     tf::Stamped<tf::Pose> start_pose;
259 |     tf::poseStampedMsgToTF(start, start_pose);
260 | 
261 |     ROS_INFO("Thinking about OMPL path..");
262 |     // Create OMPL problem:
263 |     ob::RealVectorBounds bounds(2);
264 |     bounds.setLow(-10);
265 |     bounds.setHigh(10);
266 |     _se2_space->as<ob::SE2StateSpace>()->setBounds(bounds);
267 | 
268 |     ob::RealVectorBounds velocity_bounds(1);
269 |     velocity_bounds.setHigh(0.6);
270 |     velocity_bounds.setLow(-0.1);
271 |     _velocity_space->as<ob::RealVectorStateSpace>()->setBounds(velocity_bounds);
272 | 
273 |     oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(_space, 2));
274 |     ob::RealVectorBounds cbounds(2);
275 |     cbounds.setLow(0, -0.04);
276 |     cbounds.setHigh(0, 0.3);
277 |     cbounds.setLow(1, -0.2);
278 |     cbounds.setHigh(1, 0.2);
279 |     cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
280 | 
281 |     // Create space information:
282 |     oc::SpaceInformationPtr si(new oc::SpaceInformation(_space, cspace));
283 |     si->setStatePropagator(boost::bind(&OmplGlobalPlanner::propagate, this, _1, _2, _3,_4));
284 |     si->setStateValidityChecker(boost::bind(&OmplGlobalPlanner::isStateValid, this, si.get(), _1));
285 | 
286 |     // Define problem:
287 |     ob::ScopedState<> ompl_start(_space);
288 |     ompl_start[0] = start.pose.position.x;
289 |     ompl_start[1] = start.pose.position.y;
290 |     ompl_start[2] = calcYaw(start.pose);
291 |     ompl_start[3] = 0; // Speed
292 | 
293 |     ob::ScopedState<> ompl_goal(_space);
294 |     ompl_goal[0] = goal.pose.position.x;
295 |     ompl_goal[1] = goal.pose.position.y;
296 |     ompl_goal[2] = calcYaw(start.pose);
297 |     ompl_goal[3] = 0; // Speed
298 | 
299 |     // Optimize criteria:
300 |     ob::OptimizationObjectivePtr cost_objective(new CostMapObjective(*this, si));
301 |     ob::OptimizationObjectivePtr length_objective(new ob::PathLengthOptimizationObjective(si));
302 |     //ob::OptimizationObjectivePtr objective(new CostMapWorkObjective(*this, si));
303 | 
304 |     ob::ProblemDefinitionPtr pdef(new ob::ProblemDefinition(si));
305 |     pdef->setStartAndGoalStates(ompl_start, ompl_goal, 0.1);
306 |     pdef->setOptimizationObjective(cost_objective + length_objective);
307 | 
308 |     ROS_INFO("Problem defined, running planner");
309 |     // oc::DecompositionPtr decomp(new My
310 |     // ob::PlannerPtr planner(new oc::LBTRRT(si));
311 |     // ob::PlannerPtr planner(new og::RRTConnect(si));
312 |     ob::PlannerPtr planner(new og::RRTstar(si));
313 |     // ob::PlannerPtr planner(new og::PRMstar(si)); // works
314 |     // ob::PlannerPtr planner(new og::PRM(si)); // segfault
315 |     // ob::PlannerPtr planner(new og::TRRT(si));
316 |     planner->setProblemDefinition(pdef);
317 |     planner->setup();
318 |     ob::PlannerStatus solved = planner->solve(3.0);
319 | 
320 | 
321 |     // Convert path into ROS messages:
322 |     if (solved)
323 |     {
324 |         ROS_INFO("Ompl done!");
325 |         ob::PathPtr result_path1 = pdef->getSolutionPath();
326 | 
327 |         // Cast path into geometric path:
328 |         og::PathGeometric& result_path = static_cast<og::PathGeometric&>(*result_path1);
329 | 
330 |         // result_path.interpolate(100);
331 |         // result_path->print(std::cout);
332 | 
333 |         // Create path:
334 |         plan.push_back(start);
335 | 
336 |         // Conversion loop from states to messages:
337 |         std::vector<ob::State*>& result_states = result_path.getStates();
338 |         for (std::vector<ob::State*>::iterator it = result_states.begin(); it != result_states.end(); ++it)
339 |         {
340 |             // Get the data from the state:
341 |             double x, y, theta, velocity;
342 |             get_xy_theta_v(*it, x, y, theta, velocity);
343 | 
344 |             // Place data into the pose:
345 |             geometry_msgs::PoseStamped ps = goal;
346 |             ps.header.stamp = ros::Time::now();
347 |             ps.pose.position.x = x;
348 |             ps.pose.position.y = y;
349 |             plan.push_back(ps);
350 |         }
351 | 
352 |         plan.push_back(goal);
353 |     }
354 |     else
355 |     {
356 |         ROS_ERROR("Failed to determine plan");
357 |     }
358 | 
359 | 
360 |     //publish the plan for visualization purposes
361 |     publishPlan(plan);
362 |     return !plan.empty();
363 | }
364 | 
365 | void OmplGlobalPlanner::publishPlan(const std::vector<geometry_msgs::PoseStamped>& path)
366 | {
367 |     if (!_initialized) {
368 |         ROS_ERROR(
369 |                 "This planner has not been initialized yet, but it is being used, please call initialize() before use");
370 |         return;
371 |     }
372 | 
373 |     //create a message for the plan 
374 |     nav_msgs::Path gui_path;
375 |     gui_path.poses.resize(path.size());
376 | 
377 |     if (!path.empty()) {
378 |         gui_path.header.frame_id = path[0].header.frame_id;
379 |         gui_path.header.stamp = path[0].header.stamp;
380 |     }
381 | 
382 |     // Extract the plan in world co-ordinates, we assume the path is all in the same frame
383 |     for (unsigned int i = 0; i < path.size(); i++) {
384 |         gui_path.poses[i] = path[i];
385 |     }
386 | 
387 |     _plan_pub.publish(gui_path);
388 | }
389 | 
390 | 
391 | } //end namespace global_planner
392 | 
393 | 


--------------------------------------------------------------------------------