Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef _CAMERA_INFO_MANAGER_H_
00039 #define _CAMERA_INFO_MANAGER_H_
00040
00041 #include <ros/ros.h>
00042 #include <boost/thread/mutex.hpp>
00043 #include <sensor_msgs/CameraInfo.h>
00044 #include <sensor_msgs/SetCameraInfo.h>
00045
00053 namespace camera_info_manager
00054 {
00055
00174 class CameraInfoManager
00175 {
00176 public:
00177
00178 CameraInfoManager(ros::NodeHandle nh,
00179 const std::string &cname="camera",
00180 const std::string &url="");
00181
00182 sensor_msgs::CameraInfo getCameraInfo(void);
00183 bool isCalibrated(void);
00184 bool loadCameraInfo(const std::string &url);
00185 std::string resolveURL(const std::string &url,
00186 const std::string &cname);
00187 bool setCameraName(const std::string &cname);
00188 bool setCameraInfo(const sensor_msgs::CameraInfo &camera_info);
00189 bool validateURL(const std::string &url);
00190
00191 private:
00192
00193
00194 typedef enum
00195 {
00196
00197 URL_empty = 0,
00198 URL_file,
00199 URL_package,
00200
00201 URL_invalid,
00202 URL_flash,
00203 } url_type_t;
00204
00205
00206 std::string getPackageFileName(const std::string &url);
00207 bool loadCalibration(const std::string &url,
00208 const std::string &cname);
00209 bool loadCalibrationFile(const std::string &filename,
00210 const std::string &cname);
00211 url_type_t parseURL(const std::string &url);
00212 bool saveCalibration(const sensor_msgs::CameraInfo &new_info,
00213 const std::string &url,
00214 const std::string &cname);
00215 bool saveCalibrationFile(const sensor_msgs::CameraInfo &new_info,
00216 const std::string &filename,
00217 const std::string &cname);
00218 bool setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req,
00219 sensor_msgs::SetCameraInfo::Response &rsp);
00220
00229 boost::mutex mutex_;
00230
00231
00232 ros::NodeHandle nh_;
00233 ros::ServiceServer info_service_;
00234 std::string camera_name_;
00235 std::string url_;
00236 sensor_msgs::CameraInfo cam_info_;
00237 bool loaded_cam_info_;
00238
00239 };
00240
00241 };
00242
00243 #endif // _CAMERA_INFO_MANAGER_H_