feat: add eagleye_result_logger for CSV regression logging

.devcontainer/Dockerfile

@@ -0,0 +1,25 @@
+FROM ros:humble
+
+ARG USERNAME=ros
+ARG USER_UID=1000
+ARG USER_GID=${USER_UID}
+
+# Create non-root user
+RUN groupadd --gid ${USER_GID} ${USERNAME} \
+    && useradd --uid ${USER_UID} --gid ${USER_GID} -m ${USERNAME} \
+    && echo "${USERNAME} ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers
+
+# Install dependencies
+RUN apt-get update && apt-get install -y \
+    python3-vcstool \
+    python3-colcon-common-extensions \
+    libgeographic-dev \
+    geographiclib-tools \
+    geographiclib-doc \
+    ros-humble-rmw-cyclonedds-cpp \
+    && rm -rf /var/lib/apt/lists/*
+
+# Source ROS in bashrc
+RUN echo "source /opt/ros/humble/setup.bash" >> /home/${USERNAME}/.bashrc
+
+USER ${USERNAME}

.devcontainer/devcontainer.json

@@ -0,0 +1,36 @@
+{
+  "name": "eagleye ROS2 Humble",
+  "build": {
+    "dockerfile": "Dockerfile",
+    "context": ".."
+  },
+  "workspaceFolder": "/workspaces/${localWorkspaceFolderBasename}",
+  "workspaceMount": "source=${localWorkspaceFolder},target=/workspaces/${localWorkspaceFolderBasename},type=bind",
+  "remoteUser": "ros",
+  "postCreateCommand": "bash .devcontainer/setup.sh",
+  "customizations": {
+    "vscode": {
+      "extensions": [
+        "ms-vscode.cpptools",
+        "ms-vscode.cpptools-extension-pack",
+        "ms-vscode.cmake-tools",
+        "twxs.cmake",
+        "ms-python.python",
+        "ms-iot.vscode-ros"
+      ],
+      "settings": {
+        "terminal.integrated.defaultProfile.linux": "bash",
+        "colcon.workspaceFolder": "/home/ros/ws",
+        "ros.distro": "humble",
+        "C_Cpp.default.compileCommands": "/home/ros/ws/build/compile_commands.json"
+      }
+    }
+  },
+  "mounts": [
+    "source=/tmp/.X11-unix,target=/tmp/.X11-unix,type=bind,consistency=cached"
+  ],
+  "containerEnv": {
+    "ROS_DOMAIN_ID": "42",
+    "RMW_IMPLEMENTATION": "rmw_cyclonedds_cpp"
+  }
+}

.devcontainer/setup.sh

@@ -0,0 +1,28 @@
+#!/bin/bash
+set -e
+
+WORKSPACE=/home/ros/ws
+EAGLEYE_DIR=/workspaces/eagleye
+
+# Create ROS workspace
+mkdir -p ${WORKSPACE}/src
+
+# Symlink the eagleye repo into the workspace
+ln -sf ${EAGLEYE_DIR} ${WORKSPACE}/src/eagleye
+
+# Import dependency repos
+cd ${WORKSPACE}/src
+vcs import < ${EAGLEYE_DIR}/eagleye.repos
+
+# Install ROS dependencies
+cd ${WORKSPACE}
+sudo apt-get update
+rosdep update
+rosdep install --from-paths src --ignore-src -r -y
+
+# Build
+source /opt/ros/humble/setup.bash
+colcon build --symlink-install
+
+# Add workspace to bashrc
+echo "source ${WORKSPACE}/install/setup.bash" >> ~/.bashrc

.github/workflows/build.yml

@@ -2,84 +2,67 @@ name: build
 on: pull_request
 
 jobs:
-  humble_build:
-    runs-on:  ubuntu-22.04
+  build:
+    strategy:
+      matrix:
+        include:
+          - ros_distro: humble
+            os: ubuntu-22.04
+            geographiclib_pkg: libgeographic-dev geographiclib-tools geographiclib-doc
+          - ros_distro: jazzy
+            os: ubuntu-24.04
+            geographiclib_pkg: libgeographiclib-dev geographiclib-tools geographiclib-doc
 
-    container: ros:humble
-    services:
-      ros:
-        image: ros
+    runs-on: ${{ matrix.os }}
+    container: ros:${{ matrix.ros_distro }}
 
     defaults:
       run:
         shell: bash
 
     steps:
-      - name: checkout
-        uses: actions/checkout@v4
-      - name: rosdep update
+      - uses: actions/checkout@v4
+        with:
+          path: ros_ws/src/eagleye
+
+      - name: Install system dependencies
         run: |
           apt-get update
-          rosdep update
-      - name: Create Workspace
-        run: |
-          mkdir -p ~/eagleye/src/
-          cp -r `pwd` ~/eagleye/src/
-      - name: Clone rtklib_msgs
-        run: |
-          cd ~/eagleye/src/
-          git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0
-      - name: Clone llh_converter
-        run: |
-          cd ~/eagleye/src/
-          git clone https://github.com/MapIV/llh_converter.git -b ros2
-      - name: Install GeographicLib
-        run: |
-          apt-get install -y libgeographic-dev geographiclib-tools geographiclib-doc
-      - name: Build
-        run: |
-          cd ~/eagleye/
-          source /opt/ros/humble/setup.bash
-          rosdep install --from-paths src --ignore-src -r -y
-          colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests
+          apt-get install -y --no-install-recommends \
+            python3-vcstool \
+            ${{ matrix.geographiclib_pkg }}
 
-  jazzy_build:
-    runs-on: ubuntu-24.04
+      - name: Import dependency repos
+        run: |
+          cd ros_ws/src
+          vcs import < eagleye/eagleye.repos
 
-    container: ros:jazzy
-    services:
-      ros:
-        image: ros
+      - name: Cache rosdep
+        uses: actions/cache@v4
+        with:
+          path: ~/.ros/rosdep
+          key: rosdep-${{ matrix.ros_distro }}-${{ hashFiles('ros_ws/src/**/package.xml') }}
+          restore-keys: rosdep-${{ matrix.ros_distro }}-
 
