lidar_segmentation: /home/laradmin/lar/perception/planarobstacles/lidar_segmentation/src/visualization

00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00033 #include "lidar_segmentation.h"
00034 #include "clustering.h"
00035 #include "visualization_rviz.h"
00036 
00037 vector<visualization_msgs::Marker> createTargetMarkers( vector<ClusterPtr>& clusters , vector<ClusterPtr>& clusters_Premebida ,  vector<ClusterPtr>& clusters_Dietmayer, vector<ClusterPtr>& clusters_Santos, vector<ClusterPtr>& clusters_ABD ,vector<ClusterPtr>& clusters_nn , vector<ClusterPtr>&  clusters_GT  )
00038 {               
00039         
00040         static Markers marker_list;
00041         
00042         //Reduce the elements status, ADD to REMOVE and REMOVE to delete
00043         marker_list.decrement();
00044         
00045         // Create a colormap
00046         class_colormap colormap("hsv",10, 1, false);
00047         
00048         visualization_msgs::Marker marker_ids;
00049         visualization_msgs::Marker marker_ids_prem;
00050         visualization_msgs::Marker marker_ids_diet;
00051         visualization_msgs::Marker marker_ids_santos;
00052         visualization_msgs::Marker marker_ids_abd;
00053         visualization_msgs::Marker marker_ids_nn;
00054         visualization_msgs::Marker marker_ids_gt;
00055         
00056         visualization_msgs::Marker marker_clusters;
00057         visualization_msgs::Marker marker_clusters_prem;
00058         visualization_msgs::Marker marker_clusters_diet;
00059         visualization_msgs::Marker marker_clusters_santos;
00060         visualization_msgs::Marker marker_clusters_abd;
00061         visualization_msgs::Marker marker_clusters_nn;
00062         visualization_msgs::Marker marker_clusters_gt;
00063         
00064         marker_ids.header.frame_id = "/my_frame";
00065         marker_ids.header.stamp = ros::Time::now();
00066         marker_ids_prem.header.frame_id = "/my_frame";
00067         marker_ids_prem.header.stamp = ros::Time::now();
00068         marker_ids_diet.header.frame_id = "/my_frame";
00069         marker_ids_diet.header.stamp = ros::Time::now();
00070         marker_ids_santos.header.frame_id = "/my_frame";
00071         marker_ids_santos.header.stamp = ros::Time::now();
00072         marker_ids_abd.header.frame_id = "/my_frame";
00073         marker_ids_abd.header.stamp = ros::Time::now();
00074         marker_ids_nn.header.frame_id = "/my_frame";
00075         marker_ids_nn.header.stamp = ros::Time::now();
00076         marker_ids_gt.header.frame_id = "/my_frame";
00077         marker_ids_gt.header.stamp = ros::Time::now();
00078         
00079         marker_clusters.header.frame_id = "/my_frame";
00080     marker_clusters.header.stamp = marker_ids.header.stamp;
00081         marker_clusters_prem.header.frame_id = "/my_frame";
00082         marker_clusters_prem.header.stamp = marker_ids_prem.header.stamp;
00083         marker_clusters_diet.header.frame_id = "/my_frame";
00084         marker_clusters_diet.header.stamp = marker_ids_diet.header.stamp;
00085         marker_clusters_santos.header.frame_id = "/my_frame";
00086         marker_clusters_santos.header.stamp = marker_ids_santos.header.stamp;
00087         marker_clusters_abd.header.frame_id = "/my_frame";
00088         marker_clusters_abd.header.stamp = marker_ids_abd.header.stamp;
00089         marker_clusters_nn.header.frame_id = "/my_frame";
00090         marker_clusters_nn.header.stamp = marker_ids_nn.header.stamp;
00091         marker_clusters_gt.header.frame_id = "/my_frame";
00092         marker_clusters_gt.header.stamp = marker_ids_gt.header.stamp;
00093         
00094         marker_ids.ns = "ids";
00095         marker_ids.action = visualization_msgs::Marker::ADD;
00096         marker_ids_prem.ns = "ids_prem";
00097         marker_ids_prem.action = visualization_msgs::Marker::ADD;
00098         marker_ids_diet.ns = "ids_diet";
00099         marker_ids_diet.action = visualization_msgs::Marker::ADD;
00100         marker_ids_santos.ns = "ids_santos";
00101         marker_ids_santos.action = visualization_msgs::Marker::ADD;
00102         marker_ids_abd.ns = "ids_abd";
00103         marker_ids_abd.action = visualization_msgs::Marker::ADD;
00104         marker_ids_nn.ns = "ids_nn";
00105         marker_ids_nn.action = visualization_msgs::Marker::ADD;
00106         marker_ids_gt.ns = "ids_gt";
00107         marker_ids_gt.action = visualization_msgs::Marker::ADD; 
00108         
00109         marker_clusters.ns = "clusters";
00110         marker_clusters.action = visualization_msgs::Marker::ADD;
00111         marker_clusters_prem.ns = "clusters_prem";
00112         marker_clusters_prem.action = visualization_msgs::Marker::ADD;
00113         marker_clusters_diet.ns = "clusters_diet";
00114         marker_clusters_diet.action = visualization_msgs::Marker::ADD;
00115         marker_clusters_santos.ns = "clusters_santos";
00116         marker_clusters_santos.action = visualization_msgs::Marker::ADD;
00117         marker_clusters_abd.ns = "clusters_abd";
00118         marker_clusters_abd.action = visualization_msgs::Marker::ADD;
00119         marker_clusters_nn.ns = "clusters_nn";
00120         marker_clusters_nn.action = visualization_msgs::Marker::ADD;
00121         marker_clusters_gt.ns = "clusters_gt";
00122         marker_clusters_gt.action = visualization_msgs::Marker::ADD;
00123         
00124         marker_ids.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00125         marker_ids_prem.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00126         marker_ids_diet.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00127         marker_ids_santos.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00128         marker_ids_abd.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00129         marker_ids_nn.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00130         marker_ids_gt.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
00131         
00132         marker_clusters.type = visualization_msgs::Marker::SPHERE_LIST; 
00133         marker_clusters_prem.type = visualization_msgs::Marker::SPHERE_LIST;
00134         marker_clusters_diet.type = visualization_msgs::Marker::SPHERE_LIST;
00135         marker_clusters_santos.type = visualization_msgs::Marker::SPHERE_LIST;
00136         marker_clusters_abd.type = visualization_msgs::Marker::SPHERE_LIST;
00137         marker_clusters_nn.type = visualization_msgs::Marker::SPHERE_LIST;
00138         marker_clusters_gt.type = visualization_msgs::Marker::SPHERE_LIST;
00139         
00140         marker_ids.scale.x = 0.5;
00141         marker_ids.scale.y = 0.5;
00142         marker_ids.scale.z = 0.5;
00143         marker_ids_prem.scale.x = 0.5;
00144         marker_ids_prem.scale.y = 0.5;
00145         marker_ids_prem.scale.z = 0.5;
00146         marker_ids_diet.scale.x = 0.5;
00147         marker_ids_diet.scale.y = 0.5;
00148         marker_ids_diet.scale.z = 0.5;
00149         marker_ids_santos.scale.x = 0.5;
00150         marker_ids_santos.scale.y = 0.5;
00151         marker_ids_santos.scale.z = 0.5;
00152         marker_ids_abd.scale.x = 0.5;
00153         marker_ids_abd.scale.y = 0.5;
00154         marker_ids_abd.scale.z = 0.5;
00155         marker_ids_nn.scale.x = 0.5;
00156         marker_ids_nn.scale.y = 0.5;
00157         marker_ids_nn.scale.z = 0.5;
00158         marker_ids_gt.scale.x = 0.5;
00159         marker_ids_gt.scale.y = 0.5;
00160         marker_ids_gt.scale.z = 0.5;
00161         
00162         marker_clusters.scale.x = 0.2;
00163         marker_clusters.scale.y = 0.2;
00164         marker_clusters.scale.z = 0.2;  
00165         marker_clusters_prem.scale.x = 0.2;
00166         marker_clusters_prem.scale.y = 0.2;
00167         marker_clusters_prem.scale.z = 0.2;
00168         marker_clusters_diet.scale.x = 0.2;
00169         marker_clusters_diet.scale.y = 0.2;
00170         marker_clusters_diet.scale.z = 0.2;
00171         marker_clusters_santos.scale.x = 0.2;
00172         marker_clusters_santos.scale.y = 0.2;
00173         marker_clusters_santos.scale.z = 0.2;
00174         marker_clusters_abd.scale.x = 0.2;
00175         marker_clusters_abd.scale.y = 0.2;
00176         marker_clusters_abd.scale.z = 0.2;
00177         marker_clusters_nn.scale.x = 0.2;
00178         marker_clusters_nn.scale.y = 0.2;
00179         marker_clusters_nn.scale.z = 0.2;
00180         marker_clusters_gt.scale.x = 0.2;
00181         marker_clusters_gt.scale.y = 0.2;
00182         marker_clusters_gt.scale.z = 0.2;       
00183         
00184         marker_ids.color.a = 1.0;    
00185     marker_ids.color.r = 0.0;   
00186     marker_ids.color.g = 0.0;
00187     marker_ids.color.b = 0.0;
00188         marker_ids_prem.color.a = 1.0;    
00189     marker_ids_prem.color.r = 0.0;   
00190     marker_ids_prem.color.g = 0.0;
00191     marker_ids_prem.color.b = 0.0;
00192         marker_ids_diet.color.a = 1.0;    
00193     marker_ids_diet.color.r = 0.0;   
00194     marker_ids_diet.color.g = 0.0;
00195     marker_ids_diet.color.b = 0.0;
00196         marker_ids_santos.color.a = 1.0;    
00197     marker_ids_santos.color.r = 0.0;   
00198     marker_ids_santos.color.g = 0.0;
00199     marker_ids_santos.color.b = 0.0;
00200         marker_ids_abd.color.a = 1.0;    
00201     marker_ids_abd.color.r = 0.0;   
00202     marker_ids_abd.color.g = 0.0;
00203     marker_ids_abd.color.b = 0.0;
00204         marker_ids_nn.color.a = 1.0;    
00205     marker_ids_nn.color.r = 0.0;   
00206     marker_ids_nn.color.g = 0.0;
00207     marker_ids_nn.color.b = 0.0;
00208         marker_ids_gt.color.a = 1.0;    
00209     marker_ids_gt.color.r = 0.0;   
00210     marker_ids_gt.color.g = 0.0;
00211     marker_ids_gt.color.b = 0.0;
00212 
00213         
00214         for ( uint i = 0 ; i< clusters.size() ; i++)  //search all clusters
00215         {       
00216                 ClusterPtr cluster = clusters[i];
00217                 
00218                 std_msgs::ColorRGBA color = colormap.color(i);
00219                 
00220                 //Place in the marker every point belonging to the cluster "i"          
00221                 for(uint h=0;h<cluster->support_points.size();h++)
00222                 {
00223                         geometry_msgs::Point pt;
00224                         pt.x=cluster->support_points[h]->x;
00225                         pt.y=cluster->support_points[h]->y;
00226                         pt.z=0;
00227                         
00228                         marker_clusters.points.push_back(pt);
00229                         marker_clusters.colors.push_back(color);
00230                 }
00231                                         
00232                 marker_ids.pose.position.x = cluster->centroid->x;
00233                 marker_ids.pose.position.y = cluster->centroid->y;
00234                 marker_ids.pose.position.z = 0.3;
00235                                 
00236                 //texto
00237                 boost::format fm("%d");
00238                 fm      % cluster->id;
00239                 
00240                 marker_ids.text = fm.str();
00241                 marker_ids.id = cluster->id;
00242                 marker_list.update(marker_ids); 
00243                 
00244                 marker_list.update(marker_clusters);
00245                 
00246         } //end for
00247         
00248         
00249         for ( uint i = 0; i< clusters_Premebida.size() ; i++)
00250         {
00251                 
00252                 ClusterPtr cluster_prem = clusters_Premebida[i];
00253 
00254                 std_msgs::ColorRGBA color = colormap.color(i);
00255                 
00256                 //Place in the marker every point belonging to the cluster "i"          
00257                 for(uint h=0;h<cluster_prem->support_points.size();h++)
00258                 {
00259                         geometry_msgs::Point pt;
00260                         pt.x=cluster_prem->support_points[h]->x;
00261                         pt.y=cluster_prem->support_points[h]->y;
00262                         pt.z= 1.0;
00263                         
00264                         marker_clusters_prem.points.push_back(pt);
00265                         marker_clusters_prem.colors.push_back(color);
00266                 }
00267                 
00268                 marker_ids_prem.pose.position.x = cluster_prem->centroid->x;
00269                 marker_ids_prem.pose.position.y = cluster_prem->centroid->y;
00270                 marker_ids_prem.pose.position.z = 1.3; 
00271                 
00272 //              text
00273                 boost::format fm("%d");
00274                 fm % cluster_prem->id;
00275                 
00276                 marker_ids_prem.text = fm.str();
00277                 marker_ids_prem.id = cluster_prem->id;
00278                 
00279                 marker_list.update(marker_ids_prem);
00280                 marker_list.update(marker_clusters_prem);
00281                 
00282         } //end for
00283         
00284         for ( uint i = 0; i< clusters_Dietmayer.size() ; i++)
00285         {
00286                 ClusterPtr cluster_diet = clusters_Dietmayer[i];
00287                 
00288                 std_msgs::ColorRGBA color = colormap.color(i);
00289                 
00290                 //Place in the marker every point belonging to the cluster "i"          
00291                 for(uint h=0;h<cluster_diet->support_points.size();h++)
00292                 {
00293                         geometry_msgs::Point pt;
00294                         pt.x=cluster_diet->support_points[h]->x;
00295                         pt.y=cluster_diet->support_points[h]->y;
00296                         pt.z= 2.0;
00297                         
00298                         marker_clusters_diet.points.push_back(pt);
00299                         marker_clusters_diet.colors.push_back(color);
00300                 }
00301                 
00302                 marker_ids_diet.pose.position.x = cluster_diet->centroid->x;
00303                 marker_ids_diet.pose.position.y = cluster_diet->centroid->y;
00304                 marker_ids_diet.pose.position.z = 2.3; 
00305                 
00306                 //text
00307                 boost::format fm("%d");
00308                 fm % cluster_diet->id;
00309                 
00310                 marker_ids_diet.text = fm.str();
00311                 marker_ids_diet.id = cluster_diet->id;
00312                 
00313                 marker_list.update(marker_ids_diet);
00314                 marker_list.update(marker_clusters_diet);
00315                 
00316         } //end for
00317         
00318         for ( uint i = 0; i< clusters_Santos.size() ; i++)
00319         {
00320                 ClusterPtr cluster_santos = clusters_Santos[i];
00321                 
00322                 std_msgs::ColorRGBA color = colormap.color(i);
00323                 
00324                 //Place in the marker every point belonging to the cluster "i"          
00325                 for(uint h=0;h<cluster_santos->support_points.size();h++)
00326                 {
00327                         geometry_msgs::Point pt;
00328                         pt.x=cluster_santos->support_points[h]->x;
00329                         pt.y=cluster_santos->support_points[h]->y;
00330                         pt.z= 3.0;
00331                         
00332                         marker_clusters_santos.points.push_back(pt);
00333                         marker_clusters_santos.colors.push_back(color);
00334                 }
00335                 
00336                 marker_ids_santos.pose.position.x = cluster_santos->centroid->x;
00337                 marker_ids_santos.pose.position.y = cluster_santos->centroid->y;
00338                 marker_ids_santos.pose.position.z = 3.3; 
00339                 
00340                 //text
00341                 boost::format fm("%d");
00342                 fm % cluster_santos->id;
00343                 
00344                 marker_ids_santos.text = fm.str();
00345                 marker_ids_santos.id = cluster_santos->id;
00346                 
00347                 marker_list.update(marker_ids_santos);
00348                 marker_list.update(marker_clusters_santos);
00349                 
00350         } //end for
00351         
00352         for ( uint i = 0; i< clusters_ABD.size() ; i++)
00353         {
00354                 ClusterPtr cluster_abd = clusters_ABD[i];
00355                 
00356                 std_msgs::ColorRGBA color = colormap.color(i);
00357                 
00358                 //Place in the marker every point belonging to the cluster "i"          
00359                 for(uint h=0;h<cluster_abd->support_points.size();h++)
00360                 {
00361                         geometry_msgs::Point pt;
00362                         pt.x=cluster_abd->support_points[h]->x;
00363                         pt.y=cluster_abd->support_points[h]->y;
00364                         pt.z= 4.0;
00365                         
00366                         marker_clusters_abd.points.push_back(pt);
00367                         marker_clusters_abd.colors.push_back(color);
00368                 }
00369                 
00370                 marker_ids_abd.pose.position.x = cluster_abd->centroid->x;
00371                 marker_ids_abd.pose.position.y = cluster_abd->centroid->y;
00372                 marker_ids_abd.pose.position.z = 4.3; 
00373                 
00374                 //text
00375                 boost::format fm("%d");
00376                 fm % cluster_abd->id;
00377                 
00378                 marker_ids_abd.text = fm.str();
00379                 marker_ids_abd.id = cluster_abd->id;
00380                 
00381                 marker_list.update(marker_ids_abd);
00382                 marker_list.update(marker_clusters_abd);
00383                 
00384         } //end for
00385         
00386         for ( uint i = 0; i< clusters_nn.size() ; i++)
00387         {
00388                 ClusterPtr cluster_nn = clusters_nn[i];
00389 
00390                 std_msgs::ColorRGBA color = colormap.color(i);
00391                 
00392                 //Place in the marker every point belonging to the cluster "i"          
00393                 for(uint h=0;h<cluster_nn->support_points.size();h++)
00394                 {
00395                         geometry_msgs::Point pt;
00396                         pt.x=cluster_nn->support_points[h]->x;
00397                         pt.y=cluster_nn->support_points[h]->y;
00398                         pt.z= 5.0;
00399                         
00400                         marker_clusters_nn.points.push_back(pt);
00401                         marker_clusters_nn.colors.push_back(color);
00402                 }
00403                 
00404                 marker_ids_nn.pose.position.x = cluster_nn->centroid->x;
00405                 marker_ids_nn.pose.position.y = cluster_nn->centroid->y;
00406                 marker_ids_nn.pose.position.z = 5.3; 
00407                 
00408                 //text
00409                 boost::format fm("%d");
00410                 fm % cluster_nn->id;
00411                 
00412                 marker_ids_nn.text = fm.str();
00413                 marker_ids_nn.id = cluster_nn->id;
00414                 
00415                 marker_list.update(marker_ids_nn);
00416                 marker_list.update(marker_clusters_nn);
00417                 
00418         } //end for
00419         
00420         for ( uint i = 0; i< clusters_GT.size() ; i++)
00421         {
00422                 ClusterPtr cluster_gt = clusters_GT[i];
00423 
00424                 std_msgs::ColorRGBA color = colormap.color(i);
00425                 
00426                 //Place in the marker every point belonging to the cluster "i"          
00427                 for(uint h=0;h<cluster_gt->support_points.size();h++)
00428                 {
00429                         geometry_msgs::Point pt;
00430                         pt.x=cluster_gt->support_points[h]->x;
00431                         pt.y=cluster_gt->support_points[h]->y;
00432                         pt.z= 6.0;
00433                         
00434                         marker_clusters_gt.points.push_back(pt);
00435                         marker_clusters_gt.colors.push_back(color);
00436                 }
00437                 
00438                 marker_ids_gt.pose.position.x = cluster_gt->centroid->x;
00439                 marker_ids_gt.pose.position.y = cluster_gt->centroid->y;
00440                 marker_ids_gt.pose.position.z = 6.3; 
00441                         
00442                 //text
00443                 boost::format fm("%d");
00444                 fm % cluster_gt->id;
00445                         
00446                 marker_ids_gt.text = fm.str();
00447                 marker_ids_gt.id = cluster_gt->id;
00448                         
00449                 marker_list.update(marker_ids_gt);
00450                 marker_list.update(marker_clusters_gt);
00451                 
00452         } //end for
00453         
00454         
00455         //Remove markers that should not be transmitted
00456         marker_list.clean();
00457         
00458         //Clean the marker_vector and put new markers in it;
00459         return marker_list.getOutgoingMarkers();
00460         
00461 } //end function
/home/laradmin/lar/perception/planarobstacles/lidar_segmentation/src/visualization_rviz.cpp
