int main(int argc, char **argv)
{
string port("/dev/ttyUSB0");//小车串口号
unsigned long baud = 115200;//小车串口波特率
serial::Serial my_serial(port, baud, serial::Timeout::simpleTimeout(1000));//配置串口
ros::init(argc, argv, "base_controller");//初始化串口节点
ros::NodeHandle n; //定义节点进程句柄
ros::Subscriber sub = n.subscribe("cmd_vel", 20, callback); //订阅/cmd_vel主题
ros::Publisher odom_pub= n.advertise<nav_msgs::Odometry>("odom", 20); //定义要发布/odom主题
static tf::TransformBroadcaster odom_broadcaster;//定义tf对象
geometry_msgs::TransformStamped odom_trans;//创建一个tf发布需要使用的TransformStamped类型消息
nav_msgs::Odometry odom;//定义里程计对象
geometry_msgs::Quaternion odom_quat; //四元数变量
//定义covariance矩阵，作用为解决文职和速度的不同测量的不确定性
float covariance[36] = {0.01, 0, 0, 0, 0, 0, // covariance on gps_x
0, 0.01, 0, 0, 0, 0, // covariance on gps_y
0, 0, 99999, 0, 0, 0, // covariance on gps_z
0, 0, 0, 99999, 0, 0, // large covariance on rot x
0, 0, 0, 0, 99999, 0, // large covariance on rot y
0, 0, 0, 0, 0, 0.01}; // large covariance on rot z
//载入covariance矩阵
for(int i = 0; i < 36; i++)
{
odom.pose.covariance[i] = covariance[i];;
}
ros::Rate loop_rate(10);//设置周期休眠时间
while(ros::ok())
{
rec_buffer =my_serial.readline(25,"\n"); //获取串口发送来的数据
const char *receive_data=rec_buffer.data(); //保存串口发送来的数据
if(rec_buffer.length()==21) //串口接收的数据长度正确就处理并发布里程计数据消息
{
for(int i=0;i<4;i++)//提取X，Y坐标，方向，线速度，角速度
{
position_x.data[i]=receive_data[i];
position_y.data[i]=receive_data[i+4];
oriention.data[i]=receive_data[i+8];
vel_linear.data[i]=receive_data[i+12];
vel_angular.data[i]=receive_data[i+16];
}
//将X，Y坐标，线速度缩小1000倍
position_x.d/=1000; //m
position_y.d/=1000; //m
vel_linear.d/=1000; //m/s
//里程计的偏航角需要转换成四元数才能发布
odom_quat = tf::createQuaternionMsgFromYaw(oriention.d);//将偏航角转换成四元数
//载入坐标（tf）变换时间戳
odom_trans.header.stamp = ros::Time::now();
//发布坐标变换的父子坐标系
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_footprint";
//tf位置数据：x,y,z,方向
odom_trans.transform.translation.x = position_x.d;
odom_trans.transform.translation.y = position_y.d;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//发布tf坐标变化
odom_broadcaster.sendTransform(odom_trans);
//载入里程计时间戳
odom.header.stamp = ros::Time::now();
//里程计的父子坐标系
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
//里程计位置数据：x,y,z,方向
odom.pose.pose.position.x = position_x.d;
odom.pose.pose.position.y = position_y.d;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
//载入线速度和角速度
odom.twist.twist.linear.x = vel_linear.d;
//odom.twist.twist.linear.y = odom_vy;
odom.twist.twist.angular.z = vel_angular.d;
//发布里程计
odom_pub.publish(odom);
ros::spinOnce();//周期执行
loop_rate.sleep();//周期休眠
}
//程序周期性调用
//ros::spinOnce(); //callback函数必须处理所有问题时，才可以用到
}
return 0;
}
|
楼主
标题置顶
标题高亮
