Feature/status

semantic_inference_ros/CMakeLists.txt

@@ -17,6 +17,7 @@ find_package(cv_bridge REQUIRED)
 find_package(ianvs REQUIRED)
 find_package(image_geometry REQUIRED)
 find_package(message_filters REQUIRED)
+find_package(nlohmann_json REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
 find_package(rosbag2_transport REQUIRED)
@@ -46,6 +47,7 @@ target_link_libraries(
           ianvs::ianvs
           image_geometry::image_geometry
           message_filters::message_filters
+          nlohmann_json::nlohmann_json
           rclcpp_components::component
           tf2_eigen::tf2_eigen
           tf2_ros::tf2_ros

semantic_inference_ros/package.xml

@@ -16,6 +16,7 @@
   <depend>ianvs</depend>
   <depend>image_geometry</depend>
   <depend>message_filters</depend>
+  <depend>nlohmann-json-dev</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>rosbag2_transport</depend>

semantic_inference_ros/src/segmentation_nodelet.cpp

@@ -37,13 +37,16 @@
 #include <semantic_inference/segmenter.h>
 
 #include <atomic>
+#include <chrono>
 #include <mutex>
 #include <optional>
 #include <thread>
 
 #include <cv_bridge/cv_bridge.hpp>
+#include <nlohmann/json.hpp>
 #include <opencv2/core.hpp>
 #include <rclcpp/node.hpp>
+#include <std_msgs/msg/string.hpp>
 
 #include "semantic_inference_ros/output_publisher.h"
 #include "semantic_inference_ros/ros_log_sink.h"
@@ -63,6 +66,12 @@ class SegmentationNode : public rclcpp::Node {
     ImageRotator::Config image_rotator;
     bool show_config = true;
     bool show_output_config = false;
+    struct Status {
+      std::string nickname = "semantic_inference";
+      size_t rate_window_size = 10;
+      double period_s = 0.5;
+      double max_heartbeat_s = 10.0;
+    } status;
   } const config;
 
   explicit SegmentationNode(const rclcpp::NodeOptions& options);
@@ -73,14 +82,40 @@ class SegmentationNode : public rclcpp::Node {
  private:
   void runSegmentation(const Image::ConstSharedPtr& msg);
 
+  void recordStatus(double elapsed_s, const std::string& error = "");
+
+  void publishStatus();
+
   std::unique_ptr<Segmenter> segmenter_;
   std::unique_ptr<ImageWorker> worker_;
 
+  std::mutex status_mutex_;
+  std::optional<rclcpp::Time> last_call_;
+  std::string last_status_;
+  std::list<double> elapsed_samples_s_;
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr status_pub_;
+
   OutputPublisher output_pub_;
   ImageRotator image_rotator_;
   ianvs::ImageSubscription sub_;
 };
 
+void declare_config(SegmentationNode::Config::Status& config) {
+  using namespace config;
+  name("SegmentationNode::Config");
+  field(config.nickname, "nickname");
+  field(config.rate_window_size, "rate_window_size");
+  field(config.period_s, "period_s");
+  field(config.max_heartbeat_s, "max_heartbeat_s");
+
+  checkCondition(!config.nickname.empty(), "nickname is empty");
+  check(config.rate_window_size, GT, 0, "rate_window_size");
+  check(config.period_s, GT, 0.0, "period_s");
+  check(config.max_heartbeat_s, GT, 0.0, "max_heartbeat_s");
+}
+
 void declare_config(SegmentationNode::Config& config) {
   using namespace config;
   name("SegmentationNode::Config");
@@ -89,6 +124,7 @@ void declare_config(SegmentationNode::Config& config) {
   field(config.image_rotator, "image_rotator");
   field(config.show_config, "show_config");
   field(config.show_output_config, "show_output_config");
+  field(config.status, "status");
 }
 
 SegmentationNode::SegmentationNode(const rclcpp::NodeOptions& options)
@@ -126,6 +162,12 @@ SegmentationNode::SegmentationNode(const rclcpp::NodeOptions& options)
 
   sub_.registerCallback(&ImageWorker::addMessage, worker_.get());
   sub_.subscribe("color/image_raw");
+
+  const auto period_ms = std::chrono::duration_cast<std::chrono::milliseconds>(
+      std::chrono::duration<double>(config.status.period_s));
+  status_pub_ = create_publisher<std_msgs::msg::String>("~/status", rclcpp::QoS(1));
+  timer_ =
+      create_wall_timer(period_ms, std::bind(&SegmentationNode::publishStatus, this));
 }
 
 SegmentationNode::~SegmentationNode() {
@@ -140,6 +182,7 @@ void SegmentationNode::runSegmentation(const Image::ConstSharedPtr& msg) {
     img_ptr = cv_bridge::toCvShare(msg, "rgb8");
   } catch (const cv_bridge::Exception& e) {
     SLOG(ERROR) << "cv_bridge exception: " << e.what();
+    recordStatus(0.0, "Conversion error: " + std::string(e.what()));
     return;
   }
 
@@ -148,15 +191,80 @@ void SegmentationNode::runSegmentation(const Image::ConstSharedPtr& msg) {
               << " is right type? "
               << (img_ptr->image.type() == CV_8UC3 ? "yes" : "no");
 
+  const auto start = std::chrono::steady_clock::now();
   const auto rotated = image_rotator_.rotate(img_ptr->image);
   const auto result = segmenter_->infer(rotated);
   if (!result) {
     SLOG(ERROR) << "failed to run inference!";
+    recordStatus(0.0, "Failed to run inference");
     return;
   }
 
   const auto derotated = image_rotator_.derotate(result.labels);
   output_pub_.publish(img_ptr->header, derotated, img_ptr->image);
+  const auto end = std::chrono::steady_clock::now();
+
+  const auto elapsed =
+      std::chrono::duration_cast<std::chrono::duration<double>>(end - start);
+  recordStatus(elapsed.count());
+}
+
+void SegmentationNode::recordStatus(double elapsed_s, const std::string& error) {
+  std::lock_guard<std::mutex> lock(status_mutex_);
+  last_call_ = now();
+  last_status_ = error;
+  if (!error.empty()) {
+    return;
+  }
+
+  elapsed_samples_s_.push_back(elapsed_s);
+  if (elapsed_samples_s_.size() > config.status.rate_window_size) {
+    elapsed_samples_s_.pop_front();
+  }
+}
+
+void SegmentationNode::publishStatus() {
+  std::lock_guard<std::mutex> lock(status_mutex_);
+  std::chrono::nanoseconds curr_time_ns(now().nanoseconds());
+
+  nlohmann::json record;
+  record["nickname"] = config.status.nickname;
+  record["node_name"] = get_fully_qualified_name();
+  if (!last_call_) {
+    record["status"] = "WARNING";
+    record["note"] = "Waiting for input";
+  } else {
+    const auto diff = now() - *last_call_;
+    if (diff.seconds() > config.status.max_heartbeat_s) {
+      record["status"] = "ERROR";
+      std::stringstream ss;
+      ss << "No input processed in " << diff.seconds() << " s";
+      record["note"] = ss.str();
+    } else if (!last_status_.empty()) {
+      record["status"] = "ERROR";
+      record["note"] = last_status_;
+    } else {
+      double average_elapsed_s = 0.0;
+      for (const auto sample : elapsed_samples_s_) {
+        average_elapsed_s += sample;
+      }
+      if (elapsed_samples_s_.size()) {
+        average_elapsed_s /= elapsed_samples_s_.size();
+      }
+
+      record["status"] = "NOMINAL";
+      std::stringstream ss;
+      ss << "Average elapsed time: " << average_elapsed_s << " s";
+      record["note"] = ss.str();
+    }
+  }
+
+  std::stringstream ss;
+  ss << record;
+
+  auto msg = std::make_unique<std_msgs::msg::String>();
+  msg->data = ss.str();
+  status_pub_->publish(std::move(msg));
 }
 
 }  // namespace semantic_inference