feat: added the missing steerin angle input

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

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

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)"/>

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)