Ejemplo n.º 1
0
	/** 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;
	}
Ejemplo n.º 2
0
  /** 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_);
  }
Ejemplo n.º 3
0
  /** 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;
  }
Ejemplo n.º 4
0
  /** 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;
  }
Ejemplo n.º 5
0
  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);
  }
Ejemplo n.º 6
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 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);
  }
Ejemplo n.º 7
0
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,&timestamp);
                _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;
}
Ejemplo n.º 8
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);
	}