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
00085 namespace camera_info_manager
00086 {
00087
00088 class CameraInfoManager
00089 {
00090 public:
00091
00092 CameraInfoManager(ros::NodeHandle nh,
00093 const std::string &cname="camera",
00094 const std::string &url="");
00095
00105 sensor_msgs::CameraInfo getCameraInfo(void)
00106 {
00107 boost::mutex::scoped_lock lock_(mutex_);
00108 return cam_info_;
00109 }
00110
00112 bool isCalibrated(void)
00113 {
00114 boost::mutex::scoped_lock lock_(mutex_);
00115 return (cam_info_.K[0] != 0.0);
00116 }
00117
00118 bool loadCameraInfo(const std::string &url);
00119 bool setCameraName(const std::string &cname);
00120 bool validateURL(const std::string &url);
00121
00122 private:
00123
00124
00125 typedef enum
00126 {
00127
00128 URL_empty = 0,
00129 URL_file,
00130 URL_package,
00131
00132 URL_invalid,
00133 URL_flash,
00134 } url_type_t;
00135
00136
00137 std::string getPackageFileName(const std::string &url);
00138 bool loadCalibration(const std::string &url,
00139 const std::string &cname);
00140 bool loadCalibrationFile(const std::string &filename,
00141 const std::string &cname);
00142 url_type_t parseURL(const std::string &url);
00143 bool saveCalibration(const sensor_msgs::CameraInfo &new_info,
00144 const std::string &url,
00145 const std::string &cname);
00146 bool saveCalibrationFile(const sensor_msgs::CameraInfo &new_info,
00147 const std::string &filename,
00148 const std::string &cname);
00149 bool setCameraInfo(sensor_msgs::SetCameraInfo::Request &req,
00150 sensor_msgs::SetCameraInfo::Response &rsp);
00151
00157 boost::mutex mutex_;
00158
00159
00160 ros::NodeHandle nh_;
00161 ros::ServiceServer info_service_;
00162 std::string camera_name_;
00163 std::string url_;
00164 sensor_msgs::CameraInfo cam_info_;
00165
00166 };
00167
00168 };
00169
00170
00171
00172 using camera_info_manager::CameraInfoManager;
00173
00174 #endif // _CAMERA_INFO_MANAGER_H_