#include <tf2/LinearMath/Quaternion.h> //增加头文件
#include <tf2_ros/transform_broadcaster.h> //增加头文件
/***************************************************************************/
/* 解析udp接收数据函数 */
/***************************************************************************/
void ChassisBridge::publishOdom()
{
int32_t rpm_left = (ros::Time::now().toSec() - GCAN_Driver.motor_info_.receive_time < 1) ? -GCAN_Driver.motor_info_.temp_left : 0;
int32_t rpm_right = (ros::Time::now().toSec() - GCAN_Driver.motor_info_.receive_time < 1) ? -GCAN_Driver.motor_info_.temp_right : 0;
double speed_left = (double)(rpm_left * 0.00000031958);
double speed_right = -(double)(rpm_right * 0.00000031958);
g_odom_info.speed = (double)(speed_left + speed_right) / 2.0;
g_odom_info.angular = (double)(speed_right - speed_left) / track_width_;
g_odom_info.new_get_rpm_time = ros::Time::now().toSec();
double delta_time = ros::Time::now().toSec() - g_odom_info.old_get_rpm_time;
g_odom_info.old_get_rpm_time = g_odom_info.new_get_rpm_time;
g_odom_info.mileage_angle = g_odom_info.mileage_angle + g_odom_info.angular * delta_time;
// 分解之后的位移速度,这个分解与航向角有关
double vel_x = g_odom_info.speed * cos(g_odom_info.mileage_angle);
double vel_y = g_odom_info.speed * sin(g_odom_info.mileage_angle);
g_odom_info.mileage = g_odom_info.mileage + g_odom_info.speed * delta_time;
g_odom_info.mileage_x = g_odom_info.mileage_x + vel_x * delta_time;
g_odom_info.mileage_y = g_odom_info.mileage_y + vel_y * delta_time;
cr_common::Chassis_odometry chassis_odometry_msg;
chassis_odometry_msg.header.stamp = ros::Time::now();
chassis_odometry_msg.header.frame_id = "odom";
chassis_odometry_msg.linear_vel = g_odom_info.speed;
chassis_odometry_msg.linear_y = 0;
chassis_odometry_msg.angular_rotate = g_odom_info.angular;
chassis_odometry_msg.odometry = g_odom_info.mileage;
chassis_odometry_msg.odometry_y = 0;
chassis_odometry_msg.odometry_rotate = g_odom_info.angular;
wheel_car_like_pub_.publish(chassis_odometry_msg);
nav_msgs::Odometry chassis_odom_msg;
geometry_msgs::Quaternion drobot_odom_quat = tf::createQuaternionMsgFromYaw(g_odom_info.mileage_angle); //删除本行,用下面两行代替
tf2::Quaternion q; //增加
q.setRPY(0,0,g_odom_info.mileage_angle) //增加
chassis_odom_msg.header.stamp = ros::Time::now();
chassis_odom_msg.header.frame_id = "odom";
chassis_odom_msg.child_frame_id = "base_link";
chassis_odom_msg.twist.twist.linear.x = g_odom_info.speed;
chassis_odom_msg.twist.twist.linear.y = 0;
chassis_odom_msg.twist.twist.angular.z = g_odom_info.angular;
chassis_odom_msg.twist.covariance[0] = chassis_odom_covariance_0_;
chassis_odom_msg.twist.covariance[7] = chassis_odom_covariance_7_;
chassis_odom_msg.twist.covariance[14] = chassis_odom_covariance_14_;
chassis_odom_msg.twist.covariance[35] = chassis_odom_covariance_35_;
chassis_odom_msg.pose.pose.position.x = g_odom_info.mileage_x;
chassis_odom_msg.pose.pose.position.y = g_odom_info.mileage_y;
chassis_odom_msg.pose.pose.position.z = 0.0;
chassis_odom_msg.pose.pose.orientation = drobot_odom_quat; //删除本行,用下面四行代替
chassis_odom_msg.pose.pose.orientation.x = q.x(); //增加
chassis_odom_msg.pose.pose.orientation.y = q.y(); //增加
chassis_odom_msg.pose.pose.orientation.z = q.z(); //增加
chassis_odom_msg.pose.pose.orientation.w = q.w(); //增加
chassis_odom_pub_.publish(chassis_odom_msg);
}