Miekale mit

.gitignore

@@ -1,5 +1,6 @@
 .env
 __pycache__
 .vscode/
+cursor/
 
 watod-config.local.sh

autonomy/interfacing/aggregator/CMakeLists.txt

@@ -0,0 +1,76 @@
+cmake_minimum_required(VERSION 3.10)
+project(aggregator)
+
+# Set compiler to use C++ 17 standard
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Search for dependencies required for building this package
+find_package(ament_cmake REQUIRED) # ROS2 build tool
+find_package(rclcpp REQUIRED)      # ROS2 C++ package
+find_package(sample_msgs REQUIRED) # Custom package containing ROS2 messages
+
+# Compiles source files into a library
+# A library is not executed, instead other executables can link
+# against it to access defined methods and classes.
+# We build a library so that the methods defined can be used by
+# both the unit test and ROS2 node executables.
+add_library(aggregator_lib
+  src/aggregator_core.cpp)
+# Indicate to compiler where to search for header files
+target_include_directories(aggregator_lib
+  PUBLIC include)
+# Add ROS2 dependencies required by package
+ament_target_dependencies(aggregator_lib rclcpp sample_msgs)
+
+# By default tests are built. This can be overridden by modifying the
+# colcon build command to include the -DBUILD_TESTING=OFF flag.
+option(BUILD_TESTING "Build tests" ON)
+if(BUILD_TESTING)
+  # Search for dependencies required for building tests + linting
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_lint_common REQUIRED)
+
+  # Remove the default C++ and copyright linter
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_cpplint
+    ament_cmake_copyright)
+
+  # Reinstall cpplint ignoring copyright errors
+  ament_cpplint(FILTERS "-legal/copyright")
+
+  ament_lint_auto_find_test_dependencies()
+
+  # Create test executable from source files
+  ament_add_gtest(aggregator_test test/aggregator_test.cpp)
+  # Link to the previously built library to access Aggregator classes and methods
+  target_link_libraries(aggregator_test aggregator_lib)
+
+  # Copy executable to installation location
+  install(TARGETS
+    aggregator_test
+    DESTINATION lib/${PROJECT_NAME})
+endif()
+
+# Create ROS2 node executable from source files
+add_executable(aggregator_node src/aggregator_node.cpp)
+# Link to the previously built library to access Aggregator classes and methods
+target_link_libraries(aggregator_node aggregator_lib)
+
+# Copy executable to installation location
+install(TARGETS
+  aggregator_node
+  DESTINATION lib/${PROJECT_NAME})
+
+# Copy launch files to installation location
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

autonomy/interfacing/aggregator/include/aggregator_core.hpp

@@ -0,0 +1,71 @@
+#ifndef AGGREGATOR_CORE_HPP_
+#define AGGREGATOR_CORE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "sample_msgs/msg/filtered_array.hpp"
+#include "sample_msgs/msg/unfiltered.hpp"
+
+namespace samples {
+
+/**
+ * Implementation of the internal logic used by the Aggregator Node to
+ * calculate the operating frequency of topics.
+ */
+class AggregatorCore {
+public:
+  /**
+   * Aggregator constructor.
+   *
+   * @param timestamp the Unix timestamp https://en.wikipedia.org/wiki/Unix_time
+   */
+  explicit AggregatorCore(int64_t timestamp);
+
+  /**
+   * Calculates the operating frequency on the "unfiltered" topic. Handles
+   * invalid timestamps and division by zero by returning zero.
+   *
+   * @returns frequency of messages on "unfiltered" topic
+   */
+  double raw_frequency() const;
+  /**
+   * Calculates the operating frequency on the "filtered" topic. Handles
+   * invalid timestamps and division by zero by returning zero.
+   *
+   * @returns frequency of messages on "filtered" topic
+   */
+  double filtered_frequency() const;
+
+  /**
+   * Used to keep track of latest timestamp and number of messages received
+   * over the "unfiltered" topic. Should be called before raw_frequency().
+   *
+   * @param msg
+   */
+  void add_raw_msg(const sample_msgs::msg::Unfiltered::SharedPtr msg);
+  /**
+   * Used to keep track of latest timestamp and number of messages received
+   * over the "filtered" topic. Should be called before filtered_frequency().
+   *
+   * @param msg
+   */
+  void add_filtered_msg(const sample_msgs::msg::FilteredArray::SharedPtr msg);
+
+private:
+  // Number of message received on "unfiltered" and "filtered" topics.
+  int raw_msg_count_;
+  int filtered_msg_count_;
+
+  // Unix timestamp used to determine the amount of time that has passed
+  // since the beginning of the program.
+  int64_t start_;
+
+  // Unix timestamps for last time a message was received on the "unfiltered"
+  // and "filtered" topics.
+  int64_t latest_raw_time_;
+  int64_t latest_filtered_time_;
+};
+
+} // namespace samples
+
+#endif // AGGREGATOR_CORE_HPP_

autonomy/interfacing/aggregator/include/aggregator_node.hpp

