Classes | Public Member Functions | Private Member Functions | Private Attributes | List of all members
BaseDistance Class Reference

#include <BaseDistance.h>

Classes

class  Vector2
 The Vector2 helper class to store 2D coordinates. More...
 

Public Member Functions

 BaseDistance ()
 Gets the parameters from ROS parameter server and initializes arguments with that. More...
 
void compute_distance_keeping (double *vx, double *vy, double *vth)
 Sets vx_last_, vy_last and vth_last_ to input parameters, adjusting them. More...
 
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. More...
 
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 not from the current position of base_link_frame_, but the adjusted with dx, dy and dth one, because in the next timestamp the robot will have moved a little. More...
 
bool fresh_scans (double watchdog_timeout)
 Check if the laser scans are older than a certain timeout, to use with a watchdog. More...
 
void setFootprint (double front, double rear, double left, double right, double tolerance)
 Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_. More...
 
void setSafetyLimits (double safety_dist, double slowdown_near, double slowdown_far, double rate)
 Sets safety_dist_, slowdown_near_, slowdown_far_, and d_ = 1 / rate. More...
 

Private Member Functions

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 given through vx, vy and vth. Short means smaller than safety_dist_. Adjusts mode_. More...
 
void calculateEarlyRejectDistance ()
 early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance; More...
 
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? More...
 
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 component-wise: separately on X and Y coordinates pt1 and pt2 are defined in odom_frame_. More...
 
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. More...
 
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. More...
 
void publishBaseMarker ()
 Publishes a purple cube at the position of the robot in odom_frame_ The size of the cube is the size of the footprint given through front_ etc. More...
 
void publishLaserMarker (const Vector2 &pt, const std::string &ns, int id=0)
 Publishes a red cube at the coordinates of pt using marker_pub_. More...
 
void publishNearestPoint ()
 Publishes nearest_ as a cube or a sphere of corresponding color nearest_ is defined in base_link_frame_. More...
 
void publishPoints (const std::vector< Vector2 > &points)
 Publishes coordinates in points as a point cloud Uses laser_points_pub_ to publish. More...
 
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. More...
 

Private Attributes

std::string base_link_frame_
 
double blind_spot_threshold_
 blind_spot_threshold_ Distance from base_link_frame to the end of a blind spot. Non-parallel lines always meet somewhere, so the blind spot is a triangle. If the obstacles are further away than the triangle, means they are outside of the blind spot, so there is no need to triangulate. More...
 
std::vector< Vector2blind_spots_
 blind_spots_ A vector of interpolated coordinates for the blind spots Defined in the odom_frame_ More...
 
bool complete_blind_spots_
 complete_blind_spots_ If the laser/s have blind spots, wheter to complete them. The completion will be through interpolation. More...
 
double d_
 d_ duration of one time frame, i.e. dt or Tdot More...
 
ros::Publisher debug_pub_
 
double early_reject_distance_
 early_reject_distance_ From how many meters to discard laser measurements. Laser is used to avoid obstacles, so if the obstacles are far away they're not relevant for the time being. More...
 
double front_
 Footprint of the robot, coordinates of front edge, back edge, etc. in the footprint coordinate system. E.g. front = 0.33, rear = -0.33, left = 0.33, right = -0.33. More...
 
ros::Time laser_0_last_msg_time_
 Stores the last time when we received laser messages. More...
 
ros::Time laser_1_last_msg_time_
 
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, i.e. supports up to 2 lasers only. More...
 
ros::Publisher laser_points_pub_
 
ros::Subscriber laser_subscriptions_ [2]
 
double left_
 
boost::mutex lock
 
ros::Publisher marker_pub_
 
double marker_size_
 marker_size_ When publishing points to RViz, in meters More...
 
int mode_
 mode_ Defined the movement mode for the base. Can be one of the modes defined in the cpp file: MODE_FREE - move freely, MODE_PROJECTING - slow down slowly, MODE_HARD_PROJECTING - slow down faster, MODE_BRAKING - brake right now, command 0 velocity, MODE_REPELLING - move away from obstacle. More...
 
ros::NodeHandle n_
 
int n_lasers_
 n_lasers_ Number of laser scanners that create the base scan. Note: laser_points_ is of size 2, so currently only up to 2 lasers supported. More...
 
Vector2 nearest_
 nearest_ The nearest point to the base More...
 
std::string odom_frame_
 
double rear_
 
