/** 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); }