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 <stdio.h>
00032 
00033 #include <algorithm>
00034 #include <iterator>
00035 
00036 #include <boost/bind.hpp>
00037 
00038 #include <visualization_msgs/Marker.h>
00039 #include <visualization_msgs/MarkerArray.h>
00040 #include <std_msgs/Float64MultiArray.h>
00041 
00042 #include "nav_pcontroller/BaseDistance.h"
00043 
00057 #define MODE_FREE 0
00058 #define MODE_PROJECTING 1
00059 #define MODE_HARD_PROJECTING 2
00060 #define MODE_BRAKING 3
00061 #define MODE_REPELLING 4
00062 #define MODE_PROJECTION_MASK 3
00063 
00064 #if !ROS_VERSION_MINIMUM(1, 8, 0)
00065 namespace tf {
00066 typedef btMatrix3x3 Matrix3x3;
00067 }
00068 #endif
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 
00078   n_.param<std::string>("odom_frame", odom_frame_, "odom");
00079   n_.param<std::string>("base_link_frame", base_link_frame_, "base_link");
00080   n_.param("n_lasers", n_lasers_, 2);
00081   if(n_lasers_ < 1 || n_lasers_ > 2)
00082     ROS_FATAL("Only one or two lasers are supported. %d lasers specified.", n_lasers_);
00083   n_.param("slowdown_far", slowdown_far_, 0.30);
00084   n_.param("slowdown_near", slowdown_near_, 0.15);
00085   n_.param("safety_dist", safety_dist_, 0.10);
00086   n_.param("repelling_dist", repelling_dist_, 0.20);
00087   n_.param("repelling_gain", repelling_gain_, 0.5);
00088   n_.param("repelling_gain_max", repelling_gain_max_, 0.015);
00089   ROS_INFO("## safe=%f slow=%f..%f repell=%f repell_gain=%f\n", safety_dist_, slowdown_near_, slowdown_far_, repelling_dist_, repelling_gain_);
00090   n_.param("complete_blind_spots", complete_blind_spots_, true);
00091   n_.param("blind_spot_threshold", blind_spot_threshold_, 0.85);
00092   n_.param("marker_size", marker_size_, 0.1);
00093 
00094   marker_pub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 0);
00095   laser_points_pub_ = n_.advertise<sensor_msgs::PointCloud>("laser_points", 0);
00096   debug_pub_ = n_.advertise<std_msgs::Float64MultiArray>("debug_channels", 0);
00097 
00098   laser_subscriptions_[0] = n_.subscribe<sensor_msgs::LaserScan>
00099       ("laser_1", 2, boost::bind(&BaseDistance::laserCallback, this, 0, _1));
00100   if(n_lasers_ > 1)
00101     laser_subscriptions_[1] = n_.subscribe<sensor_msgs::LaserScan>
00102         ("laser_2", 2, boost::bind(&BaseDistance::laserCallback, this, 1, _1));
00103 
00104   calculateEarlyRejectDistance();
00105 }
00106 
00107 
00108 bool BaseDistance::fresh_scans(double watchdog_timeout)
00109 {
00110 
00111   ros::Time now = ros::Time::now();
00112   if (n_lasers_ == 1) {
00113     if (now - laser_0_last_msg_time_ < ros::Duration(watchdog_timeout))
00114       return true;
00115     else
00116       return false;
00117   }
00118 
00119   if (n_lasers_ == 2) {
00120     if ( (now - laser_0_last_msg_time_ < ros::Duration(watchdog_timeout)) 
00121      and (now - laser_1_last_msg_time_ < ros::Duration(watchdog_timeout)) )
00122       return true;
00123     else
00124       return false;
00125   }
00126 
00127 
00128 }
00129 
00130 
00131 void BaseDistance::calculateEarlyRejectDistance()
00132 {
00133   // tolerance in localization in meters, i.e. error due to movement?
00134   double movement_tolerance = 0.2;
00135   // diameter is the length of the diagonal of the footprint square
00136   double diameter = sqrt((front_ - rear_) * (front_ - rear_) + (right_ - left_) * (right_ - left_));
00137   early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance;
00138 }
00139 
00140 
00141 void BaseDistance::setFootprint(double front, double rear, double left, double right, double tolerance)
00142 {
00143   ROS_INFO("setting footprint. x : %f .. %f     y : %f .. %f", rear, front, right, left);
00144 
00145   rear_ = rear;
00146   front_ = front;
00147   left_ = left;
00148   right_ = right;
00149   tolerance_ = tolerance;
00150 
00151   calculateEarlyRejectDistance();
00152 }
00153 
00154 
00155 void BaseDistance::setSafetyLimits(double safety_dist, double slowdown_near, double slowdown_far, double rate)
00156 {
00157   d_ = 1.0 / rate;
00158   safety_dist_ = safety_dist;
00159   slowdown_near_ = slowdown_near;
00160   slowdown_far_ = slowdown_far;
00161 }
00162 
00163 
00164 BaseDistance::Vector2 BaseDistance::transform(const Vector2 &v, double x, double y, double th)
00165 {
00166   return Vector2(v.x*cos(th) - v.y*sin(th) + x,
00167                  v.x*sin(th) + v.y*cos(th) + y);
00168 }
00169 
00170 
00171 bool BaseDistance::compute_pose2d(const char* from, const char* to, const ros::Time time, double *x, double *y, double *th)
00172 {
00173   tf::StampedTransform pose;
00174   try
00175   {
00176     tf_.lookupTransform(from, to, time, pose);
00177   }
00178   catch(tf::TransformException& ex)
00179   {
00180     ROS_WARN("no localization information yet (%s -> %s)", from, to);
00181     return false;
00182   }
00183 
00184   *x = pose.getOrigin().x();
00185   *y = pose.getOrigin().y();
00186 
00187   tf::Matrix3x3 m = pose.getBasis();
00188   *th = atan2(m[1][0], m[0][0]);
00189 
00190   return true;
00191 }
00192 
00193 
00194 void BaseDistance::laserCallback(int index, const sensor_msgs::LaserScan::ConstPtr &scan)
00195 {
00196   // distance points from the laser, variable to store processed points
00197   boost::shared_ptr<std::vector<Vector2> > points(new std::vector<Vector2>);
00198 
00199   points->reserve(scan->ranges.size());
00200 
00201   // compute pose of laser in the odom frame
00202   double x, y, th;
00203   compute_pose2d(odom_frame_.c_str(), scan->header.frame_id.c_str(), ros::Time(0), &x, &y, &th);
00204 
00205   double angle;
00206   int i, first_scan = 0, last_scan = 0;
00207 
00208   // find last valid scan range
00209   // last_scan will be the index of the last valid scan point
00210   // (usually the scanner has a bit less than 270 deg range,
00211   //  so a couple of first and last points in the result are invalid values)
00212   for(int i = scan->ranges.size() - 1; i >= 0; i--)
00213   {
00214     if(scan->ranges[i] > scan->range_min &&
00215        scan->ranges[i] < scan->range_max)
00216     {
00217       last_scan = i;
00218       break;
00219     }
00220   }
00221 
00222   for(angle = scan->angle_min, i = 0;
00223       i < (int) scan->ranges.size();
00224       i++, angle += scan->angle_increment)
00225   {
00226     // reject invalid ranges
00227     if(scan->ranges[i] <= scan->range_min ||
00228        scan->ranges[i] >= scan->range_max)
00229     {
00230       first_scan++;
00231       continue;
00232     }
00233 
00234     // reject readings from far away
00235     if(scan->ranges[i] >= early_reject_distance_ && i != first_scan && i != last_scan)
00236       continue;
00237 
00238     first_scan = -1;
00239 
00240     // transforms the points from the laser frame into the odom frame
00241     double xl = scan->ranges[i] * cos(angle);
00242     double yl = scan->ranges[i] * sin(angle);
00243     points->push_back(transform(Vector2(xl, yl), x, y, th));
00244   }
00245 
00246   // boost::mutex::scoped_lock mutex(lock);
00247   // Commented out the mutex because it was unused.
00248   // There are callbacks from both lasers in parallel, each populates one element
00249   // of the size = 2 array laser_points_, which is thread safe.
00250   laser_points_[index] = points;
00251 
00252   // Fills in the 2 blind spots that happen when there are 2 lasers that don't
00253   // completely overlap. Also publishes the resulting filling as markers
00254   if(complete_blind_spots_ && laser_points_[0] && laser_points_[1])
00255   {
00256     blind_spots_.clear();
00257 
00258     if(interpolateBlindPoints(10, laser_points_[0]->front(), laser_points_[1]->back()))
00259     {
00260       publishLaserMarker(laser_points_[0]->front(), "blind_spots", 0);
00261       publishLaserMarker(laser_points_[1]->back(),  "blind_spots", 1);
00262     }
00263 
00264     if(interpolateBlindPoints(10, laser_points_[0]->back(), laser_points_[1]->front()))
00265     {
00266       publishLaserMarker(laser_points_[0]->back(),  "blind_spots", 2);
00267       publishLaserMarker(laser_points_[1]->front(), "blind_spots", 3);
00268     }
00270   }
00271 
00272   if (index == 0) {
00273     laser_0_last_msg_time_ = ros::Time(scan->header.stamp);
00274   } else if (index == 1) {
00275     laser_1_last_msg_time_ = ros::Time(scan->header.stamp);;
00276   }
00277 
00278 }
00279 
00280 
00281 bool BaseDistance::interpolateBlindPoints(int n, const Vector2 &pt1, const Vector2 &pt2)
00282 {
00283   // two points in base_link_frame_
00284   Vector2 p1_rob = transform(pt1, rob_x_, rob_y_, rob_th_);
00285   Vector2 p2_rob = transform(pt2, rob_x_, rob_y_, rob_th_);
00286 
00287   // if p1 or p2 are closer to the robot than a threshold
00288   if(p1_rob.len2() < blind_spot_threshold_ * blind_spot_threshold_ ||
00289      p2_rob.len2() < blind_spot_threshold_ * blind_spot_threshold_)
00290   {
00291     for(int i = 0; i < n; i++)
00292     {
00293       // linear interpolation on x and y separately
00294       double t = (i / (n - 1.0));
00295       blind_spots_.push_back(Vector2(t * pt1.x + (1.0 - t) * pt2.x,
00296                                      t * pt1.y + (1.0 - t) * pt2.y));
00297     }
00298     return true;
00299   }
00300   else
00301   {
00302     return false;
00303   }
00304 }
00305 
00306 
00307 #define LEFT 1
00308 #define RIGHT 2
00309 #define REAR 4
00310 #define FRONT 8
00311 
00312 double BaseDistance::distance(std::vector<Vector2> &points, Vector2 *nearest, double dx, double dy, double dth)
00313 {
00314   Vector2 rnearest(0, 0), lnearest(0, 0);
00315   double rqdistance=INFINITY, ldistance=INFINITY;
00316 
00317   //int rarea, larea;
00318 
00319   if(points.size() == 0)
00320     ROS_WARN("No points received. Maybe the laser topic is wrong?");
00321 
00322   // transformation matrix to convert points from odom_frame_ to base_link_frame_
00323   tf::Matrix3x3 rob(cos(rob_th_), -sin(rob_th_), rob_x_,
00324                     sin(rob_th_),  cos(rob_th_), rob_y_,
00325                     0,             0,            1);
00326 
00327   // this transform adds some adjustment
00328   tf::Matrix3x3 adj(cos(dth), -sin(dth), dx,
00329                     sin(dth),  cos(dth), dy,
00330                     0,         0,        1);
00331 
00332   // create transformation matrix
00333   tf::Matrix3x3 m = adj*rob;
00334 
00335   // The area around the robot is divided into areas, in front, on the left, left-front, etc.
00336   // For each point in points, calculate ldist as the distance on a straight line (FRONT, LEFT, etc.)
00337   // and rqdist as the distance^2 diagonally (LEFT and REAR, etc.)
00338   // The loop calculates the minimum of all ldist into ldistance, the corresponding
00339   // point is saved into lnearest, and of all rqdist the minimum is rqdistance with rnearest.
00340   for(unsigned int i=0; i < points.size(); i++) {
00341     // laser point in base_link_frame_ with some adjustment
00342     double px = points[i].x * m[0][0] + points[i].y * m[0][1] + m[0][2];
00343     double py = points[i].x * m[1][0] + points[i].y * m[1][1] + m[1][2];
00344 
00345     // perpendicular distance from the laser point to the four wall of base footprint
00346     double dright = -py + right_;
00347     double dleft  =  py - left_;
00348     double drear  = -px + rear_;
00349     double dfront =  px - front_;
00350 
00351     // area is a 4-bit mask that defines on which of 4 sides of the base the point is
00352     // there are, basically, 8 areas surrounding the robot that the point can be in
00353     int area = 0;
00354     // distance + tolerance > 0, if distance = -1, tolerance = 2, the sum > 0
00355     area |= RIGHT * (dright > -tolerance_);
00356     area |= LEFT  * (dleft  > -tolerance_);
00357     area |= REAR  * (drear  > -tolerance_);
00358     area |= FRONT * (dfront > -tolerance_);
00359 
00360     double rqdist=INFINITY, ldist=INFINITY;
00361     switch(area) {
00362     case(RIGHT): ldist = dright; break;
00363     case(LEFT):  ldist = dleft; break;
00364     case(REAR):  ldist = drear; break;
00365     case(FRONT): ldist = dfront; break;
00366 
00367     case(LEFT  | REAR):  rqdist = dleft*dleft + drear*drear; break;
00368     case(LEFT  | FRONT): rqdist = dleft*dleft + dfront*dfront; break;
00369     case(RIGHT | REAR):  rqdist = dright*dright + drear*drear; break;
00370     case(RIGHT | FRONT): rqdist = dright*dright + dfront*dfront; break;
00371     }
00372 
00373     if(ldist < ldistance) {
00374       ldistance = ldist;
00375       lnearest = Vector2(px, py);
00376       //larea = area;
00377     }
00378 
00379     if(rqdist < rqdistance) {
00380       rqdistance = rqdist;
00381       rnearest = Vector2(px, py);
00382       //rarea = area;
00383     }
00384   }
00385 
00386   // return the point that is the closes to the edges, either diagonally or straigh
00387   double rdistance = sqrt(rqdistance);
00388   if(rdistance < ldistance) {
00389     //ROS_DEBUG("area=%d\n", rarea);
00390     if(nearest)
00391       *nearest = rnearest;
00392     return rdistance;
00393   } else {
00394     //ROS_DEBUG("area=%d\n", larea);
00395     if(nearest)
00396       *nearest = lnearest;
00397     return ldistance;
00398   }
00399 }
00400 
00401 
00402 #define CLAMP(x, min, max) (x < min) ? min : ((x > max) ? max : x)
00403 
00404 double BaseDistance::grad(std::vector<Vector2> &points, double *gx, double *gy, double *gth)
00405 {
00406   if(!gx || !gy || !gth)
00407     return 0;
00408 
00409   double d0, dx_p, dy_p, dth_p, dx_n, dy_n, dth_n;
00410 
00411   // for very small distances angular velocity of radians per second can be
00412   // considered as meters per second? sinX = X for dt -> 0? no idea what's going on here
00413   double v_len = sqrt(vx_last_*vx_last_ + vy_last_*vy_last_ + vth_last_*vth_last_);
00414 
00415   // dd -> distance traveled in one time frame d_ with current velocity
00416   double dd = CLAMP(v_len*d_*2, 0.005, 0.15);
00417 
00418   // distance to nearest point from current robot pose
00419   d0   = distance(points, &nearest_, 0, 0, 0);
00420 
00421   // distance to nearest point from robot pose if robot moved +/- dd in X, Y or Theta in @a d_
00422   dx_p  = distance(points, 0,  dd,  0,  0);
00423   dx_n  = distance(points, 0, -dd,  0,  0);
00424 
00425   dy_p  = distance(points, 0,  0,  dd,  0);
00426   dy_n  = distance(points, 0,  0, -dd,  0);
00427 
00428   dth_p = distance(points, 0,  0,  0,  dd);
00429   dth_n = distance(points, 0,  0,  0, -dd);
00430 
00431   *gx  = (dx_p  - dx_n)  / (2.0 * dd);
00432   *gy  = (dy_p  - dy_n)  / (2.0 * dd);
00433   *gth = (dth_p - dth_n) / (2.0 * dd);
00434 
00435   // compute second derivatives for finding saddle points
00436   double g2x = (dx_p  + dx_n - 2*d0) / (dd*dd);
00437   double g2y = (dy_p  + dy_n - 2*d0) / (dd*dd);
00438   double g2th = (dth_p  + dth_n - 2*d0) / (dd*dd);
00439 
00440   //ROS_DEBUG("ddx=%f, ddy=%f, ddth=%f\n", ddx, ddy, ddth);
00441   //ROS_DEBUG("gx: %f (+- %f)  gy: %f (+-%f)  gth: %f (+- %f)\n", *gx, g2x, *gy, g2y, *gth, g2th);
00442 
00443   // "dampen" if second derivative is big
00444   double g2_scaledown = 2.0;
00445   double gx_damp = (fabs(g2x) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2x);
00446   double gy_damp = (fabs(g2y) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2y);
00447   double gth_damp = (fabs(g2th) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2th);
00448 
00449   *gx = *gx * gx_damp;
00450   *gy = *gy * gy_damp;
00451   *gth = *gth * gth_damp;
00452 
00453   std_msgs::Float64MultiArray msg;
00454   msg.data.resize(7);
00455   msg.data[0] = dd;
00456 
00457   msg.data[1] = *gx;
00458   msg.data[2] = *gy;
00459   msg.data[3] = *gth;
00460 
00461   msg.data[4] = g2x;
00462   msg.data[5] = g2y;
00463   msg.data[6] = g2th;
00464 
00465   debug_pub_.publish(msg);
00466 
00467   return d0;
00468 }
00469 
00470 
00471 double BaseDistance::brake(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
00472 {
00473   double factor = 1.5;
00474   double d = distance(points, 0, -*vx*d_*factor, -*vy*d_*factor, -*vth*d_*factor);
00475   if(d < safety_dist_) { // if we are stuck in the NEXT timestep...
00476     mode_ |= MODE_BRAKING;
00477     *vx = 0.0;
00478     *vy = 0.0;
00479     *vth = 0.0;
00480   }
00481   return d;
00482 }
00483 
00484 
00485 double BaseDistance::project(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
00486 {
00487   double d, gx, gy, gth, factor = 0;
00488   d = grad(points, &gx, &gy, &gth);
00489 
00490   // normalize gradient
00491   double l_grad = 1/sqrt(gx*gx + gy*gy + gth*gth);
00492   gx*=l_grad;
00493   gy*=l_grad;
00494   gth*=l_grad;
00495 
00496   if(d < slowdown_far_) {
00497     // project (vx,vy,vth) onto (gx,gy,gth)
00498     double dp = *vx*gx + *vy*gy + *vth*gth;
00499     if(dp > 0) { // we are moving towards the obstacle
00500       if(d < slowdown_near_)
00501       {
00502         mode_ |= MODE_HARD_PROJECTING;
00503         factor = 1;
00504       }
00505       else
00506       {
00507         mode_ |= MODE_PROJECTING;
00508         factor = (d - slowdown_far_)/(slowdown_near_ - slowdown_far_);
00509       }
00510 
00511       *vx  -= gx *dp*factor;
00512       *vy  -= gy *dp*factor;
00513       *vth -= gth*dp*factor;
00514     }
00515   }
00516 
00517   if(d < repelling_dist_) {
00518     mode_ |= MODE_REPELLING;
00519     double l_grad_2d = 1/sqrt(gx*gx + gy*gy);
00520     double a = (1.0/d - 1.0/repelling_dist_)
00521         * (1.0/d - 1.0/repelling_dist_)
00522         * 0.5 * repelling_gain_;
00523 
00524     if(a > repelling_gain_max_)
00525       a = repelling_gain_max_;
00526     *vx -= gx * l_grad_2d * a;
00527     *vy -= gy * l_grad_2d * a;
00528   }
00529 
00530   return d;
00531 }
00532 
00533 
00534 void BaseDistance::compute_distance_keeping(double *vx, double *vy, double *vth)
00535 {
00536 
00537   // compute last known robot position in odom frame
00538   compute_pose2d(base_link_frame_.c_str(), odom_frame_.c_str(), ros::Time(0), &rob_x_, &rob_y_, &rob_th_);
00539 
00540   // collect all relevant obstacle points: from both lasers and blind spots, if given
00541   std::vector<Vector2> current_points;
00542 
00543   {
00544     // boost::mutex::scoped_lock current_lock(lock);
00545     // No need for mutexes as the laser points are a C array
00546 
00547     current_points.reserve((laser_points_[0] ? laser_points_[0]->size() : 0) +
00548                            (laser_points_[1] ? laser_points_[1]->size() : 0) +
00549                            blind_spots_.size());
00550 
00551     if(laser_points_[0])
00552     {
00553       std::copy(laser_points_[0]->begin(), laser_points_[0]->end(),
00554           std::back_insert_iterator<std::vector<Vector2> >(current_points));
00555     }
00556     if(laser_points_[1])
00557     {
00558       std::copy(laser_points_[1]->begin(), laser_points_[1]->end(),
00559           std::back_insert_iterator<std::vector<Vector2> >(current_points));
00560     }
00561     std::copy(blind_spots_.begin(), blind_spots_.end(),
00562               std::back_insert_iterator<std::vector<Vector2> >(current_points));
00563   }
00564 
00565   mode_ = MODE_FREE;
00566 
00567   // modify velocity vector (if too close it will start braking or backing up)
00568   project(current_points, vx, vy, vth);
00569 
00570   // if necessary, do braking (also adjusts the velocity and mode_)
00571   brake(current_points, vx, vy, vth);
00572 
00573   vx_last_ = *vx;
00574   vy_last_ = *vy;
00575   vth_last_ = *vth;
00576 
00577   publishNearestPoint();
00578   publishBaseMarker();
00579   publishPoints(current_points);
00580 }
00581 
00582 
00583 void BaseDistance::publishLaserMarker(const Vector2 &pt, const std::string &ns, int id)
00584 {
00585   visualization_msgs::Marker marker;
00586   marker.header.frame_id = odom_frame_;
00587   marker.header.stamp = ros::Time::now();
00588   marker.ns = ns;
00589   marker.type = visualization_msgs::Marker::CUBE;
00590   marker.action = visualization_msgs::Marker::ADD;
00591   marker.pose.position.z = 0.55;
00592   marker.pose.orientation.x = 0.0;
00593   marker.pose.orientation.y = 0.0;
00594   marker.pose.orientation.z = 0.0;
00595   marker.pose.orientation.w = 1.0;
00596   marker.scale.x = marker_size_;
00597   marker.scale.y = marker_size_;
00598   marker.scale.z = marker_size_;
00599   marker.color.r = 1;
00600   marker.color.g = 0;
00601   marker.color.b = 0;
00602   marker.color.a = 1.0;
00603   marker.lifetime = ros::Duration(0.2);
00604   marker.id = id;
00605   marker.pose.position.x = pt.x;
00606   marker.pose.position.y = pt.y;
00607   marker.pose.orientation.w = 1.0;
00608   marker_pub_.publish(marker);
00609 }
00610 
00611 
00612 void BaseDistance::publishNearestPoint()
00613 {
00614   // visualize closest point
00615   visualization_msgs::Marker marker;
00616   marker.header.frame_id = base_link_frame_;
00617   marker.header.stamp = ros::Time::now();
00618   marker.ns = "nearest_point";
00619   marker.id = 0;
00620   //marker.type = visualization_msgs::Marker::CUBE;
00621   if(mode_ & MODE_REPELLING)
00622     marker.type = visualization_msgs::Marker::SPHERE;
00623   else
00624     marker.type = visualization_msgs::Marker::CUBE;
00625 
00626   marker.action = visualization_msgs::Marker::ADD;
00627   marker.pose.position.x = nearest_.x;
00628   marker.pose.position.y = nearest_.y;
00629   marker.pose.position.z = 0.55;
00630   marker.pose.orientation.x = 0.0;
00631   marker.pose.orientation.y = 0.0;
00632   marker.pose.orientation.z = 0.0;
00633   marker.pose.orientation.w = 1.0;
00634   marker.scale.x = marker_size_*2;
00635   marker.scale.y = marker_size_*2;
00636   marker.scale.z = marker_size_*2;
00637 
00638   switch(mode_ & MODE_PROJECTION_MASK)
00639   {
00640   case(MODE_FREE):
00641     marker.color.r = 0.0f;
00642     marker.color.g = 1.0f;
00643     break;
00644   case(MODE_PROJECTING):
00645     marker.color.r = 0.0f;
00646     marker.color.g = 1.0f;
00647     break;
00648   case(MODE_HARD_PROJECTING):
00649     marker.color.r = 1.0f;
00650     marker.color.g = 0.5f;
00651     break;
00652   case(MODE_BRAKING):
00653     marker.color.r = 1.0f;
00654     marker.color.g = 0.0f;
00655     break;
00656   }
00657   marker.color.b = 0.0f;
00658   marker.color.a = 1.0;
00659 
00660   marker.lifetime = ros::Duration(0.1);
00661   marker_pub_.publish(marker);
00662 }
00663 
00664 
00665 void BaseDistance::publishBaseMarker()
00666 {
00667   visualization_msgs::Marker marker;
00668   marker.header.frame_id = base_link_frame_;
00669   marker.header.stamp = ros::Time::now();
00670   marker.ns = "base_footprint";
00671   marker.id = 0;
00672   marker.type = visualization_msgs::Marker::CUBE;
00673   marker.action = visualization_msgs::Marker::ADD;
00674   marker.pose.position.x = (front_ + rear_) * 0.5;
00675   marker.pose.position.y = (right_ + left_) * 0.5;
00676   marker.pose.position.z = 0.3;
00677   marker.pose.orientation.x = 0.0;
00678   marker.pose.orientation.y = 0.0;
00679   marker.pose.orientation.z = 0.0;
00680   marker.pose.orientation.w = 1.0;
00681   marker.scale.x = front_ - rear_;
00682   marker.scale.y = right_ - left_;
00683   marker.scale.z = 0.6;
00684   marker.color.r = 0.5f;
00685   marker.color.g = 0.5f;
00686   marker.color.b = 1.0f;
00687   marker.color.a = 0.5;
00688   marker.lifetime = ros::Duration();
00689   marker_pub_.publish(marker);
00690 }
00691 
00692 
00693 void BaseDistance::publishPoints(const std::vector<Vector2> &points)
00694 {
00695   sensor_msgs::PointCloud cloud;
00696 
00697   cloud.points.resize(points.size());
00698 
00699   std::vector<Vector2>::const_iterator src_it;
00700   sensor_msgs::PointCloud::_points_type::iterator dest_it;
00701 
00702   cloud.header.stamp = ros::Time::now();
00703   cloud.header.frame_id = odom_frame_;
00704   for(src_it=points.begin(), dest_it=cloud.points.begin();
00705       src_it != points.end();
00706       src_it++, dest_it++)
00707   {
00708     dest_it->x = src_it->x;
00709     dest_it->y = src_it->y;
00710     dest_it->z = 0.3;
00711   }
00712 
00713   // add one dummy point to clear the display
00714   if(points.size() == 0)
00715   {
00716     cloud.points.resize(1);
00717     cloud.points[0].x = 0.0;
00718     cloud.points[0].y = 0.0;
00719     cloud.points[0].z = 0.0;
00720   }
00721 
00722   laser_points_pub_.publish(cloud);
00723 }


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Wed Aug 2 2017 02:30:35