double repelling_dist_
 repelling_dist_ Distance from footprint edges to closest obstacle point from when to start backing up More...
 
double repelling_gain_
 
double repelling_gain_max_
 
double right_
 
double rob_th_
 
double rob_x_
 rob_x_, rob_y_, rob_th_ Pose of the base_link_frame_ in odom_frame_ More...
 
double rob_y_
 
double safety_dist_
 safety_dist_ Distance in meters when to brake when moving with current velocity. More...
 
double slowdown_far_
 slowdown_far_ Distance from footprint edges to closest obstacle point from when to start slowing More...
 
double slowdown_near_
 slowdown_near_ Distance from footprint edges to closest obstacle point from when to slow aggressively More...
 
tf::TransformListener tf_
 
double tolerance_
 tolerance_ Error in the footprint measurements in meters. More...
 
double vth_last_
 
double vx_last_
 vx_last_, vy_last_, vth_last_ Robot velocity to drive with in next time frame More...
 
double vy_last_
 

Detailed Description

This is the BaseDistance utility class for avoiding obstacles! It figures out if the robot should slow down or move freely based on laser scan data. If there are points in the laser (lasers) that are closer than safe distance the robot should start slowing down, completely brake, or even back up.

ROS topics

Subscribes to (name/type):

Publishes to (name / type):

ROS parameters

Definition at line 80 of file BaseDistance.h.

Constructor & Destructor Documentation

BaseDistance::BaseDistance ( )

Gets the parameters from ROS parameter server and initializes arguments with that.

Definition at line 70 of file BaseDistance.cc.

Member Function Documentation

double BaseDistance::brake ( std::vector< Vector2 > &  points,
double *  vx,
double *  vy,
double *  vth 
)
private

If distance to closest point when moving with given velocity is short, brake. Given velocity here is given through vx, vy and vth. Short means smaller than safety_dist_. Adjusts mode_.

Parameters
pointsLaser data in odom_frame_
[in,out]vxCurrent velocity of the robot from which to start braking when obstacle is close
[in,out]vyY component
[in,out]vthTheta component
Returns
Distance to closes obstacle point from robot pose in next time frame with velocity vx

Definition at line 473 of file BaseDistance.cc.

void BaseDistance::calculateEarlyRejectDistance ( )
private

early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance;

Definition at line 131 of file BaseDistance.cc.

void BaseDistance::compute_distance_keeping ( double *  vx,
double *  vy,
double *  vth 
)

Sets vx_last_, vy_last and vth_last_ to input parameters, adjusting them.

Parameters
[out]vxVelocity to move with, if there were no obstacles
[out]vyY component
[out]vthTheta component

Definition at line 536 of file BaseDistance.cc.

bool BaseDistance::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.

Parameters
[in]fromThe name of the parent frame
[in]toThe name of the child frame
[in]timeTimestamp of the transform
[out]xX component of translation
[out]yY component of translation
[out]thAngle of rotation around the Z axis
Returns
false if no transform could be looked up

Definition at line 171 of file BaseDistance.cc.

double BaseDistance::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 not from the current position of base_link_frame_, but the adjusted with dx, dy and dth one, because in the next timestamp the robot will have moved a little.

Parameters
pointsCoordinates in odom_frame_ (laser data)
nearestIf nearest is not false, updates it with the closest point (in adjusted base_link_frame_)
dxThe X component of adjustment vector of robot coordinates (adjustment vector is defined in base_link_frame_)
dyThe Y component of adjustment vector
dthTheta component of adjustment vector
Returns
Distance to closest of points from nearest footprint wall for next time frame

Definition at line 312 of file BaseDistance.cc.

bool BaseDistance::fresh_scans ( double  watchdog_timeout)

Check if the laser scans are older than a certain timeout, to use with a watchdog.

Definition at line 108 of file BaseDistance.cc.

double BaseDistance::grad ( std::vector< Vector2 > &  points,
double *  gx,
double *  gy,
double *  gth 
)
private

Calculates the gradient of the distance to the closes point of points Is it increasing? Is it decreasing? How fast?

Parameters
points
gxX component of gradient
gyY component of gradient
gthDistance gradient in robot's angular velocity
Returns
Distance to closest point from robot footprint walls in current robot pose

Definition at line 406 of file BaseDistance.cc.

bool BaseDistance::interpolateBlindPoints ( int  n,
const Vector2 pt1,
const Vector2 pt2 
)
private

