feat: add measurement for stops

dc_measurements/CMakeLists.txt

@@ -140,6 +140,9 @@ list(APPEND dc_measurement_plugin_libs dc_speed_measurement)
 add_library(dc_storage_measurement SHARED plugins/measurements/storage.cpp)
 list(APPEND dc_measurement_plugin_libs dc_storage_measurement)
 
+add_library(dc_stops_measurement SHARED plugins/measurements/stops.cpp)
+list(APPEND dc_measurement_plugin_libs dc_stops_measurement)
+
 add_library(dc_string_stamped_measurement SHARED plugins/measurements/string_stamped.cpp)
 list(APPEND dc_measurement_plugin_libs dc_string_stamped_measurement)
 

dc_measurements/include/dc_measurements/plugins/measurements/stops.hpp

@@ -0,0 +1,47 @@
+
+#ifndef DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__STOPS_HPP_
+#define DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__STOPS_HPP_
+
+#include "dc_core/measurement.hpp"
+#include "dc_measurements/measurement.hpp"
+#include "dc_util/string_utils.hpp"
+#include "nav2_util/node_utils.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace dc_measurements
+{
+
+class Stops : public dc_measurements::Measurement
+{
+public:
+  Stops();
+  ~Stops() override;
+  dc_interfaces::msg::StringStamped collect() override;
+
+private:
+  double start_;
+  double end_;
+
+protected:
+  /**
+   * @brief Configuration of behavior action
+   */
+  void onConfigure() override;
+  void setValidationSchema() override;
+  void odomCb(const nav_msgs::msg::Odometry& msg);
+  double getFormattedTime();
+
+  std::string odom_topic_;
+  rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr subscription_;
+  float speed_threshold_;
+  int count_limit_;
+  int count_hysteresis_;
+  int moving_count_{ 0 };
+  bool active_;
+  dc_interfaces::msg::StringStamped last_data_;
+};
+
+}  // namespace dc_measurements
+
+#endif  // DC_MEASUREMENTS__PLUGINS__MEASUREMENTS__STOPS_HPP_

dc_measurements/measurement_plugin.xml

@@ -95,6 +95,14 @@
         </class>
     </library>
 
+    <library path="dc_stops_measurement">
+        <class name="dc_measurements/Stops" type="dc_measurements::Stops" base_class_type="dc_core::Measurement">
+            <description>
+                dc_measurement_stops
+            </description>
+        </class>
+    </library>
+
     <library path="dc_storage_measurement">
         <class name="dc_measurements/Storage" type="dc_measurements::Storage" base_class_type="dc_core::Measurement">
             <description>

dc_measurements/plugins/measurements/json/stops.json

@@ -0,0 +1,20 @@
+{
+  "$schema": "http://json-schema.org/draft-07/schema#",
+  "title": "Stops",
+  "description": "Robot stops",
+  "properties": {
+    "start_time": {
+      "description": "UTC Timestamp when it stopped",
+      "type": "number"
+    },
+    "end_time": {
+      "description": "UTC Timestamp when it started again",
+      "type": "number"
+    },
+    "duration": {
+      "description": "Stop duration",
+      "type": "number"
+    }
+  },
+  "type": "object"
+}

dc_measurements/plugins/measurements/stops.cpp

@@ -0,0 +1,109 @@
+#include "dc_measurements/plugins/measurements/stops.hpp"
+
+namespace dc_measurements
+{
+
+Stops::Stops() : dc_measurements::Measurement()
+{
+}
+
+Stops::~Stops() = default;
+
+void Stops::onConfigure()
+{
+  auto node = getNode();
+
+  nav2_util::declare_parameter_if_not_declared(node, measurement_name_ + ".odom_topic", rclcpp::ParameterValue("/odom"));
+  nav2_util::declare_parameter_if_not_declared(node, measurement_name_ + ".speed_threshold",
+                                               rclcpp::ParameterValue(0.2));
+  nav2_util::declare_parameter_if_not_declared(node, measurement_name_ + ".count_limit", rclcpp::ParameterValue(8));
+  nav2_util::declare_parameter_if_not_declared(node, measurement_name_ + ".count_hysteresis", rclcpp::ParameterValue(5));
+  node->get_parameter(measurement_name_ + ".odom_topic", odom_topic_);
+  node->get_parameter(measurement_name_ + ".speed_threshold", speed_threshold_);
+  node->get_parameter(measurement_name_ + ".count_limit", count_limit_);
+  node->get_parameter(measurement_name_ + ".count_hysteresis", count_hysteresis_);
+
+  start_ = getFormattedTime();
+  subscription_ = node->create_subscription<nav_msgs::msg::Odometry>(
+      odom_topic_, 10, std::bind(&Stops::odomCb, this, std::placeholders::_1));
+}
+
+double Stops::getFormattedTime()
+{
+  auto node = node_.lock();
+  auto now_nsecs = node->get_clock()->now().nanoseconds();
+  int seconds = now_nsecs / 1e9;
+  auto nseconds = int(now_nsecs - seconds * 1e9);
+  return std::stod(std::to_string(seconds) + std::string(".") + std::to_string(nseconds));
+}
+
+void Stops::odomCb(const nav_msgs::msg::Odometry& msg)
+{
+  float speed =
+      sqrt(msg.twist.twist.linear.x * msg.twist.twist.linear.x + msg.twist.twist.linear.y * msg.twist.twist.linear.y);
+
+  if (speed > speed_threshold_)
+  {
+    moving_count_ = moving_count_ + 1;
+  }
+  else
+  {
+    moving_count_ = moving_count_ - 1;
+  }
+
+  if (moving_count_ > count_limit_)
+  {
+    moving_count_ = count_limit_;
+  }
+  else if (moving_count_ < -count_limit_)
+  {
+    moving_count_ = -count_limit_;
+  }
+
+  auto formatted_time = getFormattedTime();
+  if (moving_count_ >= count_hysteresis_ && !active_)
+  {
+    RCLCPP_DEBUG(logger_, "Moving, was stopped");
+    active_ = true;
+    end_ = formatted_time;
+
+    json data_json;
+    data_json["start"] = start_;
+    data_json["end"] = end_;
+    data_json["duration"] = end_ - start_;
+
+    dc_interfaces::msg::StringStamped pub_msg;
+    pub_msg.group_key = group_key_;
+    pub_msg.data = data_json.dump(-1, ' ', true);
+    data_json.clear();
+    auto node = getNode();
+    pub_msg.header.stamp = node->get_clock()->now();
+    last_data_ = pub_msg;
+  }
+  else if (moving_count_ <= -count_hysteresis_ && active_)
+  {
+    active_ = false;
+    RCLCPP_DEBUG(logger_, "Stopped, was moving");
+    start_ = formatted_time;
+  }
+}
+
+void Stops::setValidationSchema()
+{
+  if (enable_validator_)
+  {
+    validateSchema("dc_measurements", "stops.json");
+  }
+}
+
+dc_interfaces::msg::StringStamped Stops::collect()
+{
+  dc_interfaces::msg::StringStamped msg = last_data_;
+  last_data_ = dc_interfaces::msg::StringStamped();
+  return msg;
+}
+
+}  // namespace dc_measurements
+
+#include "pluginlib/class_list_macros.hpp"
+PLUGINLIB_EXPORT_CLASS(dc_measurements::Stops, dc_core::Measurement)