Implemented AV state subscription and steering publishing

src/hardware/steering_actuator/include/component_steering_actuator.hpp

@@ -13,6 +13,7 @@
 #include "can_interface.hpp"
 #include "canopen.hpp"
 #include "driverless_common/common.hpp"
+#include "driverless_msgs/msg/av_state_stamped.hpp"
 #include "driverless_msgs/msg/can.hpp"
 #include "driverless_msgs/msg/state.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -138,10 +139,12 @@ class SteeringActuator : public rclcpp::Node, public CanInterface {
  rclcpp::Publisher<driverless_msgs::msg::Can>::SharedPtr can_pub_;
  rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr encoder_pub_;
  rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr steering_ready_pub_;
+ rclcpp::Publisher<std_msgs::msg::Int32>::SharedPtr steering_publisher_;
  rclcpp::Subscription<driverless_msgs::msg::State>::SharedPtr state_sub_;
  rclcpp::Subscription<ackermann_msgs::msg::AckermannDriveStamped>::SharedPtr ackermann_sub_;
  rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr steer_ang_sub_;
  rclcpp::Subscription<driverless_msgs::msg::Can>::SharedPtr canopen_sub_;
+ rclcpp::Subscription<driverless_msgs::msg::AVStateStamped>::SharedPtr av_state_subscriber_;
 
  rclcpp::CallbackGroup::SharedPtr sensor_cb_group_;
  rclcpp::CallbackGroup::SharedPtr control_cb_group_;
@@ -167,12 +170,15 @@ class SteeringActuator : public rclcpp::Node, public CanInterface {
  int32_t current_enc_revolutions_ = 0; // Current Encoder Revolutions (Stepper encoder)
  uint32_t current_velocity_;
  uint32_t current_acceleration_;
+ uint8_t current_mode_; // store current AV mode and state
+ uint8_t current_state_; // store current state
 
  // time to reset node if no state received
  std::chrono::time_point<std::chrono::system_clock> last_state_time = std::chrono::system_clock::now();
 
  void update_parameters(const rcl_interfaces::msg::ParameterEvent &event);
  void configure_c5e();
+ void av_state_callback(const driverless_msgs::msg::AVStateStamped::SharedPtr msg);
  void c5e_state_request_callback();
  void as_state_callback(const driverless_msgs::msg::State::SharedPtr msg);
  void steering_angle_callback(const std_msgs::msg::Float32::SharedPtr msg);

src/hardware/steering_actuator/src/component_steering_actuator.cpp

@@ -227,6 +227,31 @@ void SteeringActuator::read_steering_data(uint16_t obj_index) {
  can_pub_->publish(std::move(_d_2_f(id, 0, out, sizeof(out))));
 }
 
+// Callback function for AVState messages
+void SteeringActuator::av_state_callback(const driverless_msgs::msg::AVStateStamped::SharedPtr msg) {
+ current_mode_ = msg->mode;
+ current_state_ = msg->state;
+
+ RCLCPP_INFO(this->get_logger(), "AV State received: mode=%d, state=%d", current_mode_, current_state_);
+
+ // Decide what steering value to publish
+ std_msgs::msg::Int32 steering_msg;
+ if (current_mode_ == driverless_msgs::msg::AVStateStamped::AUTONOMOUS &&
+ current_state_ == driverless_msgs::msg::AVStateStamped::DRIVING) {
+ // Call the existing function to compute and send steering via CAN
+ // Assuming `current_velocity_` contains a relevant steering value that needs to be sent
+ this->send_steering_data(PROFILE_VELOCITY, (uint8_t *)&current_velocity_, 4);
+
+ // Publish the same steering value using ROS topic (as an Int32 msg)
+ steering_msg.data = static_cast<int32_t>(current_velocity_);
+ } else {
+ // Set steering to 0 (idle state) if not in driving mode
+ steering_msg.data = 0;
+ }
+
+ steering_publisher_->publish(steering_msg);
+}
+
 SteeringActuator::SteeringActuator(const rclcpp::NodeOptions &options) : Node("steering_actuator_node", options) {
  // Steering parameters
  this->declare_parameter<int>("velocity", 10000);
@@ -249,6 +274,13 @@ SteeringActuator::SteeringActuator(const rclcpp::NodeOptions &options) : Node("s
  auto control_cb_opt = rclcpp::SubscriptionOptions();
  control_cb_opt.callback_group = control_cb_group_;
 
+ // Subscriber to AV State
+ av_state_subscriber_ = this->create_subscription<driverless_msgs::msg::AVStateStamped>(
+ "system/av_state", 10, std::bind(&SteeringController::av_state_callback, this, std::placeholders::_1));
+
+ // Publisher for steering control
+ steering_publisher_ = this->create_publisher<std_msgs::msg::Int32>("steering_control", 10);
+
  // Create subscriber to topic "canbus_rosbound"
  canopen_sub_ = this->create_subscription<driverless_msgs::msg::Can>(
  "/can/canopen_rosbound", QOS_ALL, std::bind(&SteeringActuator::can_callback, this, _1), sensor_cb_opt);