| 104 | } |
| 105 | |
| 106 | void process() |
| 107 | { |
| 108 | if (!LOOP_CLOSURE) |
| 109 | return; |
| 110 | |
| 111 | while (ros::ok()) |
| 112 | { |
| 113 | sensor_msgs::ImageConstPtr image_msg = NULL; |
| 114 | sensor_msgs::PointCloudConstPtr point_msg = NULL; |
| 115 | nav_msgs::Odometry::ConstPtr pose_msg = NULL; |
| 116 | |
| 117 | // find out the messages with same time stamp |
| 118 | m_buf.lock(); |
| 119 | if(!image_buf.empty() && !point_buf.empty() && !pose_buf.empty()) |
| 120 | { |
| 121 | if (image_buf.front()->header.stamp.toSec() > pose_buf.front()->header.stamp.toSec()) |
| 122 | { |
| 123 | pose_buf.pop(); |
| 124 | printf("throw pose at beginning\n"); |
| 125 | } |
| 126 | else if (image_buf.front()->header.stamp.toSec() > point_buf.front()->header.stamp.toSec()) |
| 127 | { |
| 128 | point_buf.pop(); |
| 129 | printf("throw point at beginning\n"); |
| 130 | } |
| 131 | else if (image_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec() |
| 132 | && point_buf.back()->header.stamp.toSec() >= pose_buf.front()->header.stamp.toSec()) |
| 133 | { |
| 134 | pose_msg = pose_buf.front(); |
| 135 | pose_buf.pop(); |
| 136 | while (!pose_buf.empty()) |
| 137 | pose_buf.pop(); |
| 138 | while (image_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec()) |
| 139 | image_buf.pop(); |
| 140 | image_msg = image_buf.front(); |
| 141 | image_buf.pop(); |
| 142 | |
| 143 | while (point_buf.front()->header.stamp.toSec() < pose_msg->header.stamp.toSec()) |
| 144 | point_buf.pop(); |
| 145 | point_msg = point_buf.front(); |
| 146 | point_buf.pop(); |
| 147 | } |
| 148 | } |
| 149 | m_buf.unlock(); |
| 150 | |
| 151 | if (pose_msg != NULL) |
| 152 | { |
| 153 | // skip fisrt few |
| 154 | static int skip_first_cnt = 0; |
| 155 | if (skip_first_cnt < SKIP_FIRST_CNT) |
| 156 | { |
| 157 | skip_first_cnt++; |
| 158 | continue; |
| 159 | } |
| 160 | |
| 161 | // limit frequency |
| 162 | static double last_skip_time = -1; |
| 163 | if (pose_msg->header.stamp.toSec() - last_skip_time < SKIP_TIME) |
nothing calls this directly
no test coverage detected