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 6e59906..32539b6 100644 --- a/semantic_inference_ros/src/segmentation_nodelet.cpp +++ b/semantic_inference_ros/src/segmentation_nodelet.cpp @@ -37,13 +37,16 @@ #include #include +#include #include #include #include #include +#include #include #include +#include #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_; std::unique_ptr worker_; + std::mutex status_mutex_; + std::optional last_call_; + std::string last_status_; + std::list elapsed_samples_s_; + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::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::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() { @@ -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>(end - start); + 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()); + + 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(); + msg->data = ss.str(); + status_pub_->publish(std::move(msg)); } } // namespace semantic_inference