/** Open the camera device.
*
* @param newconfig configuration parameters
* @return true, if successful
*
* if successful:
* state_ is Driver::OPENED
* camera_name_ set to camera_name string
*/
bool openCamera(Config &newconfig)
{
bool success = true;
try
{
ROS_INFO("opening uvc_cam at %dx%d, %f fps", newconfig.width, newconfig.height, newconfig.frame_rate);
uvc_cam::Cam::mode_t mode = uvc_cam::Cam::MODE_RGB;
switch (newconfig.format_mode) {
case 1:
mode = uvc_cam::Cam::MODE_RGB;
case 2:
mode = uvc_cam::Cam::MODE_YUYV;
case 3:
mode = uvc_cam::Cam::MODE_MJPG;
default:
mode = uvc_cam::Cam::MODE_RGB;
}
cam_ = new uvc_cam::Cam(newconfig.device.c_str(), mode, newconfig.width, newconfig.height, newconfig.frame_rate);
if (camera_name_ != camera_name_)
{
camera_name_ = camera_name_;
if (!cinfo_.setCameraName(camera_name_))
{
// GUID is 16 hex digits, which should be valid.
// If not, use it for log messages anyway.
ROS_WARN_STREAM("[" << camera_name_ << "] name not valid"
<< " for camera_info_manger");
}
}
// ROS_INFO_STREAM("[" << camera_name_
// << "] opened: " << newconfig.video_mode << ", "
// << newconfig.frame_rate << " fps, "
// << newconfig.iso_speed << " Mb/s");
state_ = Driver::OPENED;
calibration_matches_ = true;
// cam_->display_formats_supported();
}
catch (uvc_cam::Exception& e)
{
ROS_FATAL_STREAM("[" << camera_name_
<< "] exception opening device: " << e.what());
success = false;
}
return success;
}
/** 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_);
}
/** Open the camera device.
*
* @param newconfig configuration parameters
* @return true, if successful
*
* if successful:
* state_ is Driver::OPENED
* camera_name_ set to GUID string
*/
bool openCamera(Config &newconfig)
{
bool success = true;
try
{
if (dev_->open(newconfig.guid.c_str(), newconfig.video_mode.c_str(),
newconfig.frame_rate, newconfig.iso_speed,
newconfig.bayer_pattern.c_str(),
newconfig.bayer_method.c_str())
== 0)
{
if (camera_name_ != dev_->device_id_)
{
camera_name_ = dev_->device_id_;
if (!cinfo_->setCameraName(camera_name_))
{
// GUID is 16 hex digits, which should be valid.
// If not, use it for log messages anyway.
ROS_WARN_STREAM("[" << camera_name_ << "] name not valid"
<< " for camera_info_manger");
}
}
ROS_INFO_STREAM("[" << camera_name_
<< "] opened: " << newconfig.video_mode << ", "
<< newconfig.frame_rate << " fps, "
<< newconfig.iso_speed << " Mb/s");
state_ = Driver::OPENED;
calibration_matches_ = true;
}
}
catch (camera1394v2::Exception& e)
{
ROS_FATAL_STREAM("[" << camera_name_
<< "] exception opening device: " << e.what());
success = false;
}
return success;
}
/** 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;
}
void reconfig(swissranger_camera::SwissRangerConfig &newconfig, uint32_t level)
{
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.frame_id == "")
newconfig.frame_id = camera_name_;
if( config_.frame_id != newconfig.frame_id )
{
std::string tf_prefix = tf::getPrefixParam(nh_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
config_.frame_id = newconfig.frame_id;
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_->validateURL(newconfig.camera_info_url))
{
cinfo_->loadCameraInfo(newconfig.camera_info_url);
}
else
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
if ((config_.auto_exposure != newconfig.auto_exposure) && device_open_)
{
if( newconfig.auto_exposure == 1)
{
SRNode::dev_->setAutoExposure(true);
}
else
{
SRNode::dev_->setAutoExposure(false);
}
}
if ((config_.integration_time != newconfig.integration_time) && device_open_)
{
if( newconfig.auto_exposure != 1 )
{
SRNode::dev_->setIntegrationTime(newconfig.integration_time);
}
}
if ((config_.amp_threshold != newconfig.amp_threshold) && device_open_)
{
if( newconfig.amp_threshold >= 0 )
{
SRNode::dev_->setAmplitudeThreshold(newconfig.amp_threshold);
}
else
{
SRNode::dev_->setAmplitudeThreshold(0);
}
}
config_ = newconfig; // save new parameters
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
/** Dynamic reconfigure callback
*
* Called immediately when callback first defined. Called again
* when dynamic reconfigure starts or changes a parameter value.
*
* @param newconfig new Config values
* @param level bit-wise OR of reconfiguration levels for all
* changed parameters (0xffffffff on initial call)
**/
void reconfig(Config &newconfig, uint32_t level)
{
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.frame_id == "")
newconfig.frame_id = "camera";
std::string tf_prefix = tf::getPrefixParam(privNH_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (state_ == Driver::CLOSED)
{
// open with new values
if (openCamera(newconfig))
{
// update GUID string parameter
newconfig.guid = camera_name_;
}
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_->validateURL(newconfig.camera_info_url))
{
cinfo_->loadCameraInfo(newconfig.camera_info_url);
}
else
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
if (state_ != Driver::CLOSED) // openCamera() succeeded?
{
// configure IIDC features
if (level & Levels::RECONFIGURE_CLOSE)
{
// initialize all features for newly opened device
if (false == dev_->features_->initialize(&newconfig))
{
ROS_ERROR_STREAM("[" << camera_name_
<< "] feature initialization failure");
closeCamera(); // can't continue
}
}
else
{
// update any features that changed
dev_->features_->reconfigure(&newconfig);
}
}
config_ = newconfig; // save new parameters
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
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;
}
/** Dynamic reconfigure callback
*
* Called immediately when callback first defined. Called again
* when dynamic reconfigure starts or changes a parameter value.
*
* @param newconfig new Config values
* @param level bit-wise OR of reconfiguration levels for all
* changed parameters (0xffffffff on initial call)
**/
void reconfig(Config &newconfig, uint32_t level)
{
ROS_DEBUG("dynamic reconfigure level 0x%x", level);
// resolve frame ID using tf_prefix parameter
if (newconfig.frame_id == "")
newconfig.frame_id = "camera";
std::string tf_prefix = tf::getPrefixParam(privNH_);
ROS_DEBUG_STREAM("tf_prefix: " << tf_prefix);
newconfig.frame_id = tf::resolve(tf_prefix, newconfig.frame_id);
if (state_ != Driver::CLOSED && (level & Levels::RECONFIGURE_CLOSE))
{
// must close the device before updating these parameters
closeCamera(); // state_ --> CLOSED
}
if (state_ == Driver::CLOSED)
{
// open with new values
if (openCamera(newconfig))
{
// update camera name string
newconfig.camera_name = camera_name_;
}
}
///THIS IS A QUICK HACK TO GET EXPOSURE ON OUR CAMERA'S THIS DOES NOT WORK FOR ALL CAMERAS
if(config_.exposure != newconfig.exposure){
try {
cam_->set_control(0x9a0901, newconfig.exposure);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting exposure. Exception was " << e.what());
}
}
if(config_.absolute_exposure != newconfig.absolute_exposure){
try {
cam_->set_control(0x9a0902, newconfig.absolute_exposure);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting absolute exposure. Exception was " << e.what());
}
}
if(config_.sharpness != newconfig.sharpness){
try {
cam_->set_control(0x98091b, newconfig.sharpness);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting sharpness. Exception was " << e.what());
}
}
if(config_.power_line_frequency != newconfig.power_line_frequency){
try {
cam_->set_control(0x980918, newconfig.power_line_frequency);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting powerline frequency. Exception was " << e.what());
}
}
if(config_.white_balance_temperature != newconfig.white_balance_temperature){
try {
cam_->set_control(0x98090c, newconfig.white_balance_temperature);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting white balance temperature. Exception was " << e.what());
}
}
if(config_.gain != newconfig.gain){
try {
cam_->set_control(0x980913, newconfig.gain);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting gain. Exception was " << e.what());
}
}
if(config_.saturation != newconfig.saturation){
try {
cam_->set_control(0x980902, newconfig.saturation);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting saturation. Exception was " << e.what());
}
}
if(config_.contrast != newconfig.contrast){
try {
cam_->set_control(0x980901, newconfig.contrast);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting contrast. Exception was " << e.what());
}
}
if(config_.brightness != newconfig.brightness){
try {
cam_->set_control(0x980900, newconfig.brightness);
} catch (uvc_cam::Exception& e) {
ROS_ERROR_STREAM("Problem setting brightness. Exception was " << e.what());
}
}
if (config_.camera_info_url != newconfig.camera_info_url)
{
// set the new URL and load CameraInfo (if any) from it
if (cinfo_.validateURL(newconfig.camera_info_url))
{
cinfo_.loadCameraInfo(newconfig.camera_info_url);
}
else
{
// new URL not valid, use the old one
newconfig.camera_info_url = config_.camera_info_url;
}
}
// if (state_ != Driver::CLOSED) // openCamera() succeeded?
// {
// // configure IIDC features
// if (level & Levels::RECONFIGURE_CLOSE)
// {
// // initialize all features for newly opened device
// if (false == dev_->features_->initialize(&newconfig))
// {
// ROS_ERROR_STREAM("[" << camera_name_
// << "] feature initialization failure");
// closeCamera(); // can't continue
// }
// }
// else
// {
// // update any features that changed
// dev_->features_->reconfigure(&newconfig);
// }
// }
config_ = newconfig; // save new parameters
ROS_DEBUG_STREAM("[" << camera_name_
<< "] reconfigured: frame_id " << newconfig.frame_id
<< ", camera_info_url " << newconfig.camera_info_url);
}
