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 #include #include +#include #include #include @@ -76,6 +77,7 @@ class ContinentalARS548HwInterfaceRosWrapper final : public rclcpp::Node, rclcpp::Publisher::SharedPtr packets_pub_; rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Subscription::SharedPtr acceleration_sub_; + rclcpp::Subscription::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 @@ + @@ -54,6 +55,7 @@ + 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( + "steering_angle_input", rclcpp::QoS{1}, + std::bind( + &ContinentalARS548HwInterfaceRosWrapper::SteeringAngleCallback, this, std::placeholders::_1)); + set_new_sensor_ip_service_server_ = this->create_service( "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 request, [[maybe_unused]] const std::shared_ptr response)