BaseDistance.cc
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2009, Ingo Kresse <kresse@in.tum.de>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 #include <vector>
00032 #include <math.h>
00033 
00034 #include <stdio.h>
00035 
00036 #include <algorithm>
00037 #include <iterator>
00038 
00039 #include <boost/bind.hpp>
00040 
00041 #include "ros/ros.h"
00042 #include <sensor_msgs/LaserScan.h>
00043 #include <tf/transform_listener.h>
00044 #include <visualization_msgs/Marker.h>
00045 #include <visualization_msgs/MarkerArray.h>
00046 #include <std_msgs/Float64MultiArray.h>
00047 
00048 #include "BaseDistance.h"
00049 
00050 /* \todo: avoid unnecessary computation
00051 
00052   write standalone laser_bumper
00053 
00054   integrate
00055   test
00056   debug print about activity
00057 
00058   there are a lot of 'new' statements. -> performance problem?
00059 */
00060 
00061 
00062 #define MODE_FREE 0
00063 #define MODE_PROJECTING 1
00064 #define MODE_HARD_PROJECTING 2
00065 #define MODE_BRAKING 3
00066 #define MODE_REPELLING 4
00067 #define MODE_PROJECTION_MASK 3
00068 
00069 
00070 BaseDistance::BaseDistance()
00071   : n_("~")
00072 {
00073   d_ = 1 / 10.0;
00074   rob_x_ = rob_y_ = rob_th_ = 0.0;
00075   vx_last_ = vy_last_ = vth_last_ = 0.0;
00076 
00077   n_.param("marker_size", marker_size_, 0.1);
00078   n_.param("safety_dist", safety_dist_, 0.10);
00079   n_.param("slowdown_far", slowdown_far_, 0.30);
00080   n_.param("slowdown_near", slowdown_near_, 0.15);
00081   n_.param("repelling_dist", repelling_dist_, 0.20);
00082   n_.param("repelling_gain", repelling_gain_, 0.5);
00083   n_.param("repelling_gain_max", repelling_gain_max_, 0.015);
00084 
00085   n_.param<std::string>("odom_frame", odom_frame_, "/odom");
00086   n_.param<std::string>("base_link_frame", base_link_frame_, "/base_link");
00087 
00088   ROS_INFO("## safe=%f slow=%f..%f repell=%f repell_gain=%f\n", safety_dist_, slowdown_near_, slowdown_far_, repelling_dist_, repelling_gain_);
00089 
00090   n_.param("n_lasers", n_lasers_, 2);
00091   if(n_lasers_ < 1 || n_lasers_ > 2)
00092     ROS_FATAL("Only one or two lasers are supported. %d lasers specified.", n_lasers_);
00093 
00094   n_.param("complete_blind_spots", complete_blind_spots_, true);
00095   n_.param("blind_spot_threshold", blind_spot_threshold_, 0.85);
00096   
00097   marker_pub_ = n_.advertise<visualization_msgs::Marker>("/visualization_marker", 0);
00098   laser_points_pub_ = n_.advertise<sensor_msgs::PointCloud>("laser_points", 0);
00099   debug_pub_ = n_.advertise<std_msgs::Float64MultiArray>("debug_channels", 0);
00100 
00101   laser_subscriptions_[0] = n_.subscribe<sensor_msgs::LaserScan>
00102     ("laser_1", 2, boost::bind(&BaseDistance::laserCallback, this, 0, _1));
00103   if(n_lasers_ > 1)
00104     laser_subscriptions_[1] = n_.subscribe<sensor_msgs::LaserScan>
00105       ("laser_2", 2, boost::bind(&BaseDistance::laserCallback, this, 1, _1));
00106 
00107   calculateEarlyRejectDistance();
00108 }
00109 
00110 
00111 
00112 BaseDistance::Vector2 BaseDistance::transform(const Vector2 &v, double x, double y, double th)
00113 {
00114   return Vector2(v.x*cos(th) - v.y*sin(th) + x,
00115                  v.x*sin(th) + v.y*cos(th) + y);
00116 }
00117 
00118 
00119 void BaseDistance::setFootprint(double front, double rear, double left, double right, double tolerance)
00120 {
00121   ROS_INFO("setting footprint. x : %f .. %f     y : %f .. %f", rear, front, right, left);
00122 
00123   rear_ = rear;
00124   front_ = front;
00125   left_ = left;
00126   right_ = right;
00127   tolerance_ = tolerance;
00128 
00129   calculateEarlyRejectDistance();
00130 }
00131 
00132 void BaseDistance::calculateEarlyRejectDistance()
00133 {
00134   double movement_tolerance = 0.2;
00135   double diameter = sqrt((front_ - rear_)*(front_ - rear_) + (right_ - left_)*(right_ - left_));
00136   early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance;
00137 }
00138 
00139 
00140 bool BaseDistance::compute_pose2d(const char* from, const char* to, const ros::Time time, double *x, double *y, double *th)
00141 {
00142   tf::StampedTransform pose;
00143   try
00144   {
00145     tf_.lookupTransform(from, to, time, pose);
00146   }
00147   catch(tf::TransformException& ex)
00148   {
00149     ROS_WARN("no localization information yet (%s -> %s)", from, to);
00150     return false;
00151   }
00152 
00153   *x = pose.getOrigin().x();
00154   *y = pose.getOrigin().y();
00155 
00156   btMatrix3x3 m = pose.getBasis();
00157   *th = atan2(m[1][0], m[0][0]);
00158 
00159   return true;
00160 }
00161 
00162 void BaseDistance::laserCallback(int index, const sensor_msgs::LaserScan::ConstPtr &scan)
00163 {
00164   boost::shared_ptr<std::vector<Vector2> > points(new std::vector<Vector2>);
00165 
00166   points->reserve(scan->ranges.size());
00167 
00168   double x, y, th;
00169   compute_pose2d(odom_frame_.c_str(), scan->header.frame_id.c_str(), ros::Time(0), &x, &y, &th);
00170 
00171   double angle;
00172   int i, first_scan = 0, last_scan = 0;
00173 
00174   // find last valid scan range
00175   for(int i=scan->ranges.size()-1; i >= 0; i--)
00176   {
00177     if(!(scan->ranges[i] <= scan->range_min ||
00178        scan->ranges[i] >= scan->range_max))
00179     {
00180       last_scan = i;
00181       break;
00182     }
00183   }
00184 
00185 
00186   for(angle=scan->angle_min, i=0;
00187       i < (int) scan->ranges.size();
00188       i++, angle+=scan->angle_increment)
00189   {
00190     // reject invalid ranges
00191     if(scan->ranges[i] <= scan->range_min ||
00192        scan->ranges[i] >= scan->range_max)
00193     {
00194       first_scan++;
00195       continue;
00196     }
00197 
00198     // reject readings from far away
00199     if(scan->ranges[i] >= early_reject_distance_ && i != first_scan && i != last_scan)
00200       continue;
00201 
00202     first_scan = -1;
00203 
00204     double xl = scan->ranges[i] * cos(angle);
00205     double yl = scan->ranges[i] * sin(angle);
00206     points->push_back(transform(Vector2(xl, yl), x, y, th));
00207   }
00208 
00209   boost::mutex::scoped_lock mutex(lock);
00210   laser_points_[index] = points;
00211 
00212   if(complete_blind_spots_ && laser_points_[0] && laser_points_[1])
00213   {
00214     blind_spots_.clear();
00215 
00216     if(interpolateBlindPoints(10, laser_points_[0]->front(), laser_points_[1]->back()))
00217     {
00218       publishLaserMarker(laser_points_[0]->front(), "blind_spots", 0);
00219       publishLaserMarker(laser_points_[1]->back(),  "blind_spots", 1);
00220     }
00221 
00222     if(interpolateBlindPoints(10, laser_points_[0]->back(), laser_points_[1]->front()))
00223     {
00224       publishLaserMarker(laser_points_[0]->back(),  "blind_spots", 2);
00225       publishLaserMarker(laser_points_[1]->front(), "blind_spots", 3);
00226     }
00228   }
00229 }
00230 
00231 bool BaseDistance::interpolateBlindPoints(int n, const Vector2 &pt1, const Vector2 &pt2)
00232 {
00233   Vector2 p1_rob = transform(pt1, rob_x_, rob_y_, rob_th_);
00234   Vector2 p2_rob = transform(pt2, rob_x_, rob_y_, rob_th_);
00235 
00236   if( p1_rob.len2() < blind_spot_threshold_ * blind_spot_threshold_ ||
00237       p2_rob.len2() < blind_spot_threshold_ * blind_spot_threshold_)
00238   {
00239     for(int i=0; i < n; i++)
00240     {
00241       double t = (i/(n-1.0));
00242       blind_spots_.push_back(Vector2(t*pt1.x + (1.0-t)*pt2.x,
00243                                      t*pt1.y + (1.0-t)*pt2.y));
00244     }
00245     return true;
00246   }
00247   else
00248   {
00249     return false;
00250   }
00251 }
00252 
00253 void BaseDistance::publishLaserMarker(const Vector2 &pt, const std::string &ns, int id)
00254 {
00255   visualization_msgs::Marker marker;
00256   marker.header.frame_id = odom_frame_;
00257   marker.header.stamp = ros::Time::now();
00258   marker.ns = ns;
00259   marker.type = visualization_msgs::Marker::CUBE;
00260   marker.action = visualization_msgs::Marker::ADD;
00261   marker.pose.position.z = 0.55;
00262   marker.pose.orientation.x = 0.0;
00263   marker.pose.orientation.y = 0.0;
00264   marker.pose.orientation.z = 0.0;
00265   marker.pose.orientation.w = 1.0;
00266   marker.scale.x = marker_size_;
00267   marker.scale.y = marker_size_;
00268   marker.scale.z = marker_size_;
00269   marker.color.r = 1;
00270   marker.color.g = 0;
00271   marker.color.b = 0;
00272   marker.color.a = 1.0;
00273   marker.lifetime = ros::Duration(0.2);
00274   marker.id = id;
00275   marker.pose.position.x = pt.x;
00276   marker.pose.position.y = pt.y;
00277   marker.pose.orientation.w = 1.0;
00278   marker_pub_.publish(marker);
00279 }
00280 
00281 void BaseDistance::publishNearestPoint()
00282 {
00283   // visualize closest point
00284   visualization_msgs::Marker marker;
00285   marker.header.frame_id = base_link_frame_;
00286   marker.header.stamp = ros::Time::now();
00287   marker.ns = "nearest_point";
00288   marker.id = 0;
00289    marker.type = visualization_msgs::Marker::CUBE;
00290 
00291   if(mode_ & MODE_REPELLING)
00292     marker.type = visualization_msgs::Marker::SPHERE;
00293   else
00294     marker.type = visualization_msgs::Marker::CUBE;
00295 
00296   marker.action = visualization_msgs::Marker::ADD;
00297   marker.pose.position.x = nearest_.x;
00298   marker.pose.position.y = nearest_.y;
00299   marker.pose.position.z = 0.55;
00300   marker.pose.orientation.x = 0.0;
00301   marker.pose.orientation.y = 0.0;
00302   marker.pose.orientation.z = 0.0;
00303   marker.pose.orientation.w = 1.0;
00304   marker.scale.x = marker_size_*2;
00305   marker.scale.y = marker_size_*2;
00306   marker.scale.z = marker_size_*2;
00307 
00308   switch(mode_ & MODE_PROJECTION_MASK)
00309   {
00310     case(MODE_FREE):
00311       marker.color.r = 0.0f;
00312       marker.color.g = 1.0f;
00313       break;
00314     case(MODE_PROJECTING):
00315       marker.color.r = 0.0f;
00316       marker.color.g = 1.0f;
00317       break;
00318     case(MODE_HARD_PROJECTING):
00319       marker.color.r = 1.0f;
00320       marker.color.g = 0.5f;
00321       break;
00322     case(MODE_BRAKING):
00323       marker.color.r = 1.0f;
00324       marker.color.g = 0.0f;
00325       break;
00326   }
00327   marker.color.b = 0.0f;
00328   marker.color.a = 1.0;
00329 
00330   marker.lifetime = ros::Duration(0.1);
00331   marker_pub_.publish(marker);
00332 }
00333 
00334 void BaseDistance::publishBaseMarker()
00335 {
00336   visualization_msgs::Marker marker;
00337   marker.header.frame_id = base_link_frame_;
00338   marker.header.stamp = ros::Time::now();
00339   marker.ns = "base_footprint";
00340   marker.id = 0;
00341   marker.type = visualization_msgs::Marker::CUBE;
00342   marker.action = visualization_msgs::Marker::ADD;
00343   marker.pose.position.x = (front_ + rear_) * 0.5;
00344   marker.pose.position.y = (right_ + left_) * 0.5;
00345   marker.pose.position.z = 0.3;
00346   marker.pose.orientation.x = 0.0;
00347   marker.pose.orientation.y = 0.0;
00348   marker.pose.orientation.z = 0.0;
00349   marker.pose.orientation.w = 1.0;
00350   marker.scale.x = front_ - rear_;
00351   marker.scale.y = right_ - left_;
00352   marker.scale.z = 0.6;
00353   marker.color.r = 0.5f;
00354   marker.color.g = 0.5f;
00355   marker.color.b = 1.0f;
00356   marker.color.a = 0.5;
00357   marker.lifetime = ros::Duration();
00358   marker_pub_.publish(marker);
00359 }
00360 
00361 void BaseDistance::publishPoints(const std::vector<Vector2> &points)
00362 {
00363   sensor_msgs::PointCloud cloud;
00364 
00365   cloud.points.resize(points.size());
00366 
00367   std::vector<Vector2>::const_iterator src_it;
00368   sensor_msgs::PointCloud::_points_type::iterator dest_it;
00369 
00370   cloud.header.stamp = ros::Time::now();
00371   cloud.header.frame_id = odom_frame_;
00372   for(src_it=points.begin(), dest_it=cloud.points.begin();
00373       src_it != points.end();
00374       src_it++, dest_it++)
00375   {
00376     dest_it->x = src_it->x;
00377     dest_it->y = src_it->y;
00378     dest_it->z = 0.3;
00379   }
00380 
00381   // add one dummy point to clear the display
00382   if(points.size() == 0)
00383   {
00384     cloud.points.resize(1);
00385     cloud.points[0].x = 0.0;
00386     cloud.points[0].y = 0.0;
00387     cloud.points[0].z = 0.0;
00388   }
00389 
00390   laser_points_pub_.publish(cloud);
00391 }
00392 
00393 #define CLAMP(x, min, max) (x < min) ? min : ((x > max) ? max : x)
00394 
00395 double BaseDistance::grad(std::vector<Vector2> &points, double *gx, double *gy, double *gth)
00396 {
00397   if(!gx || !gy || !gth)
00398     return 0;
00399 
00400   double d0, dx_p, dy_p, dth_p, dx_n, dy_n, dth_n;
00401 
00402   double v_len = sqrt(vx_last_*vx_last_ + vy_last_*vy_last_ + vth_last_*vth_last_);
00403   double dd = CLAMP(v_len*d_*2, 0.005, 0.15);
00404 
00405   d0   = distance(points, &nearest_, 0, 0, 0);
00406 
00407   dx_p  = distance(points, 0,  dd,  0,  0);
00408   dx_n  = distance(points, 0, -dd,  0,  0);
00409 
00410   dy_p  = distance(points, 0,  0,  dd,  0);
00411   dy_n  = distance(points, 0,  0, -dd,  0);
00412 
00413   dth_p = distance(points, 0,  0,  0,  dd);
00414   dth_n = distance(points, 0,  0,  0, -dd);
00415 
00416   *gx  = (dx_p  - dx_n)  / (2.0 * dd);
00417   *gy  = (dy_p  - dy_n)  / (2.0 * dd);
00418   *gth = (dth_p - dth_n) / (2.0 * dd);
00419 
00420   // compute second derivatives for finding saddle points
00421   double g2x = (dx_p  + dx_n - 2*d0) / (dd*dd);
00422   double g2y = (dy_p  + dy_n - 2*d0) / (dd*dd);
00423   double g2th = (dth_p  + dth_n - 2*d0) / (dd*dd);
00424 
00425   //ROS_DEBUG("ddx=%f, ddy=%f, ddth=%f\n", ddx, ddy, ddth);
00426   //ROS_DEBUG("gx: %f (+- %f)  gy: %f (+-%f)  gth: %f (+- %f)\n", *gx, g2x, *gy, g2y, *gth, g2th);
00427 
00428   // "dampen" if second derivative is big
00429   double g2_scaledown = 2.0;
00430   double gx_damp = (fabs(g2x) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2x);
00431   double gy_damp = (fabs(g2y) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2y);
00432   double gth_damp = (fabs(g2th) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2th);
00433 
00434   *gx = *gx * gx_damp;
00435   *gy = *gy * gy_damp;
00436   *gth = *gth * gth_damp;
00437 
00438   std_msgs::Float64MultiArray msg;
00439   msg.data.resize(7);
00440   msg.data[0] = dd;
00441 
00442   msg.data[1] = *gx;
00443   msg.data[2] = *gy;
00444   msg.data[3] = *gth;
00445 
00446   msg.data[4] = g2x;
00447   msg.data[5] = g2y;
00448   msg.data[6] = g2th;
00449 
00450   debug_pub_.publish(msg);  
00451 
00452   return d0;
00453 }
00454 
00455 
00456 void BaseDistance::setSafetyLimits(double safety_dist, double slowdown_near, double slowdown_far, double rate)
00457 {
00458   d_ = 1.0 / rate;
00459   safety_dist_ = safety_dist;
00460   slowdown_near_ = slowdown_near;
00461   slowdown_far_ = slowdown_far;
00462 }
00463 
00464 
00465 double BaseDistance::brake(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
00466 {
00467   double factor = 1.5;
00468   double d = distance(points, 0, -*vx*d_*factor, -*vy*d_*factor, -*vth*d_*factor);
00469   if(d < safety_dist_) { // if we are stuck in the NEXT timestep...
00470     mode_ |= MODE_BRAKING;
00471     *vx = 0.0;
00472     *vy = 0.0;
00473     *vth = 0.0;
00474   }
00475   return d;
00476 }
00477 
00478 double BaseDistance::project(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
00479 {
00480   double d, gx, gy, gth, factor=0;
00481   d = grad(points, &gx, &gy, &gth);
00482 
00483   // normalize gradient
00484   double l_grad = 1/sqrt(gx*gx + gy*gy + gth*gth);
00485   gx*=l_grad;
00486   gy*=l_grad;
00487   gth*=l_grad;
00488 
00489   if(d < slowdown_far_) {
00490     // project (vx,vy,vth) onto (gx,gy,gth)
00491     double dp = *vx*gx + *vy*gy + *vth*gth;
00492     if(dp > 0) { // we are moving towards the obstacle
00493       if(d < slowdown_near_)
00494       {
00495         mode_ |= MODE_HARD_PROJECTING;
00496         factor = 1;
00497       }
00498       else
00499       {
00500         mode_ |= MODE_PROJECTING;
00501         factor = (d - slowdown_far_)/(slowdown_near_ - slowdown_far_);
00502       }
00503 
00504       *vx  -= gx *dp*factor;
00505       *vy  -= gy *dp*factor;
00506       *vth -= gth*dp*factor;
00507     }
00508   }
00509 
00510   if(d < repelling_dist_) {
00511     mode_ |= MODE_REPELLING;
00512     double l_grad_2d = 1/sqrt(gx*gx + gy*gy);
00513     double a = (1.0/d - 1.0/repelling_dist_)
00514              * (1.0/d - 1.0/repelling_dist_)
00515              * 0.5 * repelling_gain_;
00516 
00517     if(a > repelling_gain_max_)
00518       a = repelling_gain_max_;
00519     *vx -= gx * l_grad_2d * a;
00520     *vy -= gy * l_grad_2d * a;
00521   }
00522 
00523   return d;
00524 }
00525 
00526 void BaseDistance::compute_distance_keeping(double *vx, double *vy, double *vth)
00527 {
00528 
00529   // compute last known robot position in odom frame
00530   compute_pose2d(base_link_frame_.c_str(), odom_frame_.c_str(), ros::Time(0), &rob_x_, &rob_y_, &rob_th_);
00531 
00532   // collect all relevant obstacle points
00533   std::vector<Vector2> current_points;
00534 
00535   {
00536     boost::mutex::scoped_lock current_lock(lock);
00537 
00538     current_points.reserve((laser_points_[0] ? laser_points_[0]->size() : 0) +
00539                            (laser_points_[1] ? laser_points_[1]->size() : 0) +
00540                            blind_spots_.size());
00541 
00542     if(laser_points_[0])
00543     {
00544       std::copy(laser_points_[0]->begin(), laser_points_[0]->end(),
00545                 std::back_insert_iterator<std::vector<Vector2> >(current_points));
00546     }
00547     if(laser_points_[1])
00548     {
00549       std::copy(laser_points_[1]->begin(), laser_points_[1]->end(),
00550                 std::back_insert_iterator<std::vector<Vector2> >(current_points));
00551     }
00552     std::copy(blind_spots_.begin(), blind_spots_.end(),
00553               std::back_insert_iterator<std::vector<Vector2> >(current_points));
00554   }
00555 
00556   mode_ = MODE_FREE;
00557 
00558   // modify velocity vector vector
00559   project(current_points, vx, vy, vth);
00560 
00561   // if necessary, do braking
00562   brake(current_points, vx, vy, vth);
00563 
00564   vx_last_ = *vx;
00565   vy_last_ = *vy;
00566   vth_last_ = *vth;
00567 
00568   publishNearestPoint();
00569   publishBaseMarker();
00570   publishPoints(current_points);
00571 }
00572 
00573 #define LEFT 1
00574 #define RIGHT 2
00575 #define REAR 4
00576 #define FRONT 8
00577 
00578 double BaseDistance::distance(std::vector<Vector2> &points, Vector2 *nearest, double dx, double dy, double dth)
00579 {
00580   Vector2 rnearest(0, 0), lnearest(0, 0);
00581   double rqdistance=INFINITY, ldistance=INFINITY;
00582 
00583   int rarea, larea;
00584 
00585   if(points.size() == 0)
00586     ROS_WARN("No points received. Maybe the laser topic is wrong?");
00587 
00588   // transform from odom to base_link
00589   btMatrix3x3 rob(cos(rob_th_), -sin(rob_th_), rob_x_,
00590                   sin(rob_th_),  cos(rob_th_), rob_y_,
00591                   0,             0,            1);
00592 
00593   // this transform adds some adjustment
00594   btMatrix3x3 adj(cos(dth), -sin(dth), dx,
00595                   sin(dth),  cos(dth), dy,
00596                   0,         0,        1);
00597 
00598   // create transformation matrix
00599   btMatrix3x3 m = adj*rob;
00600 
00601   for(unsigned int i=0; i < points.size(); i++) {
00602     double px = points[i].x*m[0][0] + points[i].y*m[0][1] + m[0][2];
00603     double py = points[i].x*m[1][0] + points[i].y*m[1][1] + m[1][2];
00604 
00605     double dright = -py + right_;
00606     double dleft  =  py - left_;
00607     double drear  = -px + rear_;
00608     double dfront =  px - front_;
00609 
00610     int area=0;
00611     area |= RIGHT * (dright > -tolerance_);
00612     area |= LEFT  * (dleft  > -tolerance_);
00613     area |= REAR  * (drear  > -tolerance_);
00614     area |= FRONT * (dfront > -tolerance_);
00615 
00616     double rqdist=INFINITY, ldist=INFINITY;
00617     switch(area) {
00618     case(RIGHT): ldist = dright; break;
00619     case(LEFT):  ldist = dleft; break;
00620     case(REAR):  ldist = drear; break;
00621     case(FRONT): ldist = dfront; break;
00622 
00623     case(LEFT  | REAR):  rqdist = dleft*dleft + drear*drear; break;
00624     case(LEFT  | FRONT): rqdist = dleft*dleft + dfront*dfront; break;
00625     case(RIGHT | REAR):  rqdist = dright*dright + drear*drear; break;
00626     case(RIGHT | FRONT): rqdist = dright*dright + dfront*dfront; break;
00627     }
00628 
00629     if(ldist < ldistance) {
00630       ldistance = ldist;
00631       lnearest = Vector2(px, py);
00632       larea = area;
00633     }
00634 
00635     if(rqdist < rqdistance) {
00636       rqdistance = rqdist;
00637       rnearest = Vector2(px, py);
00638       rarea = area;
00639     }
00640   }
00641 
00642 
00643   double rdistance = sqrt(rqdistance);
00644   if(rdistance < ldistance) {
00645 
00646     //ROS_DEBUG("area=%d\n", rarea);
00647 
00648     if(nearest)
00649       *nearest = rnearest;
00650     return rdistance;
00651   } else {
00652 
00653     //ROS_DEBUG("area=%d\n", larea);
00654 
00655     if(nearest)
00656       *nearest = lnearest;
00657     return ldistance;
00658   }
00659 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Thu May 23 2013 09:13:23