$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id: DynModelExporter.cpp 814 2012-05-22 14:00:19Z ihulik $ 00005 * 00006 * Copyright (C) Brno University of Technology 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Rostislav Hulik (ihulik@fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 11.01.2012 (version 0.8) 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00033 #include <srs_env_model_percp/but_plane_detector/dyn_model_exporter.h> 00034 #include <srs_env_model_percp/topics_list.h> 00035 #include <srs_env_model_percp/services_list.h> 00036 00037 00038 #include <srs_interaction_primitives/AddPlane.h> 00039 #include <srs_interaction_primitives/RemovePrimitive.h> 00040 #include <srs_interaction_primitives/plane.h> 00041 #include <pcl/filters/project_inliers.h> 00042 00043 #include <srs_env_model/InsertPlanes.h> 00044 #include <pcl/point_cloud.h> 00045 00046 #include <pcl/point_types.h> 00047 #include <pcl/io/pcd_io.h> 00048 #include <pcl/kdtree/kdtree_flann.h> 00049 #include <pcl/kdtree/kdtree.h> 00050 #include <pcl/features/normal_3d.h> 00051 #include <pcl/surface/gp3.h> 00052 #include <pcl_ros/transforms.h> 00053 00054 #include <libxml2/libxml/parser.h> 00055 #include <libxml2/libxml/tree.h> 00056 00057 using namespace pcl; 00058 using namespace but_plane_detector; 00059 using namespace cv; 00060 00061 namespace srs_env_model_percp 00062 { 00063 00065 // Updates sent planes using but environment model server 00066 // @param planes Vector of found planes 00067 // @param scene_cloud point cloud of the scene 00069 void DynModelExporter::update(tPlanes & planes, Normals &normals, std::string color_method, cv::Mat rgb) 00070 { 00071 if (m_keep_tracking == 0) 00072 displayed_planes.clear(); 00073 std::vector<PointCloud<pcl::PointXYZ>, Eigen::aligned_allocator<PointCloud<pcl::PointXYZ> > > planesInPCloud(planes.size()); 00074 std::vector<std_msgs::ColorRGBA, Eigen::aligned_allocator<std_msgs::ColorRGBA> > colors(planes.size()); 00075 00076 00077 std::cerr << "updating..." << std::endl; 00078 00079 for (int i = 0; i < normals.m_points.rows; ++i) 00080 for (int j = 0; j < normals.m_points.cols; ++j) 00081 { 00082 Vec3f point = normals.m_points.at<Vec3f>(i, j); 00083 if (point[0] != 0.0 || point[1] != 0.0 || point[2] != 0.0) 00084 { 00085 cv::Vec4f localPlane = normals.m_planes.at<cv::Vec4f>(i, j); 00086 Plane<float> aaa(localPlane[0], localPlane[1], localPlane[2], localPlane[3]); 00087 00088 // find all planes that fit for this point 00089 for (unsigned int a = 0; a < planes.size(); ++a) 00090 { 00091 if (planes[a].distance(point) < m_max_distance && planes[a].isSimilar(aaa, m_max_plane_normal_dev, m_max_plane_shift_dev)) 00092 { 00093 PointXYZ pclpoint(point[0], point[1], point[2]); 00094 planesInPCloud[a].push_back(pclpoint); 00095 00096 if (color_method == "mean_color") 00097 { 00098 cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j); 00099 colors[a].r += (float)color[0]/255.0; 00100 colors[a].g += (float)color[1]/255.0; 00101 colors[a].b += (float)color[2]/255.0; 00102 } 00103 else if (color_method == "mean_color") 00104 { 00105 cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j); 00106 colors[a].r += point[0] / 5.0; 00107 colors[a].g += point[1] / 5.0; 00108 colors[a].b += point[2] / 5.0; 00109 } 00110 } 00111 } 00112 } 00113 } 00114 // for (int i = 0; i < normals.m_points.rows; ++i) 00115 // for (int j = 0; j < normals.m_points.cols; ++j) 00116 // { 00117 // Vec3f point = normals.m_points.at<Vec3f>(i, j); 00118 // if (point[0] != 0.0 || point[1] != 0.0 || point[2] != 0.0) 00119 // { 00120 // cv::Vec4f localPlane = normals.m_planes.at<cv::Vec4f>(i, j); 00121 // Plane<float> aaa(localPlane[0], localPlane[1], localPlane[2], localPlane[3]); 00122 // 00123 // double dist = DBL_MAX; 00124 // 00125 // int chosen = -1; 00126 // // find the best plane 00127 // for (unsigned int a = 0; a < planes.size(); ++a) 00128 // { 00129 // if (planes[a].distance(point) < dist && planes[a].distance(point) < m_max_distance && 00130 // planes[a].isSimilar(aaa, m_max_plane_normal_dev, m_max_plane_shift_dev)) 00131 // { 00132 // dist = planes[a].distance(point); 00133 // chosen = a; 00134 // } 00135 // } 00136 // 00137 // // if there is good plane, insert point into point cloud 00138 // if (chosen > -1) 00139 // { 00140 // PointXYZ pclpoint(point[0], point[1], point[2]); 00141 // planesInPCloud[chosen].push_back(pclpoint); 00142 // 00143 // if (color_method == "mean_color") 00144 // { 00145 // cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j); 00146 // colors[chosen].r += (float)color[0]/255.0; 00147 // colors[chosen].g += (float)color[1]/255.0; 00148 // colors[chosen].b += (float)color[2]/255.0; 00149 // //std::cerr << color[0] << " " << color[1] << " " << color[2] << std::endl; 00150 // } 00151 // else if (color_method == "mean_color") 00152 // { 00153 // cv::Vec<unsigned char, 3> color = rgb.at<cv::Vec<unsigned char, 3> >(i, j); 00154 // colors[chosen].r += point[0] / 5.0; 00155 // colors[chosen].g += point[1] / 5.0; 00156 // colors[chosen].b += point[2] / 5.0; 00157 // //std::cerr << color[0] << " " << color[1] << " " << color[2] << std::endl; 00158 // } 00164 // } 00165 // } 00166 // } 00167 00168 if (color_method == "mean_color" || color_method == "centroid") 00169 { 00170 for (unsigned int i = 0; i < colors.size(); ++i) 00171 if(planesInPCloud.size() > 0) 00172 { 00173 colors[i].r /= planesInPCloud[i].size(); 00174 colors[i].g /= planesInPCloud[i].size(); 00175 colors[i].b /= planesInPCloud[i].size(); 00176 colors[i].a = 1.0; 00177 } 00178 } 00179 // Indexed in point cloud 00181 00182 for (unsigned int j = 0; j < planesInPCloud.size(); ++j) 00183 { 00184 if (planesInPCloud[j].size() > 20) 00185 { 00186 double maxangle = DBL_MAX; 00187 double maxdist = DBL_MAX; 00188 int index = -1; 00189 for (unsigned int i = 0; i < displayed_planes.size(); ++i) 00190 { 00191 if (!(displayed_planes[i].is_deleted)) 00192 { 00193 double angle = acos(((planes[j].a * displayed_planes[i].plane.a) + (planes[j].b * displayed_planes[i].plane.b) + (planes[j].c * displayed_planes[i].plane.c))); 00194 double xd = planes[j].d - displayed_planes[i].plane.d; 00195 xd = (xd > 0 ? xd : - xd); 00196 00197 // Pretty nasty workaround... todo 00198 if (angle != angle) angle = 0.0; 00199 00200 if (angle <= maxangle && xd <= maxdist && angle < m_max_plane_normal_dev && xd < m_max_plane_shift_dev) 00201 { 00202 maxangle = angle; 00203 maxdist = xd; 00204 index = i; 00205 } 00206 } 00207 } 00208 00209 if (index >= 0) 00210 { 00211 std::cerr << "adding marker to concave hull..." << std::endl; 00212 DynModelExporter::addMarkerToConcaveHull(planesInPCloud[j], displayed_planes[index].plane); 00213 displayed_planes[index].update = ros::Time::now(); 00214 } 00215 else 00216 { 00217 ExportedPlane newplane; 00218 newplane.update = ros::Time::now(); 00219 newplane.plane = PlaneExt(planes[j]); 00220 newplane.is_deleted = false; 00221 newplane.to_be_deleted = false; 00222 DynModelExporter::createMarkerForConcaveHull(planesInPCloud[j], newplane.plane); 00223 00224 newplane.id = displayed_planes.size(); 00225 newplane.plane.getMeshMarker().id = newplane.id; 00226 if (color_method == "mean_color") 00227 { 00228 std::cerr << "setting color: " << colors[j].r << " " << colors[j].g << " " << colors[j].b << std::endl; 00229 newplane.plane.setColor(colors[j]); 00230 } 00231 else if (color_method == "random") 00232 { 00233 colors[j].r = (float)rand()/INT_MAX * 0.5 + 0.2; 00234 colors[j].g = (float)rand()/INT_MAX * 0.5 + 0.2; 00235 colors[j].b = (float)rand()/INT_MAX * 0.5 + 0.2; 00236 colors[j].a = 1.0; 00237 newplane.plane.setColor(colors[j]); 00238 } 00239 //newplane.plane.getShapeMarker().id = newplane.id; 00240 displayed_planes.push_back(newplane); 00241 } 00242 } 00243 } 00244 00245 // TTL fix 00246 if (m_plane_ttl > 0) 00247 { 00248 for (unsigned int i = 0; i < displayed_planes.size(); ++i) 00249 { 00250 if (!displayed_planes[i].is_deleted && !displayed_planes[i].to_be_deleted && (ros::Time::now() - displayed_planes[i].update).sec > m_plane_ttl) 00251 { 00252 std::cerr << (ros::Time::now() - displayed_planes[i].update).sec << " "; 00253 displayed_planes[i].is_deleted = true; 00254 displayed_planes[i].to_be_deleted = true; 00255 } 00256 else if (displayed_planes[i].is_deleted && displayed_planes[i].to_be_deleted) 00257 { 00258 displayed_planes[i].to_be_deleted = false; 00259 } 00260 } 00261 } 00262 } 00263 00264 00265 00266 00268 // Initialization 00270 DynModelExporter::DynModelExporter(ros::NodeHandle *node, 00271 const std::string& original_frame, 00272 const std::string& output_frame, 00273 int minOutputCount, 00274 double max_distance, 00275 double max_plane_normal_dev, 00276 double max_plane_shift_dev, 00277 int keep_tracking, 00278 int ttl, 00279 int max_poly_size 00280 ) 00281 : original_frame_(original_frame) 00282 , output_frame_(output_frame) 00283 { 00284 n = node; 00285 m_minOutputCount = minOutputCount; 00286 m_max_distance = max_distance; 00287 m_max_plane_normal_dev = max_plane_normal_dev; 00288 m_max_plane_shift_dev = max_plane_shift_dev; 00289 m_keep_tracking = keep_tracking; 00290 m_plane_ttl = ttl; 00291 m_max_poly_size = max_poly_size; 00292 } 00293 00294 void DynModelExporter::createMarkerForConcaveHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, srs_env_model_percp::PlaneExt& plane) 00295 { 00296 plane.NewPlanePoints(plane_cloud.makeShared()); 00297 plane.getMeshMarker().type = visualization_msgs::Marker::TRIANGLE_LIST; 00298 plane.getMeshMarker().action = visualization_msgs::Marker::ADD; 00299 00300 plane.getMeshMarker().ns = "Normals"; 00301 plane.getMeshMarker().pose.position.x = 0.0; 00302 plane.getMeshMarker().pose.position.y = 0.0; 00303 plane.getMeshMarker().pose.position.z = 0.0; 00304 plane.getMeshMarker().pose.orientation.x = 0.0; 00305 plane.getMeshMarker().pose.orientation.y = 0.0; 00306 plane.getMeshMarker().pose.orientation.z = 0.0; 00307 plane.getMeshMarker().pose.orientation.w = 1.0; 00308 plane.getMeshMarker().scale.x = 1.00; 00309 plane.getMeshMarker().scale.y = 1.00; 00310 plane.getMeshMarker().scale.z = 1.00; 00311 // 00312 00313 } 00314 00315 void DynModelExporter::addMarkerToConcaveHull(pcl::PointCloud<pcl::PointXYZ>& plane_cloud, srs_env_model_percp::PlaneExt& plane) 00316 { 00317 plane.AddPlanePoints(plane_cloud.makeShared(), m_max_poly_size); 00318 } 00319 00320 void DynModelExporter::xmlFileExport(std::string filename) 00321 { 00322 xmlDocPtr document = NULL; 00323 xmlNodePtr root_node = NULL; 00324 xmlNodePtr plane_node = NULL; 00325 xmlNodePtr polys_node = NULL; 00326 xmlNodePtr poly_node = NULL; 00327 xmlNodePtr outer_node = NULL; 00328 xmlNodePtr holes_node = NULL; 00329 xmlNodePtr hole_node = NULL; 00330 xmlNodePtr points_node = NULL; 00331 xmlNodePtr node = NULL; 00332 00334 // XML structure 00335 // 00336 // <planes> 00337 // <plane id=... poly_number=...> 00338 // <equation a=... b=... c=... d=... /> 00339 // <polygons> 00340 // <polygon> 00341 // <outer> 00342 // <points> 00343 // <point x=.. y=..> 00344 // </points> 00345 // </outer> 00346 // <holes> 00347 // <hole> 00348 // <points> 00349 // <point x=.. y=..> 00350 // </points> 00351 // </hole> 00352 // ....... 00353 // </holes> 00354 // </polygon> 00355 // ...... 00356 // </polygons> 00357 // </plane> 00358 // ...... 00359 // </planes> 00361 // Create a new XML document 00362 document = xmlNewDoc(BAD_CAST "1.0"); 00363 root_node = xmlNewNode(NULL, BAD_CAST "planes"); 00364 xmlDocSetRootElement(document, root_node); 00365 00366 for (unsigned int i = 0; i < displayed_planes.size(); ++i) 00367 { 00368 if (!displayed_planes[i].is_deleted) 00369 { 00370 ClipperLib::ExPolygons polygons = displayed_planes[i].plane.getPolygons(); 00371 00372 // save plane global info 00373 plane_node = xmlNewChild(root_node, NULL, BAD_CAST "plane", NULL); 00374 00375 std::stringstream str; 00376 str << i; 00377 xmlNewProp(plane_node, BAD_CAST "id", BAD_CAST str.str().c_str()); 00378 00379 str.str(""); 00380 str << polygons.size(); 00381 xmlNewProp(plane_node, BAD_CAST "poly_number", BAD_CAST str.str().c_str()); 00382 00383 // plane equation 00384 node = xmlNewChild(plane_node, NULL, BAD_CAST "equation", NULL); 00385 00386 str.str(""); 00387 str << displayed_planes[i].plane.a; 00388 xmlNewProp(node, BAD_CAST "a", BAD_CAST str.str().c_str()); 00389 00390 str.str(""); 00391 str << displayed_planes[i].plane.b; 00392 xmlNewProp(node, BAD_CAST "b", BAD_CAST str.str().c_str()); 00393 00394 str.str(""); 00395 str << displayed_planes[i].plane.c; 00396 xmlNewProp(node, BAD_CAST "c", BAD_CAST str.str().c_str()); 00397 00398 str.str(""); 00399 str << displayed_planes[i].plane.d; 00400 xmlNewProp(node, BAD_CAST "d", BAD_CAST str.str().c_str()); 00401 00402 // color 00403 node = xmlNewChild(plane_node, NULL, BAD_CAST "color", NULL); 00404 00405 str.str(""); 00406 str << displayed_planes[i].plane.color.r; 00407 xmlNewProp(node, BAD_CAST "r", BAD_CAST str.str().c_str()); 00408 00409 str.str(""); 00410 str << displayed_planes[i].plane.color.g; 00411 xmlNewProp(node, BAD_CAST "g", BAD_CAST str.str().c_str()); 00412 00413 str.str(""); 00414 str << displayed_planes[i].plane.color.b; 00415 xmlNewProp(node, BAD_CAST "b", BAD_CAST str.str().c_str()); 00416 00417 str.str(""); 00418 str << displayed_planes[i].plane.color.a; 00419 xmlNewProp(node, BAD_CAST "a", BAD_CAST str.str().c_str()); 00420 00421 // save polygons 00422 polys_node = xmlNewChild(plane_node, NULL, BAD_CAST "polygons", NULL); 00423 for (unsigned int j = 0; j < polygons.size(); ++j) 00424 { 00425 poly_node = xmlNewChild(polys_node, NULL, BAD_CAST "polygon", NULL); 00426 00427 str.str(""); 00428 str << j; 00429 xmlNewProp(poly_node, BAD_CAST "id", BAD_CAST str.str().c_str()); 00430 00431 // outer body 00432 outer_node = xmlNewChild(poly_node, NULL, BAD_CAST "outer", NULL); 00433 points_node = xmlNewChild(outer_node, NULL, BAD_CAST "points", NULL); 00434 for (unsigned int k = 0; k < polygons[j].outer.size(); ++k) 00435 { 00436 node = xmlNewChild(points_node, NULL, BAD_CAST "point", NULL); 00437 00438 str.str(""); 00439 str << polygons[j].outer[k].X; 00440 xmlNewProp(node, BAD_CAST "x", BAD_CAST str.str().c_str()); 00441 00442 str.str(""); 00443 str << polygons[j].outer[k].Y; 00444 xmlNewProp(node, BAD_CAST "y", BAD_CAST str.str().c_str()); 00445 } 00446 00447 // // export holes 00448 // holes_node = xmlNewChild(poly_node, NULL, BAD_CAST "holes", NULL); 00449 // for (unsigned int k = 0; k < polygons[j].holes.size(); ++k) 00450 // { 00451 // hole_node = xmlNewChild(holes_node, NULL, BAD_CAST "hole", NULL); 00452 // points_node = xmlNewChild(hole_node, NULL, BAD_CAST "points", NULL); 00453 // for (unsigned int l = 0; l < polygons[j].holes[k].size(); ++l) 00454 // { 00455 // node = xmlNewChild(points_node, NULL, BAD_CAST "point", NULL); 00456 // 00457 // str.str(""); 00458 // str << polygons[j].holes[k][l].X; 00459 // xmlNewProp(node, BAD_CAST "x", BAD_CAST str.str().c_str()); 00460 // 00461 // str.str(""); 00462 // str << polygons[j].holes[k][l].Y; 00463 // xmlNewProp(node, BAD_CAST "y", BAD_CAST str.str().c_str()); 00464 // } 00465 // } 00466 00467 } 00468 } 00469 } 00470 00471 xmlSaveFormatFileEnc(filename.c_str(), document, "UTF-8", 1); 00472 xmlFreeDoc(document); 00473 xmlCleanupParser(); 00474 } 00475 00476 void DynModelExporter::xmlFileImport(std::string filename) 00477 { 00479 // XML structure 00480 // 00481 // <planes> 00482 // <plane id=... poly_number=...> 00483 // <equation a=... b=... c=... d=... /> 00484 // <polygons> 00485 // <polygon> 00486 // <outer> 00487 // <points> 00488 // <point x=.. y=..> 00489 // </points> 00490 // </outer> 00491 // <holes> 00492 // <hole> 00493 // <points> 00494 // <point x=.. y=..> 00495 // </points> 00496 // </hole> 00497 // ....... 00498 // </holes> 00499 // </polygon> 00500 // ...... 00501 // </polygons> 00502 // </plane> 00503 // ...... 00504 // </planes> 00506 xmlDocPtr document = xmlParseFile(filename.c_str()); 00507 00508 if (document->children && xmlStrEqual(document->children->name, (const xmlChar *)"planes")) 00509 { 00510 // pass each node 00511 for (xmlNodePtr plane = document->children->children; plane; plane = plane->next) 00512 { 00513 00514 if (xmlStrEqual(plane->name, (const xmlChar *)"plane")) 00515 { 00516 float a = 0.0; 00517 float b = 0.0; 00518 float c = 0.0; 00519 float d = 0.0; 00520 std_msgs::ColorRGBA color; 00521 color.r = 0.0; 00522 color.g = 0.0; 00523 color.b = 0.0; 00524 color.a = 0.0; 00525 int id = 0; 00526 for (xmlAttrPtr attribute = plane->properties; attribute; attribute = attribute->next) 00527 { 00528 // get id 00529 if (xmlStrEqual(attribute->name, (const xmlChar *)"id")) 00530 { 00531 id = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00532 } 00533 } 00534 00535 ClipperLib::ExPolygons polygons; 00536 for (xmlNodePtr plane_child = plane->children; plane_child; plane_child = plane_child->next) 00537 { 00538 // get equation 00539 if (xmlStrEqual(plane_child->name, (const xmlChar *)"equation")) 00540 for (xmlAttrPtr attribute = plane_child->properties; attribute; attribute = attribute->next) 00541 { 00542 if (xmlStrEqual(attribute->name, (const xmlChar *)"a")) 00543 a = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00544 00545 if (xmlStrEqual(attribute->name, (const xmlChar *)"b")) 00546 b = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00547 00548 if (xmlStrEqual(attribute->name, (const xmlChar *)"c")) 00549 c = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00550 00551 if (xmlStrEqual(attribute->name, (const xmlChar *)"d")) 00552 d = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00553 } 00554 00555 // get color 00556 if (xmlStrEqual(plane_child->name, (const xmlChar *)"color")) 00557 for (xmlAttrPtr attribute = plane_child->properties; attribute; attribute = attribute->next) 00558 { 00559 if (xmlStrEqual(attribute->name, (const xmlChar *)"r")) 00560 color.r = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00561 00562 if (xmlStrEqual(attribute->name, (const xmlChar *)"g")) 00563 color.g = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00564 00565 if (xmlStrEqual(attribute->name, (const xmlChar *)"b")) 00566 color.b = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00567 00568 if (xmlStrEqual(attribute->name, (const xmlChar *)"a")) 00569 color.a = atof(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00570 } 00571 00572 // <POLYGONS> 00573 if (xmlStrEqual(plane_child->name, (const xmlChar *)"polygons")) 00574 for (xmlNodePtr polygon = plane_child->children; polygon; polygon = polygon->next) 00575 { 00576 // <POLYGON> 00577 if (xmlStrEqual(polygon->name, (const xmlChar *)"polygon") ) 00578 { 00579 polygons.push_back(ClipperLib::ExPolygon()); 00580 for (xmlNodePtr node = polygon->children; node; node = node->next) 00581 { 00582 // <OUTER> 00583 if (xmlStrEqual(node->name, (const xmlChar *)"outer")) 00584 { 00585 // <POINTS> 00586 for (xmlNodePtr points = node->children; points; points = points->next) 00587 if (xmlStrEqual(points->name, (const xmlChar *)"points")) 00588 { 00589 // <POINT> 00590 for (xmlNodePtr point = points->children; point; point = point->next) 00591 if (xmlStrEqual(point->name, (const xmlChar *)"point")) 00592 { 00593 int x, y; 00594 for (xmlAttrPtr attribute = point->properties; attribute; attribute = attribute->next) 00595 { 00596 if (xmlStrEqual(attribute->name, (const xmlChar *)"x")) 00597 x = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00598 if (xmlStrEqual(attribute->name, (const xmlChar *)"y")) 00599 y = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00600 } 00601 polygons.back().outer.push_back(ClipperLib::IntPoint(x, y)); 00602 } // point 00603 } // points 00604 } // outer 00605 // //<HOLES> 00606 // else if (xmlStrEqual(node->name, (const xmlChar *)"holes")) 00607 // { 00608 // // <HOLE> 00609 // for (xmlNodePtr hole = node->children; hole; hole = node->next) 00610 // if (xmlStrEqual(hole->name, (const xmlChar *)"hole")) 00611 // { 00612 // polygons.back().holes.push_back(ClipperLib::Polygon()); 00613 // // <POINTS> 00614 // for (xmlNodePtr points = hole->children; points; points = points->next) 00615 // if (xmlStrEqual(points->name, (const xmlChar *)"points")) 00616 // { 00617 // // <POINT> 00618 // for (xmlNodePtr point = points->children; point; point = point->next) 00619 // { 00620 // int x, y; 00621 // for (xmlAttrPtr attribute = point->properties; attribute; attribute = attribute->next) 00622 // { 00623 // if (xmlStrEqual(attribute->name, (const xmlChar *)"x")) 00624 // x = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00625 // if (xmlStrEqual(attribute->name, (const xmlChar *)"y")) 00626 // y = atoi(reinterpret_cast<const char*>(xmlNodeListGetString(document, attribute->children, 1))); 00627 // } 00628 // polygons.back().holes.back().push_back(ClipperLib::IntPoint(x, y)); 00629 // } // point 00630 // } // points 00631 // } // hole 00632 // } // holes 00633 } // polygon children 00634 } // polygon 00635 } // polygons 00636 } // plane 00637 00638 // save new plane 00639 displayed_planes.push_back(ExportedPlane()); 00640 displayed_planes.back().id = id; 00641 displayed_planes.back().plane = PlaneExt(Plane<float>(a, b, c, d)); 00642 displayed_planes.back().plane.setColor(color); 00643 displayed_planes.back().is_deleted = false; 00644 displayed_planes.back().to_be_deleted = false; 00645 displayed_planes.back().update = ros::Time::now(); 00646 00647 displayed_planes.back().plane.setPolygons(polygons); 00648 displayed_planes.back().plane.TriangulatePlanePolygon(); 00649 00650 displayed_planes.back().plane.getMeshMarker().type = visualization_msgs::Marker::TRIANGLE_LIST; 00651 displayed_planes.back().plane.getMeshMarker().action = visualization_msgs::Marker::ADD; 00652 00653 displayed_planes.back().plane.getMeshMarker().ns = "Normals"; 00654 displayed_planes.back().plane.getMeshMarker().pose.position.x = 0.0; 00655 displayed_planes.back().plane.getMeshMarker().pose.position.y = 0.0; 00656 displayed_planes.back().plane.getMeshMarker().pose.position.z = 0.0; 00657 displayed_planes.back().plane.getMeshMarker().pose.orientation.x = 0.0; 00658 displayed_planes.back().plane.getMeshMarker().pose.orientation.y = 0.0; 00659 displayed_planes.back().plane.getMeshMarker().pose.orientation.z = 0.0; 00660 displayed_planes.back().plane.getMeshMarker().pose.orientation.w = 1.0; 00661 displayed_planes.back().plane.getMeshMarker().scale.x = 1.00; 00662 displayed_planes.back().plane.getMeshMarker().scale.y = 1.00; 00663 displayed_planes.back().plane.getMeshMarker().scale.z = 1.00; 00664 displayed_planes.back().plane.getMeshMarker().id = id; 00665 00666 } // planes 00667 } 00668 } 00669 } 00670 00671 void DynModelExporter::getMarkerArray(visualization_msgs::MarkerArray &message, std::string output_frame_id) 00672 { 00673 std::ofstream file; 00674 file.open("/home/rosta/out.csv", std::ios_base::app); 00675 for (unsigned int i = 0; i < displayed_planes.size(); ++i) 00676 { 00677 if (!displayed_planes[i].is_deleted && displayed_planes[i].plane.getMeshMarker().points.size() > 0) 00678 { 00679 message.markers.push_back(displayed_planes[i].plane.getMeshMarker()); 00680 message.markers.back().header.frame_id = output_frame_id; 00681 message.markers.back().header.stamp = ros::Time::now(); 00682 } 00683 else if (displayed_planes[i].to_be_deleted && displayed_planes[i].plane.getMeshMarker().points.size() > 0) 00684 { 00685 message.markers.push_back(displayed_planes[i].plane.getMeshMarker()); 00686 message.markers.back().header.frame_id = output_frame_id; 00687 message.markers.back().header.stamp = ros::Time::now(); 00688 message.markers.back().action = visualization_msgs::Marker::DELETE; 00689 } 00690 } 00691 file.close(); 00692 } 00693 00694 void DynModelExporter::getShapeArray(cob_3d_mapping_msgs::ShapeArray &message, std::string output_frame_id) 00695 { 00696 message.header.frame_id = output_frame_id; 00697 00698 for (unsigned int i = 0; i < displayed_planes.size(); ++i) 00699 { 00700 if (!displayed_planes[i].is_deleted) 00701 { 00702 PlaneExt::tShapeMarker shapes = displayed_planes[i].plane.getShapeMarker(); 00703 for (unsigned int j = 0; j < shapes.size(); ++j) 00704 { 00705 message.shapes.push_back(shapes[j]); 00706 message.shapes.back().header.frame_id=output_frame_id; 00707 message.shapes.back().id = i * 100 + j; 00708 } 00709 } 00710 } 00711 } 00712 00713 00714 }// but_scenemodel