00001 00002 00003 #ifndef CAMERA_HANDLER_H 00004 #define CAMERA_HANDLER_H 00005 00006 #include "vrep_ros_plugin/GenericObjectHandler.h" 00007 #include <image_transport/image_transport.h> 00008 #include <sensor_msgs/SetCameraInfo.h> 00009 00010 00011 class CameraHandler : public GenericObjectHandler 00012 { 00013 public: 00014 CameraHandler(); 00015 ~CameraHandler(); 00016 00020 void synchronize(); 00021 00025 void handleSimulation(); 00026 00030 unsigned int getObjectType() const; 00031 00032 00033 protected: 00034 00038 void _initialize(); 00039 00043 image_transport::ImageTransport _it; 00044 00048 image_transport::CameraPublisher _pubIT; 00049 00053 sensor_msgs::CameraInfo _camera_info; 00054 00058 void computeCameraInfo(); 00059 00063 simFloat _lastPublishedImageTime; 00064 00068 double _acquisitionFrequency; 00072 bool _cameraIsRGB; 00073 00077 ros::ServiceServer _service; 00078 00085 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &res); 00086 }; 00087 00088 00089 #endif // ndef CAMERA_HANDLER_H