@@ -0,0 +1,56 @@
+#ifndef AGGREGATOR_NODE_HPP_
+#define AGGREGATOR_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "sample_msgs/msg/filtered_array.hpp"
+#include "sample_msgs/msg/unfiltered.hpp"
+
+#include "aggregator_core.hpp"
+
+/**
+ * Implementation of a ROS2 node that listens to the "unfiltered" and "filtered"
+ * topics and echoes the operating frequency of the topic to the console.
+ */
+class AggregatorNode : public rclcpp::Node {
+public:
+  // Configure pubsub nodes to keep last 20 messages.
+  // https://docs.ros.org/en/foxy/Concepts/About-Quality-of-Service-Settings.html
+  static constexpr int ADVERTISING_FREQ = 20;
+
+  /**
+   * Aggregator node constructor.
+   */
+  AggregatorNode();
+
+private:
+  /**
+   * A ROS2 subscription node callback used to unpack raw data from the
+   * "unfiltered" topic and echo the operating frequency of the topic to the
+   * console.
+   *
+   * @param msg a raw message from the "unfiltered" topic
+   */
+  void unfiltered_callback(const sample_msgs::msg::Unfiltered::SharedPtr msg);
+
+  /**
+   * A ROS2 subscription node callback used to unpack processed data from the
+   * "filtered" topic and echo the operating frequency of the topic to the
+   * console.
+   *
+   * @param msg a processed message from the "filtered" topic
+   */
+  void filtered_callback(const sample_msgs::msg::FilteredArray::SharedPtr msg);
+
+  // ROS2 subscriber listening to the unfiltered topic.
+  rclcpp::Subscription<sample_msgs::msg::Unfiltered>::SharedPtr raw_sub_;
+
+  // ROS2 subscriber listening to the filtered topic.
+  rclcpp::Subscription<sample_msgs::msg::FilteredArray>::SharedPtr
+      filtered_sub_;
+
+  // Object containing methods to determine the operating frequency on topics.
+  samples::AggregatorCore aggregator_;
+};
+
+#endif // AGGREGATOR_NODE_HPP_

autonomy/interfacing/aggregator/launch/aggregator.launch.py

@@ -0,0 +1,14 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Launch aggregator node."""
+    aggregator_node = Node(
+        package='aggregator',
+        executable='aggregator_node',
+    )
+
+    return LaunchDescription([
+        aggregator_node
+    ])

autonomy/interfacing/aggregator/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>aggregator</name>
+  <version>0.0.0</version>
+  <description>A sample ROS package for pubsub communication</description>
+
+  <maintainer email="cmahalingam@watonomous.ca">Cheran Mahalingam</maintainer>
+  <license>Apache2.0</license>
+
+  <!--https://www.ros.org/reps/rep-0149.html#dependency-tags-->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>sample_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <!--https://www.ros.org/reps/rep-0149.html#export-->
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

autonomy/interfacing/aggregator/src/aggregator_core.cpp

@@ -0,0 +1,42 @@
+#include <algorithm>
+
+#include "aggregator_core.hpp"
+
+namespace samples {
+
+AggregatorCore::AggregatorCore(int64_t timestamp)
+    : raw_msg_count_(0), filtered_msg_count_(0), start_(timestamp),
+      latest_raw_time_(-1), latest_filtered_time_(-1) {}
+
+double AggregatorCore::raw_frequency() const {
+  if (latest_raw_time_ <= start_) {
+    return 0.0;
+  }
+  return static_cast<double>(raw_msg_count_) / (latest_raw_time_ - start_);
+}
+
+double AggregatorCore::filtered_frequency() const {
+  if (latest_filtered_time_ <= start_) {
+    return 0.0;
+  }
+  return static_cast<double>(filtered_msg_count_) /
+         (latest_filtered_time_ - start_);
+}
+
+void AggregatorCore::add_raw_msg(
+    const sample_msgs::msg::Unfiltered::SharedPtr msg) {
+  latest_raw_time_ =
+      std::max(static_cast<int64_t>(msg->timestamp), latest_raw_time_);
+  raw_msg_count_++;
+}
+
+void AggregatorCore::add_filtered_msg(
+    const sample_msgs::msg::FilteredArray::SharedPtr msg) {
+  for (auto filtered_msg : msg->packets) {
+    latest_filtered_time_ = std::max(
+        static_cast<int64_t>(filtered_msg.timestamp), latest_filtered_time_);
+  }
+  filtered_msg_count_++;
+}
+
+} // namespace samples

autonomy/interfacing/aggregator/src/aggregator_node.cpp

@@ -0,0 +1,41 @@
+#include <chrono>
+#include <memory>
+
+#include "aggregator_node.hpp"
+
+AggregatorNode::AggregatorNode()
+    : Node("aggregator"),
+      aggregator_(samples::AggregatorCore(
+          std::chrono::duration_cast<std::chrono::milliseconds>(
+              std::chrono::system_clock::now().time_since_epoch())
+              .count())) {
+  raw_sub_ = this->create_subscription<sample_msgs::msg::Unfiltered>(
+      "/unfiltered_topic", ADVERTISING_FREQ,
+      std::bind(&AggregatorNode::unfiltered_callback, this,
+                std::placeholders::_1));
+  filtered_sub_ = this->create_subscription<sample_msgs::msg::FilteredArray>(
+      "/filtered_topic", ADVERTISING_FREQ,
+      std::bind(&AggregatorNode::filtered_callback, this,
+                std::placeholders::_1));
+}
+
+void AggregatorNode::unfiltered_callback(
+    const sample_msgs::msg::Unfiltered::SharedPtr msg) {
+  aggregator_.add_raw_msg(msg);
+  RCLCPP_INFO(this->get_logger(), "Raw Frequency(msg/s): %f",
+              aggregator_.raw_frequency() * 1000);
+}
+
+void AggregatorNode::filtered_callback(
+    const sample_msgs::msg::FilteredArray::SharedPtr msg) {
+  aggregator_.add_filtered_msg(msg);
+  RCLCPP_INFO(this->get_logger(), "Filtered Frequency(msg/s): %f",
+              aggregator_.filtered_frequency() * 1000);
+}
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AggregatorNode>());
+  rclcpp::shutdown();
+  return 0;
+}