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 
78  n_.param<std::string>("odom_frame", odom_frame_, "odom");
79  n_.param<std::string>("base_link_frame", base_link_frame_, "base_link");
80  n_.param("n_lasers", n_lasers_, 2);
81  if(n_lasers_ < 1 || n_lasers_ > 2)
82  ROS_FATAL("Only one or two lasers are supported. %d lasers specified.", n_lasers_);
83  n_.param("slowdown_far", slowdown_far_, 0.30);
84  n_.param("slowdown_near", slowdown_near_, 0.15);
85  n_.param("safety_dist", safety_dist_, 0.10);
86  n_.param("repelling_dist", repelling_dist_, 0.20);
87  n_.param("repelling_gain", repelling_gain_, 0.5);
88  n_.param("repelling_gain_max", repelling_gain_max_, 0.015);
89  ROS_INFO("## safe=%f slow=%f..%f repell=%f repell_gain=%f\n", safety_dist_, slowdown_near_, slowdown_far_, repelling_dist_, repelling_gain_);
90  n_.param("complete_blind_spots", complete_blind_spots_, true);
91  n_.param("blind_spot_threshold", blind_spot_threshold_, 0.85);
92  n_.param("marker_size", marker_size_, 0.1);
93 
94  marker_pub_ = n_.advertise<visualization_msgs::Marker>("visualization_marker", 0);
95  laser_points_pub_ = n_.advertise<sensor_msgs::PointCloud>("laser_points", 0);
96  debug_pub_ = n_.advertise<std_msgs::Float64MultiArray>("debug_channels", 0);
97 
98  laser_subscriptions_[0] = n_.subscribe<sensor_msgs::LaserScan>
99  ("laser_1", 2, boost::bind(&BaseDistance::laserCallback, this, 0, _1));
100  if(n_lasers_ > 1)
101  laser_subscriptions_[1] = n_.subscribe<sensor_msgs::LaserScan>
102  ("laser_2", 2, boost::bind(&BaseDistance::laserCallback, this, 1, _1));
103 
105 }
106 
107 
108 bool BaseDistance::fresh_scans(double watchdog_timeout)
109 {
110 
111  ros::Time now = ros::Time::now();
112  if (n_lasers_ == 1) {
113  if (now - laser_0_last_msg_time_ < ros::Duration(watchdog_timeout))
114  return true;
115  else
116  return false;
117  }
118 
119  if (n_lasers_ == 2) {
120  if ( (now - laser_0_last_msg_time_ < ros::Duration(watchdog_timeout))
121  and (now - laser_1_last_msg_time_ < ros::Duration(watchdog_timeout)) )
122  return true;
123  else
124  return false;
125  }
126 
127 
128 }
129 
130 
132 {
133  // tolerance in localization in meters, i.e. error due to movement?
134  double movement_tolerance = 0.2;
135  // diameter is the length of the diagonal of the footprint square
136  double diameter = sqrt((front_ - rear_) * (front_ - rear_) + (right_ - left_) * (right_ - left_));
137  early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance;
138 }
139 
140 
141 void BaseDistance::setFootprint(double front, double rear, double left, double right, double tolerance)
142 {
143  ROS_INFO("setting footprint. x : %f .. %f y : %f .. %f", rear, front, right, left);
144 
145  rear_ = rear;
146  front_ = front;
147  left_ = left;
148  right_ = right;
149  tolerance_ = tolerance;
150 
152 }
153 
154 
155 void BaseDistance::setSafetyLimits(double safety_dist, double slowdown_near, double slowdown_far, double rate)
156 {
157  d_ = 1.0 / rate;
158  safety_dist_ = safety_dist;
159  slowdown_near_ = slowdown_near;
160  slowdown_far_ = slowdown_far;
161 }
162 
163 
164 BaseDistance::Vector2 BaseDistance::transform(const Vector2 &v, double x, double y, double th)
165 {
166  return Vector2(v.x*cos(th) - v.y*sin(th) + x,
167  v.x*sin(th) + v.y*cos(th) + y);
168 }
169 
170 
171 bool BaseDistance::compute_pose2d(const char* from, const char* to, const ros::Time time, double *x, double *y, double *th)
172 {
174  try
175  {
176  tf_.lookupTransform(from, to, time, pose);
177  }
178  catch(tf::TransformException& ex)
179  {
180  ROS_WARN("no localization information yet (%s -> %s)", from, to);
181  return false;
182  }
183 
184  *x = pose.getOrigin().x();
185  *y = pose.getOrigin().y();
186 
187  tf::Matrix3x3 m = pose.getBasis();
188  *th = atan2(m[1][0], m[0][0]);
189 
190  return true;
191 }
192 
193 
194 void BaseDistance::laserCallback(int index, const sensor_msgs::LaserScan::ConstPtr &scan)
195 {
196  // distance points from the laser, variable to store processed points
197  boost::shared_ptr<std::vector<Vector2> > points(new std::vector<Vector2>);
198 
199  points->reserve(scan->ranges.size());
200 
201  // compute pose of laser in the odom frame
202  double x, y, th;
203  compute_pose2d(odom_frame_.c_str(), scan->header.frame_id.c_str(), ros::Time(0), &x, &y, &th);
204 
205  double angle;
206  int i, first_scan = 0, last_scan = 0;
207 
208  // find last valid scan range
209  // last_scan will be the index of the last valid scan point
210  // (usually the scanner has a bit less than 270 deg range,
211  // so a couple of first and last points in the result are invalid values)
212  for(int i = scan->ranges.size() - 1; i >= 0; i--)
213  {
214  if(scan->ranges[i] > scan->range_min &&
215  scan->ranges[i] < scan->range_max)
216  {
217  last_scan = i;
218  break;
219  }
220  }
221 
222  for(angle = scan->angle_min, i = 0;
223  i < (int) scan->ranges.size();
224  i++, angle += scan->angle_increment)
225  {
226  // reject invalid ranges
227  if(scan->ranges[i] <= scan->range_min ||
228  scan->ranges[i] >= scan->range_max)
229  {
230  first_scan++;
231  continue;
232  }
233 
234  // reject readings from far away
235  if(scan->ranges[i] >= early_reject_distance_ && i != first_scan && i != last_scan)
236  continue;
237 
238  first_scan = -1;
239 
240  // transforms the points from the laser frame into the odom frame
241  double xl = scan->ranges[i] * cos(angle);
242  double yl = scan->ranges[i] * sin(angle);
243  points->push_back(transform(Vector2(xl, yl), x, y, th));
244  }
245 
246  // boost::mutex::scoped_lock mutex(lock);
247  // Commented out the mutex because it was unused.
248  // There are callbacks from both lasers in parallel, each populates one element
249  // of the size = 2 array laser_points_, which is thread safe.
250  laser_points_[index] = points;
251 
252  // Fills in the 2 blind spots that happen when there are 2 lasers that don't
253  // completely overlap. Also publishes the resulting filling as markers
255  {
256  blind_spots_.clear();
257 
258  if(interpolateBlindPoints(10, laser_points_[0]->front(), laser_points_[1]->back()))
259  {
260  publishLaserMarker(laser_points_[0]->front(), "blind_spots", 0);
261  publishLaserMarker(laser_points_[1]->back(), "blind_spots", 1);
262  }
263 
264  if(interpolateBlindPoints(10, laser_points_[0]->back(), laser_points_[1]->front()))
265  {
266  publishLaserMarker(laser_points_[0]->back(), "blind_spots", 2);
267  publishLaserMarker(laser_points_[1]->front(), "blind_spots", 3);
268  }
270  }
271 
272  if (index == 0) {
273  laser_0_last_msg_time_ = ros::Time(scan->header.stamp);
274  } else if (index == 1) {
275  laser_1_last_msg_time_ = ros::Time(scan->header.stamp);;
276  }
277 
278 }
279 
280 
281 bool BaseDistance::interpolateBlindPoints(int n, const Vector2 &pt1, const Vector2 &pt2)
282 {
283  // two points in base_link_frame_
284  Vector2 p1_rob = transform(pt1, rob_x_, rob_y_, rob_th_);
285  Vector2 p2_rob = transform(pt2, rob_x_, rob_y_, rob_th_);
286 
287  // if p1 or p2 are closer to the robot than a threshold
290  {
291  for(int i = 0; i < n; i++)
292  {
293  // linear interpolation on x and y separately
294  double t = (i / (n - 1.0));
295  blind_spots_.push_back(Vector2(t * pt1.x + (1.0 - t) * pt2.x,
296  t * pt1.y + (1.0 - t) * pt2.y));
297  }
298  return true;
299  }
300  else
301  {
302  return false;
303  }
304 }
305 
306 
307 #define LEFT 1
308 #define RIGHT 2
309 #define REAR 4
310 #define FRONT 8
311 
312 double BaseDistance::distance(std::vector<Vector2> &points, Vector2 *nearest, double dx, double dy, double dth)
313 {
314  // TODO: don't take one single closest point, take 10 and average out somehow
315 
316  Vector2 rnearest(0, 0), lnearest(0, 0);
317  double rqdistance=INFINITY, ldistance=INFINITY;
318 
319  //int rarea, larea;
320 
321  if(points.size() == 0)
322  ROS_WARN("No points received. Maybe the laser topic is wrong?");
323 
324  // transformation matrix to convert points from odom_frame_ to base_link_frame_
325  tf::Matrix3x3 rob(cos(rob_th_), -sin(rob_th_), rob_x_,
326  sin(rob_th_), cos(rob_th_), rob_y_,
327  0, 0, 1);
328 
329  // this transform adds some adjustment
330  tf::Matrix3x3 adj(cos(dth), -sin(dth), dx,
331  sin(dth), cos(dth), dy,
332  0, 0, 1);
333 
334  // create transformation matrix
335  tf::Matrix3x3 m = adj*rob;
336 
337  // The area around the robot is divided into areas, in front, on the left, left-front, etc.
338  // For each point in points, calculate ldist as the distance on a straight line (FRONT, LEFT, etc.)
339  // and rqdist as the distance^2 diagonally (LEFT and REAR, etc.)
340  // The loop calculates the minimum of all ldist into ldistance, the corresponding
341  // point is saved into lnearest, and of all rqdist the minimum is rqdistance with rnearest.
342  for(unsigned int i=0; i < points.size(); i++) {
343  // laser point in base_link_frame_ with some adjustment
344  double px = points[i].x * m[0][0] + points[i].y * m[0][1] + m[0][2];
345  double py = points[i].x * m[1][0] + points[i].y * m[1][1] + m[1][2];
346 
347  // perpendicular distance from the laser point to the four wall of base footprint
348  double dright = -py + right_;
349  double dleft = py - left_;
350  double drear = -px + rear_;
351  double dfront = px - front_;
352 
353  // area is a 4-bit mask that defines on which of 4 sides of the base the point is
354  // there are, basically, 8 areas surrounding the robot that the point can be in
355  int area = 0;
356  // distance + tolerance > 0, if distance = -1, tolerance = 2, the sum > 0
357  area |= RIGHT * (dright > -tolerance_);
358  area |= LEFT * (dleft > -tolerance_);
359  area |= REAR * (drear > -tolerance_);
360  area |= FRONT * (dfront > -tolerance_);
361 
362  double rqdist=INFINITY, ldist=INFINITY;
363  switch(area) {
364  case(RIGHT): ldist = dright; break;
365  case(LEFT): ldist = dleft; break;
366  case(REAR): ldist = drear; break;
367  case(FRONT): ldist = dfront; break;
368 
369  case(LEFT | REAR): rqdist = dleft*dleft + drear*drear; break;
370  case(LEFT | FRONT): rqdist = dleft*dleft + dfront*dfront; break;
371  case(RIGHT | REAR): rqdist = dright*dright + drear*drear; break;
372  case(RIGHT | FRONT): rqdist = dright*dright + dfront*dfront; break;
373  }
374 
375  if(ldist < ldistance) {
376  ldistance = ldist;
377  lnearest = Vector2(px, py);
378  //larea = area;
379  }
380 
381  if(rqdist < rqdistance) {
382  rqdistance = rqdist;
383  rnearest = Vector2(px, py);
384  //rarea = area;
385  }
386  }
387 
388  // return the point that is the closes to the edges, either diagonally or straigh
389  double rdistance = sqrt(rqdistance);
390  if(rdistance < ldistance) {
391  //ROS_DEBUG("area=%d\n", rarea);
392  if(nearest)
393  *nearest = rnearest;
394  return rdistance;
395  } else {
396  //ROS_DEBUG("area=%d\n", larea);
397  if(nearest)
398  *nearest = lnearest;
399  return ldistance;
400  }
401 }
402 
403 
404 #define CLAMP(x, min, max) (x < min) ? min : ((x > max) ? max : x)
405 
406 double BaseDistance::grad(std::vector<Vector2> &points, double *gx, double *gy, double *gth)
407 {
408  if(!gx || !gy || !gth)
409  return 0;
410 
411  double d0, dx_p, dy_p, dth_p, dx_n, dy_n, dth_n;
412 
413  // for very small distances angular velocity of radians per second can be
414  // considered as meters per second? sinX = X for dt -> 0? no idea what's going on here
415  double v_len = sqrt(vx_last_*vx_last_ + vy_last_*vy_last_ + vth_last_*vth_last_);
416 
417  // dd -> distance traveled in one time frame d_ with current velocity
418  double dd = CLAMP(v_len*d_*2, 0.005, 0.15);
419 
420  // distance to nearest point from current robot pose
421  d0 = distance(points, &nearest_, 0, 0, 0);
422 
423  // distance to nearest point from robot pose if robot moved +/- dd in X, Y or Theta in @a d_
424  dx_p = distance(points, 0, dd, 0, 0);
425  dx_n = distance(points, 0, -dd, 0, 0);
426 
427  dy_p = distance(points, 0, 0, dd, 0);
428  dy_n = distance(points, 0, 0, -dd, 0);
429 
430  dth_p = distance(points, 0, 0, 0, dd);
431  dth_n = distance(points, 0, 0, 0, -dd);
432 
433  *gx = (dx_p - dx_n) / (2.0 * dd);
434  *gy = (dy_p - dy_n) / (2.0 * dd);
435  *gth = (dth_p - dth_n) / (2.0 * dd);
436 
437  // compute second derivatives for finding saddle points
438  double g2x = (dx_p + dx_n - 2*d0) / (dd*dd);
439  double g2y = (dy_p + dy_n - 2*d0) / (dd*dd);
440  double g2th = (dth_p + dth_n - 2*d0) / (dd*dd);
441 
442  //ROS_DEBUG("ddx=%f, ddy=%f, ddth=%f\n", ddx, ddy, ddth);
443  //ROS_DEBUG("gx: %f (+- %f) gy: %f (+-%f) gth: %f (+- %f)\n", *gx, g2x, *gy, g2y, *gth, g2th);
444 
445  // "dampen" if second derivative is big
446  double g2_scaledown = 2.0;
447  double gx_damp = (fabs(g2x) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2x);
448  double gy_damp = (fabs(g2y) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2y);
449  double gth_damp = (fabs(g2th) < g2_scaledown) ? 1.0 : g2_scaledown / fabs(g2th);
450 
451  *gx = *gx * gx_damp;
452  *gy = *gy * gy_damp;
453  *gth = *gth * gth_damp;
454 
455  std_msgs::Float64MultiArray msg;
456  msg.data.resize(7);
457  msg.data[0] = dd;
458 
459  msg.data[1] = *gx;
460  msg.data[2] = *gy;
461  msg.data[3] = *gth;
462 
463  msg.data[4] = g2x;
464  msg.data[5] = g2y;
465  msg.data[6] = g2th;
466 
467  debug_pub_.publish(msg);
468 
469  return d0;
470 }
471 
472 
473 double BaseDistance::brake(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
474 {
475  double factor = 1.5;
476  double d = distance(points, 0, -*vx*d_*factor, -*vy*d_*factor, -*vth*d_*factor);
477  if(d < safety_dist_) { // if we are stuck in the NEXT timestep...
478  mode_ |= MODE_BRAKING;
479  *vx = 0.0;
480  *vy = 0.0;
481  *vth = 0.0;
482  }
483  return d;
484 }
485 
486 
487 double BaseDistance::project(std::vector<Vector2> &points, double *vx, double *vy, double *vth)
488 {
489  double d, gx, gy, gth, factor = 0;
490  d = grad(points, &gx, &gy, &gth);
491 
492  // normalize gradient
493  double l_grad = 1/sqrt(gx*gx + gy*gy + gth*gth);
494  gx*=l_grad;
495  gy*=l_grad;
496  gth*=l_grad;
497 
498  if(d < slowdown_far_) {
499  // project (vx,vy,vth) onto (gx,gy,gth)
500  double dp = *vx*gx + *vy*gy + *vth*gth;
501  if(dp > 0) { // we are moving towards the obstacle
502  if(d < slowdown_near_)
503  {
505  factor = 1;
506  }
507  else
508  {
510  factor = (d - slowdown_far_)/(slowdown_near_ - slowdown_far_);
511  }
512 
513  *vx -= gx *dp*factor;
514  *vy -= gy *dp*factor;
515  *vth -= gth*dp*factor;
516  }
517  }
518 
519  if(d < repelling_dist_) {
521  double l_grad_2d = 1/sqrt(gx*gx + gy*gy);
522  double a = (1.0/d - 1.0/repelling_dist_)
523  * (1.0/d - 1.0/repelling_dist_)
524  * 0.5 * repelling_gain_;
525 
526  if(a > repelling_gain_max_)
528  *vx -= gx * l_grad_2d * a;
529  *vy -= gy * l_grad_2d * a;
530  }
531 
532  return d;
533 }
534 
535 
536 void BaseDistance::compute_distance_keeping(double *vx, double *vy, double *vth)
537 {
538 
539  // compute last known robot position in odom frame
541 
542  // collect all relevant obstacle points: from both lasers and blind spots, if given
543  std::vector<Vector2> current_points;
544 
545  {
546  // boost::mutex::scoped_lock current_lock(lock);
547  // No need for mutexes as the laser points are a C array
548 
549  current_points.reserve((laser_points_[0] ? laser_points_[0]->size() : 0) +
550  (laser_points_[1] ? laser_points_[1]->size() : 0) +
551  blind_spots_.size());
552 
553  if(laser_points_[0])
554  {
555  std::copy(laser_points_[0]->begin(), laser_points_[0]->end(),
556  std::back_insert_iterator<std::vector<Vector2> >(current_points));
557  }
558  if(laser_points_[1])
559  {
560  std::copy(laser_points_[1]->begin(), laser_points_[1]->end(),
561  std::back_insert_iterator<std::vector<Vector2> >(current_points));
562  }
563  std::copy(blind_spots_.begin(), blind_spots_.end(),
564  std::back_insert_iterator<std::vector<Vector2> >(current_points));
565  }
566 
567  mode_ = MODE_FREE;
568 
569  // modify velocity vector (if too close it will start braking or backing up)
570  project(current_points, vx, vy, vth);
571 
572  // if necessary, do braking (also adjusts the velocity and mode_)
573  brake(current_points, vx, vy, vth);
574 
575  vx_last_ = *vx;
576  vy_last_ = *vy;
577  vth_last_ = *vth;
578 
581  publishPoints(current_points);
582 }
583 
584 
585 void BaseDistance::publishLaserMarker(const Vector2 &pt, const std::string &ns, int id)
586 {
587  visualization_msgs::Marker marker;
588  marker.header.frame_id = odom_frame_;
589  marker.header.stamp = ros::Time::now();
590  marker.ns = ns;
591  marker.type = visualization_msgs::Marker::CUBE;
592  marker.action = visualization_msgs::Marker::ADD;
593  marker.pose.position.z = 0.55;
594  marker.pose.orientation.x = 0.0;
595  marker.pose.orientation.y = 0.0;
596  marker.pose.orientation.z = 0.0;
597  marker.pose.orientation.w = 1.0;
598  marker.scale.x = marker_size_;
599  marker.scale.y = marker_size_;
600  marker.scale.z = marker_size_;
601  marker.color.r = 1;
602  marker.color.g = 0;
603  marker.color.b = 0;
604  marker.color.a = 1.0;
605  marker.lifetime = ros::Duration(0.2);
606  marker.id = id;
607  marker.pose.position.x = pt.x;
608  marker.pose.position.y = pt.y;
609  marker.pose.orientation.w = 1.0;
610  marker_pub_.publish(marker);
611 }
612 
613 
615 {
616  // visualize closest point
617  visualization_msgs::Marker marker;
618  marker.header.frame_id = base_link_frame_;
619  marker.header.stamp = ros::Time::now();
620  marker.ns = "nearest_point";
621  marker.id = 0;
622  //marker.type = visualization_msgs::Marker::CUBE;
623  if(mode_ & MODE_REPELLING)
624  marker.type = visualization_msgs::Marker::SPHERE;
625  else
626  marker.type = visualization_msgs::Marker::CUBE;
627 
628  marker.action = visualization_msgs::Marker::ADD;
629  marker.pose.position.x = nearest_.x;
630  marker.pose.position.y = nearest_.y;
631  marker.pose.position.z = 0.55;
632  marker.pose.orientation.x = 0.0;
633  marker.pose.orientation.y = 0.0;
634  marker.pose.orientation.z = 0.0;
635  marker.pose.orientation.w = 1.0;
636  marker.scale.x = marker_size_*2;
637  marker.scale.y = marker_size_*2;
638  marker.scale.z = marker_size_*2;
639 
640  switch(mode_ & MODE_PROJECTION_MASK)
641  {
642  case(MODE_FREE):
643  marker.color.r = 0.0f;
644  marker.color.g = 1.0f;
645  break;
646  case(MODE_PROJECTING):
647  marker.color.r = 0.0f;
648  marker.color.g = 1.0f;
649  break;
650  case(MODE_HARD_PROJECTING):
651  marker.color.r = 1.0f;
652  marker.color.g = 0.5f;
653  break;
654  case(MODE_BRAKING):
655  marker.color.r = 1.0f;
656  marker.color.g = 0.0f;
657  break;
658  }
659  marker.color.b = 0.0f;
660  marker.color.a = 1.0;
661 
662  marker.lifetime = ros::Duration(0.1);
663  marker_pub_.publish(marker);
664 }
665 
666 
668 {
669  visualization_msgs::Marker marker;
670  marker.header.frame_id = base_link_frame_;
671  marker.header.stamp = ros::Time::now();
672  marker.ns = "base_footprint";
673  marker.id = 0;
674  marker.type = visualization_msgs::Marker::CUBE;
675  marker.action = visualization_msgs::Marker::ADD;
676  marker.pose.position.x = (front_ + rear_) * 0.5;
677  marker.pose.position.y = (right_ + left_) * 0.5;
678  marker.pose.position.z = 0.3;
679  marker.pose.orientation.x = 0.0;
680  marker.pose.orientation.y = 0.0;
681  marker.pose.orientation.z = 0.0;
682  marker.pose.orientation.w = 1.0;
683  marker.scale.x = front_ - rear_;
684  marker.scale.y = right_ - left_;
685  marker.scale.z = 0.6;
686  marker.color.r = 0.5f;
687  marker.color.g = 0.5f;
688  marker.color.b = 1.0f;
689  marker.color.a = 0.5;
690  marker.lifetime = ros::Duration();
691  marker_pub_.publish(marker);
692 }
693 
694 
695 void BaseDistance::publishPoints(const std::vector<Vector2> &points)
696 {
697  sensor_msgs::PointCloud cloud;
698 
699  cloud.points.resize(points.size());
700 
701  std::vector<Vector2>::const_iterator src_it;
702  sensor_msgs::PointCloud::_points_type::iterator dest_it;
703 
704  cloud.header.stamp = ros::Time::now();
705  cloud.header.frame_id = odom_frame_;
706  for(src_it=points.begin(), dest_it=cloud.points.begin();
707  src_it != points.end();
708  src_it++, dest_it++)
709  {
710  dest_it->x = src_it->x;
711  dest_it->y = src_it->y;
712  dest_it->z = 0.3;
713  }
714 
715  // add one dummy point to clear the display
716  if(points.size() == 0)
717  {
718  cloud.points.resize(1);
719  cloud.points[0].x = 0.0;
720  cloud.points[0].y = 0.0;
721  cloud.points[0].z = 0.0;
722  }
723 
724  laser_points_pub_.publish(cloud);
725 }
The Vector2 helper class to store 2D coordinates.
Definition: BaseDistance.h:87
d
double repelling_gain_max_
Definition: BaseDistance.h:209
#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:194
ros::Time laser_1_last_msg_time_
Definition: BaseDistance.h:165
#define ROS_FATAL(...)
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
void publish(const boost::shared_ptr< M > &message) const
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:269
bool fresh_scans(double watchdog_timeout)
Check if the laser scans are older than a certain timeout, to use with a watchdog.
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:258
ros::Publisher laser_points_pub_
Definition: BaseDistance.h:280
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:204
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
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:258
ros::Subscriber laser_subscriptions_[2]
Definition: BaseDistance.h:285
ros::Publisher debug_pub_
Definition: BaseDistance.h:281
TFSIMD_FORCE_INLINE const tfScalar & y() const
double rob_x_
rob_x_, rob_y_, rob_th_ Pose of the base_link_frame_ in odom_frame_
Definition: BaseDistance.h:253
#define CLAMP(x, min, max)
#define ROS_WARN(...)
TFSIMD_FORCE_INLINE tfScalar angle(const Quaternion &q1, const Quaternion &q2)
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:272
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.
TFSIMD_FORCE_INLINE const tfScalar & x() const
std::string base_link_frame_
Definition: BaseDistance.h:159
int n_lasers_
n_lasers_ Number of laser scanners that create the base scan. Note: laser_points_ is of size 2...
Definition: BaseDistance.h:177
#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.
#define ROS_INFO(...)
double tolerance_
tolerance_ Error in the footprint measurements in meters.
Definition: BaseDistance.h:189
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#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.
TFSIMD_FORCE_INLINE const tfScalar & y() const
TFSIMD_FORCE_INLINE const tfScalar & x() const
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:242
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
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:222
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...
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
ros::Time laser_0_last_msg_time_
Stores the last time when we received laser messages.
Definition: BaseDistance.h:164
#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:184
tf::TransformListener tf_
Definition: BaseDistance.h:283
double d_
d_ duration of one time frame, i.e. dt or Tdot
Definition: BaseDistance.h:171
double slowdown_near_
slowdown_near_ Distance from footprint edges to closest obstacle point from when to slow aggressively...
Definition: BaseDistance.h:199
double rob_th_
Definition: BaseDistance.h:253
#define MODE_REPELLING
Definition: BaseDistance.cc:61
double repelling_gain_
Definition: BaseDistance.h:209
std::vector< Vector2 > blind_spots_
blind_spots_ A vector of interpolated coordinates for the blind spots Defined in the odom_frame_ ...
Definition: BaseDistance.h:236
static Time now()
std::string odom_frame_
Definition: BaseDistance.h:159
double marker_size_
marker_size_ When publishing points to RViz, in meters
Definition: BaseDistance.h:277
Vector2 nearest_
nearest_ The nearest point to the base
Definition: BaseDistance.h:248
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:230
ros::Publisher marker_pub_
Definition: BaseDistance.h:279
double vy_last_
Definition: BaseDistance.h:258
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:216
double repelling_dist_
repelling_dist_ Distance from footprint edges to closest obstacle point from when to start backing up...
Definition: BaseDistance.h:209
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 Sat Jul 18 2020 03:04:46