| 353 | } |
| 354 | |
| 355 | void publish_scan_std(const std::vector<STDesc> &scan_std, |
| 356 | const ros::Publisher &std_publisher) { |
| 357 | visualization_msgs::MarkerArray ma_line; |
| 358 | visualization_msgs::Marker m_line; |
| 359 | m_line.type = visualization_msgs::Marker::LINE_LIST; |
| 360 | m_line.action = visualization_msgs::Marker::ADD; |
| 361 | m_line.ns = "my_namespace"; |
| 362 | // Don't forget to set the alpha! |
| 363 | m_line.scale.x = 0.2; |
| 364 | m_line.pose.orientation.w = 1.0; |
| 365 | m_line.header.frame_id = "map"; |
| 366 | m_line.id = 1000000; |
| 367 | int max_pub_cnt = 1; |
| 368 | for (auto var : scan_std) { |
| 369 | max_pub_cnt++; |
| 370 | m_line.color.a = 0.8; |
| 371 | m_line.points.clear(); |
| 372 | m_line.color.r = 33.0 / 255; |
| 373 | m_line.color.g = 162.0 / 255; |
| 374 | m_line.color.b = 202.0 / 255; |
| 375 | geometry_msgs::Point p; |
| 376 | p.x = var.vertex_A_[0]; |
| 377 | p.y = var.vertex_A_[1]; |
| 378 | p.z = var.vertex_A_[2]; |
| 379 | Eigen::Vector3d t_p; |
| 380 | t_p << p.x, p.y, p.z; |
| 381 | p.x = t_p[0]; |
| 382 | p.y = t_p[1]; |
| 383 | p.z = t_p[2]; |
| 384 | m_line.points.push_back(p); |
| 385 | p.x = var.vertex_B_[0]; |
| 386 | p.y = var.vertex_B_[1]; |
| 387 | p.z = var.vertex_B_[2]; |
| 388 | t_p << p.x, p.y, p.z; |
| 389 | p.x = t_p[0]; |
| 390 | p.y = t_p[1]; |
| 391 | p.z = t_p[2]; |
| 392 | m_line.points.push_back(p); |
| 393 | ma_line.markers.push_back(m_line); |
| 394 | m_line.id++; |
| 395 | m_line.points.clear(); |
| 396 | p.x = var.vertex_C_[0]; |
| 397 | p.y = var.vertex_C_[1]; |
| 398 | p.z = var.vertex_C_[2]; |
| 399 | t_p << p.x, p.y, p.z; |
| 400 | p.x = t_p[0]; |
| 401 | p.y = t_p[1]; |
| 402 | p.z = t_p[2]; |
| 403 | m_line.points.push_back(p); |
| 404 | p.x = var.vertex_B_[0]; |
| 405 | p.y = var.vertex_B_[1]; |
| 406 | p.z = var.vertex_B_[2]; |
| 407 | t_p << p.x, p.y, p.z; |
| 408 | p.x = t_p[0]; |
| 409 | p.y = t_p[1]; |
| 410 | p.z = t_p[2]; |
| 411 | m_line.points.push_back(p); |
| 412 | ma_line.markers.push_back(m_line); |