Interpolates n points between pt1 and pt2 and adds them to blind_spots_ Interpolation is linear and component-wise: separately on X and Y coordinates pt1 and pt2 are defined in odom_frame_.

Returns
true if blind_spots_ was updated

Definition at line 281 of file BaseDistance.cc.

void BaseDistance::laserCallback ( int  index,
const sensor_msgs::LaserScan::ConstPtr &  scan 
)
private

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.

Parameters
indexFor when there are multiple lasers on the base, to index over them
scanInput ROS message from the laser with index index

Definition at line 194 of file BaseDistance.cc.

double BaseDistance::project ( std::vector< Vector2 > &  points,
double *  vx,
double *  vy,
double *  vth 
)
private

Adapts the velocity given in parameters to slow down or back up Adjusts mode_, vx, vy and vth.

Parameters
pointsPoints from the laser in odom_frame_
[in,out]vxCurrent velocity in X
[in,out]vyCurrent velocity in Y
[in,out]vthCurrent velocity in Theta
Returns
Distance to closest point from robot footprint walls in current robot pose

Definition at line 487 of file BaseDistance.cc.

void BaseDistance::publishBaseMarker ( )
private

Publishes a purple cube at the position of the robot in odom_frame_ The size of the cube is the size of the footprint given through front_ etc.

Definition at line 667 of file BaseDistance.cc.

void BaseDistance::publishLaserMarker ( const Vector2 pt,
const std::string &  ns,
int  id = 0 
)
private

Publishes a red cube at the coordinates of pt using marker_pub_.

Parameters
ptPoint in odom_frame_

Definition at line 585 of file BaseDistance.cc.

void BaseDistance::publishNearestPoint ( )
private

Publishes nearest_ as a cube or a sphere of corresponding color nearest_ is defined in base_link_frame_.

Definition at line 614 of file BaseDistance.cc.

void BaseDistance::publishPoints ( const std::vector< Vector2 > &  points)
private

Publishes coordinates in points as a point cloud Uses laser_points_pub_ to publish.

Parameters
pointsCoordinates in odom_frame_

Definition at line 695 of file BaseDistance.cc.

void BaseDistance::setFootprint ( double  front,
double  rear,
double  left,
double  right,
double  tolerance 
)

Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_.

Definition at line 141 of file BaseDistance.cc.

void BaseDistance::setSafetyLimits ( double  safety_dist,
double  slowdown_near,
double  slowdown_far,
double  rate 
)

Sets safety_dist_, slowdown_near_, slowdown_far_, and d_ = 1 / rate.

Todo:
rename to use underscores

Definition at line 155 of file BaseDistance.cc.

BaseDistance::Vector2 BaseDistance::transform ( const Vector2 v,
double  x,
double  y,
double  th 
)
private

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.

Parameters
vInput vector
xx component of the translation vector
yy component of the translation vector
thAngle of rotation
Returns
Transformed vector v' = Rv + t

Definition at line 164 of file BaseDistance.cc.

Member Data Documentation

std::string BaseDistance::base_link_frame_
private

Definition at line 159 of file BaseDistance.h.

double BaseDistance::blind_spot_threshold_
private

blind_spot_threshold_ Distance from base_link_frame to the end of a blind spot. Non-parallel lines always meet somewhere, so the blind spot is a triangle. If the obstacles are further away than the triangle, means they are outside of the blind spot, so there is no need to triangulate.

Definition at line 230 of file BaseDistance.h.

std::vector<Vector2> BaseDistance::blind_spots_
private

blind_spots_ A vector of interpolated coordinates for the blind spots Defined in the odom_frame_

Definition at line 236 of file BaseDistance.h.

bool BaseDistance::complete_blind_spots_
private

complete_blind_spots_ If the laser/s have blind spots, wheter to complete them. The completion will be through interpolation.

Definition at line 222 of file BaseDistance.h.

double BaseDistance::d_
private

d_ duration of one time frame, i.e. dt or Tdot

Definition at line 171 of file BaseDistance.h.

ros::Publisher BaseDistance::debug_pub_
private

Definition at line 281 of file BaseDistance.h.

double BaseDistance::early_reject_distance_
private

early_reject_distance_ From how many meters to discard laser measurements. Laser is used to avoid obstacles, so if the obstacles are far away they're not relevant for the time being.

Definition at line 216 of file BaseDistance.h.

double BaseDistance::front_
private

Footprint of the robot, coordinates of front edge, back edge, etc. in the footprint coordinate system. E.g. front = 0.33, rear = -0.33, left = 0.33, right = -0.33.

