Classes | Public Member Functions | Private Member Functions | Private Attributes
BaseDistance Class Reference

#include <BaseDistance.h>

List of all members.

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.
void compute_distance_keeping (double *vx, double *vy, double *vth)
 Sets vx_last_, vy_last and vth_last_ to input parameters, adjusting them.
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.
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.
bool fresh_scans (double watchdog_timeout)
 Check if the laser scans are older than a certain timeout, to use with a watchdog.
void setFootprint (double front, double rear, double left, double right, double tolerance)
 Sets front_, rear_, left_, right_, tolerance_, and early_reject_distance_.
void setSafetyLimits (double safety_dist, double slowdown_near, double slowdown_far, double rate)
 Sets safety_dist_, slowdown_near_, slowdown_far_, and d_ = 1 / rate.

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_.
void calculateEarlyRejectDistance ()
 early_reject_distance_ = diameter + slowdown_far_ + tolerance_ + movement_tolerance;
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?
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_.
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.
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.
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.
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 publishNearestPoint ()
 Publishes nearest_ as a cube or a sphere of corresponding color nearest_ is defined in base_link_frame_.
void publishPoints (const std::vector< Vector2 > &points)
 Publishes coordinates in points as a point cloud Uses laser_points_pub_ to publish.
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.

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.
std::vector< Vector2blind_spots_
 blind_spots_ A vector of interpolated coordinates for the blind spots Defined in the odom_frame_
bool complete_blind_spots_
 complete_blind_spots_ If the laser/s have blind spots, wheter to complete them. The completion will be through interpolation.
double d_
 d_ duration of one time frame, i.e. dt or Tdot
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.
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.
ros::Time laser_0_last_msg_time_
 Stores the last time when we received laser messages.
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.
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
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.
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.
Vector2 nearest_
 nearest_ The nearest point to the base
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
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_
double rob_y_
double safety_dist_
 safety_dist_ Distance in meters when to brake when moving with current velocity.
double slowdown_far_
 slowdown_far_ Distance from footprint edges to closest obstacle point from when to start slowing
double slowdown_near_
 slowdown_near_ Distance from footprint edges to closest obstacle point from when to slow aggressively
tf::TransformListener tf_
double tolerance_
 tolerance_ Error in the footprint measurements in meters.
double vth_last_
double vx_last_
 vx_last_, vy_last_, vth_last_ Robot velocity to drive with in next time frame
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

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.

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.

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.

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.

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.

Definition at line 281 of file BaseDistance.h.

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.

Stores the last time when we received laser messages.

Definition at line 164 of file BaseDistance.h.

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.

Definition at line 280 of file BaseDistance.h.

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.

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.

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.

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.

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

Definition at line 209 of file BaseDistance.h.

Definition at line 209 of file BaseDistance.h.

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.

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 20:28:18