camera_info_controller.hpp

Go to the documentation of this file.

#ifndef USB_CAM_CONTROLLERS_CAMERA_INFO_CONTROLLER
#define USB_CAM_CONTROLLERS_CAMERA_INFO_CONTROLLER
 
#include <string>
 
#include <camera_info_manager/camera_info_manager.h>
#include <ros/duration.h>
#include <ros/node_handle.h>
#include <ros/time.h>
#include <sensor_msgs/CameraInfo.h>
#include <usb_cam_controllers/simple_packet_controller.hpp>
#include <usb_cam_hardware_interface/packet_interface.hpp>
 
#include <boost/make_shared.hpp>
#include <boost/shared_ptr.hpp>
 
namespace usb_cam_controllers {
 
class CameraInfoController : public SimplePacketController {
public:
  CameraInfoController() {}
 
  virtual ~CameraInfoController() {}
 
protected:
  virtual bool initImpl(usb_cam_hardware_interface::PacketInterface *hw, ros::NodeHandle &root_nh,
                        ros::NodeHandle &controller_nh) {
    frame_id_ = controller_nh.param< std::string >("camera_frame_id", "head_camera");
 
    publisher_ = controller_nh.advertise< sensor_msgs::CameraInfo >("camera_info", 1);
    info_manager_ = boost::make_shared< camera_info_manager::CameraInfoManager >(
        controller_nh, controller_nh.param< std::string >("camera_name", "head_camera"),
        controller_nh.param< std::string >("camera_info_url", ""));
 
    return true;
  }
 
  virtual void startingImpl(const ros::Time &time) {}
 
  virtual void updateImpl(const ros::Time &time, const ros::Duration &period) {
    // publish the camera info
    const sensor_msgs::CameraInfoPtr msg(new sensor_msgs::CameraInfo());
    *msg = info_manager_->getCameraInfo();
    msg->header.stamp = packet_iface_.getStamp();
    msg->header.frame_id = frame_id_;
    publisher_.publish(msg);
  }
 
  virtual void stoppingImpl(const ros::Time &time) {}
 
private:
  std::string frame_id_;
 
  ros::Publisher publisher_;
  boost::shared_ptr< camera_info_manager::CameraInfoManager > info_manager_;
};
 
} // namespace usb_cam_controllers
 
#endif