40 #include <sys/types.h> 44 #define S_ISDIR(mode) (((mode) & S_IFMT) == S_IFDIR) 52 #include <boost/algorithm/string.hpp> 86 const std::string &cname,
87 const std::string &url):
91 loaded_cam_info_(false)
120 boost::mutex::scoped_lock lock_(
mutex_);
139 return sensor_msgs::CameraInfo();
153 size_t prefix_len = std::string(
"package://").length();
154 size_t rest = url.find(
'/', prefix_len);
155 std::string
package(url.substr(prefix_len, rest - prefix_len));
167 return pkgPath + url.substr(rest);
187 boost::mutex::scoped_lock lock_(
mutex_);
218 const std::string &cname)
220 bool success =
false;
222 const std::string resURL(
resolveURL(url, cname));
234 ROS_INFO(
"using default calibration URL");
245 ROS_WARN(
"[CameraInfoManager] reading from flash not implemented yet");
251 if (!filename.empty())
276 const std::string &cname)
278 bool success =
false;
281 std::string cam_name;
282 sensor_msgs::CameraInfo cam_info;
286 if (cname != cam_name)
289 << cam_name <<
" in file " << filename);
294 boost::mutex::scoped_lock lock(
mutex_);
300 ROS_WARN_STREAM(
"Camera calibration file " << filename <<
" not found.");
321 boost::mutex::scoped_lock lock(
mutex_);
341 const std::string &cname)
343 std::string resolved;
349 size_t dollar = url.find(
'$', rest);
351 if (dollar >= url.length())
354 resolved += url.substr(rest);
359 resolved += url.substr(rest, dollar-rest);
361 if (url.substr(dollar+1, 1) !=
"{")
366 else if (url.substr(dollar+1, 6) ==
"{NAME}")
372 else if (url.substr(dollar+1, 10) ==
"{ROS_HOME}")
375 std::string ros_home;
377 if ((ros_home_env = getenv(
"ROS_HOME")))
380 ros_home = ros_home_env;
382 else if ((ros_home_env = getenv(
"HOME")))
385 ros_home = ros_home_env;
388 resolved += ros_home;
395 " invalid URL substitution (not resolved): " 420 if (boost::iequals(url.substr(0, 8),
"file:///"))
424 if (boost::iequals(url.substr(0, 9),
"flash:///"))
428 if (boost::iequals(url.substr(0, 10),
"package://"))
432 size_t rest = url.find(
'/', 10);
433 if (rest < url.length()-1 && rest > 10)
451 const std::string &url,
452 const std::string &cname)
454 bool success =
false;
456 const std::string resURL(
resolveURL(url, cname));
474 if (!filename.empty())
501 const std::string &filename,
502 const std::string &cname)
507 size_t last_slash = filename.rfind(
'/');
508 if (last_slash >= filename.length())
517 std::string dirname(filename.substr(0, last_slash+1));
518 struct stat stat_data;
519 int rc = stat(dirname.c_str(), &stat_data);
525 std::string
command(
"mkdir -p " + dirname);
526 rc = system(command.c_str());
542 else if (!S_ISDIR(stat_data.st_mode))
566 sensor_msgs::SetCameraInfo::Response &rsp)
569 std::string url_copy;
572 boost::mutex::scoped_lock lock(
mutex_);
581 ROS_ERROR(
"set_camera_info service called, but driver not running.");
582 rsp.status_message =
"Camera driver not running.";
589 rsp.status_message =
"Error storing camera calibration.";
611 for (
unsigned i = 0; i < cname.size(); ++i)
613 if (!isalnum(cname[i]) && cname[i] !=
'_')
621 boost::mutex::scoped_lock lock(
mutex_);
639 boost::mutex::scoped_lock lock(
mutex_);
658 boost::mutex::scoped_lock lock(
mutex_);
bool saveCalibration(const sensor_msgs::CameraInfo &new_info, const std::string &url, const std::string &cname)
sensor_msgs::CameraInfo getCameraInfo(void)
bool loaded_cam_info_
cam_info_ load attempted
def readCalibration(file_name)
std::string url_
URL for calibration data.
std::string getPackageFileName(const std::string &url)
bool loadCameraInfo(const std::string &url)
std::string resolveURL(const std::string &url, const std::string &cname)
ros::ServiceServer info_service_
set_camera_info service
sensor_msgs::CameraInfo cam_info_
current CameraInfo
CameraInfo Manager interface.
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
const std::string default_camera_info_url
bool loadCalibrationFile(const std::string &filename, const std::string &cname)
url_type_t parseURL(const std::string &url)
bool validateURL(const std::string &url)
boost::mutex mutex_
mutual exclusion lock for private data
CameraInfoManager(ros::NodeHandle nh, const std::string &cname="camera", const std::string &url="")
ROSLIB_DECL std::string command(const std::string &cmd)
bool writeCalibration(const std::string &file_name, const std::string &camera_name, const sensor_msgs::CameraInfo &cam_info)
ros::NodeHandle nh_
node handle for service
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
ROSLIB_DECL std::string getPath(const std::string &package_name)
#define ROS_INFO_STREAM(args)
bool setCameraInfo(const sensor_msgs::CameraInfo &camera_info)
bool loadCalibration(const std::string &url, const std::string &cname)
#define ROS_ERROR_STREAM(args)
bool setCameraInfoService(sensor_msgs::SetCameraInfo::Request &req, sensor_msgs::SetCameraInfo::Response &rsp)
bool saveCalibrationFile(const sensor_msgs::CameraInfo &new_info, const std::string &filename, const std::string &cname)
std::string camera_name_
camera name
bool setCameraName(const std::string &cname)