diff --git a/hydra_ros/include/hydra_ros/input/ros_sensors.h b/hydra_ros/include/hydra_ros/input/ros_sensors.h index 47fb004..8e65a14 100644 --- a/hydra_ros/include/hydra_ros/input/ros_sensors.h +++ b/hydra_ros/include/hydra_ros/input/ros_sensors.h @@ -87,6 +87,8 @@ struct RosCamera : InvalidSensor { double warning_timeout_s = 10.0; //! @brief Amount of time to wait before forcing Hydra to exit (0 disables exit) double error_timeout_s = 0.0; + //! @brief Whether or not to use latching for subscriber + bool latch_info_sub = false; } const config; explicit RosCamera(const Config& config); diff --git a/hydra_ros/src/input/ros_sensors.cpp b/hydra_ros/src/input/ros_sensors.cpp index 83f4d05..f6e39de 100644 --- a/hydra_ros/src/input/ros_sensors.cpp +++ b/hydra_ros/src/input/ros_sensors.cpp @@ -69,16 +69,18 @@ void fillConfigFromInfo(const CameraInfo& msg, Camera::Config& cam_config) { cam_config.cy = msg.k[5]; } -std::optional getCameraInfo(const RosCamera::Config& c, - const std::string& ns) { +std::optional getCameraInfo( + const RosCamera::Config& config, const std::string& ns) { auto nh = ianvs::NodeHandle::this_node(ns); const auto resolved_topic = nh.resolve_name("camera_info", false); LOG(INFO) << "Waiting for CameraInfo on " << resolved_topic << " to initialize sensor model"; const auto start = nh.now(); - const auto qos = rclcpp::QoS(1).durability(rclcpp::DurabilityPolicy::BestAvailable); - const size_t timeout = std::floor(c.warning_timeout_s * 1000); + const auto qos = rclcpp::QoS(1).durability( + config.latch_info_sub ? rclcpp::DurabilityPolicy::TransientLocal + : rclcpp::DurabilityPolicy::Volatile); + const size_t timeout = std::floor(config.warning_timeout_s * 1000); std::optional msg; while (!msg && rclcpp::ok()) { @@ -88,7 +90,7 @@ std::optional getCameraInfo(const RosCamera::Confi } const auto diff = nh.now() - start; - if (c.error_timeout_s && (diff.seconds() > c.error_timeout_s)) { + if (config.error_timeout_s && (diff.seconds() > config.error_timeout_s)) { LOG(ERROR) << "Sensor intrinsics lookup timed out on '" << resolved_topic << "'"; break; } @@ -170,6 +172,7 @@ void declare_config(RosCamera::Config& config) { field(config.ns, "ns"); field(config.warning_timeout_s, "warning_timeout_s", "s"); field(config.error_timeout_s, "error_timeout_s", "s"); + field(config.latch_info_sub, "latch_info_sub"); check(config.warning_timeout_s, GE, 0.0, "warning_timeout_s"); check(config.error_timeout_s, GE, 0.0, "error_timeout_s"); } diff --git a/hydra_ros/tests/test_ros_sensors.cpp b/hydra_ros/tests/test_ros_sensors.cpp index 3c550ae..6c32ec6 100644 --- a/hydra_ros/tests/test_ros_sensors.cpp +++ b/hydra_ros/tests/test_ros_sensors.cpp @@ -169,6 +169,7 @@ TEST_F(RosSensors, Camera) { config.max_range = 15; config.warning_timeout_s = 1.0; config.error_timeout_s = 5.0; + config.latch_info_sub = true; config.extrinsics = IdentitySensorExtrinsics::Config(); { // should be able to parse intrinsics and extrinsics separately