Definition at line 184 of file BaseDistance.h.

ros::Time BaseDistance::laser_0_last_msg_time_
private

Stores the last time when we received laser messages.

Definition at line 164 of file BaseDistance.h.

ros::Time BaseDistance::laser_1_last_msg_time_
private

Definition at line 165 of file BaseDistance.h.

boost::shared_ptr<std::vector<Vector2> > BaseDistance::laser_points_[2]
private

laser_points_ Data from lasers filtered and converted into the odom frame. The size is 2 for 2 lasers, i.e. supports up to 2 lasers only.

Definition at line 242 of file BaseDistance.h.

ros::Publisher BaseDistance::laser_points_pub_
private

Definition at line 280 of file BaseDistance.h.

ros::Subscriber BaseDistance::laser_subscriptions_[2]
private

Definition at line 285 of file BaseDistance.h.

double BaseDistance::left_
private

Definition at line 184 of file BaseDistance.h.

boost::mutex BaseDistance::lock
private

Definition at line 287 of file BaseDistance.h.

ros::Publisher BaseDistance::marker_pub_
private

Definition at line 279 of file BaseDistance.h.

double BaseDistance::marker_size_
private

marker_size_ When publishing points to RViz, in meters

Definition at line 277 of file BaseDistance.h.

int BaseDistance::mode_
private

mode_ Defined the movement mode for the base. Can be one of the modes defined in the cpp file: MODE_FREE - move freely, MODE_PROJECTING - slow down slowly, MODE_HARD_PROJECTING - slow down faster, MODE_BRAKING - brake right now, command 0 velocity, MODE_REPELLING - move away from obstacle.

Definition at line 269 of file BaseDistance.h.

ros::NodeHandle BaseDistance::n_
private

Definition at line 272 of file BaseDistance.h.

int BaseDistance::n_lasers_
private

n_lasers_ Number of laser scanners that create the base scan. Note: laser_points_ is of size 2, so currently only up to 2 lasers supported.

Definition at line 177 of file BaseDistance.h.

Vector2 BaseDistance::nearest_
private

nearest_ The nearest point to the base

Definition at line 248 of file BaseDistance.h.

std::string BaseDistance::odom_frame_
private

Definition at line 159 of file BaseDistance.h.

double BaseDistance::rear_
private

Definition at line 184 of file BaseDistance.h.

double BaseDistance::repelling_dist_
private

repelling_dist_ Distance from footprint edges to closest obstacle point from when to start backing up

Definition at line 209 of file BaseDistance.h.

double BaseDistance::repelling_gain_
private

Definition at line 209 of file BaseDistance.h.

double BaseDistance::repelling_gain_max_
private

Definition at line 209 of file BaseDistance.h.

double BaseDistance::right_
private

Definition at line 184 of file BaseDistance.h.

double BaseDistance::rob_th_
private

Definition at line 253 of file BaseDistance.h.

double BaseDistance::rob_x_
private

rob_x_, rob_y_, rob_th_ Pose of the base_link_frame_ in odom_frame_

Definition at line 253 of file BaseDistance.h.

double BaseDistance::rob_y_
private

Definition at line 253 of file BaseDistance.h.

double BaseDistance::safety_dist_
private

safety_dist_ Distance in meters when to brake when moving with current velocity.

Definition at line 204 of file BaseDistance.h.

double BaseDistance::slowdown_far_
private

slowdown_far_ Distance from footprint edges to closest obstacle point from when to start slowing

Definition at line 194 of file BaseDistance.h.

double BaseDistance::slowdown_near_
private

slowdown_near_ Distance from footprint edges to closest obstacle point from when to slow aggressively

Definition at line 199 of file BaseDistance.h.

tf::TransformListener BaseDistance::tf_
private

Definition at line 283 of file BaseDistance.h.

double BaseDistance::tolerance_
private

tolerance_ Error in the footprint measurements in meters.

Definition at line 189 of file BaseDistance.h.

double BaseDistance::vth_last_
private

Definition at line 258 of file BaseDistance.h.

double BaseDistance::vx_last_
private

vx_last_, vy_last_, vth_last_ Robot velocity to drive with in next time frame

Definition at line 258 of file BaseDistance.h.

double BaseDistance::vy_last_
private

Definition at line 258 of file BaseDistance.h.


The documentation for this class was generated from the following files:


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Thu Jun 6 2019 19:20:56