| 199 | } |
| 200 | |
| 201 | bool OPTCalibrationNode::initialize() |
| 202 | { |
| 203 | bool all_messages_received = false; |
| 204 | ros::Rate rate(1.0); |
| 205 | while (ros::ok() and not all_messages_received) |
| 206 | { |
| 207 | ros::spinOnce(); |
| 208 | all_messages_received = true; |
| 209 | for (size_t i = 0; all_messages_received and i < pinhole_vec_.size(); ++i) |
| 210 | all_messages_received = pinhole_vec_[i]->hasNewMessages(); |
| 211 | for (size_t i = 0; all_messages_received and i < kinect_vec_.size(); ++i) |
| 212 | all_messages_received = kinect_vec_[i]->hasNewMessages(); |
| 213 | for (size_t i = 0; all_messages_received and i < swiss_ranger_vec_.size(); ++i) |
| 214 | all_messages_received = swiss_ranger_vec_[i]->hasNewMessages(); |
| 215 | |
| 216 | if (not all_messages_received) |
| 217 | ROS_WARN_THROTTLE(5, "Not all messages received. Waiting..."); |
| 218 | rate.sleep(); |
| 219 | } |
| 220 | |
| 221 | ROS_INFO("All sensors connected."); |
| 222 | |
| 223 | calibration_ = boost::make_shared<OPTCalibration>(node_handle_); |
| 224 | calibration_->setCheckerboard(checkerboard_); |
| 225 | |
| 226 | for (size_t i = 0; i < pinhole_vec_.size(); ++i) |
| 227 | { |
| 228 | const PinholeRGBDevice::Ptr & device = pinhole_vec_[i]; |
| 229 | calibration_->addSensor(device->sensor(), true); |
| 230 | sensor_vec_.push_back(device->sensor()); |
| 231 | images_acquired_map_[device->frameId()] = 0; |
| 232 | status_msg_.sensor_ids.push_back(device->frameId()); |
| 233 | } |
| 234 | |
| 235 | for (size_t i = 0; i < kinect_vec_.size(); ++i) // TODO Add flags |
| 236 | { |
| 237 | const KinectDevice::Ptr & device = kinect_vec_[i]; |
| 238 | calibration_->addSensor(device->colorSensor(), true); |
| 239 | calibration_->addSensor(device->depthSensor(), true); |
| 240 | sensor_vec_.push_back(device->colorSensor()); |
| 241 | sensor_vec_.push_back(device->depthSensor()); |
| 242 | images_acquired_map_[device->colorFrameId()] = 0; |
| 243 | images_acquired_map_[device->depthFrameId()] = 0; |
| 244 | status_msg_.sensor_ids.push_back(device->colorFrameId()); |
| 245 | status_msg_.sensor_ids.push_back(device->depthFrameId()); |
| 246 | } |
| 247 | |
| 248 | for (size_t i = 0; i < swiss_ranger_vec_.size(); ++i) // TODO Add flags |
| 249 | { |
| 250 | const SwissRangerDevice::Ptr & device = swiss_ranger_vec_[i]; |
| 251 | calibration_->addSensor(device->intensitySensor(), true); |
| 252 | calibration_->addSensor(device->depthSensor(), true); |
| 253 | sensor_vec_.push_back(device->intensitySensor()); |
| 254 | sensor_vec_.push_back(device->depthSensor()); |
| 255 | images_acquired_map_[device->frameId()] = 0; |
| 256 | status_msg_.sensor_ids.push_back(device->frameId()); |
| 257 | } |
| 258 |
no test coverage detected