From 2506fb7ec8c080db9e4dfc5b7c7c50eda58b794b Mon Sep 17 00:00:00 2001
From: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
Date: Wed, 24 Jan 2024 13:35:38 +0900
Subject: [PATCH] feat: added the missing steerin angle input

Signed-off-by: Kenzo Lobos-Tsunekawa <kenzo.lobos@tier4.jp>
---
 .../continental_ars548_hw_interface_ros_wrapper.hpp  |  6 ++++++
 nebula_ros/launch/continental_launch_all_hw.xml      |  2 ++
 .../continental_ars548_hw_interface_ros_wrapper.cpp  | 12 ++++++++++++
 3 files changed, 20 insertions(+)

diff --git a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp
index a7f42b87c..ed2ed3964 100644
--- a/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp
+++ b/nebula_ros/include/nebula_ros/continental/continental_ars548_hw_interface_ros_wrapper.hpp
@@ -30,6 +30,7 @@
 #include <nebula_msgs/msg/nebula_packet.hpp>
 #include <nebula_msgs/msg/nebula_packets.hpp>
 #include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/float32.hpp>
 #include <std_srvs/srv/empty.hpp>
 
 #include <boost/asio.hpp>
@@ -76,6 +77,7 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node,
   rclcpp::Publisher<nebula_msgs::msg::NebulaPackets>::SharedPtr packets_pub_;
   rclcpp::Subscription<geometry_msgs::msg::TwistWithCovarianceStamped>::SharedPtr odometry_sub_;
   rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr acceleration_sub_;
+  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr steering_angle_sub_;
 
   bool standstill_{true};
 
@@ -101,6 +103,10 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node,
   /// @param msg The acceleration message
   void AccelerationCallback(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg);
 
+  /// @brief Callback to send the steering angle information to the radar device
+  /// @param msg The steering angle message
+  void SteeringAngleCallback(const std_msgs::msg::Float32::SharedPtr msg);
+
   /// @brief Service callback to set the new sensor ip
   /// @param request Empty service request
   /// @param response Empty service response
diff --git a/nebula_ros/launch/continental_launch_all_hw.xml b/nebula_ros/launch/continental_launch_all_hw.xml
index 126340217..a0f6caff0 100644
--- a/nebula_ros/launch/continental_launch_all_hw.xml
+++ b/nebula_ros/launch/continental_launch_all_hw.xml
@@ -5,6 +5,7 @@
 
     <arg name="odometry_topic" default="odometry_input"/>
     <arg name="acceleration_topic"  default="acceleration_input"/>
+    <arg name="steering_angle_topic"  default="steering_angle_input"/>
 
     <arg name="sensor_ip" default="10.13.1.114" description="Radar Sensor IP"/>
     <arg name="new_sensor_ip" default="10.13.1.113" description="New Radar Sensor IP"/>
@@ -54,6 +55,7 @@
         <remap from="/diagnostics" to="diagnostics"/>
         <remap from="odometry_input" to="$(var odometry_topic)"/>
         <remap from="acceleration_input" to="$(var acceleration_topic)"/>
+        <remap from="steering_angle_input" to="$(var steering_angle_topic)"/>
 
         <param name="sensor_model" value="$(var sensor_model)"/>
         <param name="frame_id" value="$(var frame_id)"/>
diff --git a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp
index afa3d85b0..0d7da394b 100644
--- a/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp
+++ b/nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp
@@ -63,6 +63,11 @@ ContinentalARS548HwInterfaceRosWrapper::ContinentalARS548HwInterfaceRosWrapper(
     std::bind(
       &ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback, this, std::placeholders::_1));
 
+  steering_angle_sub_ = create_subscription<std_msgs::msg::Float32>(
+    "steering_angle_input", rclcpp::QoS{1},
+    std::bind(
+      &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, std::placeholders::_1));
+
   set_new_sensor_ip_service_server_ = this->create_service<std_srvs::srv::Empty>(
     "set_new_sensor_ip", std::bind(
                            &ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback,
@@ -429,6 +434,13 @@ void ContinentalARS548HwInterfaceRosWrapper::AccelerationCallback(
   hw_interface_.SetAccelerationLongitudinalCog(msg->accel.accel.linear.x);
 }
 
+void ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback(
+  const std_msgs::msg::Float32::SharedPtr msg)
+{
+  std::scoped_lock lock(mtx_config_);
+  hw_interface_.SetSteeringAngleFrontAxle(msg->data);
+}
+
 void ContinentalARS548HwInterfaceRosWrapper::SetNewSensorIPRequestCallback(
   [[maybe_unused]] const std::shared_ptr<std_srvs::srv::Empty::Request> request,
   [[maybe_unused]] const std::shared_ptr<std_srvs::srv::Empty::Response> response)