Fix/tests (#29)

nathanhhughes · web-flow · commit 4f7d692df4a9 · 2025-08-07T16:26:15.000-04:00
* update sensor extrinsics lookup to warn after timeout

* actually visit fields

* add node init to ros tests

* fix lookup exit behavior and broadcast test tf

* add warning messages if camera info lookup goes on too long

* add proper shutdown

* fix info subscription

* clean up log statements and add ability to print full config

* fix shutdown behavior
diff --git a/hydra_ros/app/hydra_node.cpp b/hydra_ros/app/hydra_node.cpp
@@ -155,5 +155,6 @@ int main(int argc, char* argv[]) {
     google::RemoveLogSink(ros_sink.get());
   }
 
+  rclcpp::shutdown();
   return 0;
 }
diff --git a/hydra_ros/include/hydra_ros/hydra_ros_pipeline.h b/hydra_ros/include/hydra_ros/hydra_ros_pipeline.h
@@ -38,6 +38,7 @@
 #include <hydra/backend/backend_module.h>
 #include <hydra/common/hydra_pipeline.h>
 #include <hydra/frontend/graph_builder.h>
+
 #include <memory>
 
 #include "hydra_ros/input/feature_receiver.h"
@@ -52,15 +53,26 @@ class ExternalLoopClosureSubscriber;
 class HydraRosPipeline : public HydraPipeline {
  public:
   struct Config {
+    //! @brief Configuration for active window / metric-semantic reconstruction
     config::VirtualConfig<ActiveWindowModule> active_window{
         ReconstructionModule::Config()};
+    //! @brief Configuration for frontend module
     config::VirtualConfig<GraphBuilder> frontend{GraphBuilder::Config()};
+    //! @brief Configuration for backend module
     config::VirtualConfig<BackendModule> backend{BackendModule::Config()};
+    //! @brief Publish frontend scene graph in addition to backend
     bool enable_frontend_output = true;
-    bool enable_zmq_interface = true;
+    //! @brief Turn on zmq-based publishing
+    bool enable_zmq_interface = false;
+    //! @brief Configuration for sensor inputs
     RosInputModule::Config input;
+    //! @brief Receiver for language features
     config::VirtualConfig<FeatureReceiver> features;
+    //! @brief Verbosity setting for main pipeline class
     int verbosity = 1;
+    //! @brief Show the config passed to Hydra (before resolving sensor configurations)
+    bool preprint_config = false;
+    //! @brief Monitor to report whether or Hydra is running normally
     StatusMonitor::Config status_monitor;
   } const config;
 
diff --git a/hydra_ros/include/hydra_ros/input/ros_sensors.h b/hydra_ros/include/hydra_ros/input/ros_sensors.h
@@ -61,6 +61,14 @@ struct RosExtrinsics : SensorExtrinsics {
     std::string sensor_frame = "";
     //! @brief Override global robot frame ID
     std::string robot_frame = "";
+    //! @brief Amount of time before warning about missing TF (0 disables warning)
+    double warning_timeout_s = 10.0;
+    //! @brief Amount of time before forcing Hydra to exit (0 disables exit)
+    double error_timeout_s = 0.0;
+    //! @brief Sample period for looking up extrinsics transform
+    double wait_duration_s = 0.1;
+    //! @brief TF lookup verbosity
+    int verbosity = 3;
   } const config;
 
   explicit RosExtrinsics(const Config& config);
@@ -72,7 +80,12 @@ struct RosExtrinsics : SensorExtrinsics {
 
 struct RosCamera : InvalidSensor {
   struct Config : Sensor::Config {
+    //! @brief Optional namespace for overriding Hydra's default sensor namespaces
     std::string ns = "";
+    //! @brief Amount of time before warning about missing camera info (0 disables warning)
+    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;
   } const config;
 
   explicit RosCamera(const Config& config);
diff --git a/hydra_ros/include/hydra_ros/utils/tf_lookup.h b/hydra_ros/include/hydra_ros/utils/tf_lookup.h
@@ -46,27 +46,23 @@
 
 namespace hydra {
 
-PoseStatus lookupTransform(const std::string& target,
-                           const std::string& source,
-                           double wait_duration_s = 0.1,
-                           int verbosity = 10);
-
 PoseStatus lookupTransform(const tf2_ros::Buffer& buffer,
                            const std::optional<rclcpp::Time>& stamp,
                            const std::string& target,
                            const std::string& source,
-                           std::optional<size_t> max_tries = std::nullopt,
+                           size_t max_tries = 0,
                            double wait_duration_s = 0.1,
-                           int verbosity = 10);
+                           int verbosity = 10,
+                           std::string* message = nullptr);
 
 struct TFLookup {
   struct Config {
     //! Amount of time to wait between tf lookup attempts
     double wait_duration_s = 0.1;
     //! Buffer size in second for tf
     double buffer_size_s = 30.0;
-    //! Number of lookup attempts before giving up
-    int max_tries = 5;
+    //! Number of lookup attempts before giving up (0 will try forever)
+    size_t max_tries = 5;
     //! Logging verbosity of tf lookup process
     int verbosity = 3;
     //! Get the buffer size in nanoseconds
diff --git a/hydra_ros/src/hydra_ros_pipeline.cpp b/hydra_ros/src/hydra_ros_pipeline.cpp
@@ -71,14 +71,20 @@ void declare_config(HydraRosPipeline::Config& config) {
   config.features.setOptional();
   field(config.features, "features");
   field(config.verbosity, "verbosity");
+  field(config.preprint_config, "preprint_config");
   field(config.status_monitor, "status_monitor");
 }
 
 HydraRosPipeline::HydraRosPipeline(int robot_id, int config_verbosity)
     : HydraPipeline(config::fromContext<PipelineConfig>(), robot_id, config_verbosity),
       config(config::checkValid(config::fromContext<Config>())) {
-  LOG_IF(INFO, config.verbosity >= 1) << "Starting Hydra-ROS with input configuration\n"
-                                      << config::toString(config.input);
+  if (config.preprint_config) {
+    LOG(INFO) << "Using configuration to start Hydra\n" << config::toString(config);
+  } else {
+    LOG_IF(INFO, config.verbosity >= 1)
+        << "Starting Hydra-ROS with input configuration\n"
+        << config::toString(config.input);
+  }
 }
 
 HydraRosPipeline::~HydraRosPipeline() {}
diff --git a/hydra_ros/src/input/ros_sensors.cpp b/hydra_ros/src/input/ros_sensors.cpp
@@ -70,30 +70,77 @@ void fillConfigFromInfo(const CameraInfo& msg, Camera::Config& cam_config) {
   cam_config.cy = msg.k[5];
 }
 
-std::optional<sensor_msgs::msg::CameraInfo> getCameraInfo(const std::string& ns) {
+std::optional<sensor_msgs::msg::CameraInfo> getCameraInfo(const RosCamera::Config& c,
+                                                          const std::string& ns) {
   auto nh = getHydraNodeHandle(ns);
   const auto resolved_topic = nh.resolve_name("camera_info", false);
   LOG(INFO) << "Waiting for CameraInfo on " << resolved_topic
             << " to initialize sensor model";
 
-  auto msg = ianvs::getSingleMessage<CameraInfo>(nh, "camera_info", true);
-  if (!msg) {
-    LOG(ERROR) << "did not receive message on " << resolved_topic;
-    return std::nullopt;
+  const auto start = nh.now();
+  const auto qos = rclcpp::QoS(1).transient_local();
+  const size_t timeout = std::floor(c.warning_timeout_s * 1000);
+
+  std::optional<sensor_msgs::msg::CameraInfo> msg;
+  while (!msg && rclcpp::ok()) {
+    msg = ianvs::getSingleMessage<CameraInfo>(nh, "camera_info", true, qos, timeout);
+    if (!msg) {
+      LOG(WARNING) << "Waiting for CameraInfo on topic '" << resolved_topic << "'";
+    }
+
+    const auto diff = nh.now() - start;
+    if (c.error_timeout_s && (diff.seconds() > c.error_timeout_s)) {
+      LOG(ERROR) << "Sensor intrinsics lookup timed out on '" << resolved_topic << "'";
+      break;
+    }
   }
 
-  return *msg;
+  return msg;
 }
 
-ParamSensorExtrinsics::Config lookupExtrinsics(const std::string& sensor_frame,
+ParamSensorExtrinsics::Config lookupExtrinsics(const RosExtrinsics::Config& config,
+                                               const std::string& sensor_frame,
                                                const std::string& robot_frame) {
-  const auto pose = lookupTransform(robot_frame, sensor_frame);
-  CHECK(pose.is_valid) << "Could not look up extrinsics from ros!";
+  LOG(INFO) << "Looking for sensor extrinsics '" << robot_frame << "_T_" << sensor_frame
+            << "' via TF";
+
+  auto nh = getHydraNodeHandle("");
+  auto clock = nh.node().get<rclcpp::node_interfaces::NodeClockInterface>();
+
+  tf2_ros::Buffer buffer(clock->get_clock());
+  tf2_ros::TransformListener listener(buffer);
+
+  size_t warning_tries = config.warning_timeout_s / config.wait_duration_s;
+
+  const auto start = nh.now();
+  PoseStatus status;
+  while (!status && rclcpp::ok()) {
+    std::string message;
+    status = lookupTransform(buffer,
+                             std::nullopt,
+                             robot_frame,
+                             sensor_frame,
+                             warning_tries,
+                             config.wait_duration_s,
+                             config.verbosity,
+                             &message);
+    if (!status) {
+      LOG(WARNING) << "Waiting for sensor extrinsics " << message;
+    }
+
+    const auto diff = nh.now() - start;
+    if (config.error_timeout_s && (diff.seconds() > config.error_timeout_s)) {
+      LOG(ERROR) << "Sensor extrinsics lookup timed out for " << message;
+      break;
+    }
+  }
+
+  CHECK(status.is_valid) << "Could not look up extrinsics from ros!";
 
-  ParamSensorExtrinsics::Config config;
-  config.body_R_sensor = pose.target_R_source;
-  config.body_p_sensor = pose.target_p_source;
-  return config;
+  ParamSensorExtrinsics::Config params;
+  params.body_R_sensor = status.target_R_source;
+  params.body_p_sensor = status.target_p_source;
+  return params;
 }
 
 RosExtrinsics::RosExtrinsics(const Config&) {
@@ -109,13 +156,23 @@ void declare_config(RosExtrinsics::Config& config) {
   name("RosExtrinsics::Config");
   field(config.sensor_frame, "sensor_frame");
   field(config.robot_frame, "robot_frame");
+  field(config.warning_timeout_s, "warning_timeout_s", "s");
+  field(config.error_timeout_s, "error_timeout_s", "s");
+  field(config.wait_duration_s, "wait_duration_s", "s");
+  field(config.verbosity, "verbosity");
+  check(config.warning_timeout_s, GE, 0.0, "warning_timeout_s");
+  check(config.error_timeout_s, GE, 0.0, "error_timeout_s");
 }
 
 void declare_config(RosCamera::Config& config) {
   using namespace config;
   name("RosCamera::Config");
   base<Sensor::Config>(config);
   field(config.ns, "ns");
+  field(config.warning_timeout_s, "warning_timeout_s", "s");
+  field(config.error_timeout_s, "error_timeout_s", "s");
+  check(config.warning_timeout_s, GE, 0.0, "warning_timeout_s");
+  check(config.error_timeout_s, GE, 0.0, "error_timeout_s");
 }
 
 namespace input {
@@ -145,7 +202,7 @@ VirtualSensor loadExtrinsics(const VirtualSensor& sensor,
     return {};
   }
 
-  const auto info = lookupExtrinsics(frame, parent);
+  const auto info = lookupExtrinsics(derived, frame, parent);
   config::VirtualConfig<SensorExtrinsics> new_config(info);
   base_contents["extrinsics"] = config::toYaml(new_config);
   return config::fromYaml<VirtualSensor>(base_contents);
@@ -161,7 +218,7 @@ VirtualSensor loadSensor(const VirtualSensor& sensor, const std::string& sensor_
 
   const auto ns =
       derived.ns.empty() ? "~/input/" + sensor_name + std::string("/rgb") : derived.ns;
-  const auto msg = getCameraInfo(ns);
+  const auto msg = getCameraInfo(derived, ns);
   if (!msg) {
     return {};
   }
diff --git a/hydra_ros/src/utils/tf_lookup.cpp b/hydra_ros/src/utils/tf_lookup.cpp
@@ -48,34 +48,25 @@
 
 namespace hydra {
 
-rclcpp::Clock::SharedPtr getHydraClock() {
-  auto nh = getHydraNodeHandle("");
-  auto clock = nh.node().get<rclcpp::node_interfaces::NodeClockInterface>();
-  return clock->get_clock();
-}
-
 std::chrono::nanoseconds TFLookup::Config::buffer_size_ns() const {
   return std::chrono::duration_cast<std::chrono::nanoseconds>(
       std::chrono::duration<double>(buffer_size_s));
 }
 
-PoseStatus lookupTransform(const std::string& target,
-                           const std::string& source,
-                           double wait_duration_s,
-                           int verbosity) {
-  tf2_ros::Buffer buffer(getHydraClock());
-  tf2_ros::TransformListener listener(buffer);
-  return lookupTransform(
-      buffer, std::nullopt, target, source, std::nullopt, wait_duration_s, verbosity);
+rclcpp::Clock::SharedPtr getHydraClock() {
+  auto nh = getHydraNodeHandle("");
+  auto clock = nh.node().get<rclcpp::node_interfaces::NodeClockInterface>();
+  return clock->get_clock();
 }
 
 PoseStatus lookupTransform(const tf2_ros::Buffer& buffer,
                            const std::optional<rclcpp::Time>& stamp,
                            const std::string& target,
                            const std::string& source,
-                           std::optional<size_t> max_tries,
+                           size_t max_tries,
                            double wait_duration_s,
-                           int verbosity) {
+                           int verbosity,
+                           std::string* message) {
   using namespace std::chrono_literals;
   rclcpp::WallRate tf_wait_rate(1.0 / wait_duration_s);
   std::string stamp_suffix;
@@ -95,8 +86,8 @@ PoseStatus lookupTransform(const tf2_ros::Buffer& buffer,
   while (rclcpp::ok()) {
     VLOG(verbosity) << "Attempting to lookup tf @ " << lookup_time.nanoseconds()
                     << " [ns]: " << attempt_number << " / "
-                    << (max_tries ? std::to_string(max_tries.value()) : "n/a");
-    if (max_tries && attempt_number >= *max_tries) {
+                    << (max_tries ? std::to_string(max_tries) : "n/a");
+    if (max_tries && attempt_number >= max_tries) {
       break;
     }
 
@@ -112,16 +103,27 @@ PoseStatus lookupTransform(const tf2_ros::Buffer& buffer,
   }
 
   if (!have_transform) {
-    LOG(ERROR) << "Failed to find: " << target << "_T_" << source << stamp_suffix
-               << ": " << err_str;
+    std::stringstream ss;
+    ss << "'" << target << "_T_" << source << stamp_suffix;
+    if (!err_str.empty()) {
+      ss << "' (last error: '" << err_str << "')";
+    }
+
+    if (message) {
+      *message = ss.str();
+    } else {
+      LOG(ERROR) << "Failed to find " << ss.str();
+    }
+
     return {false, {}, {}};
   }
 
   geometry_msgs::msg::TransformStamped transform;
   try {
     transform = buffer.lookupTransform(target, source, lookup_time);
   } catch (const tf2::TransformException& ex) {
-    LOG(ERROR) << "Failed to look up: " << target << "_T_" << source << stamp_suffix;
+    LOG(ERROR) << "Failed to look up available transform " << target << "_T_" << source
+               << stamp_suffix;
     return {false, {}, {}};
   }
 
@@ -148,16 +150,12 @@ TFLookup::TFLookup(const Config& config)
       listener(buffer) {}
 
 PoseStatus TFLookup::getBodyPose(uint64_t timestamp_ns) const {
-  // negative or 0 for tf_max_tries means we spin forever if the transform isn't present
-  const std::optional<size_t> max_tries =
-      config.max_tries > 0 ? std::optional<size_t>(config.max_tries) : std::nullopt;
-
   rclcpp::Time curr_ros_time(timestamp_ns);
   return lookupTransform(buffer,
                          curr_ros_time,
                          GlobalInfo::instance().getFrames().odom,
                          GlobalInfo::instance().getFrames().robot,
-                         max_tries,
+                         config.max_tries,
                          config.wait_duration_s,
                          config.verbosity);
 }
diff --git a/hydra_ros/tests/hydra_ros.test b/hydra_ros/tests/hydra_ros.test
diff --git a/hydra_ros/tests/main.cpp b/hydra_ros/tests/main.cpp
diff --git a/hydra_ros/tests/test_ros_sensors.cpp b/hydra_ros/tests/test_ros_sensors.cpp

Original file line number	Diff line number	Diff line change
`@@ -155,5 +155,6 @@ int main(int argc, char* argv[]) {`
`155`	`155`	`google::RemoveLogSink(ros_sink.get());`
`156`	`156`	`}`
`157`	`157`
	`158`	`+ rclcpp::shutdown();`
`158`	`159`	`return 0;`
`159`	`160`	`}`