Adding the ARSPointCloudFilter node in the Radar ROS2 Pipeline

dev_config.sh

@@ -63,6 +63,9 @@ SAMPLES_CPP_AGGREGATOR_IMAGE=${SAMPLES_CPP_AGGREGATOR_IMAGE:-"git.uwaterloo.ca:5
 SAMPLES_CPP_PRODUCER_IMAGE=${SAMPLES_CPP_PRODUCER_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/samples_cpp_producer"}
 SAMPLES_CPP_TRANSFORMER_IMAGE=${SAMPLES_CPP_TRANSFORMER_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/samples_cpp_transformer"}
 
+# Sensor Interfacing 
+CONTINENTAL_DRIVER_IMAGE=${CONTINENTAL_DRIVER_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/continental_driver"}
+
 # Infrastructure
 INFRASTRUCTURE_VIS_TOOLS_VNC_IMAGE=${INFRASTRUCTURE_VIS_TOOLS_VNC_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/infrastructure_vis_tools_vnc"}
 INFRASTRUCTURE_DATA_STREAM_IMAGE=${DATA_STREAM_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/infrastructure_data_stream"}
@@ -96,6 +99,9 @@ echo "SAMPLES_CPP_PRODUCER_IMAGE=$SAMPLES_CPP_PRODUCER_IMAGE" >> "$PROFILES_DIR/
 echo "SAMPLES_CPP_TRANSFORMER_IMAGE=$SAMPLES_CPP_TRANSFORMER_IMAGE" >> "$PROFILES_DIR/.env"
 echo "INFRASTRUCTURE_DATA_STREAM_IMAGE=$INFRASTRUCTURE_DATA_STREAM_IMAGE" >> "$PROFILES_DIR/.env"
 
+
+echo "CONTINENTAL_DRIVER_IMAGE=$CONTINENTAL_DRIVER_IMAGE" >> "$PROFILES_DIR/.env"
+
 echo "TAG=$TAG" >> "$PROFILES_DIR/.env"
 echo "TARGET_STAGE=$TARGET_STAGE" >> "$PROFILES_DIR/.env"
 echo "CACHE_FROM_TAG=$CACHE_FROM_TAG" >> "$PROFILES_DIR/.env"

docker/continental_driver/continental_driver.Dockerfile

@@ -0,0 +1,43 @@
+FROM ros:humble AS base
+
+RUN apt-get update && apt-get install -y curl && \
+    rm -rf /var/lib/apt/lists/*
+
+# Add a docker user so we that created files in the docker container are owned by a non-root user
+RUN addgroup --gid 1000 docker && \
+    adduser --uid 1000 --ingroup docker --home /home/docker --shell /bin/bash --disabled-password --gecos "" docker && \
+    echo "docker ALL=(ALL) NOPASSWD:ALL" >> /etc/sudoers.d/nopasswd
+
+# Remap the docker user and group to be the same uid and group as the host user.
+# Any created files by the docker container will be owned by the host user.
+RUN USER=docker && \
+    GROUP=docker && \
+    curl -SsL https://github.com/boxboat/fixuid/releases/download/v0.4/fixuid-0.4-linux-amd64.tar.gz | tar -C /usr/local/bin -xzf - && \
+    chown root:root /usr/local/bin/fixuid && \
+    chmod 4755 /usr/local/bin/fixuid && \
+    mkdir -p /etc/fixuid && \
+    printf "user: $USER\ngroup: $GROUP\npaths:\n  - /home/docker/" > /etc/fixuid/config.yml
+
+USER docker:docker
+
+RUN mkdir -p ~/ament_ws/src
+WORKDIR /home/docker/ament_ws/src
+
+COPY src/sensor_interfacing/continental_driver continental_driver
+COPY src/wato_msgs/radar_msgs radar_msgs
+
+WORKDIR /home/docker/ament_ws
+RUN . /opt/ros/$ROS_DISTRO/setup.sh && \
+    rosdep update && \
+    rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \
+    colcon build \
+        --cmake-args -DCMAKE_BUILD_TYPE=Release
+
+COPY docker/wato_ros_entrypoint.sh /home/docker/wato_ros_entrypoint.sh
+COPY docker/.bashrc /home/docker/.bashrc
+
+RUN sudo chmod +x ~/wato_ros_entrypoint.sh
+
+ENTRYPOINT ["/usr/local/bin/fixuid", "-q", "/home/docker/wato_ros_entrypoint.sh"]
+
+CMD ["tail", "-f", "/dev/null"]

profiles/docker-compose.sensors.yaml

@@ -0,0 +1,13 @@
+version: "3.8"
+services:
+  continental_driver:
+    build:
+      context: ..
+      dockerfile: docker/continental_driver/continental_driver.Dockerfile
+      cache_from: 
+        - "${CONTINENTAL_DRIVER_IMAGE:?}:${CACHE_FROM_TAG}"
+        - "${CONTINENTAL_DRIVER_IMAGE:?}:develop"
+    image: "${CONTINENTAL_DRIVER_IMAGE:?}:${TAG}"
+    user: ${FIXUID:?}:${FIXGID:?}
+    volumes:
+      - ../src/sensor_interfacing/continental_driver:/home/docker/ament_ws/src/continental_driver

src/sensor_interfacing/continental_driver/CMakeLists.txt

@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.10)
+project(continental_pointcloud_filter)
+
+# 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(radar_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(continentalpointcloudfilter_lib
+  src/continental_pointcloud_filter.cpp)
+# Indicate to compiler where to search for header files
+target_include_directories(continentalpointcloudfilter_lib
+  PUBLIC include)
+# Add ROS2 dependencies required by package
+ament_target_dependencies(continentalpointcloudfilter_lib rclcpp radar_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(continental_pointcloud_filter_test test/continental_pointcloud_filter_test.cpp)
+  # Link to the previously built library to access ContinentalPointCloudFilter classes and methods
+  target_link_libraries(continental_pointcloud_filter_test continentalpointcloudfilter_lib)
+
+  # Copy executable to installation location
+  install(TARGETS
+    continental_pointcloud_filter_test
+    DESTINATION lib/${PROJECT_NAME})
+endif()
+
+# Create ROS2 node executable from source files
+add_executable(continental_pointcloud_filter_node src/continental_pointcloud_filter_node.cpp)
+# Link to the previously built library to access Aggregator classes and methods
+target_link_libraries(continental_pointcloud_filter_node continentalpointcloudfilter_lib)
+
+# Copy executable to installation location
+install(TARGETS
+  continental_pointcloud_filter_node
+  DESTINATION lib/${PROJECT_NAME})
+
+# Copy launch files to installation location
+install(DIRECTORY
+  launch
+  config
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()

src/sensor_interfacing/continental_driver/config/continental_radar_params.yaml

@@ -0,0 +1,10 @@
+continental_point_cloud_filter:
+  ros__parameters:
+    filter_mode: "continental"
+    vrel_rad: -99999.99
+    el_ang: -99999.99
+    rcs0: -99999.99
+    snr: -99999.99
+    range: -99999.99
+    az_ang0: -99999.99
+    scan_mode: "near"

src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp

@@ -0,0 +1,143 @@
+#ifndef CONTINENTAL_POINTCLOUD_FILTER_HPP_
+#define CONTINENTAL_POINTCLOUD_FILTER_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+
+#include "radar_msgs/msg/radar_packet.hpp"
+#include "radar_msgs/msg/radar_detection.hpp"
+
+namespace filtering
+{
+
+/**
+* @brief The ContinentalPointCloudFilter is responsible for filtering, and publishing radar detections
+*    coming from the ARS430 radar sensor in the Radar ROS2 pipeline. These incoming
+*    detections are first organized and transformed into custom ROS messages/packets prior
+*    to being sent into this node. Note each message has detections of two types.
+*    These detections can be from a near scan or a far scan.
+*
+*    The internal logic for this node is divided into three sections or "modes".
+*
+*    Mode 1. The node recieves, filters and publishes only NEAR scan radar detections.
+*    These are packets with the event id 3, 4 or 5.
+*
+*    Mode 2. The node recieves, filters and publishes only FAR scan radar detections
+*    These are packets with the event id 1 or 2.
+*
+*    Mode 3. The node recieves, filters and publishes BOTH NEAR and FAR scan radar detections.
+*    In this case, it is much more complex to handle detections from both NEAR and FAR scan
+*    especially if they are also from two seperate scans. To solve this, the node uses the double
+*    buffer algorithm.
+*/
+class ContinentalPointCloudFilter
+{
+public:
+  ContinentalPointCloudFilter();
+  typedef struct
+  {
+    std::string scan_mode;
+    double vrel_rad_param;
+    double el_ang_param;
+    double rcs0_param;
+    double snr_param;
+    double range_param;
+    double az_ang0_param;
+  } filter_parameters;
+
+  enum scan_type
+  {
+    NEAR,
+    FAR
+  };
+
+  /**
+  * @brief The following is the internal logic of the common_scan_filter for near and far scan modes.
+  *    This filter works by checking the timestamps of incoming messages. If messages share the same
+  *    timestamp, this means that they are part of the same scan. Therefore, all the detections from
+  *    that incoming message are filtered and appended to a buffer packet. This process continues
+  *    until the packet count is at maxiumum capacity or when there is a timestamp change indicating
+  *    that a new scan has begun and the old buffer packet is ready to be published.
+  *    Special case to consider: If the scan starts in the middle such that we don't have a
+  *    complete number of packets (18 or 12) collected, then the code discards these detections
+  *    and starts from scratch with a new scan.
+  */
+  bool common_scan_filter(
+    const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_continental,
+    const filter_parameters & parameters,
+    radar_msgs::msg::RadarPacket & publish_packet);
+
+  /**
+  * @brief The following is the internal logic of the near_far_scan_filter
+  *    for the "nearfar" mode. This is when the double buffer algorithm is utilized.
+  *    Please refer to the figure in the WATO drive for more information
+  *    on the algorithm.
+  *    Special cases to consider:
+  *    1. If the scan started in the middle such that there is an incomplete packets of
+  *       near scans but a full number of far scans. This means that we will have
+  *       less than 30 packets when publishing. Therefore, the program discards this
+  *       incomplete packet and starts from scratch.
+  *    2. If the scan started in the middle, and we are only receiving far scan packets
+  *       for that scan (no near scan packets were collected). Then the program discards
+  *       each far scan packet that is recieved from that scan.
+  */
+  bool near_far_scan_filter(
+    const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_continental,
+    const filter_parameters & parameters,
+    radar_msgs::msg::RadarPacket & publish_packet);
+
+  /**
+  * @brief Checks the Event ID of a message and returns which scan it is (NEAR OR FAR).
+  */
+  scan_type check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_continental);
+
+  /**
+  * @brief Resets all scan states (timestamp, packet count, and publish status).
+  */
+  void reset_scan_states();
+
+private:
+  /**
+  * @brief Pointfilter() filters an incoming radar packet based on set thresholds.
+  */
+  radar_msgs::msg::RadarPacket point_filter(
+    const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_continental,
+    double snr_threshold,
+    double AzAng0_threshold,
+    double range_threshold,
+    double vrel_rad_threshold,
+    double el_ang_threshold,
+    double rcs_threshold);
+
+  /**
+  * @brief Variables below are used for all the filters (Common scan filter & NearFarScan Filter).
+  */
+  int near_scan_capacity_;
+  int far_scan_capacity_;
+  unsigned int default_timestamp_;
+  typedef struct
+  {
+    unsigned int timestamp_;
+    int packet_count_;
+    bool publish_status_;
+  } scan_params;
+
+  /**
+  * @brief Variables only used for common scan filter.
+  */
+  scan_params near_scan_single_;
+  scan_params far_scan_single_;
+  radar_msgs::msg::RadarPacket buffer_packet_;
+
+  /**
+  * @brief Variables only used for Near Far Scan Filter (Double Buffer Algorithm).
+  */
+  int buffer_index_;
+  std::array<radar_msgs::msg::RadarPacket, 2> near_far_buffer_packets_;
+  std::array<scan_params, 2> near_scan_;
+  std::array<scan_params, 2> far_scan_;
+};
+
+}  // namespace filtering
+
+#endif  // CONTINENTAL_POINTCLOUD_FILTER_HPP_

src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter_node.hpp

@@ -0,0 +1,54 @@
+#ifndef CONTINENTAL_POINTCLOUD_FILTER_NODE_HPP_
+#define CONTINENTAL_POINTCLOUD_FILTER_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "radar_msgs/msg/radar_packet.hpp"
+#include "radar_msgs/msg/radar_detection.hpp"
+
+#include "continental_pointcloud_filter.hpp"
+
+/**
+* @brief Implementation of a ROS2 Point Cloud Filter node that listens to "unfilteredRadarLeft"
+*    and "unfilteredRadarRight" topics and publishes filtered radar data to "processed" radar topic.
+*/
+class ContinentalPointCloudFilterNode : public rclcpp::Node
+{
+public:
+  ContinentalPointCloudFilterNode();
+
+private:
+  filtering::ContinentalPointCloudFilter::filter_parameters parameters;
+
+  /**
+  * @brief A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfilteredRadarRight"
+  *    topic.
+  *
+  * @param msg a raw message from the "unfilteredRadarRight" topic
+  */
+  void unfiltered_continental_radar_right_callback(
+    const radar_msgs::msg::RadarPacket::SharedPtr msg);
+
+  /**
+  * @brief A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfilteredRadarLeft"
+  *    topic.
+  *
+  * @param msg a raw message from the "unfilteredRadarLeft" topic
+  */
+  void unfiltered_continental_radar_left_callback(
+    const radar_msgs::msg::RadarPacket::SharedPtr msg);
+
+  // ROS2 Subscriber listening to "unfilteredRadarLeft" topic.
+  rclcpp::Subscription<radar_msgs::msg::RadarPacket>::SharedPtr raw_left_sub_;
+
+  // ROS2 Subscriber listening to "unfilteredRadarRight" topic.
+  rclcpp::Subscription<radar_msgs::msg::RadarPacket>::SharedPtr raw_right_sub_;
+
+  // ROS2 Publisher that sends filtered messages from left and right radar to the "processed" topic.
+  rclcpp::Publisher<radar_msgs::msg::RadarPacket>::SharedPtr left_right_pub_;
+
+  // An object containing methods for near and far scan filters.
+  filtering::ContinentalPointCloudFilter pointcloudfilter_;
+};
+
+#endif  // CONTINENTAL_POINTCLOUD_FILTER_NODE_HPP_

src/sensor_interfacing/continental_driver/launch/continental_pointcloud_filter.launch.py

@@ -0,0 +1,31 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch.conditions import LaunchConfigurationEquals
+
+
+def generate_launch_description():
+    """Launch ContinentalPointCloudFilter node."""
+    continental_pointcloud_filter_param = DeclareLaunchArgument(
+        'filter_mode', default_value='continental')
+
+    continental_pointcloud_filter_node = Node(
+        LaunchConfiguration('filter_mode'),
+        condition=LaunchConfigurationEquals('filter_mode', 'continental'),
+        package='continental_pointcloud_filter',
+        executable='continental_pointcloud_filter_node',
+        parameters=[
+            {'vrel_rad': -99999.99},
+            {'el_ang': -99999.99},
+            {'rcs0': -99999.99},
+            {'snr': -99999.99},
+            {'range': -99999.99},
+            {'az_ang0': -99999.99},
+            {'scan_mode': 'near'}
+        ]
+    )
+    return LaunchDescription([
+        continental_pointcloud_filter_param,
+        continental_pointcloud_filter_node
+    ])