36 #include <boost/bind.hpp>
38 #include <visualization_msgs/Marker.h>
39 #include <visualization_msgs/MarkerArray.h>
40 #include <std_msgs/Float64MultiArray.h>
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
64 #if !ROS_VERSION_MINIMUM(1, 8, 0)
80 if(n_lasers_ < 1 || n_lasers_ > 2)
81 ROS_FATAL(
"Only one or two lasers are supported. %d lasers specified.",
n_lasers_);
93 marker_pub_ =
n_.advertise<visualization_msgs::Marker>(
"/visualization_marker", 0);
95 debug_pub_ =
n_.advertise<std_msgs::Float64MultiArray>(
"debug_channels", 0);
100 laser_subscriptions_[1] =
n_.subscribe<sensor_msgs::LaserScan>
110 double movement_tolerance = 0.2;
119 ROS_INFO(
"setting footprint. x : %f .. %f y : %f .. %f", rear, front, right, left);
142 return Vector2(v.
x*cos(th) - v.
y*sin(th) + x,
143 v.
x*sin(th) + v.
y*cos(th) + y);
149 tf::StampedTransform pose;
152 tf_.lookupTransform(from, to, time, pose);
154 catch(tf::TransformException& ex)
156 ROS_WARN(
"no localization information yet (%s -> %s)", from, to);
160 *x = pose.getOrigin().x();
161 *y = pose.getOrigin().y();
164 *th = atan2(m[1][0], m[0][0]);
173 boost::shared_ptr<std::vector<Vector2> > points(
new std::vector<Vector2>);
175 points->reserve(scan->ranges.size());
182 int i, first_scan = 0, last_scan = 0;
188 for(
int i = scan->ranges.size() - 1; i >= 0; i--)
190 if(scan->ranges[i] > scan->range_min &&
191 scan->ranges[i] < scan->range_max)
198 for(angle = scan->angle_min, i = 0;
199 i < (
int) scan->ranges.size();
200 i++, angle += scan->angle_increment)
203 if(scan->ranges[i] <= scan->range_min ||
204 scan->ranges[i] >= scan->range_max)
217 double xl = scan->ranges[i] * cos(angle);
218 double yl = scan->ranges[i] * sin(angle);
260 for(
int i = 0; i < n; i++)
263 double t = (i / (n - 1.0));
265 t * pt1.
y + (1.0 - t) * pt2.
y));
283 Vector2 rnearest(0, 0), lnearest(0, 0);
284 double rqdistance=INFINITY, ldistance=INFINITY;
288 if(points.size() == 0)
289 ROS_WARN(
"No points received. Maybe the laser topic is wrong?");
298 sin(dth), cos(dth), dy,
309 for(
unsigned int i=0; i < points.size(); i++) {
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];
315 double dright = -py +
right_;
316 double dleft = py -
left_;
317 double drear = -px +
rear_;
318 double dfront = px -
front_;
329 double rqdist=INFINITY, ldist=INFINITY;
331 case(
RIGHT): ldist = dright;
break;
332 case(
LEFT): ldist = dleft;
break;
333 case(
REAR): ldist = drear;
break;
334 case(
FRONT): ldist = dfront;
break;
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;
342 if(ldist < ldistance) {
348 if(rqdist < rqdistance) {
356 double rdistance = sqrt(rqdistance);
357 if(rdistance < ldistance) {
371 #define CLAMP(x, min, max) (x < min) ? min : ((x > max) ? max : x)
375 if(!gx || !gy || !gth)
378 double d0, dx_p, dy_p, dth_p, dx_n, dy_n, dth_n;
385 double dd =
CLAMP(v_len*
d_*2, 0.005, 0.15);
391 dx_p =
distance(points, 0, dd, 0, 0);
392 dx_n =
distance(points, 0, -dd, 0, 0);
394 dy_p =
distance(points, 0, 0, dd, 0);
395 dy_n =
distance(points, 0, 0, -dd, 0);
397 dth_p =
distance(points, 0, 0, 0, dd);
398 dth_n =
distance(points, 0, 0, 0, -dd);
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);
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);
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);
420 *gth = *gth * gth_damp;
422 std_msgs::Float64MultiArray msg;
443 double d =
distance(points, 0, -*vx*
d_*factor, -*vy*
d_*factor, -*vth*
d_*factor);
456 double d, gx, gy, gth, factor = 0;
457 d =
grad(points, &gx, &gy, >h);
460 double l_grad = 1/sqrt(gx*gx + gy*gy + gth*gth);
467 double dp = *vx*gx + *vy*gy + *vth*gth;
480 *vx -= gx *dp*factor;
481 *vy -= gy *dp*factor;
482 *vth -= gth*dp*factor;
488 double l_grad_2d = 1/sqrt(gx*gx + gy*gy);
495 *vx -= gx * l_grad_2d * a;
496 *vy -= gy * l_grad_2d * a;
510 std::vector<Vector2> current_points;
523 std::back_insert_iterator<std::vector<Vector2> >(current_points));
528 std::back_insert_iterator<std::vector<Vector2> >(current_points));
531 std::back_insert_iterator<std::vector<Vector2> >(current_points));
537 project(current_points, vx, vy, vth);
540 brake(current_points, vx, vy, vth);
554 visualization_msgs::Marker marker;
556 marker.header.stamp = ros::Time::now();
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;
571 marker.color.a = 1.0;
572 marker.lifetime = ros::Duration(0.2);
574 marker.pose.position.x = pt.
x;
575 marker.pose.position.y = pt.
y;
576 marker.pose.orientation.w = 1.0;
584 visualization_msgs::Marker marker;
586 marker.header.stamp = ros::Time::now();
587 marker.ns =
"nearest_point";
591 marker.type = visualization_msgs::Marker::SPHERE;
593 marker.type = visualization_msgs::Marker::CUBE;
595 marker.action = visualization_msgs::Marker::ADD;
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;
610 marker.color.r = 0.0f;
611 marker.color.g = 1.0f;
614 marker.color.r = 0.0f;
615 marker.color.g = 1.0f;
618 marker.color.r = 1.0f;
619 marker.color.g = 0.5f;
622 marker.color.r = 1.0f;
623 marker.color.g = 0.0f;
626 marker.color.b = 0.0f;
627 marker.color.a = 1.0;
629 marker.lifetime = ros::Duration(0.1);
636 visualization_msgs::Marker marker;
638 marker.header.stamp = ros::Time::now();
639 marker.ns =
"base_footprint";
641 marker.type = visualization_msgs::Marker::CUBE;
642 marker.action = visualization_msgs::Marker::ADD;
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;
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();
664 sensor_msgs::PointCloud cloud;
666 cloud.points.resize(points.size());
668 std::vector<Vector2>::const_iterator src_it;
669 sensor_msgs::PointCloud::_points_type::iterator dest_it;
671 cloud.header.stamp = ros::Time::now();
673 for(src_it=points.begin(), dest_it=cloud.points.begin();
674 src_it != points.end();
677 dest_it->x = src_it->x;
678 dest_it->y = src_it->y;
683 if(points.size() == 0)
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;
The Vector2 helper class to store 2D coordinates.
double repelling_gain_max_
double slowdown_far_
slowdown_far_ Distance from footprint edges to closest obstacle point from when to start slowing ...
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
BaseDistance()
Gets the parameters from ROS parameter server and initializes arguments with that.
int mode_
mode_ Defined the movement mode for the base. Can be one of the modes defined in the cpp file: MODE_F...
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
ros::Publisher laser_points_pub_
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.
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...
void setFootprint(double front, double rear, double left, double right, double tolerance)
Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_.
void publishNearestPoint()
Publishes nearest_ as a cube or a sphere of corresponding color nearest_ is defined in base_link_fram...
ros::Subscriber laser_subscriptions_[2]
ros::Publisher debug_pub_
double rob_x_
rob_x_, rob_y_, rob_th_ Pose of the base_link_frame_ in odom_frame_
#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.
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_
int n_lasers_
n_lasers_ Number of laser scanners that create the base scan. Note: laser_points_ is of size 2...
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.
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...
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...
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...
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...
tf::TransformListener tf_
double d_
d_ duration of one time frame, i.e. dt or Tdot
double slowdown_near_
slowdown_near_ Distance from footprint edges to closest obstacle point from when to slow aggressively...
std::vector< Vector2 > blind_spots_
blind_spots_ A vector of interpolated coordinates for the blind spots Defined in the odom_frame_ ...
double marker_size_
marker_size_ When publishing points to RViz, in meters
Vector2 nearest_
nearest_ The nearest point to the base
double blind_spot_threshold_
blind_spot_threshold_ Distance from base_link_frame to the end of a blind spot. Non-parallel lines al...
ros::Publisher marker_pub_
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_HARD_PROJECTING
double early_reject_distance_
early_reject_distance_ From how many meters to discard laser measurements. Laser is used to avoid obs...
double repelling_dist_
repelling_dist_ Distance from footprint edges to closest obstacle point from when to start backing up...
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.