Calculate Relative Velocity Based on Odom

Author: troygibb

README.md

@@ -76,10 +76,18 @@ The `min_value` and `max_value` parameter specifies the minimum and maximum valu
 
 Further information about the filter configurations can be found in the ARS408 technical documentation
 
-## Configuring Motion Input
+### Configuring Motion Input
 
 This driver optionally lets you input your odometry into the sensor, which will be intepreted by the radars and used in their own tracking algorithms. There are two parameters you need to set:
 
 - `odom_topic_name`: string
 - `radar_[index].send_motion`: bool
 
+## Relative Velocity
+
+Since the ARS408 tracks return their velocities under the assumption that the radar sensor frame is stationary, this driver will calculate the velocity of tracks relative to odom frame, assuming you've added an odometry topic as a parameter under `odom_topic_name`.
+
+You can also set:
+- `transform_timeout`: Used in `lookupTransform`
+- `robot_base_frame`: Default is `base_link`
+

radar_conti_ars408/src/CMakeLists.txt

@@ -167,19 +167,31 @@ install(DIRECTORY
 
 
 if(BUILD_TESTING)
+  find_package(Catch2 2 REQUIRED)
+  set(test_dependencies
+    Catch2 
+  )
+  add_executable(test_radar_transforms test/test_radar_transforms.cpp)
+  target_link_libraries(test_radar_transforms Catch2::Catch2 ${library_name})
+  ament_target_dependencies(test_radar_transforms
+    ${test_dependencies}
+    ${dependencies}
+  )
+  add_test(NAME test_radar_transforms COMMAND catch_tests)
+ 
   # Until CI has VCAN support, don't run this test
   if(NOT DEFINED ENV{CI})
     find_package(Catch2 2 REQUIRED)
     set(test_dependencies
       Catch2 
     )
-    add_executable(catch_tests test/test_radar_conti_ars408_component.cpp)
-    target_link_libraries(catch_tests Catch2::Catch2 ${library_name})
-    ament_target_dependencies(catch_tests
+    add_executable(test_radar_conti_ars408_component test/test_radar_conti_ars408_component.cpp)
+    target_link_libraries(test_radar_conti_ars408_component Catch2::Catch2 ${library_name})
+    ament_target_dependencies(test_radar_conti_ars408_component
       ${test_dependencies}
       ${dependencies}
     )
-    add_test(NAME catch_tests COMMAND catch_tests)
+    add_test(NAME test_radar_conti_ars408_component COMMAND catch_tests)
   endif()
 endif()
 

radar_conti_ars408/src/config/radar.yaml

@@ -1,19 +1,16 @@
 radar_node:
   ros__parameters:
-    can_channel: "can0"
+    can_channel: "vcan0"
     object_list_topic_name: "object_list"
     marker_array_topic_name: "marker_array"
     radar_tracks_topic_name: "radar_tracks"
     obstacle_array_topic_name: "obstacle_array"
     filter_config_topic_name: "filter_config"
     radar_state_topic_name: "radar_state"
-    odom_topic_name: "/vehicle/odometry" 
 
     radar_0:
         #link of radar, should be relative to base_link      
         link_name: "front_radar_link"
-        send_motion: true
-
         radarcfg_sensor_id: 0
         radarcfg_sensor_id_valid: 1
         radarcfg_storeinnvm: 1

radar_conti_ars408/src/include/radar_conti_ars408_component.hpp

@@ -10,6 +10,8 @@
 #include "visibility_control.h"
 #include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/odometry.hpp"
+#include "tf2_ros/transform_listener.h"
+#include "tf2_ros/buffer.h"
 
 #include "can_msgs/msg/frame.hpp"
 #include "std_msgs/msg/string.hpp"
@@ -338,6 +340,11 @@ namespace FHAC
         void initializeFilterConfigs();
         void publishFilterConfigMetadata();
         void odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg);
+        nav_msgs::msg::Odometry vehicle_odometry_;
+        std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
+        std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+        rclcpp::Duration transform_timeout_{0, 0};
+
         void sendMotionInputSignals(const size_t &sensor_id, uint8_t direction, double speed, double yaw_rate);
         void publishFovMetadata();
 
@@ -371,6 +378,7 @@ namespace FHAC
         std::unique_ptr<bond::Bond> bond_{nullptr};
 
         std::vector<std::string> radar_link_names_;
+        std::string robot_base_frame_;
         std::vector<bool> filter_config_initialized_list_;
 
         // ##################################

radar_conti_ars408/src/include/radar_transforms.hpp

@@ -0,0 +1,83 @@
+#ifndef TRANSFORMS_HPP
+#define TRANSFORMS_HPP
+
+#include <nav_msgs/msg/odometry.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <geometry_msgs/msg/vector3.hpp>
+#include <tf2/LinearMath/Quaternion.h>
+#include <tf2/LinearMath/Matrix3x3.h>
+#include <tf2_ros/buffer.h>
+#include <rclcpp/rclcpp.hpp>
+#include <cmath>
+#include <memory>
+#include <string>
+
+namespace radar_transforms
+{
+  nav_msgs::msg::Odometry transform2DOdom(const nav_msgs::msg::Odometry &odometry, const std::shared_ptr<tf2_ros::Buffer> &tf_buffer, std::string &sensor_frame_id, std::string &base_frame_id, rclcpp::Duration &transform_timeout, const rclcpp::Time &stamp, rclcpp::Clock::SharedPtr clock)
+  {
+
+    geometry_msgs::msg::TransformStamped base_link_to_sensor_transform;
+    try
+    {
+      base_link_to_sensor_transform = tf_buffer->lookupTransform(sensor_frame_id, base_frame_id, stamp, transform_timeout);
+    }
+    catch (tf2::TimeoutException &exception)
+    {
+      RCLCPP_ERROR_THROTTLE(rclcpp::get_logger("radar_conti_ars408"), *clock, 1000, "Transform timed out: %s", exception.what());
+      return nav_msgs::msg::Odometry{};
+    }
+    catch (tf2::TransformException &exception)
+    {
+      RCLCPP_ERROR_THROTTLE(rclcpp::get_logger("radar_conti_ars408"), *clock, 1000, "Transform failed: %s", exception.what());
+      return nav_msgs::msg::Odometry{};
+    }
+    nav_msgs::msg::Odometry transformed_odometry_base_link;
+    nav_msgs::msg::Odometry transformed_odometry_sensor;
+
+    // 1. get velocity of an offset frame, with location defined by transform
+    // This is the sensor's velocity oriented in the direction of base_link but the magnitudes are relative to a stationary odom frame
+    transformed_odometry_base_link.header = odometry.header;
+    transformed_odometry_base_link.header.frame_id = base_link_to_sensor_transform.header.frame_id;
+    transformed_odometry_base_link.child_frame_id = base_link_to_sensor_transform.child_frame_id;
+    transformed_odometry_base_link.twist.twist.linear.x = odometry.twist.twist.linear.x;
+    transformed_odometry_base_link.twist.twist.linear.y = odometry.twist.twist.linear.y;
+    transformed_odometry_base_link.twist.twist.angular.z = odometry.twist.twist.angular.z;
+
+    tf2::Quaternion q(
+        base_link_to_sensor_transform.transform.rotation.x,
+        base_link_to_sensor_transform.transform.rotation.y,
+        base_link_to_sensor_transform.transform.rotation.z,
+        base_link_to_sensor_transform.transform.rotation.w);
+    tf2::Matrix3x3 m(q);
+    double roll, pitch, yaw;
+    m.getRPY(roll, pitch, yaw);
+
+    transformed_odometry_sensor = transformed_odometry_base_link;
+    transformed_odometry_sensor.twist.twist.linear.x = cos(yaw) * transformed_odometry_base_link.twist.twist.linear.x - sin(yaw) * transformed_odometry_base_link.twist.twist.linear.y;
+    transformed_odometry_sensor.twist.twist.linear.y = sin(yaw) * transformed_odometry_base_link.twist.twist.linear.x + cos(yaw) * transformed_odometry_base_link.twist.twist.linear.y;
+    transformed_odometry_base_link.twist.twist.angular.z = odometry.twist.twist.angular.z;
+
+    return transformed_odometry_sensor;
+  }
+
+  geometry_msgs::msg::Vector3 correctObstacleVelocity(
+      nav_msgs::msg::Odometry &corrected_odom,
+      const geometry_msgs::msg::Vector3 &raw_obj_velocity,
+      const geometry_msgs::msg::Point &raw_obj_pos)
+  {
+    // Calculate the angular velocity components of object velocity with respect to odom frame
+    auto r = hypot(raw_obj_pos.x, raw_obj_pos.y);
+    auto tangential_velocity = r * corrected_odom.twist.twist.angular.z;
+    auto theta = atan2(raw_obj_pos.y, raw_obj_pos.x);
+    auto tangential_velocity_x = tangential_velocity * sin(theta);
+    auto tangential_velocity_y = -tangential_velocity * cos(theta);
+    geometry_msgs::msg::Vector3 corrected_velocity;
+    corrected_velocity.x = raw_obj_velocity.x + corrected_odom.twist.twist.linear.x - tangential_velocity_x;
+    corrected_velocity.y = raw_obj_velocity.y + corrected_odom.twist.twist.linear.y - tangential_velocity_y;
+    corrected_velocity.z = raw_obj_velocity.z; // Assuming no change in z
+    return corrected_velocity;
+  }
+}
+
+#endif // TRANSFORMS_HPP

radar_conti_ars408/src/src/radar_conti_ars408_component.cpp

@@ -8,6 +8,8 @@
 #include <utility>
 #include <math.h>
 #include <fmt/core.h>
+#include <tf2/LinearMath/Quaternion.h>
+#include "../include/radar_transforms.hpp"
 
 #define _USE_MATH_DEFINES
 
@@ -52,15 +54,20 @@ namespace FHAC
     auto node = shared_from_this();
     node->declare_parameter("can_channel", rclcpp::ParameterValue(""));
     node->declare_parameter("odom_topic_name", rclcpp::ParameterValue(""));
+    node->declare_parameter("robot_base_frame", rclcpp::ParameterValue("base_link"));
+    node->declare_parameter("transform_timeout", rclcpp::ParameterValue(0.1));
     node->declare_parameter("object_list_topic_name", rclcpp::ParameterValue("ars408/objectlist"));
     node->declare_parameter("marker_array_topic_name", rclcpp::ParameterValue("ars408/marker_array"));
     node->declare_parameter("radar_tracks_topic_name", rclcpp::ParameterValue("ars408/radar_tracks"));
     node->declare_parameter("obstacle_array_topic_name", rclcpp::ParameterValue("ars408/obstacle_array"));
     node->declare_parameter("filter_config_topic_name", rclcpp::ParameterValue("ars408/filter_config"));
     node->declare_parameter("radar_state_topic_name", rclcpp::ParameterValue("ars408/radar_state"));
 
+    double transform_timeout_double;
     node->get_parameter("can_channel", can_channel_);
     node->get_parameter("odom_topic_name", odom_topic_name_);
+    node->get_parameter("robot_base_frame", robot_base_frame_);
+    node->get_parameter("transform_timeout", transform_timeout_double);
     node->get_parameter("object_list_topic_name", object_list_topic_name_);
     node->get_parameter("marker_array_topic_name", marker_array_topic_name_);
     node->get_parameter("radar_tracks_topic_name", radar_tracks_topic_name_);
@@ -248,6 +255,9 @@ namespace FHAC
       odometry_subscriber_ = create_subscription<nav_msgs::msg::Odometry>(odom_topic_name_, qos_settings, std::bind(&radar_conti_ars408::odomCallback, this, std::placeholders::_1));
     }
 
