BaseDistance.cc
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Ingo Kresse <kresse@in.tum.de>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 
31 #include <stdio.h>
32 
33 #include <algorithm>
34 #include <iterator>
35 
36 #include <boost/bind.hpp>
37 
38 #include <visualization_msgs/Marker.h>
39 #include <visualization_msgs/MarkerArray.h>
40 #include <std_msgs/Float64MultiArray.h>
41 
43 
57 #define MODE_FREE 0
58 #define MODE_PROJECTING 1
59 #define MODE_HARD_PROJECTING 2
60 #define MODE_BRAKING 3
61 #define MODE_REPELLING 4
62 #define MODE_PROJECTION_MASK 3
63 
64 #if !ROS_VERSION_MINIMUM(1, 8, 0)
65 namespace tf {
66 typedef btMatrix3x3 Matrix3x3;
67 }
68 #endif
69 
71  : n_("~")
72 {
73  d_ = 1 / 10.0;
74  rob_x_ = rob_y_ = rob_th_ = 0.0;
75  vx_last_ = vy_last_ = vth_last_ = 0.0;
76 
77  n_.param<std::string>("odom_frame", odom_frame_, "odom");
78  n_.param<std::string>("base_link_frame", base_link_frame_, "base_link");
79  n_.param("n_lasers", n_lasers_, 2);
80  if(n_lasers_ < 1 || n_lasers_ > 2)
81  ROS_FATAL("Only one or two lasers are supported. %d lasers specified.", n_lasers_);
82  n_.param("slowdown_far", slowdown_far_, 0.30);
83  n_.param("slowdown_near", slowdown_near_, 0.15);
84  n_.param("safety_dist", safety_dist_, 0.10);
85  n_.param("repelling_dist", repelling_dist_, 0.20);
86  n_.param("repelling_gain", repelling_gain_, 0.5);
87  n_.param("repelling_gain_max", repelling_gain_max_, 0.015);
88  ROS_INFO("## safe=%f slow=%f..%f repell=%f repell_gain=%f\n", safety_dist_, slowdown_near_, slowdown_far_, repelling_dist_, repelling_gain_);
89  n_.param("complete_blind_spots", complete_blind_spots_, true);
90  n_.param("blind_spot_threshold", blind_spot_threshold_, 0.85);
91  n_.param("marker_size", marker_size_, 0.1);
92 
93  marker_pub_ = n_.advertise<visualization_msgs::Marker>("/visualization_marker", 0);
94  laser_points_pub_ = n_.advertise<sensor_msgs::PointCloud>("laser_points", 0);
95  debug_pub_ = n_.advertise<std_msgs::Float64MultiArray>("debug_channels", 0);
96 
97  laser_subscriptions_[0] = n_.subscribe<sensor_msgs::LaserScan>
98  ("laser_1", 2, boost::bind(&BaseDistance::laserCallback, this, 0, _1));
99  if(n_lasers_ > 1)
100  laser_subscriptions_[1] = n_.subscribe<sensor_msgs::LaserScan>
101  ("laser_2", 2, boost::bind(&BaseDistance::laserCallback, this, 1, _1));
102 
104 }
105 
106 
108 {
109  // tolerance in localization in meters, i.e. error due to movement?
110  double movement_tolerance = 0.2;
111  // diameter is the length of the diagonal of the footprint square
112  double diameter = sqrt((front_ - rear_) * (front_ - rear_) + (right_ - left_) * (right_ - left_));
113  early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance;
114 }
115 
116 
117 void BaseDistance::setFootprint(double front, double rear, double left, double right, double tolerance)
118 {
119  ROS_INFO("setting footprint. x : %f .. %f y : %f .. %f", rear, front, right, left);
120 
121  rear_ = rear;
122  front_ = front;
123  left_ = left;
124  right_ = right;
125  tolerance_ = tolerance;
126 
128 }
129 
130 
131 void BaseDistance::setSafetyLimits(double safety_dist, double slowdown_near, double slowdown_far, double rate)
132 {
133  d_ = 1.0 / rate;
134  safety_dist_ = safety_dist;
135  slowdown_near_ = slowdown_near;
136  slowdown_far_ = slowdown_far;
137 }
138 
139 
140 BaseDistance::Vector2 BaseDistance::transform(const Vector2 &v, double x, double y, double th)
141 {
142  return Vector2(v.x*cos(th) - v.y*sin(th) + x,
143  v.x*sin(th) + v.y*cos(th) + y);
144 }
145 
146 
147 bool BaseDistance::compute_pose2d(const char* from, const char* to, const ros::Time time, double *x, double *y, double *th)
148 {
149  tf::StampedTransform pose;
150  try
151  {
152  tf_.lookupTransform(from, to, time, pose);
153  }
154  catch(tf::TransformException& ex)
155  {
156  ROS_WARN("no localization information yet (%s -> %s)", from, to);
157  return false;
158  }
159 
160  *x = pose.getOrigin().x();
161  *y = pose.getOrigin().y();
162 
163  tf::Matrix3x3 m = pose.getBasis();
164  *th = atan2(m[1][0], m[0][0]);
165 
166  return true;
167 }
168 
169 
170 void BaseDistance::laserCallback(int index, const sensor_msgs::LaserScan::ConstPtr &scan)
171 {
172  // distance points from the laser, variable to store processed points
173  boost::shared_ptr<std::vector<Vector2> > points(new std::vector<Vector2>);
174 
175  points->reserve(scan->ranges.size());
176 
177  // compute pose of laser in the odom frame
178  double x, y, th;
179  compute_pose2d(odom_frame_.c_str(), scan->header.frame_id.c_str(), ros::Time(0), &x, &y, &th);
180 
181  double angle;
182  int i, first_scan = 0, last_scan = 0;
183 
184  // find last valid scan range
185  // last_scan will be the index of the last valid scan point
186  // (usually the scanner has a bit less than 270 deg range,
187  // so a couple of first and last points in the result are invalid values)
188  for(int i = scan->ranges.size() - 1; i >= 0; i--)
189  {
190  if(scan->ranges[i] > scan->range_min &&
191  scan->ranges[i] < scan->range_max)
192  {
193  last_scan = i;
194  break;
195  }
196  }
197 
198  for(angle = scan->angle_min, i = 0;
199  i < (int) scan->ranges.size();
200  i++, angle += scan->angle_increment)
201  {
202  // reject invalid ranges
203  if(scan->ranges[i] <= scan->range_min ||
204  scan->ranges[i] >= scan->range_max)
205  {
206  first_scan++;
207  continue;
208  }
209 
210  // reject readings from far away
211  if(scan->ranges[i] >= early_reject_distance_ && i != first_scan && i != last_scan)
212  continue;
213 
214  first_scan = -1;
215 
216  // transforms the points from the laser frame into the odom frame
217  double xl = scan->ranges[i] * cos(angle);
218  double yl = scan->ranges[i] * sin(angle);
219  points->push_back(transform(Vector2(xl, yl), x, y, th));
220  }
221 
222  // boost::mutex::scoped_lock mutex(lock);
223  // Commented out the mutex because it was unused.
224  // There are callbacks from both lasers in parallel, each populates one element
225  // of the size = 2 array laser_points_, which is thread safe.
226  laser_points_[index] = points;
227 
228  // Fills in the 2 blind spots that happen when there are 2 lasers that don't
229  // completely overlap. Also publishes the resulting filling as markers
231  {
232  blind_spots_.clear();
233 
234  if(interpolateBlindPoints(10, laser_points_[0]->front(), laser_points_[1]->back()))
235  {
236  publishLaserMarker(laser_points_[0]->front(), "blind_spots", 0);
237  publishLaserMarker(laser_points_[1]->back(), "blind_spots", 1);
238  }
239 
240  if(interpolateBlindPoints(10, laser_points_[0]->back(), laser_points_[1]->front()))
241  {
242  publishLaserMarker(laser_points_[0]->back(), "blind_spots", 2);
243  publishLaserMarker(laser_points_[1]->front(), "blind_spots", 3);
244  }
246  }
247 }
248 
249 
250 bool BaseDistance::interpolateBlindPoints(int n, const Vector2 &pt1, const Vector2 &pt2)
251 {
252  // two points in base_link_frame_
253  Vector2 p1_rob = transform(pt1, rob_x_, rob_y_, rob_th_);
254  Vector2 p2_rob = transform(pt2, rob_x_, rob_y_, rob_th_);
255 
256  // if p1 or p2 are closer to the robot than a threshold
259  {
260  for(int i = 0; i < n; i++)
261  {
262  // linear interpolation on x and y separately
263  double t = (i / (n - 1.0));
264  blind_spots_.push_back(Vector2(t * pt1.x + (1.0 - t) * pt2.x,
265  t * pt1.y + (1.0 - t) * pt2.y));
266  }
267  return true;
268  }
269  else
270  {
271  return false;
272  }
273 }
274 
275 
276 #define LEFT 1
277 #define RIGHT 2
278 #define REAR 4
279 #define FRONT 8
280 
281 double BaseDistance::distance(std::vector<Vector2> &points, Vector2 *nearest, double dx, double dy, double dth)
282 {
283  Vector2 rnearest(0, 0), lnearest(0, 0);
284  double rqdistance=INFINITY, ldistance=INFINITY;
285 
286  //int rarea, larea;
287 
288  if(points.size() == 0)
289  ROS_WARN("No points received. Maybe the laser topic is wrong?");
290 
291  // transformation matrix to convert points from odom_frame_ to base_link_frame_
292  tf::Matrix3x3 rob(cos(rob_th_), -sin(rob_th_), rob_x_,
293  sin(rob_th_), cos(rob_th_), rob_y_,
294  0, 0, 1);
295 
296  // this transform adds some adjustment
297  tf::Matrix3x3 adj(cos(dth), -sin(dth), dx,
298  sin(dth), cos(dth), dy,
299  0, 0, 1);
300 
301  // create transformation matrix
302  tf::Matrix3x3 m = adj*rob;
303 
304  // The area around the robot is divided into areas, in front, on the left, left-front, etc.
305  // For each point in points, calculate ldist as the distance on a straight line (FRONT, LEFT, etc.)
306  // and rqdist as the distance^2 diagonally (LEFT and REAR, etc.)
307  // The loop calculates the minimum of all ldist into ldistance, the corresponding
308  // point is saved into lnearest, and of all rqdist the minimum is rqdistance with rnearest.
309  for(unsigned int i=0; i < points.size(); i++) {
310  // laser point in base_link_frame_ with some adjustment
311  double px = points[i].x * m[0][0] + points[i].y * m[0][1] + m[0][2];
312  double py = points[i].x * m[1][0] + points[i].y * m[1][1] + m[1][2];
313 
314  // perpendicular distance from the laser point to the four wall of base footprint
315  double dright = -py + right_;
316  double dleft = py - left_;
317  double drear = -px + rear_;
318  double dfront = px - front_;
319 
320  // area is a 4-bit mask that defines on which of 4 sides of the base the point is
321  // there are, basically, 8 areas surrounding the robot that the point can be in
322  int area = 0;
323  // distance + tolerance > 0, if distance = -1, tolerance = 2, the sum > 0
324  area |= RIGHT * (dright > -tolerance_);
325  area |= LEFT * (dleft > -tolerance_);
326  area |= REAR * (drear > -tolerance_);
327  area |= FRONT * (dfront > -tolerance_);
328 
329  double rqdist=INFINITY, ldist=INFINITY;
330  switch(area) {
331  case(RIGHT): ldist = dright; break;
332  case(LEFT): ldist = dleft; break;
333  case(REAR): ldist = drear; break;
334  case(FRONT): ldist = dfront; break;
335 
336  case(LEFT | REAR): rqdist = dleft*dleft + drear*drear; break;
337  case(LEFT | FRONT): rqdist = dleft*dleft + dfront*dfront; break;
338  case(RIGHT | REAR): rqdist = dright*dright + drear*drear; break;
339  case(RIGHT | FRONT): rqdist = dright*dright + dfront*dfront; break;
340  }
341 
342  if(ldist < ldistance) {
343  ldistance = ldist;
344  lnearest = Vector2(px, py);
345  //larea = area;
346  }
347 
348  if(rqdist < rqdistance) {
349  rqdistance = rqdist;
350  rnearest = Vector2(px, py);
351  //rarea = area;
352  }
353  }
354 
355  // return the point that is the closes to the edges, either diagonally or straigh
356  double rdistance = sqrt(rqdistance);
357  if(rdistance < ldistance) {
358  //ROS_DEBUG("area=%d\n", rarea);
359  if(nearest)
360  *nearest = rnearest;
361  return rdistance;
362  } else {
363  //ROS_DEBUG("area=%d\n", larea);
364  if(nearest)
365  *nearest = lnearest;
366  return ldistance;
367  }
368 }
369 
370 
371 #define CLAMP(x, min, max) (x < min) ? min : ((x > max) ? max : x)
372 
373 double BaseDistance::grad(std::vector<Vector2> &points, double *gx, double *gy, double *gth)
374 {
375  if(!gx || !gy || !gth)
376  return 0;
377 
378  double d0, dx_p, dy_p, dth_p, dx_n, dy_n, dth_n;
379 
380  // for very small distances angular velocity of radians per second can be
381  // considered as meters per second? sinX = X for dt -> 0? no idea what's going on here
382  double v_len = sqrt(vx_last_*vx_last_ + vy_last_*vy_last_ + vth_last_*vth_last_);
383 
384  // dd -> distance traveled in one time frame d_ with current velocity
385  double dd = CLAMP(v_len*d_*2, 0.005, 0.15);
386 
387  // distance to nearest point from current robot pose
388  d0 = distance(points, &nearest_, 0, 0, 0);
389 
390  // distance to nearest point from robot pose if robot moved +/- dd in X, Y or Theta in @a d_
391  dx_p = distance(points, 0, dd, 0, 0);
392  dx_n = distance(points, 0, -dd, 0, 0);
393 
394  dy_p = distance(points, 0, 0, dd, 0);
395  dy_n = distance(points, 0, 0, -dd, 0);
396 
397  dth_p = distance(points, 0, 0, 0, dd);
398  dth_n = distance(points, 0, 0, 0, -dd);
399 
400  *gx = (dx_p - dx_n) / (2.0 * dd);
401  *gy = (dy_p - dy_n) / (2.0 * dd);
402  *gth = (dth_p - dth_n) / (2.0 * dd);
403 
404  // compute second derivatives for finding saddle points
405  double g2x = (dx_p + dx_n - 2*d0) / (dd*dd);
406  double g2y = (dy_p + dy_n - 2*d0) / (dd*dd);
407  double g2th = (dth_p + dth_n - 2*d0) / (dd*dd);
408 
409  //ROS_DEBUG("ddx=%f, ddy=%f, ddth=%f\n", ddx, ddy, ddth);
410  //ROS_DEBUG("gx: %f (+- %f) gy: %f (+-%f) gth: %f (+- %f)\n", *gx, g2x, *gy, g2y, *gth, g2th);
411 
412  // "dampen" if second derivative is big
413  double g2_scaledown = 2.0;
414  double gx_damp = (fabs(g2x) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2x);
415  double gy_damp = (fabs(g2y) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2y);
416  double gth_damp = (fabs(g2th) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2th);
417 
418  *gx = *gx * gx_damp;
419  *gy = *gy * gy_damp;
420  *gth = *gth * gth_damp;
421 
422  std_msgs::Float64MultiArray msg;
423  msg.data.resize(7);
424  msg.data[0] = dd;
425 
426  msg.data[1] = *gx;
427  msg.data[2] = *gy;
428  msg.data[3] = *gth;
429 
430  msg.data[4] = g2x;
431  msg.data[5] = g2y;
432  msg.data[6] = g2th;
433 
434  debug_pub_.publish(msg);
435 
436  return d0;
437 }
438 
439 
440 double BaseDistance::brake(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
441 {
442  double factor = 1.5;
443  double d = distance(points, 0, -*vx*d_*factor, -*vy*d_*factor, -*vth*d_*factor);
444  if(d < safety_dist_) { // if we are stuck in the NEXT timestep...
445  mode_ |= MODE_BRAKING;
446  *vx = 0.0;
447  *vy = 0.0;
448  *vth = 0.0;
449  }
450  return d;
451 }
452 
453 
454 double BaseDistance::project(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
455 {
456  double d, gx, gy, gth, factor = 0;
457  d = grad(points, &gx, &gy, &gth);
458 
459  // normalize gradient
460  double l_grad = 1/sqrt(gx*gx + gy*gy + gth*gth);
461  gx*=l_grad;
462  gy*=l_grad;
463  gth*=l_grad;
464 
465  if(d < slowdown_far_) {
466  // project (vx,vy,vth) onto (gx,gy,gth)
467  double dp = *vx*gx + *vy*gy + *vth*gth;
468  if(dp > 0) { // we are moving towards the obstacle
469  if(d < slowdown_near_)
470  {
472  factor = 1;
473  }
474  else
475  {
477  factor = (d - slowdown_far_)/(slowdown_near_ - slowdown_far_);
478  }
479 
480  *vx -= gx *dp*factor;
481  *vy -= gy *dp*factor;
482  *vth -= gth*dp*factor;
483  }
484  }
485 
486  if(d < repelling_dist_) {
488  double l_grad_2d = 1/sqrt(gx*gx + gy*gy);
489  double a = (1.0/d - 1.0/repelling_dist_)
490  * (1.0/d - 1.0/repelling_dist_)
491  * 0.5 * repelling_gain_;
492 
493  if(a > repelling_gain_max_)
495  *vx -= gx * l_grad_2d * a;
496  *vy -= gy * l_grad_2d * a;
497  }
498 
499  return d;
500 }
501 
502 
503 void BaseDistance::compute_distance_keeping(double *vx, double *vy, double *vth)
504 {
505 
506  // compute last known robot position in odom frame
507  compute_pose2d(base_link_frame_.c_str(), odom_frame_.c_str(), ros::Time(0), &rob_x_, &rob_y_, &rob_th_);
508 
509  // collect all relevant obstacle points: from both lasers and blind spots, if given
510  std::vector<Vector2> current_points;
511 
512  {
513  // boost::mutex::scoped_lock current_lock(lock);
514  // No need for mutexes as the laser points are a C array
515 
516  current_points.reserve((laser_points_[0] ? laser_points_[0]->size() : 0) +
517  (laser_points_[1] ? laser_points_[1]->size() : 0) +
518  blind_spots_.size());
519 
520  if(laser_points_[0])
521  {
522  std::copy(laser_points_[0]->begin(), laser_points_[0]->end(),
523  std::back_insert_iterator<std::vector<Vector2> >(current_points));
524  }
525  if(laser_points_[1])
526  {
527  std::copy(laser_points_[1]->begin(), laser_points_[1]->end(),
528  std::back_insert_iterator<std::vector<Vector2> >(current_points));
529  }
530  std::copy(blind_spots_.begin(), blind_spots_.end(),
531  std::back_insert_iterator<std::vector<Vector2> >(current_points));
532  }
533 
534  mode_ = MODE_FREE;
535 
536  // modify velocity vector (if too close it will start braking or backing up)
537  project(current_points, vx, vy, vth);
538 
539  // if necessary, do braking (also adjusts the velocity and mode_)
540  brake(current_points, vx, vy, vth);
541 
542  vx_last_ = *vx;
543  vy_last_ = *vy;
544  vth_last_ = *vth;
545 
548  publishPoints(current_points);
549 }
550 
551 
552 void BaseDistance::publishLaserMarker(const Vector2 &pt, const std::string &ns, int id)
553 {
554  visualization_msgs::Marker marker;
555  marker.header.frame_id = odom_frame_;
556  marker.header.stamp = ros::Time::now();
557  marker.ns = ns;
558  marker.type = visualization_msgs::Marker::CUBE;
559  marker.action = visualization_msgs::Marker::ADD;
560  marker.pose.position.z = 0.55;
561  marker.pose.orientation.x = 0.0;
562  marker.pose.orientation.y = 0.0;
563  marker.pose.orientation.z = 0.0;
564  marker.pose.orientation.w = 1.0;
565  marker.scale.x = marker_size_;
566  marker.scale.y = marker_size_;
567  marker.scale.z = marker_size_;
568  marker.color.r = 1;
569  marker.color.g = 0;
570  marker.color.b = 0;
571  marker.color.a = 1.0;
572  marker.lifetime = ros::Duration(0.2);
573  marker.id = id;
574  marker.pose.position.x = pt.x;
575  marker.pose.position.y = pt.y;
576  marker.pose.orientation.w = 1.0;
577  marker_pub_.publish(marker);
578 }
579 
580 
582 {
583  // visualize closest point
584  visualization_msgs::Marker marker;
585  marker.header.frame_id = base_link_frame_;
586  marker.header.stamp = ros::Time::now();
587  marker.ns = "nearest_point";
588  marker.id = 0;
589  //marker.type = visualization_msgs::Marker::CUBE;
590  if(mode_ & MODE_REPELLING)
591  marker.type = visualization_msgs::Marker::SPHERE;
592  else
593  marker.type = visualization_msgs::Marker::CUBE;
594 
595  marker.action = visualization_msgs::Marker::ADD;
596  marker.pose.position.x = nearest_.x;
597  marker.pose.position.y = nearest_.y;
598  marker.pose.position.z = 0.55;
599  marker.pose.orientation.x = 0.0;
600  marker.pose.orientation.y = 0.0;
601  marker.pose.orientation.z = 0.0;
602  marker.pose.orientation.w = 1.0;
603  marker.scale.x = marker_size_*2;
604  marker.scale.y = marker_size_*2;
605  marker.scale.z = marker_size_*2;
606 
607  switch(mode_ & MODE_PROJECTION_MASK)
608  {
609  case(MODE_FREE):
610  marker.color.r = 0.0f;
611  marker.color.g = 1.0f;
612  break;
613  case(MODE_PROJECTING):
614  marker.color.r = 0.0f;
615  marker.color.g = 1.0f;
616  break;
617  case(MODE_HARD_PROJECTING):
618  marker.color.r = 1.0f;
619  marker.color.g = 0.5f;
620  break;
621  case(MODE_BRAKING):
622  marker.color.r = 1.0f;
623  marker.color.g = 0.0f;
624  break;
625  }
626  marker.color.b = 0.0f;
627  marker.color.a = 1.0;
628 
629  marker.lifetime = ros::Duration(0.1);
630  marker_pub_.publish(marker);
631 }
632 
633 
635 {
636  visualization_msgs::Marker marker;
637  marker.header.frame_id = base_link_frame_;
638  marker.header.stamp = ros::Time::now();
639  marker.ns = "base_footprint";
640  marker.id = 0;
641  marker.type = visualization_msgs::Marker::CUBE;
642  marker.action = visualization_msgs::Marker::ADD;
643  marker.pose.position.x = (front_ + rear_) * 0.5;
644  marker.pose.position.y = (right_ + left_) * 0.5;
645  marker.pose.position.z = 0.3;
646  marker.pose.orientation.x = 0.0;
647  marker.pose.orientation.y = 0.0;
648  marker.pose.orientation.z = 0.0;
649  marker.pose.orientation.w = 1.0;
650  marker.scale.x = front_ - rear_;
651  marker.scale.y = right_ - left_;
652  marker.scale.z = 0.6;
653  marker.color.r = 0.5f;
654  marker.color.g = 0.5f;
655  marker.color.b = 1.0f;
656  marker.color.a = 0.5;
657  marker.lifetime = ros::Duration();
658  marker_pub_.publish(marker);
659 }
660 
661 
662 void BaseDistance::publishPoints(const std::vector<Vector2> &points)
663 {
664  sensor_msgs::PointCloud cloud;
665 
666  cloud.points.resize(points.size());
667 
668  std::vector<Vector2>::const_iterator src_it;
669  sensor_msgs::PointCloud::_points_type::iterator dest_it;
670 
671  cloud.header.stamp = ros::Time::now();
672  cloud.header.frame_id = odom_frame_;
673  for(src_it=points.begin(), dest_it=cloud.points.begin();
674  src_it != points.end();
675  src_it++, dest_it++)
676  {
677  dest_it->x = src_it->x;
678  dest_it->y = src_it->y;
679  dest_it->z = 0.3;
680  }
681 
682  // add one dummy point to clear the display
683  if(points.size() == 0)
684  {
685  cloud.points.resize(1);
686  cloud.points[0].x = 0.0;
687  cloud.points[0].y = 0.0;
688  cloud.points[0].z = 0.0;
689  }
690 
691  laser_points_pub_.publish(cloud);
692 }
The Vector2 helper class to store 2D coordinates.
Definition: BaseDistance.h:87
double repelling_gain_max_
Definition: BaseDistance.h:195
#define MODE_FREE
Definition: BaseDistance.cc:57
double slowdown_far_
slowdown_far_ Distance from footprint edges to closest obstacle point from when to start slowing ...
Definition: BaseDistance.h:180
double brake(std::vector< Vector2 > &points, double *vx, double *vy, double *vth)
If distance to closest point when moving with given velocity is short, brake. Given velocity here is ...
#define MODE_PROJECTION_MASK
Definition: BaseDistance.cc:62
#define Vector2
BaseDistance()
Gets the parameters from ROS parameter server and initializes arguments with that.
Definition: BaseDistance.cc:70
#define FRONT
int mode_
mode_ Defined the movement mode for the base. Can be one of the modes defined in the cpp file: MODE_F...
Definition: BaseDistance.h:255
void calculateEarlyRejectDistance()
early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance; ...
double vx_last_
vx_last_, vy_last_, vth_last_ Robot velocity to drive with in next time frame
Definition: BaseDistance.h:244
ros::Publisher laser_points_pub_
Definition: BaseDistance.h:266
Vector2 transform(const Vector2 &v, double x, double y, double th)
Applies ridig transformation to the input vector, i.e. rotation and translation I.e. gets transformed into a coordinate system defined by x, y and th.
double safety_dist_
safety_dist_ Distance in meters when to brake when moving with current velocity.
Definition: BaseDistance.h:190
double distance(std::vector< Vector2 > &points, Vector2 *nearest, double dx=0, double dy=0, double dth=0)
Calculates the distance to closest of points from nearest footprint wall The distance is calculated n...
#define LEFT
void setFootprint(double front, double rear, double left, double right, double tolerance)
Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_.
btMatrix3x3 Matrix3x3
Definition: BaseDistance.cc:66
void publishNearestPoint()
Publishes nearest_ as a cube or a sphere of corresponding color nearest_ is defined in base_link_fram...
double vth_last_
Definition: BaseDistance.h:244
ros::Subscriber laser_subscriptions_[2]
Definition: BaseDistance.h:271
ros::Publisher debug_pub_
Definition: BaseDistance.h:267
double rob_x_
rob_x_, rob_y_, rob_th_ Pose of the base_link_frame_ in odom_frame_
Definition: BaseDistance.h:239
#define CLAMP(x, min, max)
void publishLaserMarker(const Vector2 &pt, const std::string &ns, int id=0)
Publishes a red cube at the coordinates of pt using marker_pub_.
void publishPoints(const std::vector< Vector2 > &points)
Publishes coordinates in points as a point cloud Uses laser_points_pub_ to publish.
ros::NodeHandle n_
Definition: BaseDistance.h:258
bool compute_pose2d(const char *from, const char *to, const ros::Time time, double *x, double *y, double *th)
Computes the translation and rotation from one frame to another.
std::string base_link_frame_
Definition: BaseDistance.h:152
int n_lasers_
n_lasers_ Number of laser scanners that create the base scan. Note: laser_points_ is of size 2...
Definition: BaseDistance.h:163
#define REAR
double project(std::vector< Vector2 > &points, double *vx, double *vy, double *vth)
Adapts the velocity given in parameters to slow down or back up Adjusts mode_, vx, vy and vth.
double tolerance_
tolerance_ Error in the footprint measurements in meters.
Definition: BaseDistance.h:175
#define MODE_PROJECTING
Definition: BaseDistance.cc:58
void setSafetyLimits(double safety_dist, double slowdown_near, double slowdown_far, double rate)
Sets safety_dist_, slowdown_near_, slowdown_far_, and d_ = 1 / rate.
boost::shared_ptr< std::vector< Vector2 > > laser_points_[2]
laser_points_ Data from lasers filtered and converted into the odom frame. The size is 2 for 2 lasers...
Definition: BaseDistance.h:228
void publishBaseMarker()
Publishes a purple cube at the position of the robot in odom_frame_ The size of the cube is the size ...
bool complete_blind_spots_
complete_blind_spots_ If the laser/s have blind spots, wheter to complete them. The completion will b...
Definition: BaseDistance.h:208
bool interpolateBlindPoints(int n, const Vector2 &pt1, const Vector2 &pt2)
Interpolates n points between pt1 and pt2 and adds them to blind_spots_ Interpolation is linear and c...
#define RIGHT
void compute_distance_keeping(double *vx, double *vy, double *vth)
Sets vx_last_, vy_last and vth_last_ to input parameters, adjusting them.
double front_
Footprint of the robot, coordinates of front edge, back edge, etc. in the footprint coordinate system...
Definition: BaseDistance.h:170
tf::TransformListener tf_
Definition: BaseDistance.h:269
double d_
d_ duration of one time frame, i.e. dt or Tdot
Definition: BaseDistance.h:157
double slowdown_near_
slowdown_near_ Distance from footprint edges to closest obstacle point from when to slow aggressively...
Definition: BaseDistance.h:185
double rob_th_
Definition: BaseDistance.h:239
#define MODE_REPELLING
Definition: BaseDistance.cc:61
double repelling_gain_
Definition: BaseDistance.h:195
std::vector< Vector2 > blind_spots_
blind_spots_ A vector of interpolated coordinates for the blind spots Defined in the odom_frame_ ...
Definition: BaseDistance.h:222
std::string odom_frame_
Definition: BaseDistance.h:152
double marker_size_
marker_size_ When publishing points to RViz, in meters
Definition: BaseDistance.h:263
Vector2 nearest_
nearest_ The nearest point to the base
Definition: BaseDistance.h:234
double blind_spot_threshold_
blind_spot_threshold_ Distance from base_link_frame to the end of a blind spot. Non-parallel lines al...
Definition: BaseDistance.h:216
ros::Publisher marker_pub_
Definition: BaseDistance.h:265
double vy_last_
Definition: BaseDistance.h:244
double grad(std::vector< Vector2 > &points, double *gx, double *gy, double *gth)
Calculates the gradient of the distance to the closes point of points Is it increasing? Is it decreasing? How fast?
#define MODE_BRAKING
Definition: BaseDistance.cc:60
#define MODE_HARD_PROJECTING
Definition: BaseDistance.cc:59
double early_reject_distance_
early_reject_distance_ From how many meters to discard laser measurements. Laser is used to avoid obs...
Definition: BaseDistance.h:202
double repelling_dist_
repelling_dist_ Distance from footprint edges to closest obstacle point from when to start backing up...
Definition: BaseDistance.h:195
void laserCallback(int index, const sensor_msgs::LaserScan::ConstPtr &scan)
laserCallback Sets the laser_points_ to filtered data from lasers in the odom frame. Completes blind spots if complete_blind_spots is set to true.


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Mon Jul 18 2016 16:50:55