| 141 | } |
| 142 | |
| 143 | bool InputManager::Run() |
| 144 | { |
| 145 | |
| 146 | if (odomQueue_.empty() || pcQueue_.empty()) |
| 147 | return false; |
| 148 | |
| 149 | // ROS_DEBUG_THROTTLE(5, "First odom stamp: %f", odomQueue_.front().stamp.toSec()); |
| 150 | // ROS_DEBUG_THROTTLE(5, "First cloud stamp: %f", pcQueue_.front()->header.stamp.toSec()); |
| 151 | // ROS_DEBUG_THROTTLE(5, "Last odom stamp: %f", odomQueue_.back().stamp.toSec()); |
| 152 | // ROS_DEBUG_THROTTLE(5, "Last cloud stamp: %f", pcQueue_.back()->header.stamp.toSec()); |
| 153 | |
| 154 | for (auto i = 0; i < odomQueue_.size(); ++i) |
| 155 | { |
| 156 | auto odom = odomQueue_[i]; |
| 157 | // Use odom to estimate motion since last key frame |
| 158 | SE3 currRelativeMotion = latestOdom.pose.inverse() * odom.pose; |
| 159 | |
| 160 | if (firstOdom_) |
| 161 | { |
| 162 | ROS_INFO_THROTTLE(1.0, "first sloam call"); |
| 163 | if (callSLOAM(currRelativeMotion, odom.stamp)) |
| 164 | { |
| 165 | firstOdom_ = false; |
| 166 | latestOdom.pose = odom.pose; |
| 167 | latestOdom.stamp = odom.stamp; |
| 168 | if(publishTf_) |
| 169 | Odom2SlamTf(); |
| 170 | return true; |
| 171 | } |
| 172 | } |
| 173 | else |
| 174 | { |
| 175 | double accumMovement = currRelativeMotion.translation().norm(); |
| 176 | if (accumMovement > minOdomDistance_) |
| 177 | { |
| 178 | ROS_DEBUG_THROTTLE(1.0, "Distance %f", (accumMovement)); |
| 179 | if (callSLOAM(currRelativeMotion, odom.stamp)) |
| 180 | { |
| 181 | latestOdom.pose = odom.pose; |
| 182 | latestOdom.stamp = odom.stamp; |
| 183 | if(publishTf_) |
| 184 | Odom2SlamTf(); |
| 185 | return true; |
| 186 | } |
| 187 | } |
| 188 | } |
| 189 | } |
| 190 | return false; |
| 191 | } |
| 192 | |
| 193 | bool InputManager::callSLOAM(SE3 relativeMotion, ros::Time stamp) |
| 194 | { |