Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
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
00055
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);
00062
00063 typedef std::complex<long double> cplx;
00064
00065
00066
00067 cplx start = fun_cplx_point(*path_start);
00068 cplx end = fun_cplx_point(*path_end);
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 {
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
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)
00094 {
00095 cplx obst_l = obstacles->at(l)->getCentroidCplx();
00096
00097 cplx f0 = (long double) prescaler * (long double)a*(obst_l-map_bottom_left) * (long double)b*(obst_l-map_top_right);
00098
00099
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
00108 if (std::abs(diff)<0.05)
00109 Al /= diff;
00110 else
00111 continue;
00112 }
00113
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
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
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 }
00149