Main driver loop */
| 366 | |
| 367 | /** Main driver loop */ |
| 368 | bool spin() |
| 369 | { |
| 370 | while (nh_.ok()) |
| 371 | { |
| 372 | getParameters(); // check reconfigurable parameters |
| 373 | |
| 374 | // get current CameraInfo data |
| 375 | cam_info_ = cinfo_->getCameraInfo(); |
| 376 | cloud2_.header.frame_id = cloud_.header.frame_id = |
| 377 | image_d_.header.frame_id = image_i_.header.frame_id = |
| 378 | image_c_.header.frame_id = image_d16_.header.frame_id = |
| 379 | cam_info_.header.frame_id = camera_name_;//config_.frame_id; |
| 380 | |
| 381 | if(!device_open_) |
| 382 | { |
| 383 | try |
| 384 | { |
| 385 | if (dev_->open(config_.auto_exposure, config_.integration_time, |
| 386 | modulation_freq_, config_.amp_threshold, ether_addr_) == 0) |
| 387 | { |
| 388 | ROS_INFO_STREAM("[" << camera_name_ << "] Connected to device with ID: " << dev_->device_id_); |
| 389 | ROS_INFO_STREAM("[" << camera_name_ << "] libmesasr version: " << dev_->lib_version_); |
| 390 | device_open_ = true; |
| 391 | } |
| 392 | else |
| 393 | { |
| 394 | ros::Duration(3.0).sleep(); |
| 395 | } |
| 396 | } |
| 397 | catch (sr::Exception& e) |
| 398 | { |
| 399 | ROS_ERROR_STREAM("Exception thrown while connecting to the camera: " << e.what()); |
| 400 | ros::Duration(3.0).sleep(); |
| 401 | } |
| 402 | } |
| 403 | else |
| 404 | { |
| 405 | try |
| 406 | { |
| 407 | // Read data from the Camera |
| 408 | dev_->readData(cloud_,cloud2_,image_d_, image_i_, image_c_, image_d16_); |
| 409 | |
| 410 | cam_info_.header.stamp = image_d_.header.stamp; |
| 411 | cam_info_.height = image_d_.height; |
| 412 | cam_info_.width = image_d_.width; |
| 413 | |
| 414 | // Publish it via image_transport |
| 415 | if (info_pub_.getNumSubscribers() > 0) |
| 416 | info_pub_.publish(cam_info_); |
| 417 | if (image_pub_d_.getNumSubscribers() > 0) |
| 418 | image_pub_d_.publish(image_d_); |
| 419 | if (image_pub_i_.getNumSubscribers() > 0) |
| 420 | image_pub_i_.publish(image_i_); |
| 421 | if (image_pub_c_.getNumSubscribers() > 0) |
| 422 | image_pub_c_.publish(image_c_); |
| 423 | if (image_pub_d16_.getNumSubscribers() > 0) |
| 424 | image_pub_d16_.publish(image_d16_); |
| 425 | if (cloud_pub_.getNumSubscribers() > 0) |
no test coverage detected