-    defaults:
-      run:
-        shell: bash
-
-    steps:
-      - name: checkout
-        uses: actions/checkout@v4
-      - name: rosdep update
+      - name: rosdep install
         run: |
-          apt-get update
           rosdep update
-      - name: Create Workspace
-        run: |
-          mkdir -p ~/eagleye/src/
-          cp -r "$(pwd)" ~/eagleye/src/
-      - name: Clone rtklib_msgs
-        run: |
-          cd ~/eagleye/src/
-          git clone https://github.com/MapIV/rtklib_ros_bridge.git -b ros2-v0.1.0
-      - name: Clone llh_converter
-        run: |
-          cd ~/eagleye/src/
-          git clone https://github.com/MapIV/llh_converter.git -b ros2
-      - name: Install GeographicLib
-        run: |
-          apt-get install -y libgeographiclib-dev geographiclib-tools geographiclib-doc
+          rosdep install --from-paths ros_ws/src --ignore-src -r -y
+
+      - name: Cache colcon build
+        uses: actions/cache@v4
+        with:
+          path: |
+            ros_ws/build
+            ros_ws/install
+          key: colcon-${{ matrix.ros_distro }}-${{ hashFiles('ros_ws/src/**/*.cpp', 'ros_ws/src/**/*.hpp', 'ros_ws/src/**/CMakeLists.txt', 'ros_ws/src/**/package.xml') }}
+          restore-keys: colcon-${{ matrix.ros_distro }}-
+
       - name: Build
         run: |
-          cd ~/eagleye/
-          source /opt/ros/jazzy/setup.bash
-          rosdep install --from-paths src --ignore-src -r -y
-          colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --catkin-skip-building-tests
+          source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+          cd ros_ws
+          colcon build \
+            --cmake-args -DCMAKE_BUILD_TYPE=Release \
+            --catkin-skip-building-tests \
+            --parallel-workers $(nproc)

eagleye_rt/CMakeLists.txt

