Merge pull request #21 from WATonomous/radar_rviz_node

Adding the Radar Visualization node in the Radar ROS2 Pipeline to main

docker/perception/radar_object_detection.Dockerfile

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

profiles/docker-compose.perception.yaml

@@ -0,0 +1,13 @@
+version: "3.8"
+services:
+  radar_object_detection:
+    build:
+      context: ..
+      dockerfile: docker/perception/radar_object_detection.Dockerfile
+      cache_from: 
+        - "${RADAR_OBJECT_DETECTION_IMAGE:?}:${TAG}"
+        - "${RADAR_OBJECT_DETECTION_IMAGE:?}:main"
+    image: "${RADAR_OBJECT_DETECTION_IMAGE:?}:${TAG}"
+    user: ${FIXUID:?}:${FIXGID:?}
+    volumes:
+      - ../src/perception/radar_object_detection:/home/docker/ament_ws/src/radar_object_detection

scripts/watod-setup-env.sh

@@ -63,6 +63,9 @@ INFRASTRUCTURE_VIS_TOOLS_VNC_IMAGE=${INFRASTRUCTURE_VIS_TOOLS_VNC_IMAGE:-"git.uw
 INFRASTRUCTURE_DATA_STREAM_IMAGE=${DATA_STREAM_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/infrastructure_data_stream"}
 INFRASTRUCTURE_FOXGLOVE_IMAGE=${DATA_STREAM_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/infrastructure_foxglove"}
 
+# Perception
+RADAR_OBJECT_DETECTION_IMAGE=${RADAR_OBJECT_DETECTION_IMAGE:-"git.uwaterloo.ca:5050/watonomous/wato_monorepo/radar_object_detection"}
+
 ## -------------------------- User ID -----------------------------
 
 FIXUID=$(id -u) 
@@ -117,6 +120,8 @@ echo "INFRASTRUCTURE_DATA_STREAM_IMAGE=$INFRASTRUCTURE_DATA_STREAM_IMAGE" >> "$P
 echo "INFRASTRUCTURE_FOXGLOVE_IMAGE=$INFRASTRUCTURE_FOXGLOVE_IMAGE" >> "$PROFILES_DIR/.env"
 
 # Perception
+echo "RADAR_OBJECT_DETECTION_IMAGE=$RADAR_OBJECT_DETECTION_IMAGE" >> "$PROFILES_DIR/.env"
+
 # World Modeling
 # Control
 # Simulation

src/perception/radar_object_detection/radar_object_detection_launch/CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 3.10)
+project(radar_object_detection_launch)
+
+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()
+
+find_package(ament_cmake REQUIRED)
+
+option(BUILD_TESTING "Build tests" ON)
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME})
+
+ament_package()
+

src/perception/radar_object_detection/radar_object_detection_launch/launch/radar_object_detection.launch.py

@@ -0,0 +1,14 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    """Launch Radar object detection system."""
+    radar_vis_node = Node(
+        package='radar_vis',
+        executable='radar_vis_node',
+    )
+
+    return LaunchDescription([
+        radar_vis_node
+    ])

src/perception/radar_object_detection/radar_object_detection_launch/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>radar_object_detection_launch</name>
+  <version>0.0.0</version>
+  <description>A ROS package for launching the radar object detection system</description>
+
+  <maintainer email="[email protected]">Rijin Muralidharan</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>radar_vis</exec_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/perception/radar_object_detection/radar_vis/CMakeLists.txt

@@ -0,0 +1,57 @@
+cmake_minimum_required(VERSION 3.10)
+project(radar_vis)
+
+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()
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED) # ROS2 package containing point cloud messages
+find_package(radar_msgs REQUIRED)  # Custom package containing ROS2 Radar messages
+
+add_library(radar_vis_lib
+  src/radar_vis.cpp)
+
+target_include_directories(radar_vis_lib
+  PUBLIC include)
+
+ament_target_dependencies(radar_vis_lib rclcpp radar_msgs sensor_msgs)
+
+option(BUILD_TESTING "Build tests" ON)
+if(BUILD_TESTING)
+
+  find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(ament_lint_common REQUIRED)
+
+  list(APPEND AMENT_LINT_AUTO_EXCLUDE
+    ament_cmake_cpplint
+    ament_cmake_copyright)
+
+  ament_cpplint(FILTERS "-legal/copyright")
+
+  ament_lint_auto_find_test_dependencies()
+
+  ament_add_gtest(radar_vis_test test/radar_vis_test.cpp)
+
+  target_link_libraries(radar_vis_test radar_vis_lib)
+
+  install(TARGETS
+    radar_vis_test
+    DESTINATION lib/${PROJECT_NAME})
+endif()
+
+add_executable(radar_vis_node src/radar_vis_node.cpp)
+
+target_link_libraries(radar_vis_node radar_vis_lib)
+
+install(TARGETS
+  radar_vis_node
+  DESTINATION lib/${PROJECT_NAME})
+
+ament_package()

src/perception/radar_object_detection/radar_vis/include/radar_vis.hpp

@@ -0,0 +1,26 @@
+#ifndef RADAR_VIS_HPP_
+#define RADAR_VIS_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/msg/point_field.hpp"
+
+#include "radar_msgs/msg/radar_packet.hpp"
+#include "radar_msgs/msg/radar_detection.hpp"
+
+namespace visualization
+{
+
+class RadarVis
+{
+public:
+  RadarVis();
+
+  sensor_msgs::msg::PointCloud2 convert_packet_to_pointcloud(
+    const radar_msgs::msg::RadarPacket::SharedPtr msg);
+};
+
+}  // namespace visualization
+
+#endif  // RADAR_VIS_HPP_

