* \brief Read the DetectionArray message and use the detections * for creating/updating/deleting tracks * * \param[in] msg the DetectionArray message. */
| 502 | * \param[in] msg the DetectionArray message. |
| 503 | */ |
| 504 | void |
| 505 | detection_cb(const rtpose_wrapper::SkeletonArrayMsg::ConstPtr& msg) |
| 506 | { |
| 507 | // Read message header information: |
| 508 | std::string frame_id = msg->header.frame_id; |
| 509 | ros::Time frame_time = msg->header.stamp; |
| 510 | int temp = 0; |
| 511 | std::string frame_id_tmp = frame_id; |
| 512 | int pos = frame_id_tmp.find("_rgb_optical_frame"); |
| 513 | if (pos != std::string::npos) |
| 514 | { |
| 515 | frame_id_tmp.replace(pos, std::string("_rgb_optical_frame").size(), ""); |
| 516 | temp = pos; |
| 517 | } |
| 518 | pos = frame_id_tmp.find("_ir_optical_frame"); |
| 519 | if (pos != std::string::npos) |
| 520 | { |
| 521 | frame_id_tmp.replace(pos, std::string("_ir_optical_frame").size(), ""); |
| 522 | temp = pos; |
| 523 | } |
| 524 | pos = frame_id_tmp.find("_depth_optical_frame"); |
| 525 | if (pos != std::string::npos) |
| 526 | { |
| 527 | frame_id_tmp.replace(pos, std::string("_depth_optical_frame").size(), ""); |
| 528 | temp = pos; |
| 529 | } |
| 530 | last_received_detection_[frame_id_tmp] = frame_time; |
| 531 | |
| 532 | // Compute delay of detection message, if any: |
| 533 | double time_delay = 0.0; |
| 534 | if (frame_time > latest_time) |
| 535 | { |
| 536 | latest_time = frame_time; |
| 537 | time_delay = 0.0; |
| 538 | } |
| 539 | else |
| 540 | { |
| 541 | time_delay = (latest_time - frame_time).toSec(); |
| 542 | } |
| 543 | |
| 544 | tf::StampedTransform transform; |
| 545 | tf::StampedTransform inverse_transform; |
| 546 | // cv_bridge::CvImage::Ptr cvPtr; |
| 547 | |
| 548 | try |
| 549 | { |
| 550 | // Read transforms between camera frame and world frame: |
| 551 | if (!extrinsic_calibration) |
| 552 | { |
| 553 | //std::string frame_id_copy = frame_id; |
| 554 | //std::string new_frame_id = frame_id_tmp + frame_id_copy.erase(0,temp); |
| 555 | static tf::TransformBroadcaster world_to_camera_tf_publisher; |
| 556 | world_to_camera_tf_publisher.sendTransform |
| 557 | (tf::StampedTransform(world_to_camera_frame_transform, |
| 558 | ros::Time::now(), frame_id , world_frame_id)); |
| 559 | } |
| 560 | |
| 561 | //Calculate direct and inverse transforms between camera and world frame: |
nothing calls this directly
no test coverage detected