diff --git a/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp b/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp index a95cb8b5..590bd9b8 100644 --- a/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp +++ b/camera_info_manager/include/camera_info_manager/camera_info_manager.hpp @@ -155,6 +155,13 @@ using SetCameraInfo = sensor_msgs::srv::SetCameraInfo; will be stored there, missing parent directories being created if necessary and possible. + @par Namespace + + The CameraInfoManager constructor takes an optional namespace + argument, which is used to set the ROS namespace for the + set_camera_info service. If not provided, the namespace defaults + to the private namespace of the node (i.e., "~/set_camera_info"). + @par Loading Calibration Data Prior to Fuerte, calibration information was loaded in the @@ -178,13 +185,15 @@ class CameraInfoManager CameraInfoManager( rclcpp::Node * node, const std::string & cname = "camera", - const std::string & url = ""); + const std::string & url = "", + const std::string & ns = "~"); CAMERA_INFO_MANAGER_PUBLIC CameraInfoManager( rclcpp_lifecycle::LifecycleNode * node, const std::string & cname = "camera", - const std::string & url = ""); + const std::string & url = "", + const std::string & ns = "~"); CAMERA_INFO_MANAGER_PUBLIC CameraInfoManager( @@ -192,7 +201,8 @@ class CameraInfoManager 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); + rmw_qos_profile_t custom_qos = rmw_qos_profile_default, + const std::string & ns = "~"); CAMERA_INFO_MANAGER_PUBLIC CameraInfo getCameraInfo(void); @@ -272,6 +282,7 @@ class CameraInfoManager rclcpp::Logger logger_; ///< logger std::string camera_name_; ///< camera name std::string url_; ///< URL for calibration data + std::string namespace_; ///< ROS namespace set_camera_info service CameraInfo cam_info_; ///< current CameraInfo bool loaded_cam_info_; ///< cam_info_ load attempted }; // class CameraInfoManager diff --git a/camera_info_manager/src/camera_info_manager.cpp b/camera_info_manager/src/camera_info_manager.cpp index 7522bc84..e592e24c 100644 --- a/camera_info_manager/src/camera_info_manager.cpp +++ b/camera_info_manager/src/camera_info_manager.cpp @@ -69,20 +69,25 @@ const std::string * subordinate names, like "left/camera" and "right/camera". * @param cname default camera name * @param url default Uniform Resource Locator for loading and saving data. + * @param ns namespace for the set_camera_info service. If not specified, + * the service name will be "~/set_camera_info". */ CameraInfoManager::CameraInfoManager( rclcpp::Node * node, const std::string & cname, - const std::string & url) + const std::string & url, const std::string & ns) : CameraInfoManager(node->get_node_base_interface(), - node->get_node_services_interface(), node->get_node_logging_interface(), cname, url) + node->get_node_services_interface(), node->get_node_logging_interface(), cname, url, + rmw_qos_profile_default, ns) { } CameraInfoManager::CameraInfoManager( rclcpp_lifecycle::LifecycleNode * node, - const std::string & cname, const std::string & url) + const std::string & cname, const std::string & url, + const std::string & ns) : CameraInfoManager(node->get_node_base_interface(), - node->get_node_services_interface(), node->get_node_logging_interface(), cname, url) + node->get_node_services_interface(), node->get_node_logging_interface(), cname, url, + rmw_qos_profile_default, ns) { } @@ -90,17 +95,19 @@ CameraInfoManager::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, const std::string & url, rmw_qos_profile_t custom_qos) + const std::string & cname, const std::string & url, + rmw_qos_profile_t custom_qos, const std::string & ns) : logger_(node_logger_interface->get_logger()), camera_name_(cname), url_(url), + namespace_(ns), loaded_cam_info_(false) { using namespace std::placeholders; // register callback for camera calibration service request info_service_ = rclcpp::create_service( - node_base_interface, node_services_interface, "~/set_camera_info", + node_base_interface, node_services_interface, namespace_ + "/set_camera_info", std::bind(&CameraInfoManager::setCameraInfoService, this, _1, _2), custom_qos, nullptr); }