eband_local_planner.h
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2010, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Christian Connette
00036 *********************************************************************/
00037 
00038 #ifndef EBAND_LOCAL_PLANNER_H_
00039 #define EBAND_LOCAL_PLANNER_H_
00040 
00041 //#define DEBUG_EBAND_ // uncomment to enable additional publishing, visualization and logouts for debug
00042 
00043 #include <ros/ros.h>
00044 #include <ros/assert.h>
00045 
00046 // classes which are part of this package
00047 #include <eband_local_planner/conversions_and_types.h>
00048 #include <eband_local_planner/eband_visualization.h>
00049 
00050 // local planner specific classes
00051 #include <base_local_planner/costmap_model.h>
00052 
00053 // msgs
00054 #include <nav_msgs/Path.h>
00055 #include <nav_msgs/Odometry.h>
00056 #include <geometry_msgs/PoseStamped.h>
00057 #include <geometry_msgs/Pose.h>
00058 #include <geometry_msgs/Pose2D.h>
00059 #include <geometry_msgs/Twist.h>
00060 #include <geometry_msgs/WrenchStamped.h>
00061 
00062 // transforms
00063 #include <angles/angles.h>
00064 #include <tf/tf.h>
00065 
00066 // costmap & geometry
00067 #include <costmap_2d/costmap_2d_ros.h>
00068 
00069 // boost classes
00070 #include <boost/shared_ptr.hpp>
00071 
00072 namespace eband_local_planner{
00073 
00078 class EBandPlanner{
00079 
00080         public:
00084                 EBandPlanner();
00085 
00091                 EBandPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
00092 
00096                 ~EBandPlanner();
00097 
00103                 void initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros);
00104 
00109                 void setVisualization(boost::shared_ptr<EBandVisualization> eband_visual);
00110 
00116                 bool setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan);
00117 
00123                 bool getPlan(std::vector<geometry_msgs::PoseStamped>& global_plan);
00124 
00130                 bool getBand(std::vector<Bubble>& elastic_band);
00131 
00137                 bool addFrames(const std::vector<geometry_msgs::PoseStamped>& robot_pose, const AddAtPosition& add_frames_at);
00138 
00143                 bool optimizeBand();
00144 
00150                 bool optimizeBand(std::vector<Bubble>& band);
00151 
00152         private:
00153                 // pointer to external objects (do NOT delete object)
00154                 costmap_2d::Costmap2DROS* costmap_ros_; 
00155 
00156                 // parameters
00157                 std::vector<double> acc_lim_; 
00158                 int num_optim_iterations_; 
00159                 double internal_force_gain_; 
00160                 double external_force_gain_; 
00161                 double tiny_bubble_distance_; 
00162                 double tiny_bubble_expansion_; 
00163                 double min_bubble_overlap_; 
00164                 int max_recursion_depth_approx_equi_; 
00165                 double equilibrium_relative_overshoot_; 
00166                 double significant_force_; 
00167 
00168                 // pointer to locally created objects (delete - except for smart-ptrs:)
00169                 base_local_planner::CostmapModel* world_model_; // local world model
00170                 boost::shared_ptr<EBandVisualization> eband_visual_; // pointer to visualization object
00171 
00172                 // flags
00173                 bool initialized_, visualization_;
00174 
00175                 // data
00176                 std::vector<geometry_msgs::Point> footprint_spec_; // specification of robot footprint as vector of corner points
00177                 costmap_2d::Costmap2D costmap_; // local copy of costmap
00178                 std::vector<geometry_msgs::PoseStamped> global_plan_; // copy of the plan that shall be optimized
00179                 std::vector<Bubble> elastic_band_;
00180 
00181 
00182                 // methods
00183 
00184                 // band refinement (filling of gaps & removing redundant bubbles)
00185 
00191                 bool refineBand(std::vector<Bubble>& band);
00192 
00193 
00201                 bool removeAndFill(std::vector<Bubble>& band, std::vector<Bubble>::iterator& start_iter,std::vector<Bubble>::iterator& end_iter);
00202 
00203 
00211                 bool fillGap(std::vector<Bubble>& band, std::vector<Bubble>::iterator& start_iter,std::vector<Bubble>::iterator& end_iter);
00212 
00213 
00214                 // optimization
00215 
00221                 bool modifyBandArtificialForce(std::vector<Bubble>& band);
00222 
00223 
00231                 bool applyForces(int bubble_num, std::vector<Bubble>& band, std::vector<geometry_msgs::WrenchStamped> forces);
00232 
00233 
00243                 bool moveApproximateEquilibrium(const int& bubble_num, const std::vector<Bubble>& band, Bubble& curr_bubble,
00244                                                                                 const geometry_msgs::WrenchStamped& curr_bubble_force, geometry_msgs::Twist& curr_step_width,
00245                                                                                 const int& curr_recursion_depth);
00246 
00247 
00248                 // problem (geometry) dependant functions
00249 
00257                 bool interpolateBubbles(geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped& interpolated_center);
00258 
00259 
00267                 bool checkOverlap(Bubble bubble1, Bubble bubble2);
00268 
00269 
00277                 bool calcBubbleDistance(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double& distance);
00278 
00279 
00287                 bool calcBubbleDifference(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist& difference);
00288 
00289 
00296                 bool calcObstacleKinematicDistance(geometry_msgs::Pose center_pose, double& distance);
00297 
00298 
00307                 bool getForcesAt(int bubble_num, std::vector<Bubble> band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces);
00308 
00309 
00318                 bool calcInternalForces(int bubble_num, std::vector<Bubble> band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces);
00319 
00320 
00328                 bool calcExternalForces(int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces);
00329 
00330 
00337                 bool suppressTangentialForces(int bubble_num, std::vector<Bubble> band, geometry_msgs::WrenchStamped& forces);
00338 
00339 
00340                 // type conversion
00341 
00348                 bool convertPlanToBand(std::vector<geometry_msgs::PoseStamped> plan, std::vector<Bubble>& band);
00349 
00350 
00357                 bool convertBandToPlan(std::vector<geometry_msgs::PoseStamped>& plan, std::vector<Bubble> band);
00358 
00359 };
00360 };
00361 #endif


eband_local_planner
Author(s): Christian Connette, Bhaskara Marthi
autogenerated on Fri Jan 3 2014 11:34:16