@@ -10,7 +10,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-if($ENV{ROS_DISTRO} STREQUAL "galactic")
+if("$ENV{ROS_DISTRO}" STREQUAL "galactic")
   add_definitions(-DROS_DISTRO_GALACTIC)
 endif()
 

eagleye_rt/src/angular_velocity_offset_stop_node.cpp

@@ -32,71 +32,83 @@
 #include "eagleye_coordinate/eagleye_coordinate.hpp"
 #include "eagleye_navigation/eagleye_navigation.hpp"
 
-static geometry_msgs::msg::TwistStamped velocity;
-rclcpp::Publisher<eagleye_msgs::msg::AngularVelocityOffset>::SharedPtr pub;
-static eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop;
-static sensor_msgs::msg::Imu imu;
+class AngularVelocityOffsetStopNode : public rclcpp::Node
+{
+public:
+  AngularVelocityOffsetStopNode() : Node("eagleye_angular_velocity_offset_stop")
+  {
+    std::string subscribe_twist_topic_name = "vehicle/twist";
 
-struct AngularVelocityOffsetStopParameter angular_velocity_offset_stop_parameter;
-struct AngularVelocityOffsetStopStatus angular_velocity_offset_stop_status;
+    std::string yaml_file;
+    this->declare_parameter("yaml_file", yaml_file);
+    this->get_parameter("yaml_file", yaml_file);
+    std::cout << "yaml_file: " << yaml_file << std::endl;
 
-void velocity_callback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
-{
-  velocity = *msg;
-}
+    try
+    {
+      YAML::Node conf = YAML::LoadFile(yaml_file);
 
-void imu_callback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
-{
-  imu.header = msg->header;
-  imu.orientation = msg->orientation;
-  imu.orientation_covariance = msg->orientation_covariance;
-  imu.angular_velocity = msg->angular_velocity;
-  imu.angular_velocity_covariance = msg->angular_velocity_covariance;
-  imu.linear_acceleration = msg->linear_acceleration;
-  imu.linear_acceleration_covariance = msg->linear_acceleration_covariance;
-  angular_velocity_offset_stop.header = msg->header;
-  angular_velocity_offset_stop_estimate(velocity, imu, angular_velocity_offset_stop_parameter, &angular_velocity_offset_stop_status, &angular_velocity_offset_stop);
-  pub->publish(angular_velocity_offset_stop);
-}
+      angular_velocity_offset_stop_parameter_.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as<double>();
+      angular_velocity_offset_stop_parameter_.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as<double>();
+      angular_velocity_offset_stop_parameter_.estimated_interval = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["estimated_interval"].as<double>();
+      angular_velocity_offset_stop_parameter_.outlier_threshold = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["outlier_threshold"].as<double>();
 
-int main(int argc, char** argv)
-{
-  rclcpp::init(argc, argv);
-  auto node = rclcpp::Node::make_shared("eagleye_angular_velocity_offset_stop");
+      std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl;
+      std::cout << "imu_rate " << angular_velocity_offset_stop_parameter_.imu_rate << std::endl;
+      std::cout << "stop_judgment_threshold " << angular_velocity_offset_stop_parameter_.stop_judgment_threshold << std::endl;
+      std::cout << "estimated_minimum_interval " << angular_velocity_offset_stop_parameter_.estimated_interval << std::endl;
+      std::cout << "outlier_threshold " << angular_velocity_offset_stop_parameter_.outlier_threshold << std::endl;
+    }
+    catch (YAML::Exception& e)
+    {
+      std::cerr << "\033[1;31mangular_velocity_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl;
+      exit(3);
+    }
 
-  std::string subscribe_twist_topic_name = "vehicle/twist";
+    sub1_ = this->create_subscription<geometry_msgs::msg::TwistStamped>(
+      subscribe_twist_topic_name, 1000,
+      std::bind(&AngularVelocityOffsetStopNode::velocityCallback, this, std::placeholders::_1));
+    sub2_ = this->create_subscription<sensor_msgs::msg::Imu>(
+      "imu/data_tf_converted", 1000,
+      std::bind(&AngularVelocityOffsetStopNode::imuCallback, this, std::placeholders::_1));
+    pub_ = this->create_publisher<eagleye_msgs::msg::AngularVelocityOffset>("angular_velocity_offset_stop", 1000);
+  }
 
-  std::string yaml_file;
-  node->declare_parameter("yaml_file",yaml_file);
-  node->get_parameter("yaml_file",yaml_file);
-  std::cout << "yaml_file: " << yaml_file << std::endl;
+private:
+  geometry_msgs::msg::TwistStamped velocity_;
+  eagleye_msgs::msg::AngularVelocityOffset angular_velocity_offset_stop_;
+  sensor_msgs::msg::Imu imu_;
 
- try
-  {
-    YAML::Node conf = YAML::LoadFile(yaml_file);
+  struct AngularVelocityOffsetStopParameter angular_velocity_offset_stop_parameter_;
+  struct AngularVelocityOffsetStopStatus angular_velocity_offset_stop_status_ = {};
 
-    angular_velocity_offset_stop_parameter.imu_rate = conf["/**"]["ros__parameters"]["common"]["imu_rate"].as<double>();
-    angular_velocity_offset_stop_parameter.stop_judgment_threshold = conf["/**"]["ros__parameters"]["common"]["stop_judgment_threshold"].as<double>();
-    angular_velocity_offset_stop_parameter.estimated_interval = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["estimated_interval"].as<double>();
-    angular_velocity_offset_stop_parameter.outlier_threshold = conf["/**"]["ros__parameters"]["angular_velocity_offset_stop"]["outlier_threshold"].as<double>();
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr sub1_;
+  rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr sub2_;
+  rclcpp::Publisher<eagleye_msgs::msg::AngularVelocityOffset>::SharedPtr pub_;
 
-    std::cout << "subscribe_twist_topic_name " << subscribe_twist_topic_name << std::endl;
-    std::cout << "imu_rate " << angular_velocity_offset_stop_parameter.imu_rate << std::endl;
-    std::cout << "stop_judgment_threshold " << angular_velocity_offset_stop_parameter.stop_judgment_threshold << std::endl;
-    std::cout << "estimated_minimum_interval " << angular_velocity_offset_stop_parameter.estimated_interval << std::endl;
-    std::cout << "outlier_threshold " << angular_velocity_offset_stop_parameter.outlier_threshold << std::endl;
-  }
-  catch (YAML::Exception& e)
+  void velocityCallback(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg)
   {
-    std::cerr << "\033[1;31mangular_velocity_offset_stop Node YAML Error: " << e.msg << "\033[0m" << std::endl;
-    exit(3);
+    velocity_ = *msg;
   }
 
-  auto sub1 = node->create_subscription<geometry_msgs::msg::TwistStamped>(subscribe_twist_topic_name, 1000, velocity_callback);
-  auto sub2 = node->create_subscription<sensor_msgs::msg::Imu>("imu/data_tf_converted", 1000, imu_callback);
-  pub = node->create_publisher<eagleye_msgs::msg::AngularVelocityOffset>("angular_velocity_offset_stop", 1000);
-
-  rclcpp::spin(node);
+  void imuCallback(const sensor_msgs::msg::Imu::ConstSharedPtr msg)
+  {
+    imu_.header = msg->header;
+    imu_.orientation = msg->orientation;
+    imu_.orientation_covariance = msg->orientation_covariance;
+    imu_.angular_velocity = msg->angular_velocity;
+    imu_.angular_velocity_covariance = msg->angular_velocity_covariance;
+    imu_.linear_acceleration = msg->linear_acceleration;
+    imu_.linear_acceleration_covariance = msg->linear_acceleration_covariance;
+    angular_velocity_offset_stop_.header = msg->header;
+    angular_velocity_offset_stop_estimate(velocity_, imu_, angular_velocity_offset_stop_parameter_, &angular_velocity_offset_stop_status_, &angular_velocity_offset_stop_);
+    pub_->publish(angular_velocity_offset_stop_);
+  }
+};
 
+int main(int argc, char** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<AngularVelocityOffsetStopNode>());
   return 0;
 }