| 26 | bool init_pub = 0; |
| 27 | |
| 28 | void img_callback(const sensor_msgs::ImageConstPtr &img_msg) |
| 29 | { |
| 30 | if(first_image_flag) |
| 31 | { |
| 32 | first_image_flag = false; |
| 33 | first_image_time = img_msg->header.stamp.toSec(); |
| 34 | last_image_time = img_msg->header.stamp.toSec(); |
| 35 | return; |
| 36 | } |
| 37 | // detect unstable camera stream |
| 38 | if (img_msg->header.stamp.toSec() - last_image_time > 1.0 || img_msg->header.stamp.toSec() < last_image_time) |
| 39 | { |
| 40 | ROS_WARN("image discontinue! reset the feature tracker!"); |
| 41 | first_image_flag = true; |
| 42 | last_image_time = 0; |
| 43 | pub_count = 1; |
| 44 | std_msgs::Bool restart_flag; |
| 45 | restart_flag.data = true; |
| 46 | pub_restart.publish(restart_flag); |
| 47 | return; |
| 48 | } |
| 49 | last_image_time = img_msg->header.stamp.toSec(); |
| 50 | // frequency control |
| 51 | if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ) |
| 52 | { |
| 53 | PUB_THIS_FRAME = true; |
| 54 | // reset the frequency control |
| 55 | if (abs(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time) - FREQ) < 0.01 * FREQ) |
| 56 | { |
| 57 | first_image_time = img_msg->header.stamp.toSec(); |
| 58 | pub_count = 0; |
| 59 | } |
| 60 | } |
| 61 | else |
| 62 | PUB_THIS_FRAME = false; |
| 63 | |
| 64 | cv_bridge::CvImageConstPtr ptr; |
| 65 | if (img_msg->encoding == "8UC1") |
| 66 | { |
| 67 | sensor_msgs::Image img; |
| 68 | img.header = img_msg->header; |
| 69 | img.height = img_msg->height; |
| 70 | img.width = img_msg->width; |
| 71 | img.is_bigendian = img_msg->is_bigendian; |
| 72 | img.step = img_msg->step; |
| 73 | img.data = img_msg->data; |
| 74 | img.encoding = "mono8"; |
| 75 | ptr = cv_bridge::toCvCopy(img, sensor_msgs::image_encodings::MONO8); |
| 76 | } |
| 77 | else |
| 78 | ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::MONO8); |
| 79 | |
| 80 | cv::Mat show_img = ptr->image; |
| 81 | TicToc t_r; |
| 82 | for (int i = 0; i < NUM_OF_CAM; i++) |
| 83 | { |
| 84 | ROS_DEBUG("processing camera %d", i); |
| 85 | if (i != 1 || !STEREO_TRACK) |