src/perception/radar_object_detection/radar_vis/include/radar_vis_node.hpp

@@ -0,0 +1,42 @@
+#ifndef RADAR_VIS_NODE_HPP_
+#define RADAR_VIS_NODE_HPP_
+
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+#include "sensor_msgs/msg/point_field.hpp"
+
+#include "radar_msgs/msg/radar_packet.hpp"
+#include "radar_msgs/msg/radar_detection.hpp"
+
+#include "radar_vis.hpp"
+
+/**
+* @brief Implementation of a Radar visualization node that listens to "processed" topic
+*/
+class RadarVisNode : public rclcpp::Node
+{
+public:
+  RadarVisNode();
+
+  /**
+  * @brief A ROS2 subscription node callback used to unpack filtered radar data from the "processed"
+  *    topic. The callback listens to radar packets on this topic and processes them
+  *    in order to visualize them in tools like foxglove and RViz.
+  *
+  * @param msg a raw message from the "processed" topic
+  */
+  void process_radar_data_callback(
+    const radar_msgs::msg::RadarPacket::SharedPtr msg);
+
+private:
+  // ROS2 Subscriber listening to "processed" topic.
+  rclcpp::Subscription<radar_msgs::msg::RadarPacket>::SharedPtr raw_sub_;
+
+  // ROS2 Publisher that sends point cloud data to "visualization" topic for foxglove/Rviz.
+  rclcpp::Publisher<sensor_msgs::msg::PointCloud2>::SharedPtr raw_pub_;
+
+  // An object containing methods for converting radar packets into point clouds.
+  visualization::RadarVis packet_converter_;
+};
+
+#endif  // RADAR_VIS_NODE_HPP_

src/perception/radar_object_detection/radar_vis/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package format="3">
+  <name>radar_vis</name>
+  <version>0.0.0</version>
+  <description>A ROS package for visualizing radar detections</description>
+
+  <maintainer email="[email protected]">Rijin Muralidharan</maintainer>
+  <license>TODO</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>radar_msgs</depend>
+  <depend>sensor_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/perception/radar_object_detection/radar_vis/src/radar_vis.cpp

@@ -0,0 +1,86 @@
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "radar_vis_node.hpp"
+#include "radar_vis.hpp"
+
+namespace visualization
+{
+
+RadarVis::RadarVis()
+{}
+
+sensor_msgs::msg::PointCloud2 RadarVis::convert_packet_to_pointcloud(
+  const radar_msgs::msg::RadarPacket::SharedPtr msg)
+{
+  std::string radar_frame = "radar_fixed";
+  sensor_msgs::msg::PointCloud2 point_cloud;
+  sensor_msgs::msg::PointField point_field;
+
+  point_cloud.header = msg->header;
+  point_cloud.header.frame_id = radar_frame;
+  point_cloud.height = 1;
+  point_cloud.width = 0;
+  point_cloud.is_bigendian = false;
+  point_cloud.point_step = 16;
+  point_cloud.row_step = 0;
+  point_cloud.is_dense = true;
+  point_field.datatype = sensor_msgs::msg::PointField::FLOAT32;
+  point_field.count = 1;
+
+  // X Coordinate - Point Field
+  point_field.name = "x";
+  point_field.offset = 0;
+  point_cloud.fields.push_back(point_field);
+
+  // Y Coordinate - Point Field
+  point_field.name = "y";
+  point_field.offset = 4;
+  point_cloud.fields.push_back(point_field);
+
+  // Z Coordinate - Point Field
+  point_field.name = "z";
+  point_field.offset = 8;
+  point_cloud.fields.push_back(point_field);
+
+  // Intensity - Point Field
+  point_field.name = "intensity";
+  point_field.offset = 12;
+  point_cloud.fields.push_back(point_field);
+
+  // Convert data to 4 bytes (little endian)
+  uint8_t * tmp_ptr;
+  for (uint8_t index = 0; index < msg->detections.size(); index++) {
+    // Position X - Point Cloud Conversion
+    tmp_ptr = reinterpret_cast<uint8_t *>(&(msg->detections[index].pos_x));
+    for (int byte = 0; byte < 4; byte++) {
+      point_cloud.data.push_back(tmp_ptr[byte]);
+    }
+
+    // Position Y - Point Cloud Conversion
+    tmp_ptr = reinterpret_cast<uint8_t *>(&(msg->detections[index].pos_y));
+    for (int byte = 0; byte < 4; byte++) {
+      point_cloud.data.push_back(tmp_ptr[byte]);
+    }
+
+    // Position Z - Point Cloud Conversion
+    tmp_ptr = reinterpret_cast<uint8_t *>(&(msg->detections[index].pos_z));
+    for (int byte = 0; byte < 4; byte++) {
+      point_cloud.data.push_back(tmp_ptr[byte]);
+    }
+
+    // Intensity (rcs0) - Point Cloud Conversion
+    float intensity = (msg->detections[index].rcs0 / 2.0) + 50.0;
+    tmp_ptr = reinterpret_cast<uint8_t *>(&(intensity));
+    for (int byte = 0; byte < 4; byte++) {
+      point_cloud.data.push_back(tmp_ptr[byte]);
+    }
+    point_cloud.width++;
+  }
+
+  point_cloud.row_step = point_cloud.width * point_cloud.point_step;
+  return point_cloud;
+}
+
+}  // namespace visualization