how to get the linear velocity in the robot heading direction with gazebo/link_states?

edit

hi! Thanks for you detail explanation! the code that I try to get the robot heading is as below

void localizationStateCallback(const nav_msgs::Odometry::ConstPtr& msg) { cur_pose_.x = msg->pose.pose.position.x; cur_pose_.y = msg->pose.pose.position.y; cur_pose_.yaw = tf::getYaw(msg->pose.pose.orientation); cur_pose_.v = sqrt(msg->twist.twist.linear.x * msg->twist.twist.linear.x + msg->twist.twist.linear.y * msg->twist.twist.linear.y); // this way would loose the information about the forward or reverse of robot cur_pose_.w = msg->twist.twist.angular.z; }

so like I said, I try to get the heading velocity with the twist.linear.x / cos(yaw), this way can have the information about the robot is driving forward or backward. Unfortunately, when the robot is movinfg, the log shows that the velocity is correct at some point, like the max velocity shold be less than 3m/s, but the log shows the velocity

I try to look into the yaw and the twist.linear.x , twist.linear.y, and compute the heading velocity by calculator..., indeed, the result is the same the one in the log. So I am confused about where is the problem?

by the way, the /odom message comes from directly from /gazebo/link_states, I write a python scrupt to read the base_link states and publsh it in the /odom topic. I think the data coming from gazebo/link_states should be the accurate pose of robot. looking forward to your reply! ^ ^