From c4f86009ea03cea3c466d5edf6581e44cc0442c1 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Wed, 24 Sep 2025 18:47:46 -0400 Subject: [PATCH 1/4] (wip) start on status monitoring --- .../src/segmentation_nodelet.cpp | 46 +++++++++++++++++++ 1 file changed, 46 insertions(+) diff --git a/semantic_inference_ros/src/segmentation_nodelet.cpp b/semantic_inference_ros/src/segmentation_nodelet.cpp index 6e59906..ab0109d 100644 --- a/semantic_inference_ros/src/segmentation_nodelet.cpp +++ b/semantic_inference_ros/src/segmentation_nodelet.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -44,6 +45,7 @@ #include #include #include +#include #include "semantic_inference_ros/output_publisher.h" #include "semantic_inference_ros/ros_log_sink.h" @@ -63,6 +65,9 @@ class SegmentationNode : public rclcpp::Node { ImageRotator::Config image_rotator; bool show_config = true; bool show_output_config = false; + size_t rate_window_size = 10; + size_t status_period_s = 0.5; + double max_heartbeat_s = 10.0; } const config; explicit SegmentationNode(const rclcpp::NodeOptions& options); @@ -73,9 +78,16 @@ class SegmentationNode : public rclcpp::Node { private: void runSegmentation(const Image::ConstSharedPtr& msg); + void publishStatus(); + std::unique_ptr segmenter_; std::unique_ptr worker_; + rclcpp::TimerBase::SharedPtr timer_; + std::optional last_call_; + std::list period_samples_ns_; + rclcpp::Publisher::SharedPtr status_pub_; + OutputPublisher output_pub_; ImageRotator image_rotator_; ianvs::ImageSubscription sub_; @@ -89,6 +101,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.rate_window_size, "rate_window_size"); } SegmentationNode::SegmentationNode(const rclcpp::NodeOptions& options) @@ -126,6 +139,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::duration(config.status_period_s)); + status_pub_ = create_publisher("~/status", rclcpp::QoS(1)); + timer_ = + create_wall_timer(period_ms, std::bind(&SegmentationNode::publishStatus, this)); } SegmentationNode::~SegmentationNode() { @@ -159,6 +178,33 @@ void SegmentationNode::runSegmentation(const Image::ConstSharedPtr& msg) { output_pub_.publish(img_ptr->header, derotated, img_ptr->image); } +void SegmentationNode::publishStatus() { + std::chrono::nanoseconds curr_time_ns(now().nanoseconds()); + + std::stringstream ss; + ss << R"""({"nickname": "semantic_inference", "node_name": )""" + << get_fully_qualified_name() << ","; + + if (!last_call_) { + ss << R"""("status": "WARNING", "note": "waiting for input")"""; + } else { + const auto diff = now() - *last_call_; + if (diff.seconds() > config.max_heartbeat_s) { + ss << R"""("status": "ERROR", "note": "No input processed in )""" + << diff.seconds() << R"""( [s]")"""; + } else { + double average_elapsed_s = 0.0; + ss << R"""("status": "NOMINAL", "note": "Average elapsed time: ")""" + << average_elapsed_s << R"""( [s]")"""; + } + } + ss << "}"; + + auto msg = std::make_unique(); + msg->data = ss.str(); + status_pub_->publish(std::move(msg)); +} + } // namespace semantic_inference #include From d8ad2a9c3576190b4c946dfeaf19162a9fafede4 Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 27 Oct 2025 13:39:04 -0400 Subject: [PATCH 2/4] use json serialization and fix rate --- semantic_inference_ros/CMakeLists.txt | 2 + semantic_inference_ros/package.xml | 1 + .../src/segmentation_nodelet.cpp | 56 +++++++++++++------ 3 files changed, 43 insertions(+), 16 deletions(-) diff --git a/semantic_inference_ros/CMakeLists.txt b/semantic_inference_ros/CMakeLists.txt index 597115c..b52ee98 100644 --- a/semantic_inference_ros/CMakeLists.txt +++ b/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 diff --git a/semantic_inference_ros/package.xml b/semantic_inference_ros/package.xml index 59afb1e..2fba768 100644 --- a/semantic_inference_ros/package.xml +++ b/semantic_inference_ros/package.xml @@ -16,6 +16,7 @@ ianvs image_geometry message_filters + nlohmann-json-dev rclcpp rclcpp_components rosbag2_transport diff --git a/semantic_inference_ros/src/segmentation_nodelet.cpp b/semantic_inference_ros/src/segmentation_nodelet.cpp index ab0109d..0156670 100644 --- a/semantic_inference_ros/src/segmentation_nodelet.cpp +++ b/semantic_inference_ros/src/segmentation_nodelet.cpp @@ -43,6 +43,7 @@ #include #include +#include #include #include #include @@ -65,9 +66,12 @@ class SegmentationNode : public rclcpp::Node { ImageRotator::Config image_rotator; bool show_config = true; bool show_output_config = false; - size_t rate_window_size = 10; - size_t status_period_s = 0.5; - double max_heartbeat_s = 10.0; + 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); @@ -93,6 +97,20 @@ class SegmentationNode : public rclcpp::Node { 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"); @@ -101,7 +119,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.rate_window_size, "rate_window_size"); + field(config.status, "status"); } SegmentationNode::SegmentationNode(const rclcpp::NodeOptions& options) @@ -141,7 +159,7 @@ SegmentationNode::SegmentationNode(const rclcpp::NodeOptions& options) sub_.subscribe("color/image_raw"); const auto period_ms = std::chrono::duration_cast( - std::chrono::duration(config.status_period_s)); + std::chrono::duration(config.status.period_s)); status_pub_ = create_publisher("~/status", rclcpp::QoS(1)); timer_ = create_wall_timer(period_ms, std::bind(&SegmentationNode::publishStatus, this)); @@ -181,24 +199,30 @@ void SegmentationNode::runSegmentation(const Image::ConstSharedPtr& msg) { void SegmentationNode::publishStatus() { std::chrono::nanoseconds curr_time_ns(now().nanoseconds()); - std::stringstream ss; - ss << R"""({"nickname": "semantic_inference", "node_name": )""" - << get_fully_qualified_name() << ","; - + nlohmann::json record; + record["nickname"] = config.status.nickname; + record["node_name"] = get_fully_qualified_name(); if (!last_call_) { - ss << R"""("status": "WARNING", "note": "waiting for input")"""; + record["status"] = "WARNING"; + record["note"] = "waiting for input"; } else { const auto diff = now() - *last_call_; - if (diff.seconds() > config.max_heartbeat_s) { - ss << R"""("status": "ERROR", "note": "No input processed in )""" - << diff.seconds() << R"""( [s]")"""; + 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 { double average_elapsed_s = 0.0; - ss << R"""("status": "NOMINAL", "note": "Average elapsed time: ")""" - << average_elapsed_s << R"""( [s]")"""; + record["status"] = "NOMINAL"; + std::stringstream ss; + ss << "Average elapsed time: " << average_elapsed_s << " [s]"; + record["note"] = ss.str(); } } - ss << "}"; + + std::stringstream ss; + ss << record; auto msg = std::make_unique(); msg->data = ss.str(); From 90a0685795b36f73f92a3f43420ae3acb7fcde6b Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 27 Oct 2025 14:04:02 -0400 Subject: [PATCH 3/4] actually log inference information --- .../src/segmentation_nodelet.cpp | 47 +++++++++++++++++-- 1 file changed, 42 insertions(+), 5 deletions(-) diff --git a/semantic_inference_ros/src/segmentation_nodelet.cpp b/semantic_inference_ros/src/segmentation_nodelet.cpp index 0156670..4f3397c 100644 --- a/semantic_inference_ros/src/segmentation_nodelet.cpp +++ b/semantic_inference_ros/src/segmentation_nodelet.cpp @@ -82,14 +82,18 @@ 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_; std::unique_ptr worker_; - rclcpp::TimerBase::SharedPtr timer_; + std::mutex status_mutex_; std::optional last_call_; - std::list period_samples_ns_; + std::string last_status_; + std::list elapsed_samples_s_; + + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr status_pub_; OutputPublisher output_pub_; @@ -171,12 +175,27 @@ SegmentationNode::~SegmentationNode() { } } +void SegmentationNode::recordStatus(double elapsed_s, const std::string& error) { + std::lock_guard 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::runSegmentation(const Image::ConstSharedPtr& msg) { cv_bridge::CvImageConstPtr img_ptr; try { 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; } @@ -185,18 +204,26 @@ 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>(end - start); + recordStatus(elapsed.count()); } void SegmentationNode::publishStatus() { + std::lock_guard lock(status_mutex_); std::chrono::nanoseconds curr_time_ns(now().nanoseconds()); nlohmann::json record; @@ -204,19 +231,29 @@ void SegmentationNode::publishStatus() { record["node_name"] = get_fully_qualified_name(); if (!last_call_) { record["status"] = "WARNING"; - record["note"] = "waiting for input"; + 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]"; + 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]"; + ss << "Average elapsed time: " << average_elapsed_s << " s"; record["note"] = ss.str(); } } From 73ebf971c292c10ce50e72399ec4c97e360b9bee Mon Sep 17 00:00:00 2001 From: Nathan Hughes Date: Mon, 27 Oct 2025 14:06:31 -0400 Subject: [PATCH 4/4] minor style fixes --- .../src/segmentation_nodelet.cpp | 29 ++++++++++--------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/semantic_inference_ros/src/segmentation_nodelet.cpp b/semantic_inference_ros/src/segmentation_nodelet.cpp index 4f3397c..32539b6 100644 --- a/semantic_inference_ros/src/segmentation_nodelet.cpp +++ b/semantic_inference_ros/src/segmentation_nodelet.cpp @@ -83,6 +83,7 @@ class SegmentationNode : public rclcpp::Node { void runSegmentation(const Image::ConstSharedPtr& msg); void recordStatus(double elapsed_s, const std::string& error = ""); + void publishStatus(); std::unique_ptr segmenter_; @@ -175,20 +176,6 @@ SegmentationNode::~SegmentationNode() { } } -void SegmentationNode::recordStatus(double elapsed_s, const std::string& error) { - std::lock_guard 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::runSegmentation(const Image::ConstSharedPtr& msg) { cv_bridge::CvImageConstPtr img_ptr; try { @@ -222,6 +209,20 @@ void SegmentationNode::runSegmentation(const Image::ConstSharedPtr& msg) { recordStatus(elapsed.count()); } +void SegmentationNode::recordStatus(double elapsed_s, const std::string& error) { + std::lock_guard 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 lock(status_mutex_); std::chrono::nanoseconds curr_time_ns(now().nanoseconds());