feat(nav2): publish mkmini steering feedback

src/yhs_can/yhs_can_control/include/yhs_can_control/yhs_can_control_node.hpp

@@ -58,6 +58,7 @@ private:
   
   rclcpp::Publisher<yhs_can_interfaces::msg::ChassisInfoFb>::SharedPtr chassis_info_fb_publisher_;
   rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub_;
+  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr steering_angle_pub_;
   
   void io_cmd_callback(const yhs_can_interfaces::msg::IoCmd::SharedPtr io_cmd_msg);
   
@@ -75,4 +76,3 @@ private:
 
 
 #endif
-

src/yhs_can/yhs_can_control/src/yhs_can_control_node.cpp

@@ -19,6 +19,7 @@
 		chassis_info_fb_publisher_ = node_->create_publisher<yhs_can_interfaces::msg::ChassisInfoFb>("chassis_info_fb", 1);
 		
 		odom_pub_ = node_->create_publisher<nav_msgs::msg::Odometry>("odom", 1);
+		steering_angle_pub_ = node_->create_publisher<std_msgs::msg::Float64>("/mkmini/steering_angle", 1);
 	}
 
 	void CanControl::io_cmd_callback(const yhs_can_interfaces::msg::IoCmd::SharedPtr io_cmd_msg)
@@ -172,6 +173,9 @@ whether the can line is connected correctly, and whether the chassis is powered
 							chassis_info_msg.header.stamp = node_->get_clock()->now();
 							chassis_info_msg.ctrl_fb = msg;
 							chassis_info_fb_publisher_->publish(chassis_info_msg);
+							std_msgs::msg::Float64 steering_angle;
+							steering_angle.data = msg.ctrl_fb_steering / 180.0 * M_PI;
+							steering_angle_pub_->publish(steering_angle);
 							if(msg.ctrl_fb_gear == 2) msg.ctrl_fb_velocity = -msg.ctrl_fb_velocity;
 							publish_odom(msg.ctrl_fb_velocity, msg.ctrl_fb_steering/180*3.1415);
 						}