diff --git a/src/yhs_can/yhs_can_control/include/yhs_can_control/yhs_can_control_node.hpp b/src/yhs_can/yhs_can_control/include/yhs_can_control/yhs_can_control_node.hpp index 734bfd8..8df73c5 100644 --- a/src/yhs_can/yhs_can_control/include/yhs_can_control/yhs_can_control_node.hpp +++ b/src/yhs_can/yhs_can_control/include/yhs_can_control/yhs_can_control_node.hpp @@ -58,6 +58,7 @@ private: rclcpp::Publisher::SharedPtr chassis_info_fb_publisher_; rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr steering_angle_pub_; void io_cmd_callback(const yhs_can_interfaces::msg::IoCmd::SharedPtr io_cmd_msg); @@ -75,4 +76,3 @@ private: #endif - diff --git a/src/yhs_can/yhs_can_control/src/yhs_can_control_node.cpp b/src/yhs_can/yhs_can_control/src/yhs_can_control_node.cpp index 5e6878d..eb02125 100644 --- a/src/yhs_can/yhs_can_control/src/yhs_can_control_node.cpp +++ b/src/yhs_can/yhs_can_control/src/yhs_can_control_node.cpp @@ -19,6 +19,7 @@ chassis_info_fb_publisher_ = node_->create_publisher("chassis_info_fb", 1); odom_pub_ = node_->create_publisher("odom", 1); + steering_angle_pub_ = node_->create_publisher("/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); }