Program Listing for File camera_info_manager.hpp
↰ Return to documentation for file (include/camera_info_manager/camera_info_manager.hpp
)
/* -*- mode: C++ -*- */
/* $Id$ */
/*********************************************************************
* Software License Agreement (BSD License)
*
* Copyright (c) 2010-2012 Jack O'Quin
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the author nor other contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef CAMERA_INFO_MANAGER__CAMERA_INFO_MANAGER_HPP_
#define CAMERA_INFO_MANAGER__CAMERA_INFO_MANAGER_HPP_
#include <memory>
#include <mutex>
#include <string>
#include "rclcpp/node.hpp"
#include "rclcpp_lifecycle/lifecycle_node.hpp"
#include "sensor_msgs/msg/camera_info.hpp"
#include "sensor_msgs/srv/set_camera_info.hpp"
#include "camera_info_manager/visibility_control.h"
namespace camera_info_manager
{
using CameraInfo = sensor_msgs::msg::CameraInfo;
using SetCameraInfo = sensor_msgs::srv::SetCameraInfo;
class CameraInfoManager
{
public:
CAMERA_INFO_MANAGER_PUBLIC
CameraInfoManager(
rclcpp::Node * node,
const std::string & cname = "camera",
const std::string & url = "");
CAMERA_INFO_MANAGER_PUBLIC
CameraInfoManager(
rclcpp_lifecycle::LifecycleNode * node,
const std::string & cname = "camera",
const std::string & url = "");
CAMERA_INFO_MANAGER_PUBLIC
CameraInfoManager(
rclcpp::node_interfaces::NodeBaseInterface::SharedPtr node_base_interface,
rclcpp::node_interfaces::NodeServicesInterface::SharedPtr node_services_interface,
rclcpp::node_interfaces::NodeLoggingInterface::SharedPtr node_logger_interface,
const std::string & cname = "camera", const std::string & url = "",
rmw_qos_profile_t custom_qos = rmw_qos_profile_default);
CAMERA_INFO_MANAGER_PUBLIC
CameraInfo getCameraInfo(void);
CAMERA_INFO_MANAGER_PUBLIC
bool isCalibrated(void);
CAMERA_INFO_MANAGER_PUBLIC
bool loadCameraInfo(const std::string & url);
CAMERA_INFO_MANAGER_PUBLIC
std::string resolveURL(
const std::string & url,
const std::string & cname);
CAMERA_INFO_MANAGER_PUBLIC
bool setCameraName(const std::string & cname);
CAMERA_INFO_MANAGER_PUBLIC
bool setCameraInfo(const CameraInfo & camera_info);
CAMERA_INFO_MANAGER_PUBLIC
bool validateURL(const std::string & url);
private:
// recognized URL types
typedef enum
{
// supported URLs
URL_empty = 0, // empty string
URL_file, // file:
URL_package, // package:
// URLs not supported
URL_invalid, // anything >= is invalid
URL_flash, // flash:
} url_type_t;
// private methods
std::string getPackageFileName(const std::string & url);
bool loadCalibration(
const std::string & url,
const std::string & cname);
bool loadCalibrationFile(
const std::string & filename,
const std::string & cname);
url_type_t parseURL(const std::string & url);
bool saveCalibration(
const CameraInfo & new_info,
const std::string & url,
const std::string & cname);
bool saveCalibrationFile(
const CameraInfo & new_info,
const std::string & filename,
const std::string & cname);
void setCameraInfoService(
const std::shared_ptr<SetCameraInfo::Request> req,
std::shared_ptr<SetCameraInfo::Response> rsp);
std::mutex mutex_;
// private data
rclcpp::Service<SetCameraInfo>::SharedPtr info_service_;
rclcpp::Logger logger_;
std::string camera_name_;
std::string url_;
CameraInfo cam_info_;
bool loaded_cam_info_;
}; // class CameraInfoManager
} // namespace camera_info_manager
#endif // CAMERA_INFO_MANAGER__CAMERA_INFO_MANAGER_HPP_