eband_local_planner.cpp
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 #include <eband_local_planner/eband_local_planner.h>
00039 
00040 
00041 namespace eband_local_planner{
00042 
00043 
00044 EBandPlanner::EBandPlanner() : costmap_ros_(NULL), initialized_(false) {}
00045 
00046 
00047 EBandPlanner::EBandPlanner(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00048  : costmap_ros_(NULL), initialized_(false)
00049 {
00050         // initialize planner
00051         initialize(name, costmap_ros);
00052 }
00053 
00054 
00055 EBandPlanner::~EBandPlanner()
00056 {
00057         delete world_model_;
00058 }
00059 
00060 
00061 void EBandPlanner::initialize(std::string name, costmap_2d::Costmap2DROS* costmap_ros)
00062 {
00063         // check if the plugin is already initialized
00064         if(!initialized_)
00065         {
00066                 // copy adress of costmap (handed over from move_base via eband wrapper)
00067                 costmap_ros_ = costmap_ros;
00068 
00069                 // create a local copy of the costmap
00070                 costmap_ros_->getCostmapCopy(costmap_);
00071 
00072                 // create world model from costmap
00073                 world_model_ = new base_local_planner::CostmapModel(costmap_);
00074 
00075                 // get footprint of the robot
00076                 footprint_spec_ = costmap_ros_->getRobotFootprint();
00077 
00078 
00079                 // create Node Handle with name of plugin (as used in move_base for loading)
00080                 ros::NodeHandle pn("~/" + name);
00081 
00082                 // read parameters from parameter server
00083                 // connectivity checking
00084                 pn.param("eband_min_relative_bubble_overlap_", min_bubble_overlap_, 0.7);
00085 
00086                 // bubble geometric bounds
00087                 pn.param("eband_tiny_bubble_distance", tiny_bubble_distance_, 0.01);
00088                 pn.param("eband_tiny_bubble_expansion", tiny_bubble_expansion_, 0.01);
00089 
00090                 // optimization - force calculation
00091                 pn.param("eband_internal_force_gain", internal_force_gain_, 1.0);
00092                 pn.param("eband_external_force_gain", external_force_gain_, 2.0);
00093                 pn.param("num_iterations_eband_optimization", num_optim_iterations_, 3);
00094 
00095                 // recursive approximation of bubble equilibrium position based
00096                 pn.param("eband_equilibrium_approx_max_recursion_depth", max_recursion_depth_approx_equi_, 4);
00097                 pn.param("eband_equilibrium_relative_overshoot", equilibrium_relative_overshoot_, 0.75);
00098                 pn.param("eband_significant_force_lower_bound", significant_force_, 0.15);
00099 
00100 
00101                 // clean up band
00102                 elastic_band_.clear();
00103 
00104                 // set initialized flag
00105                 initialized_ = true;
00106 
00107                 // set flag whether visualization availlable to false by default
00108                 visualization_ = false;
00109         }
00110         else
00111         {
00112                 ROS_WARN("This planner has already been initialized, doing nothing.");
00113         }
00114 }
00115 
00116 void EBandPlanner::setVisualization(boost::shared_ptr<EBandVisualization> eband_visual)
00117 {
00118         eband_visual_ = eband_visual;
00119 
00120         visualization_ = true;
00121 }
00122 
00123 
00124 bool EBandPlanner::setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan)
00125 {
00126         // check if plugin initialized
00127         if(!initialized_)
00128         {
00129                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00130                 return false;
00131         }
00132         
00133         
00134         // check if plan valid (minimum 2 frames)
00135         if(global_plan.size() < 2)
00136         {
00137                 ROS_ERROR("Attempt to pass empty path to optimization. Valid path needs to have at least 2 Frames. This one has %d.", ((int) global_plan.size()) );
00138                 return false;
00139         }
00140         // copy plan to local member variable
00141         global_plan_ = global_plan;
00142 
00143 
00144         // also get an up to date copy of the global costmap (needed for collision checks during band creation and optimization)
00145         costmap_ros_->getCostmapCopy(costmap_);
00146 
00147         // check whether plan and costmap are in the same frame
00148         if(global_plan.front().header.frame_id != costmap_ros_->getGlobalFrameID())
00149         {
00150       ROS_ERROR("Elastic Band expects plan for optimization in the %s frame, the plan was sent in the %s frame.", 
00151           costmap_ros_->getGlobalFrameID().c_str(), global_plan.front().header.frame_id.c_str());
00152       return false;
00153     }
00154         
00155 
00156         // convert frames in path into bubbles in band -> sets center of bubbles and calculates expansion
00157         ROS_DEBUG("Converting Plan to Band");
00158         if(!convertPlanToBand(global_plan_, elastic_band_))
00159         {
00160                 ROS_WARN("Conversion from plan to elastic band failed. Plan probably not collision free. Plan not set for optimization");
00161                 // TODO try to do local repairs of band
00162                 return false;
00163         }
00164 
00165 
00166         // close gaps and remove redundant bubbles
00167         ROS_DEBUG("Refining Band");
00168         if(!refineBand(elastic_band_))
00169         {
00170                 ROS_WARN("Band is broken. Could not close gaps in converted path. Path not set. Global replanning needed");
00171                 return false;
00172         }
00173 
00174 
00175         ROS_DEBUG("Refinement done - Band set.");
00176         return true;
00177 }
00178 
00179 
00180 bool EBandPlanner::getPlan(std::vector<geometry_msgs::PoseStamped>& global_plan)
00181 {
00182         // check if plugin initialized
00183         if(!initialized_)
00184         {
00185                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00186                 return false;
00187         }
00188 
00189         // check if there is a band
00190         if(elastic_band_.empty())
00191         {
00192                 ROS_WARN("Band is empty. There was no path successfully set so far.");
00193                 return false;
00194         }
00195 
00196         // convert band to plan
00197         if(!convertBandToPlan(global_plan, elastic_band_))
00198         {
00199                 ROS_WARN("Conversion from Elastic Band to path failed.");
00200                 return false;
00201         }
00202 
00203         return true;
00204 }
00205 
00206 
00207 bool EBandPlanner::getBand(std::vector<Bubble>& elastic_band)
00208 {
00209         // check if plugin initialized
00210         if(!initialized_)
00211         {
00212                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00213                 return false;
00214         }
00215 
00216         elastic_band = elastic_band_;
00217 
00218         // check if there is a band
00219         if(elastic_band_.empty())
00220         {
00221                 ROS_WARN("Band is empty.");
00222                 return false;
00223         }
00224 
00225         return true;
00226 }
00227 
00228 
00229 bool EBandPlanner::addFrames(const std::vector<geometry_msgs::PoseStamped>& plan_to_add, const AddAtPosition& add_frames_at)
00230 {
00231         // check if plugin initialized
00232         if(!initialized_)
00233         {
00234                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00235                 return false;
00236         }
00237 
00238         // check that there is a plan at all (minimum 1 frame in this case, as robot + goal = plan)
00239         if(elastic_band_.size() < 1)
00240         {
00241                 ROS_WARN("Attempt to connect path to empty band. path not connected. Use SetPath instead");
00242                 return false;
00243         }
00244 
00245         //check that plan which shall be added is not empty
00246         if(plan_to_add.empty())
00247         {
00248                 ROS_WARN("Attempt to connect empty path to band. Nothing to do here.");
00249                 return false;
00250         }
00251 
00252 
00253         // also get an up to date copy of the global costmap (needed for collision checks during band creation and optimization)
00254         costmap_ros_->getCostmapCopy(costmap_);
00255 
00256         // check whether plan and costmap are in the same frame
00257         if(plan_to_add.at(0).header.frame_id != costmap_ros_->getGlobalFrameID())
00258         {
00259       ROS_ERROR("Elastic Band expects robot pose for optimization in the %s frame, the pose was sent in the %s frame.", 
00260           costmap_ros_->getGlobalFrameID().c_str(), plan_to_add.at(0).header.frame_id.c_str());
00261       return false;
00262     }
00263 
00264 
00265         // convert plan to band
00266         std::vector<Bubble> band_to_add;
00267         if(!convertPlanToBand(plan_to_add, band_to_add))
00268         {
00269                 ROS_DEBUG("Conversion from plan to elastic band failed. Plan not appended");
00270                 // TODO try to do local repairs of band
00271                 return false;
00272         }
00273 
00274 
00275         // connect frames to existing band
00276         ROS_DEBUG("Checking for connections between current band and new bubbles");
00277         bool connected = false;
00278         int bubble_connect = -1;
00279         if(add_frames_at == add_front)
00280         {
00281                 // add frames at the front of the current band
00282                 // - for instance to connect band and current robot position
00283                 for(int i = ((int) elastic_band_.size() - 1); i >= 0; i--)
00284                 {
00285                         // cycle over bubbles from End - connect to bubble furthest away but overlapping        
00286                         if(checkOverlap(band_to_add.back(), elastic_band_.at(i)))
00287                         {
00288                                 bubble_connect = i;
00289                                 connected = true;
00290                                 break;
00291                         }
00292                 }
00293         }
00294         else
00295         {
00296                 // add frames at the end of the current band
00297                 // - for instance to connect new frames entering the moving window
00298                 for(int i = 0; i < ((int) elastic_band_.size() - 1); i++)
00299                 {
00300                         // cycle over bubbles from Start - connect to bubble furthest away but overlapping
00301                         if(checkOverlap(band_to_add.front(), elastic_band_.at(i)))
00302                         {
00303                                 bubble_connect = i;
00304                                 connected = true;
00305                                 break;
00306                         }
00307                 }
00308         }
00309 
00310         // intanstiate local copy of band
00311         std::vector<Bubble> tmp_band;
00312         std::vector<Bubble>::iterator tmp_iter1, tmp_iter2;
00313         // copy new frames to tmp_band
00314         tmp_band.assign(band_to_add.begin(), band_to_add.end());
00315 
00316         if(connected)
00317         {
00318                 ROS_DEBUG("Connections found - composing new band by connecting new frames to bubble %d", bubble_connect);
00319                 if(add_frames_at == add_front)
00320                 {
00321                         // compose new vector by appending elastic_band to new frames
00322                         tmp_iter1 = elastic_band_.begin() + bubble_connect;
00323                         ROS_ASSERT( (tmp_iter1 >= elastic_band_.begin()) && (tmp_iter1 < elastic_band_.end()) );
00324                         tmp_band.insert(tmp_band.end(), tmp_iter1, elastic_band_.end());
00325                 }
00326                 else
00327                 {
00328                         // compose new vector by pre-appending elastic_band to new frames
00329                         tmp_iter1 = elastic_band_.begin() + bubble_connect + 1; // +1 - as insert only appends [start, end)
00330                         ROS_ASSERT( (tmp_iter1 > elastic_band_.begin()) && (tmp_iter1 <= elastic_band_.end()) );
00331                         tmp_band.insert(tmp_band.begin(), elastic_band_.begin(), tmp_iter1);
00332                 }
00333 
00334                 // done
00335                 elastic_band_ = tmp_band;
00336                 return true;
00337         }
00338 
00339         // otherwise, we need to do some more work - add complete band to tmp_band
00340         ROS_DEBUG("No direct connection found - Composing tmp band and trying to fill gap");
00341         if(add_frames_at == add_front)
00342         {
00343                 // compose new vector by appending elastic_band to new frames
00344                 tmp_band.insert(tmp_band.end(), elastic_band_.begin(), elastic_band_.end());
00345                 // and get iterators to connecting bubbles
00346                 tmp_iter1 = tmp_band.begin() + ((int) band_to_add.size()) - 1;
00347                 tmp_iter2 = tmp_iter1 + 1;
00348         }
00349         else
00350         {
00351                 // compose new vector by pre-appending elastic_band to new frames
00352                 tmp_band.insert(tmp_band.begin(), elastic_band_.begin(), elastic_band_.end());
00353                 // and get iterators to connecting bubbles
00354                 tmp_iter1 = tmp_band.begin() + ((int) elastic_band_.size()) - 1;
00355                 tmp_iter2 = tmp_iter1 + 1;
00356         }
00357 
00358         // just in case
00359         ROS_ASSERT( tmp_iter1 >= tmp_band.begin() );
00360         ROS_ASSERT( tmp_iter2 < tmp_band.end() );
00361         ROS_ASSERT( tmp_iter1 < tmp_iter2 );
00362         if(!fillGap(tmp_band, tmp_iter1, tmp_iter2))
00363         {
00364                 // we could not connect band and robot at its current position
00365                 ROS_DEBUG("Could not connect robot pose to band - Failed to fill gap.");
00366                 return false;
00367         }
00368         
00369         // otherwise - done
00370         elastic_band_ = tmp_band;
00371 
00372         return true;
00373 }
00374 
00375 
00376 bool EBandPlanner::optimizeBand()
00377 {
00378         // check if plugin initialized
00379         if(!initialized_)
00380         {
00381                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00382                 return false;
00383         }
00384 
00385         // check if there is a band
00386         if(elastic_band_.empty())
00387         {
00388                 ROS_ERROR("Band is empty. Probably Band has not been set yet");
00389                 return false;
00390         }
00391 
00392         // call optimization with member elastic_band_
00393         ROS_DEBUG("Starting optimization of band");
00394         if(!optimizeBand(elastic_band_))
00395         {
00396                 ROS_DEBUG("Aborting Optimization. Changes discarded.");
00397                 return false;
00398         }
00399 
00400         ROS_DEBUG("Elastic Band - Optimization successfull!");
00401         return true;
00402 }
00403 
00404 
00405 bool EBandPlanner::optimizeBand(std::vector<Bubble>& band)
00406 {
00407         // check if plugin initialized
00408         if(!initialized_)
00409         {
00410                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00411                 return false;
00412         }
00413 
00414         // again get an up to date copy of the global costmap (needed for collision checks during band creation and optimization)
00415         costmap_ros_->getCostmapCopy(costmap_);
00416 
00417         // check whether band and costmap are in the same frame
00418         if(band.front().center.header.frame_id != costmap_ros_->getGlobalFrameID())
00419         {
00420       ROS_ERROR("Elastic Band expects plan for optimization in the %s frame, the plan was sent in the %s frame.", 
00421           costmap_ros_->getGlobalFrameID().c_str(), band.front().center.header.frame_id.c_str());
00422       return false;
00423     }
00424 
00425         double distance;
00426         for(int i = 0; i < ((int) band.size()); i++)
00427         {
00428                 // update Size of Bubbles in band by calculating Dist to nearest Obstacle [depends kinematic, environment]
00429                 if(!calcObstacleKinematicDistance(band.at(i).center.pose, distance))
00430                 {
00431                         ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d Probably outside map coordinates.",
00432                                                         i, ((int) band.size()) );
00433                         return false;
00434                 }
00435 
00436                 if(distance == 0.0)
00437                 {
00438                         // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
00439                         ROS_DEBUG("Optimization (Elastic Band) - Calculation of Distance failed. Frame %d of %d in collision. Plan invalid. Trying to refine band.",
00440                                                 i, ((int) band.size()) );
00441                         // TODO if frame in collision try to repair band instead of aborting everything
00442                         return false;
00443                 }
00444         
00445                 band.at(i).expansion = distance;
00446         }
00447 
00448         // close gaps and remove redundant bubbles
00449         if(!refineBand(band))
00450         {
00451                 ROS_DEBUG("Elastic Band is broken. Could not close gaps in band. Global replanning needed.");
00452                 return false;
00453         }
00454 
00455         // get a copy of current (valid) band
00456         std::vector<Bubble> tmp_band = band;
00457 
00458         // now optimize iteratively (for instance by miminizing the energy-function of the full system)
00459         for(int i = 0; i < num_optim_iterations_; i++)
00460         {
00461                 ROS_DEBUG("Inside optimization: Cycle no %d", i);
00462 
00463                 // calculate forces and apply changes
00464                 if(!modifyBandArtificialForce(tmp_band))
00465                 {
00466                         ROS_DEBUG("Optimization failed while trying to modify Band.");
00467                         // something went wrong -> discard changes and stop process
00468                         return false;
00469                 }
00470 
00471                 // check whether band still valid - refine if neccesarry
00472                 if(!refineBand(tmp_band))
00473                 {
00474                         ROS_DEBUG("Optimization failed while trying to refine modified band");
00475                         // modified band is not valid anymore -> discard changes and stop process
00476                         return false;
00477                 }
00478         }
00479 
00480         // copy changes back to band
00481         band = tmp_band;
00482         return true;
00483 }
00484 
00485 
00486 // private methods
00487 
00488 bool EBandPlanner::refineBand(std::vector<Bubble>& band)
00489 {
00490         // check if plugin initialized
00491         if(!initialized_)
00492         {
00493                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
00494                 return false;
00495         }
00496 
00497         // check if band valid (minimum 2 bubbles)
00498         if(band.size() < 2)
00499         {
00500                 ROS_WARN("Attempt to convert empty band to plan. Valid band needs to have at least 2 Frames. This one has %d.", ((int) band.size()) );
00501                 return false;
00502         }
00503 
00504         // instantiate local variables
00505         bool success;
00506         std::vector<Bubble> tmp_band;
00507         std::vector<Bubble>::iterator start_iter, end_iter;
00508 
00509         // remove redundant Bubbles and fill gabs recursively
00510         tmp_band = band;
00511         start_iter = tmp_band.begin();
00512         end_iter = (tmp_band.end() - 1); // -1 because .end() points "past the end"!
00513 
00514         success = removeAndFill(tmp_band, start_iter, end_iter);
00515 
00516         if(!success)
00517                 ROS_DEBUG("Band is broken. Could not close gaps.");
00518         else
00519         {
00520                 #ifdef DEBUG_EBAND_
00521                 ROS_DEBUG("Recursive filling and removing DONE");
00522                 #endif
00523                 band = tmp_band;
00524         }
00525 
00526         return success;
00527 }
00528 
00529 
00530 bool EBandPlanner::removeAndFill(std::vector<Bubble>& band, std::vector<Bubble>::iterator& start_iter,std::vector<Bubble>::iterator& end_iter)
00531 {
00532         // instantiate local variables
00533         bool overlap;
00534         std::vector<Bubble>::iterator tmp_iter;
00535         int mid_int, diff_int;
00536 
00537         #ifdef DEBUG_EBAND_
00538         int debug_dist_start, debug_dist_iters;
00539         debug_dist_start = std::distance(band.begin(), start_iter);
00540         debug_dist_iters = std::distance(start_iter, end_iter);
00541         ROS_DEBUG("Refining Recursive - Check if Bubbles %d and %d overlapp. Total size of band %d.", debug_dist_start, (debug_dist_start + debug_dist_iters), ((int) band.size()) );
00542         #endif
00543 
00544         // check that iterators are still valid
00545         ROS_ASSERT( start_iter >= band.begin() );
00546         ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00547         ROS_ASSERT( start_iter < end_iter );
00548 
00549         // check whether start and end bubbles of this intervall overlap
00550         overlap = checkOverlap(*start_iter, *end_iter);
00551 
00552         if(overlap)
00553         {
00554 
00555                 #ifdef DEBUG_EBAND_
00556                 ROS_DEBUG("Refining Recursive - Bubbles overlapp, check for redundancies");
00557                 #endif
00558 
00559                 // if there are bubbles between start and end of intervall remove them (they are redundant as start and end of intervall do overlap)
00560                 if((start_iter + 1) < end_iter)
00561                 {
00562                         #ifdef DEBUG_EBAND_
00563                         ROS_DEBUG("Refining Recursive - Bubbles overlapp, removing Bubbles %d to %d.", (debug_dist_start + 1), (debug_dist_start + debug_dist_iters -1));
00564                         #endif
00565 
00566                         // erase bubbles between start and end (but not start and end themself) and get new iterator to end (old one is invalid)
00567                         tmp_iter = band.erase((start_iter+1), end_iter);
00568 
00569                         // write back changed iterator pointing to the end of the intervall
00570                         end_iter = tmp_iter;
00571                 }
00572 
00573                 #ifdef DEBUG_EBAND_
00574                 ROS_DEBUG("Refining Recursive - Bubbles overlapp - DONE");
00575                 #endif
00576 
00577                 // we are done here (leaf of this branch is reached)
00578                 return true;
00579         }
00580         
00581 
00582         // if bubbles do not overlap -> check whether there are still bubbles between start and end
00583         if((start_iter + 1) < end_iter)
00584         {
00585                 #ifdef DEBUG_EBAND_
00586                 ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion deeper");
00587                 #endif
00588 
00589                 // split remaining sequence of bubbles
00590                 // get distance between start and end iterator for this intervall
00591                 mid_int = std::distance(start_iter, end_iter);
00592                 mid_int = mid_int/2; // division by integer implies floor (round down)
00593 
00594                 // now get iterator pointing to the middle (roughly)
00595                 tmp_iter = start_iter + mid_int;
00596                 // and realative position of end_iter to tmp_iter
00597                 diff_int = (int) std::distance(tmp_iter, end_iter);
00598 
00599                 // after all this arithmetics - check that iterators are still valid
00600                 ROS_ASSERT( start_iter >= band.begin() );
00601                 ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00602                 ROS_ASSERT( start_iter < end_iter );
00603 
00604 
00605                 // and call removeAndFill recursively for the left intervall
00606                 if(!removeAndFill(band, start_iter, tmp_iter))
00607                 {
00608                         // band is broken in this intervall and could not be fixed
00609                         return false;
00610                 }
00611 
00612                 // carefull at this point!!! if we filled in or removed bubbles end_iter is not valid anymore
00613                 // but the relative position towards tmp_iter is still the same and tmp_iter was kept valid in the lower recursion steps
00614                 end_iter = tmp_iter + diff_int;
00615 
00616                 // check that iterators are still valid - one more time
00617                 ROS_ASSERT( start_iter >= band.begin() );
00618                 ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00619                 ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) );
00620 
00621 
00622                 // o.k. we are done with left hand intervall now do the same for the right hand intervall
00623                 // but first get relative position of start and tmp iter
00624                 diff_int = (int) std::distance(start_iter, tmp_iter);
00625                 if(!removeAndFill(band, tmp_iter, end_iter))
00626                 {
00627                         // band is broken in this intervall and could not be fixed
00628                         return false;
00629                 }
00630 
00631                 // if we filled in bubbles vector might have been reallocated -> start_iter might be invalid
00632                 start_iter = tmp_iter - diff_int;
00633 
00634                 // check that iterators are still valid - almost done
00635                 ROS_ASSERT( start_iter >= band.begin() );
00636                 ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00637                 ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) );
00638 
00639 
00640                 // we reached the leaf but we are not yet done
00641                         // -> we know that there are no redundant elements in the left intervall taken on its own
00642                         // -> and we know the same holds for the right intervall
00643                         // but the middle bubble itself might be redundant -> check it
00644                 if(checkOverlap(*(tmp_iter-1), *(tmp_iter+1)))
00645                 {
00646                         #ifdef DEBUG_EBAND_
00647                         ROS_DEBUG("Refining Recursive - Removing middle bubble");
00648                         #endif
00649 
00650                         // again: get distance between (tmp_iter + 1) and end_iter, (+1 because we will erase tmp_iter itself)
00651                         diff_int = (int) std::distance((tmp_iter + 1), end_iter);
00652 
00653                         // remove middle bubble and correct end_iter
00654                         tmp_iter = band.erase(tmp_iter);
00655                         end_iter = tmp_iter + diff_int;
00656                 }
00657 
00658                 // check that iterators are still valid - almost almost
00659                 ROS_ASSERT( start_iter >= band.begin() );
00660                 ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00661                 ROS_ASSERT( start_iter < end_iter );
00662 
00663                 #ifdef DEBUG_EBAND_
00664                 ROS_DEBUG("Refining Recursive - Bubbles do not overlapp, go one recursion deeper DONE");
00665                 #endif
00666 
00667                 //now we are done with this case
00668                 return true;
00669         }
00670 
00671 
00672         #ifdef DEBUG_EBAND_
00673         ROS_DEBUG("Refining Recursive - Gap detected, fill recursive");
00674         #endif
00675 
00676         // last possible case -> bubbles do not overlap AND there are nor bubbles in between -> try to fill gap recursively
00677         if(!fillGap(band, start_iter, end_iter))
00678         {
00679                 // band is broken in this intervall and could not be fixed (this should only be called on a leaf, so we put a log_out here;)
00680                 ROS_DEBUG("Failed to fill gap between bubble %d and %d.", (int) distance(band.begin(), start_iter), (int) distance(band.begin(), end_iter) );
00681                 return false;
00682         }
00683 
00684         #ifdef DEBUG_EBAND_
00685         ROS_DEBUG("Refining Recursive - Gap detected, fill recursive DONE");
00686         #endif
00687 
00688         // we could fill the gap (reached leaf of this branch) 
00689         return true;
00690 }
00691 
00692 
00693 bool EBandPlanner::fillGap(std::vector<Bubble>& band, std::vector<Bubble>::iterator& start_iter,std::vector<Bubble>::iterator& end_iter)
00694 {
00695         // insert bubbles in the middle between not-overlapping bubbles (e.g. (Dist > Size Bub1) && (Dist > Size Bub2) )
00696         // repeat until gaps are closed
00697 
00698         // instantiate local variables
00699         double distance = 0.0;
00700         Bubble interpolated_bubble;
00701         geometry_msgs::PoseStamped interpolated_center;
00702         std::vector<Bubble>::iterator tmp_iter;
00703         int diff_int, start_num, end_num;
00704 
00705         // make sure this method was called for a valid element in the forces or bubbles vector
00706         ROS_ASSERT( start_iter >= band.begin() );
00707         ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00708         ROS_ASSERT( start_iter < end_iter );
00709 
00710 
00711         #ifdef DEBUG_EBAND_
00712         ROS_DEBUG("Fill recursive - interpolate");
00713         #endif
00714 
00715         // interpolate between bubbles [depends kinematic]
00716         if(!interpolateBubbles(start_iter->center, end_iter->center, interpolated_center))
00717         {
00718                 // interpolation failed (for whatever reason), so return with false
00719                 start_num = std::distance(band.begin(), start_iter);
00720                 end_num = std::distance(band.begin(), end_iter);
00721                 ROS_DEBUG("Interpolation failed while trying to fill gap between bubble %d and %d.", start_num, end_num);
00722                 return false;
00723         }
00724 
00725 
00726         #ifdef DEBUG_EBAND_
00727         ROS_DEBUG("Fill recursive - calc expansion of interpolated bubble");
00728         #endif
00729         
00730         // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment]
00731         if(!calcObstacleKinematicDistance(interpolated_center.pose, distance))
00732         {
00733                 // pose probably outside map coordinates
00734                 start_num = std::distance(band.begin(), start_iter);
00735                 end_num = std::distance(band.begin(), end_iter);
00736                 ROS_DEBUG("Calculation of Distance failed for interpolated bubble - failed to fill gap between bubble %d and %d.", start_num, end_num);
00737                 return false;
00738         }
00739 
00740         if(distance <= tiny_bubble_expansion_)
00741         {
00742                 // band broken! frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
00743                 start_num = std::distance(band.begin(), start_iter);
00744                 end_num = std::distance(band.begin(), end_iter);
00745                 ROS_DEBUG("Interpolated Bubble in Collision - failed to fill gap between bubble %d and %d.", start_num, end_num);
00746                 // TODO this means only that there is an obstacle on the direct interconnection between the bubbles - think about repair or rescue strategies -
00747                 return false;
00748         }
00749         
00750 
00751         #ifdef DEBUG_EBAND_
00752         ROS_DEBUG("Fill recursive - inserting interpolated bubble at (%f, %f), with expansion %f", interpolated_center.pose.position.x, interpolated_center.pose.position.y, distance);
00753         #endif
00754 
00755         // insert bubble and assign center and expansion
00756         interpolated_bubble.center = interpolated_center;
00757         interpolated_bubble.expansion = distance;
00758         // insert bubble (vector.insert() inserts elements before given iterator) and get iterator pointing to it
00759         tmp_iter = band.insert(end_iter, interpolated_bubble);
00760         // insert is a little bit more tricky than erase, as it may require reallocation of the vector -> start and end iter could be invalid
00761         start_iter = tmp_iter - 1;
00762         end_iter = tmp_iter + 1;
00763 
00764         // check that iterators are still valid - just in case :)
00765         ROS_ASSERT( start_iter >= band.begin() );
00766         ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00767         ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) );
00768 
00769 
00770         #ifdef DEBUG_EBAND_
00771         ROS_DEBUG("Fill recursive - check overlap interpolated bubble and first bubble");
00772         #endif
00773 
00774         // we have now two intervalls (left and right of inserted bubble) which need to be checked again and filled if neccessary
00775         if(!checkOverlap(*start_iter, *tmp_iter))
00776         {
00777                 
00778                 #ifdef DEBUG_EBAND
00779                 ROS_DEBUG("Fill recursive - gap btw. interpolated and first bubble - fill recursive");
00780                 #endif
00781 
00782                 // gap in left intervall -> try to fill
00783                 if(!fillGap(band, start_iter, tmp_iter))
00784                 {
00785                         // band is broken in this intervall and could not be fixed
00786                         return false;
00787                 }
00788                 // bubbles were inserted -> make sure to keep end_iter iterator valid
00789                 end_iter = tmp_iter + 1;
00790         }
00791 
00792         // check that iterators are still valid - just in case :)
00793         ROS_ASSERT( start_iter >= band.begin() );
00794         ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00795         ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) );
00796 
00797 
00798         #ifdef DEBUG_EBAND_
00799         ROS_DEBUG("Fill recursive - check overlap interpolated bubble and second bubble");
00800         #endif
00801 
00802         if(!checkOverlap(*tmp_iter, *end_iter))
00803         {
00804                 
00805                 #ifdef DEBUG_EBAND_
00806                 ROS_DEBUG("Fill recursive - gap btw. interpolated and second bubble - fill recursive");
00807                 #endif
00808 
00809                 // get distance between start_iter and tmp_iter before filling right intervall (in case of reallocation of vector)
00810                 diff_int = (int) std::distance(start_iter, tmp_iter);
00811 
00812                 // gap in left intervall -> try to fill
00813                 if(!fillGap(band, tmp_iter, end_iter))
00814                 {
00815                         // band is broken in this intervall and could not be fixed
00816                         return false;
00817                 }
00818                 // bubbles were inserted -> make sure to keep start_iter iterator valid
00819                 start_iter = tmp_iter - diff_int;
00820         }
00821 
00822         // check that iterators are still valid - just in case :)
00823         ROS_ASSERT( start_iter >= band.begin() );
00824         ROS_ASSERT( end_iter < band.end() ); // "<" because .end() points _behind_ last element of vector
00825         ROS_ASSERT( (start_iter < tmp_iter) && (tmp_iter < end_iter) );
00826 
00827 
00828         #ifdef DEBUG_EBAND_
00829         ROS_DEBUG("Fill recursive - gap closed");
00830         #endif
00831 
00832         // bubbles overlap, iterators are kept valid -> done
00833         return true;
00834 }
00835 
00836 
00837 // optimization
00838 
00839 bool EBandPlanner::modifyBandArtificialForce(std::vector<Bubble>& band)
00840 {
00841         if(band.empty())
00842         {
00843                 ROS_ERROR("Trying to modify an empty band.");
00844                 return false;
00845         }
00846 
00847         if(band.size() <= 2)
00848         {
00849                 // nothing to do here -> we can stop right away
00850                 return true;
00851         }
00852 
00853         std::vector<geometry_msgs::WrenchStamped> internal_forces, external_forces, forces;
00854         geometry_msgs::WrenchStamped wrench;
00855 
00856         #ifdef DEBUG_EBAND_
00857         //publish original band
00858         if(visualization_)
00859                 eband_visual_->publishBand("bubbles", band);
00860         #endif
00861 
00862         // init variables to calm down debug warnings
00863         wrench.header.stamp = ros::Time::now();
00864         wrench.header.frame_id = band[0].center.header.frame_id;
00865         wrench.wrench.force.x = 0.0;
00866         wrench.wrench.force.y = 0.0;
00867         wrench.wrench.force.z = 0.0;
00868         wrench.wrench.torque.x = 0.0;
00869         wrench.wrench.torque.y = 0.0;
00870         wrench.wrench.torque.z = 0.0;
00871         internal_forces.assign(band.size(), wrench);
00872         external_forces = internal_forces;
00873         forces = internal_forces;
00874 
00875         // TODO log timigs of planner
00876         // instantiate variables for timing
00877         //ros::Time time_stamp1, time_stamp2;
00878         //ros::Duration duration;
00879         //time_stamp1 = ros::Time::now();
00880 
00881         // due to refinement band might change its size -> use while loop
00882         int i = 1;
00883         bool forward = true; // cycle 1xforwards and 1xbackwards through band
00884         while( (i>0) && (i < ((int) band.size() - 1)) )
00885         {
00886                 ROS_DEBUG("Modifying bubble %d.", i);
00887 
00888 
00889                 #ifdef DEBUG_EBAND_
00890                 ROS_DEBUG("Calculating internal force for bubble %d.", i);
00891                 #endif
00892 
00893                 if(!calcInternalForces(i, band, band.at(i), internal_forces.at(i)))
00894                 {
00895                         // calculation of internal forces failed - stopping optimization
00896                         ROS_DEBUG("Calculation of internal forces failed");
00897                         return false;
00898                 }
00899 
00900                 #ifdef DEBUG_EBAND_
00901                 if(visualization_)
00902                         // publish internal forces
00903                         eband_visual_->publishForce("internal_forces", i, eband_visual_->blue, internal_forces[i], band[i]);
00904                 // Log out debug info about next step
00905                 ROS_DEBUG("Calculating external force for bubble %d.", i);
00906                 #endif
00907 
00908 
00909                 //if(!calcExternalForces(i, band, external_forces))
00910                 if(!calcExternalForces(i, band.at(i), external_forces.at(i)))
00911                 {
00912                         // calculation of External Forces failed - stopping optimization
00913                         ROS_DEBUG("Calculation of external forces failed");
00914                         return false;
00915                 }
00916 
00917                 #ifdef DEBUG_EBAND_
00918                 if(visualization_)
00919                         //publish external forces
00920                         eband_visual_->publishForce("external_forces", i, eband_visual_->red, external_forces[i], band[i]);
00921                 // Log out debug info about next step
00922                 ROS_DEBUG("Superposing internal and external forces");
00923                 #endif
00924 
00925 
00926                 // sum up external and internal forces over all bubbles
00927                 forces.at(i).wrench.force.x = internal_forces.at(i).wrench.force.x + external_forces.at(i).wrench.force.x;
00928                 forces.at(i).wrench.force.y = internal_forces.at(i).wrench.force.y + external_forces.at(i).wrench.force.y;
00929                 forces.at(i).wrench.force.z = internal_forces.at(i).wrench.force.z + external_forces.at(i).wrench.force.z;
00930         
00931                 forces.at(i).wrench.torque.x = internal_forces.at(i).wrench.torque.x + external_forces.at(i).wrench.torque.x;
00932                 forces.at(i).wrench.torque.y = internal_forces.at(i).wrench.torque.y + external_forces.at(i).wrench.torque.y;
00933                 forces.at(i).wrench.torque.z = internal_forces.at(i).wrench.torque.z + external_forces.at(i).wrench.torque.z;
00934         
00935                 #ifdef DEBUG_EBAND_
00936                 ROS_DEBUG("Superpose forces: (x, y, theta) = (%f, %f, %f)", forces.at(i).wrench.force.x, forces.at(i).wrench.force.y, forces.at(i).wrench.torque.z);
00937                 ROS_DEBUG("Supressing tangential forces");
00938                 #endif
00939 
00940                 if(!suppressTangentialForces(i, band, forces.at(i)))
00941                 {
00942                         // suppression of tangential forces failed
00943                         ROS_DEBUG("Supression of tangential forces failed");
00944                         return false;
00945                 }
00946 
00947                 #ifdef DEBUG_EBAND_
00948                 if(visualization_)
00949                         //publish resulting forces
00950                         eband_visual_->publishForce("resulting_forces", i, eband_visual_->green, forces[i], band[i]);
00951                 #endif
00952 
00953 
00954                 ROS_DEBUG("Applying forces to modify band");
00955                 if(!applyForces(i, band, forces))
00956                 {
00957                         // band invalid
00958                         ROS_DEBUG("Band is invalid - Stopping Modification");
00959                         return false;
00960                 }
00961 
00962                 #ifdef DEBUG_EBAND_
00963                 if(visualization_)
00964                 {
00965                         // publish band with changed bubble at resulting position
00966                         eband_visual_->publishBand("bubbles", band);
00967                         ros::Duration(0.01).sleep();
00968                 }
00969                 #endif
00970 
00971 
00972                 //next bubble
00973                 if(forward)
00974                 {
00975                         i++;
00976                         if(i == ((int) band.size() - 1))
00977                         {
00978                                 // reached end of band - start backwards cycle until at start again - then stop
00979                                 forward = false;
00980                                 i--;
00981                                 ROS_DEBUG("Optimization Elastic Band - Forward cycle done, starting backward cycle");
00982                         }
00983                 }
00984                 else
00985                 {
00986                         i--;
00987                 }
00988         }
00989 
00990         return true;
00991 }
00992 
00993 
00994 bool EBandPlanner::applyForces(int bubble_num, std::vector<Bubble>& band, std::vector<geometry_msgs::WrenchStamped> forces)
00995 {
00996         //cycle over all bubbles except first and last (these are fixed)
00997         if(band.size() <= 2)
00998         {
00999                 // nothing to do here -> we can stop right away - no forces calculated
01000                 return true;
01001         }
01002 
01003         geometry_msgs::Pose2D bubble_pose2D, new_bubble_pose2D;
01004         geometry_msgs::Pose bubble_pose, new_bubble_pose;
01005         geometry_msgs::Twist bubble_jump;
01006         Bubble new_bubble = band.at(bubble_num);
01007         double distance;
01008 
01009 
01010         // move bubble
01011         bubble_pose = band.at(bubble_num).center.pose;
01012         PoseToPose2D(bubble_pose, bubble_pose2D);
01013 
01014         // move according to bubble_new = bubble_old + alpha*force -> we choose alpha to be the current expansion of the modified bubble
01015         bubble_jump.linear.x = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.x;
01016         bubble_jump.linear.y = band.at(bubble_num).expansion*forces.at(bubble_num).wrench.force.y;
01017         bubble_jump.linear.z = 0.0;
01018         bubble_jump.angular.x = 0.0;
01019         bubble_jump.angular.y = 0.0;
01020         bubble_jump.angular.z = band.at(bubble_num).expansion/costmap_ros_->getCircumscribedRadius() * forces.at(bubble_num).wrench.torque.z;
01021         bubble_jump.angular.z = angles::normalize_angle(bubble_jump.angular.z);
01022 
01023         // apply changes to calc tmp bubble position
01024         new_bubble_pose2D.x = bubble_pose2D.x + bubble_jump.linear.x;
01025         new_bubble_pose2D.y = bubble_pose2D.y + bubble_jump.linear.y;
01026         new_bubble_pose2D.theta = bubble_pose2D.theta + bubble_jump.angular.z;
01027         new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta);
01028 
01029         // apply changes to local copy
01030         Pose2DToPose(new_bubble_pose, new_bubble_pose2D);
01031         new_bubble.center.pose = new_bubble_pose;
01032 
01033         #ifdef DEBUG_EBAND_
01034         ROS_DEBUG("Try moving bubble %d at (%f, %f, %f) by (%f, %f, %f).", bubble_num, bubble_pose2D.x, bubble_pose2D.y, bubble_pose2D.theta,
01035                                 bubble_jump.linear.x, bubble_jump.linear.y, bubble_jump.angular.z);
01036         #endif
01037 
01038 
01039         // check validity of moved bubble
01040 
01041         // recalc expansion of bubble -> calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment]
01042         if(!calcObstacleKinematicDistance(new_bubble_pose, distance))
01043         {
01044                 ROS_DEBUG("Calculation of Distance failed. Frame %d of %d Probably outside map. Discarding Changes", bubble_num, ((int) band.size()) );
01045 
01046                 #ifdef DEBUG_EBAND_
01047                 if(visualization_)
01048                         eband_visual_->publishBubble("bubble_hypo", bubble_num, eband_visual_->red, new_bubble);
01049                 #endif
01050 
01051                 // this bubble must not be changed, but band is still valid -> continue with other bubbles
01052                 return true;
01053         }
01054 
01055         if(distance <= tiny_bubble_expansion_)
01056         {
01057                 // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
01058                 ROS_DEBUG("Calculation of Distance failed. Frame %d of %d in collision. Plan invalid. Discarding Changes", bubble_num, ((int) band.size()) );
01059 
01060                 #ifdef DEBUG_EBAND_
01061                 if(visualization_)
01062                         eband_visual_->publishBubble("bubble_hypo", bubble_num, eband_visual_->red, new_bubble);
01063                 #endif
01064 
01065                 // this bubble must not be changed, but band is still valid -> continue with other bubbles
01066                 return true;
01067         }
01068 
01069         // so far o.k. -> assign distance to new bubble
01070         new_bubble.expansion = distance;
01071         
01072 
01073         // check whether step was reasonable
01074 
01075         geometry_msgs::WrenchStamped new_bubble_force = forces.at(bubble_num);
01076         
01077         // check whether we get a valid force calculation here
01078         if(!getForcesAt(bubble_num, band, new_bubble, new_bubble_force))
01079         {
01080                 // error during calculation of forces for the new position - discard changes
01081                 ROS_DEBUG("Cannot calculate forces on bubble %d at new position - discarding changes", bubble_num);
01082                 return true;
01083         }
01084 
01085         #ifdef DEBUG_EBAND_
01086                 ROS_DEBUG("Check for zero-crossings in force on bubble %d", bubble_num);
01087         #endif
01088 
01089         // check for zero-crossings in the force-vector
01090         double checksum_zero, abs_new_force, abs_old_force;
01091         
01092         // project force-vectors onto each other
01093         checksum_zero = (new_bubble_force.wrench.force.x * forces.at(bubble_num).wrench.force.x) +
01094                                         (new_bubble_force.wrench.force.y * forces.at(bubble_num).wrench.force.y) +
01095                                         (new_bubble_force.wrench.torque.z * forces.at(bubble_num).wrench.torque.z);
01096 
01097         // if sign changes and ...
01098         if(checksum_zero < 0.0)
01099         {
01100                 ROS_DEBUG("Detected zero-crossings in force on bubble %d. Checking total change in force.", bubble_num);
01101                 // check the absolute values of the two vectors
01102                 abs_new_force = sqrt( (new_bubble_force.wrench.force.x * new_bubble_force.wrench.force.x) +
01103                                                         (new_bubble_force.wrench.force.y * new_bubble_force.wrench.force.y) +
01104                                                         (new_bubble_force.wrench.torque.z * new_bubble_force.wrench.torque.z) );
01105                 abs_old_force = sqrt( (forces.at(bubble_num).wrench.force.x * forces.at(bubble_num).wrench.force.x) +
01106                                                         (forces.at(bubble_num).wrench.force.x * forces.at(bubble_num).wrench.force.x) +
01107                                                         (forces.at(bubble_num).wrench.torque.z * forces.at(bubble_num).wrench.torque.z) );
01108 
01109                 // force still has a significant high value (> ~75% of old force by default)
01110                 if( (abs_new_force > equilibrium_relative_overshoot_ * abs_old_force) && (abs_new_force > significant_force_) )
01111                 {
01112                         ROS_DEBUG("Detected significante change in force (%f to %f) on bubble %d. Entering Recursive Approximation.", abs_old_force, abs_new_force, bubble_num);
01113                         // o.k. now we really have to take a closer look -> start recursive approximation to equilibrium-point
01114                         int curr_recursion_depth = 0;
01115                         geometry_msgs::Twist new_step_width;
01116                         Bubble curr_bubble = band.at(bubble_num);
01117                         geometry_msgs::WrenchStamped curr_bubble_force = forces.at(bubble_num);
01118 
01119                         // half step size
01120                         new_step_width.linear.x = 0.5*bubble_jump.linear.x;
01121                         new_step_width.linear.y = 0.5*bubble_jump.linear.y;
01122                         new_step_width.linear.z = 0.5*bubble_jump.linear.z;
01123                         new_step_width.angular.x = 0.5*bubble_jump.angular.x;
01124                         new_step_width.angular.y = 0.5*bubble_jump.angular.y;
01125                         new_step_width.angular.z = 0.5*bubble_jump.angular.z;
01126 
01127                         // one step deeper into the recursion
01128                         if(moveApproximateEquilibrium(bubble_num, band, curr_bubble, curr_bubble_force, new_step_width, curr_recursion_depth))
01129                         {
01130                                 // done with recursion - change bubble and hand it back
01131                                 new_bubble = curr_bubble;
01132 
01133                                 #ifdef DEBUG_EBAND_
01134                                 geometry_msgs::Pose2D curr_bubble_pose2D;
01135                                 PoseToPose2D(curr_bubble.center.pose, curr_bubble_pose2D);
01136                                 ROS_DEBUG("Instead - Try moving bubble %d at (%f, %f, %f) by (%f, %f, %f) to (%f, %f, %f).",
01137                                                         bubble_num, bubble_pose2D.x, bubble_pose2D.y, bubble_pose2D.theta,
01138                                                         new_step_width.linear.x, new_step_width.linear.y, new_step_width.angular.z,
01139                                                         curr_bubble_pose2D.x, curr_bubble_pose2D.y, curr_bubble_pose2D.theta);
01140                                 #endif
01141                         }
01142                 }
01143         }
01144 
01145         
01146         // check validity of resulting band (given the moved bubble)
01147 
01148         // TODO use this routine not only to check whether gap can be filled but also to fill gap (if possible)
01149         // get local copy of band, set new position of moved bubble and init iterators
01150         std::vector<Bubble> tmp_band = band;
01151         std::vector<Bubble>::iterator start_iter, end_iter;
01152         tmp_band.at(bubble_num) = new_bubble;
01153         start_iter = tmp_band.begin();
01154 
01155         // check left connection (bubble and bubble-1)
01156         start_iter = start_iter + bubble_num - 1;
01157         end_iter = start_iter + 1;
01158 
01159         // check Overlap - if bubbles do not overlap try to fill gap
01160         if(!checkOverlap(*start_iter, *end_iter))
01161         {
01162                 if(!fillGap(tmp_band, start_iter, end_iter))
01163                 {
01164                         ROS_DEBUG("Bubble at new position cannot be connected to neighbour. Discarding changes.");
01165                         // this bubble must not be changed, but band is still valid -> continue with other bubbles
01166                         return true;
01167                 }
01168         }
01169 
01170 
01171         // get fresh copy of band, set new position of bubble again and reinit iterators
01172         tmp_band = band;
01173         tmp_band.at(bubble_num) = new_bubble;
01174         start_iter = tmp_band.begin();
01175 
01176         // check right connection (bubble and bubble +1)
01177         start_iter = start_iter + bubble_num;
01178         end_iter = start_iter + 1;
01179 
01180         // check Overlap - if bubbles do not overlap try to fill gap
01181         if(!checkOverlap(*start_iter, *end_iter))
01182         {
01183                 if(!fillGap(tmp_band, start_iter, end_iter))
01184                 {
01185                         ROS_DEBUG("Bubble at new position cannot be connected to neighbour. Discarding changes.");
01186                         // this bubble must not be changed, but band is still valid -> continue with other bubbles
01187                         return true;
01188                 }
01189         }
01190 
01191 
01192         // check successful - bubble and band valid apply changes
01193 
01194         #ifdef DEBUG_EBAND_
01195         ROS_DEBUG("Frame %d of %d: Check successful - bubble and band valid. Applying Changes", bubble_num, ((int) band.size()) );
01196         #endif
01197 
01198         band.at(bubble_num) = new_bubble;
01199 
01200         return true;
01201 }
01202 
01203 
01204 bool EBandPlanner::moveApproximateEquilibrium(const int& bubble_num, const std::vector<Bubble>& band, Bubble& curr_bubble,
01205                                                 const geometry_msgs::WrenchStamped& curr_bubble_force, geometry_msgs::Twist& curr_step_width, const int& curr_recursion_depth)
01206 {
01207 
01208         double distance;
01209         Bubble new_bubble = curr_bubble;
01210         geometry_msgs::Pose2D new_bubble_pose2D, curr_bubble_pose2D;
01211         geometry_msgs::WrenchStamped new_bubble_force = curr_bubble_force;
01212 
01213         // move bubble
01214         PoseToPose2D(curr_bubble.center.pose, curr_bubble_pose2D);
01215         PoseToPose2D(new_bubble.center.pose, new_bubble_pose2D);
01216 
01217         // apply changes to calculate tmp bubble position
01218         new_bubble_pose2D.x = curr_bubble_pose2D.x + curr_step_width.linear.x;
01219         new_bubble_pose2D.y = curr_bubble_pose2D.y + curr_step_width.linear.y;
01220         new_bubble_pose2D.theta = curr_bubble_pose2D.theta + curr_step_width.angular.z;
01221         new_bubble_pose2D.theta = angles::normalize_angle(new_bubble_pose2D.theta);
01222 
01223         // apply changes to local copy
01224         Pose2DToPose(new_bubble.center.pose, new_bubble_pose2D);
01225 
01226 
01227         // check validity of moved bubble
01228 
01229         // recalc expansion of bubble -> calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment]
01230         if(!calcObstacleKinematicDistance(new_bubble.center.pose, distance))
01231                 return false;
01232 
01233         // we wont be able to calculate forces later on
01234         if(distance == 0.0)
01235                 return false;
01236 
01237 
01238         // so far o.k. -> assign distance to new bubble
01239         new_bubble.expansion = distance;
01240         
01241         // check whether we get a valid force calculation here
01242         if(!getForcesAt(bubble_num, band, new_bubble, new_bubble_force))
01243                 return false;
01244 
01245         // great - lets store this - this is better then what we had so far
01246         curr_bubble = new_bubble;
01247 
01248         // if everything is fine and we reached our maximum recursion depth
01249         if(curr_recursion_depth >= max_recursion_depth_approx_equi_)
01250                 // break recursion at this point
01251                 return true;
01252 
01253 
01254         // now - let's check for zero-crossing
01255 
01256         #ifdef DEBUG_EBAND_
01257                 ROS_DEBUG("Check for zero-crossings in force on bubble %d - Recursion %d", bubble_num, curr_recursion_depth);
01258         #endif
01259 
01260         double checksum_zero, abs_new_force, abs_old_force;
01261         int new_recursion_depth;
01262         geometry_msgs::Twist new_step_width;
01263 
01264         // check zero-crossing by projecting force-vectors onto each other
01265         checksum_zero = (new_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) +
01266                                         (new_bubble_force.wrench.force.y * curr_bubble_force.wrench.force.y) +
01267                                         (new_bubble_force.wrench.torque.z * curr_bubble_force.wrench.torque.z);
01268 
01269         if(checksum_zero < 0.0)
01270         {
01271                 #ifdef DEBUG_EBAND_
01272                         ROS_DEBUG("Detected zero-crossings in force on bubble %d - Recursion %d. Checking total change in force.", bubble_num, curr_recursion_depth);
01273                 #endif
01274 
01275                 // check the absolute values of the two vectors
01276                 abs_new_force = sqrt( (new_bubble_force.wrench.force.x * new_bubble_force.wrench.force.x) +
01277                                                         (new_bubble_force.wrench.force.y * new_bubble_force.wrench.force.y) +
01278                                                         (new_bubble_force.wrench.torque.z * new_bubble_force.wrench.torque.z) );
01279                 abs_old_force = sqrt( (curr_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) +
01280                                                         (curr_bubble_force.wrench.force.x * curr_bubble_force.wrench.force.x) +
01281                                                         (curr_bubble_force.wrench.torque.z * curr_bubble_force.wrench.torque.z) );
01282 
01283                 if( (abs_new_force > equilibrium_relative_overshoot_ * abs_old_force) && (abs_new_force > significant_force_) )
01284                 {
01285                         #ifdef DEBUG_EBAND_
01286                                 ROS_DEBUG("Detected significant change in force (%f to %f) on bubble %d - Recursion %d. Going one Recursion deeper.", abs_old_force, abs_new_force, bubble_num, curr_recursion_depth);
01287                         #endif
01288 
01289                         // o.k. now we really have to take a closer look -> start recursive approximation to equilibrium-point
01290                         new_recursion_depth = curr_recursion_depth + 1;
01291                         // half step size - backward direction
01292                         new_step_width.linear.x = -0.5*curr_step_width.linear.x;
01293                         new_step_width.linear.y = -0.5*curr_step_width.linear.y;
01294                         new_step_width.linear.z = -0.5*curr_step_width.linear.z;
01295                         new_step_width.angular.x = -0.5*curr_step_width.angular.x;
01296                         new_step_width.angular.y = -0.5*curr_step_width.angular.y;
01297                         new_step_width.angular.z = -0.5*curr_step_width.angular.z;
01298 
01299                         // one step deeper into the recursion
01300                         if(moveApproximateEquilibrium(bubble_num, band, new_bubble, new_bubble_force, new_step_width, new_recursion_depth))
01301                                 // done with recursion - change bubble and hand it back
01302                                 curr_bubble = new_bubble;
01303 
01304                         // otherwise - could not get a better value - return without change (curr_bubble as asigned above)
01305                 }
01306                 
01307                 // otherwise - this is good enough for us - return with this value (curr_bubble as asigned above)
01308         }
01309         else
01310         {
01311                 #ifdef DEBUG_EBAND_
01312                         ROS_DEBUG("No zero-crossings in force on bubble %d - Recursion %d. Continue walk in same direction. Going one recursion deeper.", bubble_num, curr_recursion_depth);
01313                 #endif
01314 
01315                 // continue walk in same direction
01316                 new_recursion_depth = curr_recursion_depth + 1;
01317                 // half step size - backward direction
01318                 new_step_width.linear.x = 0.5*curr_step_width.linear.x;
01319                 new_step_width.linear.y = 0.5*curr_step_width.linear.y;
01320                 new_step_width.linear.z = 0.5*curr_step_width.linear.z;
01321                 new_step_width.angular.x = 0.5*curr_step_width.angular.x;
01322                 new_step_width.angular.y = 0.5*curr_step_width.angular.y;
01323                 new_step_width.angular.z = 0.5*curr_step_width.angular.z;
01324 
01325                 // one step deeper into the recursion
01326                 if(moveApproximateEquilibrium(bubble_num, band, new_bubble, new_bubble_force, new_step_width, new_recursion_depth))
01327                         // done with recursion - change bubble and hand it back
01328                         curr_bubble = new_bubble;
01329 
01330                 // otherwise - could not get a better value - return without change (curr_bubble as asigned above)
01331         }
01332 
01333         // done
01334         return true;
01335 }
01336 
01337 
01338 bool EBandPlanner::getForcesAt(int bubble_num, std::vector<Bubble> band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces)
01339 {
01340         geometry_msgs::WrenchStamped internal_force, external_force;
01341 
01342         if(!calcInternalForces(bubble_num, band, curr_bubble, internal_force))
01343         {
01344                 // calculation of internal forces failed - stopping optimization
01345                 ROS_DEBUG("Calculation of internal forces failed");
01346                 return false;
01347         }
01348 
01349         if(!calcExternalForces(bubble_num, curr_bubble, external_force))
01350         {
01351                 // calculation of External Forces failed - stopping optimization
01352                 ROS_DEBUG("Calculation of external forces failed");
01353                 return false;
01354         }
01355 
01356         // sum up external and internal forces over all bubbles
01357         forces.wrench.force.x = internal_force.wrench.force.x + external_force.wrench.force.x;
01358         forces.wrench.force.y = internal_force.wrench.force.y + external_force.wrench.force.y;
01359         forces.wrench.force.z = internal_force.wrench.force.z + external_force.wrench.force.z;
01360         
01361         forces.wrench.torque.x = internal_force.wrench.torque.x + external_force.wrench.torque.x;
01362         forces.wrench.torque.y = internal_force.wrench.torque.y + external_force.wrench.torque.y;
01363         forces.wrench.torque.z = internal_force.wrench.torque.z + external_force.wrench.torque.z;
01364         
01365         if(!suppressTangentialForces(bubble_num, band, forces))
01366         {
01367                 // suppression of tangential forces failed
01368                 ROS_DEBUG("Supression of tangential forces failed");
01369                 return false;
01370         }
01371 
01372         return true;
01373 }
01374 
01375 
01376 bool EBandPlanner::calcInternalForces(int bubble_num, std::vector<Bubble> band, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces)
01377 {
01378         // check if plugin initialized
01379         if(!initialized_)
01380         {
01381                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01382                 return false;
01383         }
01384 
01385         //cycle over all bubbles except first and last (these are fixed)
01386         if(band.size() <= 2)
01387         {
01388                 // nothing to do here -> we can stop right away - no forces calculated
01389                 return true;
01390         }
01391 
01392         // init tmp variables
01393         double distance1, distance2;
01394         geometry_msgs::Twist difference1, difference2;
01395         geometry_msgs::Wrench wrench;
01396 
01397         // make sure this method was called for a valid element in the forces or bubbles vector
01398         ROS_ASSERT( bubble_num > 0 );
01399         ROS_ASSERT( bubble_num < ((int) band.size() - 1) );
01400 
01401 
01402         // get distance between bubbles
01403         if(!calcBubbleDistance(curr_bubble.center.pose, band[bubble_num-1].center.pose, distance1))
01404         {
01405                 ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting calculation of internal forces!");
01406                 return false;
01407         }
01408 
01409         if(!calcBubbleDistance(curr_bubble.center.pose, band[bubble_num+1].center.pose, distance2))
01410         {
01411                 ROS_ERROR("Failed to calculate Distance between two bubbles. Aborting calculation of internal forces!");
01412                 return false;
01413         }
01414 
01415         // get (elementwise) difference bewtween bubbles
01416         if(!calcBubbleDifference(curr_bubble.center.pose, band[bubble_num-1].center.pose, difference1))
01417         {
01418                 ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting calculation of internal forces!");
01419                 return false;
01420         }
01421 
01422         if(!calcBubbleDifference(curr_bubble.center.pose, band[bubble_num+1].center.pose, difference2))
01423         {
01424                 ROS_ERROR("Failed to calculate Difference between two bubbles. Aborting calculation of internal forces!");
01425                 return false;
01426         }
01427 
01428         // make sure to avoid division by  (almost) zero during force calculation (avoid numerical problems)
01429         // -> if difference/distance is (close to) zero then the force in this direction should be zero as well
01430         if(distance1 <= tiny_bubble_distance_)
01431                 distance1 = 1000000.0;
01432         if(distance2 <= tiny_bubble_distance_)
01433                 distance2 = 1000000.0;
01434 
01435         // now calculate wrench - forces model an elastic band and are normed (distance) to render forces for small and large bubbles the same
01436         wrench.force.x = internal_force_gain_*(difference1.linear.x/distance1 + difference2.linear.x/distance2);
01437         wrench.force.y = internal_force_gain_*(difference1.linear.y/distance1 + difference2.linear.y/distance2);
01438         wrench.force.z = internal_force_gain_*(difference1.linear.z/distance1 + difference2.linear.z/distance2);
01439         wrench.torque.x = internal_force_gain_*(difference1.angular.x/distance1 + difference2.angular.x/distance2);
01440         wrench.torque.y = internal_force_gain_*(difference1.angular.y/distance1 + difference2.angular.y/distance2);
01441         wrench.torque.z = internal_force_gain_*(difference1.angular.z/distance1 + difference2.angular.z/distance2);
01442 
01443         #ifdef DEBUG_EBAND_
01444         ROS_DEBUG("Calculating internal forces: (x, y, theta) = (%f, %f, %f)", wrench.force.x, wrench.force.y, wrench.torque.z);
01445         #endif
01446 
01447         // store wrench in vector
01448         forces.wrench = wrench;
01449 
01450         return true;
01451 }
01452 
01453 
01454 bool EBandPlanner::calcExternalForces(int bubble_num, Bubble curr_bubble, geometry_msgs::WrenchStamped& forces)
01455 {
01456         // check if plugin initialized
01457         if(!initialized_)
01458         {
01459                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01460                 return false;
01461         }
01462 
01463         // init tmp variables
01464         double distance1, distance2;
01465         geometry_msgs::Pose edge;
01466         geometry_msgs::Pose2D edge_pose2D;
01467         geometry_msgs::Wrench wrench;
01468 
01469 
01470         // calculate delta-poses (on upper edge of bubble) for x-direction
01471         edge = curr_bubble.center.pose;
01472         edge.position.x = edge.position.x + curr_bubble.expansion;
01473         // get expansion on bubble at this point
01474         if(!calcObstacleKinematicDistance(edge, distance1))
01475         {
01476                 ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
01477                 // we cannot calculate external forces for this bubble - but still continue for the other bubbles
01478                 return true;
01479         }
01480         // calculate delta-poses (on lower edge of bubble) for x-direction
01481         edge.position.x = edge.position.x - 2.0*curr_bubble.expansion;
01482         // get expansion on bubble at this point
01483         if(!calcObstacleKinematicDistance(edge, distance2))
01484         {
01485                 ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
01486                 // we cannot calculate external forces for this bubble - but still continue for the other bubbles
01487                 return true;
01488         }
01489 
01490         // calculate difference-quotient (approx. of derivative) in x-direction
01491         if(curr_bubble.expansion <= tiny_bubble_expansion_)
01492         {
01493                 // avoid division by (almost) zero to avoid numerical problems
01494                 wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_);
01495                 // actually we should never end up here - band should have been considered as broken
01496                 ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured");
01497         }
01498         else                    
01499                 wrench.force.x = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion);
01500         // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponding term
01501 
01502 
01503         // calculate delta-poses (on upper edge of bubble) for y-direction
01504         edge = curr_bubble.center.pose;
01505         edge.position.y = edge.position.y + curr_bubble.expansion;
01506         // get expansion on bubble at this point
01507         if(!calcObstacleKinematicDistance(edge, distance1))
01508         {
01509                 ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
01510                 // we cannot calculate external forces for this bubble - but still continue for the other bubbles
01511                 return true;
01512         }
01513         // calculate delta-poses (on lower edge of bubble) for x-direction
01514         edge.position.y = edge.position.y - 2.0*curr_bubble.expansion;
01515         // get expansion on bubble at this point
01516         if(!calcObstacleKinematicDistance(edge, distance2))
01517         {
01518                 ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
01519                 // we cannot calculate external forces for this bubble - but still continue for the other bubbles
01520                 return true;
01521         }
01522 
01523         // calculate difference-quotient (approx. of derivative) in x-direction
01524         if(curr_bubble.expansion <= tiny_bubble_expansion_)
01525         {
01526                 // avoid division by (almost) zero to avoid numerical problems
01527                 wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_);
01528                 // actually we should never end up here - band should have been considered as broken
01529                 ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured");
01530         }
01531         else
01532                 wrench.force.y = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion);
01533         // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term
01534 
01535 
01536         // no force in z-direction
01537         wrench.force.z = 0.0;
01538 
01539 
01540         // no torque around x and y axis
01541         wrench.torque.x = 0.0;
01542         wrench.torque.y = 0.0;
01543 
01544 
01545         // calculate delta-poses (on upper edge of bubble) for x-direction
01546         PoseToPose2D(curr_bubble.center.pose, edge_pose2D);
01547         edge_pose2D.theta = edge_pose2D.theta + (curr_bubble.expansion/costmap_ros_->getCircumscribedRadius());
01548         edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta);
01549         PoseToPose2D(edge, edge_pose2D);
01550         // get expansion on bubble at this point
01551         if(!calcObstacleKinematicDistance(edge, distance1))
01552         {
01553                 ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
01554                 // we cannot calculate external forces for this bubble - but still continue for the other bubbles
01555                 return true;
01556         }
01557         // calculate delta-poses (on lower edge of bubble) for x-direction
01558         edge_pose2D.theta = edge_pose2D.theta - 2.0*(curr_bubble.expansion/costmap_ros_->getCircumscribedRadius());
01559         edge_pose2D.theta = angles::normalize_angle(edge_pose2D.theta);
01560         PoseToPose2D(edge, edge_pose2D);
01561         // get expansion on bubble at this point
01562         if(!calcObstacleKinematicDistance(edge, distance2))
01563         {
01564                 ROS_DEBUG("Bubble %d probably at edge of map - cannot retrieve distance information to calculate external forces", bubble_num);
01565                 // we cannot calculate external forces for this bubble - but still continue for the other bubbles
01566                 return true;
01567         }
01568 
01569         // calculate difference-quotient (approx. of derivative) in x-direction
01570         if(curr_bubble.expansion <= tiny_bubble_expansion_)
01571         {
01572                 // avoid division by (almost) zero to avoid numerical problems
01573                 wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*tiny_bubble_expansion_);
01574                 // actually we should never end up here - band should have been considered as broken
01575                 ROS_DEBUG("Calculating external forces on broken band. Bubble should have been removed. Local Planner probably ill configured");
01576         }
01577         else
01578                 wrench.torque.z = -external_force_gain_*(distance2 - distance1)/(2.0*curr_bubble.expansion);
01579         // TODO above equations skip term to make forces continuous at end of influence region - test to add corresponsing term
01580 
01581 
01582         #ifdef DEBUG_EBAND_
01583         ROS_DEBUG("Calculating external forces: (x, y, theta) = (%f, %f, %f)", wrench.force.x, wrench.force.y, wrench.torque.z);
01584         #endif
01585 
01586         // assign wrench to forces vector
01587         forces.wrench = wrench;
01588 
01589         return true;
01590 }
01591 
01592 
01593 bool EBandPlanner::suppressTangentialForces(int bubble_num, std::vector<Bubble> band, geometry_msgs::WrenchStamped& forces)
01594 {
01595         //cycle over all bubbles except first and last (these are fixed)
01596         if(band.size() <= 2)
01597         {
01598                 // nothing to do here -> we can stop right away - no forces calculated
01599                 return true;
01600         }
01601 
01602         double scalar_fd, scalar_dd;
01603         geometry_msgs::Twist difference;
01604 
01605         // make sure this method was called for a valid element in the forces or bubbles vector
01606         ROS_ASSERT( bubble_num > 0 );
01607         ROS_ASSERT( bubble_num < ((int) band.size() - 1) );
01608 
01609 
01610         // get pose-difference from following to preceding bubble -> "direction of the band in this bubble"
01611         if(!calcBubbleDifference(band[bubble_num+1].center.pose, band[bubble_num-1].center.pose, difference))
01612                 return false;
01613 
01614         // "project wrench" in middle bubble onto connecting vector
01615         // scalar wrench*difference
01616         scalar_fd = forces.wrench.force.x*difference.linear.x + forces.wrench.force.y*difference.linear.y +
01617                                 forces.wrench.force.z*difference.linear.z + forces.wrench.torque.x*difference.angular.x +
01618                                 forces.wrench.torque.y*difference.angular.y + forces.wrench.torque.z*difference.angular.z;
01619 
01620         // abs of difference-vector: scalar difference*difference
01621         scalar_dd = difference.linear.x*difference.linear.x + difference.linear.y*difference.linear.y + difference.linear.z*difference.linear.z +
01622                                 difference.angular.x*difference.angular.x + difference.angular.y*difference.angular.y + difference.angular.z*difference.angular.z;
01623 
01624         // avoid division by (almost) zero -> check if bubbles have (almost) same center-pose
01625         if(scalar_dd <= tiny_bubble_distance_)
01626         {
01627                 // there are redundant bubbles, this should normally not hapen -> probably error in band refinement
01628                 ROS_DEBUG("Calculating tangential forces for redundant bubbles. Bubble should have been removed. Local Planner probably ill configured");
01629         }
01630 
01631         // calculate orthogonal components
01632         forces.wrench.force.x = forces.wrench.force.x - scalar_fd/scalar_dd * difference.linear.x;
01633         forces.wrench.force.y = forces.wrench.force.y - scalar_fd/scalar_dd * difference.linear.y;
01634         forces.wrench.force.z = forces.wrench.force.z - scalar_fd/scalar_dd * difference.linear.z;
01635         forces.wrench.torque.x = forces.wrench.torque.x - scalar_fd/scalar_dd * difference.angular.x;
01636         forces.wrench.torque.y = forces.wrench.torque.y - scalar_fd/scalar_dd * difference.angular.y;
01637         forces.wrench.torque.z = forces.wrench.torque.z - scalar_fd/scalar_dd * difference.angular.z;
01638 
01639         #ifdef DEBUG_EBAND_
01640         ROS_DEBUG("Supressing tangential forces: (x, y, theta) = (%f, %f, %f)",
01641                                 forces.wrench.force.x, forces.wrench.force.y, forces.wrench.torque.z);
01642         #endif
01643 
01644         return true;
01645 }
01646 
01647 
01648 // problem (geometry) dependant functions
01649 
01650 bool EBandPlanner::interpolateBubbles(geometry_msgs::PoseStamped start_center, geometry_msgs::PoseStamped end_center, geometry_msgs::PoseStamped& interpolated_center)
01651 {
01652         // check if plugin initialized
01653         if(!initialized_)
01654         {
01655                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01656                 return false;
01657         }
01658 
01659         // instantiate local variables
01660         geometry_msgs::Pose2D start_pose2D, end_pose2D, interpolated_pose2D;
01661         double delta_theta;
01662 
01663         // copy header
01664         interpolated_center.header = start_center.header;
01665 
01666         // interpolate angles
01667         // TODO make this in a better way
01668         // - for instance use "slerp" to interpolate directly between quaternions
01669         // - or work with pose2D right from the beginnning
01670         // convert quaternions to euler angles - at this point this no longer works in 3D !!
01671         PoseToPose2D(start_center.pose, start_pose2D);
01672         PoseToPose2D(end_center.pose, end_pose2D);
01673         // calc mean of theta angle
01674         delta_theta = end_pose2D.theta - start_pose2D.theta;
01675         delta_theta = angles::normalize_angle(delta_theta) / 2.0;
01676         interpolated_pose2D.theta = start_pose2D.theta + delta_theta;
01677         interpolated_pose2D.theta = angles::normalize_angle(interpolated_pose2D.theta);
01678         // convert back to quaternion
01679         interpolated_pose2D.x = 0.0;
01680         interpolated_pose2D.y = 0.0;
01681         Pose2DToPose(interpolated_center.pose, interpolated_pose2D);
01682 
01683         // interpolate positions
01684         interpolated_center.pose.position.x = (end_center.pose.position.x + start_center.pose.position.x)/2.0;
01685         interpolated_center.pose.position.y = (end_center.pose.position.y + start_center.pose.position.y)/2.0;
01686         interpolated_center.pose.position.z = (end_center.pose.position.z + start_center.pose.position.z)/2.0;
01687 
01688         // TODO ideally this would take into account kinematics of the robot and for instance use splines
01689 
01690         return true;
01691 }
01692 
01693 
01694 bool EBandPlanner::checkOverlap(Bubble bubble1, Bubble bubble2)
01695 {
01696         // check if plugin initialized
01697         if(!initialized_)
01698         {
01699                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01700                 return false;
01701         }
01702 
01703         // calc (kinematic) Distance between bubbles
01704         double distance = 0.0;
01705         if(!calcBubbleDistance(bubble1.center.pose, bubble2.center.pose, distance))
01706         {
01707                 ROS_ERROR("failed to calculate Distance between two bubbles. Aborting check for overlap!");
01708                 return false;
01709         }
01710         
01711         // compare with size of the two bubbles
01712         if(distance >= min_bubble_overlap_ * (bubble1.expansion + bubble2.expansion))
01713                 return false;
01714 
01715         // TODO this does not account for kinematic properties -> improve
01716 
01717         // everything fine - bubbles overlap
01718         return true;
01719 }
01720 
01721 
01722 bool EBandPlanner::calcBubbleDistance(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, double& distance)
01723 {
01724         // check if plugin initialized
01725         if(!initialized_)
01726         {
01727                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01728                 return false;
01729         }
01730 
01731         geometry_msgs::Pose2D start_pose2D, end_pose2D, diff_pose2D;
01732 
01733         // TODO make this in a better way
01734         // - or work with pose2D right from the beginnning
01735         // convert quaternions to euler angles - at this point this no longer works in 3D !!
01736         PoseToPose2D(start_center_pose, start_pose2D);
01737         PoseToPose2D(end_center_pose, end_pose2D);
01738 
01739         // get rotational difference
01740         diff_pose2D.theta = end_pose2D.theta - start_pose2D.theta;
01741         diff_pose2D.theta = angles::normalize_angle(diff_pose2D.theta);
01742         // get translational difference
01743         diff_pose2D.x = end_pose2D.x - start_pose2D.x;
01744         diff_pose2D.y = end_pose2D.y - start_pose2D.y;
01745 
01746         // calc distance
01747         double angle_to_pseudo_vel = diff_pose2D.theta * costmap_ros_->getCircumscribedRadius();
01748         distance = sqrt( (diff_pose2D.x * diff_pose2D.x) + (diff_pose2D.y * diff_pose2D.y) + (angle_to_pseudo_vel * angle_to_pseudo_vel) );
01749 
01750         // TODO take into account kinematic properties of body
01751 
01752         return true;
01753 }
01754 
01755 
01756 bool EBandPlanner::calcBubbleDifference(geometry_msgs::Pose start_center_pose, geometry_msgs::Pose end_center_pose, geometry_msgs::Twist& difference)
01757 {
01758         // check if plugin initialized
01759         if(!initialized_)
01760         {
01761                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01762                 return false;
01763         }
01764 
01765         geometry_msgs::Pose2D start_pose2D, end_pose2D, diff_pose2D;
01766 
01767         // TODO make this in a better way
01768         // - or work with pose2D right from the beginnning
01769         // convert quaternions to euler angles - at this point this no longer works in 3D !!
01770         PoseToPose2D(start_center_pose, start_pose2D);
01771         PoseToPose2D(end_center_pose, end_pose2D);
01772 
01773         // get rotational difference
01774         diff_pose2D.theta = end_pose2D.theta - start_pose2D.theta;
01775         diff_pose2D.theta = angles::normalize_angle(diff_pose2D.theta);
01776         // get translational difference
01777         diff_pose2D.x = end_pose2D.x - start_pose2D.x;
01778         diff_pose2D.y = end_pose2D.y - start_pose2D.y;
01779 
01780         difference.linear.x = diff_pose2D.x;
01781         difference.linear.y = diff_pose2D.y;
01782         difference.linear.z = 0.0;
01783         // multiply by inscribed radius to math calculation of distance
01784         difference.angular.x = 0.0;
01785         difference.angular.y = 0.0;
01786         difference.angular.z = diff_pose2D.theta*costmap_ros_->getCircumscribedRadius();
01787 
01788         // TODO take into account kinematic properties of body
01789 
01790         return true;
01791 }
01792 
01793 
01794 bool EBandPlanner::calcObstacleKinematicDistance(geometry_msgs::Pose center_pose, double& distance)
01795 {
01796         // calculate distance to nearest obstacle [depends kinematic, shape, environment]
01797 
01798         // check if plugin initialized
01799         if(!initialized_)
01800         {
01801                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01802                 return false;
01803         }
01804 
01805         unsigned int cell_x, cell_y;
01806         unsigned char disc_cost;
01807         double cont_cost;
01808 
01809         // read distance to nearest obstacle directly from costmap
01810         // (does not take into account shape and kinematic properties)
01811         // get cell for coordinates of bubble center
01812         if(!costmap_.worldToMap(center_pose.position.x, center_pose.position.y, cell_x, cell_y))
01813         {
01814                 // assume we are collision free but set distance to a very small value
01815                 distance = 0.001;
01816                 return true;
01817         }
01818 
01819         // get cost for this cell
01820         disc_cost = costmap_.getCost(cell_x, cell_y);
01821 
01822 
01823         // now it gets really dirty
01824         // TODO remove this as son as (costmap can output distances) possible --> get distance of center cell directly from costmap
01825         // calculate conservatively distance to nearest obstacel from this cost (see costmap_2d in wiki for details)
01826 
01827         // For reference: here comes an excerpt of the cost calculation within the costmap function
01828         /*if(distance == 0)
01829                 cost = LETHAL_OBSTACLE;
01830         else if(distance <= cell_inscribed_radius_)
01831                 cost = INSCRIBED_INFLATED_OBSTACLE;
01832         else {
01833                 //make sure cost falls off by Euclidean distance
01834                 double euclidean_distance = distance * resolution_;
01835                 double factor = exp(-1.0 * weight_ * (euclidean_distance - inscribed_radius_));
01836                 cost = (unsigned char) ((INSCRIBED_INFLATED_OBSTACLE - 1) * factor);
01837         }*/
01838 
01839         if(disc_cost >= 253) // 128 = cost_possibly_circumscribed // 253 = cost_inscribed_radius <-- from costmap_2D
01840         {
01841                 distance = 0.0;
01842                 return true;
01843         }
01844         else
01845         {
01846                 if(disc_cost <= 1)
01847                 {
01848                         // we are in "free space"! Here we wont get any distance measure from costmap.
01849                         // Upper (conservative) bound for cont_cost is in this case:
01850                         cont_cost = 1.0 / 253.0;
01851                 }
01852                 else
01853                 {
01854                         cont_cost = ((double) disc_cost)/253.0;
01855                 }
01856 
01857                 distance = -(log(cont_cost)/10.0); // 10.0 is the "scaling_factor" or "wheight" - see above its default is 10.0
01858                 // done with the really dirty part
01859         }
01860 
01861         return true;
01862 }
01863 
01864 
01865 // type conversions
01866 
01867 bool EBandPlanner::convertPlanToBand(std::vector<geometry_msgs::PoseStamped> plan, std::vector<Bubble>& band)
01868 {
01869         // check if plugin initialized
01870         if(!initialized_)
01871         {
01872                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01873                 return false;
01874         }
01875 
01876         // create local variables
01877         double distance = 0.0;
01878         std::vector<Bubble> tmp_band;
01879 
01880         ROS_DEBUG("Copying plan to band - Conversion started: %d frames to convert.", ((int) plan.size()) );
01881 
01882         // get local copy of referenced variable
01883         tmp_band = band;
01884 
01885         // adapt band to plan
01886         tmp_band.resize(plan.size());
01887         for(int i = 0; i < ((int) plan.size()); i++)
01888         {
01889                 #ifdef DEBUG_EBAND_
01890                 ROS_DEBUG("Checking Frame %d of %d", i, ((int) plan.size()) );
01891                 #endif
01892 
01893                 // set poses in plan as centers of bubbles
01894                 tmp_band[i].center = plan[i];
01895 
01896                 // calc Size of Bubbles by calculating Dist to nearest Obstacle [depends kinematic, environment]
01897                 if(!calcObstacleKinematicDistance(tmp_band[i].center.pose, distance))
01898                 {
01899                         // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
01900                         ROS_INFO("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d outside map", i, ((int) plan.size()) );
01901                         return false;
01902                 }
01903 
01904                 if(distance <= 0.0)
01905                 {
01906                         // frame must not be immediately in collision -> otherwise calculation of gradient will later be invalid
01907                         ROS_INFO("Calculation of Distance between bubble and nearest obstacle failed. Frame %d of %d in collision. Plan invalid", i, ((int) plan.size()) );
01908                         // TODO if frame in collision try to repair band instaed of aborting averything
01909                         return false;
01910                 }
01911 
01912 
01913                 // assign to expansion of bubble
01914                 tmp_band[i].expansion = distance;
01915         }
01916 
01917         // write to referenced variable
01918         band = tmp_band;
01919 
01920         ROS_DEBUG("Successfully converted plan to band");
01921         return true;
01922 }
01923 
01924 
01925 bool EBandPlanner::convertBandToPlan(std::vector<geometry_msgs::PoseStamped>& plan, std::vector<Bubble> band)
01926 {
01927         // check if plugin initialized
01928         if(!initialized_)
01929         {
01930                 ROS_ERROR("This planner has not been initialized, please call initialize() before using this planner");
01931                 return false;
01932         }
01933 
01934         // create local variables
01935         std::vector<geometry_msgs::PoseStamped> tmp_plan;
01936 
01937         // adapt plan to band
01938         tmp_plan.resize(band.size());
01939         for(int i = 0; i < ((int) band.size()); i++)
01940         {
01941                 // set centers of bubbles to StampedPose in plan
01942                 tmp_plan[i] = band[i].center;
01943         }
01944 
01945         //write to referenced variable and done
01946         plan = tmp_plan;
01947 
01948         return true;
01949 }
01950 
01951 
01952 }


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