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