diff --git a/dev_config.sh b/dev_config.sh index 7f3f91f0..2e00fdbc 100755 --- a/dev_config.sh +++ b/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" diff --git a/docker/continental_driver/continental_driver.Dockerfile b/docker/continental_driver/continental_driver.Dockerfile new file mode 100644 index 00000000..d9a18ac8 --- /dev/null +++ b/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"] diff --git a/profiles/docker-compose.sensors.yaml b/profiles/docker-compose.sensors.yaml new file mode 100644 index 00000000..3e439cd8 --- /dev/null +++ b/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 diff --git a/src/sensor_interfacing/continental_driver/CMakeLists.txt b/src/sensor_interfacing/continental_driver/CMakeLists.txt new file mode 100644 index 00000000..5f046e39 --- /dev/null +++ b/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() diff --git a/src/sensor_interfacing/continental_driver/config/continental_radar_params.yaml b/src/sensor_interfacing/continental_driver/config/continental_radar_params.yaml new file mode 100644 index 00000000..12baf8f3 --- /dev/null +++ b/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" diff --git a/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp b/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp new file mode 100644 index 00000000..487e2126 --- /dev/null +++ b/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 +#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 near_far_buffer_packets_; + std::array near_scan_; + std::array far_scan_; +}; + +} // namespace filtering + +#endif // CONTINENTAL_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter_node.hpp b/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter_node.hpp new file mode 100644 index 00000000..829e444a --- /dev/null +++ b/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::SharedPtr raw_left_sub_; + + // ROS2 Subscriber listening to "unfilteredRadarRight" topic. + rclcpp::Subscription::SharedPtr raw_right_sub_; + + // ROS2 Publisher that sends filtered messages from left and right radar to the "processed" topic. + rclcpp::Publisher::SharedPtr left_right_pub_; + + // An object containing methods for near and far scan filters. + filtering::ContinentalPointCloudFilter pointcloudfilter_; +}; + +#endif // CONTINENTAL_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/continental_driver/launch/continental_pointcloud_filter.launch.py b/src/sensor_interfacing/continental_driver/launch/continental_pointcloud_filter.launch.py new file mode 100755 index 00000000..557d7041 --- /dev/null +++ b/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 + ]) diff --git a/src/sensor_interfacing/continental_driver/package.xml b/src/sensor_interfacing/continental_driver/package.xml new file mode 100644 index 00000000..7e7633e8 --- /dev/null +++ b/src/sensor_interfacing/continental_driver/package.xml @@ -0,0 +1,23 @@ + + + continental_pointcloud_filter + 0.0.0 + A ROS package for filtering Continental and Carla Radar Data + + Rijin Muralidharan + TODO + + + ament_cmake + rclcpp + radar_msgs + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + + ament_cmake + + \ No newline at end of file diff --git a/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp new file mode 100644 index 00000000..040bf8fd --- /dev/null +++ b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp @@ -0,0 +1,222 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +#include "continental_pointcloud_filter_node.hpp" +#include "continental_pointcloud_filter.hpp" + +namespace filtering +{ + +ContinentalPointCloudFilter::ContinentalPointCloudFilter() +{ + near_scan_single_.timestamp_ = 0; + near_scan_single_.packet_count_ = 0; + + far_scan_single_.timestamp_ = 0; + far_scan_single_.packet_count_ = 0; + + near_scan_[0].timestamp_ = 0; + near_scan_[0].publish_status_ = false; + near_scan_[0].packet_count_ = 0; + + near_scan_[1].timestamp_ = 0; + near_scan_[1].publish_status_ = false; + near_scan_[1].packet_count_ = 0; + + far_scan_[0].timestamp_ = 0; + far_scan_[0].publish_status_ = false; + far_scan_[0].packet_count_ = 0; + + far_scan_[1].timestamp_ = 0; + far_scan_[1].publish_status_ = false; + far_scan_[1].packet_count_ = 0; + + buffer_index_ = 0; + default_timestamp_ = 0; + near_scan_capacity_ = 18; + far_scan_capacity_ = 12; +} + +radar_msgs::msg::RadarPacket ContinentalPointCloudFilter::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) +{ + radar_msgs::msg::RadarPacket filtered_continental; + for (auto detection : unfiltered_continental->detections) { + if (detection.snr < snr_threshold) { + continue; + } + if (detection.az_ang0 < AzAng0_threshold) { + continue; + } + if (detection.range < range_threshold) { + continue; + } + if (detection.vrel_rad < vrel_rad_threshold) { + continue; + } + if (detection.el_ang < el_ang_threshold) { + continue; + } + if (detection.rcs0 < rcs_threshold) { + continue; + } + filtered_continental.detections.push_back(detection); + } + return filtered_continental; +} + +ContinentalPointCloudFilter::scan_type ContinentalPointCloudFilter::check_scan_type( + const radar_msgs::msg::RadarPacket::SharedPtr msg) +{ + if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) { + return NEAR; + } else if (msg->event_id == 1 || msg->event_id == 2) { + return FAR; + } +} + +void ContinentalPointCloudFilter::reset_scan_states() +{ + near_scan_[buffer_index_].timestamp_ = 0; + near_scan_[buffer_index_].publish_status_ = false; + near_scan_[buffer_index_].packet_count_ = 0; + + far_scan_[buffer_index_].timestamp_ = 0; + far_scan_[buffer_index_].publish_status_ = false; + far_scan_[buffer_index_].packet_count_ = 0; +} + +bool ContinentalPointCloudFilter::common_scan_filter( + const radar_msgs::msg::RadarPacket::SharedPtr msg, + const filter_parameters & parameters, + radar_msgs::msg::RadarPacket & publish_packet) +{ + scan_type incoming_scan_msg = check_scan_type(msg); + + int scan_capacity = (incoming_scan_msg == NEAR) ? near_scan_capacity_ : far_scan_capacity_; + + auto scan = (incoming_scan_msg == NEAR) ? &near_scan_single_ : &far_scan_single_; + + if ((incoming_scan_msg == NEAR && parameters.scan_mode == "near") || + (incoming_scan_msg == FAR && parameters.scan_mode == "far")) + { + if (scan->timestamp_ == default_timestamp_) { + scan->timestamp_ = msg->timestamp; + } + + const radar_msgs::msg::RadarPacket filtered_packet = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + if (scan->packet_count_ != scan_capacity && msg->timestamp == scan->timestamp_) { + buffer_packet_.timestamp = scan->timestamp_; + buffer_packet_.detections.insert( + buffer_packet_.detections.end(), + filtered_packet.detections.begin(), + filtered_packet.detections.end()); + scan->packet_count_++; + + if (scan->packet_count_ == scan_capacity) { + publish_packet = buffer_packet_; + buffer_packet_.detections.clear(); + scan->packet_count_ = 0; + scan->timestamp_ = default_timestamp_; + return true; + } + } else { + // Special Case + if (scan->packet_count_ != scan_capacity) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), + "Packet is not full, size: %d! Packet Discarded.\n ", scan->packet_count_); + } + buffer_packet_ = filtered_packet; + scan->packet_count_ = 1; + scan->timestamp_ = msg->timestamp; + } + } + return false; +} + +bool ContinentalPointCloudFilter::near_far_scan_filter( + const radar_msgs::msg::RadarPacket::SharedPtr msg, + const filter_parameters & parameters, + radar_msgs::msg::RadarPacket & publish_packet) +{ + scan_type incoming_scan_msg = check_scan_type(msg); + + int scan_capacity = (incoming_scan_msg == NEAR) ? near_scan_capacity_ : far_scan_capacity_; + + auto scan = (incoming_scan_msg == NEAR) ? near_scan_.data() : far_scan_.data(); + + const radar_msgs::msg::RadarPacket filtered_packet = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + if (scan[buffer_index_].timestamp_ == default_timestamp_) { + scan[buffer_index_].timestamp_ = msg->timestamp; + } + + if (scan[buffer_index_].timestamp_ == msg->timestamp) { + // Special case + if (incoming_scan_msg == FAR && near_scan_[buffer_index_].packet_count_ == 0) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Discarding far scan packet. \n"); + scan[buffer_index_].timestamp_ = default_timestamp_; + return false; + } + near_far_buffer_packets_[buffer_index_].timestamp = scan[buffer_index_].timestamp_; + + near_far_buffer_packets_[buffer_index_].detections.insert( + near_far_buffer_packets_[buffer_index_].detections.end(), + filtered_packet.detections.begin(), + filtered_packet.detections.end()); + + scan[buffer_index_].packet_count_++; + } else { + if (scan[buffer_index_].packet_count_ != scan_capacity) { + RCLCPP_WARN( + rclcpp::get_logger("rclcpp"), "Packet is not full, Actual Size: %d! \n", + scan[buffer_index_].packet_count_); + } + + scan[buffer_index_].publish_status_ = true; + + near_far_buffer_packets_[1 - buffer_index_].timestamp = scan[1 - buffer_index_].timestamp_; + + near_far_buffer_packets_[1 - buffer_index_].detections.insert( + near_far_buffer_packets_[1 - buffer_index_].detections.end(), + filtered_packet.detections.begin(), + filtered_packet.detections.end()); + + scan[1 - buffer_index_].packet_count_++; + } + + if (scan[buffer_index_].packet_count_ == scan_capacity) { + scan[buffer_index_].publish_status_ = true; + } + + if (far_scan_[buffer_index_].publish_status_ == true) { + // Special Case + if (near_scan_[buffer_index_].packet_count_ != near_scan_capacity_ || + far_scan_[buffer_index_].packet_count_ != far_scan_capacity_) + { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Incomplete total packet count! Not 30.\n"); + near_far_buffer_packets_[buffer_index_].detections.clear(); + reset_scan_states(); + buffer_index_ = 1 - buffer_index_; + return false; + } + + publish_packet = near_far_buffer_packets_[buffer_index_]; + near_far_buffer_packets_[buffer_index_].detections.clear(); + reset_scan_states(); + buffer_index_ = 1 - buffer_index_; + return true; + } + return false; +} + +} // namespace filtering diff --git a/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter_node.cpp b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter_node.cpp new file mode 100644 index 00000000..fef93bf8 --- /dev/null +++ b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter_node.cpp @@ -0,0 +1,96 @@ +#include +#include + +#include "continental_pointcloud_filter_node.hpp" +#include "continental_pointcloud_filter.hpp" + +ContinentalPointCloudFilterNode::ContinentalPointCloudFilterNode() +: Node("continental_point_cloud_filter") +{ + /** + * @note Default values are already declared in yaml file. + */ + this->declare_parameter("filter_mode", "continental"); + this->declare_parameter("scan_mode", "near"); + this->declare_parameter("vrel_rad", 0); + this->declare_parameter("el_ang", 0); + this->declare_parameter("rcs0", 0); + this->declare_parameter("snr", 0); + this->declare_parameter("range", 0); + this->declare_parameter("az_ang0", 0); + + parameters.scan_mode = this->get_parameter("scan_mode").as_string(); + parameters.vrel_rad_param = this->get_parameter("vrel_rad").as_double(); + parameters.el_ang_param = this->get_parameter("el_ang").as_double(); + parameters.rcs0_param = this->get_parameter("rcs0").as_double(); + parameters.snr_param = this->get_parameter("snr").as_double(); + parameters.range_param = this->get_parameter("range").as_double(); + parameters.az_ang0_param = this->get_parameter("az_ang0").as_double(); + + raw_left_sub_ = this->create_subscription( + "unfilteredRadarLeft", + 1, std::bind( + &ContinentalPointCloudFilterNode::unfiltered_continental_radar_left_callback, + this, std::placeholders::_1)); + + raw_right_sub_ = this->create_subscription( + "unfilteredRadarRight", + 1, std::bind( + &ContinentalPointCloudFilterNode::unfiltered_continental_radar_right_callback, + this, std::placeholders::_1)); + + left_right_pub_ = this->create_publisher("processed", 20); +} + +void ContinentalPointCloudFilterNode::unfiltered_continental_radar_right_callback( + const radar_msgs::msg::RadarPacket::SharedPtr msg) +{ + /** + * @brief Depending on the scan mode ("near", "far", "nearfar"), + * send incoming unfiltered msgs to common_scan_filter or near_far_scan_filter. + * Append filtered detections to the buffer packets. + * Publish packet when ready to be published. + */ + if (parameters.scan_mode == "near" || parameters.scan_mode == "far") { + radar_msgs::msg::RadarPacket publish_packet; + + if (pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { + left_right_pub_->publish(publish_packet); + } + } else { + radar_msgs::msg::RadarPacket publish_packet_near_far; + + if (pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { + left_right_pub_->publish(publish_packet_near_far); + } + } +} + +/** +* @note Implementation below is the same as the right callback function. +*/ +void ContinentalPointCloudFilterNode::unfiltered_continental_radar_left_callback( + const radar_msgs::msg::RadarPacket::SharedPtr msg) +{ + if (parameters.scan_mode == "near" || parameters.scan_mode == "far") { + radar_msgs::msg::RadarPacket publish_packet; + + if (pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { + left_right_pub_->publish(publish_packet); + } + } else { + radar_msgs::msg::RadarPacket publish_packet_near_far; + + if (pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { + left_right_pub_->publish(publish_packet_near_far); + } + } +} + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/sensor_interfacing/continental_driver/test/continental_pointcloud_filter_test.cpp b/src/sensor_interfacing/continental_driver/test/continental_pointcloud_filter_test.cpp new file mode 100644 index 00000000..238f81e5 --- /dev/null +++ b/src/sensor_interfacing/continental_driver/test/continental_pointcloud_filter_test.cpp @@ -0,0 +1,636 @@ +#include +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +#include "continental_pointcloud_filter.hpp" + + +class ContinentalPointCloudFilterFixtureTest : public ::testing::Test +{ +public: + static constexpr double DEFAULT_FILTER_PARAM = -9999.99; + + void SetUp( + std::string scan_mode, double vrel_rad_param = DEFAULT_FILTER_PARAM, + double el_ang_param = DEFAULT_FILTER_PARAM, double rcs0_param = DEFAULT_FILTER_PARAM, + double snr_param = DEFAULT_FILTER_PARAM, double range_param = DEFAULT_FILTER_PARAM, + double az_ang0_param = DEFAULT_FILTER_PARAM) + { + parameters.scan_mode = scan_mode; + parameters.vrel_rad_param = vrel_rad_param; + parameters.el_ang_param = el_ang_param; + parameters.rcs0_param = rcs0_param; + parameters.snr_param = snr_param; + parameters.range_param = range_param; + parameters.az_ang0_param = az_ang0_param; + } + +protected: + filtering::ContinentalPointCloudFilter pointcloudfilter; + filtering::ContinentalPointCloudFilter::filter_parameters parameters; +}; + +// Common Scan Filter Test + +/** +* @brief Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR. +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, CheckScanType) +{ + // Near Scan message + auto msg_0 = std::make_shared(); + + msg_0->event_id = 3; + msg_0->timestamp = 2; + + // Far Scan message + auto msg_1 = std::make_shared(); + + msg_1->event_id = 1; + msg_1->timestamp = 2; + + auto scan_type_0 = pointcloudfilter.check_scan_type(msg_0); + auto scan_type_1 = pointcloudfilter.check_scan_type(msg_1); + + // Near = 0 and Far = 1 + EXPECT_EQ(0, scan_type_0); + EXPECT_EQ(1, scan_type_1); +} + +/** +* @brief Sends 18 near scan packets with the same timestamp and checks if it returns a +* complete packet ready to be published. +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearScanPackets) +{ + SetUp("near"); + + auto msg = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection; + + radar_msgs::msg::RadarPacket publish_packet; + + msg->event_id = 3; + msg->timestamp = 2; + + msg_detection.snr = 100.0; + + msg->detections.push_back(msg_detection); + + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); + } + + EXPECT_EQ(2, static_cast(publish_packet.timestamp)); + + for (int index = 0; index < 18; index++) { + EXPECT_EQ(100, publish_packet.detections[index].snr); + } +} + +/** +* @brief Sends 12 far scan packets with the same timestamp and checks if it returns a +* complete packet ready to be published. +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteFarScanPackets) +{ + SetUp("far"); + + // Far Scan Message + auto msg = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection; + + radar_msgs::msg::RadarPacket publish_packet; + + msg->event_id = 1; + msg->timestamp = 7; + + msg_detection.rcs0 = 5.0; + + msg->detections.push_back(msg_detection); + + for (int packet_size = 0; packet_size < 12; packet_size++) { + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); + } + + EXPECT_EQ(7, static_cast(publish_packet.timestamp)); + + for (int index = 0; index < 12; index++) { + EXPECT_EQ(5, publish_packet.detections[index].rcs0); + } +} + +/** +* @brief Sends 18 packets that are the same (timestamp 1), and 18 packets with a new timestamp +* (timestamp 2) and checks if it returns a complete packet ready to be published. +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) +{ + SetUp("near"); + + auto msg_0 = std::make_shared(); + auto msg_1 = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection; + + radar_msgs::msg::RadarPacket publish_packet_0; + radar_msgs::msg::RadarPacket publish_packet_1; + + // Packet Data (Scan 1) + msg_0->event_id = 3; + msg_0->timestamp = 7; + + msg_detection.rcs0 = 5.0; + + msg_0->detections.push_back(msg_detection); + + // Packet Data (Scan 2) + msg_1->event_id = 4; + msg_1->timestamp = 3; + + msg_detection.rcs0 = 20.0; + + msg_1->detections.push_back(msg_detection); + + // Scan 1 + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet_0); + } + + EXPECT_EQ(7, static_cast(publish_packet_0.timestamp)); + + for (int index = 0; index < 18; index++) { + EXPECT_EQ(5, publish_packet_0.detections[index].rcs0); + } + + // Scan 2 + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet_1); + } + + EXPECT_EQ(3, static_cast(publish_packet_1.timestamp)); + + for (int index = 0; index < 18; index++) { + EXPECT_EQ(20, publish_packet_1.detections[index].rcs0); + } +} + +/** +* @brief Sends incomplete packets (5 that are the same and +* 1 packet after that is different in timestamp). +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteScanPackets) +{ + SetUp("near"); + + // Near Scan Messages + auto msg_0 = std::make_shared(); + auto msg_1 = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection; + radar_msgs::msg::RadarPacket publish_packet; + + // Packet Data (Scan 1) + msg_0->event_id = 3; + msg_0->timestamp = 7; + + msg_detection.range = 120.0; + + msg_0->detections.push_back(msg_detection); + + // Packet Data (Scan 2) + msg_1->event_id = 4; + msg_1->timestamp = 3; + + msg_detection.range = 50.0; + + msg_1->detections.push_back(msg_detection); + + // Incomplete packet of near scans + for (int packet_size = 0; packet_size < 5; packet_size++) { + pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet); + } + + // Publish packet shouldn't be updated + EXPECT_EQ(0, static_cast(publish_packet.timestamp)); + + // Scan 2 data (sending a packet of a different timestamp) + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet); + } + + // Timestamp should be from msg_1 (msg_0 should be discarded) + EXPECT_EQ(3, static_cast(publish_packet.timestamp)); + + for (int index = 0; index < 18; index++) { + EXPECT_EQ(50, publish_packet.detections[index].range); + } +} + +/** +* @brief Sends far scan packets when in near scan mode and checks if it filters correctly. +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) +{ + SetUp("near"); + + // Near Scan Message + auto msg_0 = std::make_shared(); + + // Far Scan Message + auto msg_1 = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection; + radar_msgs::msg::RadarPacket publish_packet; + + msg_0->event_id = 3; + msg_0->timestamp = 7; + + msg_detection.range = 120.0; + + msg_0->detections.push_back(msg_detection); + + msg_1->event_id = 1; + msg_1->timestamp = 3; + + msg_detection.range = 50.0; + + msg_1->detections.push_back(msg_detection); + + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet); + pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet); + } + + EXPECT_EQ(7, static_cast(publish_packet.timestamp)); + + for (int index = 0; index < 18; index++) { + EXPECT_EQ(120, publish_packet.detections[index].range); + } +} + +/** +* @brief Checks if a packet with several detections is filtered out correctly. +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) +{ + SetUp("near", DEFAULT_FILTER_PARAM, DEFAULT_FILTER_PARAM, 70, 100, 40, DEFAULT_FILTER_PARAM); + + // Near Scan Message + auto msg = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection_0; + radar_msgs::msg::RadarDetection msg_detection_1; + radar_msgs::msg::RadarDetection msg_detection_2; + + radar_msgs::msg::RadarPacket publish_packet; + + // Packet Data + msg->event_id = 3; + msg->timestamp = 7; + + // Sending messages with no detections + for (int packet_size = 0; packet_size < 17; packet_size++) { + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); + } + + // detection snr < snr threshold + msg_detection_0.range = 120.0; + msg_detection_0.rcs0 = 75.0; + msg_detection_0.snr = 80.0; + + msg->detections.push_back(msg_detection_0); + + // Passes all the filter thresholds + msg_detection_1.range = 50.0; + msg_detection_1.rcs0 = 90.0; + msg_detection_1.snr = 200.0; + + msg->detections.push_back(msg_detection_1); + + // detection range < range threshold & detection snr < snr threshold + msg_detection_2.range = 10.0; + msg_detection_2.rcs0 = 90.0; + msg_detection_2.snr = 50.0; + + msg->detections.push_back(msg_detection_2); + + for (int packet_size = 0; packet_size < 1; packet_size++) { + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); + } + + EXPECT_EQ(7, static_cast(publish_packet.timestamp)); + + // Returns a published packet with the correct detection going through + EXPECT_EQ(1, static_cast(publish_packet.detections.size())); + EXPECT_EQ(50, static_cast(publish_packet.detections[0].range)); + EXPECT_EQ(90, static_cast(publish_packet.detections[0].rcs0)); + EXPECT_EQ(200, static_cast(publish_packet.detections[0].snr)); +} + +// Double Buffer Algorithm (Nearfar scan filter) Test + +/** +* @brief Sends 18 Near Scan Packets and 12 Far Scan Packets. Checks if it returns a +* complete packet ready to be published. +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) +{ + SetUp("nearfar"); + + auto msg_0 = std::make_shared(); + auto msg_1 = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection_0; + radar_msgs::msg::RadarDetection msg_detection_1; + + radar_msgs::msg::RadarPacket publish_packet_; + + // Near Packet Data + msg_0->event_id = 3; + msg_0->timestamp = 5; + + msg_detection_0.range = 80.0; + + msg_0->detections.push_back(msg_detection_0); + + // Far Packet Data + msg_1->event_id = 1; + msg_1->timestamp = 6; + + msg_detection_1.range = 90.0; + + msg_1->detections.push_back(msg_detection_1); + + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_); + } + + // Packet shouldn't be published at this point + EXPECT_EQ(0, static_cast(publish_packet_.timestamp)); + EXPECT_EQ(0, publish_packet_.detections.size()); + + for (int packet_size = 0; packet_size < 12; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_); + } + + EXPECT_EQ(6, static_cast(publish_packet_.timestamp)); + EXPECT_EQ(30, publish_packet_.detections.size()); + EXPECT_EQ(80, publish_packet_.detections[0].range); + EXPECT_EQ(90, publish_packet_.detections[19].range); +} + +/** +* @brief Sends 18 Near scan (Scan 1), 10 far scan (Scan 1), 3 Near scan (Scan 2), +* 2 far scan (Scan 1), 15 Near scan (Scan 2), 12 Far scan (Scan 2). +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendMultipleScanPackets) +{ + SetUp("nearfar"); + + auto msg_0 = std::make_shared(); + auto msg_1 = std::make_shared(); + auto msg_2 = std::make_shared(); + auto msg_3 = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection_0; + radar_msgs::msg::RadarDetection msg_detection_1; + + radar_msgs::msg::RadarPacket publish_packet_1; + radar_msgs::msg::RadarPacket publish_packet_2; + + // Near Packet Data (Scan 1) + msg_0->event_id = 3; + msg_0->timestamp = 5; + + msg_detection_0.range = 80.0; + + msg_0->detections.push_back(msg_detection_0); + + // Far Packet Data (Scan 1) + msg_1->event_id = 1; + msg_1->timestamp = 6; + + msg_detection_1.range = 120.0; + + msg_1->detections.push_back(msg_detection_1); + + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_1); + } + + // Scan 1 data shouldn't be published at this point + EXPECT_EQ(0, static_cast(publish_packet_1.timestamp)); + EXPECT_EQ(0, publish_packet_1.detections.size()); + + for (int packet_size = 0; packet_size < 10; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); + } + + // Scan 1 data shouldn't be published at this point + EXPECT_EQ(0, static_cast(publish_packet_1.timestamp)); + EXPECT_EQ(0, publish_packet_1.detections.size()); + + // Near Packet Data (Scan 2) + msg_2->event_id = 3; + msg_2->timestamp = 8; + + msg_detection_0.range = 150.0; + + msg_2->detections.push_back(msg_detection_0); + + for (int packet_size = 0; packet_size < 3; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_2); + } + + // Scan 2 data shouldn't be published at this point + EXPECT_EQ(0, static_cast(publish_packet_2.timestamp)); + EXPECT_EQ(0, publish_packet_2.detections.size()); + + for (int packet_size = 0; packet_size < 2; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); + } + + // Scan 1 should get published here since it is full and all scan states should get reset + EXPECT_EQ(6, static_cast(publish_packet_1.timestamp)); + EXPECT_EQ(30, publish_packet_1.detections.size()); + + // Sending the rest of the data from scan 2 + for (int packet_size = 0; packet_size < 15; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_2); + } + + EXPECT_EQ(0, static_cast(publish_packet_2.timestamp)); + EXPECT_EQ(0, publish_packet_2.detections.size()); + + // Far Packet Data (Scan 2) + msg_3->event_id = 1; + msg_3->timestamp = 9; + + msg_detection_1.range = 210.0; + + msg_3->detections.push_back(msg_detection_1); + + for (int packet_size = 0; packet_size < 12; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet_2); + } + + EXPECT_EQ(9, static_cast(publish_packet_2.timestamp)); + EXPECT_EQ(150, publish_packet_2.detections[0].range); + EXPECT_EQ(210, publish_packet_2.detections[19].range); + EXPECT_EQ(30, publish_packet_2.detections.size()); +} + +/** +* @brief Sends 5 Near (scan 1), Send 12 far (scan 1) and checks if it returns the correct packet. +* (Special Case). +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteNearPackets) +{ + SetUp("nearfar"); + + auto msg_0 = std::make_shared(); + auto msg_1 = std::make_shared(); + auto msg_2 = std::make_shared(); + auto msg_3 = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection_0; + radar_msgs::msg::RadarDetection msg_detection_1; + + radar_msgs::msg::RadarPacket publish_packet; + + // Near Packet Data (Scan 1) + msg_0->event_id = 3; + msg_0->timestamp = 5; + + msg_detection_0.range = 80.0; + + msg_0->detections.push_back(msg_detection_0); + + // Far Packet Data (Scan 1) + msg_1->event_id = 1; + msg_1->timestamp = 6; + + msg_detection_1.range = 120.0; + + msg_1->detections.push_back(msg_detection_1); + + for (int packet_size = 0; packet_size < 5; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet); + } + + EXPECT_EQ(0, static_cast(publish_packet.timestamp)); + EXPECT_EQ(0, publish_packet.detections.size()); + + for (int packet_size = 0; packet_size < 12; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet); + } + + // Special Case data should be discarded at this point + EXPECT_EQ(0, static_cast(publish_packet.timestamp)); + EXPECT_EQ(0, publish_packet.detections.size()); + + // Sending a full packet to validate that previous data was actually discarded + + // Near Packet Data (Scan 2) + msg_2->event_id = 3; + msg_2->timestamp = 7; + + msg_detection_0.range = 160.0; + + msg_2->detections.push_back(msg_detection_0); + + // Far Packet Data (Scan 2) + msg_3->event_id = 1; + msg_3->timestamp = 8; + + msg_detection_1.range = 200.0; + + msg_3->detections.push_back(msg_detection_1); + + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet); + } + + for (int packet_size = 0; packet_size < 12; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet); + } + + EXPECT_EQ(8, static_cast(publish_packet.timestamp)); + EXPECT_EQ(30, publish_packet.detections.size()); + EXPECT_EQ(160, publish_packet.detections[0].range); + EXPECT_EQ(200, publish_packet.detections[19].range); +} + +/** +* @brief Sends 7 far (scan 1), Send 1 Near (scan 2), +* then checks if the 7 far (scan 1) is discarded (Special case). +*/ +TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteFarPackets) +{ + SetUp("nearfar"); + + auto msg_0 = std::make_shared(); + auto msg_1 = std::make_shared(); + auto msg_2 = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection_0; + radar_msgs::msg::RadarDetection msg_detection_1; + radar_msgs::msg::RadarDetection msg_detection_2; + + radar_msgs::msg::RadarPacket publish_packet; + + // Far Packet Data (Scan 1) + msg_0->event_id = 1; + msg_0->timestamp = 6; + + msg_detection_0.range = 80.0; + + msg_0->detections.push_back(msg_detection_0); + + // Near Packet Data (Scan 2) + msg_1->event_id = 4; + msg_1->timestamp = 7; + + msg_detection_1.range = 120.0; + + msg_1->detections.push_back(msg_detection_1); + + // Far Packet Data (Scan 2) + msg_2->event_id = 1; + msg_2->timestamp = 8; + + msg_detection_2.range = 180.0; + + msg_2->detections.push_back(msg_detection_2); + + // Sending far packet data from scan 1 + for (int packet_size = 0; packet_size < 7; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet); + } + + // 7 Far scan packets (Scan 1) should be discarded at this point + EXPECT_EQ(0, static_cast(publish_packet.timestamp)); + EXPECT_EQ(0, publish_packet.detections.size()); + + // Sending near packet data from scan 2 + for (int packet_size = 0; packet_size < 18; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet); + } + + EXPECT_EQ(0, static_cast(publish_packet.timestamp)); + EXPECT_EQ(0, publish_packet.detections.size()); + + // Sending far packet data from scan 2 + for (int packet_size = 0; packet_size < 12; packet_size++) { + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet); + } + + EXPECT_EQ(8, static_cast(publish_packet.timestamp)); + EXPECT_EQ(30, publish_packet.detections.size()); +} diff --git a/src/wato_msgs/radar_msgs/CMakeLists.txt b/src/wato_msgs/radar_msgs/CMakeLists.txt new file mode 100644 index 00000000..fdc6c84f --- /dev/null +++ b/src/wato_msgs/radar_msgs/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.10) +project(radar_msgs) + +# 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(std_msgs REQUIRED) # ROS2 common messages + +set(msg_files + msg/RadarDetection.msg + msg/RadarPacket.msg) +# Parses message files to generate ROS2 interfaces +rosidl_generate_interfaces(radar_msgs + ${msg_files} + DEPENDENCIES std_msgs) + +ament_export_dependencies(rosidl_default_runtime) +ament_package() diff --git a/src/wato_msgs/radar_msgs/msg/RadarDetection.msg b/src/wato_msgs/radar_msgs/msg/RadarDetection.msg new file mode 100644 index 00000000..69807157 --- /dev/null +++ b/src/wato_msgs/radar_msgs/msg/RadarDetection.msg @@ -0,0 +1,19 @@ +float32 pos_x +float32 pos_y +float32 pos_z +float32 vrel_rad +float32 az_ang0 +float32 az_ang1 +float32 el_ang +float32 rcs0 +float32 rcs1 +float32 range_var +float32 vrel_rad_var +float32 az_ang_var0 +float32 az_ang_var1 +float32 el_ang_var +float32 snr +float32 range +float32 prob0 +float32 prob1 +uint8 pdh0 \ No newline at end of file diff --git a/src/wato_msgs/radar_msgs/msg/RadarPacket.msg b/src/wato_msgs/radar_msgs/msg/RadarPacket.msg new file mode 100644 index 00000000..43931644 --- /dev/null +++ b/src/wato_msgs/radar_msgs/msg/RadarPacket.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +uint8 event_id +uint32 timestamp +uint32 measurement_counter +float32 vambig +float32 center_frequency +RadarDetection[] detections \ No newline at end of file diff --git a/src/wato_msgs/radar_msgs/package.xml b/src/wato_msgs/radar_msgs/package.xml new file mode 100644 index 00000000..4320446c --- /dev/null +++ b/src/wato_msgs/radar_msgs/package.xml @@ -0,0 +1,20 @@ + + + radar_msgs + 0.0.0 + Sample ROS2 messages + + Cheran Mahalingam + TODO + + ament_cmake + std_msgs + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +