polygon_primitives_extraction: /home/laradmin/lar/perception/3D/polygon_primitives_extraction/src/polygon_primitive

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 ***************************************************************************************************/
00037 #ifndef _polygon_primitive_communication_CPP_
00038 #define _polygon_primitive_communication_CPP_
00039 
00040 
00041 #include "polygon_primitive.h"
00042 
00043 
00044 int c_polygon_primitive::export_to_polygon_primitive_msg(polygon_primitive_msg::polygon_primitive* msg)
00045 {
00046         //Header
00047         msg->header.stamp = ros::Time::now();
00048         msg->header.frame_id = data.frames.global_name;
00049 
00050         //polygon name
00051         msg->name = std::string(data.misc.name);
00052 
00053         //Frame names
00054         msg->frame_global_name = data.frames.global_name;
00055         msg->frame_local_name = data.frames.local_name;
00056 
00057         //Frames
00058         msg->frame_current.translation.x = (data.frames.current.transform.getOrigin()).x();
00059         msg->frame_current.translation.y = (data.frames.current.transform.getOrigin()).y();
00060         msg->frame_current.translation.z = (data.frames.current.transform.getOrigin()).z();
00061         
00062         msg->frame_current.rotation.x = (data.frames.current.transform.getRotation()).x();
00063         msg->frame_current.rotation.y = (data.frames.current.transform.getRotation()).y();
00064         msg->frame_current.rotation.z = (data.frames.current.transform.getRotation()).z();
00065         msg->frame_current.rotation.w = (data.frames.current.transform.getRotation()).w();
00066         
00067         msg->frame_previous.translation.x = (data.frames.previous.transform.getOrigin()).x();
00068         msg->frame_previous.translation.y = (data.frames.previous.transform.getOrigin()).y();
00069         msg->frame_previous.translation.z = (data.frames.previous.transform.getOrigin()).z();
00070         
00071         msg->frame_previous.rotation.x = (data.frames.previous.transform.getRotation()).x();
00072         msg->frame_previous.rotation.y = (data.frames.previous.transform.getRotation()).y();
00073         msg->frame_previous.rotation.z = (data.frames.previous.transform.getRotation()).z();
00074         msg->frame_previous.rotation.w = (data.frames.previous.transform.getRotation()).w();
00075 
00076         //polygon color
00077         msg->color_r = data.misc.color.r;
00078         msg->color_g = data.misc.color.g;
00079         msg->color_b = data.misc.color.b;
00080 
00081         //convex hull
00082         pcl::toROSMsg(*data.hulls.convex.polygon, msg->convex_hull);
00083         pcl::toROSMsg(*data.hulls.convex.extended_polygon, msg->extended_convex_hull);
00084         msg->convex_hull_area = data.hulls.convex.area;
00085         msg->convex_hull_solidity = data.hulls.convex.solidity;
00086 
00087         //concave hull
00088         pcl::toROSMsg(*data.hulls.concave.polygon, msg->concave_hull);
00089         pcl::toROSMsg(*data.hulls.concave.extended_polygon, msg->extended_concave_hull);
00090         msg->concave_hull_area = data.hulls.concave.area;
00091         msg->concave_hull_solidity = data.hulls.concave.solidity;
00092 
00093         //plane
00094         msg->support_plane_A = data.planes.current->values[0];
00095         msg->support_plane_B = data.planes.current->values[1];
00096         msg->support_plane_C = data.planes.current->values[2];
00097         msg->support_plane_D = data.planes.current->values[3];
00098 
00099         return 1;
00100 }
00101 
00102 int c_polygon_primitive::import_from_polygon_primitive_msg(polygon_primitive_msg::polygon_primitive* msg)
00103 {
00104 
00105         //polygon name
00106         sprintf(data.misc.name, "%s",msg->name.c_str());
00107 
00108         //Frame names
00109         data.frames.global_name = msg->frame_global_name;
00110         data.frames.local_name = msg->frame_local_name;
00111 
00112         //Frames
00113         data.frames.current.transform = tf::Transform(tf::Quaternion(msg->frame_current.rotation.x, msg->frame_current.rotation.y, msg->frame_current.rotation.z, msg->frame_current.rotation.w),  
00114                         tf::Vector3(msg->frame_current.translation.x, msg->frame_current.translation.y, msg->frame_current.translation.z));
00115 
00116         data.frames.previous.transform = tf::Transform(tf::Quaternion(msg->frame_previous.rotation.x, msg->frame_previous.rotation.y, msg->frame_previous.rotation.z, msg->frame_previous.rotation.w),  
00117                         tf::Vector3(msg->frame_previous.translation.x, msg->frame_previous.translation.y, msg->frame_previous.translation.z));
00118 
00119         //polygon color
00120         data.misc.color.r = msg->color_r;
00121         data.misc.color.g = msg->color_g;
00122         data.misc.color.b = msg->color_b;
00123 
00124         //convex hull
00125         pcl::fromROSMsg(msg->convex_hull, *data.hulls.convex.polygon);
00126         pcl::fromROSMsg(msg->extended_convex_hull, *data.hulls.convex.extended_polygon);
00127         data.hulls.convex.area = msg->convex_hull_area;
00128         data.hulls.convex.solidity = msg->convex_hull_solidity;
00129 
00130         //concave hull
00131         pcl::fromROSMsg(msg->concave_hull, *data.hulls.concave.polygon);
00132         pcl::fromROSMsg(msg->extended_concave_hull, *data.hulls.concave.extended_polygon);
00133         data.hulls.concave.area = msg->concave_hull_area;
00134         data.hulls.concave.solidity = msg->concave_hull_solidity;
00135 
00136         //plane
00137         data.planes.current->values[0] = msg->support_plane_A;
00138         data.planes.current->values[1] = msg->support_plane_B;
00139         data.planes.current->values[2] = msg->support_plane_C;
00140         data.planes.current->values[3] = msg->support_plane_D;
00141 
00142         return 1;
00143 }
00147 void c_polygon_primitive::publish_local_tf(void)
00148 {
00149         char str[1024];
00150         sprintf(str, "/%s",data.misc.name);
00151 
00152         br.sendTransform(tf::StampedTransform(data.frames.current.transform, ros::Time::now(), data.frames.global_name, data.frames.local_name));   
00153 }
00154 
00155 visualization_msgs::Marker c_polygon_primitive::create_visualization_marker_header(
00156                 std::string frame_id, ros::Time stamp, std::string name,
00157                 int action, int id, int type,
00158                 double px, double py, double pz,
00159                 double qx, double qy, double qz, double qw,
00160                 double sx, double sy, double sz,
00161                 double cr, double cg, double cb, double alpha
00162                 )
00163 {
00164         visualization_msgs::Marker mk;
00165         mk.header.frame_id = frame_id; mk.header.stamp = stamp; mk.ns = name;
00166         mk.action = action;     mk.type = type; mk.id = id;
00167         mk.pose.position.x = px; mk.pose.position.y = py; mk.pose.position.z = pz;
00168         mk.pose.orientation.x = qx;     mk.pose.orientation.y = qy;     mk.pose.orientation.z = qz;     mk.pose.orientation.w = qw;
00169         mk.scale.x = sx; mk.scale.y = sy; mk.scale.z = sz; 
00170         mk.color.r = cr; mk.color.g = cg; mk.color.b = cb; mk.color.a = alpha;
00171         return mk;
00172 };
00173 
00174 
00175 int c_polygon_primitive::create_vizualization_msgs(visualization_msgs::MarkerArray* marker_vec, unsigned int id_start)
00176 {
00177         
00178         unsigned int id=id_start;
00179 
00180         geometry_msgs::Point p;
00181         std_msgs::ColorRGBA color;
00182 
00183         if (true) //Polygon name marker (TEXT_VIEW_FACING)
00184         {
00185                 visualization_msgs::Marker msg = create_visualization_marker_header(
00186                                 data.frames.global_name, ros::Time::now(),"name",
00187                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::TEXT_VIEW_FACING,
00188                                 data.frames.current.origin.x, data.frames.current.origin.y, data.frames.current.origin.z,
00189                                 0,0,0,1,
00190                                 1,1,0.6,
00191                                 data.misc.color.r/255., data.misc.color.g/255., data.misc.color.b/255., 1
00192                                 );
00193 
00194                 char str[1024];
00195                 sprintf(str,"%s A=%3.2f S=%3.2f",data.misc.name, data.hulls.convex.area, data.hulls.convex.solidity);
00196                 std::string tmp_str(str); 
00197                 msg.text = tmp_str; 
00198                 marker_vec->markers.push_back(msg);
00199         }
00200 
00201         if (true) //concave hull marker (LINE_STRIP)
00202         {
00203                 visualization_msgs::Marker msg = create_visualization_marker_header(
00204                                 data.frames.global_name, ros::Time::now(),"concave_hull",
00205                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::LINE_STRIP,
00206                                 0,0,0,
00207                                 0,0,0,1,
00208                                 0.05,1,1,
00209                                 //data.misc.color.r/255., data.misc.color.g/255., data.misc.color.b/255., 1
00210                                 1,0,0, 1
00211                                 );
00212 
00213                 if ((int)data.hulls.concave.polygon->points.size() >0)
00214                 {
00215                         for (unsigned int u=0; u< data.hulls.concave.polygon->size(); u++)
00216                         {
00217                                 p.x = data.hulls.concave.polygon->points[u].x; 
00218                                 p.y = data.hulls.concave.polygon->points[u].y; 
00219                                 p.z = data.hulls.concave.polygon->points[u].z; 
00220                                 msg.points.push_back(p);
00221                         }
00222                         p.x = data.hulls.concave.polygon->points[0].x; 
00223                         p.y = data.hulls.concave.polygon->points[0].y; 
00224                         p.z = data.hulls.concave.polygon->points[0].z; 
00225                         msg.points.push_back(p);
00226                 }
00227                 else
00228                 {
00229                         p.x = 0; p.y = 0; p.z = 0;
00230                         msg.points.push_back(p);
00231                         p.x = 1; p.y = 1; p.z = 1;
00232                         msg.points.push_back(p);
00233                 }
00234 
00235                 marker_vec->markers.push_back(msg);
00236         }
00237 
00238 
00239         if ((int)data.hulls.convex.polygon->points.size() >1) //convex hull marker (LINE_STRIP)
00240         {
00241                 visualization_msgs::Marker msg = create_visualization_marker_header(
00242                                 data.frames.global_name, ros::Time::now(),"convex_hull",
00243                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::LINE_STRIP,
00244                                 0,0,0,
00245                                 0,0,0,1,
00246                                 0.15,1,1,
00247                                 data.misc.color.r/255., data.misc.color.g/255., data.misc.color.b/255., 1
00248                                 );
00249 
00250                 for (unsigned int u=0; u< data.hulls.convex.polygon->size(); u++)
00251                 {
00252                         p.x = data.hulls.convex.polygon->points[u].x; 
00253                         p.y = data.hulls.convex.polygon->points[u].y; 
00254                         p.z = data.hulls.convex.polygon->points[u].z; 
00255                         msg.points.push_back(p);
00256                 }
00257                 p.x = data.hulls.convex.polygon->points[0].x; 
00258                 p.y = data.hulls.convex.polygon->points[0].y; 
00259                 p.z = data.hulls.convex.polygon->points[0].z; 
00260                 msg.points.push_back(p);
00261 
00262                 marker_vec->markers.push_back(msg);
00263         }
00264 
00265         if ((int)data.hulls.convex.extended_polygon->points.size() >1) //extended convex hull marker (LINE_STRIP)
00266         {
00267                 visualization_msgs::Marker msg = create_visualization_marker_header(
00268                                 data.frames.global_name, ros::Time::now(),"extended_convex_hull",
00269                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::LINE_STRIP,
00270                                 0,0,0,
00271                                 0,0,0,1,
00272                                 0.05,1,1,
00273                                 data.misc.color.r/255./4, data.misc.color.g/255./4, data.misc.color.b/255./4, 1
00274                                 );
00275 
00276                 for (unsigned int u=0; u< data.hulls.convex.extended_polygon->size(); u++)
00277                 {
00278                         p.x = data.hulls.convex.extended_polygon->points[u].x; 
00279                         p.y = data.hulls.convex.extended_polygon->points[u].y; 
00280                         p.z = data.hulls.convex.extended_polygon->points[u].z; 
00281                         msg.points.push_back(p);
00282                 }
00283                 p.x = data.hulls.convex.extended_polygon->points[0].x; 
00284                 p.y = data.hulls.convex.extended_polygon->points[0].y; 
00285                 p.z = data.hulls.convex.extended_polygon->points[0].z; 
00286                 msg.points.push_back(p);
00287 
00288                 marker_vec->markers.push_back(msg);
00289         }
00290 
00291         if (true) //reference system x axis (ARROW)
00292         {
00293                 visualization_msgs::Marker msg = create_visualization_marker_header(
00294                                 data.frames.global_name, ros::Time::now(),"current_frame",
00295                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::ARROW,
00296                                 0,0,0,
00297                                 0,0,0,1,
00298                                 0.2,0.5,1,
00299                                 1, 0, 0, 1
00300                                 );
00301 
00302                 p.x = data.frames.current.origin.x; 
00303                 p.y = data.frames.current.origin.y; 
00304                 p.z = data.frames.current.origin.z; 
00305                 msg.points.push_back(p);
00306 
00307                 p.x = data.frames.current.arrow_x.x; 
00308                 p.y = data.frames.current.arrow_x.y; 
00309                 p.z = data.frames.current.arrow_x.z; 
00310                 msg.points.push_back(p);
00311 
00312                 marker_vec->markers.push_back(msg);
00313         }
00314 
00315         if (true) //reference system y axis (ARROW)
00316         {
00317                 visualization_msgs::Marker msg = create_visualization_marker_header(
00318                                 data.frames.global_name, ros::Time::now(),"current_frame",
00319                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::ARROW,
00320                                 0,0,0,
00321                                 0,0,0,1,
00322                                 0.2,0.5,1,
00323                                 0, 1, 0, 1
00324                                 );
00325 
00326                 p.x = data.frames.current.origin.x; 
00327                 p.y = data.frames.current.origin.y; 
00328                 p.z = data.frames.current.origin.z; 
00329                 msg.points.push_back(p);
00330 
00331                 p.x = data.frames.current.arrow_y.x; 
00332                 p.y = data.frames.current.arrow_y.y; 
00333                 p.z = data.frames.current.arrow_y.z; 
00334                 msg.points.push_back(p);
00335 
00336                 marker_vec->markers.push_back(msg);
00337         }
00338 
00339         if (true) //reference system z axis (ARROW)
00340         {
00341                 visualization_msgs::Marker msg = create_visualization_marker_header(
00342                                 data.frames.global_name, ros::Time::now(),"current_frame",
00343                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::ARROW,
00344                                 0,0,0,
00345                                 0,0,0,1,
00346                                 0.2,0.5,1,
00347                                 0, 0, 1, 1
00348                                 );
00349 
00350                 p.x = data.frames.current.origin.x; 
00351                 p.y = data.frames.current.origin.y; 
00352                 p.z = data.frames.current.origin.z; 
00353                 msg.points.push_back(p);
00354 
00355                 p.x = data.frames.current.arrow_z.x; 
00356                 p.y = data.frames.current.arrow_z.y; 
00357                 p.z = data.frames.current.arrow_z.z; 
00358                 msg.points.push_back(p);
00359 
00360                 marker_vec->markers.push_back(msg);
00361         }
00362 
00363         // ------------------------------------------
00364         //add a marker to the old frame
00365         // ------------------------------------------
00366 
00367         if (true) //grow system x axis (ARROW)
00368         {
00369                 visualization_msgs::Marker msg = create_visualization_marker_header(
00370                                 data.frames.global_name, ros::Time::now(),"previous_frame",
00371                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::ARROW,
00372                                 0,0,0,
00373                                 0,0,0,1,
00374                                 0.2,0.5,1,
00375                                 0.3, 0, 0, 0.6
00376                                 );
00377 
00378                 p.x = data.frames.previous.origin.x; 
00379                 p.y = data.frames.previous.origin.y; 
00380                 p.z = data.frames.previous.origin.z; 
00381                 msg.points.push_back(p);
00382 
00383                 p.x = data.frames.previous.arrow_x.x; 
00384                 p.y = data.frames.previous.arrow_x.y; 
00385                 p.z = data.frames.previous.arrow_x.z; 
00386                 msg.points.push_back(p);
00387 
00388                 marker_vec->markers.push_back(msg);
00389         }
00390 
00391         if (true) //grow system y axis (ARROW)
00392         {
00393                 visualization_msgs::Marker msg = create_visualization_marker_header(
00394                                 data.frames.global_name, ros::Time::now(),"previous_frame",
00395                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::ARROW,
00396                                 0,0,0,
00397                                 0,0,0,1,
00398                                 0.2,0.5,1,
00399                                 0, 0.3, 0, 0.6
00400                                 );
00401 
00402                 p.x = data.frames.previous.origin.x; 
00403                 p.y = data.frames.previous.origin.y; 
00404                 p.z = data.frames.previous.origin.z; 
00405                 msg.points.push_back(p);
00406 
00407                 p.x = data.frames.previous.arrow_y.x; 
00408                 p.y = data.frames.previous.arrow_y.y; 
00409                 p.z = data.frames.previous.arrow_y.z; 
00410                 msg.points.push_back(p);
00411 
00412                 marker_vec->markers.push_back(msg);
00413         }
00414 
00415         if (true) //grow system z axis (ARROW)
00416         {
00417                 visualization_msgs::Marker msg = create_visualization_marker_header(
00418                                 data.frames.global_name, ros::Time::now(),"previous_frame",
00419                                 visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::ARROW,
00420                                 0,0,0,
00421                                 0,0,0,1,
00422                                 0.2,0.5,1,
00423                                 0, 0, 0.3, 0.6
00424                                 );
00425 
00426                 p.x = data.frames.previous.origin.x; 
00427                 p.y = data.frames.previous.origin.y; 
00428                 p.z = data.frames.previous.origin.z; 
00429                 msg.points.push_back(p);
00430 
00431                 p.x = data.frames.previous.arrow_z.x; 
00432                 p.y = data.frames.previous.arrow_z.y; 
00433                 p.z = data.frames.previous.arrow_z.z; 
00434                 msg.points.push_back(p);
00435 
00436                 marker_vec->markers.push_back(msg);
00437         }
00438 
00439         return 1;
00440 }
00441 
00442 #endif
00443
/home/laradmin/lar/perception/3D/polygon_primitives_extraction/src/polygon_primitive_communication.cpp
