| 120 | } |
| 121 | |
| 122 | void Initialize() |
| 123 | { |
| 124 | |
| 125 | /* #region Lidar --------------------------------------------------------------------------------------------*/ |
| 126 | |
| 127 | // Read the lidar topic |
| 128 | vector<string> lidar_topic = {"/os_cloud_node/points"}; |
| 129 | nh_ptr->getParam("/lidar_topic", lidar_topic); |
| 130 | |
| 131 | Nlidar = lidar_topic.size(); |
| 132 | |
| 133 | // Read the extrincs of lidars |
| 134 | vector<double> lidar_extr = { 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1}; |
| 135 | nh_ptr->getParam("/lidar_extr", lidar_extr); |
| 136 | |
| 137 | ROS_ASSERT_MSG( (lidar_extr.size() / 16) == Nlidar, |
| 138 | "Lidar extrinsics not complete: %d < %d (= %d*16)\n", |
| 139 | lidar_extr.size(), Nlidar, Nlidar*16); |
| 140 | |
| 141 | printf("Received %d lidar(s) with extrinsics: \n", Nlidar); |
| 142 | for(int i = 0; i < Nlidar; i++) |
| 143 | { |
| 144 | // Confirm the topics |
| 145 | printf("Lidar topic #%02d: %s\n", i, lidar_topic[i].c_str()); |
| 146 | |
| 147 | Matrix4d extrinsicTf = Matrix<double, 4, 4, RowMajor>(&lidar_extr[i*16]); |
| 148 | cout << "extrinsicTf: " << endl; |
| 149 | cout << extrinsicTf << endl; |
| 150 | |
| 151 | R_B_L.push_back(extrinsicTf.block<3, 3>(0, 0)); |
| 152 | t_B_L.push_back(extrinsicTf.block<3, 1>(0, 3)); |
| 153 | |
| 154 | lidar_buf.push_back(deque<CloudPacket>(0)); |
| 155 | lidar_leftover_buf.push_back(deque<CloudPacket>(0)); |
| 156 | |
| 157 | // Subscribe to the lidar topic |
| 158 | lidar_sub.push_back(nh_ptr->subscribe<sensor_msgs::PointCloud2> |
| 159 | (lidar_topic[i], 100, |
| 160 | boost::bind(&SensorSync::PcHandler, this, |
| 161 | _1, i, (int)extrinsicTf(3, 3)))); |
| 162 | } |
| 163 | |
| 164 | nh_ptr->getParam("/min_range", min_range); |
| 165 | printf("Lidar minimum range: %f\n", min_range); |
| 166 | |
| 167 | nh_ptr->getParam("/ds_rate", ds_rate); |
| 168 | printf("Down samping rate: "); |
| 169 | for(auto rate : ds_rate) |
| 170 | printf("%d ", rate); |
| 171 | cout << endl; |
| 172 | |
| 173 | // Advertise lidar topic |
| 174 | string merged_lidar_topic; |
| 175 | nh_ptr->getParam("/merged_lidar_topic", merged_lidar_topic); |
| 176 | merged_pc_pub = nh_ptr->advertise<sensor_msgs::PointCloud2>(merged_lidar_topic, 100); |
| 177 | |
| 178 | /* #endregion Lidar -----------------------------------------------------------------------------------------*/ |
| 179 | |