#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);

}