BaseDistance.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, Ingo Kresse <kresse@in.tum.de>
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
64 #ifndef BASE_DISTANCE_H
65 #define BASE_DISTANCE_H
66 
67 #include <string>
68 #include <vector>
69 
70 #include <math.h>
71 
72 #include <boost/thread.hpp>
73 #include <boost/shared_ptr.hpp>
74 
75 #include "ros/ros.h"
76 #include <tf/transform_listener.h>
77 #include <sensor_msgs/LaserScan.h>
78 
79 
80 class BaseDistance {
81 
82 public:
83 
87  class Vector2 {
88  public:
89  double x, y;
90  Vector2() {}
91  Vector2(double x_, double y_) : x(x_), y(y_) {}
92 
93  double len2() { return x * x + y * y; }
94  double len() { return sqrt(len2()); }
95  };
96 
100  BaseDistance();
101 
105  void setFootprint(double front, double rear, double left, double right, double tolerance);
106 
111  void setSafetyLimits(double safety_dist, double slowdown_near, double slowdown_far, double rate);
112 
123  bool compute_pose2d(const char* from, const char* to, const ros::Time time, double *x, double *y, double *th);
124 
139  double distance(std::vector<Vector2> &points, Vector2 *nearest, double dx=0, double dy=0, double dth=0);
140 
147  void compute_distance_keeping(double *vx, double *vy, double *vth);
148 
149 
150 private:
151 
153 
157  double d_;
158 
164 
171 
175  double tolerance_;
176 
181 
186 
190  double safety_dist_;
191 
196 
203 
209 
217 
222  std::vector<Vector2> blind_spots_;
223 
228  boost::shared_ptr<std::vector<Vector2> > laser_points_[2];
229  //boost::shared_ptr<std::vector<Vector2> > current_points_; //!< should replace the points argument of distance(), project(), grad() and brake().
230 
235 
240 
245 
255  int mode_;
256 
257 
258  ros::NodeHandle n_;
259 
263  double marker_size_;
264 
265  ros::Publisher marker_pub_;
266  ros::Publisher laser_points_pub_;
267  ros::Publisher debug_pub_;
268 
269  tf::TransformListener tf_;
270 
271  ros::Subscriber laser_subscriptions_[2];
272 
273  boost::mutex lock;
274 
279 
286  bool interpolateBlindPoints(int n, const Vector2 &pt1, const Vector2 &pt2);
287 
294  void laserCallback(int index, const sensor_msgs::LaserScan::ConstPtr& scan);
295 
305  Vector2 transform(const Vector2 &v, double x, double y, double th);
306 
318  double brake(std::vector<Vector2> &points, double *vx, double *vy, double *vth);
319 
329  double grad(std::vector<Vector2> &points, double *gx, double *gy, double *gth);
330 
340  double project(std::vector<Vector2> &points, double *vx, double *vy, double *vth);
341 
346  void publishLaserMarker(const Vector2 &pt, const std::string &ns, int id=0);
347 
352  void publishNearestPoint();
353 
358  void publishBaseMarker();
359 
365  void publishPoints(const std::vector<Vector2> &points);
366 };
367 
368 #endif
The Vector2 helper class to store 2D coordinates.
Definition: BaseDistance.h:87
double repelling_gain_max_
Definition: BaseDistance.h:195
double slowdown_far_
slowdown_far_ Distance from footprint edges to closest obstacle point from when to start slowing ...
Definition: BaseDistance.h:180
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 Vector2
BaseDistance()
Gets the parameters from ROS parameter server and initializes arguments with that.
Definition: BaseDistance.cc:70
int mode_
mode_ Defined the movement mode for the base. Can be one of the modes defined in the cpp file: MODE_F...
Definition: BaseDistance.h:255
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
Definition: BaseDistance.h:244
ros::Publisher laser_points_pub_
Definition: BaseDistance.h:266
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.
Definition: BaseDistance.h:190
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...
double vth_last_
Definition: BaseDistance.h:244
ros::Subscriber laser_subscriptions_[2]
Definition: BaseDistance.h:271
ros::Publisher debug_pub_
Definition: BaseDistance.h:267
double rob_x_
rob_x_, rob_y_, rob_th_ Pose of the base_link_frame_ in odom_frame_
Definition: BaseDistance.h:239
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.
ros::NodeHandle n_
Definition: BaseDistance.h:258
boost::mutex lock
Definition: BaseDistance.h:273
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_
Definition: BaseDistance.h:152
int n_lasers_
n_lasers_ Number of laser scanners that create the base scan. Note: laser_points_ is of size 2...
Definition: BaseDistance.h:163
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.
Vector2(double x_, double y_)
Definition: BaseDistance.h:91
double tolerance_
tolerance_ Error in the footprint measurements in meters.
Definition: BaseDistance.h:175
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...
Definition: BaseDistance.h:228
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...
Definition: BaseDistance.h:208
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...
Definition: BaseDistance.h:170
tf::TransformListener tf_
Definition: BaseDistance.h:269
double d_
d_ duration of one time frame, i.e. dt or Tdot
Definition: BaseDistance.h:157
double slowdown_near_
slowdown_near_ Distance from footprint edges to closest obstacle point from when to slow aggressively...
Definition: BaseDistance.h:185
double rob_th_
Definition: BaseDistance.h:239
double repelling_gain_
Definition: BaseDistance.h:195
std::vector< Vector2 > blind_spots_
blind_spots_ A vector of interpolated coordinates for the blind spots Defined in the odom_frame_ ...
Definition: BaseDistance.h:222
std::string odom_frame_
Definition: BaseDistance.h:152
double marker_size_
marker_size_ When publishing points to RViz, in meters
Definition: BaseDistance.h:263
Vector2 nearest_
nearest_ The nearest point to the base
Definition: BaseDistance.h:234
double blind_spot_threshold_
blind_spot_threshold_ Distance from base_link_frame to the end of a blind spot. Non-parallel lines al...
Definition: BaseDistance.h:216
ros::Publisher marker_pub_
Definition: BaseDistance.h:265
double vy_last_
Definition: BaseDistance.h:244
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?
double early_reject_distance_
early_reject_distance_ From how many meters to discard laser measurements. Laser is used to avoid obs...
Definition: BaseDistance.h:202
double repelling_dist_
repelling_dist_ Distance from footprint edges to closest obstacle point from when to start backing up...
Definition: BaseDistance.h:195
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.


nav_pcontroller
Author(s): Ingo Kresse
autogenerated on Mon Jul 18 2016 16:50:55