00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00032 #include <ros/ros.h>
00033
00034 #include <visualization_msgs/Marker.h>
00035 #include <visualization_msgs/MarkerArray.h>
00036
00037 #include <mtt/TargetListPC.h>
00038
00039 #include <tf/transform_listener.h>
00040 #include <laser_geometry/laser_geometry.h>
00041 #include <iostream>
00042
00043 #include <pcl/point_types.h>
00044 #include <pcl/io/pcd_io.h>
00045 #include <pcl/kdtree/kdtree_flann.h>
00046 #include <pcl/surface/mls.h>
00047 #include <pcl_conversions/pcl_conversions.h>
00048
00049 #include <mtt/mtt_common.h>
00050
00051 using namespace std;
00052
00053 ros::Publisher target_publisher;
00054 ros::Publisher markers_publisher;
00055 string frame_id;
00056 ros::Time message_stamp;
00057 map<pair<string,int>, pair<visualization_msgs::Marker,int> > marker_map;
00058 int marker_id=0;
00059 double max_mean_variance=0.03;
00060 double clustering_threshold=0.3;
00061 double angular_resolution=3*M_PI/180.;
00062
00063 double point2point_distance(double xi,double yi,double xf,double yf)
00064 { return sqrt((xi-xf)*(xi-xf)+(yi-yf)*(yi-yf));}
00065
00066 double point2line_distance(double alpha,double ro,double x,double y)
00067 { return ro-x*cos(alpha)-y*sin(alpha);}
00068
00069 void free_lines(vector<t_objectPtr> &objects)
00070 {
00071 for(uint i=0;i<objects.size();i++)
00072 objects[i]->lines.clear();
00073 }
00074
00075 void cleanObjets(vector<t_objectPtr> &objects)
00076 {
00077 free_lines(objects);
00078 }
00079
00080 void calcObjectProps(vector<t_objectPtr> &objects)
00081 {
00082 uint e;
00083 double r,t;
00084 double xi,yi,xf,yf;
00085 double rmin;
00086
00087 for(uint i=0;i<objects.size();i++)
00088 {
00089 t=objects[i]->tm;
00090
00091 rmin=1e12;
00092
00093 for(e=0;e<objects[i]->lines.size();e++)
00094 {
00095 r=sqrt(pow(objects[i]->lines[e]->xi,2)+pow(objects[i]->lines[e]->yi,2));
00096
00097 if(r<rmin)
00098 rmin=r;
00099 }
00100
00101 r=sqrt(pow(objects[i]->lines[objects[i]->lines.size()-1]->xf,2)+pow(objects[i]->lines[objects[i]->lines.size()-1]->yf,2));
00102 if(r<rmin)
00103 rmin=r;
00104
00105 r=rmin;
00106
00107 objects[i]->cx=r*cos(t);
00108 objects[i]->cy=r*sin(t);
00109
00110 xi=objects[i]->lines[0]->xi;
00111 yi=objects[i]->lines[0]->yi;
00112
00113 xf=objects[i]->lines[objects[i]->lines.size()-1]->xf;
00114 yf=objects[i]->lines[objects[i]->lines.size()-1]->yf;
00115
00116 objects[i]->size=point2point_distance(xi,yi,xf,yf);
00117 }
00118 }
00119
00120 void recursive_IEPF(t_objectPtr& object,t_data& data,int start,int end)
00121 {
00124 int i,index=0;
00125 double mean_variance,max_variance,current_variance;
00126
00127 t_linePtr line(new t_line);
00128
00129 line->alpha=atan2(data.x[start]-data.x[end],data.y[end]-data.y[start])+M_PI;
00130 line->ro=data.x[start]*cos(line->alpha)+data.y[start]*sin(line->alpha);
00131 line->xi=data.x[start];
00132 line->yi=data.y[start];
00133 line->xf=data.x[end];
00134 line->yf=data.y[end];
00135
00136 mean_variance=0;
00137 max_variance=0;
00138 for(i=start;i<end;i++)
00139 {
00140 current_variance=pow(point2line_distance(line->alpha,line->ro,data.x[i],data.y[i]),2);
00141 mean_variance+=current_variance;
00142
00143 if(current_variance>max_variance)
00144 {
00145 max_variance=current_variance;
00146 index=i;
00147 }
00148 }
00149
00150 mean_variance/=end-start;
00151 mean_variance=sqrt(mean_variance);
00152
00153 if(mean_variance>max_mean_variance)
00154
00155 {
00156 recursive_IEPF(object,data,start,index);
00157 recursive_IEPF(object,data,index,end);
00158 return;
00159 }
00160
00161 object->lines.push_back(line);
00162 return;
00163 }
00164
00165 void recursive_line_fitting(t_objectPtr& object,t_cluster& cluster,t_data& data)
00166 {
00167 if(!data.n_points)
00168 return;
00169
00170 recursive_IEPF(object,data,cluster.stp,cluster.enp);
00171 }
00172
00173 bool clustersToObjects(vector<t_clustersPtr> &clusters,t_data& data,vector<t_objectPtr> &objectsPtr)
00174 {
00175 t_objectPtr object(new t_object);
00176
00177 objectsPtr.clear();
00178
00179 for(uint i=0;i<clusters.size();i++)
00180 {
00181 object->rmin=clusters[i]->rmin;
00182 object->tm=clusters[i]->tm;
00183 object->object_found=false;
00184
00185 object->partialy_occluded=clusters[i]->partialy_occluded;
00186
00187 recursive_line_fitting(object,*clusters[i],data);
00188
00189 objectsPtr.push_back(object);
00190 object.reset(new t_object);
00191 }
00192
00193 return true;
00194 }
00195
00196 void calcClusterProps(vector<t_clustersPtr> &clusters,t_data& data)
00197 {
00198 double rmin;
00199 int e;
00200
00201 for(uint i=0;i<clusters.size();i++)
00202 {
00203 rmin=1e12;
00204 clusters[i]->lenght=0;
00205 for(e=clusters[i]->stp;e<clusters[i]->enp;e++)
00206 {
00207 if(e<clusters[i]->enp-1)
00208 clusters[i]->lenght+=point2point_distance(data.x[e],data.y[e],data.x[e+1],data.y[e+1]);
00209
00210 if(data.r[e]<rmin)
00211 rmin=data.r[e];
00212 }
00213
00214
00215 clusters[i]->rmin=rmin;
00216 clusters[i]->tm=(data.t[clusters[i]->stp]+data.t[clusters[i]->enp])/2;
00217 }
00218 }
00219
00220 bool clustering(t_data& data,vector<t_clustersPtr> &clusters)
00221 {
00222 double x,y,xold=0,yold=0;
00223 double dist,threshold;
00224
00225 t_clustersPtr cluster(new t_cluster);
00226
00227 clusters.clear();
00228
00229 cluster->id=clusters.size();
00230
00231 for(int i=0;i<data.n_points;i++)
00232 {
00233 x=data.x[i];
00234 y=data.y[i];
00235
00236 if(i>0 && i<data.n_points-1)
00237 {
00238 dist = sqrt( pow(x-xold,2) + pow(y-yold,2));
00239
00240 threshold=clustering_threshold;
00241
00242 if(dist>threshold)
00243 {
00244 cluster->enp=i-1;
00245 cluster->n_points=cluster->enp-cluster->stp;
00246 cluster->partialy_occluded=false;
00247 clusters.push_back(cluster);
00248
00249 cluster.reset(new t_cluster);
00250
00251 cluster->id=clusters.size();
00252 cluster->stp=i;
00253 cluster->lenght=0;
00254 }
00255
00256 if(i==(data.n_points-1))
00257 {
00258
00259 cluster->enp=i;
00260 cluster->n_points=cluster->enp-cluster->stp;
00261 }
00262
00263 }else if(i==data.n_points-1)
00264 {
00265 cluster->enp=i;
00266 cluster->n_points=cluster->enp-cluster->stp;
00267 cluster->partialy_occluded=false;
00268 clusters.push_back(cluster);
00269
00270 }else if(i==0)
00271 {
00272 cluster->stp=0;
00273 cluster->enp=0;
00274 cluster->lenght=0;
00275 }
00276
00277 xold=x;
00278 yold=y;
00279 }
00280
00281 return true;
00282 }
00283
00284 void pointCloud2ToData(const sensor_msgs::PointCloud2& cloud,t_data& data)
00285 {
00286 pcl::PointCloud<pcl::PointXYZ> PclCloud;
00287 pcl::PCLPointCloud2 pcl_pc;
00288 pcl_conversions::toPCL(cloud, pcl_pc);
00289 pcl::fromPCLPointCloud2(pcl_pc, PclCloud);
00290
00291
00292 double theta;
00293
00294
00295 map<double,pair<uint,double> > grid;
00296 map<double,pair<uint,double> >::iterator it;
00297
00298 double spacing = angular_resolution;
00299
00300 double rightBorder;
00301 double leftBorder;
00302
00303 double r;
00304
00305
00306 for(uint i=0;i<PclCloud.points.size();i++)
00307 {
00308 theta=atan2(PclCloud.points[i].y,PclCloud.points[i].x);
00309 r=sqrt(pow(PclCloud.points[i].x,2)+pow(PclCloud.points[i].y,2));
00310
00311
00312 rightBorder = spacing * ceil(theta/spacing);
00313 leftBorder = rightBorder - spacing;
00314
00315 if(fabs(rightBorder-theta)<fabs(leftBorder-theta))
00316 theta=rightBorder;
00317 else
00318 theta=leftBorder;
00319
00320 if(grid.find(theta)!=grid.end())
00321 {
00322 if(r<grid[theta].second)
00323 {
00324 grid[theta].first=i;
00325 grid[theta].second=r;
00326 }
00327 }else
00328 {
00329 grid[theta].first=i;
00330 grid[theta].second=r;
00331 }
00332 }
00333
00334 uint i=0;
00335 for(it=grid.begin();it!=grid.end();it++)
00336 {
00337
00338 data.x[i]=PclCloud.points[(*it).second.first].x;
00339 data.y[i]=PclCloud.points[(*it).second.first].y;
00340 data.r[i]=sqrt(pow(data.x[i],2)+pow(data.y[i],2));
00341 data.t[i]=atan2(data.y[i],data.x[i]);
00342
00343 i++;
00344 }
00345
00346 data.n_points=grid.size();
00347 }
00348
00349 void convertObjectsToTargetListPC(vector<t_objectPtr> &objects,mtt::TargetListPC& targets)
00350 {
00351
00352
00353 pcl::PointCloud<pcl::PointXYZ> target_positions;
00354 pcl::PointCloud<pcl::PointXYZ> velocity;
00355
00356 targets.header.stamp = ros::Time::now();
00357 targets.header.frame_id = frame_id;
00358
00359 for(uint i=0;i<objects.size();i++)
00360 {
00361
00362
00363 targets.id.push_back(i);
00364
00365 pcl::PointXYZ position;
00366
00367 position.x = objects[i]->cx;
00368 position.y = objects[i]->cy;
00369 position.z = 0;
00370
00371 target_positions.points.push_back(position);
00372
00373 pcl::PointXYZ vel;
00374
00375 vel.x=0;
00376 vel.y=0;
00377 vel.z=0;
00378
00379 velocity.points.push_back(vel);
00380
00381
00382 pcl::PointCloud<pcl::PointXYZ> shape;
00383 pcl::PointXYZ line_point;
00384
00385 line_point.z=0;
00386
00387 uint j;
00388 for(j=0;j<objects[i]->lines.size();j++)
00389 {
00390 line_point.x=objects[i]->lines[j]->xi;
00391 line_point.y=objects[i]->lines[j]->yi;
00392
00393 shape.points.push_back(line_point);
00394 }
00395
00396 line_point.x=objects[i]->lines[j-1]->xf;
00397 line_point.y=objects[i]->lines[j-1]->yf;
00398
00399 shape.points.push_back(line_point);
00400
00401 sensor_msgs::PointCloud2 shape_cloud;
00402 pcl::toROSMsg(shape,shape_cloud);
00403
00404 shape_cloud.header.stamp=message_stamp;
00405 shape_cloud.header.frame_id=frame_id;
00406
00407 targets.obstacle_lines.push_back(shape_cloud);
00408 }
00409
00410 pcl::toROSMsg(target_positions, targets.position);
00411
00412 targets.position.header.stamp=message_stamp;
00413 targets.position.header.frame_id=frame_id;
00414
00415 pcl::toROSMsg(velocity, targets.velocity);
00416
00417 targets.velocity.header.stamp=message_stamp;
00418 targets.velocity.header.frame_id=frame_id;
00419 }
00420
00421 void cleanMarkers(void)
00422 {
00423 map<pair<string,int>, pair<visualization_msgs::Marker,int> >::iterator it;
00424
00425
00426 for(it=marker_map.begin();it!=marker_map.end();it++)
00427 it->second.second--;
00428
00429 marker_id=0;
00430 }
00431
00432 void maintenanceMarkers(vector<visualization_msgs::Marker>& markers)
00433 {
00434 map<pair<string,int>, pair<visualization_msgs::Marker,int> >::iterator it;
00435
00436 for(it=marker_map.begin();it!=marker_map.end();it++)
00437 {
00438 if( it->second.second==0 )
00439 it->second.first.action = visualization_msgs::Marker::DELETE;
00440 else if(it->second.second<=-1)
00441 {
00446 continue;
00447 }
00448
00449 markers.push_back(it->second.first);
00450 }
00451 }
00452
00453 void createMarkers(mtt::TargetListPC& targets)
00454 {
00455 std_msgs::ColorRGBA color;
00456 class_colormap colormap("hsv",10, 1, false);
00457
00458 visualization_msgs::Marker marker;
00459
00460 marker.header.frame_id = frame_id;
00461 marker.header.stamp = ros::Time::now();
00462 marker.action = visualization_msgs::Marker::ADD;
00463
00464 marker.ns = "ids";
00465 marker.pose.position.z=0.3;
00466 marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00467
00468 marker.scale.x = 0.2;
00469 marker.scale.y = 0.2;
00470 marker.scale.z = 0.2;
00471
00472 marker.color.r=0;
00473 marker.color.g=0;
00474 marker.color.b=0;
00475 marker.color.a=1;
00476
00477 pcl::PointCloud<pcl::PointXYZ> positions;
00478 pcl::fromROSMsg(targets.position,positions);
00479
00480 for(uint i=0;i<positions.points.size();i++)
00481 {
00482 marker.pose.position.x=positions.points[i].x;
00483 marker.pose.position.y=positions.points[i].y;
00484
00485 marker.text = boost::lexical_cast<string>(targets.id[i]);
00486
00487 marker.id=marker_id++;
00488 marker_map[make_pair(marker.ns,marker.id) ] = make_pair(marker,1);
00489 }
00490
00491
00492 marker.type = visualization_msgs::Marker::LINE_STRIP;
00493 marker.ns = "objects";
00494
00495 marker.pose.position.x=0;
00496 marker.pose.position.y=0;
00497 marker.pose.position.z=0;
00498
00499 marker.scale.x = 0.02;
00500 marker.scale.y = 0.1;
00501 marker.scale.z = 0.1;
00502
00503 for(uint i=0;i<targets.obstacle_lines.size();i++)
00504 {
00505 marker.color = colormap.color(i);
00506
00507 geometry_msgs::Point p;
00508 p.z = 0.1;
00509
00510 marker.points.clear();
00511
00512 uint l;
00513
00514 pcl::PointCloud<pcl::PointXYZ> shape;
00515 pcl::fromROSMsg(targets.obstacle_lines[i],shape);
00516
00517 for(l=0;l<shape.points.size();l++)
00518 {
00519 p.x = shape.points[l].x;
00520 p.y = shape.points[l].y;
00521 p.z = 0;
00522
00523 marker.points.push_back(p);
00524 }
00525
00526 marker.id=marker_id++;
00527 marker_map[make_pair(marker.ns,marker.id) ] = make_pair(marker,1);
00528 }
00529 }
00530
00531 void markersFromData(t_data& data)
00532 {
00533 visualization_msgs::Marker marker;
00534
00535 marker.header.frame_id = frame_id;
00536 marker.header.stamp = ros::Time::now();
00537 marker.action = visualization_msgs::Marker::ADD;
00538
00539 marker.ns = "data";
00540 marker.pose.position.z=0.0;
00541 marker.type = visualization_msgs::Marker::POINTS;
00542
00543 marker.scale.x = 0.02;
00544 marker.scale.y = 0.02;
00545 marker.scale.z = 0.02;
00546
00547 marker.color.r=0;
00548 marker.color.g=0;
00549 marker.color.b=1;
00550 marker.color.a=1;
00551
00552 for(int i=0;i<data.n_points;i++)
00553 {
00554 geometry_msgs::Point p;
00555
00556 p.x=data.x[i];
00557 p.y=data.y[i];
00558 p.z=0;
00559
00560 marker.points.push_back(p);
00561 }
00562
00563 marker.id=marker_id++;
00564 marker_map[make_pair(marker.ns,marker.id) ] = make_pair(marker,1);
00565 }
00566
00567 void pointsHandler(const sensor_msgs::PointCloud2& points)
00568 {
00569 t_data data;
00570 vector<t_clustersPtr> clusters;
00571 vector<t_objectPtr> objects;
00572 mtt::TargetListPC targets;
00573 visualization_msgs::MarkerArray markers;
00574
00575
00576 frame_id=points.header.frame_id;
00577 message_stamp=points.header.stamp;
00578
00579
00580 pointCloud2ToData(points,data);
00581
00582
00583 clustering(data,clusters);
00584
00585
00586 calcClusterProps(clusters,data);
00587
00588
00589 clustersToObjects(clusters,data,objects);
00590
00591
00592 calcObjectProps(objects);
00593
00594
00595 convertObjectsToTargetListPC(objects,targets);
00596
00597
00598 cleanMarkers();
00599
00600
00601 createMarkers(targets);
00602
00603
00604 markersFromData(data);
00605
00606
00607 maintenanceMarkers(markers.markers);
00608
00609
00610 target_publisher.publish(targets);
00611
00612
00613 markers_publisher.publish(markers);
00614
00615
00616 cleanObjets(objects);
00617 }
00618
00619 int main(int argc,char**argv)
00620 {
00621
00622 ros::init(argc,argv,"mtt");
00623
00624 ros::NodeHandle nh("~");
00625
00626 ros::Subscriber subs_points = nh.subscribe("/points", 1000, pointsHandler);
00627 target_publisher = nh.advertise<mtt::TargetListPC>("/targets", 1000);
00628 markers_publisher = nh.advertise<visualization_msgs::MarkerArray>("/markers", 1000);
00629
00630 visualization_msgs::MarkerArray markersMsg;
00631
00632 ros::spin();
00633
00634 return 0;
00635 }