/** Publish camera stream topics
*
* @pre image_ contains latest camera frame
*/
void publish()
{
image_.header.frame_id = config_.frame_id;
// get current CameraInfo data
cam_info_ = cinfo_->getCameraInfo();
//ROS_INFO_STREAM ("cam: " << cam_info_.width << " x " << cam_info_.height << ", image: " << image_.width << " x " << image_.height);
if (cam_info_.height != image_.height || cam_info_.width != image_.width)
{
// image size does not match: publish a matching uncalibrated
// CameraInfo instead
if (calibration_matches_)
{
// warn user once
calibration_matches_ = false;
ROS_WARN_STREAM("[" << camera_name_
<< "] camera_info_url: " << config_.camera_info_url);
ROS_WARN_STREAM("[" << camera_name_
<< "] calibration (" << cam_info_.width << "x" << cam_info_.height << ") does not match video mode (" << image_.width << "x" << image_.height << ") "
<< "(publishing uncalibrated data)");
}
cam_info_ = sensor_msgs::CameraInfo();
cam_info_.height = image_.height;
cam_info_.width = image_.width;
}
else if (!calibration_matches_)
{
// calibration OK now
calibration_matches_ = true;
ROS_INFO_STREAM("[" << camera_name_
<< "] calibration matches video mode now");
}
cam_info_.header.frame_id = config_.frame_id;
cam_info_.header.stamp = image_.header.stamp;
// @todo log a warning if (filtered) time since last published
// image is not reasonably close to configured frame_rate
// Publish via image_transport
image_pub_.publish(image_, cam_info_);
}
/** Main driver loop */
bool spin()
{
while (nh_.ok())
{
getParameters(); // check reconfigurable parameters
// get current CameraInfo data
cam_info_ = cinfo_->getCameraInfo();
cloud2_.header.frame_id = cloud_.header.frame_id =
image_d_.header.frame_id = image_i_.header.frame_id =
image_c_.header.frame_id = image_d16_.header.frame_id =
cam_info_.header.frame_id = camera_name_;//config_.frame_id;
if(!device_open_)
{
try
{
if (dev_->open(config_.auto_exposure, config_.integration_time,
modulation_freq_, config_.amp_threshold, ether_addr_) == 0)
{
ROS_INFO_STREAM("[" << camera_name_ << "] Connected to device with ID: "
<< dev_->device_id_);
ROS_INFO_STREAM("[" << camera_name_ << "] libmesasr version: " << dev_->lib_version_);
device_open_ = true;
}
else
{
ros::Duration(3.0).sleep();
}
}
catch (sr::Exception& e)
{
ROS_ERROR_STREAM("Exception thrown while connecting to the camera: "
<< e.what ());
ros::Duration(3.0).sleep();
}
}
else
{
try
{
// Read data from the Camera
dev_->readData(cloud_,cloud2_,image_d_, image_i_, image_c_, image_d16_);
cam_info_.header.stamp = image_d_.header.stamp;
cam_info_.height = image_d_.height;
cam_info_.width = image_d_.width;
// Publish it via image_transport
if (info_pub_.getNumSubscribers() > 0)
info_pub_.publish(cam_info_);
if (image_pub_d_.getNumSubscribers() > 0)
image_pub_d_.publish(image_d_);
if (image_pub_i_.getNumSubscribers() > 0)
image_pub_i_.publish(image_i_);
if (image_pub_c_.getNumSubscribers() > 0)
image_pub_c_.publish(image_c_);
if (image_pub_d16_.getNumSubscribers() > 0)
image_pub_d16_.publish(image_d16_);
if (cloud_pub_.getNumSubscribers() > 0)
cloud_pub_.publish (cloud_);
if (cloud_pub2_.getNumSubscribers() > 0)
cloud_pub2_.publish (cloud2_);
}
catch (sr::Exception& e) {
ROS_WARN("Exception thrown trying to read data: %s",
e.what());
dev_->close();
device_open_ = false;
ros::Duration(3.0).sleep();
}
}
ros::spinOnce();
}
return true;
}
int CameraNode::run()
{
ros::WallTime t_prev, t_now;
int dt_avg;
// reset timing information
dt_avg = 0;
t_prev = ros::WallTime::now();
while (ros::ok()) {
std::vector<uint8_t> data(step*height);
CamContext_grab_next_frame_blocking(cc,&data[0],-1.0f); // never timeout
int errnum = cam_iface_have_error();
if (errnum == CAM_IFACE_FRAME_TIMEOUT) {
cam_iface_clear_error();
continue; // wait again
}
if (errnum == CAM_IFACE_FRAME_DATA_MISSING_ERROR) {
cam_iface_clear_error();
} else if (errnum == CAM_IFACE_FRAME_INTERRUPTED_SYSCALL) {
cam_iface_clear_error();
} else if (errnum == CAM_IFACE_FRAME_DATA_CORRUPT_ERROR) {
cam_iface_clear_error();
} else {
_check_error();
if (!_got_frame) {
ROS_INFO("receiving images");
_got_frame = true;
}
if(dt_avg++ == 9) {
ros::WallTime t_now = ros::WallTime::now();
ros::WallDuration dur = t_now - t_prev;
t_prev = t_now;
std_msgs::Float32 rate;
rate.data = 10 / dur.toSec();
_pub_rate.publish(rate);
dt_avg = 0;
}
sensor_msgs::Image msg;
if (_host_timestamp) {
msg.header.stamp = ros::Time::now();
} else {
double timestamp;
CamContext_get_last_timestamp(cc,×tamp);
_check_error();
msg.header.stamp = ros::Time(timestamp);
}
unsigned long framenumber;
CamContext_get_last_framenumber(cc,&framenumber);
_check_error();
msg.header.seq = framenumber;
msg.header.frame_id = "0";
msg.height = height;
msg.width = width;
msg.encoding = encoding;
msg.step = step;
msg.data = data;
// get current CameraInfo data
cam_info = cam_info_manager->getCameraInfo();
cam_info.header.stamp = msg.header.stamp;
cam_info.header.seq = msg.header.seq;
cam_info.header.frame_id = msg.header.frame_id;
cam_info.height = height;
cam_info.width = width;
_pub_image.publish(msg, cam_info);
}
ros::spinOnce();
}
CamContext_stop_camera(cc);
cam_iface_shutdown();
return 0;
}
