From 96f7201ecee5db663bf6fe266cdeae561796c711 Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 25 Jan 2023 22:49:56 -0500 Subject: [PATCH 01/51] added Dockerfile and profile for radar_driver --- docker/radar_driver/radar_driver.Dockerfile | 42 +++++++++++++++++++++ profiles/docker-compose.sensors.yaml | 16 ++++++++ 2 files changed, 58 insertions(+) create mode 100644 docker/radar_driver/radar_driver.Dockerfile create mode 100644 profiles/docker-compose.sensors.yaml diff --git a/docker/radar_driver/radar_driver.Dockerfile b/docker/radar_driver/radar_driver.Dockerfile new file mode 100644 index 00000000..a602d00a --- /dev/null +++ b/docker/radar_driver/radar_driver.Dockerfile @@ -0,0 +1,42 @@ +FROM ros:foxy AS base + +RUN apt-get update && \ + apt-get install -y --no-install-recommends \ + ros-kinetic-perception-pcl build-essential \ + python-catkin-tools python-rosdep libpcap0.8-dev \ + wget curl + +# 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 /home/docker/catkin_ws/src +WORKDIR /home/docker/catkin_ws + +COPY src/sensor_interfacing/radar_driver src/radar_driver + +RUN catkin config --extend /opt/ros/kinetic && \ + catkin build && \ + rm -rf .catkin_tools build + +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 ["/home/docker/wato_ros_entrypoint.sh"] + +CMD ["roslaunch", "--wait", "radar_driver", "radarROSbag.launch"] diff --git a/profiles/docker-compose.sensors.yaml b/profiles/docker-compose.sensors.yaml new file mode 100644 index 00000000..cd2a088f --- /dev/null +++ b/profiles/docker-compose.sensors.yaml @@ -0,0 +1,16 @@ +version: "3.8" +services: + radar_driver: + build: + context: . + dockerfile: docker/radar_driver/radar_driver.Dockerfile + cache_from: + - "${RADAR_DRIVER_IMAGE:?}:${CACHE_FROM_TAG}" + - "${RADAR_DRIVER_IMAGE:?}:develop" + image: "${RADAR_DRIVER_IMAGE:?}:${TAG}" + volumes: + - ./src/sensor_interfacing/radar_driver:/home/docker/catkin_ws/src/radar_driver + - ./code_server_config/bashrc:/home/docker/.bashrc + command: roslaunch radar_driver radarROSbag.launch + depends_on: + - rosmaster From 7972a93c67424d0294505eaf25bda8c647023764 Mon Sep 17 00:00:00 2001 From: Rijin Date: Fri, 27 Jan 2023 19:45:45 -0500 Subject: [PATCH 02/51] configured radar dockerfile and profile --- dev_config.sh | 6 ++++++ docker/radar_driver/radar_driver.Dockerfile | 23 ++++++++++----------- profiles/docker-compose.sensors.yaml | 10 ++++----- 3 files changed, 21 insertions(+), 18 deletions(-) diff --git a/dev_config.sh b/dev_config.sh index cc9c2268..38e0823f 100755 --- a/dev_config.sh +++ b/dev_config.sh @@ -62,6 +62,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 +RADAR_DRIVER_IMAGE=${RADAR_DRIVER_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/radar_driver"} + ## -------------------------- User ID ----------------------------- FIXUID=$(id -u) @@ -88,6 +91,9 @@ echo "SAMPLES_CPP_AGGREGATOR_IMAGE=$SAMPLES_CPP_AGGREGATOR_IMAGE" >> "$PROFILES_ echo "SAMPLES_CPP_PRODUCER_IMAGE=$SAMPLES_CPP_PRODUCER_IMAGE" >> "$PROFILES_DIR/.env" echo "SAMPLES_CPP_TRANSFORMER_IMAGE=$SAMPLES_CPP_TRANSFORMER_IMAGE" >> "$PROFILES_DIR/.env" + +echo "RADAR_DRIVER_IMAGE=$RADAR_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/radar_driver/radar_driver.Dockerfile b/docker/radar_driver/radar_driver.Dockerfile index a602d00a..86f80471 100644 --- a/docker/radar_driver/radar_driver.Dockerfile +++ b/docker/radar_driver/radar_driver.Dockerfile @@ -1,10 +1,7 @@ FROM ros:foxy AS base -RUN apt-get update && \ - apt-get install -y --no-install-recommends \ - ros-kinetic-perception-pcl build-essential \ - python-catkin-tools python-rosdep libpcap0.8-dev \ - wget curl +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 && \ @@ -23,14 +20,16 @@ RUN USER=docker && \ USER docker:docker -RUN mkdir -p /home/docker/catkin_ws/src -WORKDIR /home/docker/catkin_ws +RUN mkdir -p ~/ament_ws/src +WORKDIR /home/docker/ament_ws/src -COPY src/sensor_interfacing/radar_driver src/radar_driver +#COPY src/sensor_interfacing/radar_driver src/radar_driver -RUN catkin config --extend /opt/ros/kinetic && \ - catkin build && \ - rm -rf .catkin_tools build +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 @@ -39,4 +38,4 @@ RUN sudo chmod +x ~/wato_ros_entrypoint.sh ENTRYPOINT ["/home/docker/wato_ros_entrypoint.sh"] -CMD ["roslaunch", "--wait", "radar_driver", "radarROSbag.launch"] +#CMD ["roslaunch", "--wait", "radar_driver", "radarROSbag.launch"] diff --git a/profiles/docker-compose.sensors.yaml b/profiles/docker-compose.sensors.yaml index cd2a088f..21b2b551 100644 --- a/profiles/docker-compose.sensors.yaml +++ b/profiles/docker-compose.sensors.yaml @@ -2,15 +2,13 @@ version: "3.8" services: radar_driver: build: - context: . + context: .. dockerfile: docker/radar_driver/radar_driver.Dockerfile cache_from: - "${RADAR_DRIVER_IMAGE:?}:${CACHE_FROM_TAG}" - "${RADAR_DRIVER_IMAGE:?}:develop" image: "${RADAR_DRIVER_IMAGE:?}:${TAG}" volumes: - - ./src/sensor_interfacing/radar_driver:/home/docker/catkin_ws/src/radar_driver - - ./code_server_config/bashrc:/home/docker/.bashrc - command: roslaunch radar_driver radarROSbag.launch - depends_on: - - rosmaster + - ./src/sensor_interfacing/radar_driver:/home/docker/ament_ws/src/radar_driver + + #command: roslaunch radar_driver radarROSbag.launch \ No newline at end of file From 9b1b56be9e36d35375c14b77e2b68dd7b12c2910 Mon Sep 17 00:00:00 2001 From: Rijin Date: Sat, 28 Jan 2023 15:51:51 +0000 Subject: [PATCH 03/51] changed radar dockerfile --- docker/radar_driver/radar_driver.Dockerfile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/docker/radar_driver/radar_driver.Dockerfile b/docker/radar_driver/radar_driver.Dockerfile index 86f80471..18f07633 100644 --- a/docker/radar_driver/radar_driver.Dockerfile +++ b/docker/radar_driver/radar_driver.Dockerfile @@ -23,8 +23,9 @@ USER docker:docker RUN mkdir -p ~/ament_ws/src WORKDIR /home/docker/ament_ws/src -#COPY src/sensor_interfacing/radar_driver src/radar_driver +COPY src/sensor_interfacing/radar_driver src/radar_driver +WORKDIR /home/docker/ament_ws RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ rosdep update && \ rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y && \ From c474d1778a502970f6d5e083cea0a9e8568acf92 Mon Sep 17 00:00:00 2001 From: Rijin Date: Sat, 28 Jan 2023 22:47:45 +0000 Subject: [PATCH 04/51] added basefiles for the radar_pointcloud_filter node --- src/sensor_interfacing/radar_driver/CMakeLists.txt | 0 .../radar_driver/include/radar_pointcloud_filter.hpp | 0 .../radar_driver/include/radar_pointcloud_filter_node.hpp | 0 .../radar_driver/launch/radar_pointcloud_filter.launch.py | 0 src/sensor_interfacing/radar_driver/package.xml | 0 .../radar_driver/src/radar_pointcloud_filter.cpp | 0 .../radar_driver/src/radar_pointcloud_filter_node.cpp | 0 .../radar_driver/test/radar_pointcloud_filter_test.cpp | 0 8 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/sensor_interfacing/radar_driver/CMakeLists.txt create mode 100644 src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp create mode 100644 src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp create mode 100644 src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py create mode 100644 src/sensor_interfacing/radar_driver/package.xml create mode 100644 src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp create mode 100644 src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp create mode 100644 src/sensor_interfacing/radar_driver/test/radar_pointcloud_filter_test.cpp diff --git a/src/sensor_interfacing/radar_driver/CMakeLists.txt b/src/sensor_interfacing/radar_driver/CMakeLists.txt new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py b/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/package.xml b/src/sensor_interfacing/radar_driver/package.xml new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/test/radar_pointcloud_filter_test.cpp b/src/sensor_interfacing/radar_driver/test/radar_pointcloud_filter_test.cpp new file mode 100644 index 00000000..e69de29b From c6a21e3dae003415d1981fcc3b299c1572c501be Mon Sep 17 00:00:00 2001 From: Rijin Date: Sun, 29 Jan 2023 20:05:46 +0000 Subject: [PATCH 05/51] updated files for RadarPointCloudfilter Node --- .../include/radar_pointcloud_filter_node.hpp | 85 +++++++++++++++++++ .../launch/radar_pointcloud_filter.launch.py | 14 +++ .../radar_driver/package.xml | 23 +++++ .../src/radar_pointcloud_filter_node.cpp | 67 +++++++++++++++ .../radar_msgs/msg/UnfilteredCarlaLeft.msg | 0 .../radar_msgs/msg/UnfilteredCarlaRight.msg | 0 .../radar_msgs/msg/UnfilteredRadarLeft.msg | 0 .../radar_msgs/msg/UnfilteredRadarRight.msg | 0 8 files changed, 189 insertions(+) mode change 100644 => 100755 src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py create mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg create mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg create mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg create mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp index e69de29b..a9a032b6 100644 --- a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp @@ -0,0 +1,85 @@ +#ifndef RADAR_POINTCLOUD_FILTER_NODE_HPP_ +#define RADAR_POINTCLOUD_FILTER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "radar_msgs/msg/UnfilteredRadarLeft.hpp" +#include "radar_msgs/msg/UnfilteredRadarRight.hpp" +#include "radar_msgs/msg/CarlaUnfilteredRadar.hpp" +#include "radar_msgs/msg/CarlaUnfilteredRadarTwo.hpp" + +#include "radar_pointcloud_filter.hpp" + + /** + * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar + * topics from CARLA ROS Bridge and ROSBags and publishes to "filtered" radar topic. + */ + +class PointCloudFilterNode : 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; + + /** + * PointCloudFilter Node Constructor. + */ + PointCloudFilterNode(); + +private: + /** + * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * topic + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_radar_right_callback( + const radar_msgs::msg::UnfilteredRadarRight::SharedPtr msg); + + + /** + * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * topic + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_radar_left_callback( + const radar_msgs::msg::UnfilteredRadarLeft::SharedPtr msg); + + + /** + * A ROS2 subscription node callback used to unpack raw CARLA radar data from + * "unfiltered" topics. + * + * @param msg a processed message from the "unfiltered" topic + */ + void unfiltered_carla_radar_left_callback( + const radar_msgs::msg::UnfilteredCarlaLeft::SharedPtr msg); + + + /** + * A ROS2 subscription node callback used to unpack raw CARLA radar data from + * "unfiltered" topics. + * + * @param msg a processed message from the "unfiltered" topic + */ + void unfiltered_carla_radar_right_callback( + const radar_msgs::msg::UnfilteredCarlaRight::SharedPtr msg); + + + // ROS2 Subscriber listening to the unfiltered radar left topic. + rclcpp::Subscription::SharedPtr raw_left_sub_ + + // ROS2 Subscriber listening to the unfiltered radar right topic. + rclcpp::Subscription::SharedPtr raw_right_sub_ + + // ROS2 Subscriber listening to the unfiltered Carla left topic. + rclcpp::Subscription::SharedPtr raw_carla_left_sub_ + + // ROS2 Subscriber listening to the unfiltered Carla right topic. + rclcpp::Subscription::SharedPtr raw_carla_right_sub_ + +}; + +#endif // RADAR_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py b/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py old mode 100644 new mode 100755 index e69de29b..f21337e2 --- a/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + """Launch PointCloudFilter node.""" + radar_pointcloud_filter_node = Node( + package='radar_pointcloud_filter', + executable='radar_pointcloud_filter_node', + ) + + return LaunchDescription([ + radar_pointcloud_filter_node + ]) diff --git a/src/sensor_interfacing/radar_driver/package.xml b/src/sensor_interfacing/radar_driver/package.xml index e69de29b..942514d3 100644 --- a/src/sensor_interfacing/radar_driver/package.xml +++ b/src/sensor_interfacing/radar_driver/package.xml @@ -0,0 +1,23 @@ + + + radar_point_cloud_filter + 0.0.0 + A sample ROS package for pubsub communication + + 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/radar_driver/src/radar_pointcloud_filter_node.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp index e69de29b..e23ade3d 100644 --- a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp @@ -0,0 +1,67 @@ +#include +#include + +#include "radar_pointcloud_filter_node.hpp" + +PointCloudFilterNode::PointCloudFilterNode() +: Node("radar_point_cloud_filter") +{ + raw_left_sub_ = this->create_subscription( + "unfilteredRadarLeft", + std::bind( + &AggregatorNode::unfiltered_radar_left_callback, this, + std::placeholders::_1)); + + raw_right_sub_ = this->create_subscription( + "unfilteredRadarRight", ADVERTISING_FREQ, + std::bind( + &AggregatorNode::unfiltered_radar_right_callback, this, + std::placeholders::_1)); + + raw_carla_left_sub_ = this->create_subscription( + "unfilteredCarlaLeft", ADVERTISING_FREQ, + std::bind( + &AggregatorNode::unfiltered_carla_radar_left_callback, this, + std::placeholders::_1)); + + raw_carla_right_sub_ = this->create_subscription( + "unfilteredCarlaRight", ADVERTISING_FREQ, + std::bind( + &AggregatorNode::unfiltered_carla_radar_right_callback, this, + std::placeholders::_1)); +} + + void unfiltered_radar_right_callback( + const radar_msgs::msg::UnfilteredRadarRight::SharedPtr msg) + { + // + } + + + void unfiltered_radar_left_callback( + const radar_msgs::msg::UnfilteredRadarLeft::SharedPtr msg) + { + // + } + + + void unfiltered_carla_radar_left_callback( + const radar_msgs::msg::UnfilteredCarlaLeft::SharedPtr msg) + { + // + } + + + void unfiltered_carla_radar_right_callback( + const radar_msgs::msg::UnfilteredCarlaRight::SharedPtr msg) + { + // + } + +int main(int argc, char ** argv) +{ + rclcpp::init(argc,argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg new file mode 100644 index 00000000..e69de29b diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg new file mode 100644 index 00000000..e69de29b diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg new file mode 100644 index 00000000..e69de29b diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg new file mode 100644 index 00000000..e69de29b From fbe12425fb494172e764d06a93b80ac08dcfb6cd Mon Sep 17 00:00:00 2001 From: Rijin Date: Mon, 30 Jan 2023 03:33:38 +0000 Subject: [PATCH 06/51] updated files for radar_pointcloud_filter node --- .../include/radar_pointcloud_filter_node.hpp | 8 ++++---- .../src/radar_pointcloud_filter_node.cpp | 8 ++++---- .../radar_msgs/msg/UnfilteredCarlaLeft.msg | 9 +++++++++ .../radar_msgs/msg/UnfilteredCarlaRight.msg | 9 +++++++++ .../radar_msgs/msg/UnfilteredRadarLeft.msg | 19 +++++++++++++++++++ .../radar_msgs/msg/UnfilteredRadarRight.msg | 19 +++++++++++++++++++ 6 files changed, 64 insertions(+), 8 deletions(-) diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp index a9a032b6..6cab2a1b 100644 --- a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp @@ -5,8 +5,8 @@ #include "radar_msgs/msg/UnfilteredRadarLeft.hpp" #include "radar_msgs/msg/UnfilteredRadarRight.hpp" -#include "radar_msgs/msg/CarlaUnfilteredRadar.hpp" -#include "radar_msgs/msg/CarlaUnfilteredRadarTwo.hpp" +#include "radar_msgs/msg/UnfilteredCarlaLeft.hpp" +#include "radar_msgs/msg/UnfilteredCarlaRight.hpp" #include "radar_pointcloud_filter.hpp" @@ -74,10 +74,10 @@ class PointCloudFilterNode : public rclcpp:: Node // ROS2 Subscriber listening to the unfiltered radar right topic. rclcpp::Subscription::SharedPtr raw_right_sub_ - // ROS2 Subscriber listening to the unfiltered Carla left topic. + // ROS2 Subscriber listening to the unfiltered Carla left topic (radar1). rclcpp::Subscription::SharedPtr raw_carla_left_sub_ - // ROS2 Subscriber listening to the unfiltered Carla right topic. + // ROS2 Subscriber listening to the unfiltered Carla right topic (radar2). rclcpp::Subscription::SharedPtr raw_carla_right_sub_ }; diff --git a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp index e23ade3d..416f4af6 100644 --- a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp @@ -9,25 +9,25 @@ PointCloudFilterNode::PointCloudFilterNode() raw_left_sub_ = this->create_subscription( "unfilteredRadarLeft", std::bind( - &AggregatorNode::unfiltered_radar_left_callback, this, + &PointCloudFilterNode::unfiltered_radar_left_callback, this, std::placeholders::_1)); raw_right_sub_ = this->create_subscription( "unfilteredRadarRight", ADVERTISING_FREQ, std::bind( - &AggregatorNode::unfiltered_radar_right_callback, this, + &PointCloudFilterNode::unfiltered_radar_right_callback, this, std::placeholders::_1)); raw_carla_left_sub_ = this->create_subscription( "unfilteredCarlaLeft", ADVERTISING_FREQ, std::bind( - &AggregatorNode::unfiltered_carla_radar_left_callback, this, + &PointCloudFilterNode::unfiltered_carla_radar_left_callback, this, std::placeholders::_1)); raw_carla_right_sub_ = this->create_subscription( "unfilteredCarlaRight", ADVERTISING_FREQ, std::bind( - &AggregatorNode::unfiltered_carla_radar_right_callback, this, + &PointCloudFilterNode::unfiltered_carla_radar_right_callback, this, std::placeholders::_1)); } diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg index e69de29b..9a9f788e 100644 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg +++ b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg @@ -0,0 +1,9 @@ +Header header +uint32 height +uint32 width +PointField[] fields +bool is_bigendian +uint32 point_step +uint32 row_step +uint8[] data +bool is_dense \ No newline at end of file diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg index e69de29b..9a9f788e 100644 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg +++ b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg @@ -0,0 +1,9 @@ +Header header +uint32 height +uint32 width +PointField[] fields +bool is_bigendian +uint32 point_step +uint32 row_step +uint8[] data +bool is_dense \ No newline at end of file diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg index e69de29b..403a0c67 100644 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg +++ b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg @@ -0,0 +1,19 @@ +float32 posX +float32 posY +float32 posZ +float32 VrelRad +float32 AzAng0 +float32 AzAng1 +float32 ElAng +float32 RCS0 +float32 RCS1 +float32 RangeVar +float32 VrelRadVar +float32 AzAngVar0 +float32 AzAngVar1 +float32 ElAngVar +float32 SNR +float32 Range +float32 Prob0 +float32 Prob1 +uint8 Pdh0 diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg index e69de29b..403a0c67 100644 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg +++ b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg @@ -0,0 +1,19 @@ +float32 posX +float32 posY +float32 posZ +float32 VrelRad +float32 AzAng0 +float32 AzAng1 +float32 ElAng +float32 RCS0 +float32 RCS1 +float32 RangeVar +float32 VrelRadVar +float32 AzAngVar0 +float32 AzAngVar1 +float32 ElAngVar +float32 SNR +float32 Range +float32 Prob0 +float32 Prob1 +uint8 Pdh0 From c877695f5b36e8731892433318888b3de611e581 Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 31 Jan 2023 01:45:13 +0000 Subject: [PATCH 07/51] updated files for radar_pointcloud_filter node --- .../radar_driver/CMakeLists.txt | 76 +++++++++++++++++++ .../include/radar_pointcloud_filter.hpp | 9 +++ .../include/radar_pointcloud_filter_node.hpp | 2 + .../src/radar_pointcloud_filter.cpp | 3 + .../src/radar_pointcloud_filter_node.cpp | 8 +- 5 files changed, 94 insertions(+), 4 deletions(-) diff --git a/src/sensor_interfacing/radar_driver/CMakeLists.txt b/src/sensor_interfacing/radar_driver/CMakeLists.txt index e69de29b..28a34ce3 100644 --- a/src/sensor_interfacing/radar_driver/CMakeLists.txt +++ b/src/sensor_interfacing/radar_driver/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.10) +project(radar_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(pointcloudfilter_lib + src/radar_pointcloud_filter.cpp) +# Indicate to compiler where to search for header files +target_include_directories(pointcloudfilter_lib + PUBLIC include) +# Add ROS2 dependencies required by package +ament_target_dependencies(pointcloudfilter_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(radar_pointcloud_filter_test test/radar_pointcloud_filter_test.cpp) + # Link to the previously built library to access RadarPointCloudFilter classes and methods + target_link_libraries(radar_pointcloud_filter_test pointcloudfilter_lib) + + # Copy executable to installation location + install(TARGETS + radar_pointcloud_filter_test + DESTINATION lib/${PROJECT_NAME}) +endif() + +# Create ROS2 node executable from source files +add_executable(radar_pointcloud_filter_node src/radar_pointcloud_filter_node.cpp) +# Link to the previously built library to access Aggregator classes and methods +target_link_libraries(radar_pointcloud_filter_node pointcloudfilter_lib) + +# Copy executable to installation location +install(TARGETS + radar_pointcloud_filter_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy launch files to installation location +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp index e69de29b..77d08009 100644 --- a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp +++ b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp @@ -0,0 +1,9 @@ +#ifndef RADAR_POINTCLOUD_FILTER_HPP_ +#define RADAR_POINTCLOUD_FILTER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "radar_msgs/msg/UnfilteredRadarLeft.hpp" +#include "radar_msgs/msg/UnfilteredRadarRight.hpp" +#include "radar_msgs/msg/UnfilteredCarlaLeft.hpp" +#include "radar_msgs/msg/UnfilteredCarlaRight.hpp" diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp index 6cab2a1b..dc51f01e 100644 --- a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp @@ -80,6 +80,8 @@ class PointCloudFilterNode : public rclcpp:: Node // ROS2 Subscriber listening to the unfiltered Carla right topic (radar2). rclcpp::Subscription::SharedPtr raw_carla_right_sub_ + // Add an object below from radar_pointcloud_filter.hpp that contains the methods + }; #endif // RADAR_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp index e69de29b..f02fda5b 100644 --- a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp +++ b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp @@ -0,0 +1,3 @@ +#include + +#include "radar_pointcloud_filter.hpp" \ No newline at end of file diff --git a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp index 416f4af6..5accb6d4 100644 --- a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp @@ -34,28 +34,28 @@ PointCloudFilterNode::PointCloudFilterNode() void unfiltered_radar_right_callback( const radar_msgs::msg::UnfilteredRadarRight::SharedPtr msg) { - // + //messages from unfiltered right radar topic (rosbags) } void unfiltered_radar_left_callback( const radar_msgs::msg::UnfilteredRadarLeft::SharedPtr msg) { - // + //messages from unfiltered left radar topic (rosbags) } void unfiltered_carla_radar_left_callback( const radar_msgs::msg::UnfilteredCarlaLeft::SharedPtr msg) { - // + //messages from unfiltered left radar topic (CARLA ROS Bridge) } void unfiltered_carla_radar_right_callback( const radar_msgs::msg::UnfilteredCarlaRight::SharedPtr msg) { - // + //messages from unfiltered right radar topic (CARLA ROS Bridge) } int main(int argc, char ** argv) From 9d7e1869f169c38dd4ac9480c3fdca229637b7f8 Mon Sep 17 00:00:00 2001 From: Rijin Date: Mon, 6 Feb 2023 03:28:47 +0000 Subject: [PATCH 08/51] added new ROS nodes and updated files for point cloud filters --- .../CMakeLists.txt | 24 ++--- .../config/ars_radar_params.yaml | 3 + .../include/ars_pointcloud_filter.hpp | 23 +++++ .../include/ars_pointcloud_filter_node.hpp | 65 ++++++++++++++ .../launch/ars_pointcloud_filter.launch.py | 19 ++++ .../package.xml | 4 +- .../src/ars_pointcloud_filter.cpp | 13 +++ .../src/ars_pointcloud_filter_node.cpp | 45 ++++++++++ .../test/ars_pointcloud_filter_test.cpp} | 0 .../CarlaPointCloudFilter/CMakeLists.txt | 0 .../config/carla_radar_params.yaml | 3 + .../include/carla_pointcloud_filter.hpp | 0 .../include/carla_pointcloud_filter_node.hpp | 67 ++++++++++++++ .../launch/carla_pointcloud_filter.launch.py | 0 .../CarlaPointCloudFilter/package.xml | 0 .../src/carla_pointcloud_filter.cpp | 0 .../src/carla_pointcloud_filter_node.cpp | 11 +++ .../test/carla_pointcloud_filter_test.cpp | 0 .../include/radar_pointcloud_filter.hpp | 9 -- .../include/radar_pointcloud_filter_node.hpp | 87 ------------------- .../launch/radar_pointcloud_filter.launch.py | 14 --- .../src/radar_pointcloud_filter.cpp | 3 - .../src/radar_pointcloud_filter_node.cpp | 67 -------------- ...lteredRadarLeft.msg => RadarDetection.msg} | 2 +- ...Left.msg => UnfilteredCarlaLeftPacket.msg} | 0 ...ght.msg => UnfilteredCarlaRightPacket.msg} | 0 .../msg/UnfilteredRadarLeftPacket.msg | 8 ++ .../radar_msgs/msg/UnfilteredRadarRight.msg | 19 ---- .../msg/UnfilteredRadarRightPacket.msg | 8 ++ 29 files changed, 280 insertions(+), 214 deletions(-) rename src/sensor_interfacing/{radar_driver => ARSPointCloudFilter}/CMakeLists.txt (75%) create mode 100644 src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml create mode 100644 src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp create mode 100644 src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp create mode 100755 src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py rename src/sensor_interfacing/{radar_driver => ARSPointCloudFilter}/package.xml (84%) create mode 100644 src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp create mode 100644 src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp rename src/sensor_interfacing/{radar_driver/test/radar_pointcloud_filter_test.cpp => ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp} (100%) create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/CMakeLists.txt create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter.hpp create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/package.xml create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter.cpp create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp create mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/test/carla_pointcloud_filter_test.cpp delete mode 100644 src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp delete mode 100644 src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp delete mode 100755 src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py delete mode 100644 src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp delete mode 100644 src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp rename src/wato_msgs/radar_msgs/msg/{UnfilteredRadarLeft.msg => RadarDetection.msg} (95%) rename src/wato_msgs/radar_msgs/msg/{UnfilteredCarlaLeft.msg => UnfilteredCarlaLeftPacket.msg} (100%) rename src/wato_msgs/radar_msgs/msg/{UnfilteredCarlaRight.msg => UnfilteredCarlaRightPacket.msg} (100%) create mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeftPacket.msg delete mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg create mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg diff --git a/src/sensor_interfacing/radar_driver/CMakeLists.txt b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt similarity index 75% rename from src/sensor_interfacing/radar_driver/CMakeLists.txt rename to src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt index 28a34ce3..43ee51fc 100644 --- a/src/sensor_interfacing/radar_driver/CMakeLists.txt +++ b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10) -project(radar_pointcloud_filter) +project(ars_radar_pointcloud_filter) # Set compiler to use C++ 17 standard if(NOT CMAKE_CXX_STANDARD) @@ -20,13 +20,13 @@ find_package(radar_msgs REQUIRED) # Custom package containing ROS2 messages # 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(pointcloudfilter_lib - src/radar_pointcloud_filter.cpp) +add_library(arspointcloudfilter_lib + src/ars_pointcloud_filter.cpp) # Indicate to compiler where to search for header files -target_include_directories(pointcloudfilter_lib +target_include_directories(arspointcloudfilter_lib PUBLIC include) # Add ROS2 dependencies required by package -ament_target_dependencies(pointcloudfilter_lib rclcpp radar_msgs) +ament_target_dependencies(arspointcloudfilter_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. @@ -48,24 +48,24 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() # Create test executable from source files - ament_add_gtest(radar_pointcloud_filter_test test/radar_pointcloud_filter_test.cpp) - # Link to the previously built library to access RadarPointCloudFilter classes and methods - target_link_libraries(radar_pointcloud_filter_test pointcloudfilter_lib) + ament_add_gtest(ars_pointcloud_filter_test test/ars_pointcloud_filter_test.cpp) + # Link to the previously built library to access ARSPointCloudFilter classes and methods + target_link_libraries(ars_pointcloud_filter_test arspointcloudfilter_lib) # Copy executable to installation location install(TARGETS - radar_pointcloud_filter_test + ars_pointcloud_filter_test DESTINATION lib/${PROJECT_NAME}) endif() # Create ROS2 node executable from source files -add_executable(radar_pointcloud_filter_node src/radar_pointcloud_filter_node.cpp) +add_executable(ars_pointcloud_filter_node src/ars_pointcloud_filter_node.cpp) # Link to the previously built library to access Aggregator classes and methods -target_link_libraries(radar_pointcloud_filter_node pointcloudfilter_lib) +target_link_libraries(ars_pointcloud_filter_node arspointcloudfilter_lib) # Copy executable to installation location install(TARGETS - radar_pointcloud_filter_node + ars_pointcloud_filter_node DESTINATION lib/${PROJECT_NAME}) # Copy launch files to installation location diff --git a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml new file mode 100644 index 00000000..f5b401b2 --- /dev/null +++ b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml @@ -0,0 +1,3 @@ +ars_point_cloud_filter: + ros__parameters: + filter_mode: "ars" diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp new file mode 100644 index 00000000..44851ed3 --- /dev/null +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -0,0 +1,23 @@ +#ifndef RADAR_POINTCLOUD_FILTER_HPP_ +#define RADAR_POINTCLOUD_FILTER_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "radar_msgs/msg/UnfilteredRadarLeftPacket.hpp" +#include "radar_msgs/msg/UnfilteredRadarRightPacket.hpp" +#include "radar_msgs/msg/UnfilteredCarlaLeftPacket.hpp" +#include "radar_msgs/msg/UnfilteredCarlaRightPacket.hpp" +#include "radar_msgs/msg/RadarDetection.hpp + + + +//SAMPLE FUNCTION + +class PointCloudFilter +{ + +public: + void snr_filter(); + +} + diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp new file mode 100644 index 00000000..528bc2f6 --- /dev/null +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -0,0 +1,65 @@ +#ifndef ARS_POINTCLOUD_FILTER_NODE_HPP_ +#define ARS_POINTCLOUD_FILTER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "radar_msgs/msg/UnfilteredRadarLeftPacket.hpp" +#include "radar_msgs/msg/UnfilteredRadarRightPacket.hpp" +#include "radar_msgs/msg/UnfilteredCarlaLeftPacket.hpp" +#include "radar_msgs/msg/UnfilteredCarlaRightPacket.hpp" +#include "radar_msgs/msg/RadarDetection.hpp + +#include "ars_pointcloud_filter.hpp" + + /** + * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar + * topics from ARS ROSbags and publishes to "filtered" radar topic. + */ + +class ARSPointCloudFilterNode : 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; + + /** + * PointCloudFilter Node Constructor. + */ + ARSPointCloudFilterNode(); + +private: + + /** + * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * topic + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_ars_radar_right_callback( + const radar_msgs::msg::UnfilteredRadarRightPacket::SharedPtr msg); + + + /** + * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * topic + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_ars_radar_left_callback( + const radar_msgs::msg::UnfilteredRadarLeftPacket::SharedPtr msg); + + + // ROS2 Subscriber listening to the unfiltered radar left topic. + rclcpp::Subscription::SharedPtr raw_left_sub_ + + // ROS2 Subscriber listening to the unfiltered radar right topic. + rclcpp::Subscription::SharedPtr raw_right_sub_ + + // Add an object below from radar_pointcloud_filter.hpp that contains the methods (ADD LATER ONCE LOGIC CREATED) + +}; + + + +#endif // ARS_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py new file mode 100755 index 00000000..089e8330 --- /dev/null +++ b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py @@ -0,0 +1,19 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from launch.actions import LogInfo, DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.conditions import LaunchConfigurationEquals + + + +def generate_launch_description(): + """Launch ARSPointCloudFilter node.""" + return LaunchDescription([ + DeclareLaunchArgument('filter_mode', default_value='ars'), + Node( + LaunchConfiguration('filter_mode'), + condition=LaunchConfigurationEquals('filter_mode', 'ars'), + package = 'ars_pointcloud_filter', + executable = 'ars_pointcloud_filter_node' + ) + ]) diff --git a/src/sensor_interfacing/radar_driver/package.xml b/src/sensor_interfacing/ARSPointCloudFilter/package.xml similarity index 84% rename from src/sensor_interfacing/radar_driver/package.xml rename to src/sensor_interfacing/ARSPointCloudFilter/package.xml index 942514d3..8193c6f2 100644 --- a/src/sensor_interfacing/radar_driver/package.xml +++ b/src/sensor_interfacing/ARSPointCloudFilter/package.xml @@ -1,8 +1,8 @@ - radar_point_cloud_filter + ars_pointcloud_filter 0.0.0 - A sample ROS package for pubsub communication + A ROS package for pubsub communication Rijin Muralidharan TODO diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp new file mode 100644 index 00000000..43425d1c --- /dev/null +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -0,0 +1,13 @@ +#include + +#include "radar_pointcloud_filter.hpp" + + +//Packets will depend on which parameter is selected (could be carla packets or hardware packets) +void PointCloudFilter :: snr_filter +{ + RadarDetection snr_filter(const RadarDetection& packet) + { + RadarDetection unfiltered_kace + } +} \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp new file mode 100644 index 00000000..5c4361e3 --- /dev/null +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -0,0 +1,45 @@ +#include +#include + +#include "ars_pointcloud_filter_node.hpp" + +ARSPointCloudFilterNode::ARSPointCloudFilterNode() +: Node("ars_point_cloud_filter") +{ + + raw_left_sub_ = this->create_subscription( + "unfilteredRadarLeft", + std::bind( + &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, + std::placeholders::_1)); + + raw_right_sub_ = this->create_subscription( + "unfilteredRadarRight", ADVERTISING_FREQ, + std::bind( + &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, + std::placeholders::_1)); + +} + + +void ARSPointCloudFilterNode :: unfiltered_ars_radar_right_callback( + const radar_msgs::msg::UnfilteredRadarRightPacket::SharedPtr msg) +{ + //messages from unfiltered right radar topic (ars) + +} + + +void ARSPointCloudFilterNode :: unfiltered_ars_radar_left_callback( + const radar_msgs::msg::UnfilteredRadarLeftPacket::SharedPtr msg) +{ + //messages from unfiltered left radar topic (ars) +} + +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/radar_driver/test/radar_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp similarity index 100% rename from src/sensor_interfacing/radar_driver/test/radar_pointcloud_filter_test.cpp rename to src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/CarlaPointCloudFilter/CMakeLists.txt new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml b/src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml new file mode 100644 index 00000000..2803d6a8 --- /dev/null +++ b/src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml @@ -0,0 +1,3 @@ +radar_point_cloud_filter: + ros__parameters: + filter_mode: "carla" diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter.hpp b/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter.hpp new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp b/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp new file mode 100644 index 00000000..549ed239 --- /dev/null +++ b/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp @@ -0,0 +1,67 @@ +#ifndef CARLA_POINTCLOUD_FILTER_NODE_HPP_ +#define CARLA_POINTCLOUD_FILTER_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +#include "radar_msgs/msg/UnfilteredRadarLeftPacket.hpp" +#include "radar_msgs/msg/UnfilteredRadarRightPacket.hpp" +#include "radar_msgs/msg/UnfilteredCarlaLeftPacket.hpp" +#include "radar_msgs/msg/UnfilteredCarlaRightPacket.hpp" +#include "radar_msgs/msg/RadarDetection.hpp + +#include "carla_pointcloud_filter.hpp" + + /** + * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar + * topics from CARLA ROS Bridge and ROSBags and publishes to "filtered" radar topic. + */ + +class CarlaPointCloudFilterNode : 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; + + /** + * PointCloudFilter Node Constructor. + */ + CarlaPointCloudFilterNode(); + +private: + /** + * A ROS2 subscription node callback used to unpack raw CARLA radar data from + * "unfiltered" topics. + * + * @param msg a processed message from the "unfiltered" topic + */ + void unfiltered_carla_radar_left_callback( + const radar_msgs::msg::UnfilteredCarlaLeft::SharedPtr msg); + + + /** + * A ROS2 subscription node callback used to unpack raw CARLA radar data from + * "unfiltered" topics. + * + * @param msg a processed message from the "unfiltered" topic + */ + void unfiltered_carla_radar_right_callback( + const radar_msgs::msg::UnfilteredCarlaRight::SharedPtr msg); + + + + // ROS2 Subscriber listening to the unfiltered Carla left topic (radar1). + rclcpp::Subscription::SharedPtr raw_carla_left_sub_ + + // ROS2 Subscriber listening to the unfiltered Carla right topic (radar2). + rclcpp::Subscription::SharedPtr raw_carla_right_sub_ + + std::string filter_mode_; + + // // Add an object below from radar_pointcloud_filter.hpp that contains the methods (ADD LATER ONCE LOGIC CREATED) + +}; + + + +#endif // CARLA_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py b/src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/package.xml b/src/sensor_interfacing/CarlaPointCloudFilter/package.xml new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter.cpp b/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter.cpp new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp b/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp new file mode 100644 index 00000000..433ee5a1 --- /dev/null +++ b/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp @@ -0,0 +1,11 @@ + raw_carla_left_sub_ = this->create_subscription( + "unfilteredCarlaLeft", ADVERTISING_FREQ, + std::bind( + &CarlaRadarFilter::unfiltered_carla_radar_left_callback, this, + std::placeholders::_1)); + + raw_carla_right_sub_ = this->create_subscription( + "unfilteredCarlaRight", ADVERTISING_FREQ, + std::bind( + &CarlaRadarFilter::unfiltered_carla_radar_right_callback, this, + std::placeholders::_1)); \ No newline at end of file diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/test/carla_pointcloud_filter_test.cpp b/src/sensor_interfacing/CarlaPointCloudFilter/test/carla_pointcloud_filter_test.cpp new file mode 100644 index 00000000..e69de29b diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp deleted file mode 100644 index 77d08009..00000000 --- a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter.hpp +++ /dev/null @@ -1,9 +0,0 @@ -#ifndef RADAR_POINTCLOUD_FILTER_HPP_ -#define RADAR_POINTCLOUD_FILTER_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "radar_msgs/msg/UnfilteredRadarLeft.hpp" -#include "radar_msgs/msg/UnfilteredRadarRight.hpp" -#include "radar_msgs/msg/UnfilteredCarlaLeft.hpp" -#include "radar_msgs/msg/UnfilteredCarlaRight.hpp" diff --git a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp b/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp deleted file mode 100644 index dc51f01e..00000000 --- a/src/sensor_interfacing/radar_driver/include/radar_pointcloud_filter_node.hpp +++ /dev/null @@ -1,87 +0,0 @@ -#ifndef RADAR_POINTCLOUD_FILTER_NODE_HPP_ -#define RADAR_POINTCLOUD_FILTER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "radar_msgs/msg/UnfilteredRadarLeft.hpp" -#include "radar_msgs/msg/UnfilteredRadarRight.hpp" -#include "radar_msgs/msg/UnfilteredCarlaLeft.hpp" -#include "radar_msgs/msg/UnfilteredCarlaRight.hpp" - -#include "radar_pointcloud_filter.hpp" - - /** - * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar - * topics from CARLA ROS Bridge and ROSBags and publishes to "filtered" radar topic. - */ - -class PointCloudFilterNode : 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; - - /** - * PointCloudFilter Node Constructor. - */ - PointCloudFilterNode(); - -private: - /** - * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" - * topic - * - * @param msg a raw message from the "unfiltered" topic - */ - void unfiltered_radar_right_callback( - const radar_msgs::msg::UnfilteredRadarRight::SharedPtr msg); - - - /** - * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" - * topic - * - * @param msg a raw message from the "unfiltered" topic - */ - void unfiltered_radar_left_callback( - const radar_msgs::msg::UnfilteredRadarLeft::SharedPtr msg); - - - /** - * A ROS2 subscription node callback used to unpack raw CARLA radar data from - * "unfiltered" topics. - * - * @param msg a processed message from the "unfiltered" topic - */ - void unfiltered_carla_radar_left_callback( - const radar_msgs::msg::UnfilteredCarlaLeft::SharedPtr msg); - - - /** - * A ROS2 subscription node callback used to unpack raw CARLA radar data from - * "unfiltered" topics. - * - * @param msg a processed message from the "unfiltered" topic - */ - void unfiltered_carla_radar_right_callback( - const radar_msgs::msg::UnfilteredCarlaRight::SharedPtr msg); - - - // ROS2 Subscriber listening to the unfiltered radar left topic. - rclcpp::Subscription::SharedPtr raw_left_sub_ - - // ROS2 Subscriber listening to the unfiltered radar right topic. - rclcpp::Subscription::SharedPtr raw_right_sub_ - - // ROS2 Subscriber listening to the unfiltered Carla left topic (radar1). - rclcpp::Subscription::SharedPtr raw_carla_left_sub_ - - // ROS2 Subscriber listening to the unfiltered Carla right topic (radar2). - rclcpp::Subscription::SharedPtr raw_carla_right_sub_ - - // Add an object below from radar_pointcloud_filter.hpp that contains the methods - -}; - -#endif // RADAR_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py b/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py deleted file mode 100755 index f21337e2..00000000 --- a/src/sensor_interfacing/radar_driver/launch/radar_pointcloud_filter.launch.py +++ /dev/null @@ -1,14 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - """Launch PointCloudFilter node.""" - radar_pointcloud_filter_node = Node( - package='radar_pointcloud_filter', - executable='radar_pointcloud_filter_node', - ) - - return LaunchDescription([ - radar_pointcloud_filter_node - ]) diff --git a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp deleted file mode 100644 index f02fda5b..00000000 --- a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter.cpp +++ /dev/null @@ -1,3 +0,0 @@ -#include - -#include "radar_pointcloud_filter.hpp" \ No newline at end of file diff --git a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp b/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp deleted file mode 100644 index 5accb6d4..00000000 --- a/src/sensor_interfacing/radar_driver/src/radar_pointcloud_filter_node.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include -#include - -#include "radar_pointcloud_filter_node.hpp" - -PointCloudFilterNode::PointCloudFilterNode() -: Node("radar_point_cloud_filter") -{ - raw_left_sub_ = this->create_subscription( - "unfilteredRadarLeft", - std::bind( - &PointCloudFilterNode::unfiltered_radar_left_callback, this, - std::placeholders::_1)); - - raw_right_sub_ = this->create_subscription( - "unfilteredRadarRight", ADVERTISING_FREQ, - std::bind( - &PointCloudFilterNode::unfiltered_radar_right_callback, this, - std::placeholders::_1)); - - raw_carla_left_sub_ = this->create_subscription( - "unfilteredCarlaLeft", ADVERTISING_FREQ, - std::bind( - &PointCloudFilterNode::unfiltered_carla_radar_left_callback, this, - std::placeholders::_1)); - - raw_carla_right_sub_ = this->create_subscription( - "unfilteredCarlaRight", ADVERTISING_FREQ, - std::bind( - &PointCloudFilterNode::unfiltered_carla_radar_right_callback, this, - std::placeholders::_1)); -} - - void unfiltered_radar_right_callback( - const radar_msgs::msg::UnfilteredRadarRight::SharedPtr msg) - { - //messages from unfiltered right radar topic (rosbags) - } - - - void unfiltered_radar_left_callback( - const radar_msgs::msg::UnfilteredRadarLeft::SharedPtr msg) - { - //messages from unfiltered left radar topic (rosbags) - } - - - void unfiltered_carla_radar_left_callback( - const radar_msgs::msg::UnfilteredCarlaLeft::SharedPtr msg) - { - //messages from unfiltered left radar topic (CARLA ROS Bridge) - } - - - void unfiltered_carla_radar_right_callback( - const radar_msgs::msg::UnfilteredCarlaRight::SharedPtr msg) - { - //messages from unfiltered right radar topic (CARLA ROS Bridge) - } - -int main(int argc, char ** argv) -{ - rclcpp::init(argc,argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg b/src/wato_msgs/radar_msgs/msg/RadarDetection.msg similarity index 95% rename from src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg rename to src/wato_msgs/radar_msgs/msg/RadarDetection.msg index 403a0c67..9e937089 100644 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeft.msg +++ b/src/wato_msgs/radar_msgs/msg/RadarDetection.msg @@ -16,4 +16,4 @@ float32 SNR float32 Range float32 Prob0 float32 Prob1 -uint8 Pdh0 +uint8 Pdh0 \ No newline at end of file diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeftPacket.msg similarity index 100% rename from src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeft.msg rename to src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeftPacket.msg diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRightPacket.msg similarity index 100% rename from src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRight.msg rename to src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRightPacket.msg diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeftPacket.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeftPacket.msg new file mode 100644 index 00000000..1ccb1cd4 --- /dev/null +++ b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeftPacket.msg @@ -0,0 +1,8 @@ +Header header +uint8 EventID +uint32 TimeStamp +uint32 MeasurementCounter +float32 Vambig +float32 CenterFrequency +RadarDetection[] Detections + diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg deleted file mode 100644 index 403a0c67..00000000 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRight.msg +++ /dev/null @@ -1,19 +0,0 @@ -float32 posX -float32 posY -float32 posZ -float32 VrelRad -float32 AzAng0 -float32 AzAng1 -float32 ElAng -float32 RCS0 -float32 RCS1 -float32 RangeVar -float32 VrelRadVar -float32 AzAngVar0 -float32 AzAngVar1 -float32 ElAngVar -float32 SNR -float32 Range -float32 Prob0 -float32 Prob1 -uint8 Pdh0 diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg new file mode 100644 index 00000000..1ccb1cd4 --- /dev/null +++ b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg @@ -0,0 +1,8 @@ +Header header +uint8 EventID +uint32 TimeStamp +uint32 MeasurementCounter +float32 Vambig +float32 CenterFrequency +RadarDetection[] Detections + From 3dbd5213b5234ab1ff5ebca464bc850ad39c1a62 Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 7 Feb 2023 04:25:55 +0000 Subject: [PATCH 09/51] updated radar custom message types and implementation of pointcloud filters --- .../include/ars_pointcloud_filter.hpp | 27 ++++++++------ .../include/ars_pointcloud_filter_node.hpp | 19 ++++------ .../ARSPointCloudFilter/package.xml | 2 +- .../src/ars_pointcloud_filter.cpp | 37 +++++++++++++++---- .../src/ars_pointcloud_filter_node.cpp | 8 ++-- ...redRadarLeftPacket.msg => RadarPacket.msg} | 0 .../msg/UnfilteredCarlaLeftPacket.msg | 9 ----- .../msg/UnfilteredCarlaRightPacket.msg | 9 ----- .../msg/UnfilteredRadarRightPacket.msg | 8 ---- 9 files changed, 59 insertions(+), 60 deletions(-) rename src/wato_msgs/radar_msgs/msg/{UnfilteredRadarLeftPacket.msg => RadarPacket.msg} (100%) delete mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeftPacket.msg delete mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRightPacket.msg delete mode 100644 src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 44851ed3..7e7d82fa 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -3,21 +3,26 @@ #include "rclcpp/rclcpp.hpp" -#include "radar_msgs/msg/UnfilteredRadarLeftPacket.hpp" -#include "radar_msgs/msg/UnfilteredRadarRightPacket.hpp" -#include "radar_msgs/msg/UnfilteredCarlaLeftPacket.hpp" -#include "radar_msgs/msg/UnfilteredCarlaRightPacket.hpp" -#include "radar_msgs/msg/RadarDetection.hpp +#include "radar_msgs/msg/RadarPacket.hpp" +#include "radar_msgs/msg/RadarDetection.hpp" +class ARSPointCloudFilter +{ +public: + ARSPointCloudFilter(); -//SAMPLE FUNCTION + void snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_left, + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_right); -class PointCloudFilter -{ + void radar_velocity_filter(); -public: - void snr_filter(); + void azimuth_angle_filter(); + + void radar_cross_section_filter(); + + +private: -} +}; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 528bc2f6..271181c9 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -3,11 +3,8 @@ #include "rclcpp/rclcpp.hpp" -#include "radar_msgs/msg/UnfilteredRadarLeftPacket.hpp" -#include "radar_msgs/msg/UnfilteredRadarRightPacket.hpp" -#include "radar_msgs/msg/UnfilteredCarlaLeftPacket.hpp" -#include "radar_msgs/msg/UnfilteredCarlaRightPacket.hpp" -#include "radar_msgs/msg/RadarDetection.hpp +#include "radar_msgs/msg/RadarPacket.hpp" +#include "radar_msgs/msg/RadarDetection.hpp" #include "ars_pointcloud_filter.hpp" @@ -37,7 +34,7 @@ class ARSPointCloudFilterNode : public rclcpp:: Node * @param msg a raw message from the "unfiltered" topic */ void unfiltered_ars_radar_right_callback( - const radar_msgs::msg::UnfilteredRadarRightPacket::SharedPtr msg); + const radar_msgs::msg::RadarPacket::SharedPtr msg); /** @@ -47,14 +44,14 @@ class ARSPointCloudFilterNode : public rclcpp:: Node * @param msg a raw message from the "unfiltered" topic */ void unfiltered_ars_radar_left_callback( - const radar_msgs::msg::UnfilteredRadarLeftPacket::SharedPtr msg); + const radar_msgs::msg::RadarPacket::SharedPtr msg); - // ROS2 Subscriber listening to the unfiltered radar left topic. - rclcpp::Subscription::SharedPtr raw_left_sub_ + // ROS2 Subscriber listening to the unfiltered radar packet topic. + rclcpp::Subscription::SharedPtr raw_left_sub_ - // ROS2 Subscriber listening to the unfiltered radar right topic. - rclcpp::Subscription::SharedPtr raw_right_sub_ + // ROS2 Subscriber listening to the unfiltered radar packet topic. + rclcpp::Subscription::SharedPtr raw_right_sub_ // Add an object below from radar_pointcloud_filter.hpp that contains the methods (ADD LATER ONCE LOGIC CREATED) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/package.xml b/src/sensor_interfacing/ARSPointCloudFilter/package.xml index 8193c6f2..3f55f068 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/package.xml +++ b/src/sensor_interfacing/ARSPointCloudFilter/package.xml @@ -2,7 +2,7 @@ ars_pointcloud_filter 0.0.0 - A ROS package for pubsub communication + A ROS package for filtering ARS and Carla Radar Data Rijin Muralidharan TODO diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 43425d1c..b38b0165 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -1,13 +1,36 @@ #include -#include "radar_pointcloud_filter.hpp" +#include "ars_pointcloud_filter.hpp" -//Packets will depend on which parameter is selected (could be carla packets or hardware packets) -void PointCloudFilter :: snr_filter -{ - RadarDetection snr_filter(const RadarDetection& packet) +//Packets will depend on which node is selected to run (could be carla packets or ars radar packets) + +void ARSPointCloudFilter :: snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_left, + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_right) +{ + //create a new filtered message (based on UnfilteredRadarPacket message type (will be changed)) + radar_msgs::msg::RadarPacket::SharedPtr filtered_left; + double threshold = 12.0; + for (auto detection:unfiltered_ars_left->Detections) { - RadarDetection unfiltered_kace + // -> or dot operator? + if(detection.snr < threshold) + { + //push filtered point somewhere + } } -} \ No newline at end of file + return filtered_packets +} + +void ARSPointCloudFilter::radar_velocity_filter() +{ + + + + +} + +void ARSPointCloudFilter::azimuth_angle_filter() +{ + +} diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 5c4361e3..755536f6 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -7,13 +7,13 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() : Node("ars_point_cloud_filter") { - raw_left_sub_ = this->create_subscription( + raw_left_sub_ = this->create_subscription( "unfilteredRadarLeft", std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); - raw_right_sub_ = this->create_subscription( + raw_right_sub_ = this->create_subscription( "unfilteredRadarRight", ADVERTISING_FREQ, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, @@ -23,7 +23,7 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() void ARSPointCloudFilterNode :: unfiltered_ars_radar_right_callback( - const radar_msgs::msg::UnfilteredRadarRightPacket::SharedPtr msg) + const radar_msgs::msg::RadarPacket::SharedPtr msg) { //messages from unfiltered right radar topic (ars) @@ -31,7 +31,7 @@ void ARSPointCloudFilterNode :: unfiltered_ars_radar_right_callback( void ARSPointCloudFilterNode :: unfiltered_ars_radar_left_callback( - const radar_msgs::msg::UnfilteredRadarLeftPacket::SharedPtr msg) + const radar_msgs::msg::RadarPacket::SharedPtr msg) { //messages from unfiltered left radar topic (ars) } diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeftPacket.msg b/src/wato_msgs/radar_msgs/msg/RadarPacket.msg similarity index 100% rename from src/wato_msgs/radar_msgs/msg/UnfilteredRadarLeftPacket.msg rename to src/wato_msgs/radar_msgs/msg/RadarPacket.msg diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeftPacket.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeftPacket.msg deleted file mode 100644 index 9a9f788e..00000000 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaLeftPacket.msg +++ /dev/null @@ -1,9 +0,0 @@ -Header header -uint32 height -uint32 width -PointField[] fields -bool is_bigendian -uint32 point_step -uint32 row_step -uint8[] data -bool is_dense \ No newline at end of file diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRightPacket.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRightPacket.msg deleted file mode 100644 index 9a9f788e..00000000 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredCarlaRightPacket.msg +++ /dev/null @@ -1,9 +0,0 @@ -Header header -uint32 height -uint32 width -PointField[] fields -bool is_bigendian -uint32 point_step -uint32 row_step -uint8[] data -bool is_dense \ No newline at end of file diff --git a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg b/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg deleted file mode 100644 index 1ccb1cd4..00000000 --- a/src/wato_msgs/radar_msgs/msg/UnfilteredRadarRightPacket.msg +++ /dev/null @@ -1,8 +0,0 @@ -Header header -uint8 EventID -uint32 TimeStamp -uint32 MeasurementCounter -float32 Vambig -float32 CenterFrequency -RadarDetection[] Detections - From 1de1e5872ca65f2bbc615255a7bb4fd05bf5aa7c Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 8 Feb 2023 03:05:14 +0000 Subject: [PATCH 10/51] adde code ffor range, snr and angle filtrs and updated docker files --- docker/radar_driver/radar_driver.Dockerfile | 3 +- profiles/docker-compose.sensors.yaml | 2 +- .../include/ars_pointcloud_filter.hpp | 12 ++--- .../include/ars_pointcloud_filter_node.hpp | 1 + .../src/ars_pointcloud_filter.cpp | 51 ++++++++++++------- .../src/ars_pointcloud_filter_node.cpp | 1 + .../launch/carla_pointcloud_filter.launch.py | 0 7 files changed, 43 insertions(+), 27 deletions(-) mode change 100644 => 100755 src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py diff --git a/docker/radar_driver/radar_driver.Dockerfile b/docker/radar_driver/radar_driver.Dockerfile index 18f07633..c5992956 100644 --- a/docker/radar_driver/radar_driver.Dockerfile +++ b/docker/radar_driver/radar_driver.Dockerfile @@ -23,7 +23,8 @@ USER docker:docker RUN mkdir -p ~/ament_ws/src WORKDIR /home/docker/ament_ws/src -COPY src/sensor_interfacing/radar_driver src/radar_driver +COPY src/sensor_interfacing/ARSPointCloudFilter src/ARSPointCloudFilter + WORKDIR /home/docker/ament_ws RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ diff --git a/profiles/docker-compose.sensors.yaml b/profiles/docker-compose.sensors.yaml index 21b2b551..9d64b163 100644 --- a/profiles/docker-compose.sensors.yaml +++ b/profiles/docker-compose.sensors.yaml @@ -9,6 +9,6 @@ services: - "${RADAR_DRIVER_IMAGE:?}:develop" image: "${RADAR_DRIVER_IMAGE:?}:${TAG}" volumes: - - ./src/sensor_interfacing/radar_driver:/home/docker/ament_ws/src/radar_driver + - ./src/sensor_interfacing/ARSPointCloudFilter:/home/docker/ament_ws/src/ARSPointCloudFilter #command: roslaunch radar_driver radarROSbag.launch \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 7e7d82fa..0118da3d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -3,6 +3,7 @@ #include "rclcpp/rclcpp.hpp" +//importing custom message types #include "radar_msgs/msg/RadarPacket.hpp" #include "radar_msgs/msg/RadarDetection.hpp" @@ -12,15 +13,12 @@ class ARSPointCloudFilter public: ARSPointCloudFilter(); - void snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_left, - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_right); + void snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, float snr_threshold); - void radar_velocity_filter(); - - void azimuth_angle_filter(); - - void radar_cross_section_filter(); + void azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + float AzAng0_threshold, float AzAng1_threshold); + void range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, float range_threshold); private: diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 271181c9..ebf1027f 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -54,6 +54,7 @@ class ARSPointCloudFilterNode : public rclcpp:: Node rclcpp::Subscription::SharedPtr raw_right_sub_ // Add an object below from radar_pointcloud_filter.hpp that contains the methods (ADD LATER ONCE LOGIC CREATED) + }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index b38b0165..21b9fc8c 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -3,34 +3,49 @@ #include "ars_pointcloud_filter.hpp" -//Packets will depend on which node is selected to run (could be carla packets or ars radar packets) - -void ARSPointCloudFilter :: snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_left, - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars_right) +// unfiltered left and right ars (how is this combined)? +void ARSPointCloudFilter :: snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + float snr_threshold) { - //create a new filtered message (based on UnfilteredRadarPacket message type (will be changed)) - radar_msgs::msg::RadarPacket::SharedPtr filtered_left; - double threshold = 12.0; - for (auto detection:unfiltered_ars_left->Detections) + radar_msgs::msg::RadarPacket filtered_ars; + for (auto detection:unfiltered_ars->Detections) { - // -> or dot operator? - if(detection.snr < threshold) + if(detection->SNR < snr_threshold) { - //push filtered point somewhere + //an array of filtered objects + filtered_ars.Detections.push_back(detection); } } - return filtered_packets + return filtered_ars; } -void ARSPointCloudFilter::radar_velocity_filter() +void ARSPointCloudFilter::azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + float AzAng0_threshold, float AzAng1_threshold) { - - - + radar_msgs::msg::RadarPacket filtered_ars; + for (auto detection:unfiltered_ars->Detections) + { + if(abs(detection->AzAng0) < abs(AzAng0_threshold) && detection->AzAng1 < abs(AzAng1_threshold)) + { + //point angles are within the defined constraints + filtered_ars.Detections.push_back(detection); + } + } + return filtered_ars; } -void ARSPointCloudFilter::azimuth_angle_filter() +void ARSPointCloudFilter::range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + float range_threshold) { - + radar_msgs::msg::RadarPacket filtered_ars; + for (auto detection:unfiltered_ars->Detections) + { + if(detection->Range < range_threshold) + { + //an array of filtered objects + filtered_ars.Detections.push_back(detection); + } + } + return filtered_ars; } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 755536f6..e402a346 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -25,6 +25,7 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() void ARSPointCloudFilterNode :: unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { + //messages from unfiltered right radar topic (ars) } diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py b/src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py old mode 100644 new mode 100755 From 8c851b17bc29f50535cb75bfa9d80c85ff09e358 Mon Sep 17 00:00:00 2001 From: Rijin Date: Thu, 9 Feb 2023 02:35:05 +0000 Subject: [PATCH 11/51] fixed ros errors --- docker/radar_driver/radar_driver.Dockerfile | 4 +- .../ARSPointCloudFilter/CMakeLists.txt | 3 +- .../include/ars_pointcloud_filter.hpp | 14 +++---- .../include/ars_pointcloud_filter_node.hpp | 15 +++----- .../launch/ars_pointcloud_filter.launch.py | 2 +- .../src/ars_pointcloud_filter.cpp | 30 +++++++-------- .../src/ars_pointcloud_filter_node.cpp | 11 +++--- src/wato_msgs/radar_msgs/CMakeLists.txt | 26 +++++++++++++ .../radar_msgs/msg/RadarDetection.msg | 38 +++++++++---------- src/wato_msgs/radar_msgs/msg/RadarPacket.msg | 15 ++++---- src/wato_msgs/radar_msgs/package.xml | 20 ++++++++++ 11 files changed, 111 insertions(+), 67 deletions(-) create mode 100644 src/wato_msgs/radar_msgs/CMakeLists.txt create mode 100644 src/wato_msgs/radar_msgs/package.xml diff --git a/docker/radar_driver/radar_driver.Dockerfile b/docker/radar_driver/radar_driver.Dockerfile index c5992956..5ceeb2a2 100644 --- a/docker/radar_driver/radar_driver.Dockerfile +++ b/docker/radar_driver/radar_driver.Dockerfile @@ -24,7 +24,7 @@ RUN mkdir -p ~/ament_ws/src WORKDIR /home/docker/ament_ws/src COPY src/sensor_interfacing/ARSPointCloudFilter src/ARSPointCloudFilter - +COPY src/wato_msgs/radar_msgs radar_msgs WORKDIR /home/docker/ament_ws RUN . /opt/ros/$ROS_DISTRO/setup.sh && \ @@ -40,4 +40,4 @@ RUN sudo chmod +x ~/wato_ros_entrypoint.sh ENTRYPOINT ["/home/docker/wato_ros_entrypoint.sh"] -#CMD ["roslaunch", "--wait", "radar_driver", "radarROSbag.launch"] +CMD ["tail", "-f", "/dev/null"] diff --git a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt index 43ee51fc..7795264d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt +++ b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10) -project(ars_radar_pointcloud_filter) +project(ars_pointcloud_filter) # Set compiler to use C++ 17 standard if(NOT CMAKE_CXX_STANDARD) @@ -71,6 +71,7 @@ install(TARGETS # Copy launch files to installation location install(DIRECTORY launch + config DESTINATION share/${PROJECT_NAME}) ament_package() diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 0118da3d..1deb5d1d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -4,8 +4,8 @@ #include "rclcpp/rclcpp.hpp" //importing custom message types -#include "radar_msgs/msg/RadarPacket.hpp" -#include "radar_msgs/msg/RadarDetection.hpp" +#include "radar_msgs/msg/radar_packet.hpp" +#include "radar_msgs/msg/radar_detection.hpp" class ARSPointCloudFilter { @@ -13,14 +13,14 @@ class ARSPointCloudFilter public: ARSPointCloudFilter(); - void snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, float snr_threshold); + radar_msgs::msg::RadarPacket snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); - void azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - float AzAng0_threshold, float AzAng1_threshold); + radar_msgs::msg::RadarPacket azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double AzAng0_threshold, double AzAng1_threshold); - void range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, float range_threshold); + radar_msgs::msg::RadarPacket range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold); -private: }; +#endif diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index ebf1027f..2cd8b521 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -3,8 +3,9 @@ #include "rclcpp/rclcpp.hpp" -#include "radar_msgs/msg/RadarPacket.hpp" -#include "radar_msgs/msg/RadarDetection.hpp" +//ERROR +#include "radar_msgs/msg/radar_packet.hpp" +#include "radar_msgs/msg/radar_detection.hpp" #include "ars_pointcloud_filter.hpp" @@ -13,13 +14,9 @@ * topics from ARS ROSbags and publishes to "filtered" radar topic. */ -class ARSPointCloudFilterNode : public rclcpp:: Node +class ARSPointCloudFilterNode : 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; - /** * PointCloudFilter Node Constructor. */ @@ -48,10 +45,10 @@ class ARSPointCloudFilterNode : public rclcpp:: Node // ROS2 Subscriber listening to the unfiltered radar packet topic. - rclcpp::Subscription::SharedPtr raw_left_sub_ + rclcpp::Subscription::SharedPtr raw_left_sub_; // ROS2 Subscriber listening to the unfiltered radar packet topic. - rclcpp::Subscription::SharedPtr raw_right_sub_ + rclcpp::Subscription::SharedPtr raw_right_sub_; // Add an object below from radar_pointcloud_filter.hpp that contains the methods (ADD LATER ONCE LOGIC CREATED) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py index 089e8330..775aaceb 100755 --- a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py @@ -1,6 +1,6 @@ from launch import LaunchDescription from launch_ros.actions import Node -from launch.actions import LogInfo, DeclareLaunchArgument +from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch.conditions import LaunchConfigurationEquals diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 21b9fc8c..570a1681 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -4,47 +4,47 @@ // unfiltered left and right ars (how is this combined)? -void ARSPointCloudFilter :: snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - float snr_threshold) +radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double snr_threshold) { radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection:unfiltered_ars->Detections) + for (auto detection:unfiltered_ars->detections) { - if(detection->SNR < snr_threshold) + if(detection.snr < snr_threshold) { //an array of filtered objects - filtered_ars.Detections.push_back(detection); + filtered_ars.detections.push_back(detection); } } return filtered_ars; } -void ARSPointCloudFilter::azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - float AzAng0_threshold, float AzAng1_threshold) +radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double AzAng0_threshold, double AzAng1_threshold) { radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection:unfiltered_ars->Detections) + for (auto detection:unfiltered_ars->detections) { - if(abs(detection->AzAng0) < abs(AzAng0_threshold) && detection->AzAng1 < abs(AzAng1_threshold)) + if(abs(detection.az_ang0) < abs(AzAng0_threshold) && detection.az_ang1 < abs(AzAng1_threshold)) { //point angles are within the defined constraints - filtered_ars.Detections.push_back(detection); + filtered_ars.detections.push_back(detection); } } return filtered_ars; } -void ARSPointCloudFilter::range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - float range_threshold) +radar_msgs::msg::RadarPacket ARSPointCloudFilter::range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double range_threshold) { radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection:unfiltered_ars->Detections) + for (auto detection:unfiltered_ars->detections) { - if(detection->Range < range_threshold) + if(detection.range < range_threshold) { //an array of filtered objects - filtered_ars.Detections.push_back(detection); + filtered_ars.detections.push_back(detection); } } return filtered_ars; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index e402a346..989c68e5 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -8,13 +8,13 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() { raw_left_sub_ = this->create_subscription( - "unfilteredRadarLeft", + "unfilteredRadarLeft",1, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); raw_right_sub_ = this->create_subscription( - "unfilteredRadarRight", ADVERTISING_FREQ, + "unfilteredRadarRight", 1, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); @@ -22,18 +22,19 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() } -void ARSPointCloudFilterNode :: unfiltered_ars_radar_right_callback( +void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { - + RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); //messages from unfiltered right radar topic (ars) } -void ARSPointCloudFilterNode :: unfiltered_ars_radar_left_callback( +void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); //messages from unfiltered left radar topic (ars) } 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 index 9e937089..69807157 100644 --- a/src/wato_msgs/radar_msgs/msg/RadarDetection.msg +++ b/src/wato_msgs/radar_msgs/msg/RadarDetection.msg @@ -1,19 +1,19 @@ -float32 posX -float32 posY -float32 posZ -float32 VrelRad -float32 AzAng0 -float32 AzAng1 -float32 ElAng -float32 RCS0 -float32 RCS1 -float32 RangeVar -float32 VrelRadVar -float32 AzAngVar0 -float32 AzAngVar1 -float32 ElAngVar -float32 SNR -float32 Range -float32 Prob0 -float32 Prob1 -uint8 Pdh0 \ No newline at end of file +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 index 1ccb1cd4..43931644 100644 --- a/src/wato_msgs/radar_msgs/msg/RadarPacket.msg +++ b/src/wato_msgs/radar_msgs/msg/RadarPacket.msg @@ -1,8 +1,7 @@ -Header header -uint8 EventID -uint32 TimeStamp -uint32 MeasurementCounter -float32 Vambig -float32 CenterFrequency -RadarDetection[] Detections - +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 + + From faacc329d86fff9eb477804a97d8a511dcfaf8ef Mon Sep 17 00:00:00 2001 From: Rijin Date: Fri, 10 Feb 2023 03:08:31 +0000 Subject: [PATCH 12/51] modified ros launch file and updated code structure --- .../include/ars_pointcloud_filter.hpp | 10 ++++----- .../include/ars_pointcloud_filter_node.hpp | 6 +---- .../launch/ars_pointcloud_filter.launch.py | 22 +++++++++++-------- .../src/ars_pointcloud_filter.cpp | 18 +++++++-------- .../src/ars_pointcloud_filter_node.cpp | 11 ++++------ 5 files changed, 30 insertions(+), 37 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 1deb5d1d..806273bf 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -1,15 +1,14 @@ -#ifndef RADAR_POINTCLOUD_FILTER_HPP_ -#define RADAR_POINTCLOUD_FILTER_HPP_ +#ifndef ARS_POINTCLOUD_FILTER_HPP_ +#define ARS_POINTCLOUD_FILTER_HPP_ #include "rclcpp/rclcpp.hpp" -//importing custom message types +// importing custom message types #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" class ARSPointCloudFilter { - public: ARSPointCloudFilter(); @@ -20,7 +19,6 @@ class ARSPointCloudFilter radar_msgs::msg::RadarPacket range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold); - }; -#endif +#endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 2cd8b521..e884b761 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -23,7 +23,6 @@ class ARSPointCloudFilterNode : public rclcpp::Node ARSPointCloudFilterNode(); private: - /** * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" * topic @@ -50,11 +49,8 @@ class ARSPointCloudFilterNode : public rclcpp::Node // ROS2 Subscriber listening to the unfiltered radar packet topic. rclcpp::Subscription::SharedPtr raw_right_sub_; - // Add an object below from radar_pointcloud_filter.hpp that contains the methods (ADD LATER ONCE LOGIC CREATED) + // Add an object below from radar_pointcloud_filter.hpp that contains the methods - }; - - #endif // ARS_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py index 775aaceb..46d50473 100755 --- a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py @@ -4,16 +4,20 @@ from launch.substitutions import LaunchConfiguration from launch.conditions import LaunchConfigurationEquals - - def generate_launch_description(): """Launch ARSPointCloudFilter node.""" + + ars_pointcloud_filter_param = DeclareLaunchArgument( + 'filter_mode', default_value='ars') + + ars_pointcloud_filter_node = Node( + LaunchConfiguration('filter_mode'), + condition=LaunchConfigurationEquals('filter_mode', 'ars'), + package='ars_pointcloud_filter', + executable='ars_pointcloud_filter_node', + ) + return LaunchDescription([ - DeclareLaunchArgument('filter_mode', default_value='ars'), - Node( - LaunchConfiguration('filter_mode'), - condition=LaunchConfigurationEquals('filter_mode', 'ars'), - package = 'ars_pointcloud_filter', - executable = 'ars_pointcloud_filter_node' - ) + ars_pointcloud_filter_param, + ars_pointcloud_filter_node ]) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 570a1681..89060174 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -3,16 +3,15 @@ #include "ars_pointcloud_filter.hpp" -// unfiltered left and right ars (how is this combined)? radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) -{ +{ radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection:unfiltered_ars->detections) + for (auto detection : unfiltered_ars->detections) { if(detection.snr < snr_threshold) { - //an array of filtered objects + // an array of filtered objects filtered_ars.detections.push_back(detection); } } @@ -20,30 +19,29 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter(const radar_msgs::m } radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double AzAng0_threshold, double AzAng1_threshold) + double AzAng0_threshold, double AzAng1_threshold) { radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection:unfiltered_ars->detections) + for (auto detection : unfiltered_ars->detections) { if(abs(detection.az_ang0) < abs(AzAng0_threshold) && detection.az_ang1 < abs(AzAng1_threshold)) { - //point angles are within the defined constraints + // point angles are within the defined constraints filtered_ars.detections.push_back(detection); } } return filtered_ars; - } radar_msgs::msg::RadarPacket ARSPointCloudFilter::range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold) { radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection:unfiltered_ars->detections) + for (auto detection : unfiltered_ars->detections) { if(detection.range < range_threshold) { - //an array of filtered objects + // an array of filtered objects filtered_ars.detections.push_back(detection); } } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 989c68e5..ba1849f8 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -6,9 +6,8 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() : Node("ars_point_cloud_filter") { - raw_left_sub_ = this->create_subscription( - "unfilteredRadarLeft",1, + "unfilteredRadarLeft",1 , std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); @@ -21,26 +20,24 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() } - void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - //messages from unfiltered right radar topic (ars) + // messages from unfiltered right radar topic (ars) } - void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - //messages from unfiltered left radar topic (ars) + // messages from unfiltered left radar topic (ars) } int main(int argc, char ** argv) { - rclcpp::init(argc,argv); + rclcpp::init(argc, argv); rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; From 29f3cb6b51a348a7d1170e7a53598234f1830f77 Mon Sep 17 00:00:00 2001 From: Rijin Date: Sat, 11 Feb 2023 05:01:29 +0000 Subject: [PATCH 13/51] modified filter code and updated code structure --- .../include/ars_pointcloud_filter.hpp | 19 +- .../include/ars_pointcloud_filter_node.hpp | 49 +++--- .../launch/ars_pointcloud_filter.launch.py | 4 +- .../src/ars_pointcloud_filter.cpp | 163 ++++++++++++++---- .../src/ars_pointcloud_filter_node.cpp | 9 +- 5 files changed, 170 insertions(+), 74 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 806273bf..8c802269 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -12,13 +12,22 @@ class ARSPointCloudFilter public: ARSPointCloudFilter(); - radar_msgs::msg::RadarPacket snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); + radar_msgs::msg::RadarPacket snr_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); - radar_msgs::msg::RadarPacket azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double AzAng0_threshold, double AzAng1_threshold); + radar_msgs::msg::RadarPacket azimuth_angle_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold); - radar_msgs::msg::RadarPacket range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold); + radar_msgs::msg::RadarPacket range_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold); -}; + radar_msgs::msg::RadarPacket vrel_rad_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold); + + radar_msgs::msg::RadarPacket el_ang_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold); + radar_msgs::msg::RadarPacket rcs_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold); +}; #endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index e884b761..d4d2f2df 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -2,47 +2,41 @@ #define ARS_POINTCLOUD_FILTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" - -//ERROR #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" - #include "ars_pointcloud_filter.hpp" - /** - * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar - * topics from ARS ROSbags and publishes to "filtered" radar topic. - */ - +/** +* Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar +* topics from ARS ROSbags and publishes to "filtered" radar topic. +*/ class ARSPointCloudFilterNode : public rclcpp::Node { public: - /** - * PointCloudFilter Node Constructor. - */ + /** + * PointCloudFilter Node Constructor. + */ ARSPointCloudFilterNode(); private: - /** - * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" - * topic - * - * @param msg a raw message from the "unfiltered" topic - */ - void unfiltered_ars_radar_right_callback( + /** + * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * topic + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg); - - /** - * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" - * topic - * - * @param msg a raw message from the "unfiltered" topic - */ - void unfiltered_ars_radar_left_callback( + /** + * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * topic + * + * @param msg a raw message from the "unfiltered" topic + */ + void unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg); - // ROS2 Subscriber listening to the unfiltered radar packet topic. rclcpp::Subscription::SharedPtr raw_left_sub_; @@ -50,7 +44,6 @@ class ARSPointCloudFilterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr raw_right_sub_; // Add an object below from radar_pointcloud_filter.hpp that contains the methods - }; #endif // ARS_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py index 46d50473..e97c1eb9 100755 --- a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py @@ -4,11 +4,11 @@ from launch.substitutions import LaunchConfiguration from launch.conditions import LaunchConfigurationEquals + def generate_launch_description(): """Launch ARSPointCloudFilter node.""" - ars_pointcloud_filter_param = DeclareLaunchArgument( - 'filter_mode', default_value='ars') + 'filter_mode', default_value='ars') ars_pointcloud_filter_node = Node( LaunchConfiguration('filter_mode'), diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 89060174..4278b3d0 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -3,47 +3,144 @@ #include "ars_pointcloud_filter.hpp" -radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double snr_threshold) -{ - radar_msgs::msg::RadarPacket filtered_ars; +// Filter Template + +radar_msgs::msg::RadarPacket ARSPointCloudFilter::main_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double threshold, double filter_type) + { + radar_msgs::msg::RadarPacket main_filtered_ars; for (auto detection : unfiltered_ars->detections) { - if(detection.snr < snr_threshold) - { - // an array of filtered objects - filtered_ars.detections.push_back(detection); - } + if(detection.{filter_type?} < threshold) + { + // Push filtered point/detection into an array + main_filtered_ars.detections.push_back(detection); + } } - return filtered_ars; -} + return main_filtered_ars; + } -radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double AzAng0_threshold, double AzAng1_threshold) +// SNR Filter +radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) { - radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection : unfiltered_ars->detections) - { - if(abs(detection.az_ang0) < abs(AzAng0_threshold) && detection.az_ang1 < abs(AzAng1_threshold)) - { - // point angles are within the defined constraints - filtered_ars.detections.push_back(detection); - } - } + //filter out only nearscan packets + if (unfiltered_ars->event_id == 12) + { + radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, snr_threshold, snr) return filtered_ars; + } } -radar_msgs::msg::RadarPacket ARSPointCloudFilter::range_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double range_threshold) +// Azimuth Angle Filter +radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold) { - radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection : unfiltered_ars->detections) - { - if(detection.range < range_threshold) - { - // an array of filtered objects - filtered_ars.detections.push_back(detection); - } - } + //filter out only nearscan packets + if (unfiltered_ars->event_id == 12) + { + radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, AzAng0_threshold, az_ang0) return filtered_ars; + } } + + + + +/// ORIGINAL IMPLEMENTATION +// // SNR Filter +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars; +// for (auto detection : unfiltered_ars->detections) +// { +// if(detection.snr < snr_threshold) +// { +// // Push filtered point/detection into an array +// filtered_ars.detections.push_back(detection); +// } +// } +// return filtered_ars; +// } + +// // Azimuth Angle Filter +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars; +// for (auto detection : unfiltered_ars->detections) +// { +// if(abs(detection.az_ang0) < abs(AzAng0_threshold)) +// { +// // Point angles are within the defined constraints +// filtered_ars.detections.push_back(detection); +// } +// } +// return filtered_ars; +// } + +// // Range Filter +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::range_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars; +// for (auto detection : unfiltered_ars->detections) +// { +// if(detection.range < range_threshold) +// { +// // an array of filtered objects +// filtered_ars.detections.push_back(detection); +// } +// } +// return filtered_ars; +// } + +// // Relative Radial Velocity +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::vrel_rad_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars; +// for (auto detection : unfiltered_ars->detections) +// { +// if(detection.vrel_rad < vrel_rad_threshold) +// { +// // an array of filtered objects +// filtered_ars.detections.push_back(detection); +// } +// } +// return filtered_ars; +// } + +// // Elevation Angle +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::el_ang_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars; +// for (auto detection : unfiltered_ars->detections) +// { +// if(detection.el_ang < el_ang_threshold) +// { +// // an array of filtered objects +// filtered_ars.detections.push_back(detection); +// } +// } +// return filtered_ars; +// } + +// // Radar Cross Section +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::rcs_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars; +// for (auto detection : unfiltered_ars->detections) +// { +// if(detection.rcs0 < rcs_threshold) +// { +// // an array of filtered objects +// filtered_ars.detections.push_back(detection); +// } +// } +// return filtered_ars; +// } + diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index ba1849f8..a2c8098d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -3,21 +3,19 @@ #include "ars_pointcloud_filter_node.hpp" -ARSPointCloudFilterNode::ARSPointCloudFilterNode() -: Node("ars_point_cloud_filter") +ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") { raw_left_sub_ = this->create_subscription( - "unfilteredRadarLeft",1 , + "unfilteredRadarLeft", 1 , std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); raw_right_sub_ = this->create_subscription( - "unfilteredRadarRight", 1, + "unfilteredRadarRight", 1 , std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); - } void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( @@ -25,7 +23,6 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); // messages from unfiltered right radar topic (ars) - } void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( From 41c4de36189310acd9124d4d194ed7ce0bb1744e Mon Sep 17 00:00:00 2001 From: Rijin Date: Sun, 12 Feb 2023 01:38:05 +0000 Subject: [PATCH 14/51] creating a generic test filter --- .../src/ars_pointcloud_filter.cpp | 71 +++++++++++++------ 1 file changed, 50 insertions(+), 21 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 4278b3d0..46a26377 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -3,15 +3,16 @@ #include "ars_pointcloud_filter.hpp" -// Filter Template + +// TEST - Filter Template radar_msgs::msg::RadarPacket ARSPointCloudFilter::main_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double threshold, double filter_type) + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double threshold, &filter_type) { radar_msgs::msg::RadarPacket main_filtered_ars; for (auto detection : unfiltered_ars->detections) { - if(detection.{filter_type?} < threshold) + if(filter_type(detection) < threshold) { // Push filtered point/detection into an array main_filtered_ars.detections.push_back(detection); @@ -20,32 +21,60 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::main_filter( return main_filtered_ars; } -// SNR Filter +// TEST - LAMBDA - SNR Filter radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) { - //filter out only nearscan packets - if (unfiltered_ars->event_id == 12) - { - radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, snr_threshold, snr) - return filtered_ars; - } -} + auto filter_type=[snr_threshold](const radar_msgs::msg::RadarPacket::SharedPtr test_packet){ + return test_packet->detections.detection.snr < snr_threshold; + }; + + radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, snr_threshold, filter_type) + return filtered_ars; -// Azimuth Angle Filter -radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold) -{ - //filter out only nearscan packets - if (unfiltered_ars->event_id == 12) - { - radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, AzAng0_threshold, az_ang0) - return filtered_ars; - } } +// // Filter Template + +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::main_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double threshold, double filter_type) +// { +// radar_msgs::msg::RadarPacket main_filtered_ars; +// for (auto detection : unfiltered_ars->detections) +// { +// if(detection.{filter_type?} < threshold) +// { +// // Push filtered point/detection into an array +// main_filtered_ars.detections.push_back(detection); +// } +// } +// return main_filtered_ars; +// } + + + +// // SNR Filter +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, snr_threshold, snr) +// return filtered_ars; + +// } + +// // Azimuth Angle Filter +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold) +// { +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, AzAng0_threshold, az_ang0) +// return filtered_ars; + +// } + + + /// ORIGINAL IMPLEMENTATION // // SNR Filter From b2aaae3545a549931122d05572cb0469228a5633 Mon Sep 17 00:00:00 2001 From: Rijin Date: Mon, 13 Feb 2023 04:28:45 +0000 Subject: [PATCH 15/51] updated radarpointcloud filter node --- docker/radar_driver/radar_driver.Dockerfile | 2 +- profiles/docker-compose.sensors.yaml | 2 +- .../config/ars_radar_params.yaml | 6 + .../include/ars_pointcloud_filter.hpp | 34 ++-- .../include/ars_pointcloud_filter_node.hpp | 1 + .../src/ars_pointcloud_filter.cpp | 152 +++++++++++++----- .../src/ars_pointcloud_filter_node.cpp | 23 +++ 7 files changed, 166 insertions(+), 54 deletions(-) diff --git a/docker/radar_driver/radar_driver.Dockerfile b/docker/radar_driver/radar_driver.Dockerfile index 5ceeb2a2..4b22730c 100644 --- a/docker/radar_driver/radar_driver.Dockerfile +++ b/docker/radar_driver/radar_driver.Dockerfile @@ -23,7 +23,7 @@ USER docker:docker RUN mkdir -p ~/ament_ws/src WORKDIR /home/docker/ament_ws/src -COPY src/sensor_interfacing/ARSPointCloudFilter src/ARSPointCloudFilter +COPY src/sensor_interfacing/ARSPointCloudFilter ARSPointCloudFilter COPY src/wato_msgs/radar_msgs radar_msgs WORKDIR /home/docker/ament_ws diff --git a/profiles/docker-compose.sensors.yaml b/profiles/docker-compose.sensors.yaml index 9d64b163..2cfc1be9 100644 --- a/profiles/docker-compose.sensors.yaml +++ b/profiles/docker-compose.sensors.yaml @@ -9,6 +9,6 @@ services: - "${RADAR_DRIVER_IMAGE:?}:develop" image: "${RADAR_DRIVER_IMAGE:?}:${TAG}" volumes: - - ./src/sensor_interfacing/ARSPointCloudFilter:/home/docker/ament_ws/src/ARSPointCloudFilter + - ../src/sensor_interfacing/ARSPointCloudFilter:/home/docker/ament_ws/src/ARSPointCloudFilter #command: roslaunch radar_driver radarROSbag.launch \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml index f5b401b2..0d334ec1 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml +++ b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml @@ -1,3 +1,9 @@ ars_point_cloud_filter: ros__parameters: filter_mode: "ars" + vrel_rad: -99999 + el_ang: -99999 + rcs0: -99999 + snr: -99999 + range: -99999 + az_ang0: -99999 \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 8c802269..202815dd 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -12,22 +12,32 @@ class ARSPointCloudFilter public: ARSPointCloudFilter(); - radar_msgs::msg::RadarPacket snr_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); + // point_filter - Solution 1 + radar_msgs::msg::RadarPacket point_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double snr_threshold, + double AzAng0_threshold, + double range_threshold, + double vrel_rad_threshold, + double el_ang_threshold, + double rcs_threshold); - radar_msgs::msg::RadarPacket azimuth_angle_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold); + // radar_msgs::msg::RadarPacket snr_filter( + // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); - radar_msgs::msg::RadarPacket range_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold); + // radar_msgs::msg::RadarPacket azimuth_angle_filter( + // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold); - radar_msgs::msg::RadarPacket vrel_rad_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold); + // radar_msgs::msg::RadarPacket range_filter( + // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold); - radar_msgs::msg::RadarPacket el_ang_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold); + // radar_msgs::msg::RadarPacket vrel_rad_filter( + // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold); - radar_msgs::msg::RadarPacket rcs_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold); + // radar_msgs::msg::RadarPacket el_ang_filter( + // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold); + + // radar_msgs::msg::RadarPacket rcs_filter( + // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold); }; #endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index d4d2f2df..b18a3b89 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -44,6 +44,7 @@ class ARSPointCloudFilterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr raw_right_sub_; // Add an object below from radar_pointcloud_filter.hpp that contains the methods + ARSPointCloudFilter pointcloudfilter_; }; #endif // ARS_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 46a26377..83990bea 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -2,49 +2,63 @@ #include "ars_pointcloud_filter.hpp" +// SOLUTION 1: Via Conditions and ROS parameters -// TEST - Filter Template - -radar_msgs::msg::RadarPacket ARSPointCloudFilter::main_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double threshold, &filter_type) - { - radar_msgs::msg::RadarPacket main_filtered_ars; - for (auto detection : unfiltered_ars->detections) +radar_msgs::msg::RadarPacket point_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double snr_threshold, + double AzAng0_threshold, + double range_threshold, + double vrel_rad_threshold, + double el_ang_threshold, + double rcs_threshold) { - if(filter_type(detection) < threshold) + radar_msgs::msg::RadarPacket filtered_ars; + for (auto detection : unfiltered_ars->detections) { - // Push filtered point/detection into an array - main_filtered_ars.detections.push_back(detection); + if(detection.snr < snr_threshold) + { + continue; + } + else if (detection.az_ang0 < AzAng0_threshold) + { + continue; + } + else if (detection.range < range_threshold) + { + continue; + } + else if (detection.vrel_rad < vrel_rad_threshold) + { + continue; + } + else if (detection.el_ang < el_ang_threshold) + { + continue; + } + else if (detection.rcs0 < rcs_threshold) + { + continue; + } + filtered_ars.detections.push_back(detection); } + return filtered_ars; } - return main_filtered_ars; - } - -// TEST - LAMBDA - SNR Filter -radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) -{ - auto filter_type=[snr_threshold](const radar_msgs::msg::RadarPacket::SharedPtr test_packet){ - return test_packet->detections.detection.snr < snr_threshold; - }; - - radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, snr_threshold, filter_type) - return filtered_ars; -} +// // SOLUTION 2: Via Lambda Functions +// // LAMDA - Filter Template -// // Filter Template - -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::main_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double threshold, double filter_type) +// radar_msgs::msg::RadarPacket main_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, +// const std::function &filter_type) // { // radar_msgs::msg::RadarPacket main_filtered_ars; // for (auto detection : unfiltered_ars->detections) // { -// if(detection.{filter_type?} < threshold) +// if(filter_type(detection)) // { // // Push filtered point/detection into an array // main_filtered_ars.detections.push_back(detection); @@ -53,30 +67,88 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( // return main_filtered_ars; // } - - -// // SNR Filter +// // LAMBDA - SNR Filter // radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) // { -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, snr_threshold, snr) -// return filtered_ars; +// auto filter_type=[snr_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { +// return test_detection.snr < snr_threshold; +// }; + +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); +// return filtered_ars; // } -// // Azimuth Angle Filter +// // LAMBDA - Azimuth Angle Filter // radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter( // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold) // { -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, AzAng0_threshold, az_ang0) -// return filtered_ars; - +// auto filter_type=[AzAng0_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { +// return test_detection.az_ang0 < AzAng0_threshold; +// }; + +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); +// return filtered_ars; + +// } + +// // LAMBDA - Range Filter +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::range_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold) +// { +// auto filter_type=[range_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { +// return test_detection.range < range_threshold; +// }; + +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); +// return filtered_ars; + // } +// // LAMBDA - Relative Radial Velocity +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::vrel_rad_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold) +// { +// auto filter_type=[vrel_rad_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { +// return test_detection.snr < vrel_rad_threshold; +// }; + +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); +// return filtered_ars; +// } + +// // LAMBDA - Elevation Angle +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::el_ang_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold) +// { +// auto filter_type=[el_ang_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { +// return test_detection.snr < el_ang_threshold; +// }; + +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); +// return filtered_ars; +// } + +// // LAMBDA - Radar Cross Section +// radar_msgs::msg::RadarPacket ARSPointCloudFilter::rcs_filter( +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold) +// { +// auto filter_type=[rcs_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { +// return test_detection.snr < rcs_threshold; +// }; + +// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); +// return filtered_ars; +// } + + + -/// ORIGINAL IMPLEMENTATION +// ------------------------------ +// // ORIGINAL IMPLEMENTATION // // SNR Filter // radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) @@ -143,7 +215,7 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( // // Elevation Angle // radar_msgs::msg::RadarPacket ARSPointCloudFilter::el_ang_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold) +// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold) // { // radar_msgs::msg::RadarPacket filtered_ars; // for (auto detection : unfiltered_ars->detections) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index a2c8098d..103f58d6 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -5,6 +5,15 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") { + //default values already declared in ars_radar_params.yaml + this->declare_parameter("filter_mode"); + this->declare_parameter("vrel_rad"); + this->declare_parameter("el_ang"); + this->declare_parameter("rcs0"); + this->declare_parameter("snr"); + this->declare_parameter("range"); + this->declare_parameter("az_ang0"); + raw_left_sub_ = this->create_subscription( "unfilteredRadarLeft", 1 , std::bind( @@ -16,6 +25,7 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); + } void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( @@ -23,6 +33,19 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); // messages from unfiltered right radar topic (ars) + + rclcpp::Parameter mode_param = this->get_parameter("filter_mode"); + rclcpp::Parameter vrel_rad_param = this->get_parameter("vrel_rad").as_double(); + rclcpp::Parameter el_ang_param = this->get_parameter("el_ang").as_double(); + rclcpp::Parameter rcs0_param = this->get_parameter("rcs0").as_double(); + rclcpp::Parameter snr_param = this->get_parameter("snr").as_double(); + rclcpp::Parameter range_param = this->get_parameter("range").as_double(); + rclcpp::Parameter az_ang0_param = this->get_parameter("az_ang0".as_double()); + + // Send unfiltered packets along with set thresholds to the filter + pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,az_ang0_param, + el_ang_param,rcs0_param); + } void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( From 58bbaf863a1787bd69c2f0a6f032ed3b7b8e6f7a Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 14 Feb 2023 04:06:46 +0000 Subject: [PATCH 16/51] updated pointcloud filter node files --- .../config/ars_radar_params.yaml | 12 +++++------ .../src/ars_pointcloud_filter.cpp | 6 +++++- .../src/ars_pointcloud_filter_node.cpp | 20 +++++++++++-------- 3 files changed, 23 insertions(+), 15 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml index 0d334ec1..c51cdd55 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml +++ b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml @@ -1,9 +1,9 @@ ars_point_cloud_filter: ros__parameters: filter_mode: "ars" - vrel_rad: -99999 - el_ang: -99999 - rcs0: -99999 - snr: -99999 - range: -99999 - az_ang0: -99999 \ No newline at end of file + vrel_rad: -99999.99 + el_ang: -99999.99 + rcs0: -99999.99 + snr: -99999.99 + range: -99999.99 + az_ang0: -99999.99 \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 83990bea..10690309 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -4,6 +4,10 @@ // SOLUTION 1: Via Conditions and ROS parameters +ARSPointCloudFilter::ARSPointCloudFilter() +{ + +} radar_msgs::msg::RadarPacket point_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, @@ -147,7 +151,7 @@ radar_msgs::msg::RadarPacket point_filter( -// ------------------------------ +// // ------------------------------ // // ORIGINAL IMPLEMENTATION // // SNR Filter // radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 103f58d6..7e1abbc6 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -2,6 +2,9 @@ #include #include "ars_pointcloud_filter_node.hpp" +#include "ars_pointcloud_filter.cpp" + + ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") { @@ -34,16 +37,17 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); // messages from unfiltered right radar topic (ars) - rclcpp::Parameter mode_param = this->get_parameter("filter_mode"); - rclcpp::Parameter vrel_rad_param = this->get_parameter("vrel_rad").as_double(); - rclcpp::Parameter el_ang_param = this->get_parameter("el_ang").as_double(); - rclcpp::Parameter rcs0_param = this->get_parameter("rcs0").as_double(); - rclcpp::Parameter snr_param = this->get_parameter("snr").as_double(); - rclcpp::Parameter range_param = this->get_parameter("range").as_double(); - rclcpp::Parameter az_ang0_param = this->get_parameter("az_ang0".as_double()); + std::string mode_param = this->get_parameter("filter_mode").as_string(); + double vrel_rad_param = this->get_parameter("vrel_rad").as_double(); + double el_ang_param = this->get_parameter("el_ang").as_double(); + double rcs0_param = this->get_parameter("rcs0").as_double(); + double snr_param = this->get_parameter("snr").as_double(); + double range_param = this->get_parameter("range").as_double(); + double az_ang0_param = this->get_parameter("az_ang0").as_double(); // Send unfiltered packets along with set thresholds to the filter - pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,az_ang0_param, + + pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param, el_ang_param,rcs0_param); } From a0632c341ef777cf5d4337ea117f291a07a0058c Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 15 Feb 2023 02:06:50 +0000 Subject: [PATCH 17/51] updated pointcloud filter node ROS files --- .../include/ars_pointcloud_filter.hpp | 4 ++++ .../include/ars_pointcloud_filter_node.hpp | 6 +++++- .../src/ars_pointcloud_filter.cpp | 4 ++++ .../src/ars_pointcloud_filter_node.cpp | 15 +++++++++------ 4 files changed, 22 insertions(+), 7 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 202815dd..313311ad 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -7,6 +7,9 @@ #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" +namespace filtering +{ + class ARSPointCloudFilter { public: @@ -40,4 +43,5 @@ class ARSPointCloudFilter // radar_msgs::msg::RadarPacket rcs_filter( // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold); }; +} #endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index b18a3b89..650bb132 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -2,10 +2,13 @@ #define ARS_POINTCLOUD_FILTER_NODE_HPP_ #include "rclcpp/rclcpp.hpp" + #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" + #include "ars_pointcloud_filter.hpp" + /** * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar * topics from ARS ROSbags and publishes to "filtered" radar topic. @@ -44,7 +47,8 @@ class ARSPointCloudFilterNode : public rclcpp::Node rclcpp::Subscription::SharedPtr raw_right_sub_; // Add an object below from radar_pointcloud_filter.hpp that contains the methods - ARSPointCloudFilter pointcloudfilter_; + filtering::ARSPointCloudFilter pointcloudfilter_; }; + #endif // ARS_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 10690309..da387f34 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -4,6 +4,9 @@ // SOLUTION 1: Via Conditions and ROS parameters +namespace filtering +{ + ARSPointCloudFilter::ARSPointCloudFilter() { @@ -249,3 +252,4 @@ radar_msgs::msg::RadarPacket point_filter( // return filtered_ars; // } +} // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 7e1abbc6..d67a17ff 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -2,7 +2,6 @@ #include #include "ars_pointcloud_filter_node.hpp" -#include "ars_pointcloud_filter.cpp" @@ -34,8 +33,9 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { + RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - // messages from unfiltered right radar topic (ars) + // messages from unfiltered right radar topic (ars) std::string mode_param = this->get_parameter("filter_mode").as_string(); double vrel_rad_param = this->get_parameter("vrel_rad").as_double(); @@ -45,10 +45,13 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( double range_param = this->get_parameter("range").as_double(); double az_ang0_param = this->get_parameter("az_ang0").as_double(); - // Send unfiltered packets along with set thresholds to the filter - - pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param, - el_ang_param,rcs0_param); + // Send unfiltered packets along with set parameter thresholds to the filter + + // RCLCPP_INFO(this->get_logger(), "Publishing: %d\n", + // ARSPointCloudFilterNode::pointcloudfilter_.point_filter + // (msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param).event_id); + + pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); } From 3850a0f8438123448ea615ed08ab298ccd6ed9f3 Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 15 Feb 2023 02:55:47 +0000 Subject: [PATCH 18/51] resolved node build errors --- .../ARSPointCloudFilter/src/ars_pointcloud_filter.cpp | 2 +- .../src/ars_pointcloud_filter_node.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index da387f34..25b7a392 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -12,7 +12,7 @@ ARSPointCloudFilter::ARSPointCloudFilter() } -radar_msgs::msg::RadarPacket point_filter( +radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, double AzAng0_threshold, diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index d67a17ff..19fff5e4 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -47,11 +47,11 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( // Send unfiltered packets along with set parameter thresholds to the filter - // RCLCPP_INFO(this->get_logger(), "Publishing: %d\n", - // ARSPointCloudFilterNode::pointcloudfilter_.point_filter - // (msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param).event_id); + RCLCPP_INFO(this->get_logger(), "Publishing: %d\n", + ARSPointCloudFilterNode::pointcloudfilter_.point_filter + (msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param).event_id); - pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); + // pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); } From 4fb221f98fdf0bb4dc7e23fdee576e6870b9fcb5 Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 15 Feb 2023 03:21:23 +0000 Subject: [PATCH 19/51] updated radar left and right unfiltered callbacks --- .../config/ars_radar_params.yaml | 3 ++- .../launch/ars_pointcloud_filter.launch.py | 16 ++++++++++++++ .../src/ars_pointcloud_filter_node.cpp | 22 +++++++++++++------ 3 files changed, 33 insertions(+), 8 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml index c51cdd55..d4fe8248 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml +++ b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml @@ -6,4 +6,5 @@ ars_point_cloud_filter: rcs0: -99999.99 snr: -99999.99 range: -99999.99 - az_ang0: -99999.99 \ No newline at end of file + az_ang0: -99999.99 + scan: "near" diff --git a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py index e97c1eb9..aa518ac8 100755 --- a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py @@ -15,9 +15,25 @@ def generate_launch_description(): condition=LaunchConfigurationEquals('filter_mode', 'ars'), package='ars_pointcloud_filter', executable='ars_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': 'near'} + ] ) return LaunchDescription([ ars_pointcloud_filter_param, ars_pointcloud_filter_node ]) + vrel_rad: -99999.99 + el_ang: -99999.99 + rcs0: -99999.99 + snr: -99999.99 + range: -99999.99 + az_ang0: -99999.99 + scan: "near" \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 19fff5e4..8b46ccd7 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -9,6 +9,7 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter { //default values already declared in ars_radar_params.yaml this->declare_parameter("filter_mode"); + this->declare_parameter("scan"); this->declare_parameter("vrel_rad"); this->declare_parameter("el_ang"); this->declare_parameter("rcs0"); @@ -37,7 +38,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); // messages from unfiltered right radar topic (ars) - std::string mode_param = this->get_parameter("filter_mode").as_string(); + std::string scan_param = this->get_parameter("scan").as_string(); double vrel_rad_param = this->get_parameter("vrel_rad").as_double(); double el_ang_param = this->get_parameter("el_ang").as_double(); double rcs0_param = this->get_parameter("rcs0").as_double(); @@ -46,12 +47,19 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( double az_ang0_param = this->get_parameter("az_ang0").as_double(); // Send unfiltered packets along with set parameter thresholds to the filter - - RCLCPP_INFO(this->get_logger(), "Publishing: %d\n", - ARSPointCloudFilterNode::pointcloudfilter_.point_filter - (msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param).event_id); - - // pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); + if(scan_param == "near") + { + if(msg->event_id == 12) + { + + const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( + msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); + + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", test_filtered_ars.event_id); + + // pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); + } + } } From f43323baadce451339602106dafbd3fdbceace7f Mon Sep 17 00:00:00 2001 From: Rijin Date: Sun, 19 Feb 2023 19:39:15 +0000 Subject: [PATCH 20/51] updated pointcloud filter ROS node --- .../config/ars_radar_params.yaml | 2 +- .../include/ars_pointcloud_filter.hpp | 9 +++ .../include/ars_pointcloud_filter_node.hpp | 26 +++++++- .../src/ars_pointcloud_filter.cpp | 59 +++++++++++++++++++ .../src/ars_pointcloud_filter_node.cpp | 38 +++++------- 5 files changed, 108 insertions(+), 26 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml index d4fe8248..ebbebf9f 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml +++ b/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml @@ -7,4 +7,4 @@ ars_point_cloud_filter: snr: -99999.99 range: -99999.99 az_ang0: -99999.99 - scan: "near" + scan_mode: "near" diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 313311ad..7c44b92b 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -24,6 +24,15 @@ class ARSPointCloudFilter double vrel_rad_threshold, double el_ang_threshold, double rcs_threshold); + + void near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + radar_msgs::msg::RadarPacket &buffer_packet); + + + + + + // radar_msgs::msg::RadarPacket snr_filter( // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 650bb132..215b13cc 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -16,12 +16,28 @@ class ARSPointCloudFilterNode : public rclcpp::Node { public: - /** - * PointCloudFilter Node Constructor. - */ + + /** + * PointCloudFilter Node Constructor. + */ ARSPointCloudFilterNode(); + struct filter_parameters + { + 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 parameters; + private: + radar_msgs::msg::RadarPacket buffer_packet; + /** * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" * topic @@ -46,8 +62,12 @@ class ARSPointCloudFilterNode : public rclcpp::Node // ROS2 Subscriber listening to the unfiltered radar packet 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_; + // Add an object below from radar_pointcloud_filter.hpp that contains the methods filtering::ARSPointCloudFilter pointcloudfilter_; + }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 25b7a392..16b45c9d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -1,5 +1,8 @@ #include +#include "rclcpp/rclcpp.hpp" + +#include "ars_pointcloud_filter_node.hpp" #include "ars_pointcloud_filter.hpp" // SOLUTION 1: Via Conditions and ROS parameters @@ -55,6 +58,62 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( + +void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, radar_msgs::msg::RadarPacket &buffer_packet) +{ + if((msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5)) + { + // Filter out far scan packets + if(!buffer_packet.detections.empty()) + { + // If timestamp is the same as buffer packet + if(msg->timestamp == buffer_packet.timestamp) + { + // Filter out based on thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( + msg, ARSPointCloudFilterNode::parameters.snr_param, ARSPointCloudFilterNode::parameters.az_ang0_param, ARSPointCloudFilterNode::parameters.range_param, + ARSPointCloudFilterNode::parameters.vrel_rad_param, ARSPointCloudFilterNode::parameters.el_ang_param, ARSPointCloudFilterNode::parameters.rcs0_param); + + // Append all detections to buffer + buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + } + else + { + // Publish buffer packet + // RCLCPP_INFO(this->get_logger(), "Publishing %d\n", buffer_packet.event_id); + left_right_pub_->publish(buffer_packet); + + // Filter incoming data + const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( + msg, ARSPointCloudFilterNode::parameters.snr_param, ARSPointCloudFilterNode::parameters.az_ang0_param, ARSPointCloudFilterNode::parameters.range_param, + ARSPointCloudFilterNode::parameters.vrel_rad_param, ARSPointCloudFilterNode::parameters.el_ang_param, ARSPointCloudFilterNode::parameters.rcs0_param); + + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering + buffer_packet = test_filtered_ars; + + } + } + + else + { + // Filter incoming data based on parameters + const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( + msg, ARSPointCloudFilterNode::parameters.snr_param, ARSPointCloudFilterNode::parameters.az_ang0_param, ARSPointCloudFilterNode::parameters.range_param, + ARSPointCloudFilterNode::parameters.vrel_rad_param, ARSPointCloudFilterNode::parameters.el_ang_param, ARSPointCloudFilterNode::parameters.rcs0_param); + + // Append filtered data to the buffer packet + buffer_packet = test_filtered_ars; + } + + } +} + + + + + + // // SOLUTION 2: Via Lambda Functions // // LAMDA - Filter Template diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 8b46ccd7..891cadda 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -2,6 +2,7 @@ #include #include "ars_pointcloud_filter_node.hpp" +#include "ars_pointcloud_filter.hpp" @@ -9,7 +10,7 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter { //default values already declared in ars_radar_params.yaml this->declare_parameter("filter_mode"); - this->declare_parameter("scan"); + this->declare_parameter("scan_mode"); this->declare_parameter("vrel_rad"); this->declare_parameter("el_ang"); this->declare_parameter("rcs0"); @@ -17,6 +18,14 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter this->declare_parameter("range"); this->declare_parameter("az_ang0"); + 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( @@ -28,6 +37,8 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); + + left_right_pub_ = this->create_publisher("processed", 20); } @@ -38,29 +49,12 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); // messages from unfiltered right radar topic (ars) - std::string scan_param = this->get_parameter("scan").as_string(); - double vrel_rad_param = this->get_parameter("vrel_rad").as_double(); - double el_ang_param = this->get_parameter("el_ang").as_double(); - double rcs0_param = this->get_parameter("rcs0").as_double(); - double snr_param = this->get_parameter("snr").as_double(); - double range_param = this->get_parameter("range").as_double(); - double az_ang0_param = this->get_parameter("az_ang0").as_double(); - - // Send unfiltered packets along with set parameter thresholds to the filter - if(scan_param == "near") + // Send Unfiltered packets along with set parameter thresholds to the filter + if(parameters.scan_mode == "near") { - if(msg->event_id == 12) - { - - const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( - msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); - - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", test_filtered_ars.event_id); - - // pointcloudfilter_.point_filter(msg,snr_param,az_ang0_param,range_param,vrel_rad_param,el_ang_param,rcs0_param); - } + filtering::ARSPointCloudFilter::near_scan_filter(msg, ARSPointCloudFilterNode::buffer_packet) } - + } void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( From 653d751ecd2b95cb4a517c3c258d4dea74caa9ab Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 21 Feb 2023 03:56:43 +0000 Subject: [PATCH 21/51] implementing near and far scan mode filters --- .../include/ars_pointcloud_filter.hpp | 10 +- .../src/ars_pointcloud_filter.cpp | 108 ++++++++++++++++-- .../src/ars_pointcloud_filter_node.cpp | 9 +- 3 files changed, 113 insertions(+), 14 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 7c44b92b..36bdfd0d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -7,6 +7,8 @@ #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" +#include "ars_pointcloud_filter_node.hpp" + namespace filtering { @@ -26,13 +28,17 @@ class ARSPointCloudFilter double rcs_threshold); void near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - radar_msgs::msg::RadarPacket &buffer_packet); + radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, + double rcs0_param, double snr_param, double range_param, double az_ang0_param); + + // void far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + // radar_msgs::msg::RadarPacket &buffer_packet, struct ARSPointCloudFilterNode::filter_parameters parameters); + - // radar_msgs::msg::RadarPacket snr_filter( // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 16b45c9d..9ea6d499 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -59,7 +59,9 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( -void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, radar_msgs::msg::RadarPacket &buffer_packet) +void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, + radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, + double rcs0_param, double snr_param, double range_param, double az_ang0_param) { if((msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5)) { @@ -70,14 +72,14 @@ void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S if(msg->timestamp == buffer_packet.timestamp) { // Filter out based on thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( - msg, ARSPointCloudFilterNode::parameters.snr_param, ARSPointCloudFilterNode::parameters.az_ang0_param, ARSPointCloudFilterNode::parameters.range_param, - ARSPointCloudFilterNode::parameters.vrel_rad_param, ARSPointCloudFilterNode::parameters.el_ang_param, ARSPointCloudFilterNode::parameters.rcs0_param); + const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( + msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); // Append all detections to buffer buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); } + else { // Publish buffer packet @@ -85,9 +87,8 @@ void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S left_right_pub_->publish(buffer_packet); // Filter incoming data - const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( - msg, ARSPointCloudFilterNode::parameters.snr_param, ARSPointCloudFilterNode::parameters.az_ang0_param, ARSPointCloudFilterNode::parameters.range_param, - ARSPointCloudFilterNode::parameters.vrel_rad_param, ARSPointCloudFilterNode::parameters.el_ang_param, ARSPointCloudFilterNode::parameters.rcs0_param); + const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( + msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet = test_filtered_ars; @@ -98,9 +99,8 @@ void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S else { // Filter incoming data based on parameters - const radar_msgs::msg::RadarPacket test_filtered_ars = ARSPointCloudFilterNode::pointcloudfilter_.point_filter( - msg, ARSPointCloudFilterNode::parameters.snr_param, ARSPointCloudFilterNode::parameters.az_ang0_param, ARSPointCloudFilterNode::parameters.range_param, - ARSPointCloudFilterNode::parameters.vrel_rad_param, ARSPointCloudFilterNode::parameters.el_ang_param, ARSPointCloudFilterNode::parameters.rcs0_param); + const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( + msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); // Append filtered data to the buffer packet buffer_packet = test_filtered_ars; @@ -108,6 +108,94 @@ void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S } } + + + + +// void ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, +// radar_msgs::msg::RadarPacket &buffer_packet, struct ARSPointCloudFilterNode::filter_parameters parameters) +// { +// if((msg->event_id == 1 || msg->event_id == 2)) +// { +// // Filter out far scan packets +// if(!buffer_packet.detections.empty()) +// { +// // If timestamp is the same as buffer packet +// if(msg->timestamp == buffer_packet.timestamp) +// { +// // Filter out based on thresholds +// const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( +// msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, +// parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + +// // Append all detections to buffer +// buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), +// test_filtered_ars.detections.end()); +// } + +// else +// { +// // Publish buffer packet +// // RCLCPP_INFO(this->get_logger(), "Publishing %d\n", buffer_packet.event_id); +// left_right_pub_->publish(buffer_packet); + +// // Filter incoming data +// const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( +// msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, +// parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + +// // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering +// buffer_packet = test_filtered_ars; + +// } +// } + +// else +// { +// // Filter incoming data based on parameters +// const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( +// msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, +// parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + +// // Append filtered data to the buffer packet +// buffer_packet = test_filtered_ars; +// } + +// } +// } + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 891cadda..b9df8b42 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -52,9 +52,14 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( // Send Unfiltered packets along with set parameter thresholds to the filter if(parameters.scan_mode == "near") { - filtering::ARSPointCloudFilter::near_scan_filter(msg, ARSPointCloudFilterNode::buffer_packet) + pointcloudfilter_.near_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, + parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param); } - + // else if (parameters.scan_mode == "far") + // { + // pointcloudfilter_.far_scan_filter(msg,buffer_packet,parameters); + // } + } void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( From 6da6ef4ed2ec59fffa5e802b88280aa2198246ce Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 22 Feb 2023 02:50:47 +0000 Subject: [PATCH 22/51] created near scan and far scan filters --- .../include/ars_pointcloud_filter.hpp | 32 +- .../include/ars_pointcloud_filter_node.hpp | 3 + .../src/ars_pointcloud_filter.cpp | 352 +++--------------- .../src/ars_pointcloud_filter_node.cpp | 61 ++- 4 files changed, 116 insertions(+), 332 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 36bdfd0d..4288d041 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -29,34 +29,14 @@ class ARSPointCloudFilter void near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, - double rcs0_param, double snr_param, double range_param, double az_ang0_param); + double rcs0_param, double snr_param, double range_param, double az_ang0_param, std::pair &publish_packet); - // void far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - // radar_msgs::msg::RadarPacket &buffer_packet, struct ARSPointCloudFilterNode::filter_parameters parameters); - - - - - - - - // radar_msgs::msg::RadarPacket snr_filter( - // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold); - - // radar_msgs::msg::RadarPacket azimuth_angle_filter( - // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold); - - // radar_msgs::msg::RadarPacket range_filter( - // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold); - - // radar_msgs::msg::RadarPacket vrel_rad_filter( - // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold); - - // radar_msgs::msg::RadarPacket el_ang_filter( - // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold); + void far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, + double rcs0_param, double snr_param, double range_param, double az_ang0_param, std::pair &publish_packet); - // radar_msgs::msg::RadarPacket rcs_filter( - // const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold); }; + } + #endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 215b13cc..48c6faf9 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -67,6 +67,9 @@ class ARSPointCloudFilterNode : public rclcpp::Node // Add an object below from radar_pointcloud_filter.hpp that contains the methods filtering::ARSPointCloudFilter pointcloudfilter_; + + // Create a pair to check if packet can be published + std::pair publish_packet; }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 9ea6d499..a017ae4d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -15,6 +15,8 @@ ARSPointCloudFilter::ARSPointCloudFilter() } +// Point Filter Implementation + radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, @@ -57,11 +59,11 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( } - - +// Near Scan Filter Implementation void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, - double rcs0_param, double snr_param, double range_param, double az_ang0_param) + double rcs0_param, double snr_param, double range_param, double az_ang0_param, + std::pair &publish_packet) { if((msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5)) { @@ -72,331 +74,85 @@ void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S if(msg->timestamp == buffer_packet.timestamp) { // Filter out based on thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); // Append all detections to buffer buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); } - else { - // Publish buffer packet - // RCLCPP_INFO(this->get_logger(), "Publishing %d\n", buffer_packet.event_id); - left_right_pub_->publish(buffer_packet); - + publish_packet.first = true; + publish_packet.second = buffer_packet; + // Filter incoming data - const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet = test_filtered_ars; - } } - else { // Filter incoming data based on parameters - const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); // Append filtered data to the buffer packet buffer_packet = test_filtered_ars; } - } } - - -// void ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, -// radar_msgs::msg::RadarPacket &buffer_packet, struct ARSPointCloudFilterNode::filter_parameters parameters) -// { -// if((msg->event_id == 1 || msg->event_id == 2)) -// { -// // Filter out far scan packets -// if(!buffer_packet.detections.empty()) -// { -// // If timestamp is the same as buffer packet -// if(msg->timestamp == buffer_packet.timestamp) -// { -// // Filter out based on thresholds -// const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( -// msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, -// parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); +// Far Scan Filter Implementation +void ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, + radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, + double rcs0_param, double snr_param, double range_param, double az_ang0_param, + std::pair &publish_packet) +{ + if((msg->event_id == 1 || msg->event_id == 2)) + { + // Filter out near scan packets + if(!buffer_packet.detections.empty()) + { + // If timestamp is the same as buffer packet + if(msg->timestamp == buffer_packet.timestamp) + { + // Filter incoming data based on parameters + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); -// // Append all detections to buffer -// buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), -// test_filtered_ars.detections.end()); -// } - -// else -// { -// // Publish buffer packet -// // RCLCPP_INFO(this->get_logger(), "Publishing %d\n", buffer_packet.event_id); -// left_right_pub_->publish(buffer_packet); - -// // Filter incoming data -// const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( -// msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, -// parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + // Append all detections to buffer + buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + } + else + { + publish_packet.first = true; + publish_packet.second = buffer_packet; -// // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering -// buffer_packet = test_filtered_ars; - -// } -// } - -// else -// { -// // Filter incoming data based on parameters -// const radar_msgs::msg::RadarPacket test_filtered_ars = pointcloudfilter_.point_filter( -// msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, -// parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + // Filter incoming data based on parameters + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); + + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering + buffer_packet = test_filtered_ars; + } + } + else + { + // Filter incoming data based on parameters + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); -// // Append filtered data to the buffer packet -// buffer_packet = test_filtered_ars; -// } - -// } -// } - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -// // SOLUTION 2: Via Lambda Functions -// // LAMDA - Filter Template - -// radar_msgs::msg::RadarPacket main_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, -// const std::function &filter_type) -// { -// radar_msgs::msg::RadarPacket main_filtered_ars; -// for (auto detection : unfiltered_ars->detections) -// { -// if(filter_type(detection)) -// { -// // Push filtered point/detection into an array -// main_filtered_ars.detections.push_back(detection); -// } -// } -// return main_filtered_ars; -// } - -// // LAMBDA - SNR Filter -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) -// { -// auto filter_type=[snr_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { -// return test_detection.snr < snr_threshold; -// }; - -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); -// return filtered_ars; - -// } - -// // LAMBDA - Azimuth Angle Filter -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold) -// { -// auto filter_type=[AzAng0_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { -// return test_detection.az_ang0 < AzAng0_threshold; -// }; - -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); -// return filtered_ars; - -// } - -// // LAMBDA - Range Filter -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::range_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold) -// { -// auto filter_type=[range_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { -// return test_detection.range < range_threshold; -// }; - -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); -// return filtered_ars; - -// } - -// // LAMBDA - Relative Radial Velocity -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::vrel_rad_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold) -// { -// auto filter_type=[vrel_rad_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { -// return test_detection.snr < vrel_rad_threshold; -// }; - -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); -// return filtered_ars; -// } - -// // LAMBDA - Elevation Angle -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::el_ang_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold) -// { -// auto filter_type=[el_ang_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { -// return test_detection.snr < el_ang_threshold; -// }; - -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); -// return filtered_ars; -// } - -// // LAMBDA - Radar Cross Section -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::rcs_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold) -// { -// auto filter_type=[rcs_threshold](const radar_msgs::msg::RadarDetection test_detection)->bool { -// return test_detection.snr < rcs_threshold; -// }; - -// radar_msgs::msg::RadarPacket filtered_ars = main_filter(unfiltered_ars, filter_type); -// return filtered_ars; -// } - - - - - - -// // ------------------------------ -// // ORIGINAL IMPLEMENTATION -// // SNR Filter -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::snr_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold) -// { -// radar_msgs::msg::RadarPacket filtered_ars; -// for (auto detection : unfiltered_ars->detections) -// { -// if(detection.snr < snr_threshold) -// { -// // Push filtered point/detection into an array -// filtered_ars.detections.push_back(detection); -// } -// } -// return filtered_ars; -// } - -// // Azimuth Angle Filter -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::azimuth_angle_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double AzAng0_threshold) -// { -// radar_msgs::msg::RadarPacket filtered_ars; -// for (auto detection : unfiltered_ars->detections) -// { -// if(abs(detection.az_ang0) < abs(AzAng0_threshold)) -// { -// // Point angles are within the defined constraints -// filtered_ars.detections.push_back(detection); -// } -// } -// return filtered_ars; -// } - -// // Range Filter -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::range_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double range_threshold) -// { -// radar_msgs::msg::RadarPacket filtered_ars; -// for (auto detection : unfiltered_ars->detections) -// { -// if(detection.range < range_threshold) -// { -// // an array of filtered objects -// filtered_ars.detections.push_back(detection); -// } -// } -// return filtered_ars; -// } - -// // Relative Radial Velocity -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::vrel_rad_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double vrel_rad_threshold) -// { -// radar_msgs::msg::RadarPacket filtered_ars; -// for (auto detection : unfiltered_ars->detections) -// { -// if(detection.vrel_rad < vrel_rad_threshold) -// { -// // an array of filtered objects -// filtered_ars.detections.push_back(detection); -// } -// } -// return filtered_ars; -// } - -// // Elevation Angle -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::el_ang_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double el_ang_threshold) -// { -// radar_msgs::msg::RadarPacket filtered_ars; -// for (auto detection : unfiltered_ars->detections) -// { -// if(detection.el_ang < el_ang_threshold) -// { -// // an array of filtered objects -// filtered_ars.detections.push_back(detection); -// } -// } -// return filtered_ars; -// } + // Append filtered data to the buffer packet + buffer_packet = test_filtered_ars; + } + } +} -// // Radar Cross Section -// radar_msgs::msg::RadarPacket ARSPointCloudFilter::rcs_filter( -// const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double rcs_threshold) -// { -// radar_msgs::msg::RadarPacket filtered_ars; -// for (auto detection : unfiltered_ars->detections) -// { -// if(detection.rcs0 < rcs_threshold) -// { -// // an array of filtered objects -// filtered_ars.detections.push_back(detection); -// } -// } -// return filtered_ars; -// } } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index b9df8b42..39649e74 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -40,6 +40,7 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter left_right_pub_ = this->create_publisher("processed", 20); + } void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( @@ -47,18 +48,34 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - // messages from unfiltered right radar topic (ars) - // Send Unfiltered packets along with set parameter thresholds to the filter + // Send Unfiltered packets along with filter thresholds if(parameters.scan_mode == "near") { pointcloudfilter_.near_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, - parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param); + parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); + + if(publish_packet.first == true) + { + // Publish buffer packet + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); + left_right_pub_->publish(publish_packet.second); + } + + } + + else if (parameters.scan_mode == "far") + { + pointcloudfilter_.far_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, + parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); + + if(publish_packet.first == true) + { + // Publish buffer packet + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); + left_right_pub_->publish(publish_packet.second); + } } - // else if (parameters.scan_mode == "far") - // { - // pointcloudfilter_.far_scan_filter(msg,buffer_packet,parameters); - // } } @@ -66,7 +83,35 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - // messages from unfiltered left radar topic (ars) + + // Send Unfiltered packets along with set parameter thresholds to the filter + if(parameters.scan_mode == "near") + { + pointcloudfilter_.near_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, + parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); + + if(publish_packet.first == true) + { + // Publish buffer packet + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); + left_right_pub_->publish(publish_packet.second); + } + + } + + else if (parameters.scan_mode == "far") + { + pointcloudfilter_.far_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, + parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); + + if(publish_packet.first == true) + { + // Publish buffer packet + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); + left_right_pub_->publish(publish_packet.second); + } + } + } int main(int argc, char ** argv) From fe75cbc44cec8d82a75d74bda4699169202c307f Mon Sep 17 00:00:00 2001 From: Rijin Date: Thu, 23 Feb 2023 02:53:12 +0000 Subject: [PATCH 23/51] updated near and far scan filters structure --- .../include/ars_pointcloud_filter.hpp | 44 +++++++---- .../include/ars_pointcloud_filter_node.hpp | 17 +---- .../src/ars_pointcloud_filter.cpp | 75 ++++++++++++------- .../src/ars_pointcloud_filter_node.cpp | 58 +++++++------- 4 files changed, 109 insertions(+), 85 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 4288d041..19701b8b 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -7,33 +7,47 @@ #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" -#include "ars_pointcloud_filter_node.hpp" namespace filtering { class ARSPointCloudFilter { + public: + ARSPointCloudFilter(); - // point_filter - Solution 1 radar_msgs::msg::RadarPacket point_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double snr_threshold, - double AzAng0_threshold, - double range_threshold, - double vrel_rad_threshold, - double el_ang_threshold, - double rcs_threshold); + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double snr_threshold, + double AzAng0_threshold, + double range_threshold, + double vrel_rad_threshold, + double el_ang_threshold, + double rcs_threshold); + + 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; - void near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, - double rcs0_param, double snr_param, double range_param, double az_ang0_param, std::pair &publish_packet); + bool near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + + bool far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + +private: - void far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, - double rcs0_param, double snr_param, double range_param, double az_ang0_param, std::pair &publish_packet); + // Create a buffer packet to hold detections from incoming messages (with the same timestamps) + radar_msgs::msg::RadarPacket buffer_packet; }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 48c6faf9..68f63571 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -22,21 +22,9 @@ class ARSPointCloudFilterNode : public rclcpp::Node */ ARSPointCloudFilterNode(); - struct filter_parameters - { - 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 parameters; + filtering::ARSPointCloudFilter::filter_parameters parameters; private: - radar_msgs::msg::RadarPacket buffer_packet; /** * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" @@ -68,9 +56,6 @@ class ARSPointCloudFilterNode : public rclcpp::Node // Add an object below from radar_pointcloud_filter.hpp that contains the methods filtering::ARSPointCloudFilter pointcloudfilter_; - // Create a pair to check if packet can be published - std::pair publish_packet; - }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index a017ae4d..893b6af3 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -5,8 +5,6 @@ #include "ars_pointcloud_filter_node.hpp" #include "ars_pointcloud_filter.hpp" -// SOLUTION 1: Via Conditions and ROS parameters - namespace filtering { @@ -60,58 +58,68 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( // Near Scan Filter Implementation -void ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, - double rcs0_param, double snr_param, double range_param, double az_ang0_param, - std::pair &publish_packet) +bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, + const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { if((msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5)) { - // Filter out far scan packets + // Filter out far scan packets if(!buffer_packet.detections.empty()) { // If timestamp is the same as buffer packet if(msg->timestamp == buffer_packet.timestamp) { - // Filter out based on thresholds + // Filter out detections based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append all detections to buffer buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); + return false; + } else { - publish_packet.first = true; - publish_packet.second = buffer_packet; + /** + Publish buffer packet since incoming message now has a different timestamp. + This means that the buffer packet is full with detections from 18 packets of the same timestamp. + **/ + publish_packet = buffer_packet; - // Filter incoming data + // Filter new incoming data const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet = test_filtered_ars; + + return true; } } else { // Filter incoming data based on parameters - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append filtered data to the buffer packet buffer_packet = test_filtered_ars; + + return false; } } + + return false; } // Far Scan Filter Implementation -void ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - radar_msgs::msg::RadarPacket &buffer_packet, double vrel_rad_param, double el_ang_param, - double rcs0_param, double snr_param, double range_param, double az_ang0_param, - std::pair &publish_packet) +bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, + const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { if((msg->event_id == 1 || msg->event_id == 2)) { @@ -121,38 +129,51 @@ void ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::Sh // If timestamp is the same as buffer packet if(msg->timestamp == buffer_packet.timestamp) { - // Filter incoming data based on parameters + // Filter incoming data based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append all detections to buffer buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); + + return false; } else { - publish_packet.first = true; - publish_packet.second = buffer_packet; + /** + Publish buffer packet since incoming message now has a different timestamp. + This means that the buffer packet is full with detections from 18 packets of the same timestamp. + **/ + publish_packet = buffer_packet; - // Filter incoming data based on parameters + // // Filter new incoming data const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet = test_filtered_ars; + + return true; } } else { // Filter incoming data based on parameters const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, snr_param, az_ang0_param, range_param, vrel_rad_param, el_ang_param, rcs0_param); + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append filtered data to the buffer packet buffer_packet = test_filtered_ars; + + return false; } } -} + return false; +} } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 39649e74..9214f4b7 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -4,8 +4,6 @@ #include "ars_pointcloud_filter_node.hpp" #include "ars_pointcloud_filter.hpp" - - ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") { //default values already declared in ars_radar_params.yaml @@ -40,7 +38,6 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter left_right_pub_ = this->create_publisher("processed", 20); - } void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( @@ -49,31 +46,40 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - // Send Unfiltered packets along with filter thresholds + // If scan mode is near if(parameters.scan_mode == "near") { - pointcloudfilter_.near_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, - parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); - - if(publish_packet.first == true) + // Create a temporary empty packet for when we have a full near scan filtered packet ready to be published + radar_msgs::msg::RadarPacket publish_packet; + + /** + Send message to near scan filter and append detections to buffer packet based on timestamp. + Check if packet is ready to be published (if timestamp differs). Return true if ready to be published, return false otherwise. + **/ + if(pointcloudfilter_.near_scan_filter(msg, parameters, publish_packet) == true) { // Publish buffer packet - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); - left_right_pub_->publish(publish_packet.second); + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); + left_right_pub_->publish(publish_packet); } } + // If scan mode is far else if (parameters.scan_mode == "far") { - pointcloudfilter_.far_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, - parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); - - if(publish_packet.first == true) + // Create a temporary empty packet for when we have a full far scan filtered packet ready to be published + radar_msgs::msg::RadarPacket publish_packet; + + /** + Send message to far scan filter and append detections to buffer packet based on timestamp. + Check if packet is ready to be published (if timestamp differs). Return true if ready to be published, return false otherwise. + **/ + if(pointcloudfilter_.far_scan_filter(msg, parameters, publish_packet) == true) { // Publish buffer packet - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); - left_right_pub_->publish(publish_packet.second); + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); + left_right_pub_->publish(publish_packet); } } @@ -84,31 +90,29 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - // Send Unfiltered packets along with set parameter thresholds to the filter + // Send Unfiltered packets along with filter thresholds if(parameters.scan_mode == "near") { - pointcloudfilter_.near_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, - parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); + radar_msgs::msg::RadarPacket publish_packet; - if(publish_packet.first == true) + if(pointcloudfilter_.near_scan_filter(msg, parameters, publish_packet) == true) { // Publish buffer packet - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); - left_right_pub_->publish(publish_packet.second); + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); + left_right_pub_->publish(publish_packet); } } else if (parameters.scan_mode == "far") { - pointcloudfilter_.far_scan_filter(msg, buffer_packet, parameters.vrel_rad_param, parameters.el_ang_param, - parameters.rcs0_param, parameters.snr_param, parameters.range_param, parameters.az_ang0_param, publish_packet); + radar_msgs::msg::RadarPacket publish_packet; - if(publish_packet.first == true) + if(pointcloudfilter_.far_scan_filter(msg, parameters, publish_packet) == true) { // Publish buffer packet - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.second.event_id); - left_right_pub_->publish(publish_packet.second); + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); + left_right_pub_->publish(publish_packet); } } From 0e2931e6bd40d9820fbf848cfbde8d079ac95848 Mon Sep 17 00:00:00 2001 From: Rijin Date: Sat, 25 Feb 2023 22:49:11 +0000 Subject: [PATCH 24/51] wrote pseudocode for double buffer --- .../include/ars_pointcloud_filter.hpp | 12 ++++ .../src/ars_pointcloud_filter.cpp | 62 +++++++++++++++++++ .../src/ars_pointcloud_filter_node.cpp | 14 +++++ 3 files changed, 88 insertions(+) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 19701b8b..8006d72d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -44,11 +44,23 @@ class ARSPointCloudFilter bool far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + bool near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + private: // Create a buffer packet to hold detections from incoming messages (with the same timestamps) radar_msgs::msg::RadarPacket buffer_packet; + // Create two buffer packets (for when both near and far scan mode filters are activated) + radar_msgs::msg::RadarPacket buffer_packet_near_far_01; + radar_msgs::msg::RadarPacket buffer_packet_near_far_02; + + // Number of detection packets + int total_near_scan_packets = 0; // Would equal to 18 when full + int total_far_scan_packets = 0; // Would equal to 12 when full + int total_near_far = 30; + }; } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 893b6af3..9697cad8 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -176,4 +176,66 @@ bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::Sh return false; } + + + + +// Near + Far Scan Filter Implementation (Double Buffer Algorithm) +bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, + const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) +{ + // Scan 1 (same timestamp for 18 near scan packets and a slightly different timestamp for 12 far scan packets) + // But they should be appended to the same packet (published in 30) + + // Pseudocode + + // Save near and far scan timestamps as member variables + + if event id == near and msg timestamp != near timestamp member variable + then update member timestamp, make scan1_near == false, scan2_near == true + + else keep the same timestamp for near, make scan1_near = true, scan2_near = false + + if event id is far and msg timestamp != far timestamp member variable + then update member timestamp, make scan1_far == false, scan2_far = true + + else keep the same timestamp for far, make scan1_far = true, scan2_far = false + + if (msg timetamp == near or far member timestamps) + { + if msg timestamp == near timestamp and scan1_near == true and scan2_near = false, + { + append detections to buffer packet 1 + total_near_packets_count++ + } + + // Means near scans are full in buffer packet 1 + else + { + Append detections to buffer packet 2 + } + + if msg timestamp == far timestamp and scan1_far == true, scan2_far = false, + { + Append detections to buffer packet 1 + total_far_packets_count++ + } + + // Means far scans are full in buffer packet 1 + else + { + Append to buffer packet 2 (this means that we have 30 packets in buffer packet 1) + Publish buffer packet 1, (both member timestamps have already been updated at this point for scan 2) + Reset packet counters + } + } + +} + + + + + + + } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 9214f4b7..dff7860b 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -82,6 +82,20 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( left_right_pub_->publish(publish_packet); } } + + // If scan mode is near_far (Double Buffer) + else + { + // Create a temporary empty packet for when we have a full near and far scan filtered packet ready to be published + // Publish packet of 30 from first scan or second scan + radar_msgs::msg::RadarPacket publish_packet_near_far; + + pointcloudfilter_.near_far_scan_filter(msg,parameters,publish_packet_near_far); + + left_right_pub_->publish(publish_packet_near_far); + + + } } From 5d9724b1c8173e5f93a6e0fc16313f8e47c7618b Mon Sep 17 00:00:00 2001 From: Rijin Date: Sun, 26 Feb 2023 23:37:01 +0000 Subject: [PATCH 25/51] added double buffer algorithm for near + far scan filter --- .../include/ars_pointcloud_filter.hpp | 16 ++- .../src/ars_pointcloud_filter.cpp | 131 ++++++++++++------ .../src/ars_pointcloud_filter_node.cpp | 26 +++- 3 files changed, 122 insertions(+), 51 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 8006d72d..345211c8 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -53,13 +53,15 @@ class ARSPointCloudFilter radar_msgs::msg::RadarPacket buffer_packet; // Create two buffer packets (for when both near and far scan mode filters are activated) - radar_msgs::msg::RadarPacket buffer_packet_near_far_01; - radar_msgs::msg::RadarPacket buffer_packet_near_far_02; - - // Number of detection packets - int total_near_scan_packets = 0; // Would equal to 18 when full - int total_far_scan_packets = 0; // Would equal to 12 when full - int total_near_far = 30; + radar_msgs::msg::RadarPacket near_far_buffer_packets[2]; + + // Variables for double buffer + int buffer_index; + unsigned int default_timestamp; + unsigned int near_timestamp; + unsigned int far_timestamp; + unsigned int next_near_timestamp; + unsigned int next_far_timestamp; }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 9697cad8..a7383b02 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -10,7 +10,10 @@ namespace filtering ARSPointCloudFilter::ARSPointCloudFilter() { - + buffer_index = 0; + default_timestamp = -1; + near_timestamp = -1; + far_timestamp = -1; } // Point Filter Implementation @@ -177,65 +180,115 @@ bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::Sh } - - - // Near + Far Scan Filter Implementation (Double Buffer Algorithm) bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { - // Scan 1 (same timestamp for 18 near scan packets and a slightly different timestamp for 12 far scan packets) - // But they should be appended to the same packet (published in 30) + // Rearranged Implementation - // Pseudocode + // Check if its near + if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) + { + // Default case + if (near_timestamp == default_timestamp) + { + near_timestamp = msg->timestamp; + } - // Save near and far scan timestamps as member variables + // Check if new message is part of the same scan + if (msg->timestamp == near_timestamp) + { + // Append to buffer packet 1 + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // Append all detections to buffer packet 1 + near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + + return false; - if event id == near and msg timestamp != near timestamp member variable - then update member timestamp, make scan1_near == false, scan2_near == true + } + // This means 18 packets are collected in buffer packet 1 (all with the same timestamps) => new near scan data + else + { + next_near_timestamp = msg->timestamp; - else keep the same timestamp for near, make scan1_near = true, scan2_near = false + const radar_msgs::msg::RadarPacket test_filtered_ars = 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 event id is far and msg timestamp != far timestamp member variable - then update member timestamp, make scan1_far == false, scan2_far = true + // Append to buffer packet 2 + near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + + return false; - else keep the same timestamp for far, make scan1_far = true, scan2_far = false + } + } - if (msg timetamp == near or far member timestamps) + // Check if its far + if (msg->event_id == 1 || msg->event_id == 2) { - if msg timestamp == near timestamp and scan1_near == true and scan2_near = false, - { - append detections to buffer packet 1 - total_near_packets_count++ - } + // Default case + if (far_timestamp == default_timestamp) + { + far_timestamp = msg->timestamp; + } - // Means near scans are full in buffer packet 1 - else - { - Append detections to buffer packet 2 - } + // Check if new message is part of the same scan + if (msg->timestamp == far_timestamp) + { + // Append to buffer packet 1 + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // Append all detections to buffer packet 1 + near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + + return false; - if msg timestamp == far timestamp and scan1_far == true, scan2_far = false, - { - Append detections to buffer packet 1 - total_far_packets_count++ - } + } + // This means 12 packets are collected in buffer packet 1 (all with the same timestamps)=> new far scan data + else + { + next_far_timestamp = msg->timestamp; - // Means far scans are full in buffer packet 1 - else - { - Append to buffer packet 2 (this means that we have 30 packets in buffer packet 1) - Publish buffer packet 1, (both member timestamps have already been updated at this point for scan 2) - Reset packet counters - } - } + // This means we technically have 30 packets in buffer packet 1 at this point -} + // Publish buffer_packet + publish_packet = near_far_buffer_packets[buffer_index]; + // Filter new far scan incoming data + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // Append to buffer packet 2 + near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + // Change timestamps, next_near and next_far become the "new" timestamps to check if incoming messages are part of the same scan + near_timestamp = next_near_timestamp; + far_timestamp = next_far_timestamp; + // Clear the buffer packet array + near_far_buffer_packets[buffer_index].detections.clear(); + // Change index so next iteration, buffer packet 2 is considered to be part of scan 1 and buffer packet 1 is part of scan 2 + buffer_index = 1 - buffer_index; + return true; + } + } + + return false; +} } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index dff7860b..8f87b16d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -87,13 +87,14 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( else { // Create a temporary empty packet for when we have a full near and far scan filtered packet ready to be published - // Publish packet of 30 from first scan or second scan radar_msgs::msg::RadarPacket publish_packet_near_far; - pointcloudfilter_.near_far_scan_filter(msg,parameters,publish_packet_near_far); - - left_right_pub_->publish(publish_packet_near_far); - + if(pointcloudfilter_.near_far_scan_filter(msg,parameters,publish_packet_near_far) == true) + { + // Publish buffer packet + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); + left_right_pub_->publish(publish_packet_near_far); + } } @@ -130,6 +131,21 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( } } + // If scan mode is near_far (Double Buffer) + else + { + // Create a temporary empty packet for when we have a full near and far scan filtered packet ready to be published + radar_msgs::msg::RadarPacket publish_packet_near_far; + + if(pointcloudfilter_.near_far_scan_filter(msg,parameters,publish_packet_near_far) == true) + { + // Publish buffer packet + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); + left_right_pub_->publish(publish_packet_near_far); + } + + } + } int main(int argc, char ** argv) From 380e93f1fee8e56db64258afd947baa5d392fc2e Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 28 Feb 2023 01:41:51 +0000 Subject: [PATCH 26/51] updated code structure and started testing --- .../include/ars_pointcloud_filter.hpp | 22 ++-- .../include/ars_pointcloud_filter_node.hpp | 25 ++-- .../launch/ars_pointcloud_filter.launch.py | 14 +-- .../src/ars_pointcloud_filter.cpp | 108 ++++++++++-------- .../src/ars_pointcloud_filter_node.cpp | 22 ++-- .../test/ars_pointcloud_filter_test.cpp | 8 ++ 6 files changed, 97 insertions(+), 102 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 345211c8..b02937af 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -1,25 +1,22 @@ #ifndef ARS_POINTCLOUD_FILTER_HPP_ #define ARS_POINTCLOUD_FILTER_HPP_ +#include #include "rclcpp/rclcpp.hpp" // importing custom message types #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" - namespace filtering { - class ARSPointCloudFilter { - public: - ARSPointCloudFilter(); radar_msgs::msg::RadarPacket point_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, double AzAng0_threshold, double range_threshold, @@ -27,7 +24,7 @@ class ARSPointCloudFilter double el_ang_threshold, double rcs_threshold); - typedef struct + typedef struct { std::string scan_mode; double vrel_rad_param; @@ -39,16 +36,18 @@ class ARSPointCloudFilter } filter_parameters; bool near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet); bool far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet); bool near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet); private: - // Create a buffer packet to hold detections from incoming messages (with the same timestamps) radar_msgs::msg::RadarPacket buffer_packet; @@ -62,9 +61,8 @@ class ARSPointCloudFilter unsigned int far_timestamp; unsigned int next_near_timestamp; unsigned int next_far_timestamp; - }; -} +} // namespace filtering #endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 68f63571..3048b4aa 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -8,26 +8,23 @@ #include "ars_pointcloud_filter.hpp" - /** * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar -* topics from ARS ROSbags and publishes to "filtered" radar topic. +* topics from ARS ROSbags and publishes to "processed" radar topic. */ class ARSPointCloudFilterNode : public rclcpp::Node { public: + /** + * PointCloudFilter Node Constructor. + */ + ARSPointCloudFilterNode(); - /** - * PointCloudFilter Node Constructor. - */ - ARSPointCloudFilterNode(); - - filtering::ARSPointCloudFilter::filter_parameters parameters; + filtering::ARSPointCloudFilter::filter_parameters parameters; private: - /** - * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfiltered" * topic * * @param msg a raw message from the "unfiltered" topic @@ -44,19 +41,17 @@ class ARSPointCloudFilterNode : public rclcpp::Node void unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg); - // ROS2 Subscriber listening to the unfiltered radar packet topic. + // ROS2 Subscriber listening to the unfiltered radar packet topic (left sensor). rclcpp::Subscription::SharedPtr raw_left_sub_; - // ROS2 Subscriber listening to the unfiltered radar packet topic. + // ROS2 Subscriber listening to the unfiltered radar packet topic (right sensor). 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_; - // Add an object below from radar_pointcloud_filter.hpp that contains the methods + // Object containing methods for near and far scan filters filtering::ARSPointCloudFilter pointcloudfilter_; - }; - #endif // ARS_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py index aa518ac8..d331a045 100755 --- a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py @@ -22,18 +22,10 @@ def generate_launch_description(): {'snr': -99999.99}, {'range': -99999.99}, {'az_ang0': -99999.99}, - {'scan': 'near'} + {'scan_mode': 'near'} ] - ) - + ) return LaunchDescription([ ars_pointcloud_filter_param, ars_pointcloud_filter_node - ]) - vrel_rad: -99999.99 - el_ang: -99999.99 - rcs0: -99999.99 - snr: -99999.99 - range: -99999.99 - az_ang0: -99999.99 - scan: "near" \ No newline at end of file + ]) \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index a7383b02..d01b2109 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -59,10 +59,11 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( return filtered_ars; } - // Near Scan Filter Implementation -bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) + +bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet) { if((msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5)) { @@ -76,18 +77,20 @@ bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Append all detections to buffer - buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + + // Append all detections to buffer + buffer_packet.detections.insert(buffer_packet.detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); return false; - } + else { /** - Publish buffer packet since incoming message now has a different timestamp. - This means that the buffer packet is full with detections from 18 packets of the same timestamp. + Publish buffer packet since the incoming message will now have a different timestamp. + This means that the buffer packet is full with detections from 18 packets + of the same timestamp. **/ publish_packet = buffer_packet; @@ -104,7 +107,7 @@ bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S } else { - // Filter incoming data based on parameters + // Filter incoming data based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); @@ -114,19 +117,19 @@ bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S return false; } - } + } return false; } - // Far Scan Filter Implementation bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet) { if((msg->event_id == 1 || msg->event_id == 2)) { - // Filter out near scan packets + // Filter out near scan packets if(!buffer_packet.detections.empty()) { // If timestamp is the same as buffer packet @@ -137,34 +140,38 @@ bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::Sh msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append all detections to buffer - buffer_packet.detections.insert(buffer_packet.detections.end(), test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + // Append all detections to buffer packet + buffer_packet.detections.insert(buffer_packet.detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); return false; } + else { /** - Publish buffer packet since incoming message now has a different timestamp. - This means that the buffer packet is full with detections from 18 packets of the same timestamp. + Publish buffer packet since the incoming message will now have a different timestamp. + This means that the buffer packet is full of detections from 18 packets + of the same timestamp. **/ publish_packet = buffer_packet; - // // Filter new incoming data + // Filter new incoming data const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering + // Replace existing data in buffer packet with new data (new timestamp) after filtering buffer_packet = test_filtered_ars; return true; } } + else { - // Filter incoming data based on parameters + // Filter incoming data based on parameters const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); @@ -179,17 +186,15 @@ bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::Sh return false; } - // Near + Far Scan Filter Implementation (Double Buffer Algorithm) bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet) { - // Rearranged Implementation - // Check if its near if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) { - // Default case + // Default case when no near packets are appended if (near_timestamp == default_timestamp) { near_timestamp = msg->timestamp; @@ -198,20 +203,21 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Check if new message is part of the same scan if (msg->timestamp == near_timestamp) { - // Append to buffer packet 1 + // Append to buffer packet 1 // Filter out detections based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append all detections to buffer packet 1 - near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + // Append all detections to buffer packet 1 + near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); return false; } - // This means 18 packets are collected in buffer packet 1 (all with the same timestamps) => new near scan data + // This means all 18 near scan packets are in buffer packet 1 (all with the same timestamps) else { next_near_timestamp = msg->timestamp; @@ -220,9 +226,10 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append to buffer packet 2 - near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + // Append to buffer packet 2 + near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); return false; @@ -232,7 +239,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Check if its far if (msg->event_id == 1 || msg->event_id == 2) { - // Default case + // Default case when no far packets are appended if (far_timestamp == default_timestamp) { far_timestamp = msg->timestamp; @@ -241,37 +248,38 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Check if new message is part of the same scan if (msg->timestamp == far_timestamp) { - // Append to buffer packet 1 + // Append to buffer packet 1 // Filter out detections based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append all detections to buffer packet 1 - near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - + // Append all detections to buffer packet 1 + near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); return false; } - // This means 12 packets are collected in buffer packet 1 (all with the same timestamps)=> new far scan data + // This means that all 12 far scan packets are in buffer packet 1 (all with the same timestamps) else { next_far_timestamp = msg->timestamp; - // This means we technically have 30 packets in buffer packet 1 at this point + // This also means that there are 30 packets in buffer packet 1 at this point // Publish buffer_packet publish_packet = near_far_buffer_packets[buffer_index]; - // Filter new far scan incoming data + // Filter new far scan incoming data const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append to buffer packet 2 - near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + // Append to buffer packet 2 + near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); // Change timestamps, next_near and next_far become the "new" timestamps to check if incoming messages are part of the same scan near_timestamp = next_near_timestamp; @@ -280,12 +288,12 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Clear the buffer packet array near_far_buffer_packets[buffer_index].detections.clear(); - // Change index so next iteration, buffer packet 2 is considered to be part of scan 1 and buffer packet 1 is part of scan 2 + // Flip index for next iteration buffer_index = 1 - buffer_index; return true; - } + } return false; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 8f87b16d..e9b2b3a7 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -6,7 +6,8 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") { - //default values already declared in ars_radar_params.yaml + // Default values already declared in ars_radar_params.yaml + this->declare_parameter("filter_mode"); this->declare_parameter("scan_mode"); this->declare_parameter("vrel_rad"); @@ -62,7 +63,6 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); } - } // If scan mode is far @@ -89,15 +89,17 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( // Create a temporary empty packet for when we have a full near and far scan filtered packet ready to be published radar_msgs::msg::RadarPacket publish_packet_near_far; - if(pointcloudfilter_.near_far_scan_filter(msg,parameters,publish_packet_near_far) == true) + /** + Send message to near_far_scan_filter and append detections to the buffer packets based on timestamp. + Check if packet is ready to be published (30 packets). Return true if ready to be published, return false otherwise. + **/ + if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far) == true) { // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); left_right_pub_->publish(publish_packet_near_far); } - } - } void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( @@ -105,18 +107,15 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - // Send Unfiltered packets along with filter thresholds if(parameters.scan_mode == "near") { radar_msgs::msg::RadarPacket publish_packet; if(pointcloudfilter_.near_scan_filter(msg, parameters, publish_packet) == true) { - // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); } - } else if (parameters.scan_mode == "far") @@ -125,27 +124,22 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( if(pointcloudfilter_.far_scan_filter(msg, parameters, publish_packet) == true) { - // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); } } - // If scan mode is near_far (Double Buffer) else { - // Create a temporary empty packet for when we have a full near and far scan filtered packet ready to be published radar_msgs::msg::RadarPacket publish_packet_near_far; - if(pointcloudfilter_.near_far_scan_filter(msg,parameters,publish_packet_near_far) == true) + if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far) == true) { // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); left_right_pub_->publish(publish_packet_near_far); } - } - } int main(int argc, char ** argv) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index e69de29b..442b16c1 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -0,0 +1,8 @@ +#include +#include + +#include "gtest/gtest.h" +#include "rclcpp/rclcpp.hpp" + +#include "ars_pointcloud_filter.hpp" + From d22c1b2bc103bb75a35ccd62e794b9bcc20a8be3 Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 28 Feb 2023 01:46:43 +0000 Subject: [PATCH 27/51] updated code structure and started testing --- .../ARSPointCloudFilter/include/ars_pointcloud_filter.hpp | 8 ++++---- .../ARSPointCloudFilter/src/ars_pointcloud_filter.cpp | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index b02937af..9184340f 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -36,16 +36,16 @@ class ARSPointCloudFilter } filter_parameters; bool near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet); + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet); bool far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); bool near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet); + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet); private: // Create a buffer packet to hold detections from incoming messages (with the same timestamps) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index d01b2109..49ddb156 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -203,7 +203,6 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Check if new message is part of the same scan if (msg->timestamp == near_timestamp) { - // Append to buffer packet 1 // Filter out detections based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, @@ -217,6 +216,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke return false; } + // This means all 18 near scan packets are in buffer packet 1 (all with the same timestamps) else { @@ -248,7 +248,6 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Check if new message is part of the same scan if (msg->timestamp == far_timestamp) { - // Append to buffer packet 1 // Filter out detections based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, @@ -261,6 +260,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke return false; } + // This means that all 12 far scan packets are in buffer packet 1 (all with the same timestamps) else { From d63f61bd5e7f255b91c60043ee9bb84042c79fc6 Mon Sep 17 00:00:00 2001 From: Rijin Date: Thu, 2 Mar 2023 03:44:04 +0000 Subject: [PATCH 28/51] changed near and far filter implementation; added counts --- .../include/ars_pointcloud_filter.hpp | 8 + .../src/ars_pointcloud_filter.cpp | 172 +++++++++--------- .../test/ars_pointcloud_filter_test.cpp | 64 ++++++- 3 files changed, 156 insertions(+), 88 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 9184340f..45182527 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -48,9 +48,17 @@ class ARSPointCloudFilter radar_msgs::msg::RadarPacket &publish_packet); private: + + // For near and far scan filters only + int near_packet_count_; + int far_packet_count_; + unsigned int packet_timestamp_; + // Create a buffer packet to hold detections from incoming messages (with the same timestamps) radar_msgs::msg::RadarPacket buffer_packet; + + // Create two buffer packets (for when both near and far scan mode filters are activated) radar_msgs::msg::RadarPacket near_far_buffer_packets[2]; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 49ddb156..3db5265e 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -10,6 +10,10 @@ namespace filtering ARSPointCloudFilter::ARSPointCloudFilter() { + near_packet_count_ = 0; + far_packet_count_ = 0; + packet_timestamp_ = -1; + buffer_index = 0; default_timestamp = -1; near_timestamp = -1; @@ -61,131 +65,125 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( // Near Scan Filter Implementation +// Test cases +// 1. Single packet (near) +// 2. Single packet (far) +// 3. Multiple packets with same timestamps (near) +// 4. Multiple packets with different timestamps (near) + bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { - if((msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5)) + if(msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) { - // Filter out far scan packets - if(!buffer_packet.detections.empty()) - { - // If timestamp is the same as buffer packet - if(msg->timestamp == buffer_packet.timestamp) - { - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Append all detections to buffer - buffer_packet.detections.insert(buffer_packet.detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - return false; - } + if(packet_timestamp_ == -1) + { + packet_timestamp_ = msg->timestamp; + } - else - { - /** - Publish buffer packet since the incoming message will now have a different timestamp. - This means that the buffer packet is full with detections from 18 packets - of the same timestamp. - **/ - publish_packet = buffer_packet; - - // Filter new incoming data - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering - buffer_packet = test_filtered_ars; - - return true; - } + // If near packet is not full + if(near_packet_count_ != 18 && msg->timestamp == packet_timestamp_) + { + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // Append all detections to buffer + buffer_packet.detections.insert(buffer_packet.detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + near_packet_count_++; + return false; } else { - // Filter incoming data based on given thresholds + /** + Publish buffer packet since we have 18 packets. + **/ + if(near_packet_count_!=18) + { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Near packet is not full, size: %d ! \n ",near_packet_count_); + } + publish_packet = buffer_packet; + + // Filter new incoming data const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append filtered data to the buffer packet + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet = test_filtered_ars; + near_packet_count_ = 1; - return false; - } + assert(msg->timestamp!=packet_timestamp_); + packet_timestamp_ = msg->timestamp; + return true; + } } return false; } + // Far Scan Filter Implementation bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { - if((msg->event_id == 1 || msg->event_id == 2)) +if(msg->event_id == 1 || msg->event_id == 2) { - // Filter out near scan packets - if(!buffer_packet.detections.empty()) - { - // If timestamp is the same as buffer packet - if(msg->timestamp == buffer_packet.timestamp) - { - // Filter incoming data based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Append all detections to buffer packet - buffer_packet.detections.insert(buffer_packet.detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - - return false; - } - - else - { - /** - Publish buffer packet since the incoming message will now have a different timestamp. - This means that the buffer packet is full of detections from 18 packets - of the same timestamp. - **/ - publish_packet = buffer_packet; - - // Filter new incoming data - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Replace existing data in buffer packet with new data (new timestamp) after filtering - buffer_packet = test_filtered_ars; - - return true; - } + if(packet_timestamp_ == -1) + { + packet_timestamp_ = msg->timestamp; } + // If near packet is not full + if(far_packet_count_ != 12 && msg->timestamp == packet_timestamp_) + { + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // Append all detections to buffer + buffer_packet.detections.insert(buffer_packet.detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + far_packet_count_++; + return false; + } else { - // Filter incoming data based on parameters - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + /** + Publish buffer packet since we have 18 packets. + **/ + if(far_packet_count_!=12) + { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Far packet is not full, size: %d ! \n ",far_packet_count_); + } + publish_packet = buffer_packet; - // Append filtered data to the buffer packet + // Filter new incoming data + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet = test_filtered_ars; + far_packet_count_ = 1; - return false; + assert(msg->timestamp!=packet_timestamp_); + + packet_timestamp_ = msg->timestamp; + return true; } } - return false; } +// CHANGE FROM TIMESTAMP TO COUNTS!! (30) Add Asserts and log statements // Near + Far Scan Filter Implementation (Double Buffer Algorithm) bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 442b16c1..2c4fd9cb 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -1,8 +1,70 @@ #include -#include +#include +#include #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" #include "ars_pointcloud_filter.hpp" +TEST(ARSPointCloudFilterTest, PointFilter) +{ + filtering::ARSPointCloudFilter pointcloudfilter; + auto msg = std::make_shared(); + auto msg_detection = std::make_shared(); + + msg->event_id = 3; + msg->timestamp = 2; + msg->measurement_counter = 1; + msg->vambig = 3.0; + msg->center_frequency = 6.0; + + msg_detection->pos_x = 1.0; + msg_detection->pos_y = 3.0; + msg_detection->pos_z = 4.0; + msg_detection->vrel_rad = 100.0; + msg_detection->az_ang0 = 60.0; + msg_detection->az_ang1 = 50.0; + msg_detection->el_ang = 20.0; + msg_detection->rcs0 = 5.0; + msg_detection->rcs1 = 10.0; + msg_detection->range_var = 600.0; + msg_detection->vrel_rad_var = 175.0; + msg_detection->az_ang_var0 = 12.0; + msg_detection->az_ang_var1 = 17.0; + msg_detection->el_ang_var = 19.0; + msg_detection->snr = 100.0; + msg_detection->range = 90.0; + msg_detection->prob0 = 0.0; + msg_detection->prob1 = 1.0; + msg_detection->pdh0 = 255.0; + + msg->detections.push_back(*msg_detection); + + + msg_detection->pos_x = 1.0; + msg_detection->pos_y = 3.0; + msg_detection->pos_z = 4.0; + msg_detection->vrel_rad = 100.0; + msg_detection->az_ang0 = 60.0; + msg_detection->az_ang1 = 50.0; + msg_detection->el_ang = 20.0; + msg_detection->rcs0 = 5.0; + msg_detection->rcs1 = 10.0; + msg_detection->range_var = 600.0; + msg_detection->vrel_rad_var = 175.0; + msg_detection->az_ang_var0 = 12.0; + msg_detection->az_ang_var1 = 17.0; + msg_detection->el_ang_var = 19.0; + msg_detection->snr = 300.0; + msg_detection->range = 90.0; + msg_detection->prob0 = 0.0; + msg_detection->prob1 = 1.0; + msg_detection->pdh0 = 255.0; + + msg->detections.push_back(*msg_detection); + + auto test_packet = pointcloudfilter.point_filter(msg, 200, -9999.99, -9999.99, -9999.99, -9999.99, -9999.99); + + EXPECT_EQ(300, test_packet.detections[0].snr); +} From 738b44edd4c2b8ffdf1460efda040185ef4c5f10 Mon Sep 17 00:00:00 2001 From: Rijin Date: Fri, 3 Mar 2023 05:01:22 +0000 Subject: [PATCH 29/51] updated near, far scan and double buffer implementation --- .../include/ars_pointcloud_filter.hpp | 15 ++-- .../src/ars_pointcloud_filter.cpp | 79 ++++++++++--------- 2 files changed, 48 insertions(+), 46 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 45182527..45a23c82 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -57,18 +57,17 @@ class ARSPointCloudFilter // Create a buffer packet to hold detections from incoming messages (with the same timestamps) radar_msgs::msg::RadarPacket buffer_packet; - - // Create two buffer packets (for when both near and far scan mode filters are activated) radar_msgs::msg::RadarPacket near_far_buffer_packets[2]; // Variables for double buffer - int buffer_index; - unsigned int default_timestamp; - unsigned int near_timestamp; - unsigned int far_timestamp; - unsigned int next_near_timestamp; - unsigned int next_far_timestamp; + int buffer_index_; + unsigned int default_timestamp_; + unsigned int near_timestamp_; + unsigned int far_timestamp_; + unsigned int next_near_timestamp_; + unsigned int next_far_timestamp_; + int total_packet_count[2]; }; } // namespace filtering diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 3db5265e..5aeaef6a 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -14,10 +14,10 @@ ARSPointCloudFilter::ARSPointCloudFilter() far_packet_count_ = 0; packet_timestamp_ = -1; - buffer_index = 0; - default_timestamp = -1; - near_timestamp = -1; - far_timestamp = -1; + buffer_index_ = 0; + default_timestamp_ = -1; + near_timestamp_ = -1; + far_timestamp_ = -1; } // Point Filter Implementation @@ -77,7 +77,7 @@ bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S { if(msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) { - if(packet_timestamp_ == -1) + if(packet_timestamp_ == default_timestamp_) { packet_timestamp_ = msg->timestamp; } @@ -134,7 +134,7 @@ bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::Sh { if(msg->event_id == 1 || msg->event_id == 2) { - if(packet_timestamp_ == -1) + if(packet_timestamp_ == default_timestamp_) { packet_timestamp_ = msg->timestamp; } @@ -193,13 +193,13 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) { // Default case when no near packets are appended - if (near_timestamp == default_timestamp) + if (near_timestamp_ == default_timestamp_) { - near_timestamp = msg->timestamp; + near_timestamp_ = msg->timestamp; } // Check if new message is part of the same scan - if (msg->timestamp == near_timestamp) + if (msg->timestamp == near_timestamp_) { // Filter out detections based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( @@ -207,44 +207,41 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append all detections to buffer packet 1 - near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); - - return false; - + total_packet_count[buffer_index_]++; } // This means all 18 near scan packets are in buffer packet 1 (all with the same timestamps) else { - next_near_timestamp = msg->timestamp; + next_near_timestamp_ = msg->timestamp; const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append to buffer packet 2 - near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); - - return false; - + total_packet_count[1-buffer_index_]++; } + return false; } // Check if its far if (msg->event_id == 1 || msg->event_id == 2) { // Default case when no far packets are appended - if (far_timestamp == default_timestamp) + if (far_timestamp_ == default_timestamp_) { - far_timestamp = msg->timestamp; + far_timestamp_ = msg->timestamp; } // Check if new message is part of the same scan - if (msg->timestamp == far_timestamp) + if (msg->timestamp == far_timestamp_) { // Filter out detections based on given thresholds const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( @@ -252,22 +249,16 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append all detections to buffer packet 1 - near_far_buffer_packets[buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); - return false; - + total_packet_count[buffer_index_]++; } // This means that all 12 far scan packets are in buffer packet 1 (all with the same timestamps) else { - next_far_timestamp = msg->timestamp; - - // This also means that there are 30 packets in buffer packet 1 at this point - - // Publish buffer_packet - publish_packet = near_far_buffer_packets[buffer_index]; + next_far_timestamp_ = msg->timestamp; // Filter new far scan incoming data const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( @@ -275,25 +266,37 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); // Append to buffer packet 2 - near_far_buffer_packets[1 - buffer_index].detections.insert(near_far_buffer_packets[buffer_index].detections.end(), + near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); + total_packet_count[1 - buffer_index_]++; + } + if(total_packet_count[buffer_index_] == 30) + { + // This means that there are 30 packets in buffer packet 1 at this point + + // Publish buffer_packet + publish_packet = near_far_buffer_packets[buffer_index_]; + // Change timestamps, next_near and next_far become the "new" timestamps to check if incoming messages are part of the same scan - near_timestamp = next_near_timestamp; - far_timestamp = next_far_timestamp; + near_timestamp_ = next_near_timestamp_; + far_timestamp_ = next_far_timestamp_; // Clear the buffer packet array - near_far_buffer_packets[buffer_index].detections.clear(); - + near_far_buffer_packets[buffer_index_].detections.clear(); + + total_packet_count[buffer_index_] = 0; + // Flip index for next iteration - buffer_index = 1 - buffer_index; + buffer_index_ = 1 - buffer_index_; return true; - } + } + return false; } - + return false; } From 6d2b65b4f3a5f537eaaea286f412b4df40f701d0 Mon Sep 17 00:00:00 2001 From: Rijin Date: Mon, 6 Mar 2023 02:50:22 +0000 Subject: [PATCH 30/51] updated near + far scan filters (double buffer algo) --- .../include/ars_pointcloud_filter.hpp | 46 ++- .../src/ars_pointcloud_filter.cpp | 378 +++++++++++------- .../src/ars_pointcloud_filter_node.cpp | 2 +- .../test/ars_pointcloud_filter_test.cpp | 6 + 4 files changed, 270 insertions(+), 162 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 45a23c82..deed3a68 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -10,6 +10,13 @@ namespace filtering { + +enum scan_type +{ + NEAR, + FAR +}; + class ARSPointCloudFilter { public: @@ -34,7 +41,7 @@ class ARSPointCloudFilter double range_param; double az_ang0_param; } filter_parameters; - + bool near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); @@ -46,30 +53,41 @@ class ARSPointCloudFilter bool near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + + scan_type check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars); + + void reset_scan_states(const int &buffer_index); private: + // For all Filters + unsigned int default_timestamp_; // For near and far scan filters only - int near_packet_count_; - int far_packet_count_; + int packet_count_; unsigned int packet_timestamp_; // Create a buffer packet to hold detections from incoming messages (with the same timestamps) - radar_msgs::msg::RadarPacket buffer_packet; + radar_msgs::msg::RadarPacket buffer_packet_; - // Create two buffer packets (for when both near and far scan mode filters are activated) - radar_msgs::msg::RadarPacket near_far_buffer_packets[2]; + // Create a struct for implementing current and next timestamps in near and far scans + typedef struct + { + unsigned int timestamp_; + int total_packet_count_; + bool publish_status_; + } scan_params; - // Variables for double buffer int buffer_index_; - unsigned int default_timestamp_; - unsigned int near_timestamp_; - unsigned int far_timestamp_; - unsigned int next_near_timestamp_; - unsigned int next_far_timestamp_; - int total_packet_count[2]; + + // Create two buffer packets (for when both near and far scan mode filters are activated) + std::array near_far_buffer_packets_; + + // Not pointers + std::array *near_scan_; + std::array *far_scan_; + }; } // namespace filtering -#endif // ARS_POINTCLOUD_FILTER_HPP_ +#endif // ARS_POINTCLOUD_FILTER_HPP_ \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 5aeaef6a..ed509fc4 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -10,14 +10,15 @@ namespace filtering ARSPointCloudFilter::ARSPointCloudFilter() { - near_packet_count_ = 0; - far_packet_count_ = 0; + packet_count_ = 0; packet_timestamp_ = -1; + + // make timestamps equal to 0 buffer_index_ = 0; - default_timestamp_ = -1; - near_timestamp_ = -1; - far_timestamp_ = -1; + default_timestamp_ = 0; + // near_timestamp_ = 0; + // far_timestamp_ = 0; } // Point Filter Implementation @@ -63,14 +64,32 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( return filtered_ars; } -// Near Scan Filter Implementation +// Implementation for checking event id and returning which scan it is (NEAR OR FAR) +scan_type ARSPointCloudFilter::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 ARSPointCloudFilter::reset_scan_states(const int &buffer_index) +{ + near_scan_[buffer_index].timestamp_ = 0; + near_scan_[buffer_index].publish_status_ = false; + near_scan_[buffer_index].total_packet_count_= 0; -// Test cases -// 1. Single packet (near) -// 2. Single packet (far) -// 3. Multiple packets with same timestamps (near) -// 4. Multiple packets with different timestamps (near) + far_scan_[buffer_index].timestamp_ = 0; + far_scan_[buffer_index].publish_status_ = false; + far_scan_[buffer_index].total_packet_count_= 0; +} + +// Near Scan Filter Implementation bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) @@ -81,43 +100,34 @@ bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::S { packet_timestamp_ = msg->timestamp; } + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = 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 near packet is not full - if(near_packet_count_ != 18 && msg->timestamp == packet_timestamp_) + if(packet_count_ != 18 && msg->timestamp == packet_timestamp_) { - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append all detections to buffer - buffer_packet.detections.insert(buffer_packet.detections.end(), + buffer_packet_.detections.insert(buffer_packet_.detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); - near_packet_count_++; - return false; + packet_count_++; } else { /** Publish buffer packet since we have 18 packets. **/ - if(near_packet_count_!=18) + if(packet_count_!=18) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Near packet is not full, size: %d ! \n ",near_packet_count_); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Near packet is not full, size: %d ! \n ",packet_count_); } - publish_packet = buffer_packet; - - // Filter new incoming data - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + publish_packet = buffer_packet_; // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering - buffer_packet = test_filtered_ars; - near_packet_count_ = 1; - - assert(msg->timestamp!=packet_timestamp_); + buffer_packet_ = test_filtered_ars; + packet_count_ = 1; packet_timestamp_ = msg->timestamp; return true; @@ -132,49 +142,41 @@ bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::Sh const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { -if(msg->event_id == 1 || msg->event_id == 2) + if(msg->event_id == 1 || msg->event_id == 2) { if(packet_timestamp_ == default_timestamp_) { packet_timestamp_ = msg->timestamp; } + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = 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 near packet is not full - if(far_packet_count_ != 12 && msg->timestamp == packet_timestamp_) + if(packet_count_ != 12 && msg->timestamp == packet_timestamp_) { - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append all detections to buffer - buffer_packet.detections.insert(buffer_packet.detections.end(), + buffer_packet_.detections.insert(buffer_packet_.detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); - far_packet_count_++; - return false; + packet_count_++; } else { /** Publish buffer packet since we have 18 packets. **/ - if(far_packet_count_!=12) - { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Far packet is not full, size: %d ! \n ",far_packet_count_); - } - publish_packet = buffer_packet; - - // Filter new incoming data - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering - buffer_packet = test_filtered_ars; - far_packet_count_ = 1; + if(packet_count_!=12) + { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Far packet is not full, size: %d ! \n ", packet_count_); + } + publish_packet = buffer_packet_; - assert(msg->timestamp!=packet_timestamp_); + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering + buffer_packet_ = test_filtered_ars; + packet_count_ = 1; packet_timestamp_ = msg->timestamp; return true; @@ -183,121 +185,203 @@ if(msg->event_id == 1 || msg->event_id == 2) return false; } -// CHANGE FROM TIMESTAMP TO COUNTS!! (30) Add Asserts and log statements + // Near + Far Scan Filter Implementation (Double Buffer Algorithm) bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet) + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet) { - // Check if its near - if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) - { - // Default case when no near packets are appended - if (near_timestamp_ == default_timestamp_) - { - near_timestamp_ = msg->timestamp; - } + // Variables to create on every call back (message can be near or far) + scan_type incoming_scan_msg = check_scan_type(msg); - // Check if new message is part of the same scan - if (msg->timestamp == near_timestamp_) - { - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Append all detections to buffer packet 1 - near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - total_packet_count[buffer_index_]++; - } + int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; - // This means all 18 near scan packets are in buffer packet 1 (all with the same timestamps) - else - { - next_near_timestamp_ = msg->timestamp; + // Returns which scan parameters the program needs to work on (not a pointer) + std::array* scan = (incoming_scan_msg == NEAR) ? near_scan_ : far_scan_; - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append to buffer packet 2 - near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - total_packet_count[1-buffer_index_]++; - } - return false; + if(msg->timestamp == default_timestamp_) + { + scan[buffer_index_].timestamp_ = msg->timestamp; } - // Check if its far - if (msg->event_id == 1 || msg->event_id == 2) + if(scan[buffer_index_].timestamp_ == msg->timestamp) { - // Default case when no far packets are appended - if (far_timestamp_ == default_timestamp_) - { - far_timestamp_ = msg->timestamp; - } + near_far_buffer_packets_[buffer_index_].detections.insert(near_far_buffer_packets_[buffer_index_].detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); - // Check if new message is part of the same scan - if (msg->timestamp == far_timestamp_) + scan[buffer_index_].total_packet_count_++; + } + else + { + if(scan[buffer_index_].total_packet_count_ != scan_capacity) { - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // Append all detections to buffer packet 1 - near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - total_packet_count[buffer_index_]++; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, Actual Size: %d, Expected: %d ! \n ", scan[buffer_index_].total_packet_count_, scan_capacity); } + // ready to be published + scan[buffer_index_].publish_status_ = true; + + near_far_buffer_packets_[1 - buffer_index_].detections.insert(near_far_buffer_packets_[buffer_index_].detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + scan[1 - buffer_index_].total_packet_count_++; + } + + if(scan[buffer_index_].total_packet_count_ == scan_capacity) + { + scan[buffer_index_].publish_status_ == true; + } + + if(near_scan_[buffer_index_].publish_status_ == true && far_scan_[buffer_index_].publish_status_ == true) + { + publish_packet = near_far_buffer_packets_[buffer_index_]; - // This means that all 12 far scan packets are in buffer packet 1 (all with the same timestamps) - else - { - next_far_timestamp_ = msg->timestamp; + near_far_buffer_packets_[buffer_index_].detections.clear(); + + reset_scan_states(buffer_index_); + + buffer_index_ = 1 - buffer_index_; + + return true; + } + return false; + + + // Edge Cases + // 1. When there are more than 18 or 12 packets of the same timestamp being sent (rare) + // 2. Data collections start in the middle instead of right in the beginning (0 packet count) + + + + - // Filter new far scan incoming data - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - // Append to buffer packet 2 - near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - total_packet_count[1 - buffer_index_]++; - } - if(total_packet_count[buffer_index_] == 30) - { - // This means that there are 30 packets in buffer packet 1 at this point - // Publish buffer_packet - publish_packet = near_far_buffer_packets[buffer_index_]; + + + + + + + + + + + + + + // // Check if its near + // if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) + // { + // // Default case when no near packets are appended + // if (near_timestamp_ == default_timestamp_) + // { + // near_timestamp_ = msg->timestamp; + // } + + // // Check if new message is part of the same scan + // if (msg->timestamp == near_timestamp_) + // { + // // Filter out detections based on given thresholds + // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // // Append all detections to buffer packet 1 + // near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), + // test_filtered_ars.detections.begin(), + // test_filtered_ars.detections.end()); + // total_packet_count[buffer_index_]++; + // } + + // // This means all 18 near scan packets are in buffer packet 1 (all with the same timestamps) + // else + // { + // next_near_timestamp_ = msg->timestamp; + + // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // // Append to buffer packet 2 + // near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), + // test_filtered_ars.detections.begin(), + // test_filtered_ars.detections.end()); + // total_packet_count[1-buffer_index_]++; + // } + // } + + // // Check if its far + // else if (msg->event_id == 1 || msg->event_id == 2) + // { + // // Default case when no far packets are appended + // if (far_timestamp_ == default_timestamp_) + // { + // far_timestamp_ = msg->timestamp; + // } + + // // Check if new message is part of the same scan + // if (msg->timestamp == far_timestamp_) + // { + // // Filter out detections based on given thresholds + // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // // Append all detections to buffer packet 1 + // near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), + // test_filtered_ars.detections.begin(), + // test_filtered_ars.detections.end()); + // total_packet_count[buffer_index_]++; + // } - // Change timestamps, next_near and next_far become the "new" timestamps to check if incoming messages are part of the same scan - near_timestamp_ = next_near_timestamp_; - far_timestamp_ = next_far_timestamp_; + // // This means that all 12 far scan packets are in buffer packet 1 (all with the same timestamps) + // else + // { + // next_far_timestamp_ = msg->timestamp; + + // // Filter new far scan incoming data + // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + + // // Append to buffer packet 2 + // near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), + // test_filtered_ars.detections.begin(), + // test_filtered_ars.detections.end()); + // total_packet_count[1 - buffer_index_]++; + // } + + // if(total_packet_count[buffer_index_] == 30) + // { + // // This means that there are 30 packets in buffer packet 1 at this point + + // // Publish buffer_packet + // publish_packet = near_far_buffer_packets[buffer_index_]; + + // // Change timestamps, next_near and next_far become the "new" timestamps to check if incoming messages are part of the same scan + // near_timestamp_ = next_near_timestamp_; + // far_timestamp_ = next_far_timestamp_; - // Clear the buffer packet array - near_far_buffer_packets[buffer_index_].detections.clear(); + // // Clear the buffer packet array + // near_far_buffer_packets[buffer_index_].detections.clear(); - total_packet_count[buffer_index_] = 0; + // total_packet_count[buffer_index_] = 0; - // Flip index for next iteration - buffer_index_ = 1 - buffer_index_; + // // Flip index for next iteration + // buffer_index_ = 1 - buffer_index_; - return true; + // return true; - } - return false; - } + // } + // } - return false; + // return false; } } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index e9b2b3a7..f9d58cb4 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -93,7 +93,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( Send message to near_far_scan_filter and append detections to the buffer packets based on timestamp. Check if packet is ready to be published (30 packets). Return true if ready to be published, return false otherwise. **/ - if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far) == true) + if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 2c4fd9cb..a9405725 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -7,6 +7,12 @@ #include "ars_pointcloud_filter.hpp" +// Unit test cases +// 1. Single packet (near) +// 2. Single packet (far) +// 3. Multiple packets with same timestamps (near) +// 4. Multiple packets with different timestamps (near) + TEST(ARSPointCloudFilterTest, PointFilter) { filtering::ARSPointCloudFilter pointcloudfilter; From 4fc2b8aa6fdaba07c62d5456e8c3508c3df22d87 Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 7 Mar 2023 04:15:36 +0000 Subject: [PATCH 31/51] added new implementation for near, far scan and double buffer; added some tests as well --- .../include/ars_pointcloud_filter.hpp | 52 +-- .../src/ars_pointcloud_filter.cpp | 388 +++++------------- .../src/ars_pointcloud_filter_node.cpp | 41 +- .../test/ars_pointcloud_filter_test.cpp | 93 +++-- 4 files changed, 213 insertions(+), 361 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index deed3a68..6545a8cc 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -11,12 +11,6 @@ namespace filtering { -enum scan_type -{ - NEAR, - FAR -}; - class ARSPointCloudFilter { public: @@ -42,12 +36,14 @@ class ARSPointCloudFilter double az_ang0_param; } filter_parameters; - bool near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet); + enum scan_type + { + NEAR, + FAR + }; - bool far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - const filter_parameters ¶meters, + bool common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); bool near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, @@ -59,32 +55,42 @@ class ARSPointCloudFilter void reset_scan_states(const int &buffer_index); private: + // For all Filters unsigned int default_timestamp_; - // For near and far scan filters only - int packet_count_; - unsigned int packet_timestamp_; - - // Create a buffer packet to hold detections from incoming messages (with the same timestamps) - radar_msgs::msg::RadarPacket buffer_packet_; - // Create a struct for implementing current and next timestamps in near and far scans + // Create a struct with variables for near and far scans typedef struct { unsigned int timestamp_; - int total_packet_count_; + int packet_count_; bool publish_status_; } scan_params; + /* + Near and far Scan filters + */ + + scan_params near_scan_single_; + scan_params far_scan_single_; + + // Create a buffer packet to hold detections from incoming messages (with the same timestamps) + radar_msgs::msg::RadarPacket buffer_packet_; + + + /* + Near + Far Scan Filter (Double buffer) + */ + int buffer_index_; - // Create two buffer packets (for when both near and far scan mode filters are activated) + // Create two buffer packets std::array near_far_buffer_packets_; - // Not pointers - std::array *near_scan_; - std::array *far_scan_; + // Create an array of structs for each scan type + std::array near_scan_; + std::array far_scan_; }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index ed509fc4..1b364171 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -10,62 +10,74 @@ namespace filtering ARSPointCloudFilter::ARSPointCloudFilter() { - packet_count_ = 0; - packet_timestamp_ = -1; - + 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; - // make timestamps equal to 0 buffer_index_ = 0; default_timestamp_ = 0; - // near_timestamp_ = 0; - // far_timestamp_ = 0; + } // Point Filter Implementation radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double snr_threshold, - double AzAng0_threshold, - double range_threshold, - double vrel_rad_threshold, - double el_ang_threshold, - double rcs_threshold) + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, 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_ars; + for (auto detection : unfiltered_ars->detections) + { + if(detection.snr < snr_threshold) + { + continue; + } + else if (detection.az_ang0 < AzAng0_threshold) { - radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection : unfiltered_ars->detections) - { - if(detection.snr < snr_threshold) - { - continue; - } - else if (detection.az_ang0 < AzAng0_threshold) - { - continue; - } - else if (detection.range < range_threshold) - { - continue; - } - else if (detection.vrel_rad < vrel_rad_threshold) - { - continue; - } - else if (detection.el_ang < el_ang_threshold) - { - continue; - } - else if (detection.rcs0 < rcs_threshold) - { - continue; - } - filtered_ars.detections.push_back(detection); - } - return filtered_ars; + continue; } + else if (detection.range < range_threshold) + { + continue; + } + else if (detection.vrel_rad < vrel_rad_threshold) + { + continue; + } + else if (detection.el_ang < el_ang_threshold) + { + continue; + } + else if (detection.rcs0 < rcs_threshold) + { + continue; + } + filtered_ars.detections.push_back(detection); + } + return filtered_ars; +} -// Implementation for checking event id and returning which scan it is (NEAR OR FAR) -scan_type ARSPointCloudFilter::check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr msg) +// Checks Event ID and returns which scan it is (NEAR OR FAR) +ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr msg) { if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) { @@ -81,129 +93,84 @@ void ARSPointCloudFilter::reset_scan_states(const int &buffer_index) { near_scan_[buffer_index].timestamp_ = 0; near_scan_[buffer_index].publish_status_ = false; - near_scan_[buffer_index].total_packet_count_= 0; + near_scan_[buffer_index].packet_count_= 0; far_scan_[buffer_index].timestamp_ = 0; far_scan_[buffer_index].publish_status_ = false; - far_scan_[buffer_index].total_packet_count_= 0; + far_scan_[buffer_index].packet_count_= 0; } -// Near Scan Filter Implementation -bool ARSPointCloudFilter::near_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, +// A common filter for near and far scan modes +bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { - if(msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) - { - if(packet_timestamp_ == default_timestamp_) - { - packet_timestamp_ = msg->timestamp; - } - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); + // Variables to create on every callback (message can be near or far) + scan_type incoming_scan_msg = check_scan_type(msg); - // If near packet is not full - if(packet_count_ != 18 && msg->timestamp == packet_timestamp_) - { - // Append all detections to buffer - buffer_packet_.detections.insert(buffer_packet_.detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - packet_count_++; - } - else - { - /** - Publish buffer packet since we have 18 packets. - **/ - if(packet_count_!=18) - { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Near packet is not full, size: %d ! \n ",packet_count_); - } - publish_packet = buffer_packet_; - - // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering - buffer_packet_ = test_filtered_ars; - packet_count_ = 1; - - packet_timestamp_ = msg->timestamp; - return true; - } - } - return false; -} + int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; + // Returns which scan parameters the program needs to work on + auto scan = (incoming_scan_msg == NEAR) ? near_scan_single_ : far_scan_single_; -// Far Scan Filter Implementation -bool ARSPointCloudFilter::far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet) -{ - if(msg->event_id == 1 || msg->event_id == 2) - { - if(packet_timestamp_ == default_timestamp_) - { - packet_timestamp_ = msg->timestamp; - } - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = 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.timestamp_ == default_timestamp_) + { + scan.timestamp_ = msg->timestamp; + } + + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = 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 near packet is not full - if(packet_count_ != 12 && msg->timestamp == packet_timestamp_) - { - // Append all detections to buffer - buffer_packet_.detections.insert(buffer_packet_.detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - packet_count_++; - } - else + // If near/far packet is not full + if(scan.packet_count_ != scan_capacity && msg->timestamp == scan.timestamp_) + { + // Append all detections to buffer + buffer_packet_.detections.insert(buffer_packet_.detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + scan.packet_count_++; + } + else + { + if(scan.packet_count_!= scan_capacity) { - /** - Publish buffer packet since we have 18 packets. - **/ - if(packet_count_!=12) - { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Far packet is not full, size: %d ! \n ", packet_count_); - } - publish_packet = buffer_packet_; - - // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering - buffer_packet_ = test_filtered_ars; - packet_count_ = 1; - - packet_timestamp_ = msg->timestamp; - return true; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Near packet is not full, size: %d ! \n ", scan.packet_count_); } + // Publish buffer packet + publish_packet = buffer_packet_; + + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering + buffer_packet_ = test_filtered_ars; + scan.packet_count_ = 1; + + scan.timestamp_ = msg->timestamp; + return true; } return false; } - // Near + Far Scan Filter Implementation (Double Buffer Algorithm) bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { - // Variables to create on every call back (message can be near or far) + // Variables to create on every callback (message can be near or far) scan_type incoming_scan_msg = check_scan_type(msg); int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; - // Returns which scan parameters the program needs to work on (not a pointer) - std::array* scan = (incoming_scan_msg == NEAR) ? near_scan_ : far_scan_; + // Returns which scan parameters the program needs to work on + auto scan = (incoming_scan_msg == NEAR) ? near_scan_ : far_scan_; const radar_msgs::msg::RadarPacket test_filtered_ars = 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(msg->timestamp == default_timestamp_) + if(scan[buffer_index_].timestamp_ == default_timestamp_) { scan[buffer_index_].timestamp_ = msg->timestamp; } @@ -211,29 +178,29 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].timestamp_ == msg->timestamp) { near_far_buffer_packets_[buffer_index_].detections.insert(near_far_buffer_packets_[buffer_index_].detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); - scan[buffer_index_].total_packet_count_++; + scan[buffer_index_].packet_count_++; } else { - if(scan[buffer_index_].total_packet_count_ != scan_capacity) + if(scan[buffer_index_].packet_count_ != scan_capacity) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, Actual Size: %d, Expected: %d ! \n ", scan[buffer_index_].total_packet_count_, scan_capacity); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, Actual Size: %d, Expected: %d ! \n ", scan[buffer_index_].packet_count_, scan_capacity); } - // ready to be published + scan[buffer_index_].publish_status_ = true; near_far_buffer_packets_[1 - buffer_index_].detections.insert(near_far_buffer_packets_[buffer_index_].detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - scan[1 - buffer_index_].total_packet_count_++; + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + scan[1 - buffer_index_].packet_count_++; } - if(scan[buffer_index_].total_packet_count_ == scan_capacity) + if(scan[buffer_index_].packet_count_ == scan_capacity) { - scan[buffer_index_].publish_status_ == true; + scan[buffer_index_].publish_status_ = true; } if(near_scan_[buffer_index_].publish_status_ == true && far_scan_[buffer_index_].publish_status_ == true) @@ -250,138 +217,9 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke } return false; - - // Edge Cases + // Edge Cases to test // 1. When there are more than 18 or 12 packets of the same timestamp being sent (rare) // 2. Data collections start in the middle instead of right in the beginning (0 packet count) - - - - - - - - - - - - - - - - - - - - - - // // Check if its near - // if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) - // { - // // Default case when no near packets are appended - // if (near_timestamp_ == default_timestamp_) - // { - // near_timestamp_ = msg->timestamp; - // } - - // // Check if new message is part of the same scan - // if (msg->timestamp == near_timestamp_) - // { - // // Filter out detections based on given thresholds - // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // // Append all detections to buffer packet 1 - // near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - // test_filtered_ars.detections.begin(), - // test_filtered_ars.detections.end()); - // total_packet_count[buffer_index_]++; - // } - - // // This means all 18 near scan packets are in buffer packet 1 (all with the same timestamps) - // else - // { - // next_near_timestamp_ = msg->timestamp; - - // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // // Append to buffer packet 2 - // near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - // test_filtered_ars.detections.begin(), - // test_filtered_ars.detections.end()); - // total_packet_count[1-buffer_index_]++; - // } - // } - - // // Check if its far - // else if (msg->event_id == 1 || msg->event_id == 2) - // { - // // Default case when no far packets are appended - // if (far_timestamp_ == default_timestamp_) - // { - // far_timestamp_ = msg->timestamp; - // } - - // // Check if new message is part of the same scan - // if (msg->timestamp == far_timestamp_) - // { - // // Filter out detections based on given thresholds - // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // // Append all detections to buffer packet 1 - // near_far_buffer_packets[buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - // test_filtered_ars.detections.begin(), - // test_filtered_ars.detections.end()); - // total_packet_count[buffer_index_]++; - // } - - // // This means that all 12 far scan packets are in buffer packet 1 (all with the same timestamps) - // else - // { - // next_far_timestamp_ = msg->timestamp; - - // // Filter new far scan incoming data - // const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( - // msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, - // parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - - // // Append to buffer packet 2 - // near_far_buffer_packets[1 - buffer_index_].detections.insert(near_far_buffer_packets[buffer_index_].detections.end(), - // test_filtered_ars.detections.begin(), - // test_filtered_ars.detections.end()); - // total_packet_count[1 - buffer_index_]++; - // } - - // if(total_packet_count[buffer_index_] == 30) - // { - // // This means that there are 30 packets in buffer packet 1 at this point - - // // Publish buffer_packet - // publish_packet = near_far_buffer_packets[buffer_index_]; - - // // Change timestamps, next_near and next_far become the "new" timestamps to check if incoming messages are part of the same scan - // near_timestamp_ = next_near_timestamp_; - // far_timestamp_ = next_far_timestamp_; - - // // Clear the buffer packet array - // near_far_buffer_packets[buffer_index_].detections.clear(); - - // total_packet_count[buffer_index_] = 0; - - // // Flip index for next iteration - // buffer_index_ = 1 - buffer_index_; - - // return true; - - // } - // } - - // return false; } } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index f9d58cb4..015115ec 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -48,16 +48,16 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); // If scan mode is near - if(parameters.scan_mode == "near") + if(parameters.scan_mode == "near" || parameters.scan_mode == "far") { - // Create a temporary empty packet for when we have a full near scan filtered packet ready to be published + // Create a temporary empty packet for when we have a full near/far scan filtered packet ready to be published radar_msgs::msg::RadarPacket publish_packet; /** - Send message to near scan filter and append detections to buffer packet based on timestamp. + Send message to scan filter and append detections to buffer packet based on timestamp. Check if packet is ready to be published (if timestamp differs). Return true if ready to be published, return false otherwise. **/ - if(pointcloudfilter_.near_scan_filter(msg, parameters, publish_packet) == true) + if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) { // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); @@ -65,24 +65,6 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( } } - // If scan mode is far - else if (parameters.scan_mode == "far") - { - // Create a temporary empty packet for when we have a full far scan filtered packet ready to be published - radar_msgs::msg::RadarPacket publish_packet; - - /** - Send message to far scan filter and append detections to buffer packet based on timestamp. - Check if packet is ready to be published (if timestamp differs). Return true if ready to be published, return false otherwise. - **/ - if(pointcloudfilter_.far_scan_filter(msg, parameters, publish_packet) == true) - { - // Publish buffer packet - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); - left_right_pub_->publish(publish_packet); - } - } - // If scan mode is near_far (Double Buffer) else { @@ -107,28 +89,17 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - if(parameters.scan_mode == "near") + if(parameters.scan_mode == "near" || parameters.scan_mode == "far" ) { radar_msgs::msg::RadarPacket publish_packet; - if(pointcloudfilter_.near_scan_filter(msg, parameters, publish_packet) == true) + if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) { RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); } } - else if (parameters.scan_mode == "far") - { - radar_msgs::msg::RadarPacket publish_packet; - - if(pointcloudfilter_.far_scan_filter(msg, parameters, publish_packet) == true) - { - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); - left_right_pub_->publish(publish_packet); - } - } - else { radar_msgs::msg::RadarPacket publish_packet_near_far; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index a9405725..5777629b 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -25,52 +25,89 @@ TEST(ARSPointCloudFilterTest, PointFilter) msg->vambig = 3.0; msg->center_frequency = 6.0; - msg_detection->pos_x = 1.0; - msg_detection->pos_y = 3.0; - msg_detection->pos_z = 4.0; + // Fake detection Data 1 msg_detection->vrel_rad = 100.0; msg_detection->az_ang0 = 60.0; - msg_detection->az_ang1 = 50.0; msg_detection->el_ang = 20.0; msg_detection->rcs0 = 5.0; - msg_detection->rcs1 = 10.0; - msg_detection->range_var = 600.0; - msg_detection->vrel_rad_var = 175.0; - msg_detection->az_ang_var0 = 12.0; - msg_detection->az_ang_var1 = 17.0; - msg_detection->el_ang_var = 19.0; msg_detection->snr = 100.0; msg_detection->range = 90.0; - msg_detection->prob0 = 0.0; - msg_detection->prob1 = 1.0; - msg_detection->pdh0 = 255.0; msg->detections.push_back(*msg_detection); - - msg_detection->pos_x = 1.0; - msg_detection->pos_y = 3.0; - msg_detection->pos_z = 4.0; + // Fake detection Data 2 msg_detection->vrel_rad = 100.0; - msg_detection->az_ang0 = 60.0; - msg_detection->az_ang1 = 50.0; + msg_detection->az_ang0 = 20.0; msg_detection->el_ang = 20.0; msg_detection->rcs0 = 5.0; - msg_detection->rcs1 = 10.0; - msg_detection->range_var = 600.0; - msg_detection->vrel_rad_var = 175.0; - msg_detection->az_ang_var0 = 12.0; - msg_detection->az_ang_var1 = 17.0; - msg_detection->el_ang_var = 19.0; msg_detection->snr = 300.0; msg_detection->range = 90.0; - msg_detection->prob0 = 0.0; - msg_detection->prob1 = 1.0; - msg_detection->pdh0 = 255.0; msg->detections.push_back(*msg_detection); auto test_packet = pointcloudfilter.point_filter(msg, 200, -9999.99, -9999.99, -9999.99, -9999.99, -9999.99); + // Fake detection data 1 should be removed and fake detection data 2 should be at index 0 EXPECT_EQ(300, test_packet.detections[0].snr); + + test_packet = pointcloudfilter.point_filter(msg, 100, 40, -9999.99, -9999.99, -9999.99, -9999.99); + // Fake detection data 1 should be left + EXPECT_EQ(100, test_packet.detections[0].snr); + EXPECT_EQ(60, test_packet.detections[0].az_ang0); + + // Fake detection data 2 should not exist, so all values should return 0 + EXPECT_EQ(0, test_packet.detections[1].snr); + EXPECT_EQ(0, test_packet.detections[1].az_ang0); +} + +TEST(ARSPointCloudFilterTest, CheckScanType) +{ + filtering::ARSPointCloudFilter pointcloudfilter; + + // Near Scan message + auto msg = std::make_shared(); + + msg->event_id = 3; + msg->timestamp = 2; + msg->measurement_counter = 1; + msg->vambig = 3.0; + msg->center_frequency = 6.0; + + // Far Scan message + auto diff_msg = std::make_shared(); + + diff_msg->event_id = 1; + diff_msg->timestamp = 2; + diff_msg->measurement_counter = 1; + diff_msg->vambig = 3.0; + diff_msg->center_frequency = 6.0; + + auto scan_type_0 = pointcloudfilter.check_scan_type(msg); + auto scan_type_1 = pointcloudfilter.check_scan_type(diff_msg); + + EXPECT_EQ(0, scan_type_0); + EXPECT_EQ(1, scan_type_1); } + + +// TEST(ARSPointCloudFilterTest, ResetScanState) +// { +// // Need getters and setters to access the private variables in this test +// } + +// TEST(ARSPointCloudFilterTest, CommonScanFilter) +// { +// filtering::ARSPointCloudFilter pointcloudfilter; +// auto msg = std::make_shared(); + +// // make a fake publish packet +// // make a fake parameter list +// // make a fake message + +// // verify with buffer packet thats in the member variables +// // one case for each branch + +// // Near Scan message + + +// } \ No newline at end of file From 7ccec610b7c0107275fca1606284182fc6437dd1 Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 8 Mar 2023 02:31:19 +0000 Subject: [PATCH 32/51] updated filter implementation and unit tests for common scan filter --- .../src/ars_pointcloud_filter.cpp | 86 ++++++++++++------- .../src/ars_pointcloud_filter_node.cpp | 34 +++++--- .../test/ars_pointcloud_filter_test.cpp | 83 +++++++++++++----- 3 files changed, 142 insertions(+), 61 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 1b364171..7a3854b6 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -101,6 +101,11 @@ void ARSPointCloudFilter::reset_scan_states(const int &buffer_index) } + + + + + // A common filter for near and far scan modes bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, @@ -114,45 +119,66 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: // Returns which scan parameters the program needs to work on auto scan = (incoming_scan_msg == NEAR) ? near_scan_single_ : far_scan_single_; - - if(scan.timestamp_ == default_timestamp_) - { - scan.timestamp_ = msg->timestamp; - } - - // Filter out detections based on given thresholds - const radar_msgs::msg::RadarPacket test_filtered_ars = 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 near/far packet is not full - if(scan.packet_count_ != scan_capacity && msg->timestamp == scan.timestamp_) - { - // Append all detections to buffer - buffer_packet_.detections.insert(buffer_packet_.detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); - scan.packet_count_++; - } - else + if ((incoming_scan_msg == NEAR && parameters.scan_mode == "near") || (incoming_scan_msg == FAR && parameters.scan_mode == "far")) { - if(scan.packet_count_!= scan_capacity) + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Timestamp %d \n ",scan.timestamp_); + + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Size %d \n ",scan.packet_count_); + + if(scan.timestamp_ == default_timestamp_) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Near packet is not full, size: %d ! \n ", scan.packet_count_); + scan.timestamp_ = msg->timestamp; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for Default Timestamp \n "); } - // Publish buffer packet - publish_packet = buffer_packet_; - // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering - buffer_packet_ = test_filtered_ars; - scan.packet_count_ = 1; + // Filter out detections based on given thresholds + const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, + parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); - scan.timestamp_ = msg->timestamp; - return true; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Filtering the message \n "); + + // If near/far packet is not full + if(scan.packet_count_ != scan_capacity && msg->timestamp == scan.timestamp_) + { + // Append all detections to buffer + buffer_packet_.detections.insert(buffer_packet_.detections.end(), + test_filtered_ars.detections.begin(), + test_filtered_ars.detections.end()); + scan.packet_count_++; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending to buffer packet, size: %d\n ",scan.packet_count_); + } + else + { + if(scan.packet_count_!= scan_capacity) + { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! \n ", scan.packet_count_); + } + // Publish buffer packet + publish_packet = buffer_packet_; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing Packet, size: %d\n ",scan.packet_count_); + + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering + buffer_packet_ = test_filtered_ars; + scan.packet_count_ = 1; + + scan.timestamp_ = msg->timestamp; + return true; + } } return false; } + + + + + + + + + + // Near + Far Scan Filter Implementation (Double Buffer Algorithm) bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 015115ec..8acd028f 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -48,9 +48,9 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); // If scan mode is near - if(parameters.scan_mode == "near" || parameters.scan_mode == "far") + if(parameters.scan_mode == "near") { - // Create a temporary empty packet for when we have a full near/far scan filtered packet ready to be published + // Create a temporary empty packet for when we have a full near scan filtered packet ready to be published radar_msgs::msg::RadarPacket publish_packet; /** @@ -64,20 +64,25 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( left_right_pub_->publish(publish_packet); } } + // If scan mode is far + else if(parameters.scan_mode == "far") + { + radar_msgs::msg::RadarPacket publish_packet; + + if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) + { + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); + left_right_pub_->publish(publish_packet); + } + } // If scan mode is near_far (Double Buffer) else { - // Create a temporary empty packet for when we have a full near and far scan filtered packet ready to be published radar_msgs::msg::RadarPacket publish_packet_near_far; - /** - Send message to near_far_scan_filter and append detections to the buffer packets based on timestamp. - Check if packet is ready to be published (30 packets). Return true if ready to be published, return false otherwise. - **/ if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { - // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); left_right_pub_->publish(publish_packet_near_far); } @@ -89,7 +94,17 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - if(parameters.scan_mode == "near" || parameters.scan_mode == "far" ) + if(parameters.scan_mode == "near") + { + radar_msgs::msg::RadarPacket publish_packet; + + if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) + { + RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); + left_right_pub_->publish(publish_packet); + } + } + else if(parameters.scan_mode == "far") { radar_msgs::msg::RadarPacket publish_packet; @@ -99,7 +114,6 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( left_right_pub_->publish(publish_packet); } } - else { radar_msgs::msg::RadarPacket publish_packet_near_far; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 5777629b..c7d5d7a4 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -7,12 +7,6 @@ #include "ars_pointcloud_filter.hpp" -// Unit test cases -// 1. Single packet (near) -// 2. Single packet (far) -// 3. Multiple packets with same timestamps (near) -// 4. Multiple packets with different timestamps (near) - TEST(ARSPointCloudFilterTest, PointFilter) { filtering::ARSPointCloudFilter pointcloudfilter; @@ -89,25 +83,72 @@ TEST(ARSPointCloudFilterTest, CheckScanType) EXPECT_EQ(1, scan_type_1); } +TEST(ARSPointCloudFilterTest, CommonScanFilter) +{ + // ONLY NEAR SCAN DATA + filtering::ARSPointCloudFilter pointcloudfilter; + auto msg = std::make_shared(); -// TEST(ARSPointCloudFilterTest, ResetScanState) -// { -// // Need getters and setters to access the private variables in this test -// } + auto msg_detection = std::make_shared(); -// TEST(ARSPointCloudFilterTest, CommonScanFilter) -// { -// filtering::ARSPointCloudFilter pointcloudfilter; -// auto msg = std::make_shared(); + // Fake Publish packet + radar_msgs::msg::RadarPacket publish_packet; -// // make a fake publish packet -// // make a fake parameter list -// // make a fake message + // Fake parameters + filtering::ARSPointCloudFilter::filter_parameters parameters; + parameters.scan_mode = "near"; + parameters.vrel_rad_param = -9999.99; + parameters.el_ang_param = -9999.99; + parameters.rcs0_param = -9999.99; + parameters.snr_param = -9999.99; + parameters.range_param = -9999.99; + parameters.az_ang0_param = -9999.99; -// // verify with buffer packet thats in the member variables -// // one case for each branch + // Data input (Packet 1) + msg->event_id = 3; + msg->timestamp = 2; + msg->measurement_counter = 1; + msg->vambig = 3.0; + msg->center_frequency = 6.0; -// // Near Scan message + msg_detection->vrel_rad = 100.0; + msg_detection->az_ang0 = 60.0; + msg_detection->el_ang = 20.0; + msg_detection->rcs0 = 5.0; + msg_detection->snr = 100.0; + msg_detection->range = 90.0; + + msg->detections.push_back(*msg_detection); + + // Check if it doesn't return data and only gives it when its ready to publish + + for (int packet_size = 0; packet_size < 18; packet_size++) + { + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); + } + + EXPECT_EQ(3, publish_packet.event_id); + EXPECT_EQ(unsigned(2), publish_packet.timestamp); + + for(int i = 0; i < 18; i++) + { + EXPECT_EQ(100.0, publish_packet.detections[i].snr); + } + + + // Packet count check + // verify with buffer packet thats in the member variables + // one case for each branch + // Near Scan message + + // Check if timestamp is set correctly + + +} -// } \ No newline at end of file +// Unit test cases +// 1. Single packet (near) +// 2. Single packet (far) +// 3. Multiple packets with same timestamps (near) +// 4. Multiple packets with different timestamps (near) \ No newline at end of file From e770d9f2ae3e39a705aed8b5dafdf3ecdab494ea Mon Sep 17 00:00:00 2001 From: Rijin Date: Thu, 9 Mar 2023 05:15:56 +0000 Subject: [PATCH 33/51] Updated unit tests for common scan filters --- .../include/ars_pointcloud_filter.hpp | 19 +-- .../src/ars_pointcloud_filter.cpp | 54 +++++--- .../test/ars_pointcloud_filter_test.cpp | 126 ++++++++---------- 3 files changed, 104 insertions(+), 95 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 6545a8cc..eecda2ab 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -15,15 +15,6 @@ class ARSPointCloudFilter { public: ARSPointCloudFilter(); - - radar_msgs::msg::RadarPacket point_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, - double snr_threshold, - double AzAng0_threshold, - double range_threshold, - double vrel_rad_threshold, - double el_ang_threshold, - double rcs_threshold); typedef struct { @@ -56,6 +47,16 @@ class ARSPointCloudFilter private: + // Filter message based on thresholds + radar_msgs::msg::RadarPacket point_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + double snr_threshold, + double AzAng0_threshold, + double range_threshold, + double vrel_rad_threshold, + double el_ang_threshold, + double rcs_threshold); + // For all Filters unsigned int default_timestamp_; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 7a3854b6..f3881148 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -51,23 +51,23 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( { continue; } - else if (detection.az_ang0 < AzAng0_threshold) + if (detection.az_ang0 < AzAng0_threshold) { continue; } - else if (detection.range < range_threshold) + if (detection.range < range_threshold) { continue; } - else if (detection.vrel_rad < vrel_rad_threshold) + if (detection.vrel_rad < vrel_rad_threshold) { continue; } - else if (detection.el_ang < el_ang_threshold) + if (detection.el_ang < el_ang_threshold) { continue; } - else if (detection.rcs0 < rcs_threshold) + if (detection.rcs0 < rcs_threshold) { continue; } @@ -117,17 +117,17 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; // Returns which scan parameters the program needs to work on - auto scan = (incoming_scan_msg == NEAR) ? near_scan_single_ : far_scan_single_; + 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")) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Timestamp %d \n ",scan.timestamp_); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Timestamp %d \n ",scan->timestamp_); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Size %d \n ",scan.packet_count_); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Size %d \n ",scan->packet_count_); - if(scan.timestamp_ == default_timestamp_) + if(scan->timestamp_ == default_timestamp_) { - scan.timestamp_ = msg->timestamp; + scan->timestamp_ = msg->timestamp; RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for Default Timestamp \n "); } @@ -139,30 +139,46 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Filtering the message \n "); // If near/far packet is not full - if(scan.packet_count_ != scan_capacity && msg->timestamp == scan.timestamp_) + if(scan->packet_count_ != scan_capacity && msg->timestamp == scan->timestamp_) { + buffer_packet_.timestamp = scan->timestamp_; // Append all detections to buffer buffer_packet_.detections.insert(buffer_packet_.detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); - scan.packet_count_++; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending to buffer packet, size: %d\n ",scan.packet_count_); + scan->packet_count_++; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending to buffer packet, size: %d\n ",scan->packet_count_); + + if(scan->packet_count_ == scan_capacity) + { + // Publish buffer packet + publish_packet = buffer_packet_; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing Packet, size: %d\n ",scan->packet_count_); + + scan->packet_count_ = 0; + + scan->timestamp_ = default_timestamp_; + return true; + } + } else { - if(scan.packet_count_!= scan_capacity) + if(scan->packet_count_!= scan_capacity) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! \n ", scan.packet_count_); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! \n ", scan->packet_count_); } + scan->publish_status_ = true; + // Publish buffer packet publish_packet = buffer_packet_; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing Packet, size: %d\n ",scan.packet_count_); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing Packet, size: %d\n ",scan->packet_count_); // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet_ = test_filtered_ars; - scan.packet_count_ = 1; + scan->packet_count_ = 1; - scan.timestamp_ = msg->timestamp; + scan->timestamp_ = msg->timestamp; return true; } } @@ -203,6 +219,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].timestamp_ == msg->timestamp) { + 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(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); @@ -218,6 +235,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke 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_[buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index c7d5d7a4..031d0fa6 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -7,52 +7,52 @@ #include "ars_pointcloud_filter.hpp" -TEST(ARSPointCloudFilterTest, PointFilter) -{ - filtering::ARSPointCloudFilter pointcloudfilter; - auto msg = std::make_shared(); - auto msg_detection = std::make_shared(); - - msg->event_id = 3; - msg->timestamp = 2; - msg->measurement_counter = 1; - msg->vambig = 3.0; - msg->center_frequency = 6.0; - - // Fake detection Data 1 - msg_detection->vrel_rad = 100.0; - msg_detection->az_ang0 = 60.0; - msg_detection->el_ang = 20.0; - msg_detection->rcs0 = 5.0; - msg_detection->snr = 100.0; - msg_detection->range = 90.0; - - msg->detections.push_back(*msg_detection); - - // Fake detection Data 2 - msg_detection->vrel_rad = 100.0; - msg_detection->az_ang0 = 20.0; - msg_detection->el_ang = 20.0; - msg_detection->rcs0 = 5.0; - msg_detection->snr = 300.0; - msg_detection->range = 90.0; - - msg->detections.push_back(*msg_detection); +// TEST(ARSPointCloudFilterTest, PointCloudFilter) +// { +// filtering::ARSPointCloudFilter pointcloudfilter; +// auto msg = std::make_shared(); +// auto msg_detection = std::make_shared(); + +// msg->event_id = 3; +// msg->timestamp = 2; +// msg->measurement_counter = 1; +// msg->vambig = 3.0; +// msg->center_frequency = 6.0; + +// // Fake detection Data 1 +// msg_detection->vrel_rad = 100.0; +// msg_detection->az_ang0 = 60.0; +// msg_detection->el_ang = 20.0; +// msg_detection->rcs0 = 5.0; +// msg_detection->snr = 100.0; +// msg_detection->range = 90.0; + +// msg->detections.push_back(*msg_detection); + +// // Fake detection Data 2 +// msg_detection->vrel_rad = 100.0; +// msg_detection->az_ang0 = 20.0; +// msg_detection->el_ang = 20.0; +// msg_detection->rcs0 = 5.0; +// msg_detection->snr = 300.0; +// msg_detection->range = 90.0; + +// msg->detections.push_back(*msg_detection); - auto test_packet = pointcloudfilter.point_filter(msg, 200, -9999.99, -9999.99, -9999.99, -9999.99, -9999.99); +// auto test_packet = pointcloudfilter.point_filter(msg, 200, -9999.99, -9999.99, -9999.99, -9999.99, -9999.99); - // Fake detection data 1 should be removed and fake detection data 2 should be at index 0 - EXPECT_EQ(300, test_packet.detections[0].snr); +// // Fake detection data 1 should be removed and fake detection data 2 should be at index 0 +// EXPECT_EQ(300, test_packet.detections[0].snr); - test_packet = pointcloudfilter.point_filter(msg, 100, 40, -9999.99, -9999.99, -9999.99, -9999.99); - // Fake detection data 1 should be left - EXPECT_EQ(100, test_packet.detections[0].snr); - EXPECT_EQ(60, test_packet.detections[0].az_ang0); +// test_packet = pointcloudfilter.point_filter(msg, 100, 40, -9999.99, -9999.99, -9999.99, -9999.99); +// // Fake detection data 1 should be left +// EXPECT_EQ(100, test_packet.detections[0].snr); +// EXPECT_EQ(60, test_packet.detections[0].az_ang0); - // Fake detection data 2 should not exist, so all values should return 0 - EXPECT_EQ(0, test_packet.detections[1].snr); - EXPECT_EQ(0, test_packet.detections[1].az_ang0); -} +// // Fake detection data 2 should not exist, so all values should return 0 +// EXPECT_EQ(0, test_packet.detections[1].snr); +// EXPECT_EQ(0, test_packet.detections[1].az_ang0); +// } TEST(ARSPointCloudFilterTest, CheckScanType) { @@ -83,13 +83,13 @@ TEST(ARSPointCloudFilterTest, CheckScanType) EXPECT_EQ(1, scan_type_1); } -TEST(ARSPointCloudFilterTest, CommonScanFilter) +TEST(ARSPointCloudFilterTest, NearScan_CommonScanFilter) { // ONLY NEAR SCAN DATA filtering::ARSPointCloudFilter pointcloudfilter; auto msg = std::make_shared(); - auto msg_detection = std::make_shared(); + radar_msgs::msg::RadarDetection msg_detection; // Fake Publish packet radar_msgs::msg::RadarPacket publish_packet; @@ -111,14 +111,14 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter) msg->vambig = 3.0; msg->center_frequency = 6.0; - msg_detection->vrel_rad = 100.0; - msg_detection->az_ang0 = 60.0; - msg_detection->el_ang = 20.0; - msg_detection->rcs0 = 5.0; - msg_detection->snr = 100.0; - msg_detection->range = 90.0; + msg_detection.vrel_rad = 100.0; + msg_detection.az_ang0 = 60.0; + msg_detection.el_ang = 20.0; + msg_detection.rcs0 = 5.0; + msg_detection.snr = 100.0; + msg_detection.range = 90.0; - msg->detections.push_back(*msg_detection); + msg->detections.push_back(msg_detection); // Check if it doesn't return data and only gives it when its ready to publish @@ -127,28 +127,18 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter) pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - EXPECT_EQ(3, publish_packet.event_id); EXPECT_EQ(unsigned(2), publish_packet.timestamp); for(int i = 0; i < 18; i++) { - EXPECT_EQ(100.0, publish_packet.detections[i].snr); + EXPECT_EQ(100, publish_packet.detections[i].snr); } - - - // Packet count check - // verify with buffer packet thats in the member variables - // one case for each branch - - // Near Scan message - - // Check if timestamp is set correctly - - + } // Unit test cases -// 1. Single packet (near) -// 2. Single packet (far) -// 3. Multiple packets with same timestamps (near) -// 4. Multiple packets with different timestamps (near) \ No newline at end of file +// 1. Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) +// 2. Have another test case that sends 12 far scan packets +// 3. send 19 near scan packets and see if it behaves correctly +// 4. Send far scan packets when in near scan mode, does it filter correctly +// 5. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) \ No newline at end of file From b71b4e8bd3145d70f5e7c77e7b899b1f58f6814d Mon Sep 17 00:00:00 2001 From: Rijin Date: Fri, 10 Mar 2023 04:23:00 +0000 Subject: [PATCH 34/51] updated unit tests for common scan filter on the pointcloudfilter ROS node --- .../ARSPointCloudFilter/CMakeLists.txt | 18 +- .../ARSPointCloudFilter/package.xml | 4 +- .../src/ars_pointcloud_filter.cpp | 7 +- .../test/ars_pointcloud_filter_test.cpp | 463 +++++++++++++++--- 4 files changed, 404 insertions(+), 88 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt index 7795264d..34642162 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt +++ b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt @@ -34,18 +34,18 @@ 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) + # find_package(ament_lint_auto REQUIRED) 37-48 + # 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) + # # 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") + # # Reinstall cpplint ignoring copyright errors + # ament_cpplint(FILTERS "-legal/copyright") - ament_lint_auto_find_test_dependencies() + # ament_lint_auto_find_test_dependencies() # Create test executable from source files ament_add_gtest(ars_pointcloud_filter_test test/ars_pointcloud_filter_test.cpp) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/package.xml b/src/sensor_interfacing/ARSPointCloudFilter/package.xml index 3f55f068..5f90f25d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/package.xml +++ b/src/sensor_interfacing/ARSPointCloudFilter/package.xml @@ -12,8 +12,8 @@ rclcpp radar_msgs - ament_lint_auto - ament_lint_common + ament_cmake_gtest diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index f3881148..4da83388 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -141,7 +141,9 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: // If near/far packet is not full if(scan->packet_count_ != scan_capacity && msg->timestamp == scan->timestamp_) { + // Update buffer packet timestamp buffer_packet_.timestamp = scan->timestamp_; + // Append all detections to buffer buffer_packet_.detections.insert(buffer_packet_.detections.end(), test_filtered_ars.detections.begin(), @@ -155,6 +157,8 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: publish_packet = buffer_packet_; RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing Packet, size: %d\n ",scan->packet_count_); + buffer_packet_.detections.clear(); + scan->packet_count_ = 0; scan->timestamp_ = default_timestamp_; @@ -168,7 +172,6 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: { RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! \n ", scan->packet_count_); } - scan->publish_status_ = true; // Publish buffer packet publish_packet = buffer_packet_; @@ -220,6 +223,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].timestamp_ == msg->timestamp) { 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(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); @@ -236,6 +240,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke 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_[buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 031d0fa6..35e5de67 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -7,83 +7,61 @@ #include "ars_pointcloud_filter.hpp" -// TEST(ARSPointCloudFilterTest, PointCloudFilter) -// { -// filtering::ARSPointCloudFilter pointcloudfilter; -// auto msg = std::make_shared(); -// auto msg_detection = std::make_shared(); +// // The fixture for testing class ARSPointCloudFilterTest +// class ARSPointCloudFilterTest : public ::testing::Test { +// protected: -// msg->event_id = 3; -// msg->timestamp = 2; -// msg->measurement_counter = 1; -// msg->vambig = 3.0; -// msg->center_frequency = 6.0; +// ARSPointCloudFilterTest() { +// filtering::ARSPointCloudFilter::filter_parameters parameters; +// parameters.scan_mode = "near"; +// parameters.vrel_rad_param = -9999.99; +// parameters.el_ang_param = -9999.99; +// parameters.rcs0_param = -9999.99; +// parameters.snr_param = -9999.99; +// parameters.range_param = -9999.99; +// parameters.az_ang0_param = -9999.99; +// } -// // Fake detection Data 1 -// msg_detection->vrel_rad = 100.0; -// msg_detection->az_ang0 = 60.0; -// msg_detection->el_ang = 20.0; -// msg_detection->rcs0 = 5.0; -// msg_detection->snr = 100.0; -// msg_detection->range = 90.0; +// void SetUp() override { +// // Code here will be called immediately after the constructor (right +// // before each test). +// } -// msg->detections.push_back(*msg_detection); +// void TearDown() override { +// // Code here will be called immediately after each test (right +// // before the destructor). +// } -// // Fake detection Data 2 -// msg_detection->vrel_rad = 100.0; -// msg_detection->az_ang0 = 20.0; -// msg_detection->el_ang = 20.0; -// msg_detection->rcs0 = 5.0; -// msg_detection->snr = 300.0; -// msg_detection->range = 90.0; +// }; -// msg->detections.push_back(*msg_detection); - -// auto test_packet = pointcloudfilter.point_filter(msg, 200, -9999.99, -9999.99, -9999.99, -9999.99, -9999.99); - -// // Fake detection data 1 should be removed and fake detection data 2 should be at index 0 -// EXPECT_EQ(300, test_packet.detections[0].snr); - -// test_packet = pointcloudfilter.point_filter(msg, 100, 40, -9999.99, -9999.99, -9999.99, -9999.99); -// // Fake detection data 1 should be left -// EXPECT_EQ(100, test_packet.detections[0].snr); -// EXPECT_EQ(60, test_packet.detections[0].az_ang0); - -// // Fake detection data 2 should not exist, so all values should return 0 -// EXPECT_EQ(0, test_packet.detections[1].snr); -// EXPECT_EQ(0, test_packet.detections[1].az_ang0); -// } +// Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR TEST(ARSPointCloudFilterTest, CheckScanType) { filtering::ARSPointCloudFilter pointcloudfilter; // Near Scan message - auto msg = std::make_shared(); + auto msg_0 = std::make_shared(); - msg->event_id = 3; - msg->timestamp = 2; - msg->measurement_counter = 1; - msg->vambig = 3.0; - msg->center_frequency = 6.0; + msg_0->event_id = 3; + msg_0->timestamp = 2; // Far Scan message - auto diff_msg = std::make_shared(); + auto msg_1 = std::make_shared(); - diff_msg->event_id = 1; - diff_msg->timestamp = 2; - diff_msg->measurement_counter = 1; - diff_msg->vambig = 3.0; - diff_msg->center_frequency = 6.0; + msg_1->event_id = 1; + msg_1->timestamp = 2; - auto scan_type_0 = pointcloudfilter.check_scan_type(msg); - auto scan_type_1 = pointcloudfilter.check_scan_type(diff_msg); + auto scan_type_0 = pointcloudfilter.check_scan_type(msg_0); + auto scan_type_1 = pointcloudfilter.check_scan_type(msg_1); EXPECT_EQ(0, scan_type_0); EXPECT_EQ(1, scan_type_1); } -TEST(ARSPointCloudFilterTest, NearScan_CommonScanFilter) + +// Send 18 near scan packets with the same timestamp and check if it returns a complete packet ready to be published +TEST(ARSPointCloudFilterTest, CommonScanFilter_01) { // ONLY NEAR SCAN DATA filtering::ARSPointCloudFilter pointcloudfilter; @@ -94,7 +72,6 @@ TEST(ARSPointCloudFilterTest, NearScan_CommonScanFilter) // Fake Publish packet radar_msgs::msg::RadarPacket publish_packet; - // Fake parameters filtering::ARSPointCloudFilter::filter_parameters parameters; parameters.scan_mode = "near"; parameters.vrel_rad_param = -9999.99; @@ -104,41 +81,375 @@ TEST(ARSPointCloudFilterTest, NearScan_CommonScanFilter) parameters.range_param = -9999.99; parameters.az_ang0_param = -9999.99; - // Data input (Packet 1) + // Packet Data msg->event_id = 3; msg->timestamp = 2; - msg->measurement_counter = 1; - msg->vambig = 3.0; - msg->center_frequency = 6.0; - msg_detection.vrel_rad = 100.0; - msg_detection.az_ang0 = 60.0; - msg_detection.el_ang = 20.0; - msg_detection.rcs0 = 5.0; msg_detection.snr = 100.0; - msg_detection.range = 90.0; msg->detections.push_back(msg_detection); - // Check if it doesn't return data and only gives it when its ready to publish - for (int packet_size = 0; packet_size < 18; packet_size++) { pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - EXPECT_EQ(unsigned(2), publish_packet.timestamp); + EXPECT_EQ(2, publish_packet.timestamp); - for(int i = 0; i < 18; i++) + for(int index = 0; index < 18; index++) { - EXPECT_EQ(100, publish_packet.detections[i].snr); + EXPECT_EQ(100, publish_packet.detections[index].snr); } } +// Send 12 far scan packets with the same timestamp and check if it returns a complete packet ready to be published +TEST(ARSPointCloudFilterTest, CommonScanFilter_02) +{ + // ONLY FAR SCAN DATA + filtering::ARSPointCloudFilter pointcloudfilter; + auto msg = std::make_shared(); + + radar_msgs::msg::RadarDetection msg_detection; + + // Fake Publish packet + radar_msgs::msg::RadarPacket publish_packet; + + // Fake parameters + filtering::ARSPointCloudFilter::filter_parameters parameters; + parameters.scan_mode = "far"; + parameters.vrel_rad_param = -9999.99; + parameters.el_ang_param = -9999.99; + parameters.rcs0_param = -9999.99; + parameters.snr_param = -9999.99; + parameters.range_param = -9999.99; + parameters.az_ang0_param = -9999.99; + + // Packet Data + 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, publish_packet.timestamp); + + for(int index = 0; index < 12; index++) + { + EXPECT_EQ(5, publish_packet.detections[index].rcs0); + } +} + +// Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) +TEST(ARSPointCloudFilterTest, CommonScanFilter_03) +{ + // Near Scan data + filtering::ARSPointCloudFilter pointcloudfilter; + 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; + + // Fake Publish packets + radar_msgs::msg::RadarPacket publish_packet_0; + radar_msgs::msg::RadarPacket publish_packet_1; + + // Fake parameters + filtering::ARSPointCloudFilter::filter_parameters parameters; + parameters.scan_mode = "near"; + parameters.vrel_rad_param = -9999.99; + parameters.el_ang_param = -9999.99; + parameters.rcs0_param = -9999.99; + parameters.snr_param = -9999.99; + parameters.range_param = -9999.99; + parameters.az_ang0_param = -9999.99; + + // Packet Data (Scan 1) + msg_0->event_id = 3; + msg_0->timestamp = 7; + + + msg_detection_0.rcs0 = 5.0; + + msg_0->detections.push_back(msg_detection_0); + + // Packet Data (Scan 2) + msg_1->event_id = 4; + msg_1->timestamp = 3; + + msg_detection_1.rcs0 = 20.0; + + msg_1->detections.push_back(msg_detection_1); + + // 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, 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, publish_packet_1.timestamp); + + for(int index = 0; index < 18; index++) + { + EXPECT_EQ(20, publish_packet_1.detections[index].rcs0); + } + +} + +// Send incomplete packets (5 that are the same) (18 that is different in timestamp) +TEST(ARSPointCloudFilterTest, CommonScanFilter_04) +{ + // Near Scan data + filtering::ARSPointCloudFilter pointcloudfilter; + 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; + + // Fake Publish packets + radar_msgs::msg::RadarPacket publish_packet_; + + // Fake parameters + filtering::ARSPointCloudFilter::filter_parameters parameters; + parameters.scan_mode = "near"; + parameters.vrel_rad_param = -9999.99; + parameters.el_ang_param = -9999.99; + parameters.rcs0_param = -9999.99; + parameters.snr_param = -9999.99; + parameters.range_param = -9999.99; + parameters.az_ang0_param = -9999.99; + + // Packet Data (Scan 1) + msg_0->event_id = 3; + msg_0->timestamp = 7; + + msg_detection_0.range = 120.0; + + msg_0->detections.push_back(msg_detection_0); + + // Packet Data (Scan 2) + msg_1->event_id = 4; + msg_1->timestamp = 3; + + msg_detection_1.range = 50.0; + + msg_1->detections.push_back(msg_detection_1); + + // 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, publish_packet_.timestamp); + + + // Scan 2 data (sending a packet of a different timestamp) + for (int packet_size = 0; packet_size < 1; packet_size++) + { + pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet_); + } + + // Timestamp should be from msg_0 + EXPECT_EQ(7, publish_packet_.timestamp); + + for(int index = 0; index < 5; index++) + { + EXPECT_EQ(120, publish_packet_.detections[index].range); + } +} + +// Send far scan packets when in near scan mode. Check if it filters correctly. +TEST(ARSPointCloudFilterTest, CommonScanFilter_05) +{ + // Near Scan data + filtering::ARSPointCloudFilter pointcloudfilter; + 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; + + // Fake Publish packets + radar_msgs::msg::RadarPacket publish_packet_; + + // Fake parameters + filtering::ARSPointCloudFilter::filter_parameters parameters; + parameters.scan_mode = "near"; + parameters.vrel_rad_param = -9999.99; + parameters.el_ang_param = -9999.99; + parameters.rcs0_param = -9999.99; + parameters.snr_param = -9999.99; + parameters.range_param = -9999.99; + parameters.az_ang0_param = -9999.99; + + // Packet Data (Scan 1) + msg_0->event_id = 3; + msg_0->timestamp = 7; + + msg_detection_0.range = 120.0; + + msg_0->detections.push_back(msg_detection_0); + + // Packet Data (Scan 2) + msg_1->event_id = 1; + msg_1->timestamp = 3; + + msg_detection_1.range = 50.0; + + msg_1->detections.push_back(msg_detection_1); + + + 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, publish_packet_.timestamp); + + for(int index = 0; index < 18; index++) + { + EXPECT_EQ(120, publish_packet_.detections[index].range); + } + +} + +// Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) +TEST(ARSPointCloudFilterTest, CommonScanFilter_06) +{ + // Near Scan data + filtering::ARSPointCloudFilter pointcloudfilter; + 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; + + // Fake Publish packets + radar_msgs::msg::RadarPacket publish_packet_; + + // Fake parameters + filtering::ARSPointCloudFilter::filter_parameters parameters; + parameters.scan_mode = "near"; + parameters.vrel_rad_param = -9999.99; + parameters.el_ang_param = -9999.99; + parameters.rcs0_param = 70.0; + parameters.snr_param = 100.0; + parameters.range_param = 40.0; + parameters.az_ang0_param = -9999.99; + + // Packet Data (Scan 1) + msg->event_id = 3; + msg->timestamp = 7; + + 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); + + msg_detection_1.range = 50.0; + msg_detection_1.rcs0 = 60.0; + msg_detection_1.snr = 200.0; + + msg->detections.push_back(msg_detection_1); + + 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 < 18; packet_size++) + { + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet_); + } + + EXPECT_EQ(7, publish_packet_.timestamp); + + // Return published packet with empty detections + EXPECT_EQ(0, publish_packet_.detections.size()); + +} // Unit test cases -// 1. Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) -// 2. Have another test case that sends 12 far scan packets -// 3. send 19 near scan packets and see if it behaves correctly -// 4. Send far scan packets when in near scan mode, does it filter correctly -// 5. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) \ No newline at end of file +// 1. Send 18 near scan packets and check if it returns a complete 18 packet ready to be published (done) +// 2. Send 12 far scan packets and check if it returns a complete 12 packet ready to be published (done) +// 3. Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) (done) +// 4. Send incomplete packets (5 that are the same) (1 that is different in timestamp) (done) +// 5. Send 19 near scan packets and see if it behaves correctly (Is not required) (done) +// 6. Send far scan packets when in near scan mode, does it filter correctly (done) +// 7. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) (done) + + + + +// TEST(ARSPointCloudFilterTest, PointCloudFilter) +// { +// filtering::ARSPointCloudFilter pointcloudfilter; +// auto msg = std::make_shared(); +// auto msg_detection = std::make_shared(); + +// msg->event_id = 3; +// msg->timestamp = 2; +// msg->measurement_counter = 1; +// msg->vambig = 3.0; +// msg->center_frequency = 6.0; + +// // Fake detection Data 1 +// msg_detection->vrel_rad = 100.0; +// msg_detection->az_ang0 = 60.0; +// msg_detection->el_ang = 20.0; +// msg_detection->rcs0 = 5.0; +// msg_detection->snr = 100.0; +// msg_detection->range = 90.0; + +// msg->detections.push_back(*msg_detection); + +// // Fake detection Data 2 +// msg_detection->vrel_rad = 100.0; +// msg_detection->az_ang0 = 20.0; +// msg_detection->el_ang = 20.0; +// msg_detection->rcs0 = 5.0; +// msg_detection->snr = 300.0; +// msg_detection->range = 90.0; + +// msg->detections.push_back(*msg_detection); + +// auto test_packet = pointcloudfilter.point_filter(msg, 200, -9999.99, -9999.99, -9999.99, -9999.99, -9999.99); + +// // Fake detection data 1 should be removed and fake detection data 2 should be at index 0 +// EXPECT_EQ(300, test_packet.detections[0].snr); + +// test_packet = pointcloudfilter.point_filter(msg, 100, 40, -9999.99, -9999.99, -9999.99, -9999.99); +// // Fake detection data 1 should be left +// EXPECT_EQ(100, test_packet.detections[0].snr); +// EXPECT_EQ(60, test_packet.detections[0].az_ang0); + +// // Fake detection data 2 should not exist, so all values should return 0 +// EXPECT_EQ(0, test_packet.detections[1].snr); +// EXPECT_EQ(0, test_packet.detections[1].az_ang0); +// } From 00dd5dd28cf186f10f7014358ea0a20ff8abd6bf Mon Sep 17 00:00:00 2001 From: Rijin Date: Sat, 11 Mar 2023 00:51:30 +0000 Subject: [PATCH 35/51] Updated unit tests for common scan filter --- .../test/ars_pointcloud_filter_test.cpp | 86 +------------------ 1 file changed, 4 insertions(+), 82 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 35e5de67..6306b75b 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -7,34 +7,6 @@ #include "ars_pointcloud_filter.hpp" -// // The fixture for testing class ARSPointCloudFilterTest -// class ARSPointCloudFilterTest : public ::testing::Test { -// protected: - -// ARSPointCloudFilterTest() { -// filtering::ARSPointCloudFilter::filter_parameters parameters; -// parameters.scan_mode = "near"; -// parameters.vrel_rad_param = -9999.99; -// parameters.el_ang_param = -9999.99; -// parameters.rcs0_param = -9999.99; -// parameters.snr_param = -9999.99; -// parameters.range_param = -9999.99; -// parameters.az_ang0_param = -9999.99; -// } - -// void SetUp() override { -// // Code here will be called immediately after the constructor (right -// // before each test). -// } - -// void TearDown() override { -// // Code here will be called immediately after each test (right -// // before the destructor). -// } - -// }; - - // Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR TEST(ARSPointCloudFilterTest, CheckScanType) { @@ -55,15 +27,15 @@ TEST(ARSPointCloudFilterTest, CheckScanType) auto scan_type_0 = pointcloudfilter.check_scan_type(msg_0); auto scan_type_1 = pointcloudfilter.check_scan_type(msg_1); + // Near = 0 & Far = 1 EXPECT_EQ(0, scan_type_0); EXPECT_EQ(1, scan_type_1); } - // Send 18 near scan packets with the same timestamp and check if it returns a complete packet ready to be published TEST(ARSPointCloudFilterTest, CommonScanFilter_01) { - // ONLY NEAR SCAN DATA + // Near scan data filtering::ARSPointCloudFilter pointcloudfilter; auto msg = std::make_shared(); @@ -106,7 +78,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_01) // Send 12 far scan packets with the same timestamp and check if it returns a complete packet ready to be published TEST(ARSPointCloudFilterTest, CommonScanFilter_02) { - // ONLY FAR SCAN DATA + // Far Scan Message filtering::ARSPointCloudFilter pointcloudfilter; auto msg = std::make_shared(); @@ -216,7 +188,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_03) } -// Send incomplete packets (5 that are the same) (18 that is different in timestamp) +// Send incomplete packets (5 that are the same) (1 following packet that is different in timestamp) TEST(ARSPointCloudFilterTest, CommonScanFilter_04) { // Near Scan data @@ -265,7 +237,6 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_04) // Publish packet shouldn't be updated EXPECT_EQ(0, publish_packet_.timestamp); - // Scan 2 data (sending a packet of a different timestamp) for (int packet_size = 0; packet_size < 1; packet_size++) { @@ -404,52 +375,3 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_06) // 6. Send far scan packets when in near scan mode, does it filter correctly (done) // 7. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) (done) - - - -// TEST(ARSPointCloudFilterTest, PointCloudFilter) -// { -// filtering::ARSPointCloudFilter pointcloudfilter; -// auto msg = std::make_shared(); -// auto msg_detection = std::make_shared(); - -// msg->event_id = 3; -// msg->timestamp = 2; -// msg->measurement_counter = 1; -// msg->vambig = 3.0; -// msg->center_frequency = 6.0; - -// // Fake detection Data 1 -// msg_detection->vrel_rad = 100.0; -// msg_detection->az_ang0 = 60.0; -// msg_detection->el_ang = 20.0; -// msg_detection->rcs0 = 5.0; -// msg_detection->snr = 100.0; -// msg_detection->range = 90.0; - -// msg->detections.push_back(*msg_detection); - -// // Fake detection Data 2 -// msg_detection->vrel_rad = 100.0; -// msg_detection->az_ang0 = 20.0; -// msg_detection->el_ang = 20.0; -// msg_detection->rcs0 = 5.0; -// msg_detection->snr = 300.0; -// msg_detection->range = 90.0; - -// msg->detections.push_back(*msg_detection); - -// auto test_packet = pointcloudfilter.point_filter(msg, 200, -9999.99, -9999.99, -9999.99, -9999.99, -9999.99); - -// // Fake detection data 1 should be removed and fake detection data 2 should be at index 0 -// EXPECT_EQ(300, test_packet.detections[0].snr); - -// test_packet = pointcloudfilter.point_filter(msg, 100, 40, -9999.99, -9999.99, -9999.99, -9999.99); -// // Fake detection data 1 should be left -// EXPECT_EQ(100, test_packet.detections[0].snr); -// EXPECT_EQ(60, test_packet.detections[0].az_ang0); - -// // Fake detection data 2 should not exist, so all values should return 0 -// EXPECT_EQ(0, test_packet.detections[1].snr); -// EXPECT_EQ(0, test_packet.detections[1].az_ang0); -// } From 51f5fce0375d02dd094ed5abda0f89ba5bb0ea5e Mon Sep 17 00:00:00 2001 From: Rijin Date: Sun, 12 Mar 2023 06:25:24 +0000 Subject: [PATCH 36/51] updated tests for the ARSPointCloudFilter ROS Node --- .../include/ars_pointcloud_filter.hpp | 2 +- .../src/ars_pointcloud_filter.cpp | 47 ++- .../test/ars_pointcloud_filter_test.cpp | 330 +++++++++++++----- 3 files changed, 269 insertions(+), 110 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index eecda2ab..be409218 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -43,7 +43,7 @@ class ARSPointCloudFilter scan_type check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars); - void reset_scan_states(const int &buffer_index); + void reset_scan_states(); private: diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 4da83388..cf588264 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -89,15 +89,15 @@ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_ } } -void ARSPointCloudFilter::reset_scan_states(const int &buffer_index) +void ARSPointCloudFilter::reset_scan_states() { - near_scan_[buffer_index].timestamp_ = 0; - near_scan_[buffer_index].publish_status_ = false; - near_scan_[buffer_index].packet_count_= 0; + 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; + far_scan_[buffer_index_].timestamp_ = 0; + far_scan_[buffer_index_].publish_status_ = false; + far_scan_[buffer_index_].packet_count_= 0; } @@ -131,7 +131,7 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for Default Timestamp \n "); } - // Filter out detections based on given thresholds + // Filter out detections based on given thresholds (PASS IN ONLY PARAMETERS) const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( msg, parameters.snr_param, parameters.az_ang0_param, parameters.range_param, parameters.vrel_rad_param, parameters.el_ang_param, parameters.rcs0_param); @@ -209,39 +209,51 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; // Returns which scan parameters the program needs to work on - auto scan = (incoming_scan_msg == NEAR) ? near_scan_ : far_scan_; + auto scan = (incoming_scan_msg == NEAR) ? near_scan_.data() : far_scan_.data(); + + // RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Timestamp %d \n ",scan->timestamp_); + // RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Size %d \n ",scan->packet_count_); + + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Filtering the message \n "); const radar_msgs::msg::RadarPacket test_filtered_ars = 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_) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Comparing Scan buffer Timestamp with default timestamp, Scan Buffer Timestamp: %d\n ", scan[buffer_index_].timestamp_); scan[buffer_index_].timestamp_ = msg->timestamp; } if(scan[buffer_index_].timestamp_ == msg->timestamp) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for incoming msg timestamp \n "); + near_far_buffer_packets_[buffer_index_].timestamp = scan[buffer_index_].timestamp_; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending detections to packet, Buffer Index: %d\n ", buffer_index_); + near_far_buffer_packets_[buffer_index_].detections.insert(near_far_buffer_packets_[buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.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, Expected: %d ! \n ", 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; + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending detections to other packet, Buffer Index: %d! \n ", 1 - buffer_index_); + 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_[buffer_index_].detections.end(), + near_far_buffer_packets_[1 - buffer_index_].detections.insert(near_far_buffer_packets_[1 - buffer_index_].detections.end(), test_filtered_ars.detections.begin(), test_filtered_ars.detections.end()); scan[1 - buffer_index_].packet_count_++; @@ -249,16 +261,20 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].packet_count_ == scan_capacity) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for scan capacity: %d \n ", scan_capacity); scan[buffer_index_].publish_status_ = true; } if(near_scan_[buffer_index_].publish_status_ == true && far_scan_[buffer_index_].publish_status_ == true) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking if both packets are ready to be published! \n "); publish_packet = near_far_buffer_packets_[buffer_index_]; + + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing packet, Detections: %d \n ", publish_packet.detections.size()); near_far_buffer_packets_[buffer_index_].detections.clear(); - reset_scan_states(buffer_index_); + reset_scan_states(); buffer_index_ = 1 - buffer_index_; @@ -267,8 +283,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke return false; // Edge Cases to test - // 1. When there are more than 18 or 12 packets of the same timestamp being sent (rare) - // 2. Data collections start in the middle instead of right in the beginning (0 packet count) + // 1. Data collections start in the middle instead of right in the beginning (0 packet count) } } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 6306b75b..dddbb166 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -7,11 +7,33 @@ #include "ars_pointcloud_filter.hpp" -// Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR -TEST(ARSPointCloudFilterTest, CheckScanType) + +class ARSPointCloudFilterFixtureTest : 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::ARSPointCloudFilter pointcloudfilter; + filtering::ARSPointCloudFilter::filter_parameters parameters; +}; +// Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR +TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) +{ // Near Scan message auto msg_0 = std::make_shared(); @@ -32,11 +54,15 @@ TEST(ARSPointCloudFilterTest, CheckScanType) EXPECT_EQ(1, scan_type_1); } +// ---------------------- COMMON SCAN FILTER Test ------------------ + // Send 18 near scan packets with the same timestamp and check if it returns a complete packet ready to be published -TEST(ARSPointCloudFilterTest, CommonScanFilter_01) +TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) { - // Near scan data - filtering::ARSPointCloudFilter pointcloudfilter; + + SetUp("near"); + + // Near scan message auto msg = std::make_shared(); radar_msgs::msg::RadarDetection msg_detection; @@ -44,15 +70,6 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_01) // Fake Publish packet radar_msgs::msg::RadarPacket publish_packet; - filtering::ARSPointCloudFilter::filter_parameters parameters; - parameters.scan_mode = "near"; - parameters.vrel_rad_param = -9999.99; - parameters.el_ang_param = -9999.99; - parameters.rcs0_param = -9999.99; - parameters.snr_param = -9999.99; - parameters.range_param = -9999.99; - parameters.az_ang0_param = -9999.99; - // Packet Data msg->event_id = 3; msg->timestamp = 2; @@ -66,7 +83,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_01) pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - EXPECT_EQ(2, publish_packet.timestamp); + EXPECT_EQ(2, int(publish_packet.timestamp)); for(int index = 0; index < 18; index++) { @@ -76,10 +93,12 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_01) } // Send 12 far scan packets with the same timestamp and check if it returns a complete packet ready to be published -TEST(ARSPointCloudFilterTest, CommonScanFilter_02) +TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) { + + SetUp("far"); + // Far Scan Message - filtering::ARSPointCloudFilter pointcloudfilter; auto msg = std::make_shared(); radar_msgs::msg::RadarDetection msg_detection; @@ -87,16 +106,6 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_02) // Fake Publish packet radar_msgs::msg::RadarPacket publish_packet; - // Fake parameters - filtering::ARSPointCloudFilter::filter_parameters parameters; - parameters.scan_mode = "far"; - parameters.vrel_rad_param = -9999.99; - parameters.el_ang_param = -9999.99; - parameters.rcs0_param = -9999.99; - parameters.snr_param = -9999.99; - parameters.range_param = -9999.99; - parameters.az_ang0_param = -9999.99; - // Packet Data msg->event_id = 1; msg->timestamp = 7; @@ -110,7 +119,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_02) pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - EXPECT_EQ(7, publish_packet.timestamp); + EXPECT_EQ(7, int(publish_packet.timestamp)); for(int index = 0; index < 12; index++) { @@ -119,10 +128,11 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_02) } // Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) -TEST(ARSPointCloudFilterTest, CommonScanFilter_03) +TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) { - // Near Scan data - filtering::ARSPointCloudFilter pointcloudfilter; + SetUp("near"); + + // Near Scan Messages auto msg_0 = std::make_shared(); auto msg_1 = std::make_shared(); @@ -133,16 +143,6 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_03) radar_msgs::msg::RadarPacket publish_packet_0; radar_msgs::msg::RadarPacket publish_packet_1; - // Fake parameters - filtering::ARSPointCloudFilter::filter_parameters parameters; - parameters.scan_mode = "near"; - parameters.vrel_rad_param = -9999.99; - parameters.el_ang_param = -9999.99; - parameters.rcs0_param = -9999.99; - parameters.snr_param = -9999.99; - parameters.range_param = -9999.99; - parameters.az_ang0_param = -9999.99; - // Packet Data (Scan 1) msg_0->event_id = 3; msg_0->timestamp = 7; @@ -166,7 +166,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_03) pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet_0); } - EXPECT_EQ(7, publish_packet_0.timestamp); + EXPECT_EQ(7, int(publish_packet_0.timestamp)); for(int index = 0; index < 18; index++) { @@ -179,20 +179,20 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_03) pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet_1); } - EXPECT_EQ(3, publish_packet_1.timestamp); + EXPECT_EQ(3, int(publish_packet_1.timestamp)); for(int index = 0; index < 18; index++) { EXPECT_EQ(20, publish_packet_1.detections[index].rcs0); } - } // Send incomplete packets (5 that are the same) (1 following packet that is different in timestamp) -TEST(ARSPointCloudFilterTest, CommonScanFilter_04) +TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) { - // Near Scan data - filtering::ARSPointCloudFilter pointcloudfilter; + SetUp("near"); + + // Near Scan Messages auto msg_0 = std::make_shared(); auto msg_1 = std::make_shared(); @@ -202,16 +202,6 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_04) // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet_; - // Fake parameters - filtering::ARSPointCloudFilter::filter_parameters parameters; - parameters.scan_mode = "near"; - parameters.vrel_rad_param = -9999.99; - parameters.el_ang_param = -9999.99; - parameters.rcs0_param = -9999.99; - parameters.snr_param = -9999.99; - parameters.range_param = -9999.99; - parameters.az_ang0_param = -9999.99; - // Packet Data (Scan 1) msg_0->event_id = 3; msg_0->timestamp = 7; @@ -235,7 +225,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_04) } // Publish packet shouldn't be updated - EXPECT_EQ(0, publish_packet_.timestamp); + EXPECT_EQ(0, int(publish_packet_.timestamp)); // Scan 2 data (sending a packet of a different timestamp) for (int packet_size = 0; packet_size < 1; packet_size++) @@ -244,7 +234,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_04) } // Timestamp should be from msg_0 - EXPECT_EQ(7, publish_packet_.timestamp); + EXPECT_EQ(7, int(publish_packet_.timestamp)); for(int index = 0; index < 5; index++) { @@ -253,10 +243,11 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_04) } // Send far scan packets when in near scan mode. Check if it filters correctly. -TEST(ARSPointCloudFilterTest, CommonScanFilter_05) +TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) { - // Near Scan data - filtering::ARSPointCloudFilter pointcloudfilter; + SetUp("near"); + + // Near Scan Messages auto msg_0 = std::make_shared(); auto msg_1 = std::make_shared(); @@ -266,16 +257,6 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_05) // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet_; - // Fake parameters - filtering::ARSPointCloudFilter::filter_parameters parameters; - parameters.scan_mode = "near"; - parameters.vrel_rad_param = -9999.99; - parameters.el_ang_param = -9999.99; - parameters.rcs0_param = -9999.99; - parameters.snr_param = -9999.99; - parameters.range_param = -9999.99; - parameters.az_ang0_param = -9999.99; - // Packet Data (Scan 1) msg_0->event_id = 3; msg_0->timestamp = 7; @@ -299,7 +280,7 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_05) pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet_); } - EXPECT_EQ(7, publish_packet_.timestamp); + EXPECT_EQ(7, int(publish_packet_.timestamp)); for(int index = 0; index < 18; index++) { @@ -309,10 +290,11 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_05) } // Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) -TEST(ARSPointCloudFilterTest, CommonScanFilter_06) +TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) { - // Near Scan data - filtering::ARSPointCloudFilter pointcloudfilter; + SetUp("near", DEFAULT_FILTER_PARAM, DEFAULT_FILTER_PARAM, 70.0, 100.0, 40.0, DEFAULT_FILTER_PARAM); + + // Near Scan Message auto msg = std::make_shared(); radar_msgs::msg::RadarDetection msg_detection_0; @@ -322,51 +304,54 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_06) // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet_; - // Fake parameters - filtering::ARSPointCloudFilter::filter_parameters parameters; - parameters.scan_mode = "near"; - parameters.vrel_rad_param = -9999.99; - parameters.el_ang_param = -9999.99; - parameters.rcs0_param = 70.0; - parameters.snr_param = 100.0; - parameters.range_param = 40.0; - parameters.az_ang0_param = -9999.99; - // Packet Data (Scan 1) msg->event_id = 3; msg->timestamp = 7; + // Sending message with no detections + for(int packet_size = 0; packet_size < 17; packet_size++) + { + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet_); + } + + // message 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 parameters msg_detection_1.range = 50.0; - msg_detection_1.rcs0 = 60.0; + msg_detection_1.rcs0 = 90.0; msg_detection_1.snr = 200.0; msg->detections.push_back(msg_detection_1); + // message range < range threshold + // message 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 < 18; packet_size++) + for (int packet_size = 0; packet_size < 1; packet_size++) { pointcloudfilter.common_scan_filter(msg, parameters, publish_packet_); } - EXPECT_EQ(7, publish_packet_.timestamp); + EXPECT_EQ(7, int(publish_packet_.timestamp)); - // Return published packet with empty detections - EXPECT_EQ(0, publish_packet_.detections.size()); + // Return published packet with one detection going through + + EXPECT_EQ(1, int(publish_packet_.detections.size())); + EXPECT_EQ(50, int(publish_packet_.detections[0].range)); + EXPECT_EQ(90, int(publish_packet_.detections[0].rcs0)); + EXPECT_EQ(200, int(publish_packet_.detections[0].snr)); } -// Unit test cases +// Unit test cases (Common Scan Filter) // 1. Send 18 near scan packets and check if it returns a complete 18 packet ready to be published (done) // 2. Send 12 far scan packets and check if it returns a complete 12 packet ready to be published (done) // 3. Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) (done) @@ -375,3 +360,162 @@ TEST(ARSPointCloudFilterTest, CommonScanFilter_06) // 6. Send far scan packets when in near scan mode, does it filter correctly (done) // 7. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) (done) +// ---------------------- DOUBLE BUFFER Test ------------------ + +// Send 18 Near Scan Packets and 12 Far Scan Packets +TEST_F(ARSPointCloudFilterFixtureTest, 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; + + // Fake Publish packets + 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 = 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_); + } + + // Shouldn't be published at this point + EXPECT_EQ(0, int(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, int(publish_packet_.timestamp)); + EXPECT_EQ(30, publish_packet_.detections.size()); +} + + +// 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) +TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) +{ + 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; + + // Fake Publish packets + 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, int(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); + } + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 1 \n "); + // Scan 1 data shouldn't be published at this point + EXPECT_EQ(0, int(publish_packet_1.timestamp)); + EXPECT_EQ(0, publish_packet_1.detections.size()); + + // Near Packet Data (Scan 2) + msg_0->event_id = 3; + msg_0->timestamp = 8; + + msg_detection_0.range = 150.0; + + msg_0->detections.push_back(msg_detection_0); + + for (int packet_size = 0; packet_size < 3; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_2); + } + + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 2 \n "); + + for (int packet_size = 0; packet_size < 2; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); + } + // PACKET 1 SHOULD GET PUBLISHED HERE SINCE SCAN 1 DATA IS FULL AND ALL SCAN STATES SHOULD BE RESET + + EXPECT_EQ(6, int(publish_packet_1.timestamp)); + EXPECT_EQ(30, publish_packet_1.detections.size()); + + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 3 \n "); + + // SENDING DATA FROM SCAN 2 + for (int packet_size = 0; packet_size < 15; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_2); + } + + EXPECT_EQ(0, int(publish_packet_2.timestamp)); + EXPECT_EQ(0, publish_packet_2.detections.size()); + + // Far Packet Data (Scan 2) + msg_1->event_id = 1; + msg_1->timestamp = 9; + + msg_detection_1.range = 210.0; + + msg_1->detections.push_back(msg_detection_1); + + for (int packet_size = 0; packet_size < 12; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_2); + } + + EXPECT_EQ(9, int(publish_packet_2.timestamp)); + EXPECT_EQ(30, publish_packet_2.detections.size()); // DETECTIONS ARE RETURNING 60 for some reason (seems that publish packet doesnt reset in the actual implementation) + +} + +// Unit Test Cases +// 1. Send 18 Near Scan Packets and 12 Far Scan Packets (done) +// (It shouldn't publish after 18 packets. It should publish together when there is also complete 12 far scan packets) +// 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) +// 3. Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), then check if the 7 far is published +// 4. Send 5 Near (scan 1), Send 12 far \ No newline at end of file From db9f34ca2744b9e08de088da2bf4bcbcb6f06e7e Mon Sep 17 00:00:00 2001 From: Rijin Date: Mon, 13 Mar 2023 00:45:29 +0000 Subject: [PATCH 37/51] added more tests for double buffer algo --- .../src/ars_pointcloud_filter.cpp | 7 +- .../test/ars_pointcloud_filter_test.cpp | 155 ++++++++++++++++-- 2 files changed, 147 insertions(+), 15 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index cf588264..d068c2f0 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -91,6 +91,7 @@ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_ void ARSPointCloudFilter::reset_scan_states() { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Reseting all states! \n "); near_scan_[buffer_index_].timestamp_ = 0; near_scan_[buffer_index_].publish_status_ = false; near_scan_[buffer_index_].packet_count_= 0; @@ -223,6 +224,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].timestamp_ == default_timestamp_) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Buffer Index: %d\n ", buffer_index_); RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Comparing Scan buffer Timestamp with default timestamp, Scan Buffer Timestamp: %d\n ", scan[buffer_index_].timestamp_); scan[buffer_index_].timestamp_ = msg->timestamp; } @@ -265,9 +267,12 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke scan[buffer_index_].publish_status_ = true; } - if(near_scan_[buffer_index_].publish_status_ == true && far_scan_[buffer_index_].publish_status_ == true) + // Removed "near_scan_[buffer_index_].publish_status_ == true" from if condition + // then if this is true, then we know in general that packet is done collecting data from that particular scan (whether its the full 30 or not) + if(far_scan_[buffer_index_].publish_status_ == true) { RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking if both packets are ready to be published! \n "); + publish_packet = near_far_buffer_packets_[buffer_index_]; RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing packet, Detections: %d \n ", publish_packet.detections.size()); diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index dddbb166..a93bc00a 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -408,6 +408,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) EXPECT_EQ(6, int(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); } @@ -418,6 +420,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) 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; @@ -455,23 +459,28 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); } + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 1 \n "); + // Scan 1 data shouldn't be published at this point EXPECT_EQ(0, int(publish_packet_1.timestamp)); EXPECT_EQ(0, publish_packet_1.detections.size()); // Near Packet Data (Scan 2) - msg_0->event_id = 3; - msg_0->timestamp = 8; + msg_2->event_id = 3; + msg_2->timestamp = 8; msg_detection_0.range = 150.0; - msg_0->detections.push_back(msg_detection_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_0, parameters, publish_packet_2); + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_2); } + EXPECT_EQ(0, int(publish_packet_2.timestamp)); + EXPECT_EQ(0, publish_packet_2.detections.size()); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 2 \n "); @@ -479,14 +488,14 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); } - // PACKET 1 SHOULD GET PUBLISHED HERE SINCE SCAN 1 DATA IS FULL AND ALL SCAN STATES SHOULD BE RESET + // Packet 1 should get published here since scan 1 data is full and all scan states should be reset EXPECT_EQ(6, int(publish_packet_1.timestamp)); EXPECT_EQ(30, publish_packet_1.detections.size()); RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 3 \n "); - // SENDING DATA FROM SCAN 2 + // Sending data from scan 2 for (int packet_size = 0; packet_size < 15; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_2); @@ -496,26 +505,144 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) EXPECT_EQ(0, publish_packet_2.detections.size()); // Far Packet Data (Scan 2) - msg_1->event_id = 1; - msg_1->timestamp = 9; + msg_3->event_id = 1; + msg_3->timestamp = 9; msg_detection_1.range = 210.0; - msg_1->detections.push_back(msg_detection_1); + msg_3->detections.push_back(msg_detection_1); for (int packet_size = 0; packet_size < 12; packet_size++) { - pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_2); + pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet_2); } EXPECT_EQ(9, int(publish_packet_2.timestamp)); - EXPECT_EQ(30, publish_packet_2.detections.size()); // DETECTIONS ARE RETURNING 60 for some reason (seems that publish packet doesnt reset in the actual implementation) + 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()); +} + +TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) +{ + 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; + + // Fake Publish packets + radar_msgs::msg::RadarPacket publish_packet_0; + + // 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_0); + } + + EXPECT_EQ(0, int(publish_packet_0.timestamp)); + EXPECT_EQ(0, publish_packet_0.detections.size()); + + for (int packet_size = 0; packet_size < 12; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_0); + } + EXPECT_EQ(6, int(publish_packet_0.timestamp)); + EXPECT_EQ(17, publish_packet_0.detections.size()); + EXPECT_EQ(80, publish_packet_0.detections[0].range); + EXPECT_EQ(120, publish_packet_0.detections[6].range); +} + +// Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), then check if the 7 far is published +TEST_F(ARSPointCloudFilterFixtureTest, 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; + + // Fake Publish packets + radar_msgs::msg::RadarPacket publish_packet_0; + + // 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_0); + } + + EXPECT_EQ(0, int(publish_packet_0.timestamp)); + EXPECT_EQ(0, publish_packet_0.detections.size()); + + // Sending near packet data from scan 2 + for (int packet_size = 0; packet_size < 1; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_0); + } + + EXPECT_EQ(0, int(publish_packet_0.timestamp)); + EXPECT_EQ(0, publish_packet_0.detections.size()); + + // Sending far packet data from scan 2 + for (int packet_size = 0; packet_size < 1; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_0); + } + + EXPECT_EQ(6, int(publish_packet_0.timestamp)); + EXPECT_EQ(7, publish_packet_0.detections.size()); + } // Unit Test Cases // 1. Send 18 Near Scan Packets and 12 Far Scan Packets (done) // (It shouldn't publish after 18 packets. It should publish together when there is also complete 12 far scan packets) -// 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) -// 3. Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), then check if the 7 far is published -// 4. Send 5 Near (scan 1), Send 12 far \ No newline at end of file +// 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) (done) +// 3. Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (Scan 2), Send 1 Far (Scan 2)then check if the 7 far is published +// 4. Send 5 Near (scan 1), Send 12 far (scan 1) (done) \ No newline at end of file From e5596dc5f222f212391c5bfe9117ab9a46e6ad5c Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 14 Mar 2023 02:51:50 +0000 Subject: [PATCH 38/51] updated unit tests and reorganized code structure --- .../include/ars_pointcloud_filter.hpp | 29 ++-- .../include/ars_pointcloud_filter_node.hpp | 4 +- .../src/ars_pointcloud_filter.cpp | 21 --- .../src/ars_pointcloud_filter_node.cpp | 41 ++--- .../test/ars_pointcloud_filter_test.cpp | 145 ++++++++++-------- 5 files changed, 112 insertions(+), 128 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index be409218..a0228be8 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -4,7 +4,6 @@ #include #include "rclcpp/rclcpp.hpp" -// importing custom message types #include "radar_msgs/msg/radar_packet.hpp" #include "radar_msgs/msg/radar_detection.hpp" @@ -47,7 +46,9 @@ class ARSPointCloudFilter private: - // Filter message based on thresholds + /** + * @brief Pointfilter() filters an incoming radar message based on the parameters passed + */ radar_msgs::msg::RadarPacket point_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, @@ -57,11 +58,11 @@ class ARSPointCloudFilter double el_ang_threshold, double rcs_threshold); - // For all Filters + /** + * @brief Variables below are used for all the filters + */ unsigned int default_timestamp_; - - - // Create a struct with variables for near and far scans + typedef struct { unsigned int timestamp_; @@ -69,27 +70,19 @@ class ARSPointCloudFilter bool publish_status_; } scan_params; - /* - Near and far Scan filters + /** + * @brief Near & Far Scan Filter (Common Scan Filter) */ - scan_params near_scan_single_; scan_params far_scan_single_; - - // Create a buffer packet to hold detections from incoming messages (with the same timestamps) radar_msgs::msg::RadarPacket buffer_packet_; - /* - Near + Far Scan Filter (Double buffer) + /** + * @brief Near Far Scan Filter (Double Buffer Algorithm) */ - int buffer_index_; - - // Create two buffer packets std::array near_far_buffer_packets_; - - // Create an array of structs for each scan type std::array near_scan_; std::array far_scan_; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 3048b4aa..91abff5d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -41,10 +41,10 @@ class ARSPointCloudFilterNode : public rclcpp::Node void unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg); - // ROS2 Subscriber listening to the unfiltered radar packet topic (left sensor). + // ROS2 Subscriber listening to the unfiltered radar packet topic (left sensor) rclcpp::Subscription::SharedPtr raw_left_sub_; - // ROS2 Subscriber listening to the unfiltered radar packet topic (right sensor). + // ROS2 Subscriber listening to the unfiltered radar packet topic (right sensor) rclcpp::Subscription::SharedPtr raw_right_sub_; // ROS2 publisher that sends filtered messages from left and right radar to the processed topic. diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index d068c2f0..fd7a45d8 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -102,11 +102,6 @@ void ARSPointCloudFilter::reset_scan_states() } - - - - - // A common filter for near and far scan modes bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, @@ -189,16 +184,6 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: return false; } - - - - - - - - - - // Near + Far Scan Filter Implementation (Double Buffer Algorithm) bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, @@ -212,9 +197,6 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Returns which scan parameters the program needs to work on auto scan = (incoming_scan_msg == NEAR) ? near_scan_.data() : far_scan_.data(); - // RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Timestamp %d \n ",scan->timestamp_); - // RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Size %d \n ",scan->packet_count_); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Filtering the message \n "); const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( @@ -286,9 +268,6 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke return true; } return false; - - // Edge Cases to test - // 1. Data collections start in the middle instead of right in the beginning (0 packet count) } } // namespace filtering \ No newline at end of file diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 8acd028f..c0945463 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -6,8 +6,7 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") { - // Default values already declared in ars_radar_params.yaml - + // Default values are already declared in ars_radar_params.yaml this->declare_parameter("filter_mode"); this->declare_parameter("scan_mode"); this->declare_parameter("vrel_rad"); @@ -26,16 +25,12 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter parameters.az_ang0_param = this->get_parameter("az_ang0").as_double(); raw_left_sub_ = this->create_subscription( - "unfilteredRadarLeft", 1 , - std::bind( - &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, - std::placeholders::_1)); + "unfilteredRadarLeft", 1 , std::bind(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, + this, std::placeholders::_1)); raw_right_sub_ = this->create_subscription( - "unfilteredRadarRight", 1 , - std::bind( - &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, - std::placeholders::_1)); + "unfilteredRadarRight", 1 , std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, + this, std::placeholders::_1)); left_right_pub_ = this->create_publisher("processed", 20); @@ -47,24 +42,27 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - // If scan mode is near + /** + * Scan Mode == Near + * @brief Send incoming scan message to Near Scan filter and append detections to buffer packet based on timestamp. + Check if packet is ready to be published. Return true if ready, return false otherwise. + */ if(parameters.scan_mode == "near") { - // Create a temporary empty packet for when we have a full near scan filtered packet ready to be published radar_msgs::msg::RadarPacket publish_packet; - /** - Send message to scan filter and append detections to buffer packet based on timestamp. - Check if packet is ready to be published (if timestamp differs). Return true if ready to be published, return false otherwise. - **/ if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) { - // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); } } - // If scan mode is far + + /** + * Scan Mode == Far + * @brief Send incoming scan message to Far Scan filter and append detections to buffer packet based on timestamp. + Check if packet is ready to be published. Return true if ready, return false otherwise. + */ else if(parameters.scan_mode == "far") { radar_msgs::msg::RadarPacket publish_packet; @@ -76,7 +74,12 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( } } - // If scan mode is near_far (Double Buffer) + /** + * Scan Mode == NearFar + * @brief Send incoming scan message to NearFar Scan filter and append detections + * to buffer packet based on double buffer algorithm. Check if packet is ready to be published. + * Return true if ready, return false otherwise. + */ else { radar_msgs::msg::RadarPacket publish_packet_near_far; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index a93bc00a..7c3fdda8 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -31,7 +31,9 @@ class ARSPointCloudFilterFixtureTest : public ::testing::Test filtering::ARSPointCloudFilter::filter_parameters parameters; }; -// Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR +/** +* @brief Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR +*/ TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) { // Near Scan message @@ -49,17 +51,17 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) auto scan_type_0 = pointcloudfilter.check_scan_type(msg_0); auto scan_type_1 = pointcloudfilter.check_scan_type(msg_1); - // Near = 0 & Far = 1 + // Near = 0 and Far = 1 EXPECT_EQ(0, scan_type_0); EXPECT_EQ(1, scan_type_1); } -// ---------------------- COMMON SCAN FILTER Test ------------------ - -// Send 18 near scan packets with the same timestamp and check if it returns a complete packet ready to be published +/** +* @brief Sends 18 near scan packets with the same timestamp and checks if it returns a + complete packet ready to be published +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) { - SetUp("near"); // Near scan message @@ -67,10 +69,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) radar_msgs::msg::RadarDetection msg_detection; - // Fake Publish packet radar_msgs::msg::RadarPacket publish_packet; - // Packet Data msg->event_id = 3; msg->timestamp = 2; @@ -89,10 +89,12 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) { EXPECT_EQ(100, publish_packet.detections[index].snr); } - } -// Send 12 far scan packets with the same timestamp and check if it returns a complete packet ready to be published +/** +* @brief Sends 12 far scan packets with the same timestamp and checks if it returns a + complete packet ready to be published +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) { @@ -103,10 +105,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) radar_msgs::msg::RadarDetection msg_detection; - // Fake Publish packet radar_msgs::msg::RadarPacket publish_packet; - // Packet Data msg->event_id = 1; msg->timestamp = 7; @@ -127,19 +127,19 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) } } -// Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) +/** +* @brief Sends 18 packets that are the same (timestamp 1), and 18 packets with a new timestamp. Checks if it + returns a complete packet ready to be published. +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) { SetUp("near"); - // Near Scan Messages 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::RadarDetection msg_detection; - // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet_0; radar_msgs::msg::RadarPacket publish_packet_1; @@ -147,18 +147,17 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) msg_0->event_id = 3; msg_0->timestamp = 7; + msg_detection.rcs0 = 5.0; - msg_detection_0.rcs0 = 5.0; - - msg_0->detections.push_back(msg_detection_0); + msg_0->detections.push_back(msg_detection); // Packet Data (Scan 2) msg_1->event_id = 4; msg_1->timestamp = 3; - msg_detection_1.rcs0 = 20.0; + msg_detection.rcs0 = 20.0; - msg_1->detections.push_back(msg_detection_1); + msg_1->detections.push_back(msg_detection); // Scan 1 for (int packet_size = 0; packet_size < 18; packet_size++) @@ -187,7 +186,9 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) } } -// Send incomplete packets (5 that are the same) (1 following packet that is different in timestamp) +/** +* @brief Sends incomplete packets (5 that are the same) (1 following packet that is different in timestamp) +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) { SetUp("near"); @@ -196,53 +197,53 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) 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::RadarDetection msg_detection; - // Fake Publish packets - radar_msgs::msg::RadarPacket publish_packet_; + radar_msgs::msg::RadarPacket publish_packet; // Packet Data (Scan 1) msg_0->event_id = 3; msg_0->timestamp = 7; - msg_detection_0.range = 120.0; + msg_detection.range = 120.0; - msg_0->detections.push_back(msg_detection_0); + msg_0->detections.push_back(msg_detection); // Packet Data (Scan 2) msg_1->event_id = 4; msg_1->timestamp = 3; - msg_detection_1.range = 50.0; + msg_detection.range = 50.0; - msg_1->detections.push_back(msg_detection_1); + 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_); + pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet); } // Publish packet shouldn't be updated - EXPECT_EQ(0, int(publish_packet_.timestamp)); + EXPECT_EQ(0, int(publish_packet.timestamp)); // Scan 2 data (sending a packet of a different timestamp) for (int packet_size = 0; packet_size < 1; packet_size++) { - pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet_); + pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet); } // Timestamp should be from msg_0 - EXPECT_EQ(7, int(publish_packet_.timestamp)); + EXPECT_EQ(7, int(publish_packet.timestamp)); for(int index = 0; index < 5; index++) { - EXPECT_EQ(120, publish_packet_.detections[index].range); + EXPECT_EQ(120, publish_packet.detections[index].range); } } -// Send far scan packets when in near scan mode. Check if it filters correctly. +/** +* @brief Sends far scan packets when in near scan mode. Checks if it filters correctly. +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) { SetUp("near"); @@ -251,45 +252,46 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) 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::RadarDetection msg_detection; - // Fake Publish packets - radar_msgs::msg::RadarPacket publish_packet_; + radar_msgs::msg::RadarPacket publish_packet; // Packet Data (Scan 1) msg_0->event_id = 3; msg_0->timestamp = 7; - msg_detection_0.range = 120.0; + msg_detection.range = 120.0; - msg_0->detections.push_back(msg_detection_0); + msg_0->detections.push_back(msg_detection); // Packet Data (Scan 2) msg_1->event_id = 1; msg_1->timestamp = 3; - msg_detection_1.range = 50.0; + msg_detection.range = 50.0; - msg_1->detections.push_back(msg_detection_1); + 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_); + pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet); + pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet); } - EXPECT_EQ(7, int(publish_packet_.timestamp)); + EXPECT_EQ(7, int(publish_packet.timestamp)); for(int index = 0; index < 18; index++) { - EXPECT_EQ(120, publish_packet_.detections[index].range); + EXPECT_EQ(120, publish_packet.detections[index].range); } } -// Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) +/** +* @brief Checks if a packet with several detections is filtered out and if the same packet is + returned with the correct unfiltered detection. +*/ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) { SetUp("near", DEFAULT_FILTER_PARAM, DEFAULT_FILTER_PARAM, 70.0, 100.0, 40.0, DEFAULT_FILTER_PARAM); @@ -302,7 +304,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) radar_msgs::msg::RadarDetection msg_detection_2; // Fake Publish packets - radar_msgs::msg::RadarPacket publish_packet_; + radar_msgs::msg::RadarPacket publish_packet; // Packet Data (Scan 1) msg->event_id = 3; @@ -311,10 +313,10 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) // Sending message with no detections for(int packet_size = 0; packet_size < 17; packet_size++) { - pointcloudfilter.common_scan_filter(msg, parameters, publish_packet_); + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - // message snr < snr threshold + // msg snr < snr threshold msg_detection_0.range = 120.0; msg_detection_0.rcs0 = 75.0; msg_detection_0.snr = 80.0; @@ -328,8 +330,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) msg->detections.push_back(msg_detection_1); - // message range < range threshold - // message snr < snr threshold + // msg range < range threshold & msg snr < snr threshold msg_detection_2.range = 10.0; msg_detection_2.rcs0 = 90.0; msg_detection_2.snr = 50.0; @@ -338,17 +339,16 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) for (int packet_size = 0; packet_size < 1; packet_size++) { - pointcloudfilter.common_scan_filter(msg, parameters, publish_packet_); + pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - EXPECT_EQ(7, int(publish_packet_.timestamp)); - - // Return published packet with one detection going through + EXPECT_EQ(7, int(publish_packet.timestamp)); - EXPECT_EQ(1, int(publish_packet_.detections.size())); - EXPECT_EQ(50, int(publish_packet_.detections[0].range)); - EXPECT_EQ(90, int(publish_packet_.detections[0].rcs0)); - EXPECT_EQ(200, int(publish_packet_.detections[0].snr)); + // Returns a published packet with one detection going through + EXPECT_EQ(1, int(publish_packet.detections.size())); + EXPECT_EQ(50, int(publish_packet.detections[0].range)); + EXPECT_EQ(90, int(publish_packet.detections[0].rcs0)); + EXPECT_EQ(200, int(publish_packet.detections[0].snr)); } // Unit test cases (Common Scan Filter) @@ -360,9 +360,11 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) // 6. Send far scan packets when in near scan mode, does it filter correctly (done) // 7. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) (done) -// ---------------------- DOUBLE BUFFER Test ------------------ +// Double Buffer Algorithm Test -// Send 18 Near Scan Packets and 12 Far Scan Packets +/** +* @brief Send 18 Near Scan Packets and 12 Far Scan Packets +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) { SetUp("nearfar"); @@ -412,8 +414,10 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) EXPECT_EQ(90, publish_packet_.detections[19].range); } - -// 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) +/** +* @brief 18 Near scan (scan 1), 10 far scan (scan 1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), + 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { SetUp("nearfar"); @@ -523,6 +527,9 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) EXPECT_EQ(30, publish_packet_2.detections.size()); } +/** +* @brief Send 5 Near (scan 1), Send 12 far (scan 1) +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) { SetUp("nearfar"); @@ -571,7 +578,9 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) EXPECT_EQ(120, publish_packet_0.detections[6].range); } -// Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), then check if the 7 far is published +/** +* @brief Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), then check if the 7 far is published +*/ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) { SetUp("nearfar"); From 623ff92806d41f78fc614c2fe2a9d62f650c67d0 Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 14 Mar 2023 22:37:02 +0000 Subject: [PATCH 39/51] Saving code structure before major changes --- .../ARSPointCloudFilter/src/ars_pointcloud_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index fd7a45d8..b16cec39 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -9,7 +9,7 @@ namespace filtering { ARSPointCloudFilter::ARSPointCloudFilter() -{ +{ near_scan_single_.timestamp_ = 0; near_scan_single_.packet_count_= 0; From 6c5e527c38aaf68867b83136802e3f06b3050451 Mon Sep 17 00:00:00 2001 From: Rijin Date: Thu, 16 Mar 2023 03:07:47 +0000 Subject: [PATCH 40/51] updated unit tests and filter implementation --- .../ARSPointCloudFilter/CMakeLists.txt | 18 +-- .../ARSPointCloudFilter/package.xml | 4 +- .../src/ars_pointcloud_filter.cpp | 49 +++++--- .../test/ars_pointcloud_filter_test.cpp | 112 +++++++++++------- 4 files changed, 114 insertions(+), 69 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt index 34642162..b655fe85 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt +++ b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt @@ -34,18 +34,18 @@ 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) 37-48 - # find_package(ament_lint_common 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) + # 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") + # Reinstall cpplint ignoring copyright errors + ament_cpplint(FILTERS "-legal/copyright") - # ament_lint_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() # Create test executable from source files ament_add_gtest(ars_pointcloud_filter_test test/ars_pointcloud_filter_test.cpp) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/package.xml b/src/sensor_interfacing/ARSPointCloudFilter/package.xml index 5f90f25d..3f55f068 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/package.xml +++ b/src/sensor_interfacing/ARSPointCloudFilter/package.xml @@ -12,8 +12,8 @@ rclcpp radar_msgs - + ament_lint_auto + ament_lint_common ament_cmake_gtest diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index b16cec39..1a187c96 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -128,7 +128,7 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: } // Filter out detections based on given thresholds (PASS IN ONLY PARAMETERS) - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + 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); @@ -142,8 +142,8 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: // Append all detections to buffer buffer_packet_.detections.insert(buffer_packet_.detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + filtered_packet.detections.begin(), + filtered_packet.detections.end()); scan->packet_count_++; RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending to buffer packet, size: %d\n ",scan->packet_count_); @@ -160,25 +160,18 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: scan->timestamp_ = default_timestamp_; return true; } - } else { if(scan->packet_count_!= scan_capacity) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! \n ", scan->packet_count_); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! Packet Discarded.\n ", scan->packet_count_); } - - // Publish buffer packet - publish_packet = buffer_packet_; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing Packet, size: %d\n ",scan->packet_count_); - + // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering - buffer_packet_ = test_filtered_ars; + buffer_packet_ = filtered_packet; scan->packet_count_ = 1; - scan->timestamp_ = msg->timestamp; - return true; } } return false; @@ -199,7 +192,7 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Filtering the message \n "); - const radar_msgs::msg::RadarPacket test_filtered_ars = point_filter( + 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); @@ -213,6 +206,13 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].timestamp_ == msg->timestamp) { + // Special case (If you start in the middle with far scans) + if(incoming_scan_msg == FAR && near_scan_[buffer_index_].packet_count_ == 0) + { + scan[buffer_index_].timestamp_ = default_timestamp_; + return false; + } + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for incoming msg timestamp \n "); near_far_buffer_packets_[buffer_index_].timestamp = scan[buffer_index_].timestamp_; @@ -220,8 +220,8 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending detections to packet, Buffer Index: %d\n ", buffer_index_); near_far_buffer_packets_[buffer_index_].detections.insert(near_far_buffer_packets_[buffer_index_].detections.end(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + filtered_packet.detections.begin(), + filtered_packet.detections.end()); scan[buffer_index_].packet_count_++; } else @@ -238,8 +238,8 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke 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(), - test_filtered_ars.detections.begin(), - test_filtered_ars.detections.end()); + filtered_packet.detections.begin(), + filtered_packet.detections.end()); scan[1 - buffer_index_].packet_count_++; } @@ -253,6 +253,19 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // then if this is true, then we know in general that packet is done collecting data from that particular scan (whether its the full 30 or not) if(far_scan_[buffer_index_].publish_status_ == true) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking packet count Near: %d, Far: %d \n ",near_scan_[buffer_index_].packet_count_,far_scan_[buffer_index_].packet_count_); + + // If the packet started in the middle, and it does not have all 30 detections (special case discard) + if(near_scan_[buffer_index_].packet_count_ != 18 || far_scan_[buffer_index_].packet_count_ != 12) + { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking incomplete packet count %d \n ",near_scan_[buffer_index_].packet_count_); + + near_far_buffer_packets_[buffer_index_].detections.clear(); + reset_scan_states(); + buffer_index_ = 1 - buffer_index_; + return false; + } + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking if both packets are ready to be published! \n "); publish_packet = near_far_buffer_packets_[buffer_index_]; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 7c3fdda8..ecd6c555 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -227,17 +227,17 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) EXPECT_EQ(0, int(publish_packet.timestamp)); // Scan 2 data (sending a packet of a different timestamp) - for (int packet_size = 0; packet_size < 1; packet_size++) + for (int packet_size = 0; packet_size < 18; packet_size++) { pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet); } - // Timestamp should be from msg_0 - EXPECT_EQ(7, int(publish_packet.timestamp)); + // Timestamp should be from msg_1 (msg_0 should be discarded) + EXPECT_EQ(3, int(publish_packet.timestamp)); - for(int index = 0; index < 5; index++) + for(int index = 0; index < 18; index++) { - EXPECT_EQ(120, publish_packet.detections[index].range); + EXPECT_EQ(50, publish_packet.detections[index].range); } } @@ -285,7 +285,6 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) { EXPECT_EQ(120, publish_packet.detections[index].range); } - } /** @@ -349,16 +348,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) EXPECT_EQ(50, int(publish_packet.detections[0].range)); EXPECT_EQ(90, int(publish_packet.detections[0].rcs0)); EXPECT_EQ(200, int(publish_packet.detections[0].snr)); - } -// Unit test cases (Common Scan Filter) -// 1. Send 18 near scan packets and check if it returns a complete 18 packet ready to be published (done) -// 2. Send 12 far scan packets and check if it returns a complete 12 packet ready to be published (done) -// 3. Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) (done) -// 4. Send incomplete packets (5 that are the same) (1 that is different in timestamp) (done) -// 5. Send 19 near scan packets and see if it behaves correctly (Is not required) (done) -// 6. Send far scan packets when in near scan mode, does it filter correctly (done) -// 7. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) (done) // Double Buffer Algorithm Test @@ -485,7 +475,6 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) EXPECT_EQ(0, int(publish_packet_2.timestamp)); EXPECT_EQ(0, publish_packet_2.detections.size()); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 2 \n "); for (int packet_size = 0; packet_size < 2; packet_size++) @@ -536,12 +525,14 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) 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; // Fake Publish packets - radar_msgs::msg::RadarPacket publish_packet_0; + radar_msgs::msg::RadarPacket publish_packet; // Near Packet Data (Scan 1) msg_0->event_id = 3; @@ -561,21 +552,53 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) for (int packet_size = 0; packet_size < 5; packet_size++) { - pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_0); + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet); } - EXPECT_EQ(0, int(publish_packet_0.timestamp)); - EXPECT_EQ(0, publish_packet_0.detections.size()); + EXPECT_EQ(0, int(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_0); + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet); + } + + // Special Case data should be discarded at this point + EXPECT_EQ(0, int(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); } - EXPECT_EQ(6, int(publish_packet_0.timestamp)); - EXPECT_EQ(17, publish_packet_0.detections.size()); - EXPECT_EQ(80, publish_packet_0.detections[0].range); - EXPECT_EQ(120, publish_packet_0.detections[6].range); + for(int packet_size = 0; packet_size < 12; packet_size++) + { + pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet); + } + + EXPECT_EQ(8, int(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); } /** @@ -593,8 +616,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) radar_msgs::msg::RadarDetection msg_detection_1; radar_msgs::msg::RadarDetection msg_detection_2; - // Fake Publish packets - radar_msgs::msg::RadarPacket publish_packet_0; + // Fake Publish Packet + radar_msgs::msg::RadarPacket publish_packet; // Far Packet Data (Scan 1) msg_0->event_id = 1; @@ -623,33 +646,42 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) // 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_0); + pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet); } - EXPECT_EQ(0, int(publish_packet_0.timestamp)); - EXPECT_EQ(0, publish_packet_0.detections.size()); + EXPECT_EQ(0, int(publish_packet.timestamp)); + EXPECT_EQ(0, publish_packet.detections.size()); // Sending near packet data from scan 2 - for (int packet_size = 0; packet_size < 1; packet_size++) + for (int packet_size = 0; packet_size < 18; packet_size++) { - pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_0); + pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet); } - EXPECT_EQ(0, int(publish_packet_0.timestamp)); - EXPECT_EQ(0, publish_packet_0.detections.size()); + EXPECT_EQ(0, int(publish_packet.timestamp)); + EXPECT_EQ(0, publish_packet.detections.size()); // Sending far packet data from scan 2 - for (int packet_size = 0; packet_size < 1; packet_size++) + for (int packet_size = 0; packet_size < 12; packet_size++) { - pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_0); + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet); } - EXPECT_EQ(6, int(publish_packet_0.timestamp)); - EXPECT_EQ(7, publish_packet_0.detections.size()); - + EXPECT_EQ(8, int(publish_packet.timestamp)); + EXPECT_EQ(30, publish_packet.detections.size()); } -// Unit Test Cases + +// Unit test cases (Common Scan Filter) +// 1. Send 18 near scan packets and check if it returns a complete 18 packet ready to be published (done) +// 2. Send 12 far scan packets and check if it returns a complete 12 packet ready to be published (done) +// 3. Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) (done) +// 4. Send incomplete packets (5 that are the same) (1 that is different in timestamp) (done) +// 5. Send 19 near scan packets and see if it behaves correctly (is not required) (done) +// 6. Send far scan packets when in near scan mode, does it filter correctly (done) +// 7. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) (done) + +// Unit Test Cases (Double Buffer Algorithm) // 1. Send 18 Near Scan Packets and 12 Far Scan Packets (done) // (It shouldn't publish after 18 packets. It should publish together when there is also complete 12 far scan packets) // 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) (done) From 982a25c12eabf105a69e3448642adc158966abf5 Mon Sep 17 00:00:00 2001 From: Rijin Date: Fri, 17 Mar 2023 02:53:04 +0000 Subject: [PATCH 41/51] removed log statements, and changed files based on the linter --- .../ARSPointCloudFilter/CMakeLists.txt | 18 +- .../launch/ars_pointcloud_filter.launch.py | 4 +- .../ARSPointCloudFilter/package.xml | 4 +- .../src/ars_pointcloud_filter.cpp | 86 +++------ .../src/ars_pointcloud_filter_node.cpp | 13 +- .../test/ars_pointcloud_filter_test.cpp | 163 +++++++++--------- 6 files changed, 122 insertions(+), 166 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt index b655fe85..77d00dc3 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt +++ b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt @@ -34,18 +34,18 @@ 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) + # 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) + # # 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") + # # Reinstall cpplint ignoring copyright errors + # ament_cpplint(FILTERS "-legal/copyright") - ament_lint_auto_find_test_dependencies() + # ament_lint_auto_find_test_dependencies() # Create test executable from source files ament_add_gtest(ars_pointcloud_filter_test test/ars_pointcloud_filter_test.cpp) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py index d331a045..5639a890 100755 --- a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py @@ -24,8 +24,8 @@ def generate_launch_description(): {'az_ang0': -99999.99}, {'scan_mode': 'near'} ] - ) + ) return LaunchDescription([ ars_pointcloud_filter_param, ars_pointcloud_filter_node - ]) \ No newline at end of file + ]) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/package.xml b/src/sensor_interfacing/ARSPointCloudFilter/package.xml index 3f55f068..c02effcb 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/package.xml +++ b/src/sensor_interfacing/ARSPointCloudFilter/package.xml @@ -12,8 +12,8 @@ rclcpp radar_msgs - ament_lint_auto - ament_lint_common + ament_cmake_gtest diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 1a187c96..ed08aed5 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -37,8 +37,9 @@ ARSPointCloudFilter::ARSPointCloudFilter() } -// Point Filter Implementation - +/** +* @brief Filters packet detections based on set thresholds +*/ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, double AzAng0_threshold, double range_threshold, double vrel_rad_threshold, @@ -76,7 +77,9 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( return filtered_ars; } -// Checks Event ID and returns which scan it is (NEAR OR FAR) +/** +* @brief Checks Event ID and returns which scan it is (NEAR OR FAR) +*/ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr msg) { if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) @@ -89,9 +92,11 @@ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_ } } +/** +* @brief Resets all scan states +*/ void ARSPointCloudFilter::reset_scan_states() { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Reseting all states! \n "); near_scan_[buffer_index_].timestamp_ = 0; near_scan_[buffer_index_].publish_status_ = false; near_scan_[buffer_index_].packet_count_= 0; @@ -99,10 +104,11 @@ void ARSPointCloudFilter::reset_scan_states() far_scan_[buffer_index_].timestamp_ = 0; far_scan_[buffer_index_].publish_status_ = false; far_scan_[buffer_index_].packet_count_= 0; - } -// A common filter for near and far scan modes +/** +* @brief A common filter for near and far scan modes +*/ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) @@ -112,28 +118,21 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; - // Returns which scan parameters the program needs to work on + // Returns which scan parameters the program is using 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")) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Timestamp %d \n ",scan->timestamp_); - - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Initial Packet Size %d \n ",scan->packet_count_); - if(scan->timestamp_ == default_timestamp_) { scan->timestamp_ = msg->timestamp; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for Default Timestamp \n "); } - // Filter out detections based on given thresholds (PASS IN ONLY PARAMETERS) + // Filter out detections based on given thresholds 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); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Filtering the message \n "); - // If near/far packet is not full if(scan->packet_count_ != scan_capacity && msg->timestamp == scan->timestamp_) { @@ -145,30 +144,23 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: filtered_packet.detections.begin(), filtered_packet.detections.end()); scan->packet_count_++; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending to buffer packet, size: %d\n ",scan->packet_count_); if(scan->packet_count_ == scan_capacity) { - // Publish buffer packet publish_packet = buffer_packet_; - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing Packet, size: %d\n ",scan->packet_count_); - buffer_packet_.detections.clear(); - scan->packet_count_ = 0; - scan->timestamp_ = default_timestamp_; return true; } } else { + // Special Case (If the scans start in the middle) if(scan->packet_count_!= scan_capacity) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! Packet Discarded.\n ", scan->packet_count_); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! Packet Discarded.\n ", scan->packet_count_); } - - // Replace existing data in buffer packet with new incoming data (new timestamp) after filtering buffer_packet_ = filtered_packet; scan->packet_count_ = 1; scan->timestamp_ = msg->timestamp; @@ -177,48 +169,39 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: return false; } -// Near + Far Scan Filter Implementation (Double Buffer Algorithm) +/** +* @brief Near + Far Scan Filter Implementation (Double Buffer Algorithm) +*/ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { - // Variables to create on every callback (message can be near or far) scan_type incoming_scan_msg = check_scan_type(msg); int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; - // Returns which scan parameters the program needs to work on auto scan = (incoming_scan_msg == NEAR) ? near_scan_.data() : far_scan_.data(); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Filtering the message \n "); - 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_) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Buffer Index: %d\n ", buffer_index_); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Comparing Scan buffer Timestamp with default timestamp, Scan Buffer Timestamp: %d\n ", scan[buffer_index_].timestamp_); scan[buffer_index_].timestamp_ = msg->timestamp; } if(scan[buffer_index_].timestamp_ == msg->timestamp) { - // Special case (If you start in the middle with far scans) + // Special case (If you start in the middle with far scans, and no near scans were collected before) if(incoming_scan_msg == FAR && near_scan_[buffer_index_].packet_count_ == 0) { + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Ignoring far scan packet! \n"); scan[buffer_index_].timestamp_ = default_timestamp_; return false; } - - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for incoming msg timestamp \n "); near_far_buffer_packets_[buffer_index_].timestamp = scan[buffer_index_].timestamp_; - - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending detections to packet, Buffer Index: %d\n ", buffer_index_); - near_far_buffer_packets_[buffer_index_].detections.insert(near_far_buffer_packets_[buffer_index_].detections.end(), filtered_packet.detections.begin(), filtered_packet.detections.end()); @@ -228,15 +211,11 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke { 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_); + 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; - - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Appending detections to other packet, Buffer Index: %d! \n ", 1 - buffer_index_); - 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()); @@ -245,42 +224,29 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].packet_count_ == scan_capacity) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking for scan capacity: %d \n ", scan_capacity); scan[buffer_index_].publish_status_ = true; } - // Removed "near_scan_[buffer_index_].publish_status_ == true" from if condition - // then if this is true, then we know in general that packet is done collecting data from that particular scan (whether its the full 30 or not) + // If true, packet is done collecting data from that particular scan (whether its the full 30 or not) if(far_scan_[buffer_index_].publish_status_ == true) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking packet count Near: %d, Far: %d \n ",near_scan_[buffer_index_].packet_count_,far_scan_[buffer_index_].packet_count_); - - // If the packet started in the middle, and it does not have all 30 detections (special case discard) + // If the packet started in the middle, and it does not have all 30 detections (Special Case) if(near_scan_[buffer_index_].packet_count_ != 18 || far_scan_[buffer_index_].packet_count_ != 12) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking incomplete packet count %d \n ",near_scan_[buffer_index_].packet_count_); - + 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; } - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Checking if both packets are ready to be published! \n "); - publish_packet = near_far_buffer_packets_[buffer_index_]; - - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Publishing packet, Detections: %d \n ", publish_packet.detections.size()); - near_far_buffer_packets_[buffer_index_].detections.clear(); - reset_scan_states(); - buffer_index_ = 1 - buffer_index_; - return true; } return false; } -} // namespace filtering \ No newline at end of file +} // namespace filtering diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index c0945463..d0a0d244 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -24,22 +24,20 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter 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(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, + raw_left_sub_ = this->create_subscription("unfilteredRadarLeft", + 1, std::bind(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); - raw_right_sub_ = this->create_subscription( - "unfilteredRadarRight", 1 , std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, + raw_right_sub_ = this->create_subscription("unfilteredRadarRight", + 1, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); - - left_right_pub_ = this->create_publisher("processed", 20); + left_right_pub_ = this->create_publisher("processed", 20); } void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); /** @@ -123,7 +121,6 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far) == true) { - // Publish buffer packet RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); left_right_pub_->publish(publish_packet_near_far); } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index ecd6c555..dc5985d5 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -13,9 +13,10 @@ class ARSPointCloudFilterFixtureTest : 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) + 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; @@ -32,7 +33,7 @@ class ARSPointCloudFilterFixtureTest : public ::testing::Test }; /** -* @brief Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR +* @brief Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR. */ TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) { @@ -58,7 +59,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) /** * @brief Sends 18 near scan packets with the same timestamp and checks if it returns a - complete packet ready to be published + complete packet ready to be published. */ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) { @@ -83,8 +84,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - EXPECT_EQ(2, int(publish_packet.timestamp)); - + EXPECT_EQ(2, static_cast(publish_packet.timestamp)); for(int index = 0; index < 18; index++) { EXPECT_EQ(100, publish_packet.detections[index].snr); @@ -93,11 +93,10 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) /** * @brief Sends 12 far scan packets with the same timestamp and checks if it returns a - complete packet ready to be published + complete packet ready to be published. */ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) { - SetUp("far"); // Far Scan Message @@ -119,8 +118,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - EXPECT_EQ(7, int(publish_packet.timestamp)); - + EXPECT_EQ(7, static_cast(publish_packet.timestamp)); for(int index = 0; index < 12; index++) { EXPECT_EQ(5, publish_packet.detections[index].rcs0); @@ -128,8 +126,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) } /** -* @brief Sends 18 packets that are the same (timestamp 1), and 18 packets with a new timestamp. Checks if it - returns a complete packet ready to be published. +* @brief Sends 18 packets that are the same (timestamp 1), and 18 packets with a new timestamp. + Checks if it returns a complete packet ready to be published. */ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) { @@ -159,14 +157,13 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) msg_1->detections.push_back(msg_detection); - // Scan 1 + // 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, int(publish_packet_0.timestamp)); - + 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); @@ -178,8 +175,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet_1); } - EXPECT_EQ(3, int(publish_packet_1.timestamp)); - + 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); @@ -187,7 +183,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) } /** -* @brief Sends incomplete packets (5 that are the same) (1 following packet that is different in timestamp) +* @brief Sends incomplete packets (5 that are the same and + 1 packet after that is different in timestamp). */ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) { @@ -205,7 +202,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) msg_0->event_id = 3; msg_0->timestamp = 7; - msg_detection.range = 120.0; + msg_detection.range = 120.0; msg_0->detections.push_back(msg_detection); @@ -224,7 +221,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) } // Publish packet shouldn't be updated - EXPECT_EQ(0, int(publish_packet.timestamp)); + 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++) @@ -233,8 +230,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) } // Timestamp should be from msg_1 (msg_0 should be discarded) - EXPECT_EQ(3, int(publish_packet.timestamp)); - + EXPECT_EQ(3, static_cast(publish_packet.timestamp)); for(int index = 0; index < 18; index++) { EXPECT_EQ(50, publish_packet.detections[index].range); @@ -260,7 +256,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) msg_0->event_id = 3; msg_0->timestamp = 7; - msg_detection.range = 120.0; + msg_detection.range = 120.0; msg_0->detections.push_back(msg_detection); @@ -278,8 +274,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet); pointcloudfilter.common_scan_filter(msg_1, parameters, publish_packet); } - - EXPECT_EQ(7, int(publish_packet.timestamp)); + EXPECT_EQ(7, static_cast(publish_packet.timestamp)); for(int index = 0; index < 18; index++) { @@ -293,7 +288,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) */ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) { - SetUp("near", DEFAULT_FILTER_PARAM, DEFAULT_FILTER_PARAM, 70.0, 100.0, 40.0, DEFAULT_FILTER_PARAM); + SetUp("near", DEFAULT_FILTER_PARAM, DEFAULT_FILTER_PARAM, 70, 100, 40, DEFAULT_FILTER_PARAM); // Near Scan Message auto msg = std::make_shared(); @@ -316,21 +311,21 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) } // msg snr < snr threshold - msg_detection_0.range = 120.0; + 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 parameters - msg_detection_1.range = 50.0; + 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); // msg range < range threshold & msg snr < snr threshold - msg_detection_2.range = 10.0; + msg_detection_2.range = 10.0; msg_detection_2.rcs0 = 90.0; msg_detection_2.snr = 50.0; @@ -340,20 +335,19 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) { pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - - EXPECT_EQ(7, int(publish_packet.timestamp)); + EXPECT_EQ(7, static_cast(publish_packet.timestamp)); // Returns a published packet with one detection going through - EXPECT_EQ(1, int(publish_packet.detections.size())); - EXPECT_EQ(50, int(publish_packet.detections[0].range)); - EXPECT_EQ(90, int(publish_packet.detections[0].rcs0)); - EXPECT_EQ(200, int(publish_packet.detections[0].snr)); + 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 Test /** -* @brief Send 18 Near Scan Packets and 12 Far Scan Packets +* @brief Send 18 Near Scan Packets and 12 Far Scan Packets. */ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) { @@ -372,7 +366,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) msg_0->event_id = 3; msg_0->timestamp = 5; - msg_detection_0.range = 80.0; + msg_detection_0.range = 80.0; msg_0->detections.push_back(msg_detection_0); @@ -390,7 +384,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) } // Shouldn't be published at this point - EXPECT_EQ(0, int(publish_packet_.timestamp)); + 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++) @@ -398,15 +392,16 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_); } - EXPECT_EQ(6, int(publish_packet_.timestamp)); + 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 18 Near scan (scan 1), 10 far scan (scan 1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), - 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) +* @brief 18 Near scan (scan 1), 10 far scan (scan 1), 3 Near (scan 2), + 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), + 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) */ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { @@ -428,7 +423,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) msg_0->event_id = 3; msg_0->timestamp = 5; - msg_detection_0.range = 80.0; + msg_detection_0.range = 80.0; msg_0->detections.push_back(msg_detection_0); @@ -436,7 +431,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) msg_1->event_id = 1; msg_1->timestamp = 6; - msg_detection_1.range = 120.0; + msg_detection_1.range = 120.0; msg_1->detections.push_back(msg_detection_1); @@ -446,7 +441,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) } // Scan 1 data shouldn't be published at this point - EXPECT_EQ(0, int(publish_packet_1.timestamp)); + 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++) @@ -454,17 +449,15 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); } - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 1 \n "); - // Scan 1 data shouldn't be published at this point - EXPECT_EQ(0, int(publish_packet_1.timestamp)); + 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_detection_0.range = 150.0; msg_2->detections.push_back(msg_detection_0); @@ -472,36 +465,32 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_2); } - EXPECT_EQ(0, int(publish_packet_2.timestamp)); + EXPECT_EQ(0, static_cast(publish_packet_2.timestamp)); EXPECT_EQ(0, publish_packet_2.detections.size()); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 2 \n "); - for (int packet_size = 0; packet_size < 2; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); } - // Packet 1 should get published here since scan 1 data is full and all scan states should be reset - EXPECT_EQ(6, int(publish_packet_1.timestamp)); + // Packet 1 should get published here as scan 1 data is full and all scan states should be reset + EXPECT_EQ(6, static_cast(publish_packet_1.timestamp)); EXPECT_EQ(30, publish_packet_1.detections.size()); - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"NEW CHECKPOINT 3 \n "); - // Sending data from scan 2 for (int packet_size = 0; packet_size < 15; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_2); } - EXPECT_EQ(0, int(publish_packet_2.timestamp)); + 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_detection_1.range = 210.0; msg_3->detections.push_back(msg_detection_1); @@ -510,7 +499,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet_2); } - EXPECT_EQ(9, int(publish_packet_2.timestamp)); + 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()); @@ -538,7 +527,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) msg_0->event_id = 3; msg_0->timestamp = 5; - msg_detection_0.range = 80.0; + msg_detection_0.range = 80.0; msg_0->detections.push_back(msg_detection_0); @@ -546,7 +535,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) msg_1->event_id = 1; msg_1->timestamp = 6; - msg_detection_1.range = 120.0; + msg_detection_1.range = 120.0; msg_1->detections.push_back(msg_detection_1); @@ -555,7 +544,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet); } - EXPECT_EQ(0, int(publish_packet.timestamp)); + 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++) @@ -564,7 +553,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) } // Special Case data should be discarded at this point - EXPECT_EQ(0, int(publish_packet.timestamp)); + 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 @@ -573,7 +562,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) msg_2->event_id = 3; msg_2->timestamp = 7; - msg_detection_0.range = 160.0; + msg_detection_0.range = 160.0; msg_2->detections.push_back(msg_detection_0); @@ -581,7 +570,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) msg_3->event_id = 1; msg_3->timestamp = 8; - msg_detection_1.range = 200.0; + msg_detection_1.range = 200.0; msg_3->detections.push_back(msg_detection_1); @@ -595,14 +584,15 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet); } - EXPECT_EQ(8, int(publish_packet.timestamp)); + 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 Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), then check if the 7 far is published +* @brief Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), + then check if the 7 far is published */ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) { @@ -623,7 +613,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) msg_0->event_id = 1; msg_0->timestamp = 6; - msg_detection_0.range = 80.0; + msg_detection_0.range = 80.0; msg_0->detections.push_back(msg_detection_0); @@ -631,7 +621,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) msg_1->event_id = 4; msg_1->timestamp = 7; - msg_detection_1.range = 120.0; + msg_detection_1.range = 120.0; msg_1->detections.push_back(msg_detection_1); @@ -639,7 +629,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) msg_2->event_id = 1; msg_2->timestamp = 8; - msg_detection_2.range = 180.0; + msg_detection_2.range = 180.0; msg_2->detections.push_back(msg_detection_2); @@ -649,7 +639,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet); } - EXPECT_EQ(0, int(publish_packet.timestamp)); + EXPECT_EQ(0, static_cast(publish_packet.timestamp)); EXPECT_EQ(0, publish_packet.detections.size()); // Sending near packet data from scan 2 @@ -658,7 +648,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet); } - EXPECT_EQ(0, int(publish_packet.timestamp)); + EXPECT_EQ(0, static_cast(publish_packet.timestamp)); EXPECT_EQ(0, publish_packet.detections.size()); // Sending far packet data from scan 2 @@ -667,23 +657,26 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet); } - EXPECT_EQ(8, int(publish_packet.timestamp)); + EXPECT_EQ(8, static_cast(publish_packet.timestamp)); EXPECT_EQ(30, publish_packet.detections.size()); } // Unit test cases (Common Scan Filter) -// 1. Send 18 near scan packets and check if it returns a complete 18 packet ready to be published (done) -// 2. Send 12 far scan packets and check if it returns a complete 12 packet ready to be published (done) -// 3. Send 18 that are the same (different timestamp), and 18 that are different (different timestamp) (done) -// 4. Send incomplete packets (5 that are the same) (1 that is different in timestamp) (done) -// 5. Send 19 near scan packets and see if it behaves correctly (is not required) (done) -// 6. Send far scan packets when in near scan mode, does it filter correctly (done) -// 7. Check if it filters out all the packets and returns an empty packet (pass thresholds into pointfilter) (done) +// 1. Send 18 near scan packets and check if it returns a complete 18 packet ready to be published +// 2. Send 12 far scan packets and check if it returns a complete 12 packet ready to be published +// 3. Send 18 that are the same (timestamp 1), and 18 that are different (timestamp 2) +// 4. Send incomplete packets (5 that are the same) (1 that is different in timestamp) +// 5. Send far scan packets when in near scan mode, does it filter correctly +// 6. Check if it filters out all the packets and returns an empty packet +// (pass thresholds into pointfilter) // Unit Test Cases (Double Buffer Algorithm) -// 1. Send 18 Near Scan Packets and 12 Far Scan Packets (done) -// (It shouldn't publish after 18 packets. It should publish together when there is also complete 12 far scan packets) -// 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) (done) -// 3. Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (Scan 2), Send 1 Far (Scan 2)then check if the 7 far is published -// 4. Send 5 Near (scan 1), Send 12 far (scan 1) (done) \ No newline at end of file +// 1. Send 18 Near Scan Packets and 12 Far Scan Packets +// (It shouldn't publish after 18 packets. It should publish together +// when there is also complete 12 far scan packets) +// 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1), +// (SCAN 1 SHOULD BE PUBLISHED) 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) +// 3. Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (Scan 2), +// Send 1 Far (Scan 2)then check if the 7 far is published +// 4. Send 5 Near (scan 1), Send 12 far (scan 1) From e0c57fe0f94e4de9e41a4bf4ea8fbe0085af2eb7 Mon Sep 17 00:00:00 2001 From: Rijin Date: Fri, 17 Mar 2023 03:10:14 +0000 Subject: [PATCH 42/51] updated node callbacks and structure --- .../src/ars_pointcloud_filter.cpp | 2 +- .../src/ars_pointcloud_filter_node.cpp | 43 ++++--------------- 2 files changed, 9 insertions(+), 36 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index ed08aed5..86f6c200 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -159,7 +159,7 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: // Special Case (If the scans start in the middle) if(scan->packet_count_!= scan_capacity) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d ! Packet Discarded.\n ", scan->packet_count_); + 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; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index d0a0d244..5591c56f 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -24,13 +24,11 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter 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(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, - this, std::placeholders::_1)); + raw_left_sub_ = this->create_subscription("unfilteredRadarLeft", 1, + std::bind(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); - raw_right_sub_ = this->create_subscription("unfilteredRadarRight", - 1, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, - this, std::placeholders::_1)); + raw_right_sub_ = this->create_subscription("unfilteredRadarRight", 1, + std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); left_right_pub_ = this->create_publisher("processed", 20); } @@ -41,27 +39,11 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); /** - * Scan Mode == Near - * @brief Send incoming scan message to Near Scan filter and append detections to buffer packet based on timestamp. + * Scan Mode = Near or Scan Mode = Far + * @brief Send incoming scan message to common Scan filter and append detections to buffer packet based on timestamp. Check if packet is ready to be published. Return true if ready, return false otherwise. */ - if(parameters.scan_mode == "near") - { - radar_msgs::msg::RadarPacket publish_packet; - - if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) - { - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); - left_right_pub_->publish(publish_packet); - } - } - - /** - * Scan Mode == Far - * @brief Send incoming scan message to Far Scan filter and append detections to buffer packet based on timestamp. - Check if packet is ready to be published. Return true if ready, return false otherwise. - */ - else if(parameters.scan_mode == "far") + if(parameters.scan_mode == "near" || parameters.scan_mode == "far") { radar_msgs::msg::RadarPacket publish_packet; @@ -95,7 +77,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( { RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - if(parameters.scan_mode == "near") + if(parameters.scan_mode == "near" || parameters.scan_mode == "far") { radar_msgs::msg::RadarPacket publish_packet; @@ -105,16 +87,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( left_right_pub_->publish(publish_packet); } } - else if(parameters.scan_mode == "far") - { - radar_msgs::msg::RadarPacket publish_packet; - if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) - { - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); - left_right_pub_->publish(publish_packet); - } - } else { radar_msgs::msg::RadarPacket publish_packet_near_far; From 6613e889ff3ebf576f2a744c25a9a983b24acc5c Mon Sep 17 00:00:00 2001 From: Rijin Date: Sat, 18 Mar 2023 18:46:40 +0000 Subject: [PATCH 43/51] updated code structure --- .../include/ars_pointcloud_filter.hpp | 25 ++++++++++++---- .../include/ars_pointcloud_filter_node.hpp | 25 +++++++--------- .../src/ars_pointcloud_filter_node.cpp | 30 +++++++++++-------- 3 files changed, 48 insertions(+), 32 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index a0228be8..6e16091b 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -10,6 +10,10 @@ namespace filtering { +/** +* @brief Implementation of the internal logic used by the ARSPointCloudFilter Node to filter and publish +* incoming radar packets. +*/ class ARSPointCloudFilter { public: @@ -32,22 +36,34 @@ class ARSPointCloudFilter FAR }; + /** + * @brief A common filter for near and far scan modes + */ bool common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + /** + * @brief Near + Far Scan Filter Implementation (Double Buffer Algorithm) + */ bool near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); + /** + * @brief Checks Event ID and returns which scan it is (NEAR OR FAR) + */ scan_type check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars); + /** + * @brief Resets all scan states + */ void reset_scan_states(); private: /** - * @brief Pointfilter() filters an incoming radar message based on the parameters passed + * @brief Pointfilter() filters an incoming radar packet based on set thresholds */ radar_msgs::msg::RadarPacket point_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, @@ -59,7 +75,7 @@ class ARSPointCloudFilter double rcs_threshold); /** - * @brief Variables below are used for all the filters + * @brief Variables below are used for all the filters (Common scan filter & NearFarScan Filter) */ unsigned int default_timestamp_; @@ -71,15 +87,14 @@ class ARSPointCloudFilter } scan_params; /** - * @brief Near & Far Scan Filter (Common Scan Filter) + * @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 Near Far Scan Filter (Double Buffer Algorithm) + * @brief Variables only used for Near Far Scan Filter (Double Buffer Algorithm) */ int buffer_index_; std::array near_far_buffer_packets_; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 91abff5d..de822b40 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -9,48 +9,45 @@ #include "ars_pointcloud_filter.hpp" /** -* Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar -* topics from ARS ROSbags and publishes to "processed" radar topic. +* @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 ARSPointCloudFilterNode : public rclcpp::Node { public: - /** - * PointCloudFilter Node Constructor. - */ - ARSPointCloudFilterNode(); + ARSPointCloudFilterNode(); filtering::ARSPointCloudFilter::filter_parameters parameters; private: /** - * A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfiltered" + * A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfilteredRadarRight" * topic * - * @param msg a raw message from the "unfiltered" topic + * @param msg a raw message from the "unfilteredRadarRight" topic */ void unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg); /** - * A ROS2 subscription node callback used to unpack raw radar data from the "unfiltered" + * A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfilteredRadarLeft" * topic * - * @param msg a raw message from the "unfiltered" topic + * @param msg a raw message from the "unfilteredRadarLeft" topic */ void unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg); - // ROS2 Subscriber listening to the unfiltered radar packet topic (left sensor) + // ROS2 Subscriber listening to "unfilteredRadarLeft" topic rclcpp::Subscription::SharedPtr raw_left_sub_; - // ROS2 Subscriber listening to the unfiltered radar packet topic (right sensor) + // 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. + // ROS2 publisher that sends filtered messages from left and right radar to the "processed" topic. rclcpp::Publisher::SharedPtr left_right_pub_; - // Object containing methods for near and far scan filters + // An object containing methods for near and far scan filters filtering::ARSPointCloudFilter pointcloudfilter_; }; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 5591c56f..ade43d93 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -6,7 +6,9 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") { - // Default values are already declared in ars_radar_params.yaml + /** + * @note Default values are already declared in yaml file + */ this->declare_parameter("filter_mode"); this->declare_parameter("scan_mode"); this->declare_parameter("vrel_rad"); @@ -24,10 +26,10 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter 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, + raw_left_sub_ = this->create_subscription("unfilteredRadarLeft", 1, std::bind(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); - raw_right_sub_ = this->create_subscription("unfilteredRadarRight", 1, + raw_right_sub_ = this->create_subscription("unfilteredRadarRight", 1, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); left_right_pub_ = this->create_publisher("processed", 20); @@ -39,15 +41,15 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); /** - * Scan Mode = Near or Scan Mode = Far - * @brief Send incoming scan message to common Scan filter and append detections to buffer packet based on timestamp. - Check if packet is ready to be published. Return true if ready, return false otherwise. + * @brief When scan mode = near or far, + * send incoming unfiltered msgs to common scan filter and append filtered detections to buffer packet + * based on timestamp. Publish packet if 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) == true) + if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); @@ -55,10 +57,9 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( } /** - * Scan Mode == NearFar - * @brief Send incoming scan message to NearFar Scan filter and append detections - * to buffer packet based on double buffer algorithm. Check if packet is ready to be published. - * Return true if ready, return false otherwise. + * @brief When scan mode = nearfar, + * send incoming unfiltered msgs to NearFar Scan filter and append filtered detections + * to buffer packets based on double buffer algorithm. Publish packet if ready to be published. */ else { @@ -72,6 +73,9 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( } } +/** +* @brief Implementation below is the same as the right callback function +*/ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { @@ -81,7 +85,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( { radar_msgs::msg::RadarPacket publish_packet; - if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet) == true) + if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); @@ -92,7 +96,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( { radar_msgs::msg::RadarPacket publish_packet_near_far; - if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far) == true) + if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); left_right_pub_->publish(publish_packet_near_far); From 21041e235af4d8f5803801398b9ff72aad8c2de9 Mon Sep 17 00:00:00 2001 From: Rijin Date: Sat, 18 Mar 2023 19:08:15 +0000 Subject: [PATCH 44/51] updated code structure again --- .../src/ars_pointcloud_filter.cpp | 47 ++++++------------- .../test/ars_pointcloud_filter_test.cpp | 10 ++-- 2 files changed, 21 insertions(+), 36 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 86f6c200..e79ab612 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -37,9 +37,6 @@ ARSPointCloudFilter::ARSPointCloudFilter() } -/** -* @brief Filters packet detections based on set thresholds -*/ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, double AzAng0_threshold, double range_threshold, double vrel_rad_threshold, @@ -77,9 +74,6 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( return filtered_ars; } -/** -* @brief Checks Event ID and returns which scan it is (NEAR OR FAR) -*/ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr msg) { if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) @@ -92,9 +86,6 @@ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_ } } -/** -* @brief Resets all scan states -*/ void ARSPointCloudFilter::reset_scan_states() { near_scan_[buffer_index_].timestamp_ = 0; @@ -106,40 +97,31 @@ void ARSPointCloudFilter::reset_scan_states() far_scan_[buffer_index_].packet_count_= 0; } -/** -* @brief A common filter for near and far scan modes -*/ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { - // Variables to create on every callback (message can be near or far) scan_type incoming_scan_msg = check_scan_type(msg); int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; - // Returns which scan parameters the program is using 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 ((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; } - // Filter out detections based on given thresholds 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 near/far packet is not full if(scan->packet_count_ != scan_capacity && msg->timestamp == scan->timestamp_) { - // Update buffer packet timestamp buffer_packet_.timestamp = scan->timestamp_; - - // Append all detections to buffer buffer_packet_.detections.insert(buffer_packet_.detections.end(), filtered_packet.detections.begin(), filtered_packet.detections.end()); @@ -159,7 +141,8 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: // Special Case (If the scans start in the middle) if(scan->packet_count_!= scan_capacity) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Packet is not full, size: %d! Packet Discarded.\n ", scan->packet_count_); + 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; @@ -169,9 +152,6 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: return false; } -/** -* @brief Near + Far Scan Filter Implementation (Double Buffer Algorithm) -*/ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) @@ -196,29 +176,32 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke // Special case (If you start in the middle with far scans, and no near scans were collected before) if(incoming_scan_msg == FAR && near_scan_[buffer_index_].packet_count_ == 0) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Ignoring far scan packet! \n"); + RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Ignoring 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()); + 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_); + 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()); + 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_++; } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index dc5985d5..2882ea72 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -32,6 +32,8 @@ class ARSPointCloudFilterFixtureTest : public ::testing::Test filtering::ARSPointCloudFilter::filter_parameters parameters; }; +// Common Scan Filter Test + /** * @brief Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR. */ @@ -344,7 +346,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) EXPECT_EQ(200, static_cast(publish_packet.detections[0].snr)); } -// Double Buffer Algorithm Test +// Double Buffer Algorithm (Nearfar scan filter) Test /** * @brief Send 18 Near Scan Packets and 12 Far Scan Packets. @@ -400,8 +402,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) /** * @brief 18 Near scan (scan 1), 10 far scan (scan 1), 3 Near (scan 2), - 2 far (scan 1) (SCAN 1 SHOULD BE PUBLISHED), 15 Near (scan 2), - 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) + 2 far (scan 1) (Scan 1 packet should be published), 15 Near (scan 2), + 12 Far (scan2) (Scan 2 packet should be published) */ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { @@ -676,7 +678,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) // (It shouldn't publish after 18 packets. It should publish together // when there is also complete 12 far scan packets) // 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1), -// (SCAN 1 SHOULD BE PUBLISHED) 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) +// (SCAN 1 SHOULD BE PUBLISHED) 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) // 3. Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (Scan 2), // Send 1 Far (Scan 2)then check if the 7 far is published // 4. Send 5 Near (scan 1), Send 12 far (scan 1) From 759b1f3209cfe6fa2c535efe815f0bf128fc8328 Mon Sep 17 00:00:00 2001 From: Rijin Date: Mon, 20 Mar 2023 03:59:59 +0000 Subject: [PATCH 45/51] updated comments --- .../include/ars_pointcloud_filter.hpp | 17 +++++++++++++---- .../src/ars_pointcloud_filter_node.cpp | 10 +--------- 2 files changed, 14 insertions(+), 13 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 6e16091b..5975d1ae 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -11,8 +11,17 @@ namespace filtering { /** -* @brief Implementation of the internal logic used by the ARSPointCloudFilter Node to filter and publish -* incoming radar packets. +* @brief The ARSPointCloudFilter is responsible for filtering and publishing radar detections +* coming through custom ROS messages/packets. These packets can be composed of near scan +* or far scan detections. +* There are various modes that can be set for the node. +* Mode 1. Recieves, filters and publishes only NEAR scan radar detections (Event ID 3, 4 or 5). +* Mode 2. Recieves, filters and publishes only FAR scan radar detections (Event ID 1, or 2). +* Mode 3. Recieves, filters and publishes BOTH near and far scan radar detections. In this case, +* the implementation below follows the double buffer algorithm which allows the node +* to not only handle incoming packets from different scans but also filter and publish them +* accordingly (Refer to google drawing on the WATO Drive for more info) +* */ class ARSPointCloudFilter { @@ -37,14 +46,14 @@ class ARSPointCloudFilter }; /** - * @brief A common filter for near and far scan modes + * @brief A common filter for near and far scan modes - EXPLAIN */ bool common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet); /** - * @brief Near + Far Scan Filter Implementation (Double Buffer Algorithm) + * @brief Near + Far Scan Filter Implementation (Double Buffer Algorithm) - EXPLAIN */ bool near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, const filter_parameters ¶meters, diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index ade43d93..7cc5b2d3 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -38,8 +38,6 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - /** * @brief When scan mode = near or far, * send incoming unfiltered msgs to common scan filter and append filtered detections to buffer packet @@ -51,7 +49,6 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); } } @@ -67,27 +64,23 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); left_right_pub_->publish(publish_packet_near_far); } } } /** -* @brief Implementation below is the same as the right callback function +* @note Implementation below is the same as the right callback function */ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Subscribing: %d\n", msg->event_id); - if(parameters.scan_mode == "near" || parameters.scan_mode == "far") { radar_msgs::msg::RadarPacket publish_packet; if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet.event_id); left_right_pub_->publish(publish_packet); } } @@ -98,7 +91,6 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { - RCLCPP_INFO(this->get_logger(), "Publishing %d\n", publish_packet_near_far.event_id); left_right_pub_->publish(publish_packet_near_far); } } From c982b1b5616abca9fa44320754eb8c40d61826c9 Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 21 Mar 2023 04:25:34 +0000 Subject: [PATCH 46/51] added linter changes --- .../ARSPointCloudFilter/CMakeLists.txt | 18 +-- .../include/ars_pointcloud_filter.hpp | 94 ++++++++++----- .../include/ars_pointcloud_filter_node.hpp | 3 +- .../ARSPointCloudFilter/package.xml | 4 +- .../src/ars_pointcloud_filter.cpp | 114 ++++++++---------- .../src/ars_pointcloud_filter_node.cpp | 10 +- .../test/ars_pointcloud_filter_test.cpp | 79 ++++++------ 7 files changed, 171 insertions(+), 151 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt index 77d00dc3..7795264d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt +++ b/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt @@ -34,18 +34,18 @@ 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) + 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) + # 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") + # Reinstall cpplint ignoring copyright errors + ament_cpplint(FILTERS "-legal/copyright") - # ament_lint_auto_find_test_dependencies() + ament_lint_auto_find_test_dependencies() # Create test executable from source files ament_add_gtest(ars_pointcloud_filter_test test/ars_pointcloud_filter_test.cpp) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 5975d1ae..1484a687 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -12,22 +12,29 @@ namespace filtering /** * @brief The ARSPointCloudFilter is responsible for filtering and publishing radar detections -* coming through custom ROS messages/packets. These packets can be composed of near scan -* or far scan detections. -* There are various modes that can be set for the node. -* Mode 1. Recieves, filters and publishes only NEAR scan radar detections (Event ID 3, 4 or 5). -* Mode 2. Recieves, filters and publishes only FAR scan radar detections (Event ID 1, or 2). -* Mode 3. Recieves, filters and publishes BOTH near and far scan radar detections. In this case, -* the implementation below follows the double buffer algorithm which allows the node -* to not only handle incoming packets from different scans but also filter and publish them -* accordingly (Refer to google drawing on the WATO Drive for more info) +* coming from the ARS430 radar sensor in the Radar ROS2 pipeline. These incoming +* detections are organized and transformed into custom ROS messages/packets from the +* previous node and later is sent into this node. Note each message/packet consist +* 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 ARSPointCloudFilter { public: ARSPointCloudFilter(); - typedef struct { std::string scan_mode; @@ -39,55 +46,76 @@ class ARSPointCloudFilter double az_ang0_param; } filter_parameters; - enum scan_type + enum scan_type { NEAR, FAR }; /** - * @brief A common filter for near and far scan modes - EXPLAIN + * @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 is 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_ars, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet); + bool common_scan_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet); /** - * @brief Near + Far Scan Filter Implementation (Double Buffer Algorithm) - EXPLAIN + * @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. + * Explain more. Please refer to the figure in the WATO drive for more information + * on the algorithm. + * + * Main special sases 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_ars, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet); - + bool near_far_scan_filter( + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet); /** - * @brief Checks Event ID and returns which scan it is (NEAR OR FAR) + * @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_ars); /** - * @brief Resets all scan states + * @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_ars, - double snr_threshold, - double AzAng0_threshold, - double range_threshold, - double vrel_rad_threshold, - double el_ang_threshold, - double rcs_threshold); + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + 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) */ unsigned int default_timestamp_; - typedef struct { unsigned int timestamp_; @@ -109,9 +137,9 @@ class ARSPointCloudFilter std::array near_far_buffer_packets_; std::array near_scan_; std::array far_scan_; - }; } // namespace filtering -#endif // ARS_POINTCLOUD_FILTER_HPP_ \ No newline at end of file + +#endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index de822b40..9c0434c3 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -15,7 +15,6 @@ class ARSPointCloudFilterNode : public rclcpp::Node { public: - ARSPointCloudFilterNode(); filtering::ARSPointCloudFilter::filter_parameters parameters; @@ -44,7 +43,7 @@ class ARSPointCloudFilterNode : public rclcpp::Node // 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. + // 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 diff --git a/src/sensor_interfacing/ARSPointCloudFilter/package.xml b/src/sensor_interfacing/ARSPointCloudFilter/package.xml index c02effcb..3f55f068 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/package.xml +++ b/src/sensor_interfacing/ARSPointCloudFilter/package.xml @@ -12,8 +12,8 @@ rclcpp radar_msgs - + ament_lint_auto + ament_lint_common ament_cmake_gtest diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index e79ab612..23ed6817 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -9,64 +9,56 @@ namespace filtering { ARSPointCloudFilter::ARSPointCloudFilter() -{ +{ near_scan_single_.timestamp_ = 0; - near_scan_single_.packet_count_= 0; + near_scan_single_.packet_count_ = 0; far_scan_single_.timestamp_ = 0; - far_scan_single_.packet_count_= 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_[0].packet_count_ = 0; near_scan_[1].timestamp_ = 0; near_scan_[1].publish_status_ = false; - near_scan_[1].packet_count_= 0; + 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_[0].packet_count_ = 0; far_scan_[1].timestamp_ = 0; far_scan_[1].publish_status_ = false; - far_scan_[1].packet_count_= 0; + far_scan_[1].packet_count_ = 0; buffer_index_ = 0; default_timestamp_ = 0; - } radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, - double AzAng0_threshold, double range_threshold, double vrel_rad_threshold, - double el_ang_threshold, double rcs_threshold) + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, 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_ars; - for (auto detection : unfiltered_ars->detections) - { - if(detection.snr < snr_threshold) - { + for (auto detection : unfiltered_ars->detections){ + if(detection.snr < snr_threshold) { continue; } - if (detection.az_ang0 < AzAng0_threshold) - { + if (detection.az_ang0 < AzAng0_threshold) { continue; } - if (detection.range < range_threshold) - { + if (detection.range < range_threshold) { continue; } - if (detection.vrel_rad < vrel_rad_threshold) - { + if (detection.vrel_rad < vrel_rad_threshold) { continue; } - if (detection.el_ang < el_ang_threshold) - { + if (detection.el_ang < el_ang_threshold) { continue; } - if (detection.rcs0 < rcs_threshold) - { + if (detection.rcs0 < rcs_threshold) { continue; } filtered_ars.detections.push_back(detection); @@ -74,14 +66,13 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( return filtered_ars; } -ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr msg) +ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type( + const radar_msgs::msg::RadarPacket::SharedPtr msg) { - if (msg->event_id == 3 || msg->event_id == 4 || msg->event_id == 5) - { + 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) - { + else if (msg->event_id == 1 || msg->event_id == 2) { return FAR; } } @@ -90,16 +81,17 @@ void ARSPointCloudFilter::reset_scan_states() { near_scan_[buffer_index_].timestamp_ = 0; near_scan_[buffer_index_].publish_status_ = false; - near_scan_[buffer_index_].packet_count_= 0; + 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; + far_scan_[buffer_index_].packet_count_ = 0; } -bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet) +bool ARSPointCloudFilter::common_scan_filter( + const radar_msgs::msg::RadarPacket::SharedPtr msg, + const filter_parameters ¶meters, + radar_msgs::msg::RadarPacket &publish_packet) { scan_type incoming_scan_msg = check_scan_type(msg); @@ -108,13 +100,12 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: 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")) + (incoming_scan_msg == FAR && parameters.scan_mode == "far")) { - if(scan->timestamp_ == default_timestamp_) - { + 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); @@ -122,13 +113,13 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: 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()); + 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) - { + if(scan->packet_count_ == scan_capacity) { publish_packet = buffer_packet_; buffer_packet_.detections.clear(); scan->packet_count_ = 0; @@ -138,11 +129,11 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: } else { - // Special Case (If the scans start in the middle) + // 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_); + 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; @@ -152,7 +143,7 @@ bool ARSPointCloudFilter::common_scan_filter(const radar_msgs::msg::RadarPacket: return false; } -bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, +bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters ¶meters, radar_msgs::msg::RadarPacket &publish_packet) { @@ -173,35 +164,34 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke if(scan[buffer_index_].timestamp_ == msg->timestamp) { - // Special case (If you start in the middle with far scans, and no near scans were collected before) + // Special case if(incoming_scan_msg == FAR && near_scan_[buffer_index_].packet_count_ == 0) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Ignoring far scan packet. \n"); + 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()); + 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", + 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()); + near_far_buffer_packets_[1 - buffer_index_].detections.end(), + filtered_packet.detections.begin(), + filtered_packet.detections.end()); scan[1 - buffer_index_].packet_count_++; } @@ -210,13 +200,13 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke scan[buffer_index_].publish_status_ = true; } - // If true, packet is done collecting data from that particular scan (whether its the full 30 or not) if(far_scan_[buffer_index_].publish_status_ == true) { - // If the packet started in the middle, and it does not have all 30 detections (Special Case) - if(near_scan_[buffer_index_].packet_count_ != 18 || far_scan_[buffer_index_].packet_count_ != 12) + // Special Case + if(near_scan_[buffer_index_].packet_count_ != 18 || + far_scan_[buffer_index_].packet_count_ != 12) { - RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"Incomplete total packet count! Not 30.\n"); + 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_; diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 7cc5b2d3..6c74d145 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -26,11 +26,13 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter 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(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, this, std::placeholders::_1)); + raw_left_sub_ = this->create_subscription("unfilteredRadarLeft", + 1, std::bind(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, + this, std::placeholders::_1)); - raw_right_sub_ = this->create_subscription("unfilteredRadarRight", 1, - std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, this, std::placeholders::_1)); + raw_right_sub_ = this->create_subscription("unfilteredRadarRight", + 1, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, + this, std::placeholders::_1)); left_right_pub_ = this->create_publisher("processed", 20); } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 2882ea72..5e64cbbb 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -61,13 +61,12 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) /** * @brief Sends 18 near scan packets with the same timestamp and checks if it returns a - complete packet ready to be published. +* complete packet ready to be published. */ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) { SetUp("near"); - // Near scan message auto msg = std::make_shared(); radar_msgs::msg::RadarDetection msg_detection; @@ -87,6 +86,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) } EXPECT_EQ(2, static_cast(publish_packet.timestamp)); + for(int index = 0; index < 18; index++) { EXPECT_EQ(100, publish_packet.detections[index].snr); @@ -95,7 +95,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) /** * @brief Sends 12 far scan packets with the same timestamp and checks if it returns a - complete packet ready to be published. +* complete packet ready to be published. */ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) { @@ -121,6 +121,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) } EXPECT_EQ(7, static_cast(publish_packet.timestamp)); + for(int index = 0; index < 12; index++) { EXPECT_EQ(5, publish_packet.detections[index].rcs0); @@ -128,8 +129,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) } /** -* @brief Sends 18 packets that are the same (timestamp 1), and 18 packets with a new timestamp. - Checks if it returns a complete packet ready to be published. +* @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(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) { @@ -166,6 +167,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) } 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); @@ -178,6 +180,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) } 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); @@ -197,7 +200,6 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) auto msg_1 = std::make_shared(); radar_msgs::msg::RadarDetection msg_detection; - radar_msgs::msg::RadarPacket publish_packet; // Packet Data (Scan 1) @@ -233,6 +235,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) // 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); @@ -240,21 +243,21 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) } /** -* @brief Sends far scan packets when in near scan mode. Checks if it filters correctly. +* @brief Sends far scan packets when in near scan mode and checks if it filters correctly. */ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) { SetUp("near"); - // Near Scan Messages + // 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; - // Packet Data (Scan 1) msg_0->event_id = 3; msg_0->timestamp = 7; @@ -262,7 +265,6 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) msg_0->detections.push_back(msg_detection); - // Packet Data (Scan 2) msg_1->event_id = 1; msg_1->timestamp = 3; @@ -270,12 +272,12 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) 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++) @@ -285,8 +287,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) } /** -* @brief Checks if a packet with several detections is filtered out and if the same packet is - returned with the correct unfiltered detection. +* @brief Checks if a packet with several detections is filtered out correctly. */ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) { @@ -299,34 +300,33 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) radar_msgs::msg::RadarDetection msg_detection_1; radar_msgs::msg::RadarDetection msg_detection_2; - // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet; - // Packet Data (Scan 1) + // Packet Data msg->event_id = 3; msg->timestamp = 7; - // Sending message with no detections + // Sending messages with no detections for(int packet_size = 0; packet_size < 17; packet_size++) { pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } - // msg snr < snr threshold + // 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 parameters + // 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); - // msg range < range threshold & msg snr < snr threshold + // 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; @@ -337,9 +337,10 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) { pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } + EXPECT_EQ(7, static_cast(publish_packet.timestamp)); - // Returns a published packet with one detection going through + // 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)); @@ -349,7 +350,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) // Double Buffer Algorithm (Nearfar scan filter) Test /** -* @brief Send 18 Near Scan Packets and 12 Far Scan Packets. +* @brief Sends 18 Near Scan Packets and 12 Far Scan Packets. Checks if it returns a +* complete packet ready to be published. */ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) { @@ -361,10 +363,9 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) radar_msgs::msg::RadarDetection msg_detection_0; radar_msgs::msg::RadarDetection msg_detection_1; - // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet_; - // Near Packet Data (Scan 1) + // Near Packet Data msg_0->event_id = 3; msg_0->timestamp = 5; @@ -372,7 +373,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) msg_0->detections.push_back(msg_detection_0); - // Far Packet Data (Scan 1) + // Far Packet Data msg_1->event_id = 1; msg_1->timestamp = 6; @@ -385,7 +386,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_); } - // Shouldn't be published at this point + // Packet shouldn't be published at this point EXPECT_EQ(0, static_cast(publish_packet_.timestamp)); EXPECT_EQ(0, publish_packet_.detections.size()); @@ -401,9 +402,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) } /** -* @brief 18 Near scan (scan 1), 10 far scan (scan 1), 3 Near (scan 2), - 2 far (scan 1) (Scan 1 packet should be published), 15 Near (scan 2), - 12 Far (scan2) (Scan 2 packet should be published) +* @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(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { @@ -417,7 +417,6 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) radar_msgs::msg::RadarDetection msg_detection_0; radar_msgs::msg::RadarDetection msg_detection_1; - // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet_1; radar_msgs::msg::RadarPacket publish_packet_2; @@ -467,6 +466,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { 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()); @@ -475,14 +476,14 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); } - // Packet 1 should get published here as scan 1 data is full and all scan states should be reset + // 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 data from scan 2 + // 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_0, parameters, publish_packet_2); + pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_2); } EXPECT_EQ(0, static_cast(publish_packet_2.timestamp)); @@ -508,7 +509,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) } /** -* @brief Send 5 Near (scan 1), Send 12 far (scan 1) +* @brief Sends 5 Near (scan 1), Send 12 far (scan 1) and checks if it returns the correct packet. +* (Special Case) */ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) { @@ -522,10 +524,9 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) radar_msgs::msg::RadarDetection msg_detection_0; radar_msgs::msg::RadarDetection msg_detection_1; - // Fake Publish packets radar_msgs::msg::RadarPacket publish_packet; - // Near Packet Data (Scan 1) + // Near Packet Data (Scan 1) msg_0->event_id = 3; msg_0->timestamp = 5; @@ -593,8 +594,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) } /** -* @brief Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (scan 2), - then check if the 7 far is published +* @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(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) { @@ -608,7 +609,6 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) radar_msgs::msg::RadarDetection msg_detection_1; radar_msgs::msg::RadarDetection msg_detection_2; - // Fake Publish Packet radar_msgs::msg::RadarPacket publish_packet; // Far Packet Data (Scan 1) @@ -641,6 +641,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) 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()); From 7b4672e471f46a9c6a1986ffd7055d1a7f1334e2 Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 22 Mar 2023 00:10:12 +0000 Subject: [PATCH 47/51] all linter suggestions are added and tests are passing --- .../include/ars_pointcloud_filter.hpp | 53 ++++---- .../include/ars_pointcloud_filter_node.hpp | 6 +- .../src/ars_pointcloud_filter.cpp | 93 +++++++------- .../src/ars_pointcloud_filter_node.cpp | 67 +++++----- .../test/ars_pointcloud_filter_test.cpp | 115 +++++++----------- 5 files changed, 142 insertions(+), 192 deletions(-) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp index 1484a687..88c93b1d 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp @@ -11,11 +11,11 @@ namespace filtering { /** -* @brief The ARSPointCloudFilter is responsible for filtering and publishing radar detections -* coming from the ARS430 radar sensor in the Radar ROS2 pipeline. These incoming -* detections are organized and transformed into custom ROS messages/packets from the -* previous node and later is sent into this node. Note each message/packet consist -* detections of two types. These detections can be from a near scan or a far scan. +* @brief The ARSPointCloudFilter 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". * @@ -25,11 +25,10 @@ namespace filtering * 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 +* 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 ARSPointCloudFilter { @@ -56,39 +55,36 @@ class ARSPointCloudFilter * @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 is 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. - * + * 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_ars, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet); + 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. - * Explain more. Please refer to the figure in the WATO drive for more information + * 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. - * - * Main special sases to consider: + * 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 + * 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. + * 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_ars, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet); + 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) */ @@ -139,7 +135,6 @@ class ARSPointCloudFilter std::array far_scan_; }; -} // namespace filtering - +} // namespace filtering #endif // ARS_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index 9c0434c3..ea9bfb8e 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -9,7 +9,7 @@ #include "ars_pointcloud_filter.hpp" /** -* @brief Implementation of a ROS2 Point Cloud Filter node that listens to "unfilteredRadarLeft" +* @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 ARSPointCloudFilterNode : public rclcpp::Node @@ -21,7 +21,7 @@ class ARSPointCloudFilterNode : public rclcpp::Node private: /** * A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfilteredRadarRight" - * topic + * topic * * @param msg a raw message from the "unfilteredRadarRight" topic */ @@ -30,7 +30,7 @@ class ARSPointCloudFilterNode : public rclcpp::Node /** * A ROS2 subscription node callback used to unpack raw ARS radar data from the "unfilteredRadarLeft" - * topic + * topic * * @param msg a raw message from the "unfilteredRadarLeft" topic */ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp index 23ed6817..56659542 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp @@ -42,8 +42,8 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( double el_ang_threshold, double rcs_threshold) { radar_msgs::msg::RadarPacket filtered_ars; - for (auto detection : unfiltered_ars->detections){ - if(detection.snr < snr_threshold) { + for (auto detection : unfiltered_ars->detections) { + if (detection.snr < snr_threshold) { continue; } if (detection.az_ang0 < AzAng0_threshold) { @@ -71,8 +71,7 @@ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type( { 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) { + } else if (msg->event_id == 1 || msg->event_id == 2) { return FAR; } } @@ -90,8 +89,8 @@ void ARSPointCloudFilter::reset_scan_states() bool ARSPointCloudFilter::common_scan_filter( const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet) + const filter_parameters & parameters, + radar_msgs::msg::RadarPacket & publish_packet) { scan_type incoming_scan_msg = check_scan_type(msg); @@ -102,16 +101,15 @@ bool ARSPointCloudFilter::common_scan_filter( if ((incoming_scan_msg == NEAR && parameters.scan_mode == "near") || (incoming_scan_msg == FAR && parameters.scan_mode == "far")) { - if(scan->timestamp_ == default_timestamp_) { + 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); + 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_) - { + if (scan->packet_count_ != scan_capacity && msg->timestamp == scan->timestamp_) { buffer_packet_.timestamp = scan->timestamp_; buffer_packet_.detections.insert( buffer_packet_.detections.end(), @@ -119,21 +117,19 @@ bool ARSPointCloudFilter::common_scan_filter( filtered_packet.detections.end()); scan->packet_count_++; - if(scan->packet_count_ == scan_capacity) { + 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 - { + } 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_); + 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; @@ -143,9 +139,10 @@ bool ARSPointCloudFilter::common_scan_filter( return false; } -bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacket::SharedPtr msg, - const filter_parameters ¶meters, - radar_msgs::msg::RadarPacket &publish_packet) +bool ARSPointCloudFilter::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); @@ -154,57 +151,55 @@ bool ARSPointCloudFilter::near_far_scan_filter(const radar_msgs::msg::RadarPacke 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); + 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_) - { + if (scan[buffer_index_].timestamp_ == default_timestamp_) { scan[buffer_index_].timestamp_ = msg->timestamp; } - if(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) - { + 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()); + 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_); + } 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()); + 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) - { + if (scan[buffer_index_].packet_count_ == scan_capacity) { scan[buffer_index_].publish_status_ = true; } - if(far_scan_[buffer_index_].publish_status_ == true) - { + if (far_scan_[buffer_index_].publish_status_ == true) { // Special Case - if(near_scan_[buffer_index_].packet_count_ != 18 || - far_scan_[buffer_index_].packet_count_ != 12) + if (near_scan_[buffer_index_].packet_count_ != 18 || + far_scan_[buffer_index_].packet_count_ != 12) { RCLCPP_WARN(rclcpp::get_logger("rclcpp"), "Incomplete total packet count! Not 30.\n"); near_far_buffer_packets_[buffer_index_].detections.clear(); diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp index 6c74d145..9389d8ed 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp @@ -4,7 +4,8 @@ #include "ars_pointcloud_filter_node.hpp" #include "ars_pointcloud_filter.hpp" -ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter") +ARSPointCloudFilterNode::ARSPointCloudFilterNode() +: Node("ars_point_cloud_filter") { /** * @note Default values are already declared in yaml file @@ -26,13 +27,17 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode(): Node("ars_point_cloud_filter 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(&ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, - this, std::placeholders::_1)); + raw_left_sub_ = this->create_subscription( + "unfilteredRadarLeft", + 1, std::bind( + &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, + this, std::placeholders::_1)); - raw_right_sub_ = this->create_subscription("unfilteredRadarRight", - 1, std::bind( &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, - this, std::placeholders::_1)); + raw_right_sub_ = this->create_subscription( + "unfilteredRadarRight", + 1, std::bind( + &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, + this, std::placeholders::_1)); left_right_pub_ = this->create_publisher("processed", 20); } @@ -41,31 +46,21 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { /** - * @brief When scan mode = near or far, - * send incoming unfiltered msgs to common scan filter and append filtered detections to buffer packet - * based on timestamp. Publish packet if ready to be published. + * @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") - { + if (parameters.scan_mode == "near" || parameters.scan_mode == "far") { radar_msgs::msg::RadarPacket publish_packet; - if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) - { + if (pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { left_right_pub_->publish(publish_packet); } - } - - /** - * @brief When scan mode = nearfar, - * send incoming unfiltered msgs to NearFar Scan filter and append filtered detections - * to buffer packets based on double buffer algorithm. Publish packet if ready to be published. - */ - else - { + } else { radar_msgs::msg::RadarPacket publish_packet_near_far; - if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) - { + if (pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { left_right_pub_->publish(publish_packet_near_far); } } @@ -77,22 +72,16 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { - if(parameters.scan_mode == "near" || parameters.scan_mode == "far") - { + if (parameters.scan_mode == "near" || parameters.scan_mode == "far") { radar_msgs::msg::RadarPacket publish_packet; - if(pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) - { + if (pointcloudfilter_.common_scan_filter(msg, parameters, publish_packet)) { left_right_pub_->publish(publish_packet); } - } - - else - { + } else { radar_msgs::msg::RadarPacket publish_packet_near_far; - if(pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) - { + if (pointcloudfilter_.near_far_scan_filter(msg, parameters, publish_packet_near_far)) { left_right_pub_->publish(publish_packet_near_far); } } @@ -100,8 +89,8 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( int main(int argc, char ** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp index 5e64cbbb..eb2b0004 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp @@ -13,10 +13,11 @@ class ARSPointCloudFilterFixtureTest : 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) + 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; @@ -80,15 +81,13 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) msg->detections.push_back(msg_detection); - for (int packet_size = 0; packet_size < 18; packet_size++) - { + 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++) - { + for (int index = 0; index < 18; index++) { EXPECT_EQ(100, publish_packet.detections[index].snr); } } @@ -115,21 +114,19 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) msg->detections.push_back(msg_detection); - for (int packet_size = 0; packet_size < 12; packet_size++) - { + 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++) - { + 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 +* @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(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) @@ -161,34 +158,30 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) msg_1->detections.push_back(msg_detection); // Scan 1 - for (int packet_size = 0; packet_size < 18; packet_size++) - { + 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++) - { + 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++) - { + 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++) - { + 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 +* @brief Sends incomplete packets (5 that are the same and 1 packet after that is different in timestamp). */ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) @@ -219,8 +212,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) msg_1->detections.push_back(msg_detection); // Incomplete packet of near scans - for (int packet_size = 0; packet_size < 5; packet_size++) - { + for (int packet_size = 0; packet_size < 5; packet_size++) { pointcloudfilter.common_scan_filter(msg_0, parameters, publish_packet); } @@ -228,22 +220,20 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) 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++) - { + 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++) - { + 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. +* @brief Sends far scan packets when in near scan mode and checks if it filters correctly. */ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) { @@ -272,16 +262,14 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) msg_1->detections.push_back(msg_detection); - for (int packet_size = 0; packet_size < 18; packet_size++) - { + 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++) - { + for (int index = 0; index < 18; index++) { EXPECT_EQ(120, publish_packet.detections[index].range); } } @@ -307,8 +295,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) msg->timestamp = 7; // Sending messages with no detections - for(int packet_size = 0; packet_size < 17; packet_size++) - { + for (int packet_size = 0; packet_size < 17; packet_size++) { pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } @@ -333,8 +320,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) msg->detections.push_back(msg_detection_2); - for (int packet_size = 0; packet_size < 1; packet_size++) - { + for (int packet_size = 0; packet_size < 1; packet_size++) { pointcloudfilter.common_scan_filter(msg, parameters, publish_packet); } @@ -350,7 +336,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) // Double Buffer Algorithm (Nearfar scan filter) Test /** -* @brief Sends 18 Near Scan Packets and 12 Far Scan Packets. Checks if it returns a +* @brief Sends 18 Near Scan Packets and 12 Far Scan Packets. Checks if it returns a * complete packet ready to be published. */ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) @@ -381,8 +367,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) msg_1->detections.push_back(msg_detection_1); - for (int packet_size = 0; packet_size < 18; packet_size++) - { + for (int packet_size = 0; packet_size < 18; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_); } @@ -390,8 +375,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) 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++) - { + for (int packet_size = 0; packet_size < 12; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_); } @@ -402,8 +386,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) } /** -* @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) +* @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(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) { @@ -436,8 +420,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) msg_1->detections.push_back(msg_detection_1); - for (int packet_size = 0; packet_size < 18; packet_size++) - { + for (int packet_size = 0; packet_size < 18; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet_1); } @@ -445,8 +428,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) 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++) - { + for (int packet_size = 0; packet_size < 10; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); } @@ -462,8 +444,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) msg_2->detections.push_back(msg_detection_0); - for (int packet_size = 0; packet_size < 3; packet_size++) - { + for (int packet_size = 0; packet_size < 3; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_2); } @@ -471,8 +452,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) 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++) - { + for (int packet_size = 0; packet_size < 2; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet_1); } @@ -481,8 +461,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) 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++) - { + for (int packet_size = 0; packet_size < 15; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet_2); } @@ -497,8 +476,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) msg_3->detections.push_back(msg_detection_1); - for (int packet_size = 0; packet_size < 12; packet_size++) - { + for (int packet_size = 0; packet_size < 12; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet_2); } @@ -542,16 +520,14 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) msg_1->detections.push_back(msg_detection_1); - for (int packet_size = 0; packet_size < 5; packet_size++) - { + 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++) - { + for (int packet_size = 0; packet_size < 12; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet); } @@ -577,13 +553,11 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) msg_3->detections.push_back(msg_detection_1); - for(int packet_size = 0; packet_size < 18; packet_size++) - { + 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++) - { + for (int packet_size = 0; packet_size < 12; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_3, parameters, publish_packet); } @@ -594,8 +568,8 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) } /** -* @brief Sends 7 far (scan 1), Send 1 Near (scan 2), - then checks if the 7 far (scan 1) is discarded (Special case) +* @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(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) { @@ -636,8 +610,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) 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++) - { + for (int packet_size = 0; packet_size < 7; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_0, parameters, publish_packet); } @@ -646,8 +619,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) EXPECT_EQ(0, publish_packet.detections.size()); // Sending near packet data from scan 2 - for (int packet_size = 0; packet_size < 18; packet_size++) - { + for (int packet_size = 0; packet_size < 18; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_1, parameters, publish_packet); } @@ -655,8 +627,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) EXPECT_EQ(0, publish_packet.detections.size()); // Sending far packet data from scan 2 - for (int packet_size = 0; packet_size < 12; packet_size++) - { + for (int packet_size = 0; packet_size < 12; packet_size++) { pointcloudfilter.near_far_scan_filter(msg_2, parameters, publish_packet); } From 029458e4133941b111b71234c517fdd3e32d89f0 Mon Sep 17 00:00:00 2001 From: Rijin Date: Wed, 22 Mar 2023 00:27:55 +0000 Subject: [PATCH 48/51] deleted files for CarlaPointCloudFilternode --- .../include/ars_pointcloud_filter_node.hpp | 2 +- .../CarlaPointCloudFilter/CMakeLists.txt | 0 .../config/carla_radar_params.yaml | 3 - .../include/carla_pointcloud_filter.hpp | 0 .../include/carla_pointcloud_filter_node.hpp | 67 ------------------- .../launch/carla_pointcloud_filter.launch.py | 0 .../CarlaPointCloudFilter/package.xml | 0 .../src/carla_pointcloud_filter.cpp | 0 .../src/carla_pointcloud_filter_node.cpp | 11 --- .../test/carla_pointcloud_filter_test.cpp | 0 10 files changed, 1 insertion(+), 82 deletions(-) delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/CMakeLists.txt delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter.hpp delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp delete mode 100755 src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/package.xml delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter.cpp delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp delete mode 100644 src/sensor_interfacing/CarlaPointCloudFilter/test/carla_pointcloud_filter_test.cpp diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp index ea9bfb8e..fec5e2bd 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp @@ -43,7 +43,7 @@ class ARSPointCloudFilterNode : public rclcpp::Node // 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 + // 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 diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/CarlaPointCloudFilter/CMakeLists.txt deleted file mode 100644 index e69de29b..00000000 diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml b/src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml deleted file mode 100644 index 2803d6a8..00000000 --- a/src/sensor_interfacing/CarlaPointCloudFilter/config/carla_radar_params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -radar_point_cloud_filter: - ros__parameters: - filter_mode: "carla" diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter.hpp b/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter.hpp deleted file mode 100644 index e69de29b..00000000 diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp b/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp deleted file mode 100644 index 549ed239..00000000 --- a/src/sensor_interfacing/CarlaPointCloudFilter/include/carla_pointcloud_filter_node.hpp +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef CARLA_POINTCLOUD_FILTER_NODE_HPP_ -#define CARLA_POINTCLOUD_FILTER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "radar_msgs/msg/UnfilteredRadarLeftPacket.hpp" -#include "radar_msgs/msg/UnfilteredRadarRightPacket.hpp" -#include "radar_msgs/msg/UnfilteredCarlaLeftPacket.hpp" -#include "radar_msgs/msg/UnfilteredCarlaRightPacket.hpp" -#include "radar_msgs/msg/RadarDetection.hpp - -#include "carla_pointcloud_filter.hpp" - - /** - * Implementation of a ROS2 Point Cloud Filter node that listens to "unfiltered" radar - * topics from CARLA ROS Bridge and ROSBags and publishes to "filtered" radar topic. - */ - -class CarlaPointCloudFilterNode : 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; - - /** - * PointCloudFilter Node Constructor. - */ - CarlaPointCloudFilterNode(); - -private: - /** - * A ROS2 subscription node callback used to unpack raw CARLA radar data from - * "unfiltered" topics. - * - * @param msg a processed message from the "unfiltered" topic - */ - void unfiltered_carla_radar_left_callback( - const radar_msgs::msg::UnfilteredCarlaLeft::SharedPtr msg); - - - /** - * A ROS2 subscription node callback used to unpack raw CARLA radar data from - * "unfiltered" topics. - * - * @param msg a processed message from the "unfiltered" topic - */ - void unfiltered_carla_radar_right_callback( - const radar_msgs::msg::UnfilteredCarlaRight::SharedPtr msg); - - - - // ROS2 Subscriber listening to the unfiltered Carla left topic (radar1). - rclcpp::Subscription::SharedPtr raw_carla_left_sub_ - - // ROS2 Subscriber listening to the unfiltered Carla right topic (radar2). - rclcpp::Subscription::SharedPtr raw_carla_right_sub_ - - std::string filter_mode_; - - // // Add an object below from radar_pointcloud_filter.hpp that contains the methods (ADD LATER ONCE LOGIC CREATED) - -}; - - - -#endif // CARLA_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py b/src/sensor_interfacing/CarlaPointCloudFilter/launch/carla_pointcloud_filter.launch.py deleted file mode 100755 index e69de29b..00000000 diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/package.xml b/src/sensor_interfacing/CarlaPointCloudFilter/package.xml deleted file mode 100644 index e69de29b..00000000 diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter.cpp b/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter.cpp deleted file mode 100644 index e69de29b..00000000 diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp b/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp deleted file mode 100644 index 433ee5a1..00000000 --- a/src/sensor_interfacing/CarlaPointCloudFilter/src/carla_pointcloud_filter_node.cpp +++ /dev/null @@ -1,11 +0,0 @@ - raw_carla_left_sub_ = this->create_subscription( - "unfilteredCarlaLeft", ADVERTISING_FREQ, - std::bind( - &CarlaRadarFilter::unfiltered_carla_radar_left_callback, this, - std::placeholders::_1)); - - raw_carla_right_sub_ = this->create_subscription( - "unfilteredCarlaRight", ADVERTISING_FREQ, - std::bind( - &CarlaRadarFilter::unfiltered_carla_radar_right_callback, this, - std::placeholders::_1)); \ No newline at end of file diff --git a/src/sensor_interfacing/CarlaPointCloudFilter/test/carla_pointcloud_filter_test.cpp b/src/sensor_interfacing/CarlaPointCloudFilter/test/carla_pointcloud_filter_test.cpp deleted file mode 100644 index e69de29b..00000000 From 91d281aca3f26400e8323813e2505af9b6b798e9 Mon Sep 17 00:00:00 2001 From: Rijin Date: Thu, 23 Mar 2023 01:10:39 +0000 Subject: [PATCH 49/51] renamed radar to continental --- dev_config.sh | 4 +- .../continental_driver.Dockerfile} | 4 +- profiles/docker-compose.sensors.yaml | 14 +++-- .../include/ars_pointcloud_filter_node.hpp | 53 ------------------ .../CMakeLists.txt | 24 ++++----- .../config/continental_radar_params.yaml} | 4 +- .../continental_pointcloud_filter.hpp} | 21 ++++---- .../continental_pointcloud_filter_node.hpp | 54 +++++++++++++++++++ .../continental_pointcloud_filter.launch.py} | 18 +++---- .../package.xml | 4 +- .../src/continental_pointcloud_filter.cpp} | 26 ++++----- .../continental_pointcloud_filter_node.cpp} | 34 ++++++------ .../continental_pointcloud_filter_test.cpp} | 30 +++++------ 13 files changed, 145 insertions(+), 145 deletions(-) rename docker/{radar_driver/radar_driver.Dockerfile => continental_driver/continental_driver.Dockerfile} (93%) delete mode 100644 src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp rename src/sensor_interfacing/{ARSPointCloudFilter => ContinentalPointCloudFilter}/CMakeLists.txt (71%) rename src/sensor_interfacing/{ARSPointCloudFilter/config/ars_radar_params.yaml => ContinentalPointCloudFilter/config/continental_radar_params.yaml} (72%) rename src/sensor_interfacing/{ARSPointCloudFilter/include/ars_pointcloud_filter.hpp => ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp} (89%) create mode 100644 src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter_node.hpp rename src/sensor_interfacing/{ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py => ContinentalPointCloudFilter/launch/continental_pointcloud_filter.launch.py} (56%) rename src/sensor_interfacing/{ARSPointCloudFilter => ContinentalPointCloudFilter}/package.xml (82%) rename src/sensor_interfacing/{ARSPointCloudFilter/src/ars_pointcloud_filter.cpp => ContinentalPointCloudFilter/src/continental_pointcloud_filter.cpp} (89%) rename src/sensor_interfacing/{ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp => ContinentalPointCloudFilter/src/continental_pointcloud_filter_node.cpp} (72%) rename src/sensor_interfacing/{ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp => ContinentalPointCloudFilter/test/continental_pointcloud_filter_test.cpp} (94%) diff --git a/dev_config.sh b/dev_config.sh index 38e0823f..e3c7e60d 100755 --- a/dev_config.sh +++ b/dev_config.sh @@ -63,7 +63,7 @@ SAMPLES_CPP_PRODUCER_IMAGE=${SAMPLES_CPP_PRODUCER_IMAGE:-"git.uwaterloo.ca:5050/ SAMPLES_CPP_TRANSFORMER_IMAGE=${SAMPLES_CPP_TRANSFORMER_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/samples_cpp_transformer"} # Sensor Interfacing -RADAR_DRIVER_IMAGE=${RADAR_DRIVER_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/radar_driver"} +CONTINENTAL_DRIVER_IMAGE=${CONTINENTAL_DRIVER_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/continental_driver"} ## -------------------------- User ID ----------------------------- @@ -92,7 +92,7 @@ echo "SAMPLES_CPP_PRODUCER_IMAGE=$SAMPLES_CPP_PRODUCER_IMAGE" >> "$PROFILES_DIR/ echo "SAMPLES_CPP_TRANSFORMER_IMAGE=$SAMPLES_CPP_TRANSFORMER_IMAGE" >> "$PROFILES_DIR/.env" -echo "RADAR_DRIVER_IMAGE=$RADAR_DRIVER_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" diff --git a/docker/radar_driver/radar_driver.Dockerfile b/docker/continental_driver/continental_driver.Dockerfile similarity index 93% rename from docker/radar_driver/radar_driver.Dockerfile rename to docker/continental_driver/continental_driver.Dockerfile index 4b22730c..aaa07940 100644 --- a/docker/radar_driver/radar_driver.Dockerfile +++ b/docker/continental_driver/continental_driver.Dockerfile @@ -1,4 +1,4 @@ -FROM ros:foxy AS base +FROM ros:humble AS base RUN apt-get update && apt-get install -y curl && \ rm -rf /var/lib/apt/lists/* @@ -23,7 +23,7 @@ USER docker:docker RUN mkdir -p ~/ament_ws/src WORKDIR /home/docker/ament_ws/src -COPY src/sensor_interfacing/ARSPointCloudFilter ARSPointCloudFilter +COPY src/sensor_interfacing/ContinentalPointCloudFilter ContinentalPointCloudFilter COPY src/wato_msgs/radar_msgs radar_msgs WORKDIR /home/docker/ament_ws diff --git a/profiles/docker-compose.sensors.yaml b/profiles/docker-compose.sensors.yaml index 2cfc1be9..85b0aa58 100644 --- a/profiles/docker-compose.sensors.yaml +++ b/profiles/docker-compose.sensors.yaml @@ -1,14 +1,12 @@ version: "3.8" services: - radar_driver: + continental_driver: build: context: .. - dockerfile: docker/radar_driver/radar_driver.Dockerfile + dockerfile: docker/continental_driver/continental_driver.Dockerfile cache_from: - - "${RADAR_DRIVER_IMAGE:?}:${CACHE_FROM_TAG}" - - "${RADAR_DRIVER_IMAGE:?}:develop" - image: "${RADAR_DRIVER_IMAGE:?}:${TAG}" + - "${CONTINENTAL_DRIVER_IMAGE:?}:${CACHE_FROM_TAG}" + - "${CONTINENTAL_DRIVER_IMAGE:?}:develop" + image: "${CONTINENTAL_DRIVER_IMAGE:?}:${TAG}" volumes: - - ../src/sensor_interfacing/ARSPointCloudFilter:/home/docker/ament_ws/src/ARSPointCloudFilter - - #command: roslaunch radar_driver radarROSbag.launch \ No newline at end of file + - ../src/sensor_interfacing/ContinentalPointCloudFilter:/home/docker/ament_ws/src/ContinentalPointCloudFilter diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp b/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp deleted file mode 100644 index fec5e2bd..00000000 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter_node.hpp +++ /dev/null @@ -1,53 +0,0 @@ -#ifndef ARS_POINTCLOUD_FILTER_NODE_HPP_ -#define ARS_POINTCLOUD_FILTER_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" - -#include "radar_msgs/msg/radar_packet.hpp" -#include "radar_msgs/msg/radar_detection.hpp" - -#include "ars_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 ARSPointCloudFilterNode : public rclcpp::Node -{ -public: - ARSPointCloudFilterNode(); - filtering::ARSPointCloudFilter::filter_parameters parameters; - -private: - /** - * 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_ars_radar_right_callback( - const radar_msgs::msg::RadarPacket::SharedPtr msg); - - /** - * 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_ars_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::ARSPointCloudFilter pointcloudfilter_; -}; - -#endif // ARS_POINTCLOUD_FILTER_NODE_HPP_ diff --git a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/ContinentalPointCloudFilter/CMakeLists.txt similarity index 71% rename from src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt rename to src/sensor_interfacing/ContinentalPointCloudFilter/CMakeLists.txt index 7795264d..5f046e39 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/CMakeLists.txt +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.10) -project(ars_pointcloud_filter) +project(continental_pointcloud_filter) # Set compiler to use C++ 17 standard if(NOT CMAKE_CXX_STANDARD) @@ -20,13 +20,13 @@ find_package(radar_msgs REQUIRED) # Custom package containing ROS2 messages # 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(arspointcloudfilter_lib - src/ars_pointcloud_filter.cpp) +add_library(continentalpointcloudfilter_lib + src/continental_pointcloud_filter.cpp) # Indicate to compiler where to search for header files -target_include_directories(arspointcloudfilter_lib +target_include_directories(continentalpointcloudfilter_lib PUBLIC include) # Add ROS2 dependencies required by package -ament_target_dependencies(arspointcloudfilter_lib rclcpp radar_msgs) +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. @@ -48,24 +48,24 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() # Create test executable from source files - ament_add_gtest(ars_pointcloud_filter_test test/ars_pointcloud_filter_test.cpp) - # Link to the previously built library to access ARSPointCloudFilter classes and methods - target_link_libraries(ars_pointcloud_filter_test arspointcloudfilter_lib) + 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 - ars_pointcloud_filter_test + continental_pointcloud_filter_test DESTINATION lib/${PROJECT_NAME}) endif() # Create ROS2 node executable from source files -add_executable(ars_pointcloud_filter_node src/ars_pointcloud_filter_node.cpp) +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(ars_pointcloud_filter_node arspointcloudfilter_lib) +target_link_libraries(continental_pointcloud_filter_node continentalpointcloudfilter_lib) # Copy executable to installation location install(TARGETS - ars_pointcloud_filter_node + continental_pointcloud_filter_node DESTINATION lib/${PROJECT_NAME}) # Copy launch files to installation location diff --git a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml b/src/sensor_interfacing/ContinentalPointCloudFilter/config/continental_radar_params.yaml similarity index 72% rename from src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml rename to src/sensor_interfacing/ContinentalPointCloudFilter/config/continental_radar_params.yaml index ebbebf9f..12baf8f3 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/config/ars_radar_params.yaml +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/config/continental_radar_params.yaml @@ -1,6 +1,6 @@ -ars_point_cloud_filter: +continental_point_cloud_filter: ros__parameters: - filter_mode: "ars" + filter_mode: "continental" vrel_rad: -99999.99 el_ang: -99999.99 rcs0: -99999.99 diff --git a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp b/src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp similarity index 89% rename from src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp rename to src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp index 88c93b1d..58e01d98 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/include/ars_pointcloud_filter.hpp +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp @@ -1,5 +1,5 @@ -#ifndef ARS_POINTCLOUD_FILTER_HPP_ -#define ARS_POINTCLOUD_FILTER_HPP_ +#ifndef CONTINENTAL_POINTCLOUD_FILTER_HPP_ +#define CONTINENTAL_POINTCLOUD_FILTER_HPP_ #include #include "rclcpp/rclcpp.hpp" @@ -11,7 +11,7 @@ namespace filtering { /** -* @brief The ARSPointCloudFilter is responsible for filtering and publishing radar detections +* @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. @@ -30,10 +30,10 @@ namespace filtering * especially if they are also from two seperate scans. To solve this, the node uses the double * buffer algorithm. */ -class ARSPointCloudFilter +class ContinentalPointCloudFilter { public: - ARSPointCloudFilter(); + ContinentalPointCloudFilter(); typedef struct { std::string scan_mode; @@ -63,7 +63,7 @@ class ARSPointCloudFilter * and starts from scratch with a new scan. */ bool common_scan_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_continental, const filter_parameters & parameters, radar_msgs::msg::RadarPacket & publish_packet); @@ -82,13 +82,14 @@ class ARSPointCloudFilter * each far scan packet that is recieved from that scan. */ bool near_far_scan_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + 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_ars); + scan_type check_scan_type(const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_continental); /** * @brief Resets all scan states (timestamp, packet count, and publish status) @@ -100,7 +101,7 @@ class ARSPointCloudFilter * @brief Pointfilter() filters an incoming radar packet based on set thresholds */ radar_msgs::msg::RadarPacket point_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, + const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_continental, double snr_threshold, double AzAng0_threshold, double range_threshold, @@ -137,4 +138,4 @@ class ARSPointCloudFilter } // namespace filtering -#endif // ARS_POINTCLOUD_FILTER_HPP_ +#endif // CONTINENTAL_POINTCLOUD_FILTER_HPP_ diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter_node.hpp b/src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter_node.hpp new file mode 100644 index 00000000..829e444a --- /dev/null +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/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/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py b/src/sensor_interfacing/ContinentalPointCloudFilter/launch/continental_pointcloud_filter.launch.py similarity index 56% rename from src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py rename to src/sensor_interfacing/ContinentalPointCloudFilter/launch/continental_pointcloud_filter.launch.py index 5639a890..557d7041 100755 --- a/src/sensor_interfacing/ARSPointCloudFilter/launch/ars_pointcloud_filter.launch.py +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/launch/continental_pointcloud_filter.launch.py @@ -6,15 +6,15 @@ def generate_launch_description(): - """Launch ARSPointCloudFilter node.""" - ars_pointcloud_filter_param = DeclareLaunchArgument( - 'filter_mode', default_value='ars') + """Launch ContinentalPointCloudFilter node.""" + continental_pointcloud_filter_param = DeclareLaunchArgument( + 'filter_mode', default_value='continental') - ars_pointcloud_filter_node = Node( + continental_pointcloud_filter_node = Node( LaunchConfiguration('filter_mode'), - condition=LaunchConfigurationEquals('filter_mode', 'ars'), - package='ars_pointcloud_filter', - executable='ars_pointcloud_filter_node', + condition=LaunchConfigurationEquals('filter_mode', 'continental'), + package='continental_pointcloud_filter', + executable='continental_pointcloud_filter_node', parameters=[ {'vrel_rad': -99999.99}, {'el_ang': -99999.99}, @@ -26,6 +26,6 @@ def generate_launch_description(): ] ) return LaunchDescription([ - ars_pointcloud_filter_param, - ars_pointcloud_filter_node + continental_pointcloud_filter_param, + continental_pointcloud_filter_node ]) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/package.xml b/src/sensor_interfacing/ContinentalPointCloudFilter/package.xml similarity index 82% rename from src/sensor_interfacing/ARSPointCloudFilter/package.xml rename to src/sensor_interfacing/ContinentalPointCloudFilter/package.xml index 3f55f068..7e7633e8 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/package.xml +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/package.xml @@ -1,8 +1,8 @@ - ars_pointcloud_filter + continental_pointcloud_filter 0.0.0 - A ROS package for filtering ARS and Carla Radar Data + A ROS package for filtering Continental and Carla Radar Data Rijin Muralidharan TODO diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp b/src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter.cpp similarity index 89% rename from src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp rename to src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter.cpp index 56659542..c5e8e68f 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter.cpp +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter.cpp @@ -2,13 +2,13 @@ #include "rclcpp/rclcpp.hpp" -#include "ars_pointcloud_filter_node.hpp" -#include "ars_pointcloud_filter.hpp" +#include "continental_pointcloud_filter_node.hpp" +#include "continental_pointcloud_filter.hpp" namespace filtering { -ARSPointCloudFilter::ARSPointCloudFilter() +ContinentalPointCloudFilter::ContinentalPointCloudFilter() { near_scan_single_.timestamp_ = 0; near_scan_single_.packet_count_ = 0; @@ -36,13 +36,13 @@ ARSPointCloudFilter::ARSPointCloudFilter() default_timestamp_ = 0; } -radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( - const radar_msgs::msg::RadarPacket::SharedPtr unfiltered_ars, double snr_threshold, +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_ars; - for (auto detection : unfiltered_ars->detections) { + radar_msgs::msg::RadarPacket filtered_continental; + for (auto detection : unfiltered_continental->detections) { if (detection.snr < snr_threshold) { continue; } @@ -61,12 +61,12 @@ radar_msgs::msg::RadarPacket ARSPointCloudFilter::point_filter( if (detection.rcs0 < rcs_threshold) { continue; } - filtered_ars.detections.push_back(detection); + filtered_continental.detections.push_back(detection); } - return filtered_ars; + return filtered_continental; } -ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type( +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) { @@ -76,7 +76,7 @@ ARSPointCloudFilter::scan_type ARSPointCloudFilter::check_scan_type( } } -void ARSPointCloudFilter::reset_scan_states() +void ContinentalPointCloudFilter::reset_scan_states() { near_scan_[buffer_index_].timestamp_ = 0; near_scan_[buffer_index_].publish_status_ = false; @@ -87,7 +87,7 @@ void ARSPointCloudFilter::reset_scan_states() far_scan_[buffer_index_].packet_count_ = 0; } -bool ARSPointCloudFilter::common_scan_filter( +bool ContinentalPointCloudFilter::common_scan_filter( const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters & parameters, radar_msgs::msg::RadarPacket & publish_packet) @@ -139,7 +139,7 @@ bool ARSPointCloudFilter::common_scan_filter( return false; } -bool ARSPointCloudFilter::near_far_scan_filter( +bool ContinentalPointCloudFilter::near_far_scan_filter( const radar_msgs::msg::RadarPacket::SharedPtr msg, const filter_parameters & parameters, radar_msgs::msg::RadarPacket & publish_packet) diff --git a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp b/src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter_node.cpp similarity index 72% rename from src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp rename to src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter_node.cpp index 9389d8ed..584fae50 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/src/ars_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter_node.cpp @@ -1,23 +1,23 @@ #include #include -#include "ars_pointcloud_filter_node.hpp" -#include "ars_pointcloud_filter.hpp" +#include "continental_pointcloud_filter_node.hpp" +#include "continental_pointcloud_filter.hpp" -ARSPointCloudFilterNode::ARSPointCloudFilterNode() -: Node("ars_point_cloud_filter") +ContinentalPointCloudFilterNode::ContinentalPointCloudFilterNode() +: Node("continental_point_cloud_filter") { /** * @note Default values are already declared in yaml file */ - this->declare_parameter("filter_mode"); - this->declare_parameter("scan_mode"); - this->declare_parameter("vrel_rad"); - this->declare_parameter("el_ang"); - this->declare_parameter("rcs0"); - this->declare_parameter("snr"); - this->declare_parameter("range"); - this->declare_parameter("az_ang0"); + 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(); @@ -30,19 +30,19 @@ ARSPointCloudFilterNode::ARSPointCloudFilterNode() raw_left_sub_ = this->create_subscription( "unfilteredRadarLeft", 1, std::bind( - &ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback, + &ContinentalPointCloudFilterNode::unfiltered_continental_radar_left_callback, this, std::placeholders::_1)); raw_right_sub_ = this->create_subscription( "unfilteredRadarRight", 1, std::bind( - &ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback, + &ContinentalPointCloudFilterNode::unfiltered_continental_radar_right_callback, this, std::placeholders::_1)); left_right_pub_ = this->create_publisher("processed", 20); } -void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( +void ContinentalPointCloudFilterNode::unfiltered_continental_radar_right_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { /** @@ -69,7 +69,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_right_callback( /** * @note Implementation below is the same as the right callback function */ -void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( +void ContinentalPointCloudFilterNode::unfiltered_continental_radar_left_callback( const radar_msgs::msg::RadarPacket::SharedPtr msg) { if (parameters.scan_mode == "near" || parameters.scan_mode == "far") { @@ -90,7 +90,7 @@ void ARSPointCloudFilterNode::unfiltered_ars_radar_left_callback( int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } diff --git a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp b/src/sensor_interfacing/ContinentalPointCloudFilter/test/continental_pointcloud_filter_test.cpp similarity index 94% rename from src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp rename to src/sensor_interfacing/ContinentalPointCloudFilter/test/continental_pointcloud_filter_test.cpp index eb2b0004..a28edebb 100644 --- a/src/sensor_interfacing/ARSPointCloudFilter/test/ars_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/ContinentalPointCloudFilter/test/continental_pointcloud_filter_test.cpp @@ -5,10 +5,10 @@ #include "gtest/gtest.h" #include "rclcpp/rclcpp.hpp" -#include "ars_pointcloud_filter.hpp" +#include "continental_pointcloud_filter.hpp" -class ARSPointCloudFilterFixtureTest : public ::testing::Test +class ContinentalPointCloudFilterFixtureTest : public ::testing::Test { public: static constexpr double DEFAULT_FILTER_PARAM = -9999.99; @@ -29,8 +29,8 @@ class ARSPointCloudFilterFixtureTest : public ::testing::Test } protected: - filtering::ARSPointCloudFilter pointcloudfilter; - filtering::ARSPointCloudFilter::filter_parameters parameters; + filtering::ContinentalPointCloudFilter pointcloudfilter; + filtering::ContinentalPointCloudFilter::filter_parameters parameters; }; // Common Scan Filter Test @@ -38,7 +38,7 @@ class ARSPointCloudFilterFixtureTest : public ::testing::Test /** * @brief Checks if check_scan_type() correctly identifies if a packet is NEAR or FAR. */ -TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) +TEST_F(ContinentalPointCloudFilterFixtureTest, CheckScanType) { // Near Scan message auto msg_0 = std::make_shared(); @@ -64,7 +64,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckScanType) * @brief Sends 18 near scan packets with the same timestamp and checks if it returns a * complete packet ready to be published. */ -TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearScanPackets) { SetUp("near"); @@ -96,7 +96,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearScanPackets) * @brief Sends 12 far scan packets with the same timestamp and checks if it returns a * complete packet ready to be published. */ -TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteFarScanPackets) { SetUp("far"); @@ -129,7 +129,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteFarScanPackets) * @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(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) { SetUp("near"); @@ -184,7 +184,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) * @brief Sends incomplete packets (5 that are the same and 1 packet after that is different in timestamp). */ -TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteScanPackets) { SetUp("near"); @@ -235,7 +235,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteScanPackets) /** * @brief Sends far scan packets when in near scan mode and checks if it filters correctly. */ -TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) { SetUp("near"); @@ -277,7 +277,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendFarScanPacketsInNearMode) /** * @brief Checks if a packet with several detections is filtered out correctly. */ -TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) +TEST_F(ContinentalPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) { SetUp("near", DEFAULT_FILTER_PARAM, DEFAULT_FILTER_PARAM, 70, 100, 40, DEFAULT_FILTER_PARAM); @@ -339,7 +339,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) * @brief Sends 18 Near Scan Packets and 12 Far Scan Packets. Checks if it returns a * complete packet ready to be published. */ -TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) { SetUp("nearfar"); @@ -389,7 +389,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) * @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(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendMultipleScanPackets) { SetUp("nearfar"); @@ -490,7 +490,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendMultipleScanPackets) * @brief Sends 5 Near (scan 1), Send 12 far (scan 1) and checks if it returns the correct packet. * (Special Case) */ -TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteNearPackets) { SetUp("nearfar"); @@ -571,7 +571,7 @@ TEST_F(ARSPointCloudFilterFixtureTest, SendIncompleteNearPackets) * @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(ARSPointCloudFilterFixtureTest, SendIncompleteFarPackets) +TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteFarPackets) { SetUp("nearfar"); From 7e9942058518cf91013ef5ae5286b81ec73ced96 Mon Sep 17 00:00:00 2001 From: Rijin Date: Thu, 23 Mar 2023 01:53:17 +0000 Subject: [PATCH 50/51] renamed ROS node to continental driver --- .../continental_driver.Dockerfile | 4 +- profiles/docker-compose.sensors.yaml | 3 +- .../include/continental_pointcloud_filter.hpp | 141 ------------------ .../CMakeLists.txt | 0 .../config/continental_radar_params.yaml | 0 .../include/continental_pointcloud_filter.hpp | 141 ++++++++++++++++++ .../continental_pointcloud_filter_node.hpp | 0 .../continental_pointcloud_filter.launch.py | 0 .../package.xml | 0 .../src/continental_pointcloud_filter.cpp | 0 .../continental_pointcloud_filter_node.cpp | 10 +- .../continental_pointcloud_filter_test.cpp | 36 +---- 12 files changed, 158 insertions(+), 177 deletions(-) delete mode 100644 src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/CMakeLists.txt (100%) rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/config/continental_radar_params.yaml (100%) create mode 100644 src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/include/continental_pointcloud_filter_node.hpp (100%) rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/launch/continental_pointcloud_filter.launch.py (100%) rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/package.xml (100%) rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/src/continental_pointcloud_filter.cpp (100%) rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/src/continental_pointcloud_filter_node.cpp (92%) rename src/sensor_interfacing/{ContinentalPointCloudFilter => continental_driver}/test/continental_pointcloud_filter_test.cpp (91%) diff --git a/docker/continental_driver/continental_driver.Dockerfile b/docker/continental_driver/continental_driver.Dockerfile index aaa07940..d9a18ac8 100644 --- a/docker/continental_driver/continental_driver.Dockerfile +++ b/docker/continental_driver/continental_driver.Dockerfile @@ -23,7 +23,7 @@ USER docker:docker RUN mkdir -p ~/ament_ws/src WORKDIR /home/docker/ament_ws/src -COPY src/sensor_interfacing/ContinentalPointCloudFilter ContinentalPointCloudFilter +COPY src/sensor_interfacing/continental_driver continental_driver COPY src/wato_msgs/radar_msgs radar_msgs WORKDIR /home/docker/ament_ws @@ -38,6 +38,6 @@ COPY docker/.bashrc /home/docker/.bashrc RUN sudo chmod +x ~/wato_ros_entrypoint.sh -ENTRYPOINT ["/home/docker/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 index 85b0aa58..3e439cd8 100644 --- a/profiles/docker-compose.sensors.yaml +++ b/profiles/docker-compose.sensors.yaml @@ -8,5 +8,6 @@ services: - "${CONTINENTAL_DRIVER_IMAGE:?}:${CACHE_FROM_TAG}" - "${CONTINENTAL_DRIVER_IMAGE:?}:develop" image: "${CONTINENTAL_DRIVER_IMAGE:?}:${TAG}" + user: ${FIXUID:?}:${FIXGID:?} volumes: - - ../src/sensor_interfacing/ContinentalPointCloudFilter:/home/docker/ament_ws/src/ContinentalPointCloudFilter + - ../src/sensor_interfacing/continental_driver:/home/docker/ament_ws/src/continental_driver diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp b/src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp deleted file mode 100644 index 58e01d98..00000000 --- a/src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter.hpp +++ /dev/null @@ -1,141 +0,0 @@ -#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) - */ - 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/ContinentalPointCloudFilter/CMakeLists.txt b/src/sensor_interfacing/continental_driver/CMakeLists.txt similarity index 100% rename from src/sensor_interfacing/ContinentalPointCloudFilter/CMakeLists.txt rename to src/sensor_interfacing/continental_driver/CMakeLists.txt diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/config/continental_radar_params.yaml b/src/sensor_interfacing/continental_driver/config/continental_radar_params.yaml similarity index 100% rename from src/sensor_interfacing/ContinentalPointCloudFilter/config/continental_radar_params.yaml rename to src/sensor_interfacing/continental_driver/config/continental_radar_params.yaml 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..92dc7727 --- /dev/null +++ b/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp @@ -0,0 +1,141 @@ +#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). + */ + 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/ContinentalPointCloudFilter/include/continental_pointcloud_filter_node.hpp b/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter_node.hpp similarity index 100% rename from src/sensor_interfacing/ContinentalPointCloudFilter/include/continental_pointcloud_filter_node.hpp rename to src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter_node.hpp diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/launch/continental_pointcloud_filter.launch.py b/src/sensor_interfacing/continental_driver/launch/continental_pointcloud_filter.launch.py similarity index 100% rename from src/sensor_interfacing/ContinentalPointCloudFilter/launch/continental_pointcloud_filter.launch.py rename to src/sensor_interfacing/continental_driver/launch/continental_pointcloud_filter.launch.py diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/package.xml b/src/sensor_interfacing/continental_driver/package.xml similarity index 100% rename from src/sensor_interfacing/ContinentalPointCloudFilter/package.xml rename to src/sensor_interfacing/continental_driver/package.xml diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter.cpp b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp similarity index 100% rename from src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter.cpp rename to src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter_node.cpp b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter_node.cpp similarity index 92% rename from src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter_node.cpp rename to src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter_node.cpp index 584fae50..fef93bf8 100644 --- a/src/sensor_interfacing/ContinentalPointCloudFilter/src/continental_pointcloud_filter_node.cpp +++ b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter_node.cpp @@ -8,7 +8,7 @@ ContinentalPointCloudFilterNode::ContinentalPointCloudFilterNode() : Node("continental_point_cloud_filter") { /** - * @note Default values are already declared in yaml file + * @note Default values are already declared in yaml file. */ this->declare_parameter("filter_mode", "continental"); this->declare_parameter("scan_mode", "near"); @@ -47,9 +47,9 @@ void ContinentalPointCloudFilterNode::unfiltered_continental_radar_right_callbac { /** * @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. + * 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; @@ -67,7 +67,7 @@ void ContinentalPointCloudFilterNode::unfiltered_continental_radar_right_callbac } /** -* @note Implementation below is the same as the right callback function +* @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) diff --git a/src/sensor_interfacing/ContinentalPointCloudFilter/test/continental_pointcloud_filter_test.cpp b/src/sensor_interfacing/continental_driver/test/continental_pointcloud_filter_test.cpp similarity index 91% rename from src/sensor_interfacing/ContinentalPointCloudFilter/test/continental_pointcloud_filter_test.cpp rename to src/sensor_interfacing/continental_driver/test/continental_pointcloud_filter_test.cpp index a28edebb..238f81e5 100644 --- a/src/sensor_interfacing/ContinentalPointCloudFilter/test/continental_pointcloud_filter_test.cpp +++ b/src/sensor_interfacing/continental_driver/test/continental_pointcloud_filter_test.cpp @@ -62,7 +62,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, CheckScanType) /** * @brief Sends 18 near scan packets with the same timestamp and checks if it returns a -* complete packet ready to be published. +* complete packet ready to be published. */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearScanPackets) { @@ -94,7 +94,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearScanPackets) /** * @brief Sends 12 far scan packets with the same timestamp and checks if it returns a -* complete packet ready to be published. +* complete packet ready to be published. */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteFarScanPackets) { @@ -127,7 +127,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteFarScanPackets) /** * @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. +* (timestamp 2) and checks if it returns a complete packet ready to be published. */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) { @@ -182,7 +182,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, SendTwoDifferentScanPackets) /** * @brief Sends incomplete packets (5 that are the same and - 1 packet after that is different in timestamp). +* 1 packet after that is different in timestamp). */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteScanPackets) { @@ -337,7 +337,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, CheckIfDetectionsFiltered) /** * @brief Sends 18 Near Scan Packets and 12 Far Scan Packets. Checks if it returns a -* complete packet ready to be published. +* complete packet ready to be published. */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) { @@ -387,7 +387,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, SendCompleteNearAndFarPackets) /** * @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) +* 2 far scan (Scan 1), 15 Near scan (Scan 2), 12 Far scan (Scan 2). */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendMultipleScanPackets) { @@ -488,7 +488,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, SendMultipleScanPackets) /** * @brief Sends 5 Near (scan 1), Send 12 far (scan 1) and checks if it returns the correct packet. -* (Special Case) +* (Special Case). */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteNearPackets) { @@ -569,7 +569,7 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteNearPackets) /** * @brief Sends 7 far (scan 1), Send 1 Near (scan 2), -* then checks if the 7 far (scan 1) is discarded (Special case) +* then checks if the 7 far (scan 1) is discarded (Special case). */ TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteFarPackets) { @@ -634,23 +634,3 @@ TEST_F(ContinentalPointCloudFilterFixtureTest, SendIncompleteFarPackets) EXPECT_EQ(8, static_cast(publish_packet.timestamp)); EXPECT_EQ(30, publish_packet.detections.size()); } - - -// Unit test cases (Common Scan Filter) -// 1. Send 18 near scan packets and check if it returns a complete 18 packet ready to be published -// 2. Send 12 far scan packets and check if it returns a complete 12 packet ready to be published -// 3. Send 18 that are the same (timestamp 1), and 18 that are different (timestamp 2) -// 4. Send incomplete packets (5 that are the same) (1 that is different in timestamp) -// 5. Send far scan packets when in near scan mode, does it filter correctly -// 6. Check if it filters out all the packets and returns an empty packet -// (pass thresholds into pointfilter) - -// Unit Test Cases (Double Buffer Algorithm) -// 1. Send 18 Near Scan Packets and 12 Far Scan Packets -// (It shouldn't publish after 18 packets. It should publish together -// when there is also complete 12 far scan packets) -// 2. 18 Near scan (scan1), 10 far scan (scan1), 3 Near (scan 2), 2 far (scan 1), -// (SCAN 1 SHOULD BE PUBLISHED) 15 Near (scan 2), 12 Far (scan2) (SCAN 2 GETS PUBLISHED HERE) -// 3. Send 7 far (scan 1) (NEAR SCANS WERE IGNORED), Send 1 Near (Scan 2), -// Send 1 Far (Scan 2)then check if the 7 far is published -// 4. Send 5 Near (scan 1), Send 12 far (scan 1) From 3b07ebbdf461e4eadc4bf236718cb4618f64cf92 Mon Sep 17 00:00:00 2001 From: Rijin Date: Tue, 2 May 2023 00:51:11 +0000 Subject: [PATCH 51/51] added variables for packet capacities --- .../include/continental_pointcloud_filter.hpp | 2 ++ .../src/continental_pointcloud_filter.cpp | 10 ++++++---- 2 files changed, 8 insertions(+), 4 deletions(-) diff --git a/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp b/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp index 92dc7727..487e2126 100644 --- a/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp +++ b/src/sensor_interfacing/continental_driver/include/continental_pointcloud_filter.hpp @@ -112,6 +112,8 @@ class ContinentalPointCloudFilter /** * @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 { diff --git a/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp index c5e8e68f..040bf8fd 100644 --- a/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp +++ b/src/sensor_interfacing/continental_driver/src/continental_pointcloud_filter.cpp @@ -34,6 +34,8 @@ ContinentalPointCloudFilter::ContinentalPointCloudFilter() buffer_index_ = 0; default_timestamp_ = 0; + near_scan_capacity_ = 18; + far_scan_capacity_ = 12; } radar_msgs::msg::RadarPacket ContinentalPointCloudFilter::point_filter( @@ -94,7 +96,7 @@ bool ContinentalPointCloudFilter::common_scan_filter( { scan_type incoming_scan_msg = check_scan_type(msg); - int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; + 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_; @@ -146,7 +148,7 @@ bool ContinentalPointCloudFilter::near_far_scan_filter( { scan_type incoming_scan_msg = check_scan_type(msg); - int scan_capacity = (incoming_scan_msg == NEAR) ? 18 : 12; + 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(); @@ -198,8 +200,8 @@ bool ContinentalPointCloudFilter::near_far_scan_filter( if (far_scan_[buffer_index_].publish_status_ == true) { // Special Case - if (near_scan_[buffer_index_].packet_count_ != 18 || - far_scan_[buffer_index_].packet_count_ != 12) + 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();