#include <BaseDistance.h>
Classes | |
class | Vector2 |
Public Member Functions | |
BaseDistance () | |
void | compute_distance_keeping (double *vx, double *vy, double *vth) |
bool | compute_pose2d (const char *from, const char *to, const ros::Time time, double *x, double *y, double *th) |
double | distance (std::vector< Vector2 > &points, Vector2 *nearest, double dx=0, double dy=0, double dth=0) |
void | setFootprint (double front, double rear, double left, double right, double tolerance) |
void | setSafetyLimits (double safety_dist, double slowdown_near, double slowdown_far, double rate) |
Private Member Functions | |
double | brake (std::vector< Vector2 > &points, double *vx, double *vy, double *vth) |
void | calculateEarlyRejectDistance () |
double | grad (std::vector< Vector2 > &points, double *gx, double *gy, double *gth) |
bool | interpolateBlindPoints (int n, const Vector2 &pt1, const Vector2 &pt2) |
void | laserCallback (int index, const sensor_msgs::LaserScan::ConstPtr &scan) |
double | project (std::vector< Vector2 > &points, double *vx, double *vy, double *vth) |
void | publishBaseMarker () |
void | publishLaserMarker (const Vector2 &pt, const std::string &ns, int id=0) |
void | publishNearestPoint () |
void | publishPoints (const std::vector< Vector2 > &points) |
Vector2 | transform (const Vector2 &v, double x, double y, double th) |
Private Attributes | |
std::string | base_link_frame_ |
double | blind_spot_threshold_ |
std::vector< Vector2 > | blind_spots_ |
bool | complete_blind_spots_ |
double | d_ |
ros::Publisher | debug_pub_ |
double | early_reject_distance_ |
double | front_ |
boost::shared_ptr< std::vector < Vector2 > > | laser_points_ [2] |
ros::Publisher | laser_points_pub_ |
ros::Subscriber | laser_subscriptions_ [2] |
double | left_ |
boost::mutex | lock |
ros::Publisher | marker_pub_ |
double | marker_size_ |
int | mode_ |
ros::NodeHandle | n_ |
int | n_lasers_ |
Vector2 | nearest_ |
std::string | odom_frame_ |
double | rear_ |
double | repelling_dist_ |
double | repelling_gain_ |
double | repelling_gain_max_ |
double | right_ |
double | rob_th_ |
double | rob_x_ |
double | rob_y_ |
double | safety_dist_ |
double | slowdown_far_ |
double | slowdown_near_ |
tf::TransformListener | tf_ |
double | tolerance_ |
double | vth_last_ |
double | vx_last_ |
double | vy_last_ |
Definition at line 42 of file BaseDistance.h.
Definition at line 70 of file BaseDistance.cc.
double BaseDistance::brake | ( | std::vector< Vector2 > & | points, |
double * | vx, | ||
double * | vy, | ||
double * | vth | ||
) | [private] |
Definition at line 465 of file BaseDistance.cc.
void BaseDistance::calculateEarlyRejectDistance | ( | ) | [private] |
Definition at line 132 of file BaseDistance.cc.
void BaseDistance::compute_distance_keeping | ( | double * | vx, |
double * | vy, | ||
double * | vth | ||
) |
Definition at line 526 of file BaseDistance.cc.
bool BaseDistance::compute_pose2d | ( | const char * | from, |
const char * | to, | ||
const ros::Time | time, | ||
double * | x, | ||
double * | y, | ||
double * | th | ||
) |
Definition at line 140 of file BaseDistance.cc.
double BaseDistance::distance | ( | std::vector< Vector2 > & | points, |
Vector2 * | nearest, | ||
double | dx = 0 , |
||
double | dy = 0 , |
||
double | dth = 0 |
||
) |
Definition at line 578 of file BaseDistance.cc.
double BaseDistance::grad | ( | std::vector< Vector2 > & | points, |
double * | gx, | ||
double * | gy, | ||
double * | gth | ||
) | [private] |
Definition at line 395 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_
Definition at line 231 of file BaseDistance.cc.
void BaseDistance::laserCallback | ( | int | index, |
const sensor_msgs::LaserScan::ConstPtr & | scan | ||
) | [private] |
Definition at line 162 of file BaseDistance.cc.
double BaseDistance::project | ( | std::vector< Vector2 > & | points, |
double * | vx, | ||
double * | vy, | ||
double * | vth | ||
) | [private] |
Definition at line 478 of file BaseDistance.cc.
void BaseDistance::publishBaseMarker | ( | ) | [private] |
Definition at line 334 of file BaseDistance.cc.
void BaseDistance::publishLaserMarker | ( | const Vector2 & | pt, |
const std::string & | ns, | ||
int | id = 0 |
||
) | [private] |
Definition at line 253 of file BaseDistance.cc.
void BaseDistance::publishNearestPoint | ( | ) | [private] |
Definition at line 281 of file BaseDistance.cc.
void BaseDistance::publishPoints | ( | const std::vector< Vector2 > & | points | ) | [private] |
Definition at line 361 of file BaseDistance.cc.
void BaseDistance::setFootprint | ( | double | front, |
double | rear, | ||
double | left, | ||
double | right, | ||
double | tolerance | ||
) |
Definition at line 119 of file BaseDistance.cc.
void BaseDistance::setSafetyLimits | ( | double | safety_dist, |
double | slowdown_near, | ||
double | slowdown_far, | ||
double | rate | ||
) |
Definition at line 456 of file BaseDistance.cc.
BaseDistance::Vector2 BaseDistance::transform | ( | const Vector2 & | v, |
double | x, | ||
double | y, | ||
double | th | ||
) | [private] |
Definition at line 112 of file BaseDistance.cc.
std::string BaseDistance::base_link_frame_ [private] |
Definition at line 80 of file BaseDistance.h.
double BaseDistance::blind_spot_threshold_ [private] |
Definition at line 83 of file BaseDistance.h.
std::vector<Vector2> BaseDistance::blind_spots_ [private] |
Definition at line 87 of file BaseDistance.h.
bool BaseDistance::complete_blind_spots_ [private] |
Definition at line 82 of file BaseDistance.h.
double BaseDistance::d_ [private] |
Definition at line 78 of file BaseDistance.h.
ros::Publisher BaseDistance::debug_pub_ [private] |
Definition at line 100 of file BaseDistance.h.
double BaseDistance::early_reject_distance_ [private] |
Definition at line 75 of file BaseDistance.h.
double BaseDistance::front_ [private] |
Definition at line 76 of file BaseDistance.h.
boost::shared_ptr<std::vector<Vector2> > BaseDistance::laser_points_[2] [private] |
Definition at line 88 of file BaseDistance.h.
Definition at line 99 of file BaseDistance.h.
ros::Subscriber BaseDistance::laser_subscriptions_[2] [private] |
Definition at line 96 of file BaseDistance.h.
double BaseDistance::left_ [private] |
Definition at line 76 of file BaseDistance.h.
boost::mutex BaseDistance::lock [private] |
Definition at line 106 of file BaseDistance.h.
ros::Publisher BaseDistance::marker_pub_ [private] |
Definition at line 98 of file BaseDistance.h.
double BaseDistance::marker_size_ [private] |
Definition at line 74 of file BaseDistance.h.
int BaseDistance::mode_ [private] |
Definition at line 94 of file BaseDistance.h.
ros::NodeHandle BaseDistance::n_ [private] |
Definition at line 85 of file BaseDistance.h.
int BaseDistance::n_lasers_ [private] |
Definition at line 81 of file BaseDistance.h.
Vector2 BaseDistance::nearest_ [private] |
Definition at line 93 of file BaseDistance.h.
std::string BaseDistance::odom_frame_ [private] |
Definition at line 80 of file BaseDistance.h.
double BaseDistance::rear_ [private] |
Definition at line 76 of file BaseDistance.h.
double BaseDistance::repelling_dist_ [private] |
Definition at line 78 of file BaseDistance.h.
double BaseDistance::repelling_gain_ [private] |
Definition at line 78 of file BaseDistance.h.
double BaseDistance::repelling_gain_max_ [private] |
Definition at line 78 of file BaseDistance.h.
double BaseDistance::right_ [private] |
Definition at line 76 of file BaseDistance.h.
double BaseDistance::rob_th_ [private] |
Definition at line 91 of file BaseDistance.h.
double BaseDistance::rob_x_ [private] |
Definition at line 91 of file BaseDistance.h.
double BaseDistance::rob_y_ [private] |
Definition at line 91 of file BaseDistance.h.
double BaseDistance::safety_dist_ [private] |
Definition at line 78 of file BaseDistance.h.
double BaseDistance::slowdown_far_ [private] |
Definition at line 78 of file BaseDistance.h.
double BaseDistance::slowdown_near_ [private] |
Definition at line 78 of file BaseDistance.h.
tf::TransformListener BaseDistance::tf_ [private] |
Definition at line 101 of file BaseDistance.h.
double BaseDistance::tolerance_ [private] |
Definition at line 77 of file BaseDistance.h.
double BaseDistance::vth_last_ [private] |
Definition at line 92 of file BaseDistance.h.
double BaseDistance::vx_last_ [private] |
Definition at line 92 of file BaseDistance.h.
double BaseDistance::vy_last_ [private] |
Definition at line 92 of file BaseDistance.h.