#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.
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< Vector2 >	blind_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):

"~laser_1" sensor_msgs/LaserScan: laser scan from first laser
"~laser_2" sensor_msgs/LaserScan: laser scan from second laser if available

Publishes to (name / type):

"/visualization_marker" visualization_msgs/Marker : markers for base footprint and laser nearest point
"~laser_points" sensor_msgs/PointCloud : laser scanner points from combined lasers & interpolated blind spots
"~debug_channels" std_msgs/Float64MultiArray : debug array for velocity gradient debugging of grad method

ROS parameters

"odom_frame" (string) : odometry frame, default: "/odom"
"base_link_frame" (string) : base frame, default: "/base_link"
"n_lasers" (int) : number of lasers on robot's base, default: 2
"slowdown_far" (double) : distance from footprint edges to closest obstacle point from when to start slowing, default: 0.30 m
"slowdown_near" (double) : distance from footprint edges to closest obstacle point from when to slow aggressively, default: 0.15 m
"safety_dist" (double) : distance in meters when to brake when moving with current velocity, default: 0.10 m
"repelling_dist" (double) : distance from footprint edges to closest obstacle point from when to start backing up, default: 0.20 m
"repelling_gain" (double) : gain for repelling speed, default: 0.5
"repelling_gain_max" (double) : maximum repelling speed gain, default: 0.015
"complete_blind_spots" (bool) : interpolate the angles where lasers can't see or not, default: true
"blind_spot_threshold" (double) : distance from base_link_frame to the end of a blind spot, default: 0.85 m
"marker_size" (double) : size of markers in Rviz, default: 0.1 m

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:

	points	Laser data in odom_frame_
[in,out]	vx	Current velocity of the robot from which to start braking when obstacle is close
[in,out]	vy	Y component
[in,out]	vth	Theta 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]	vx	Velocity to move with, if there were no obstacles
[out]	vy	Y component
[out]	vth	Theta 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]	from	The name of the parent frame
[in]	to	The name of the child frame
[in]	time	Timestamp of the transform
[out]	x	X component of translation
[out]	y	Y component of translation
[out]	th	Angle 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:

points	Coordinates in odom_frame_ (laser data)
nearest	If nearest is not false, updates it with the closest point (in adjusted base_link_frame_)
dx	The X component of adjustment vector of robot coordinates (adjustment vector is defined in base_link_frame_)
dy	The Y component of adjustment vector
dth	Theta 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
gx	X component of gradient
gy	Y component of gradient
gth	Distance 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:

index	For when there are multiple lasers on the base, to index over them
scan	Input 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:

	points	Points from the laser in odom_frame_
[in,out]	vx	Current velocity in X
[in,out]	vy	Current velocity in Y
[in,out]	vth	Current 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:

pt	Point 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:

points Coordinates 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:

v	Input vector
x	x component of the translation vector
y	y component of the translation vector
th	Angle 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]