Fawkes API
Fawkes Development Version
|
00001 00002 /*************************************************************************** 00003 * visualization_thread.cpp - Visualization via rviz 00004 * 00005 * Created: Fri Nov 11 00:20:45 2011 00006 * Copyright 2011 Tim Niemueller [www.niemueller.de] 00007 ****************************************************************************/ 00008 00009 /* This program is free software; you can redistribute it and/or modify 00010 * it under the terms of the GNU General Public License as published by 00011 * the Free Software Foundation; either version 2 of the License, or 00012 * (at your option) any later version. 00013 * 00014 * This program is distributed in the hope that it will be useful, 00015 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00016 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00017 * GNU Library General Public License for more details. 00018 * 00019 * Read the full text in the LICENSE.GPL file in the doc directory. 00020 */ 00021 00022 #include "visualization_thread.h" 00023 #include "cluster_colors.h" 00024 00025 #include <core/threading/mutex_locker.h> 00026 #include <utils/math/angle.h> 00027 00028 #include <ros/ros.h> 00029 #include <visualization_msgs/MarkerArray.h> 00030 #ifdef USE_POSEPUB 00031 # include <geometry_msgs/PointStamped.h> 00032 #endif 00033 #include <Eigen/Geometry> 00034 00035 extern "C" 00036 { 00037 #ifdef HAVE_QHULL_2011 00038 # include "libqhull/libqhull.h" 00039 # include "libqhull/mem.h" 00040 # include "libqhull/qset.h" 00041 # include "libqhull/geom.h" 00042 # include "libqhull/merge.h" 00043 # include "libqhull/poly.h" 00044 # include "libqhull/io.h" 00045 # include "libqhull/stat.h" 00046 #else 00047 # include "qhull/qhull.h" 00048 # include "qhull/mem.h" 00049 # include "qhull/qset.h" 00050 # include "qhull/geom.h" 00051 # include "qhull/merge.h" 00052 # include "qhull/poly.h" 00053 # include "qhull/io.h" 00054 # include "qhull/stat.h" 00055 #endif 00056 } 00057 00058 #define CFG_PREFIX "/perception/tabletop-objects/" 00059 #define CFG_PREFIX_VIS "/perception/tabletop-objects/visualization/" 00060 00061 using namespace fawkes; 00062 00063 /** @class TabletopVisualizationThread "visualization_thread.h" 00064 * Send Marker messages to rviz. 00065 * This class takes input from the table top object detection thread and 00066 * publishes according marker messages for visualization in rviz. 00067 * @author Tim Niemueller 00068 */ 00069 00070 /** Constructor. */ 00071 TabletopVisualizationThread::TabletopVisualizationThread() 00072 : fawkes::Thread("TabletopVisualizationThread", Thread::OPMODE_WAITFORWAKEUP) 00073 { 00074 set_coalesce_wakeups(true); 00075 } 00076 00077 00078 void 00079 TabletopVisualizationThread::init() 00080 { 00081 cfg_show_frustrum_ = false; 00082 cfg_show_cvxhull_vertices_ = true; 00083 cfg_show_cvxhull_line_highlighting_ = true; 00084 cfg_show_cvxhull_vertex_ids_ = true; 00085 try { 00086 cfg_show_frustrum_ = config->get_bool(CFG_PREFIX_VIS"show_frustrum"); 00087 } catch (Exception &e) {} // ignored, use default 00088 if (cfg_show_frustrum_) { 00089 cfg_horizontal_va_ = deg2rad(config->get_float(CFG_PREFIX"horizontal_viewing_angle")); 00090 cfg_vertical_va_ = deg2rad(config->get_float(CFG_PREFIX"vertical_viewing_angle")); 00091 } 00092 cfg_duration_ = 120; 00093 try { 00094 cfg_duration_ = config->get_uint(CFG_PREFIX_VIS"display_duration"); 00095 } catch (Exception &e) {} // ignored, use default 00096 00097 try { 00098 cfg_show_cvxhull_vertices_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_vertices"); 00099 } catch (Exception &e) {} // ignored, use default 00100 try { 00101 cfg_show_cvxhull_line_highlighting_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_line_highlighting"); 00102 } catch (Exception &e) {} // ignored, use default 00103 try { 00104 cfg_show_cvxhull_vertex_ids_ = config->get_bool(CFG_PREFIX_VIS"show_convex_hull_vertex_ids"); 00105 } catch (Exception &e) {} // ignored, use default 00106 00107 vispub_ = new ros::Publisher(); 00108 *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100); 00109 #ifdef USE_POSEPUB 00110 posepub_ = new ros::Publisher(); 00111 *posepub_ = rosnode->advertise<geometry_msgs::PointStamped>("table_point", 10); 00112 #endif 00113 last_id_num_ = 0; 00114 } 00115 00116 void 00117 TabletopVisualizationThread::finalize() 00118 { 00119 visualization_msgs::MarkerArray m; 00120 00121 for (size_t i = 0; i < last_id_num_; ++i) { 00122 visualization_msgs::Marker delop; 00123 delop.header.frame_id = frame_id_; 00124 delop.header.stamp = ros::Time::now(); 00125 delop.ns = "tabletop"; 00126 delop.id = i; 00127 delop.action = visualization_msgs::Marker::DELETE; 00128 m.markers.push_back(delop); 00129 } 00130 vispub_->publish(m); 00131 00132 00133 vispub_->shutdown(); 00134 delete vispub_; 00135 #ifdef USE_POSEPUB 00136 posepub_->shutdown(); 00137 delete posepub_; 00138 #endif 00139 } 00140 00141 00142 void 00143 TabletopVisualizationThread::loop() 00144 { 00145 MutexLocker lock(&mutex_); 00146 visualization_msgs::MarkerArray m; 00147 00148 unsigned int idnum = 0; 00149 00150 for (size_t i = 0; i < centroids_.size(); ++i) { 00151 try { 00152 tf::Stamped<tf::Point> 00153 centroid(tf::Point(centroids_[i][0], centroids_[i][1], centroids_[i][2]), 00154 fawkes::Time(0, 0), frame_id_); 00155 tf::Stamped<tf::Point> baserel_centroid; 00156 tf_listener->transform_point("/base_link", centroid, baserel_centroid); 00157 00158 char *tmp; 00159 if (asprintf(&tmp, "TObj %zu", i) != -1) { 00160 // Copy to get memory freed on exception 00161 std::string id = tmp; 00162 free(tmp); 00163 00164 visualization_msgs::Marker text; 00165 text.header.frame_id = "/base_link"; 00166 text.header.stamp = ros::Time::now(); 00167 text.ns = "tabletop"; 00168 text.id = idnum++; 00169 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 00170 text.action = visualization_msgs::Marker::ADD; 00171 text.pose.position.x = baserel_centroid[0]; 00172 text.pose.position.y = baserel_centroid[1]; 00173 text.pose.position.z = baserel_centroid[2] + 0.13; 00174 text.pose.orientation.w = 1.; 00175 text.scale.z = 0.05; // 5cm high 00176 text.color.r = text.color.g = text.color.b = 1.0f; 00177 text.color.a = 1.0; 00178 text.lifetime = ros::Duration(cfg_duration_, 0); 00179 text.text = id; 00180 m.markers.push_back(text); 00181 } 00182 00183 visualization_msgs::Marker sphere; 00184 sphere.header.frame_id = "/base_link"; 00185 sphere.header.stamp = ros::Time::now(); 00186 sphere.ns = "tabletop"; 00187 sphere.id = idnum++; 00188 sphere.type = visualization_msgs::Marker::CYLINDER; 00189 sphere.action = visualization_msgs::Marker::ADD; 00190 sphere.pose.position.x = baserel_centroid[0]; 00191 sphere.pose.position.y = baserel_centroid[1]; 00192 sphere.pose.position.z = baserel_centroid[2]; 00193 sphere.pose.orientation.w = 1.; 00194 sphere.scale.x = sphere.scale.y = 0.08; 00195 sphere.scale.z = 0.09; 00196 sphere.color.r = (float)cluster_colors[i][0] / 255.f; 00197 sphere.color.g = (float)cluster_colors[i][1] / 255.f; 00198 sphere.color.b = (float)cluster_colors[i][2] / 255.f; 00199 sphere.color.a = 1.0; 00200 sphere.lifetime = ros::Duration(cfg_duration_, 0); 00201 m.markers.push_back(sphere); 00202 } catch (tf::TransformException &e) {} // ignored 00203 } 00204 00205 Eigen::Vector4f normal_end = (table_centroid_ + (normal_ * -0.15)); 00206 00207 visualization_msgs::Marker normal; 00208 normal.header.frame_id = frame_id_; 00209 normal.header.stamp = ros::Time::now(); 00210 normal.ns = "tabletop"; 00211 normal.id = idnum++; 00212 normal.type = visualization_msgs::Marker::ARROW; 00213 normal.action = visualization_msgs::Marker::ADD; 00214 normal.points.resize(2); 00215 normal.points[0].x = table_centroid_[0]; 00216 normal.points[0].y = table_centroid_[1]; 00217 normal.points[0].z = table_centroid_[2]; 00218 normal.points[1].x = normal_end[0]; 00219 normal.points[1].y = normal_end[1]; 00220 normal.points[1].z = normal_end[2]; 00221 normal.scale.x = 0.02; 00222 normal.scale.y = 0.04; 00223 normal.color.r = 0.4; 00224 normal.color.g = normal.color.b = 0.f; 00225 normal.color.a = 1.0; 00226 normal.lifetime = ros::Duration(cfg_duration_, 0); 00227 m.markers.push_back(normal); 00228 00229 if (cfg_show_cvxhull_line_highlighting_) { 00230 // "Good" lines are highlighted 00231 visualization_msgs::Marker hull_lines; 00232 hull_lines.header.frame_id = frame_id_; 00233 hull_lines.header.stamp = ros::Time::now(); 00234 hull_lines.ns = "tabletop"; 00235 hull_lines.id = idnum++; 00236 hull_lines.type = visualization_msgs::Marker::LINE_LIST; 00237 hull_lines.action = visualization_msgs::Marker::ADD; 00238 hull_lines.points.resize(good_table_hull_edges_.size()); 00239 hull_lines.colors.resize(good_table_hull_edges_.size()); 00240 for (size_t i = 0; i < good_table_hull_edges_.size(); ++i) { 00241 hull_lines.points[i].x = good_table_hull_edges_[i][0]; 00242 hull_lines.points[i].y = good_table_hull_edges_[i][1]; 00243 hull_lines.points[i].z = good_table_hull_edges_[i][2]; 00244 hull_lines.colors[i].r = 0.; 00245 hull_lines.colors[i].b = 0.; 00246 hull_lines.colors[i].a = 0.4; 00247 if (good_table_hull_edges_[i][3] > 0.) { 00248 hull_lines.colors[i].g = 1.0; 00249 } else { 00250 hull_lines.colors[i].g = 0.5; 00251 } 00252 } 00253 hull_lines.color.a = 1.0; 00254 hull_lines.scale.x = 0.01; 00255 hull_lines.lifetime = ros::Duration(cfg_duration_, 0); 00256 m.markers.push_back(hull_lines); 00257 } else { 00258 // Table surrounding polygon 00259 visualization_msgs::Marker hull; 00260 hull.header.frame_id = frame_id_; 00261 hull.header.stamp = ros::Time::now(); 00262 hull.ns = "tabletop"; 00263 hull.id = idnum++; 00264 hull.type = visualization_msgs::Marker::LINE_STRIP; 00265 hull.action = visualization_msgs::Marker::ADD; 00266 hull.points.resize(table_hull_vertices_.size() + 1); 00267 for (size_t i = 0; i < table_hull_vertices_.size(); ++i) { 00268 hull.points[i].x = table_hull_vertices_[i][0]; 00269 hull.points[i].y = table_hull_vertices_[i][1]; 00270 hull.points[i].z = table_hull_vertices_[i][2]; 00271 } 00272 hull.points[table_hull_vertices_.size()].x = table_hull_vertices_[0][0]; 00273 hull.points[table_hull_vertices_.size()].y = table_hull_vertices_[0][1]; 00274 hull.points[table_hull_vertices_.size()].z = table_hull_vertices_[0][2]; 00275 hull.scale.x = 0.005; 00276 hull.color.r = 0.4; 00277 hull.color.g = hull.color.b = 0.f; 00278 hull.color.a = 0.2; 00279 hull.lifetime = ros::Duration(cfg_duration_, 0); 00280 m.markers.push_back(hull); 00281 } 00282 00283 if (cfg_show_cvxhull_vertices_) { 00284 visualization_msgs::Marker hull_points; 00285 hull_points.header.frame_id = frame_id_; 00286 hull_points.header.stamp = ros::Time::now(); 00287 hull_points.ns = "tabletop"; 00288 hull_points.id = idnum++; 00289 hull_points.type = visualization_msgs::Marker::SPHERE_LIST; 00290 hull_points.action = visualization_msgs::Marker::ADD; 00291 hull_points.points.resize(table_hull_vertices_.size()); 00292 for (size_t i = 0; i < table_hull_vertices_.size(); ++i) { 00293 hull_points.points[i].x = table_hull_vertices_[i][0]; 00294 hull_points.points[i].y = table_hull_vertices_[i][1]; 00295 hull_points.points[i].z = table_hull_vertices_[i][2]; 00296 } 00297 hull_points.scale.x = 0.01; 00298 hull_points.scale.y = 0.01; 00299 hull_points.scale.z = 0.01; 00300 hull_points.color.r = 0.8; 00301 hull_points.color.g = hull_points.color.b = 0.f; 00302 hull_points.color.a = 1.0; 00303 hull_points.lifetime = ros::Duration(cfg_duration_, 0); 00304 m.markers.push_back(hull_points); 00305 } 00306 00307 // hull texts 00308 if (cfg_show_cvxhull_vertex_ids_) { 00309 for (size_t i = 0; i < table_hull_vertices_.size(); ++i) { 00310 00311 char *tmp; 00312 if (asprintf(&tmp, "Cvx_%zu", i) != -1) { 00313 // Copy to get memory freed on exception 00314 std::string id = tmp; 00315 free(tmp); 00316 00317 visualization_msgs::Marker text; 00318 text.header.frame_id = frame_id_; 00319 text.header.stamp = ros::Time::now(); 00320 text.ns = "tabletop"; 00321 text.id = idnum++; 00322 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING; 00323 text.action = visualization_msgs::Marker::ADD; 00324 text.pose.position.x = table_hull_vertices_[i][0]; 00325 text.pose.position.y = table_hull_vertices_[i][1]; 00326 text.pose.position.z = table_hull_vertices_[i][2] + 0.1; 00327 text.pose.orientation.w = 1.; 00328 text.scale.z = 0.03; 00329 text.color.r = text.color.g = text.color.b = 1.0f; 00330 text.color.a = 1.0; 00331 text.lifetime = ros::Duration(cfg_duration_, 0); 00332 text.text = id; 00333 m.markers.push_back(text); 00334 } 00335 } 00336 } 00337 00338 // Table model surrounding polygon 00339 if (! table_model_vertices_.empty()) { 00340 visualization_msgs::Marker hull_model; 00341 hull_model.header.frame_id = frame_id_; 00342 hull_model.header.stamp = ros::Time::now(); 00343 hull_model.ns = "tabletop"; 00344 hull_model.id = idnum++; 00345 hull_model.type = visualization_msgs::Marker::LINE_STRIP; 00346 hull_model.action = visualization_msgs::Marker::ADD; 00347 hull_model.points.resize(table_model_vertices_.size() + 1); 00348 for (size_t i = 0; i < table_model_vertices_.size(); ++i) { 00349 hull_model.points[i].x = table_model_vertices_[i][0]; 00350 hull_model.points[i].y = table_model_vertices_[i][1]; 00351 hull_model.points[i].z = table_model_vertices_[i][2]; 00352 } 00353 hull_model.points[table_model_vertices_.size()].x = table_model_vertices_[0][0]; 00354 hull_model.points[table_model_vertices_.size()].y = table_model_vertices_[0][1]; 00355 hull_model.points[table_model_vertices_.size()].z = table_model_vertices_[0][2]; 00356 hull_model.scale.x = 0.0075; 00357 hull_model.color.r = 0.5; 00358 hull_model.color.g = hull_model.color.b = 0.f; 00359 hull_model.color.a = 1.0; 00360 hull_model.lifetime = ros::Duration(cfg_duration_, 0); 00361 m.markers.push_back(hull_model); 00362 } 00363 00364 //triangulate_hull(); 00365 00366 if (table_model_vertices_.size() == 4) { 00367 visualization_msgs::Marker plane; 00368 plane.header.frame_id = frame_id_; 00369 plane.header.stamp = ros::Time::now(); 00370 plane.ns = "tabletop"; 00371 plane.id = idnum++; 00372 plane.type = visualization_msgs::Marker::TRIANGLE_LIST; 00373 plane.action = visualization_msgs::Marker::ADD; 00374 plane.points.resize(6); 00375 for (unsigned int i = 0; i < 3; ++i) { 00376 plane.points[i].x = table_model_vertices_[i][0]; 00377 plane.points[i].y = table_model_vertices_[i][1]; 00378 plane.points[i].z = table_model_vertices_[i][2]; 00379 } 00380 for (unsigned int i = 2; i < 5; ++i) { 00381 plane.points[i+1].x = table_model_vertices_[i % 4][0]; 00382 plane.points[i+1].y = table_model_vertices_[i % 4][1]; 00383 plane.points[i+1].z = table_model_vertices_[i % 4][2]; 00384 } 00385 plane.pose.orientation.w = 1.; 00386 plane.scale.x = 1.0; 00387 plane.scale.y = 1.0; 00388 plane.scale.z = 1.0; 00389 plane.color.r = ((float)table_color[0] / 255.f) * 0.8; 00390 plane.color.g = ((float)table_color[1] / 255.f) * 0.8; 00391 plane.color.b = ((float)table_color[2] / 255.f) * 0.8; 00392 plane.color.a = 1.0; 00393 plane.lifetime = ros::Duration(cfg_duration_, 0); 00394 m.markers.push_back(plane); 00395 } 00396 00397 if (cfg_show_frustrum_) { 00398 // Frustrum 00399 visualization_msgs::Marker frustrum; 00400 frustrum.header.frame_id = frame_id_; 00401 frustrum.header.stamp = ros::Time::now(); 00402 frustrum.ns = "tabletop"; 00403 frustrum.id = idnum++; 00404 frustrum.type = visualization_msgs::Marker::LINE_LIST; 00405 frustrum.action = visualization_msgs::Marker::ADD; 00406 frustrum.points.resize(8); 00407 frustrum.points[0].x = frustrum.points[2].x = frustrum.points[4].x = frustrum.points[6].x = 0.; 00408 frustrum.points[0].y = frustrum.points[2].y = frustrum.points[4].y = frustrum.points[6].y = 0.; 00409 frustrum.points[0].z = frustrum.points[2].z = frustrum.points[4].z = frustrum.points[6].z = 0.; 00410 00411 const float half_hva = cfg_horizontal_va_ * 0.5; 00412 const float half_vva = cfg_vertical_va_ * 0.5; 00413 00414 Eigen::Matrix3f upper_right_m; 00415 upper_right_m = 00416 Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ()) 00417 * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY()); 00418 Eigen::Vector3f upper_right = upper_right_m * Eigen::Vector3f(4,0,0); 00419 00420 Eigen::Matrix3f upper_left_m; 00421 upper_left_m = 00422 Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ()) 00423 * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY()); 00424 Eigen::Vector3f upper_left = upper_left_m * Eigen::Vector3f(4,0,0); 00425 00426 Eigen::Matrix3f lower_right_m; 00427 lower_right_m = 00428 Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ()) 00429 * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY()); 00430 Eigen::Vector3f lower_right = lower_right_m * Eigen::Vector3f(2,0,0); 00431 00432 Eigen::Matrix3f lower_left_m; 00433 lower_left_m = 00434 Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ()) 00435 * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY()); 00436 Eigen::Vector3f lower_left = lower_left_m * Eigen::Vector3f(2,0,0); 00437 00438 frustrum.points[1].x = upper_right[0]; 00439 frustrum.points[1].y = upper_right[1]; 00440 frustrum.points[1].z = upper_right[2]; 00441 00442 frustrum.points[3].x = lower_right[0]; 00443 frustrum.points[3].y = lower_right[1]; 00444 frustrum.points[3].z = lower_right[2]; 00445 00446 frustrum.points[5].x = lower_left[0]; 00447 frustrum.points[5].y = lower_left[1]; 00448 frustrum.points[5].z = lower_left[2]; 00449 00450 frustrum.points[7].x = upper_left[0]; 00451 frustrum.points[7].y = upper_left[1]; 00452 frustrum.points[7].z = upper_left[2]; 00453 00454 frustrum.scale.x = 0.005; 00455 frustrum.color.r = 1.0; 00456 frustrum.color.g = frustrum.color.b = 0.f; 00457 frustrum.color.a = 1.0; 00458 frustrum.lifetime = ros::Duration(cfg_duration_, 0); 00459 m.markers.push_back(frustrum); 00460 00461 00462 visualization_msgs::Marker frustrum_triangles; 00463 frustrum_triangles.header.frame_id = frame_id_; 00464 frustrum_triangles.header.stamp = ros::Time::now(); 00465 frustrum_triangles.ns = "tabletop"; 00466 frustrum_triangles.id = idnum++; 00467 frustrum_triangles.type = visualization_msgs::Marker::TRIANGLE_LIST; 00468 frustrum_triangles.action = visualization_msgs::Marker::ADD; 00469 frustrum_triangles.points.resize(9); 00470 frustrum_triangles.points[0].x = 00471 frustrum_triangles.points[3].x = frustrum_triangles.points[6].x = 0.; 00472 frustrum_triangles.points[0].y = 00473 frustrum_triangles.points[3].y = frustrum_triangles.points[3].y = 0.; 00474 frustrum_triangles.points[0].z 00475 = frustrum_triangles.points[3].z = frustrum_triangles.points[3].z = 0.; 00476 00477 frustrum_triangles.points[1].x = upper_right[0]; 00478 frustrum_triangles.points[1].y = upper_right[1]; 00479 frustrum_triangles.points[1].z = upper_right[2]; 00480 00481 frustrum_triangles.points[2].x = lower_right[0]; 00482 frustrum_triangles.points[2].y = lower_right[1]; 00483 frustrum_triangles.points[2].z = lower_right[2]; 00484 00485 frustrum_triangles.points[4].x = lower_left[0]; 00486 frustrum_triangles.points[4].y = lower_left[1]; 00487 frustrum_triangles.points[4].z = lower_left[2]; 00488 00489 frustrum_triangles.points[5].x = upper_left[0]; 00490 frustrum_triangles.points[5].y = upper_left[1]; 00491 frustrum_triangles.points[5].z = upper_left[2]; 00492 00493 frustrum_triangles.points[7].x = lower_left[0]; 00494 frustrum_triangles.points[7].y = lower_left[1]; 00495 frustrum_triangles.points[7].z = lower_left[2]; 00496 00497 frustrum_triangles.points[8].x = lower_right[0]; 00498 frustrum_triangles.points[8].y = lower_right[1]; 00499 frustrum_triangles.points[8].z = lower_right[2]; 00500 00501 frustrum_triangles.scale.x = 1; 00502 frustrum_triangles.scale.y = 1; 00503 frustrum_triangles.scale.z = 1; 00504 frustrum_triangles.color.r = 1.0; 00505 frustrum_triangles.color.g = frustrum_triangles.color.b = 0.f; 00506 frustrum_triangles.color.a = 0.23; 00507 frustrum_triangles.lifetime = ros::Duration(cfg_duration_, 0); 00508 m.markers.push_back(frustrum_triangles); 00509 } // end show frustrum 00510 00511 for (size_t i = idnum; i < last_id_num_; ++i) { 00512 visualization_msgs::Marker delop; 00513 delop.header.frame_id = frame_id_; 00514 delop.header.stamp = ros::Time::now(); 00515 delop.ns = "tabletop"; 00516 delop.id = i; 00517 delop.action = visualization_msgs::Marker::DELETE; 00518 m.markers.push_back(delop); 00519 } 00520 last_id_num_ = idnum; 00521 00522 vispub_->publish(m); 00523 00524 #ifdef USE_POSEPUB 00525 geometry_msgs::PointStamped p; 00526 p.header.frame_id = frame_id_; 00527 p.header.stamp = ros::Time::now(); 00528 p.point.x = table_centroid_[0]; 00529 p.point.y = table_centroid_[1]; 00530 p.point.z = table_centroid_[2]; 00531 posepub_->publish(p); 00532 #endif 00533 } 00534 00535 00536 /** Visualize the given data. 00537 * @param frame_id coordinate frame ID 00538 * @param table_centroid table centroid 00539 * @param normal table normal vector 00540 * @param table_hull_vertices vertices of the table convex hull 00541 * @param table_model_vertices vertices belonging to the table model 00542 * @param good_table_hull_edges good chosen edges 00543 * @param centroids object centroids 00544 */ 00545 void 00546 TabletopVisualizationThread::visualize(const std::string &frame_id, 00547 Eigen::Vector4f &table_centroid, 00548 Eigen::Vector4f &normal, 00549 V_Vector4f &table_hull_vertices, 00550 V_Vector4f &table_model_vertices, 00551 V_Vector4f &good_table_hull_edges, 00552 V_Vector4f ¢roids) throw() 00553 { 00554 MutexLocker lock(&mutex_); 00555 frame_id_ = frame_id; 00556 table_centroid_ = table_centroid; 00557 normal_ = normal; 00558 table_hull_vertices_ = table_hull_vertices; 00559 table_model_vertices_ = table_model_vertices; 00560 good_table_hull_edges_ = good_table_hull_edges; 00561 centroids_ = centroids; 00562 wakeup(); 00563 } 00564 00565 00566 void 00567 TabletopVisualizationThread::triangulate_hull() 00568 { 00569 if (table_model_vertices_.empty()) { 00570 table_triangle_vertices_.clear(); 00571 return; 00572 } 00573 00574 00575 // Don't need to, resizing and overwriting them all later 00576 //table_triangle_vertices_.clear(); 00577 00578 // True if qhull should free points in qh_freeqhull() or reallocation 00579 boolT ismalloc = True; 00580 qh DELAUNAY = True; 00581 00582 int dim = 3; 00583 char *flags = const_cast<char *>("qhull Qt Pp");; 00584 FILE *outfile = NULL; 00585 FILE *errfile = stderr; 00586 00587 // Array of coordinates for each point 00588 coordT *points = (coordT *)calloc(table_model_vertices_.size() * dim, sizeof(coordT)); 00589 00590 for (size_t i = 0; i < table_model_vertices_.size(); ++i) 00591 { 00592 points[i * dim + 0] = (coordT)table_model_vertices_[i][0]; 00593 points[i * dim + 1] = (coordT)table_model_vertices_[i][1]; 00594 points[i * dim + 2] = (coordT)table_model_vertices_[i][2]; 00595 } 00596 00597 // Compute convex hull 00598 int exitcode = qh_new_qhull(dim, table_model_vertices_.size(), points, 00599 ismalloc, flags, outfile, errfile); 00600 00601 if (exitcode != 0) { 00602 // error, return empty vector 00603 // Deallocates memory (also the points) 00604 qh_freeqhull(!qh_ALL); 00605 int curlong, totlong; 00606 qh_memfreeshort (&curlong, &totlong); 00607 return; 00608 } 00609 00610 qh_triangulate(); 00611 00612 int num_facets = qh num_facets; 00613 00614 table_triangle_vertices_.resize(num_facets * dim); 00615 facetT *facet; 00616 size_t i = 0; 00617 FORALLfacets 00618 { 00619 vertexT *vertex; 00620 int vertex_n, vertex_i; 00621 FOREACHvertex_i_(facet->vertices) 00622 { 00623 table_triangle_vertices_[i + vertex_i][0] = vertex->point[0]; 00624 table_triangle_vertices_[i + vertex_i][1] = vertex->point[1]; 00625 table_triangle_vertices_[i + vertex_i][2] = vertex->point[2]; 00626 } 00627 00628 i += dim; 00629 } 00630 00631 // Deallocates memory (also the points) 00632 qh_freeqhull(!qh_ALL); 00633 int curlong, totlong; 00634 qh_memfreeshort(&curlong, &totlong); 00635 }