homotopy_class_planner.hpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  * Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2016,
00006  *  TU Dortmund - Institute of Control Theory and Systems Engineering.
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of the institute nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * Author: Christoph Rösmann
00037  *********************************************************************/
00038 
00039 #include <teb_local_planner/homotopy_class_planner.h>
00040 
00041 namespace teb_local_planner
00042 {
00043   
00044   
00045 template<typename BidirIter, typename Fun>
00046 std::complex<long double> HomotopyClassPlanner::calculateHSignature(BidirIter path_start, BidirIter path_end, Fun fun_cplx_point, const ObstContainer* obstacles, double prescaler)
00047 {
00048     if (obstacles->empty()) 
00049       return std::complex<double>(0,0);
00050    
00051    
00052     ROS_ASSERT_MSG(prescaler>0.1 && prescaler<=1, "Only a prescaler on the interval (0.1,1] ist allowed.");
00053     
00054     // guess values for f0
00055     // paper proposes a+b=N-1 && |a-b|<=1, 1...N obstacles
00056     int m = obstacles->size()-1;
00057     
00058     int a = (int) std::ceil(double(m)/2.0);
00059     int b = m-a;
00060     
00061     std::advance(path_end, -1); // reduce path_end by 1 (since we check line segments between those path points
00062     
00063     typedef std::complex<long double> cplx;
00064     // guess map size (only a really really coarse guess is required
00065     // use distance from start to goal as distance to each direction
00066     // TODO: one could move the map determination outside this function, since it remains constant for the whole planning interval
00067     cplx start = fun_cplx_point(*path_start);
00068     cplx end = fun_cplx_point(*path_end); // path_end points to the last point now after calling std::advance before
00069     cplx delta = end-start;
00070     cplx normal(-delta.imag(), delta.real());
00071     cplx map_bottom_left;
00072     cplx map_top_right;
00073     if (std::abs(delta) < 3.0)
00074     { // set minimum bound on distance (we do not want to have numerical instabilities) and 3.0 performs fine...
00075       map_bottom_left = start + cplx(0, -3);
00076       map_top_right = start + cplx(3, 3);
00077     }
00078     else
00079     {
00080       map_bottom_left = start - normal;
00081       map_top_right = start + delta + normal;
00082     }
00083     
00084     cplx H = 0;
00085     std::vector<double> imag_proposals(5);
00086      
00087     // iterate path
00088     while(path_start != path_end)
00089     {
00090       cplx z1 = fun_cplx_point(*path_start);
00091       cplx z2 = fun_cplx_point(*boost::next(path_start));
00092 
00093       for (unsigned int l=0; l<obstacles->size(); ++l) // iterate all obstacles
00094       {
00095         cplx obst_l = obstacles->at(l)->getCentroidCplx();
00096         //cplx f0 = (long double) prescaler * std::pow(obst_l-map_bottom_left,a) * std::pow(obst_l-map_top_right,b);
00097         cplx f0 = (long double) prescaler * (long double)a*(obst_l-map_bottom_left) * (long double)b*(obst_l-map_top_right);
00098         
00099         // denum contains product with all obstacles exepct j==l
00100         cplx Al = f0;
00101         for (unsigned int j=0; j<obstacles->size(); ++j)
00102         {
00103           if (j==l) 
00104               continue;
00105           cplx obst_j = obstacles->at(j)->getCentroidCplx();
00106           cplx diff = obst_l - obst_j;
00107           //if (diff.real()!=0 || diff.imag()!=0)
00108           if (std::abs(diff)<0.05) // skip really close obstacles
00109               Al /= diff;
00110           else
00111             continue;
00112         }
00113         // compute log value
00114         double diff2 = std::abs(z2-obst_l);
00115         double diff1 = std::abs(z1-obst_l);
00116         if (diff2 == 0 || diff1 == 0)
00117           continue;
00118         double log_real = std::log(diff2)-std::log(diff1);
00119         // complex ln has more than one solution -> choose minimum abs angle -> paper
00120         double arg_diff = std::arg(z2-obst_l)-std::arg(z1-obst_l);
00121         imag_proposals.at(0) = arg_diff;
00122         imag_proposals.at(1) = arg_diff+2*M_PI;
00123         imag_proposals.at(2) = arg_diff-2*M_PI;
00124         imag_proposals.at(3) = arg_diff+4*M_PI;
00125         imag_proposals.at(4) = arg_diff-4*M_PI;
00126         double log_imag = *std::min_element(imag_proposals.begin(),imag_proposals.end(),smaller_than_abs);
00127         cplx log_value(log_real,log_imag);
00128         //cplx log_value = std::log(z2-obst_l)-std::log(z1-obst_l); // the principal solution doesn't seem to work
00129         H += Al*log_value;  
00130       }
00131       ++path_start;
00132     }
00133     return H;
00134 }
00135 
00136 
00137 template<typename BidirIter, typename Fun>
00138 void HomotopyClassPlanner::addAndInitNewTeb(BidirIter path_start, BidirIter path_end, Fun fun_position,
00139                                             double start_orientation, double goal_orientation, boost::optional<const Eigen::Vector2d&> start_velocity)
00140 {
00141   tebs_.push_back( TebOptimalPlannerPtr( new TebOptimalPlanner(*cfg_, obstacles_, robot_model_) ) );
00142   tebs_.back()->teb().initTEBtoGoal(path_start, path_end, fun_position, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, 
00143                                     cfg_->robot.acc_lim_x, cfg_->robot.acc_lim_theta, start_orientation, goal_orientation, cfg_->trajectory.min_samples);
00144   if (start_velocity)
00145     tebs_.back()->setVelocityStart(*start_velocity);
00146 }
00147   
00148 } // namespace teb_local_planner
00149 


teb_local_planner
Author(s): Christoph Rösmann
autogenerated on Mon Oct 24 2016 05:31:15