diff --git a/dc_measurements/CMakeLists.txt b/dc_measurements/CMakeLists.txt index b3eef5e3..40644ddd 100644 --- a/dc_measurements/CMakeLists.txt +++ b/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) diff --git a/dc_measurements/include/dc_measurements/plugins/measurements/stops.hpp b/dc_measurements/include/dc_measurements/plugins/measurements/stops.hpp new file mode 100644 index 00000000..846fa7d0 --- /dev/null +++ b/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::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_ diff --git a/dc_measurements/measurement_plugin.xml b/dc_measurements/measurement_plugin.xml index cb16e31d..245818fd 100644 --- a/dc_measurements/measurement_plugin.xml +++ b/dc_measurements/measurement_plugin.xml @@ -95,6 +95,14 @@ + + + + dc_measurement_stops + + + + diff --git a/dc_measurements/plugins/measurements/json/stops.json b/dc_measurements/plugins/measurements/json/stops.json new file mode 100644 index 00000000..212a9690 --- /dev/null +++ b/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" +} diff --git a/dc_measurements/plugins/measurements/stops.cpp b/dc_measurements/plugins/measurements/stops.cpp new file mode 100644 index 00000000..3046ff44 --- /dev/null +++ b/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( + 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)