22 #include "visualization_thread.h"
23 #include "cluster_colors.h"
25 #include <core/threading/mutex_locker.h>
26 #include <utils/math/angle.h>
29 #include <visualization_msgs/MarkerArray.h>
31 # include <geometry_msgs/PointStamped.h>
33 #include <Eigen/Geometry>
37 #ifdef HAVE_QHULL_2011
38 # include "libqhull/libqhull.h"
39 # include "libqhull/mem.h"
40 # include "libqhull/qset.h"
41 # include "libqhull/geom.h"
42 # include "libqhull/merge.h"
43 # include "libqhull/poly.h"
44 # include "libqhull/io.h"
45 # include "libqhull/stat.h"
47 # include "qhull/qhull.h"
48 # include "qhull/mem.h"
49 # include "qhull/qset.h"
50 # include "qhull/geom.h"
51 # include "qhull/merge.h"
52 # include "qhull/poly.h"
53 # include "qhull/io.h"
54 # include "qhull/stat.h"
58 #define CFG_PREFIX "/perception/tabletop-objects/"
59 #define CFG_PREFIX_VIS "/perception/tabletop-objects/visualization/"
61 using namespace fawkes;
72 : fawkes::
Thread(
"TabletopVisualizationThread",
Thread::OPMODE_WAITFORWAKEUP)
81 cfg_show_frustrum_ =
false;
82 cfg_show_cvxhull_vertices_ =
true;
83 cfg_show_cvxhull_line_highlighting_ =
true;
84 cfg_show_cvxhull_vertex_ids_ =
true;
86 cfg_show_frustrum_ =
config->
get_bool(CFG_PREFIX_VIS
"show_frustrum");
88 if (cfg_show_frustrum_) {
94 cfg_duration_ =
config->
get_uint(CFG_PREFIX_VIS
"display_duration");
98 cfg_show_cvxhull_vertices_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_vertices");
101 cfg_show_cvxhull_line_highlighting_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_line_highlighting");
104 cfg_show_cvxhull_vertex_ids_ =
config->
get_bool(CFG_PREFIX_VIS
"show_convex_hull_vertex_ids");
107 vispub_ =
new ros::Publisher();
108 *vispub_ =
rosnode->advertise<visualization_msgs::MarkerArray>(
"visualization_marker_array", 100);
110 posepub_ =
new ros::Publisher();
111 *posepub_ =
rosnode->advertise<geometry_msgs::PointStamped>(
"table_point", 10);
119 visualization_msgs::MarkerArray m;
121 for (
size_t i = 0; i < last_id_num_; ++i) {
122 visualization_msgs::Marker delop;
123 delop.header.frame_id = frame_id_;
124 delop.header.stamp = ros::Time::now();
125 delop.ns =
"tabletop";
127 delop.action = visualization_msgs::Marker::DELETE;
128 m.markers.push_back(delop);
136 posepub_->shutdown();
146 visualization_msgs::MarkerArray m;
148 unsigned int idnum = 0;
150 for (
size_t i = 0; i < centroids_.size(); ++i) {
153 centroid(tf::Point(centroids_[i][0], centroids_[i][1], centroids_[i][2]),
159 if (asprintf(&tmp,
"TObj %zu", i) != -1) {
161 std::string
id = tmp;
164 visualization_msgs::Marker text;
165 text.header.frame_id =
"/base_link";
166 text.header.stamp = ros::Time::now();
167 text.ns =
"tabletop";
169 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
170 text.action = visualization_msgs::Marker::ADD;
171 text.pose.position.x = baserel_centroid[0];
172 text.pose.position.y = baserel_centroid[1];
173 text.pose.position.z = baserel_centroid[2] + 0.13;
174 text.pose.orientation.w = 1.;
176 text.color.r = text.color.g = text.color.b = 1.0f;
178 text.lifetime = ros::Duration(cfg_duration_, 0);
180 m.markers.push_back(text);
183 visualization_msgs::Marker sphere;
184 sphere.header.frame_id =
"/base_link";
185 sphere.header.stamp = ros::Time::now();
186 sphere.ns =
"tabletop";
188 sphere.type = visualization_msgs::Marker::CYLINDER;
189 sphere.action = visualization_msgs::Marker::ADD;
190 sphere.pose.position.x = baserel_centroid[0];
191 sphere.pose.position.y = baserel_centroid[1];
192 sphere.pose.position.z = baserel_centroid[2];
193 sphere.pose.orientation.w = 1.;
194 sphere.scale.x = sphere.scale.y = 0.08;
195 sphere.scale.z = 0.09;
196 sphere.color.r = (float)cluster_colors[i][0] / 255.f;
197 sphere.color.g = (float)cluster_colors[i][1] / 255.f;
198 sphere.color.b = (float)cluster_colors[i][2] / 255.f;
199 sphere.color.a = 1.0;
200 sphere.lifetime = ros::Duration(cfg_duration_, 0);
201 m.markers.push_back(sphere);
205 Eigen::Vector4f normal_end = (table_centroid_ + (normal_ * -0.15));
207 visualization_msgs::Marker normal;
208 normal.header.frame_id = frame_id_;
209 normal.header.stamp = ros::Time::now();
210 normal.ns =
"tabletop";
212 normal.type = visualization_msgs::Marker::ARROW;
213 normal.action = visualization_msgs::Marker::ADD;
214 normal.points.resize(2);
215 normal.points[0].x = table_centroid_[0];
216 normal.points[0].y = table_centroid_[1];
217 normal.points[0].z = table_centroid_[2];
218 normal.points[1].x = normal_end[0];
219 normal.points[1].y = normal_end[1];
220 normal.points[1].z = normal_end[2];
221 normal.scale.x = 0.02;
222 normal.scale.y = 0.04;
223 normal.color.r = 0.4;
224 normal.color.g = normal.color.b = 0.f;
225 normal.color.a = 1.0;
226 normal.lifetime = ros::Duration(cfg_duration_, 0);
227 m.markers.push_back(normal);
229 if (cfg_show_cvxhull_line_highlighting_) {
231 visualization_msgs::Marker hull_lines;
232 hull_lines.header.frame_id = frame_id_;
233 hull_lines.header.stamp = ros::Time::now();
234 hull_lines.ns =
"tabletop";
235 hull_lines.id = idnum++;
236 hull_lines.type = visualization_msgs::Marker::LINE_LIST;
237 hull_lines.action = visualization_msgs::Marker::ADD;
238 hull_lines.points.resize(good_table_hull_edges_.size());
239 hull_lines.colors.resize(good_table_hull_edges_.size());
240 for (
size_t i = 0; i < good_table_hull_edges_.size(); ++i) {
241 hull_lines.points[i].x = good_table_hull_edges_[i][0];
242 hull_lines.points[i].y = good_table_hull_edges_[i][1];
243 hull_lines.points[i].z = good_table_hull_edges_[i][2];
244 hull_lines.colors[i].r = 0.;
245 hull_lines.colors[i].b = 0.;
246 hull_lines.colors[i].a = 0.4;
247 if (good_table_hull_edges_[i][3] > 0.) {
248 hull_lines.colors[i].g = 1.0;
250 hull_lines.colors[i].g = 0.5;
253 hull_lines.color.a = 1.0;
254 hull_lines.scale.x = 0.01;
255 hull_lines.lifetime = ros::Duration(cfg_duration_, 0);
256 m.markers.push_back(hull_lines);
259 visualization_msgs::Marker hull;
260 hull.header.frame_id = frame_id_;
261 hull.header.stamp = ros::Time::now();
262 hull.ns =
"tabletop";
264 hull.type = visualization_msgs::Marker::LINE_STRIP;
265 hull.action = visualization_msgs::Marker::ADD;
266 hull.points.resize(table_hull_vertices_.size() + 1);
267 for (
size_t i = 0; i < table_hull_vertices_.size(); ++i) {
268 hull.points[i].x = table_hull_vertices_[i][0];
269 hull.points[i].y = table_hull_vertices_[i][1];
270 hull.points[i].z = table_hull_vertices_[i][2];
272 hull.points[table_hull_vertices_.size()].x = table_hull_vertices_[0][0];
273 hull.points[table_hull_vertices_.size()].y = table_hull_vertices_[0][1];
274 hull.points[table_hull_vertices_.size()].z = table_hull_vertices_[0][2];
275 hull.scale.x = 0.005;
277 hull.color.g = hull.color.b = 0.f;
279 hull.lifetime = ros::Duration(cfg_duration_, 0);
280 m.markers.push_back(hull);
283 if (cfg_show_cvxhull_vertices_) {
284 visualization_msgs::Marker hull_points;
285 hull_points.header.frame_id = frame_id_;
286 hull_points.header.stamp = ros::Time::now();
287 hull_points.ns =
"tabletop";
288 hull_points.id = idnum++;
289 hull_points.type = visualization_msgs::Marker::SPHERE_LIST;
290 hull_points.action = visualization_msgs::Marker::ADD;
291 hull_points.points.resize(table_hull_vertices_.size());
292 for (
size_t i = 0; i < table_hull_vertices_.size(); ++i) {
293 hull_points.points[i].x = table_hull_vertices_[i][0];
294 hull_points.points[i].y = table_hull_vertices_[i][1];
295 hull_points.points[i].z = table_hull_vertices_[i][2];
297 hull_points.scale.x = 0.01;
298 hull_points.scale.y = 0.01;
299 hull_points.scale.z = 0.01;
300 hull_points.color.r = 0.8;
301 hull_points.color.g = hull_points.color.b = 0.f;
302 hull_points.color.a = 1.0;
303 hull_points.lifetime = ros::Duration(cfg_duration_, 0);
304 m.markers.push_back(hull_points);
308 if (cfg_show_cvxhull_vertex_ids_) {
309 for (
size_t i = 0; i < table_hull_vertices_.size(); ++i) {
312 if (asprintf(&tmp,
"Cvx_%zu", i) != -1) {
314 std::string
id = tmp;
317 visualization_msgs::Marker text;
318 text.header.frame_id = frame_id_;
319 text.header.stamp = ros::Time::now();
320 text.ns =
"tabletop";
322 text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
323 text.action = visualization_msgs::Marker::ADD;
324 text.pose.position.x = table_hull_vertices_[i][0];
325 text.pose.position.y = table_hull_vertices_[i][1];
326 text.pose.position.z = table_hull_vertices_[i][2] + 0.1;
327 text.pose.orientation.w = 1.;
329 text.color.r = text.color.g = text.color.b = 1.0f;
331 text.lifetime = ros::Duration(cfg_duration_, 0);
333 m.markers.push_back(text);
339 if (! table_model_vertices_.empty()) {
340 visualization_msgs::Marker hull_model;
341 hull_model.header.frame_id = frame_id_;
342 hull_model.header.stamp = ros::Time::now();
343 hull_model.ns =
"tabletop";
344 hull_model.id = idnum++;
345 hull_model.type = visualization_msgs::Marker::LINE_STRIP;
346 hull_model.action = visualization_msgs::Marker::ADD;
347 hull_model.points.resize(table_model_vertices_.size() + 1);
348 for (
size_t i = 0; i < table_model_vertices_.size(); ++i) {
349 hull_model.points[i].x = table_model_vertices_[i][0];
350 hull_model.points[i].y = table_model_vertices_[i][1];
351 hull_model.points[i].z = table_model_vertices_[i][2];
353 hull_model.points[table_model_vertices_.size()].x = table_model_vertices_[0][0];
354 hull_model.points[table_model_vertices_.size()].y = table_model_vertices_[0][1];
355 hull_model.points[table_model_vertices_.size()].z = table_model_vertices_[0][2];
356 hull_model.scale.x = 0.0075;
357 hull_model.color.r = 0.5;
358 hull_model.color.g = hull_model.color.b = 0.f;
359 hull_model.color.a = 1.0;
360 hull_model.lifetime = ros::Duration(cfg_duration_, 0);
361 m.markers.push_back(hull_model);
366 if (table_model_vertices_.size() == 4) {
367 visualization_msgs::Marker plane;
368 plane.header.frame_id = frame_id_;
369 plane.header.stamp = ros::Time::now();
370 plane.ns =
"tabletop";
372 plane.type = visualization_msgs::Marker::TRIANGLE_LIST;
373 plane.action = visualization_msgs::Marker::ADD;
374 plane.points.resize(6);
375 for (
unsigned int i = 0; i < 3; ++i) {
376 plane.points[i].x = table_model_vertices_[i][0];
377 plane.points[i].y = table_model_vertices_[i][1];
378 plane.points[i].z = table_model_vertices_[i][2];
380 for (
unsigned int i = 2; i < 5; ++i) {
381 plane.points[i+1].x = table_model_vertices_[i % 4][0];
382 plane.points[i+1].y = table_model_vertices_[i % 4][1];
383 plane.points[i+1].z = table_model_vertices_[i % 4][2];
385 plane.pose.orientation.w = 1.;
389 plane.color.r = ((float)table_color[0] / 255.f) * 0.8;
390 plane.color.g = ((float)table_color[1] / 255.f) * 0.8;
391 plane.color.b = ((float)table_color[2] / 255.f) * 0.8;
393 plane.lifetime = ros::Duration(cfg_duration_, 0);
394 m.markers.push_back(plane);
397 if (cfg_show_frustrum_) {
399 visualization_msgs::Marker frustrum;
400 frustrum.header.frame_id = frame_id_;
401 frustrum.header.stamp = ros::Time::now();
402 frustrum.ns =
"tabletop";
403 frustrum.id = idnum++;
404 frustrum.type = visualization_msgs::Marker::LINE_LIST;
405 frustrum.action = visualization_msgs::Marker::ADD;
406 frustrum.points.resize(8);
407 frustrum.points[0].x = frustrum.points[2].x = frustrum.points[4].x = frustrum.points[6].x = 0.;
408 frustrum.points[0].y = frustrum.points[2].y = frustrum.points[4].y = frustrum.points[6].y = 0.;
409 frustrum.points[0].z = frustrum.points[2].z = frustrum.points[4].z = frustrum.points[6].z = 0.;
411 const float half_hva = cfg_horizontal_va_ * 0.5;
412 const float half_vva = cfg_vertical_va_ * 0.5;
414 Eigen::Matrix3f upper_right_m;
416 Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
417 * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
418 Eigen::Vector3f upper_right = upper_right_m * Eigen::Vector3f(4,0,0);
420 Eigen::Matrix3f upper_left_m;
422 Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
423 * Eigen::AngleAxisf(-half_vva, Eigen::Vector3f::UnitY());
424 Eigen::Vector3f upper_left = upper_left_m * Eigen::Vector3f(4,0,0);
426 Eigen::Matrix3f lower_right_m;
428 Eigen::AngleAxisf(-half_hva, Eigen::Vector3f::UnitZ())
429 * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
430 Eigen::Vector3f lower_right = lower_right_m * Eigen::Vector3f(2,0,0);
432 Eigen::Matrix3f lower_left_m;
434 Eigen::AngleAxisf(half_hva, Eigen::Vector3f::UnitZ())
435 * Eigen::AngleAxisf(half_vva, Eigen::Vector3f::UnitY());
436 Eigen::Vector3f lower_left = lower_left_m * Eigen::Vector3f(2,0,0);
438 frustrum.points[1].x = upper_right[0];
439 frustrum.points[1].y = upper_right[1];
440 frustrum.points[1].z = upper_right[2];
442 frustrum.points[3].x = lower_right[0];
443 frustrum.points[3].y = lower_right[1];
444 frustrum.points[3].z = lower_right[2];
446 frustrum.points[5].x = lower_left[0];
447 frustrum.points[5].y = lower_left[1];
448 frustrum.points[5].z = lower_left[2];
450 frustrum.points[7].x = upper_left[0];
451 frustrum.points[7].y = upper_left[1];
452 frustrum.points[7].z = upper_left[2];
454 frustrum.scale.x = 0.005;
455 frustrum.color.r = 1.0;
456 frustrum.color.g = frustrum.color.b = 0.f;
457 frustrum.color.a = 1.0;
458 frustrum.lifetime = ros::Duration(cfg_duration_, 0);
459 m.markers.push_back(frustrum);
462 visualization_msgs::Marker frustrum_triangles;
463 frustrum_triangles.header.frame_id = frame_id_;
464 frustrum_triangles.header.stamp = ros::Time::now();
465 frustrum_triangles.ns =
"tabletop";
466 frustrum_triangles.id = idnum++;
467 frustrum_triangles.type = visualization_msgs::Marker::TRIANGLE_LIST;
468 frustrum_triangles.action = visualization_msgs::Marker::ADD;
469 frustrum_triangles.points.resize(9);
470 frustrum_triangles.points[0].x =
471 frustrum_triangles.points[3].x = frustrum_triangles.points[6].x = 0.;
472 frustrum_triangles.points[0].y =
473 frustrum_triangles.points[3].y = frustrum_triangles.points[3].y = 0.;
474 frustrum_triangles.points[0].z
475 = frustrum_triangles.points[3].z = frustrum_triangles.points[3].z = 0.;
477 frustrum_triangles.points[1].x = upper_right[0];
478 frustrum_triangles.points[1].y = upper_right[1];
479 frustrum_triangles.points[1].z = upper_right[2];
481 frustrum_triangles.points[2].x = lower_right[0];
482 frustrum_triangles.points[2].y = lower_right[1];
483 frustrum_triangles.points[2].z = lower_right[2];
485 frustrum_triangles.points[4].x = lower_left[0];
486 frustrum_triangles.points[4].y = lower_left[1];
487 frustrum_triangles.points[4].z = lower_left[2];
489 frustrum_triangles.points[5].x = upper_left[0];
490 frustrum_triangles.points[5].y = upper_left[1];
491 frustrum_triangles.points[5].z = upper_left[2];
493 frustrum_triangles.points[7].x = lower_left[0];
494 frustrum_triangles.points[7].y = lower_left[1];
495 frustrum_triangles.points[7].z = lower_left[2];
497 frustrum_triangles.points[8].x = lower_right[0];
498 frustrum_triangles.points[8].y = lower_right[1];
499 frustrum_triangles.points[8].z = lower_right[2];
501 frustrum_triangles.scale.x = 1;
502 frustrum_triangles.scale.y = 1;
503 frustrum_triangles.scale.z = 1;
504 frustrum_triangles.color.r = 1.0;
505 frustrum_triangles.color.g = frustrum_triangles.color.b = 0.f;
506 frustrum_triangles.color.a = 0.23;
507 frustrum_triangles.lifetime = ros::Duration(cfg_duration_, 0);
508 m.markers.push_back(frustrum_triangles);
511 for (
size_t i = idnum; i < last_id_num_; ++i) {
512 visualization_msgs::Marker delop;
513 delop.header.frame_id = frame_id_;
514 delop.header.stamp = ros::Time::now();
515 delop.ns =
"tabletop";
517 delop.action = visualization_msgs::Marker::DELETE;
518 m.markers.push_back(delop);
520 last_id_num_ = idnum;
525 geometry_msgs::PointStamped p;
526 p.header.frame_id = frame_id_;
527 p.header.stamp = ros::Time::now();
528 p.point.x = table_centroid_[0];
529 p.point.y = table_centroid_[1];
530 p.point.z = table_centroid_[2];
531 posepub_->publish(p);
547 Eigen::Vector4f &table_centroid,
548 Eigen::Vector4f &normal,
555 frame_id_ = frame_id;
556 table_centroid_ = table_centroid;
558 table_hull_vertices_ = table_hull_vertices;
559 table_model_vertices_ = table_model_vertices;
560 good_table_hull_edges_ = good_table_hull_edges;
561 centroids_ = centroids;
567 TabletopVisualizationThread::triangulate_hull()
569 if (table_model_vertices_.empty()) {
570 table_triangle_vertices_.clear();
579 boolT ismalloc = True;
583 char *flags =
const_cast<char *
>(
"qhull Qt Pp");;
584 FILE *outfile = NULL;
585 FILE *errfile = stderr;
588 coordT *points = (coordT *)calloc(table_model_vertices_.size() * dim,
sizeof(coordT));
590 for (
size_t i = 0; i < table_model_vertices_.size(); ++i)
592 points[i * dim + 0] = (coordT)table_model_vertices_[i][0];
593 points[i * dim + 1] = (coordT)table_model_vertices_[i][1];
594 points[i * dim + 2] = (coordT)table_model_vertices_[i][2];
598 int exitcode = qh_new_qhull(dim, table_model_vertices_.size(), points,
599 ismalloc, flags, outfile, errfile);
604 qh_freeqhull(!qh_ALL);
605 int curlong, totlong;
606 qh_memfreeshort (&curlong, &totlong);
612 int num_facets = qh num_facets;
614 table_triangle_vertices_.resize(num_facets * dim);
620 int vertex_n, vertex_i;
621 FOREACHvertex_i_(facet->vertices)
623 table_triangle_vertices_[i + vertex_i][0] = vertex->point[0];
624 table_triangle_vertices_[i + vertex_i][1] = vertex->point[1];
625 table_triangle_vertices_[i + vertex_i][2] = vertex->point[2];
632 qh_freeqhull(!qh_ALL);
633 int curlong, totlong;
634 qh_memfreeshort(&curlong, &totlong);