StereoNode.cpp

Go to the documentation of this file.

/*********************************************************************
* Software License Agreement (BSD License)
*
*  Copyright (c) 2012, Kevin Hallenbeck
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of Kevin Hallenbeck nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/

#include <ueye/StereoNode.h>

namespace ueye {

StereoNode::StereoNode(ros::NodeHandle node, ros::NodeHandle priv_nh)
: it_(node)
{
        running_ = false;
        configured_ = false;
        force_streaming_ = false;
        auto_exposure_ = false;
        auto_gain_ = false;
        trigger_mode_ = zoom_ = -1;
        l_stamp_ = r_stamp_ = ros::Time(0);

        // Check for a valid uEye installation and supported version
        char *Version;
        int Major, Minor, Build;
        if(l_cam_.checkVersion(Major, Minor, Build, Version)){
                ROS_INFO("Loaded uEye SDK %s.", Version);
        }else{
                ROS_WARN("Loaded uEye SDK %d.%d.%d. Expecting %s.", Major, Minor, Build, Version);
        }

        // Make sure there is at least one camera available
        int NumberOfCameras = l_cam_.getNumberOfCameras();
        if(NumberOfCameras > 0){
                if(NumberOfCameras == 1){
                        ROS_ERROR("Found 1 uEye camera.");
                        ros::shutdown();
                }else{
                        ROS_INFO("Found %d uEye cameras.", NumberOfCameras);
                }
        }else{
                ROS_ERROR("Found 0 uEye cameras.");
                ros::shutdown();
                return;
        }

        // Service call for setting calibration.
        l_srv_cam_info_ = node.advertiseService("left/set_camera_info" , &StereoNode::setCameraInfoL, this);
        r_srv_cam_info_ = node.advertiseService("right/set_camera_info", &StereoNode::setCameraInfoR, this);

        // Special publisher for images to support compression
        l_pub_stream_ = it_.advertiseCamera("left/image_raw" , 0);
        r_pub_stream_ = it_.advertiseCamera("right/image_raw", 0);

        // Open cameras with either serialNo, deviceId, or cameraId
        int id = 0;
        if(priv_nh.getParam("lSerialNo", id)){
                if(!l_cam_.openCameraSerNo(id)){
                        ROS_ERROR("Failed to open uEye camera with serialNo: %u.", id);
                        ros::shutdown();
                        return;
                }
        }else if(priv_nh.getParam("lDeviceId", id)){
                if(!l_cam_.openCameraDevId(id)){
                        ROS_ERROR("Failed to open uEye camera with deviceId: %u.", id);
                        ros::shutdown();
                        return;
                }
        }else{
                priv_nh.getParam("lCameraId", id);
                if(!l_cam_.openCameraCamId(id)){
                        ROS_ERROR("Failed to open uEye camera with cameraId: %u.", id);
                        ros::shutdown();
                        return;
                }
        }
        ROS_INFO("Left  camera: %s %u", l_cam_.getCameraName(), l_cam_.getCameraSerialNo());
        id = 0;
        if(priv_nh.getParam("rSerialNo", id)){
                if(!r_cam_.openCameraSerNo(id)){
                        ROS_ERROR("Failed to open uEye camera with serialNo: %u.", id);
                        ros::shutdown();
                        return;
                }
        }else if(priv_nh.getParam("rDeviceId", id)){
                if(!r_cam_.openCameraDevId(id)){
                        ROS_ERROR("Failed to open uEye camera with deviceId: %u.", id);
                        ros::shutdown();
                        return;
                }
        }else{
                priv_nh.getParam("rCameraId", id);
                if(!r_cam_.openCameraCamId(id)){
                        ROS_ERROR("Failed to open uEye camera with cameraId: %u.", id);
                        ros::shutdown();
                        return;
                }
        }
        ROS_INFO("Right camera: %s %u", r_cam_.getCameraName(), r_cam_.getCameraSerialNo());

        // Disable trigger delays
        l_cam_.setTriggerDelay(0);
        r_cam_.setTriggerDelay(0);

        timer_force_trigger_ = node.createTimer(ros::Duration(1.0), &StereoNode::timerForceTrigger, this);
        timer_force_trigger_.stop();

        // Setup Dynamic Reconfigure
        dynamic_reconfigure::Server<stereoConfig>::CallbackType f = boost::bind(&StereoNode::reconfig, this, _1, _2);
        srv_.setCallback(f);    //start dynamic reconfigure

        // Set up Timer
        timer_ = node.createTimer(ros::Duration(0.5), &StereoNode::timerCallback, this);
}

StereoNode::~StereoNode()
{
        closeCamera();
}

void StereoNode::handlePath(std::string &path)
{
        // Set default path if not present
        if(path.length() == 0){
                path = ros::package::getPath("ueye");
        }

        // Remove trailing "/" from folder path
        unsigned int length = path.length();
        if(length > 0){
                if(path.c_str()[length-1] == '/'){
                        path = path.substr(0, length-1);
                }
        }
        config_path_ = path;
}
void StereoNode::reconfigCam(stereoConfig &config, uint32_t level, Camera &cam)
{
        // Hardware Gamma Correction
        if (cam.getHardwareGamma() != config.hardware_gamma){
                cam.setHardwareGamma(&config.hardware_gamma);
        }

        // Gain Boost
        if (cam.getGainBoost() != config.gain_boost){
                cam.setGainBoost(&config.gain_boost);
        }

        // Hardware Gain
        if (cam.getAutoGain() != config.auto_gain){
                cam.setAutoGain(&config.auto_gain);
        }
        if (!config.auto_gain){
                cam.setHardwareGain(&config.gain);
        }

        // Zoom
        if (cam.getZoom() != config.zoom){
                cam.setZoom(&config.zoom);
        }

        // Frame Rate
        if(config.trigger == stereo_TGR_SOFTWARE){
                //In software trigger mode we don't want to set a frame rate
                double d = 2.0;
                cam.setFrameRate(&d);
        }else{
                cam.setFrameRate(&config.frame_rate);
                ROS_INFO("config.trigger %d", config.trigger);
        }

        // Exposure
        if (cam.getAutoExposure() != config.auto_exposure){
                cam.setAutoExposure(&config.auto_exposure);
        }
        if (!config.auto_exposure){
                cam.setExposure(&config.exposure_time);
        }
}
void StereoNode::reconfig(stereoConfig &config, uint32_t level)
{
        force_streaming_ = config.force_streaming;
        handlePath(config.config_path);

        const FlashMode flash_active_mode = config.flash_polarity ? FLASH_FREERUN_ACTIVE_HI : FLASH_FREERUN_ACTIVE_LO;

        if(trigger_mode_ != config.trigger){
                stopCamera();
                TriggerMode l_trigger, r_trigger;
                FlashMode l_flash = FLASH_OFF;
                FlashMode r_flash = FLASH_OFF;
                switch(config.trigger){
                case stereo_TGR_SOFTWARE:
                case stereo_TGR_HARDWARE_RISING:
                        l_trigger = r_trigger = TRIGGER_LO_HI;
                        break;
                case stereo_TGR_HARDWARE_FALLING:
                        l_trigger = r_trigger = TRIGGER_HI_LO;
                        break;
                case stereo_TGR_L_MASTER_R_RISING:
                        l_trigger = TRIGGER_OFF;
                        r_trigger = TRIGGER_LO_HI;
                        l_flash = flash_active_mode;
                        break;
                case stereo_TGR_L_MASTER_R_FALLING:
                        l_trigger = TRIGGER_OFF;
                        r_trigger = TRIGGER_HI_LO;
                        l_flash = flash_active_mode;
                        break;
                case stereo_TGR_R_MASTER_L_RISING:
                        l_trigger = TRIGGER_LO_HI;
                        r_trigger = TRIGGER_OFF;
                        r_flash = flash_active_mode;
                        break;
                case stereo_TGR_R_MASTER_L_FALLING:
                        l_trigger = TRIGGER_HI_LO;
                        r_trigger = TRIGGER_OFF;
                        r_flash = flash_active_mode;
                        break;
                case stereo_TGR_AUTO:
                default:
                        config.trigger = stereo_TGR_AUTO;
                        l_trigger = r_trigger = TRIGGER_OFF;
                        break;
                }
                if(!(l_cam_.setTriggerMode(l_trigger) && r_cam_.setTriggerMode(r_trigger))){
                        ROS_ERROR("Failed to configure triggers");
                        l_cam_.setTriggerMode(TRIGGER_OFF);
                        r_cam_.setTriggerMode(TRIGGER_OFF);
                        config.trigger = stereo_TGR_AUTO;
                        l_cam_.setFlash(FLASH_OFF);
                        r_cam_.setFlash(FLASH_OFF);
                }else{
                        l_cam_.setFlash(l_flash, config.flash_delay, config.flash_duration);
                        r_cam_.setFlash(r_flash, config.flash_delay, config.flash_duration);
                }
        }

        // Latch Auto Parameters
        if(auto_gain_ && !config.auto_gain){
                config.gain = l_cam_.getHardwareGain();
        }
        auto_gain_ = config.auto_gain;
        if(auto_exposure_ && !config.auto_exposure){
                config.exposure_time = l_cam_.getExposure();
        }
        auto_exposure_ = config.auto_exposure;

        // Pixel Clock
        if (l_cam_.getPixelClock() != config.l_pixel_clock){
                l_cam_.setPixelClock(&config.l_pixel_clock);
        }
        if (r_cam_.getPixelClock() != config.r_pixel_clock){
                r_cam_.setPixelClock(&config.r_pixel_clock);
        }

        reconfigCam(config, level, l_cam_);
        reconfigCam(config, level, r_cam_);

        trigger_mode_ = config.trigger;
        if(trigger_mode_ == stereo_TGR_SOFTWARE){
                timer_force_trigger_.setPeriod(ros::Duration(1/config.frame_rate));
        }

        if(zoom_ != config.zoom){
                zoom_ = config.zoom;
                loadIntrinsics(l_cam_, l_msg_camera_info_);
                loadIntrinsics(r_cam_, r_msg_camera_info_);
        }

        l_msg_camera_info_.header.frame_id = r_msg_camera_info_.header.frame_id =
                        config.frame_id;
        configured_ = true;
}

void StereoNode::timerCallback(const ros::TimerEvent& event)
{
        if(force_streaming_ ||
                        (l_pub_stream_.getNumSubscribers() > 0) ||
                        (r_pub_stream_.getNumSubscribers() > 0))
        {
                startCamera();
        }else{
                stopCamera();
        }
}
void StereoNode::timerForceTrigger(const ros::TimerEvent& event)
{
        if(trigger_mode_ == stereo_TGR_SOFTWARE){
                bool success = true;
                success &= l_cam_.forceTrigger();
                success &= r_cam_.forceTrigger();
                if(!success){
                        ROS_WARN("Failed to force trigger");
                }else{
//                      ROS_INFO("Force trigger");
                }
        }
}

// Support camera calibration requests
// http://www.ros.org/wiki/camera_calibration/Tutorials/MonocularCalibration
bool StereoNode::setCameraInfo(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp,
                Camera& cam, sensor_msgs::CameraInfo &msg_info)
{
        ROS_INFO("New camera info received");
        sensor_msgs::CameraInfo &info = req.camera_info;
        info.header.frame_id = msg_info.header.frame_id;

        // Sanity check: the image dimensions should match the resolution of the sensor.
        unsigned int height = cam.getHeight();
        unsigned int width = cam.getWidth();

        if (info.width != width || info.height != height) {
                rsp.success = false;
                rsp.status_message = (boost::format("Camera_info resolution %ix%i does not match current video "
                "setting, camera running at resolution %ix%i.")
                % info.width % info.height % width % height).str();
                ROS_ERROR("%s", rsp.status_message.c_str());
                return true;
        }

        std::string camname = cam.getCameraName();
        std::stringstream ini_stream;
        if (!camera_calibration_parsers::writeCalibrationIni(ini_stream, camname, info)) {
                rsp.status_message = "Error formatting camera_info for storage.";
                rsp.success = false;
        }else{
                std::string ini = ini_stream.str();
                fstream param_file;
                std::string filename = config_path_ + "/" + configFileName(cam);
                param_file.open(filename.c_str(), ios::in | ios::out | ios::trunc);

                if (param_file.is_open()) {
                        param_file<< ini.c_str();
                        param_file.close();

                        msg_info = info;
                        rsp.success = true;
                }else{
                        rsp.success = false;
                        rsp.status_message = "file write failed";
                }
        }
        if (!rsp.success){
                ROS_ERROR("%s", rsp.status_message.c_str());
        }
        return true;
}
bool StereoNode::setCameraInfoL(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp)
{
        return setCameraInfo(req, rsp, l_cam_, l_msg_camera_info_);
}
bool StereoNode::setCameraInfoR(sensor_msgs::SetCameraInfo::Request& req, sensor_msgs::SetCameraInfo::Response& rsp)
{
        return setCameraInfo(req, rsp, r_cam_, r_msg_camera_info_);
}

// Try to load previously saved camera calibration from a file.
void StereoNode::loadIntrinsics(Camera &cam, sensor_msgs::CameraInfo &msg_info)
{
        char buffer[12800];

        std::string MyPath = config_path_ + "/" + configFileName(cam);
        fstream param_file;
        param_file.open(MyPath.c_str(), ios::out | ios::in);

        if (param_file.is_open()) {
                param_file.read(buffer, 12800);
                param_file.close();
        }

        // Parse calibration file
        std::string camera_name;
        if (camera_calibration_parsers::parseCalibrationIni(buffer, camera_name, msg_info)){
                ROS_INFO("Calibration : %s %u", camera_name.c_str(), cam.getCameraSerialNo());
        }else{
                ROS_WARN("Failed to load intrinsics for camera from file");
        }
}

// Add properties to image message
sensor_msgs::ImagePtr StereoNode::processFrame(IplImage* frame, Camera &cam, cv_bridge::CvImage &converter,
                sensor_msgs::CameraInfoPtr &info, sensor_msgs::CameraInfo &msg_info)
{
        msg_info.roi.x_offset = 0;
        msg_info.roi.y_offset = 0;
        msg_info.height = cam.getHeight();
        msg_info.width = cam.getWidth();
        msg_info.roi.width = 0;
        msg_info.roi.height = 0;
        sensor_msgs::CameraInfoPtr msg(new sensor_msgs::CameraInfo(msg_info));
        info = msg;

        converter.header = msg_info.header;
        converter.encoding = "bgr8";
        converter.image = frame;
        return converter.toImageMsg();
}

// Timestamp and publish the image. Called by the streaming thread.
void StereoNode::publishImageL(IplImage * frame)
{
        l_msg_camera_info_.header.seq++;
        l_stamp_ = ros::Time::now();
        double diff = (l_stamp_ - r_stamp_).toSec();
        if((diff >= 0) && (diff < 0.02)){
                l_msg_camera_info_.header = r_msg_camera_info_.header;
        }else{
                l_msg_camera_info_.header.stamp = l_stamp_;
        }
        sensor_msgs::CameraInfoPtr info;
        sensor_msgs::ImagePtr img = processFrame(frame, l_cam_, l_converter_, info, l_msg_camera_info_);
        l_pub_stream_.publish(img, info);
}
void StereoNode::publishImageR(IplImage * frame)
{
        r_msg_camera_info_.header.seq++;
        r_stamp_ = ros::Time::now();
        double diff = (r_stamp_ - l_stamp_).toSec();
        if((diff >= 0) && (diff < 0.02)){
                r_msg_camera_info_.header = l_msg_camera_info_.header;
        }else{
                r_msg_camera_info_.header.stamp = r_stamp_;
        }
        sensor_msgs::CameraInfoPtr info;
        sensor_msgs::ImagePtr img = processFrame(frame, r_cam_, r_converter_, info, r_msg_camera_info_);
        r_pub_stream_.publish(img, info);
}

void StereoNode::startCamera()
{
        if(running_ || !configured_)
                return;
        l_cam_.startVideoCapture(boost::bind(&StereoNode::publishImageL, this, _1));
        r_cam_.startVideoCapture(boost::bind(&StereoNode::publishImageR, this, _1));
        timer_force_trigger_.start();
        ROS_INFO("Started video stream.");
        running_ = true;
}

void StereoNode::stopCamera()
{
        timer_force_trigger_.stop();
        if(!running_)
                return;
        ROS_INFO("Stopping video stream.");
        l_cam_.stopVideoCapture();
        r_cam_.stopVideoCapture();
        ROS_INFO("Stopped video stream.");
        running_ = false;
}

void StereoNode::closeCamera(){
        stopCamera();
        r_cam_.closeCamera();
        l_cam_.closeCamera();
}

} // namespace ueye