ROS学习笔记-编码器的脉冲数据转化成ROS的odom数据

int main(int argc, char **argv)
{
	ros::init(argc, argv, "Odom"); //初始化一个节点
	ros::NodeHandle n;
	cm.canOpenCom();    //驱动协议用的是canopen
	cm.Power(2, true); //启动左电机
	cm.Power(1, true); //启动右电机

	ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50); //发布odom话题
	ros::Duration(1.0).sleep();    //等待话题发布完成
	tf::TransformBroadcaster odom_broadcaster;

	// base_footprint与odom坐标系的偏差
	double x = 0.0;
	double y = 0.0;
	double th = 0.0;

	u_int32_t right_pulse = QueryCan(1, 0x60630020, &cm);	   //读取编码器
	u_int32_t last_right_pulse = QueryCan(1, 0x60630020, &cm); //读取编码器
	u_int32_t left_pulse = QueryCan(2, 0x60630020, &cm);	   //读取编码器
	u_int32_t last_left_pulse = QueryCan(2, 0x60630020, &cm); //读取编码器
	double radius = 0.075;									   //轮子的半径
	double wheel_track = 0.588;								   //两轮之间的距离

	ros::Time current_time, last_time;
	current_time = ros::Time::now();
	last_time = ros::Time::now();

	ros::Rate r(20); //发布频率
	while (n.ok())
	{

		ros::spinOnce(); // check for incoming messages
		current_time = ros::Time::now();
		double ctime = current_time.toSec();
		double dt = (current_time - last_time).toSec();
		right_pulse = QueryCan(1, 0x60630020, &cm); //读取编码器
		left_pulse = QueryCan(2, 0x60630020, &cm); //读取编码器

		int32_t delta_right_pulse = (int32_t)right_pulse - (int32_t)last_right_pulse;
		int32_t delta_left_pulse = (int32_t)left_pulse - (int32_t)last_left_pulse;

		double dright = ((double)delta_right_pulse / 10000) * 2 * pi * radius; //右轮单位时间内走的距离 10000脉冲为一圈
		double dleft = ((double)delta_left_pulse / 10000) * 2 * pi * radius;	//左轮单位时间内走的距离

		last_right_pulse = right_pulse;
		last_left_pulse = left_pulse;

		double dxy_ave = (dright + dleft) / 2;		 //中心点移动距离
		double dth = (dright - dleft) / wheel_track; //中心点转动角度

		double vxy = dxy_ave / dt;		 //线速度
		double vth = dth / dt;			 //角速度

		double delta_x = dxy_ave * cos(th + (dth / 2.0));
		double delta_y = dxy_ave * sin(th + (dth / 2.0));
		double delta_th = dth;

		x += delta_x; // base_footprint与odom坐标系的偏差
		y += delta_y;
		th += delta_th;

		geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);    //转换成四元数

		geometry_msgs::TransformStamped odom_trans;
		odom_trans.header.stamp = current_time;
		odom_trans.header.frame_id = "odom";
		odom_trans.child_frame_id = "base_footprint";

		odom_trans.transform.translation.x = x;
		odom_trans.transform.translation.y = y;
		odom_trans.transform.translation.z = 0.0;
		odom_trans.transform.rotation = odom_quat;

		odom_broadcaster.sendTransform(odom_trans);    //发布odom和base_footprint的tf数

		nav_msgs::Odometry odom;
		odom.header.stamp = current_time;
		odom.header.frame_id = "odom";

		odom.pose.pose.position.x = x;
		odom.pose.pose.position.y = y;
		odom.pose.pose.position.z = 0.0;
		odom.pose.pose.orientation = odom_quat;

		odom.child_frame_id = "base_footprint";
		odom.twist.twist.linear.x = vxy;    //两轮差动小车只有线速度x
		odom.twist.twist.linear.y = 0;
		odom.twist.twist.angular.z = vth;

		odom_pub.publish(odom);    //发布odom数据

		last_time = current_time;
		r.sleep();
	}
	ros::spin();
	return 0;
}

参考资料：

https://github.com/ROBOTIS-GIT/OpenCR/blob/master/arduino/opencr_arduino/opencr/libraries/turtlebot3/examples/turtlebot3_burger/turtlebot3_core/turtlebot3_core.ino

https://blog.csdn.net/SimileciWH/article/details/84976187