| 225 | } |
| 226 | |
| 227 | int InputManager::FindPC(const ros::Time stamp, CloudT::Ptr cloud) |
| 228 | { |
| 229 | if (pcQueue_.empty()) |
| 230 | return false; |
| 231 | while (!pcQueue_.empty()) |
| 232 | { |
| 233 | if (pcQueue_.front()->header.stamp.toSec() < stamp.toSec() - 0.05) |
| 234 | { |
| 235 | // message too old |
| 236 | ROS_DEBUG_THROTTLE(1, "PC MSG TOO OLD"); |
| 237 | pcQueue_.pop(); |
| 238 | } |
| 239 | else if (pcQueue_.front()->header.stamp.toSec() > stamp.toSec() + 0.05) |
| 240 | { |
| 241 | // message too new |
| 242 | ROS_DEBUG_THROTTLE(1, "PC MSG TOO NEW"); |
| 243 | return CLOUD_TOO_NEW; |
| 244 | } |
| 245 | else |
| 246 | { |
| 247 | pcl::fromROSMsg(*pcQueue_.front(), *cloud); |
| 248 | pcQueue_.pop(); |
| 249 | ROS_DEBUG("Calling SLOAM"); |
| 250 | return CLOUD_FOUND; |
| 251 | } |
| 252 | } |
| 253 | return CLOUD_TOO_OLD; |
| 254 | } |
| 255 | |
| 256 | void InputManager::Odom2SlamTf() |
| 257 | { |
nothing calls this directly
no outgoing calls
no test coverage detected