diff --git a/seat_car_controller/include/seat_car_controller.h b/seat_car_controller/include/seat_car_controller.h index 432e632d9f79d1d7fe45719d925ca111825e94a7..d8671452860c51081a9022507c206eac18f073c3 100644 --- a/seat_car_controller/include/seat_car_controller.h +++ b/seat_car_controller/include/seat_car_controller.h @@ -15,6 +15,7 @@ #include <ros/node_handle.h> #include <std_msgs/Int16.h> #include <std_msgs/UInt16.h> +#include <std_msgs/UInt8.h> #include <std_msgs/Float32.h> #include <geometry_msgs/Twist.h> #include <sensor_msgs/Imu.h> diff --git a/seat_car_controller/include/seat_car_controller_impl.h b/seat_car_controller/include/seat_car_controller_impl.h index a5cc39e6be0743b06b484e7ffeeb02932294e61c..67117828d89e4ff8e4f8eb3f6cf5cd47d21b8f30 100644 --- a/seat_car_controller/include/seat_car_controller_impl.h +++ b/seat_car_controller/include/seat_car_controller_impl.h @@ -254,7 +254,7 @@ namespace seat_car_controller this->yaw_pub = root_nh.advertise<std_msgs::Float32>(this->yaw_topic,1); this->twist_pub = root_nh.advertise<geometry_msgs::Twist>(this->twist_topic,1); - this->steer_angle_pub = root_nh.advertise<std_msgs::Int16>(this->steering_fb_topic,1); + this->steer_angle_pub = root_nh.advertise<std_msgs::UInt8>(this->steering_fb_topic,1); this->last_cmd_drive=0.0; this->last_cmd_steer=0.0; @@ -272,7 +272,7 @@ namespace seat_car_controller static double yaw=0.0; static double last_steer_angle=0.0; geometry_msgs::Twist twist_msg; - std_msgs::Int16 steer_angle_msg; + std_msgs::UInt8 steer_angle_msg; steer_l_pos=this->joints_[0].getPosition(); steer_r_pos=this->joints_[1].getPosition(); @@ -439,7 +439,7 @@ namespace seat_car_controller std_msgs::Float32 yaw_msg; this->heading=tf::getYaw(msg->orientation); - yaw_msg.data=-this->heading*180.0/3.14159; + yaw_msg.data=-this->heading; //*180.0/3.14159; this->yaw_pub.publish(yaw_msg); }