00001 #ifndef LIDAR_TRACKING_H
00002 #define LIDAR_TRACKING_H
00003
00004 #include <sensor_msgs/LaserScan.h>
00005 #include <ros/ros.h>
00006 #include <deque>
00007 #include <tf/transform_listener.h>
00008 #include <costmap_2d/costmap_2d_ros.h>
00009
00010 using namespace std;
00011 using namespace ros;
00012
00013 class Cluster{
00014 public:
00015 double x;
00016 double y;
00017 double x_dot;
00018 double y_dot;
00019 double r;
00020 int count;
00021 double last_seen;
00022
00023 Cluster();
00024 void addPoint(double x, double y, double t);
00025 void getVelocity(double* x_dot, double* y_dot);
00026
00027 private:
00028 bool fitLine(double* slope, double* intercept);
00029
00030 struct Point{double x, y, xx, xy, t;};
00031 deque<Point> points;
00032 double sumX;
00033 double sumY;
00034 double sumXX;
00035 double sumXY;
00036 int n;
00037 double curr_x_dot;
00038 double curr_y_dot;
00039 bool points_changed;
00040 };
00041
00042 class LidarTracking{
00043 public:
00044 LidarTracking();
00045 void velocityCallback(const geometry_msgs::TwistConstPtr& msg);
00046 void lidarCallback(const sensor_msgs::LaserScanConstPtr& msg);
00047 void getClusters(vector<float> ranges, vector<float> angles, vector<Cluster>* raw_clusters);
00048 bool hasMapCollision(Cluster c);
00049 bool onMap(int x, int y);
00050 void forwardPropogateClusters(double t);
00051 void matchClusters(vector<Cluster> clusters, double t);
00052 void linearExtrapolate(ros::Time stamp);
00053 private:
00054 ros::Publisher dynObs_pub;
00055 ros::Publisher marker_pub;
00056 ros::Subscriber scan_sub;
00057 ros::Subscriber vel_sub;
00058 ros::NodeHandle n;
00059 vector<Cluster> clusters;
00060 double last_t;
00061 bool turning;
00062 bool useCostmap;
00063 string laser_link;
00064 string map_frame;
00065 tf::TransformListener tf_;
00066 costmap_2d::Costmap2DROS* costmap_ros;
00067 costmap_2d::Costmap2D cost_map;
00068 };
00069
00070 #endif
00071
