| 484 | } |
| 485 | |
| 486 | void |
| 487 | Track3D::createMarker(visualization_msgs::MarkerArray::Ptr& msg) |
| 488 | { |
| 489 | if(visibility_ == Track3D::NOT_VISIBLE) |
| 490 | return; |
| 491 | |
| 492 | double _x, _y, _z; |
| 493 | filter_->getState(_x, _y, _z); |
| 494 | |
| 495 | visualization_msgs::Marker marker; |
| 496 | |
| 497 | marker.header.frame_id = frame_id_; |
| 498 | marker.header.stamp = ros::Time::now(); |
| 499 | |
| 500 | marker.ns = "people"; |
| 501 | marker.id = id_; |
| 502 | |
| 503 | marker.type = visualization_msgs::Marker::CUBE; |
| 504 | marker.action = visualization_msgs::Marker::ADD; |
| 505 | |
| 506 | marker.pose.position.x = _x; |
| 507 | marker.pose.position.y = _y; |
| 508 | marker.pose.position.z = _z; |
| 509 | marker.pose.orientation.x = 0.0; |
| 510 | marker.pose.orientation.y = 0.0; |
| 511 | marker.pose.orientation.z = 0.0; |
| 512 | marker.pose.orientation.w = 1.0; |
| 513 | |
| 514 | marker.scale.x = 0.1; |
| 515 | marker.scale.y = 0.1; |
| 516 | marker.scale.z = 0.1; |
| 517 | |
| 518 | marker.color.r = color_(2); |
| 519 | marker.color.g = color_(1); |
| 520 | marker.color.b = color_(0); |
| 521 | marker.color.a = 1.0; |
| 522 | |
| 523 | marker.lifetime = ros::Duration(0.2); |
| 524 | |
| 525 | msg->markers.push_back(marker); |
| 526 | |
| 527 | /////////////////////////////////// |
| 528 | |
| 529 | visualization_msgs::Marker text_marker; |
| 530 | |
| 531 | text_marker.header.frame_id = frame_id_; |
| 532 | text_marker.header.stamp = ros::Time::now(); |
| 533 | |
| 534 | text_marker.ns = "numbers"; |
| 535 | text_marker.id = id_; |
| 536 | |
| 537 | text_marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; |
| 538 | text_marker.action = visualization_msgs::Marker::ADD; |
| 539 | |
| 540 | std::stringstream ss; |
| 541 | ss << id_; |
| 542 | text_marker.text = ss.str(); |
| 543 | |