| 191 | } |
| 192 | |
| 193 | bool InputManager::callSLOAM(SE3 relativeMotion, ros::Time stamp) |
| 194 | { |
| 195 | |
| 196 | CloudT::Ptr cloud(new CloudT); |
| 197 | auto r = FindPC(stamp, cloud); |
| 198 | if (r == CLOUD_FOUND) |
| 199 | { |
| 200 | odomQueue_.pop_front(); |
| 201 | SE3 keyPose = SE3(); |
| 202 | SE3 prevKeyPose = firstOdom_ ? SE3() : keyPoses_[keyPoses_.size() - 1]; |
| 203 | pcl_conversions::toPCL(stamp, cloud->header.stamp); |
| 204 | bool success = sloam_->run(relativeMotion, prevKeyPose, cloud, stamp, keyPose); |
| 205 | if (success) |
| 206 | { |
| 207 | keyPoses_.push_back(keyPose); |
| 208 | return true; |
| 209 | } |
| 210 | } |
| 211 | else |
| 212 | { |
| 213 | if (r == CLOUD_TOO_NEW) |
| 214 | odomQueue_.pop_front(); |
| 215 | ROS_DEBUG_THROTTLE(1, "Corresponding point cloud not found. Skipping."); |
| 216 | } |
| 217 | return false; |
| 218 | } |
| 219 | |
| 220 | void InputManager::PCCb_(const sensor_msgs::PointCloud2ConstPtr &cloudMsg) |
| 221 | { |