+    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+    transform_timeout_ = rclcpp::Duration::from_seconds(transform_timeout_double);
     generateUUIDTable();
 
     return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS;
@@ -695,6 +705,12 @@ namespace FHAC
 
     std::map<int, radar_conti_ars408_msgs::msg::Object>::iterator itr;
 
+    nav_msgs::msg::Odometry corrected_odom;
+    if (!odom_topic_name_.empty())
+    {
+      corrected_odom = radar_transforms::transform2DOdom(vehicle_odometry_, tf_buffer_, radar_link_names_[sensor_id], robot_base_frame_, transform_timeout_, this->get_clock()->now(), this->get_clock());
+    }
+
     for (itr = object_map_list_[sensor_id].begin(); itr != object_map_list_[sensor_id].end(); ++itr)
     {
 
@@ -729,9 +745,11 @@ namespace FHAC
       radar_track.position.y = itr->second.object_general.obj_distlat.data;
       radar_track.position.z = 0.0;
 
-      radar_track.velocity.x = itr->second.object_general.obj_vrellong.data;
-      radar_track.velocity.y = itr->second.object_general.obj_vrellat.data;
-      radar_track.velocity.z = 0.0;
+      geometry_msgs::msg::Vector3 radar_track_vel_raw;
+      radar_track_vel_raw.x = itr->second.object_general.obj_vrellong.data;
+      radar_track_vel_raw.y = itr->second.object_general.obj_vrellat.data;
+      radar_track_vel_raw.z = 0.0;
+      radar_track.velocity = radar_transforms::correctObstacleVelocity(corrected_odom, radar_track_vel_raw, radar_track.position);
 
       radar_track.acceleration.x = 0.0;
       radar_track.acceleration.y = 0.0;
@@ -1194,6 +1212,9 @@ namespace FHAC
 
   void radar_conti_ars408::odomCallback(const nav_msgs::msg::Odometry::SharedPtr msg)
   {
+
+    vehicle_odometry_ = *msg;
+
     // Threshold for standstill detection
     const double standstill_threshold = 0.01;