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