/home/laradmin/lar/perception/3D/polygon_primitives_extraction/src/extract_polygon_primitives_rvizmarkers.cpp
Go to the documentation of this file.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 ***************************************************************************************************/ 00036 #ifndef _extract_polygon_primitives_RVIZMARKERS_CPP_ 00037 #define _extract_polygon_primitives_RVIZMARKERS_CPP_ 00038 00042 #include "extract_polygon_primitives.h" 00043 00044 visualization_msgs::Marker create_visualization_marker_header( 00045 std::string frame_id, ros::Time stamp, std::string name, 00046 int action, int id, int type, 00047 double px, double py, double pz, 00048 double qx, double qy, double qz, double qw, 00049 double sx, double sy, double sz, 00050 double cr, double cg, double cb, double alpha 00051 ) 00052 { 00053 visualization_msgs::Marker mk; 00054 mk.header.frame_id = frame_id; mk.header.stamp = stamp; mk.ns = name; 00055 mk.action = action; mk.type = type; mk.id = id; 00056 mk.pose.position.x = px; mk.pose.position.y = py; mk.pose.position.z = pz; 00057 mk.pose.orientation.x = qx; mk.pose.orientation.y = qy; mk.pose.orientation.z = qz; mk.pose.orientation.w = qw; 00058 mk.scale.x = sx; mk.scale.y = sy; mk.scale.z = sz; 00059 mk.color.r = cr; mk.color.g = cg; mk.color.b = cb; mk.color.a = alpha; 00060 return mk; 00061 } 00062 00063 void create_publish_marker_array(std::vector<c_polygon_primitive> *plg, visualization_msgs::MarkerArray *marker_array_msg) 00064 { 00065 //Create and publish the marker array 00066 geometry_msgs::Point p; 00067 std_msgs::ColorRGBA color; 00068 std::vector<visualization_msgs::Marker> marker_vec; 00069 00070 //for (int i=0, id=0; i<(int)plg->size();i++) 00071 for (int i=0, id=0; i<plg->size();i++) 00072 { 00073 00074 00075 (*plg)[i].create_vizualization_msgs(marker_array_msg, 0+100*i); 00076 continue; 00077 00078 //if (true) //Polygon name marker (TEXT_VIEW_FACING) 00079 //{ 00080 //visualization_msgs::Marker msg = create_visualization_marker_header( 00081 //(*plg)[i].data.frames.global_name, ros::Time::now(),"name", 00082 //visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::TEXT_VIEW_FACING, 00083 //(*plg)[i].data.frames.current.origin.x, (*plg)[i].data.frames.current.origin.y, (*plg)[i].data.frames.current.origin.z, 00084 //0,0,0,1, 00085 //1,1,0.6, 00086 //(*plg)[i].data.misc.color.r/255., (*plg)[i].data.misc.color.g/255., (*plg)[i].data.misc.color.b/255., 1 00087 //); 00088 00089 //char str[1024]; 00090 //sprintf(str,"%s A=%3.2f S=%3.2f",(*plg)[i].data.misc.name, (*plg)[i].data.hulls.convex.area, (*plg)[i].data.hulls.convex.solidity); 00091 //std::string tmp_str(str); 00092 //msg.text = tmp_str; 00093 //marker_vec.push_back(msg); 00094 //} 00095 00096 00097 //if (true) //concave hull marker (LINE_STRIP) 00098 //{ 00099 //visualization_msgs::Marker msg = create_visualization_marker_header( 00100 //(*plg)[i].data.frames.global_name, ros::Time::now(),"concave_hull", 00101 //visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::LINE_STRIP, 00102 //0,0,0, 00103 //0,0,0,1, 00104 //0.05,1,1, 00106 //0, 1, 0,1 00107 //); 00108 00109 //if ((int)(*plg)[i].data.hulls.concave.polygon->points.size() >0) 00110 //{ 00111 //for (unsigned int u=0; u< (*plg)[i].data.hulls.concave.polygon->size(); u++) 00112 //{ 00113 //p.x = (*plg)[i].data.hulls.concave.polygon->points[u].x; 00114 //p.y = (*plg)[i].data.hulls.concave.polygon->points[u].y; 00115 //p.z = (*plg)[i].data.hulls.concave.polygon->points[u].z; 00116 //msg.points.push_back(p); 00117 //} 00118 //p.x = (*plg)[i].data.hulls.concave.polygon->points[0].x; 00119 //p.y = (*plg)[i].data.hulls.concave.polygon->points[0].y; 00120 //p.z = (*plg)[i].data.hulls.concave.polygon->points[0].z; 00121 //msg.points.push_back(p); 00122 //} 00123 //else 00124 //{ 00125 //p.x = 0; p.y = 0; p.z = 0; 00126 //msg.points.push_back(p); 00127 //p.x = 1; p.y = 1; p.z = 1; 00128 //msg.points.push_back(p); 00129 //} 00130 00131 //ROS_INFO("msg r=%f g=%f b=%f", msg.color.r, msg.color.g, msg.color.b); 00132 //marker_vec.push_back(msg); 00133 //} 00134 00135 00136 //if ((int)(*plg)[i].data.hulls.convex.polygon->points.size() >1) //convex hull marker (LINE_STRIP) 00137 //{ 00138 //visualization_msgs::Marker msg = create_visualization_marker_header( 00139 //(*plg)[i].data.frames.global_name, ros::Time::now(),"convex_hull", 00140 //visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::LINE_STRIP, 00141 //0,0,0, 00142 //0,0,0,1, 00143 //0.05,1,1, 00145 //1,0,0, 1 00146 //); 00147 00148 //for (unsigned int u=0; u< (*plg)[i].data.hulls.convex.polygon->size(); u++) 00149 //{ 00150 //p.x = (*plg)[i].data.hulls.convex.polygon->points[u].x; 00151 //p.y = (*plg)[i].data.hulls.convex.polygon->points[u].y; 00152 //p.z = (*plg)[i].data.hulls.convex.polygon->points[u].z; 00153 //msg.points.push_back(p); 00154 //} 00155 //p.x = (*plg)[i].data.hulls.convex.polygon->points[0].x; 00156 //p.y = (*plg)[i].data.hulls.convex.polygon->points[0].y; 00157 //p.z = (*plg)[i].data.hulls.convex.polygon->points[0].z; 00158 //msg.points.push_back(p); 00159 00160 //marker_vec.push_back(msg); 00161 //} 00162 00163 //if ((int)(*plg)[i].data.hulls.convex.extended_polygon->points.size() >1) //extended convex hull marker (LINE_STRIP) 00164 //{ 00165 //visualization_msgs::Marker msg = create_visualization_marker_header( 00166 //(*plg)[i].data.frames.global_name, ros::Time::now(),"extended_convex_hull", 00167 //visualization_msgs::Marker::ADD, id++, visualization_msgs::Marker::LINE_STRIP, 00168 //0,0,0, 00169 //0,0,0,1, 00170 //0.05,1,1, 00171 //(*plg)[i].data.misc.color.r/255./4, (*plg)[i].data.misc.color.g/255./4, (*plg)[i].data.misc.color.b/255./4, 1 00172 //); 00173 00174 //for (unsigned int u=0; u< (*plg)[i].data.hulls.convex.extended_polygon->size(); u++) 00175 //{ 00176 //p.x = (*plg)[i].data.hulls.convex.extended_polygon->points[u].x; 00177 //p.y = (*plg)[i].data.hulls.convex.extended_polygon->points[u].y; 00178 //p.z = (*plg)[i].data.hulls.convex.extended_polygon->points[u].z; 00179 //msg.points.push_back(p); 00180 //} 00181 //p.x = (*plg)[i].data.hulls.convex.extended_polygon->points[0].x; 00182 //p.y = (*plg)[i].data.hulls.convex.extended_polygon->points[0].y; 00183 //p.z = (*plg)[i].data.hulls.convex.extended_polygon->points[0].z; 00184 //msg.points.push_back(p); 00185 00186 //marker_vec.push_back(msg); 00187 //} 00188 00189 00190 00191 //} 00192 00193 00194 00195 00196 00197 00198 //Draw the camera projections 00199 00200 //for (size_t j=0; j< (*plg)[i].cp.size(); j++) 00201 //{ 00202 //visualization_msgs::Marker msg11; //declare the msg 00203 //msg11.header.frame_id = (*plg)[i].data.frames.global_name; 00204 //msg11.header.stamp = ros::Time::now(); 00205 //msg11.ns = "raw_projection"; 00206 //msg11.action = visualization_msgs::Marker::ADD; 00207 //msg11.pose.orientation.w = 1.0; 00208 //msg11.type = visualization_msgs::Marker::POINTS; 00209 //msg11.id = id++; 00210 00211 //msg11.scale.x = 1; 00212 //msg11.scale.y = 1; 00213 //msg11.color.r = (*plg)[i].data.misc.color.r/255.; 00214 //msg11.color.g = (*plg)[i].data.misc.color.g/255.; 00215 //msg11.color.b = (*plg)[i].data.misc.color.b/255.; 00216 //msg11.color.a = 1; 00217 00218 //for (size_t u=0; u< (*plg)[i].cp[j].vertex_projectable.points.size(); u++) 00219 //{ 00220 //p.x = (*plg)[i].cp[j].vertex_projectable.points[u].x; 00221 //p.y = (*plg)[i].cp[j].vertex_projectable.points[u].y; 00222 //p.z = (*plg)[i].cp[j].vertex_projectable.points[u].z; 00224 00225 //msg11.points.push_back(p); 00226 00227 //uint32_t rgb = (uint32_t)(*plg)[i].cp[j].vertex_projectable.points[u].rgb; 00228 //uint8_t r = (rgb >> 16) & 0x0000ff; 00229 //uint8_t g = (rgb >> 8) & 0x0000ff; 00230 //uint8_t b = (rgb) & 0x0000ff; 00231 00232 00233 //color.r = (float)r/255.0; 00234 //color.g = (float)g/255.0; 00235 //color.b = (float)b/255.0; 00236 //color.r = 1.0; 00237 //color.g = 0.0; 00238 //color.b = 0.0; 00239 //color.a =1; 00240 00241 //msg11.colors.push_back(color); 00242 00243 //} 00244 00245 //marker_vec.push_back(msg11); 00246 //} 00247 } 00248 00249 // marker_array_msg->set_markers_vec(marker_vec); 00250 } 00251 00252 00253 00254 00255 #endif 00256