ROS 之 odom 里程计串口通讯stm32及解析

学习ROS也有一个月了,起初看了重德智能的一些视频,然后又看了古月老师的9讲视频(都在B站找的),边看跟着边操作,但是看完之后也忘得差不多了。现在写个里程计的程序,都扭扭捏捏下不了手。终于经过几天的软磨硬泡,写了这一段,在此仅作为记录学习的过程。如果有错误和不足,也希望大家帮忙指出,提出自己的意见,共同进步吧。

 

首先,还没有写 launch 文件,就直接从main 函数开始记录。

//接收其他节点的速度和角度
double Robot_Speed = 0;
double Robot_Angle = 0;

void cmdCallback(const geometry_msgs::Twist& msg)
{
    //以线速度的X方向的速度表示整体车行速度
    Robot_Speed = msg.linear.x;
    //以角速度的Z方向的速度表示整体车行角度
    Robot_Angle = msg.angular.z;
}

int main(int argc, char** argv)
{
    //初始化ROS节点
    ros::init(argc, argv, "comm_bringup");
    //创建节点句柄
    ros::NodeHandle nh;

    //初始化comm_odom
    comm_odom::comm_odom car;
    if(!car.init())
        ROS_ERROR("comm odom initialized failed.");
    ROS_INFO("comm odom initialized successful.");

    ros::Subscriber sub = nh.subscribe("cmd_vel", 50, cmdCallback);

    //循环运行
    ros::Rate loop_rate(50);
    while (ros::ok())
    {
        ros::spinOnce();

        //机器人控制
        car.spinOnce(Robot_Speed, Robot_Angle);

        loop_rate.sleep();
    }

    return 0;
}

创建了一个新的 命名空间 comm_odom::comm_odom car; 这个 car 可以从头文件里面去查看具体的定义。

然后看到 car.init() 这个函数如下,就是初始化 串口的参数,这个串口用的 boost ,好像还有个用 serial 的包,不知道这两个是什么区别。暂时先跟着用的boost 的串口。代码如下,

bool comm_odom::init()
{
	//串口连接
	sp.set_option(boost::asio::serial_port::baud_rate(115200));
	sp.set_option(boost::asio::serial_port::flow_control(boost::asio::serial_port::flow_control::none));
	sp.set_option(boost::asio::serial_port::parity(boost::asio::serial_port::parity::none));
	sp.set_option(boost::asio::serial_port::stop_bits(boost::asio::serial_port::stop_bits::one));
	sp.set_option(boost::asio::serial_port::character_size(8));

	ros::Time::init();
	current_time_ = ros::Time::now();
	last_time_ = ros::Time::now();

	//定义发布消息的名称
	pub_ = nh.advertise<nav_msgs::Odometry>("odom", 50);

	return true;
}

其中,先全局定义了串口的设备

//串口准备
boost::asio::io_service iosev;
boost::asio::serial_port sp(iosev, "/dev/ttyUSB0");

如果检测到了串口并连接上了,就进入下面的循环了。循环主要就是处理串口的一些数据了,包括串口发送速度,接收速度解析等。进入下面这个函数。

//机器人控制
        car.spinOnce(Robot_Speed, Robot_Angle);
//机器人控制 通信
bool comm_odom::spinOnce(double Robot_S, double Robot_A)
{
	//下发机器人期望速度
	writeSpeed(Robot_S, Robot_A);

	//读取机器人实际速度
	readSpeed();

	current_time_ = ros::Time::now();

	//发布TF
	geometry_msgs::TransformStamped odom_trans;
	odom_trans.header.stamp = current_time_;
	odom_trans.header.frame_id = "odom";
	odom_trans.child_frame_id = "base_footprint";

	geometry_msgs::Quaternion odom_quat;
	odom_quat = tf::createQuaternionMsgFromYaw(th_);
	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);

	//发布里程计消息
	nav_msgs::Odometry msgl;
	msgl.header.stamp = current_time_;
	msgl.header.frame_id = "odom";

	msgl.pose.pose.position.x = x_;
	msgl.pose.pose.position.y = y_;
	msgl.pose.pose.position.z = 0.0;
	msgl.pose.pose.orientation = odom_quat;
	msgl.pose.covariance = odom_pose_covariance;

	msgl.child_frame_id = "base_footprint";
	msgl.twist.twist.linear.x = vx_;
	msgl.twist.twist.linear.y = vy_;
	msgl.twist.twist.angular.z = vth_;
	msgl.twist.covariance = odom_twist_covariance;

	//发布
	pub_.publish(msgl);
}

这个函数的实现就这几步,下发速度给stm32,接收stm32的编码器反馈的速度,然后发布TF ,发布里程计消息。

好像从哪里看到过,里程计的消息发布给slam算法,然后slam经过算法的处理,地图构建啥的。还不知道到底给 slam 了,它要怎么用。TF 就不知道是干嘛的,应该是少不了。应该是关于坐标系的。有点蒙圈。

先看前面的下发机器人速度

void comm_odom::writeSpeed(double Robot_S, double Robot_A)
{
	unsigned char buf[16] = {0};
	int i, length = 0;

	speed_data.d = Robot_S;
	angle_data.d = Robot_A;

	// 设置消息头
	for(i = 0; i < 2; i++)
		buf[i] = header[i];

	// 设置消息类型 联合体分离高低字节数据
	send_command.d = GET_SPEED;
	for (i = 0; i < 2; i++)
		buf[i + 2] = send_command.data[i];

	//设置机器人的速度 和角度
	length = 8;
	buf[4] = length;
	for (i = 0; i < 4; i++)
	{
		buf[i + 5] = speed_data.data[i];
		buf[i + 9] = angle_data.data[i];
	}

	// 设置校验值、消息尾
	buf[5 + length] = getCrc8(buf, 5 + length);
	buf[6 + length] = ender[0];
	buf[7 + length] = ender[1];

	// 通过串口下发数据
	boost::asio::write(sp, boost::asio::buffer(buf));
}

这个协议还是比较清晰的,只是我定义的是发送速度和角度,没有经过计算,计算放到了stm32下面处理,然后就是消息头,消息类型,速度角度,校验,消息尾。将数据整在一起,就通过串口下发下去了。

这个速度角度怎么来的呢? 估计是从slam 那来的吧,肯定需要一段指令,比如是按地图上的路线进行运行,然后slam通过下发速度角度,获取反馈,实时的更新不同的指令。当然这是猜测,具体是啥,也不造。我的目标就是这样。

再来到读取串口数据

//读取速度
bool comm_odom::readSpeed()
{
	int i, length = 0;
	unsigned char checkSum;
	unsigned char buf[200];
	//当前瞬时的速度和 角角度
	float cur_speed, cur_vangle;

	//读取串口数据
	boost::asio::read(sp, boost::asio::buffer(buf));
	ros::Time curr_time;

	for(i = 0; i < 2; i++)
		recv_header.data[i] = buf[i];

	//检查信息头
	if (recv_header.data[0] != header[0] || recv_header.data[1] != header[1])
	{
		ROS_ERROR("Recv message header error! ");
		return false;
	}

	for(i = 0; i < 2; i++)
		recv_command.data[i] = buf[i + 2];

	length = buf[4];
	checkSum = getCrc8(buf, 5 + length);

	//检查信息类型
	if (recv_command.d != SPEED_INFO)
	{
		ROS_ERROR("Recv command type error! ");
		return false;
	}

	//检查信息尾
	if (buf[6 + length] != ender[0] || buf[6 + length + 1] != ender[1])
	{
		ROS_ERROR("Recv command ender error! ");
		return false;
	}

	//检查校验值
	RecvCheckSum.data[0] = buf[5 + length];
	if (checkSum != RecvCheckSum.d)
	{
		ROS_ERROR("Recv data check sum error! ");
		return false;
	}

	//读取速度值, 角度值
	for(i = 0; i < 4; i++)
	{
		vel_front_left.odometry_char[i] = buf[i + 5];
		vel_front_right.odometry_char[i]= buf[i + 9];
		vel_back_left.odometry_char[i]  = buf[i + 13];
		vel_back_right.odometry_char[i]	= buf[i + 17];

		ang_front_left.odometry_char[i] = buf[i + 21];
		ang_front_right.odometry_char[i]= buf[i + 25];
		ang_back_left.odometry_char[i]  = buf[i + 29];
		ang_back_right.odometry_char[i]	= buf[i + 33];
	}

	//融合四个轮子数据, 得到车子整体的线速度 和 角速度,将车子看成整体
	speedfusion(&cur_speed, &cur_vangle);

	//当前小车的线速度 角速度
	vx_ = cur_speed;
	vth_ = cur_vangle;

	curr_time = ros::Time::now();

	double dt = (curr_time - last_time_).toSec();
	double delta_x = (vx_ * cos(th_) - vy_ * sin(th_)) * dt;
	double delta_y = (vx_ * sin(th_) + vy_ * cos(th_)) * dt;
	double delta_th = vth_ * dt;

	x_ += delta_x;
	y_ += delta_y;
	th_ += delta_th;
	last_time_ = curr_time;

	return true;
}

这个也就是通过协议,进行解析,我这儿的是四个轮子行走,加四个转向,所以需要定义四个速度和角度,然后网上几乎都是两个差速轮,我这四个轮八个电机,于是就写了个速度融合,把所有轮速的速度角度换算到中心位置,然后进行平均,理想状态就是换算到中心时,速度都是差不多的。

但是实际情况可能会出现,内偏磨损或外偏磨损的情况,这个要分析受力,结构等等太麻烦了,就不管了,直接先平均,勉强能消除一部分误差。后面可以根据实际情况,分析具体更接近内轮还是外轮的速度。

融合程序就是下面这个,根据阿克曼推导过来的,经过验证,能满足简单需求,后续可能完善更多情况等。

void comm_odom::speedfusion(float *speed, float *vangle)
{
	//四轮对应到中间质点的 角度
	float th_front_left, th_front_right;//, th_back_left, th_back_right;
	//四轮对应到中间质点的 速度
	float v_front_left, v_front_right, v_back_left, v_back_right;

	//将四个轮子的速度角度转化为 车体本身的速度角度 积分时将车体看成质点
	if( (ang_front_left.odometry_float < -2.0f && ang_front_right.odometry_float > 2.0f)
	 || (ang_front_left.odometry_float > 2.0f && ang_front_right.odometry_float < -2.0f))
	{
		 ROS_ERROR("Recv wheel angle error! angle = (%.3f, %.3f, %.3f, %.3f)", ang_front_left.odometry_float
		 , ang_front_right.odometry_float, ang_back_left.odometry_float,ang_back_right.odometry_float);
	}
	else
	{
		//左偏为正,当设定的角度为45度时,轮子产生最大角度为55度。
		if(ang_front_left.odometry_float > 1.0f && ang_front_left.odometry_float < 55.0f)
		{
			//左轮角度α 右轮角度β 中间模拟中间质点角度θ 轴长L 轴宽W
			//θ_left = atan(L / (L/tanα + W/2) / 2)
			th_front_left = atan(CAR_AXIS_LENGTH / (CAR_AXIS_LENGTH / tan(ang_front_left.odometry_float * ANGLE_TO_RADIAN) + CAR_AXIS_WIDE * 0.5f) * 0.5f);
			//v = (v1 * sinα ) / (2 * sinθ)
			v_front_left = (vel_front_left.odometry_float * sin(ang_front_left.odometry_float * ANGLE_TO_RADIAN)) / (2 * sin(th_front_left));

			//θ_rigth = atan(L / (2 * (L / tanβ) - W))
			th_front_right = atan(CAR_AXIS_LENGTH / (2 * (CAR_AXIS_LENGTH / tan(ang_front_right.odometry_float * ANGLE_TO_RADIAN)) - CAR_AXIS_WIDE) );
			//v = (v2 * sinβ) / (2 * sinθ)
			v_front_right = (vel_front_right.odometry_float * sin(ang_front_right.odometry_float * ANGLE_TO_RADIAN)) / (2 * sin(th_front_right));

			if( (ang_back_left.odometry_float > 2.0f && ang_back_left.odometry_float < 55.0f) ||
				(ang_back_left.odometry_float < -2.0f && ang_back_left.odometry_float > -55.0f) )
			{
				ROS_INFO("Back wheel angle error! angle = (%.3f, %.3f, %.3f, %.3f)", ang_front_left.odometry_float
		 		, ang_front_right.odometry_float, ang_back_left.odometry_float,ang_back_right.odometry_float);
			}
			
			//v = (v3 * L / 2) / (sinθ * (L / 2 / tanθ - W / 2) )
			v_back_left = (vel_back_left.odometry_float * CAR_AXIS_LENGTH * 0.5f) / (sin((th_front_left + th_front_right) * 0.5f) * 
							(CAR_AXIS_LENGTH / 2 / tan((th_front_left + th_front_right) * 0.5f) - CAR_AXIS_WIDE * 0.5f));
			//v = (vv * L / 2) / (sinθ * (L / 2 / tanθ + W / 2) )
			v_back_right = (vel_back_right.odometry_float * CAR_AXIS_LENGTH * 0.5f) / (sin((th_front_left + th_front_right) * 0.5f) * 
							(CAR_AXIS_LENGTH * 0.5f / tan((th_front_left + th_front_right) * 0.5f) + CAR_AXIS_WIDE * 0.5f));
		}
		//右偏为正
		else if(ang_front_left.odometry_float < -1.0f && ang_front_left.odometry_float > -55.0f)
		{
			float left_angle =  fabs(ang_front_left.odometry_float) * ANGLE_TO_RADIAN;
			float right_angle = fabs(ang_front_right.odometry_float) * ANGLE_TO_RADIAN;
			
			//左轮角度α 右轮角度β 中间模拟中间质点角度θ 轴长L 轴宽W
			//θ_left = atan(L / (2 * (L / tanβ) - W))
			th_front_left = atan(CAR_AXIS_LENGTH / (2 * (CAR_AXIS_LENGTH / tan(left_angle)) - CAR_AXIS_WIDE) );
			//v = (v1 * sinβ) / (2 * sinθ)
			v_front_left = (vel_front_left.odometry_float * sin(left_angle)) / (2 * sin(th_front_left));

			//θ_rigth = atan(L / (L/tanα + W/2) / 2)
			th_front_right = atan(CAR_AXIS_LENGTH / (CAR_AXIS_LENGTH / tan(right_angle) + CAR_AXIS_WIDE * 0.5f) * 0.5f);
			//v = (v2 * sinα ) / (2 * sinθ)
			v_front_right = (vel_front_right.odometry_float * sin(right_angle)) / (2 * sin(th_front_right));

			if( (ang_back_left.odometry_float > 2.0f && ang_back_left.odometry_float < 55.0f) ||
				(ang_back_left.odometry_float < -2.0f && ang_back_left.odometry_float > -55.0f) )
			{
				ROS_INFO("Back wheel angle error! angle = (%.3f, %.3f, %.3f, %.3f)", ang_front_left.odometry_float
		 		, ang_front_right.odometry_float, ang_back_left.odometry_float,ang_back_right.odometry_float);
			}
			
			//v = (v3 * L / 2) / (sinθ * (L / 2 / tanθ + W / 2) )
			v_back_left = (vel_back_left.odometry_float * CAR_AXIS_LENGTH * 0.5f) / (sin((th_front_left + th_front_right) * 0.5f) * 
							(CAR_AXIS_LENGTH * 0.5f / tan((th_front_left + th_front_right) * 0.5f) + CAR_AXIS_WIDE * 0.5f));
			//v = (vv * L / 2) / (sinθ * (L / 2 / tanθ - W / 2) )
			v_back_right = (vel_back_right.odometry_float * CAR_AXIS_LENGTH * 0.5f) / (sin((th_front_left + th_front_right) * 0.5f) * 
							(CAR_AXIS_LENGTH * 0.5f / tan((th_front_left + th_front_right) * 0.5f) - CAR_AXIS_WIDE * 0.5f));
		}
		//不偏,角度都为0
		else
		{
			th_front_left = 0.0f;
			th_front_right = 0.0f;

			v_front_left  = vel_front_left.odometry_float;
			v_front_right = vel_front_right.odometry_float;
			v_back_left   = vel_back_left.odometry_float;
			v_back_right  = vel_back_right.odometry_float;
		}

		*speed = (v_front_left + v_front_right + v_back_left + v_back_right) * 0.25f;
		//得到的角速度 w = v / r = v * 2 * sinθ / L ;
		*vangle = *speed * 2 * sin((th_front_left + th_front_right) * 0.5f) / CAR_AXIS_LENGTH;
	}
}

四个轮子,转弯暂时以前两轮摆角度,后两轮角度不变,四轮速度变化这种形式转弯。以后会添加更多形式转弯。现在先不管,动起来再说。

这个程序也就不做分析了,基本上了解阿克曼原理,就能写出来。

最后返回的速度 和 角速度。角速度,这个划下划线,差点就搞错了。

然后将这两个参数带入积分运算模型中,积分就行了,积分这里网上也一大堆,也都差不多。

得到的这些数据,就直接传递给了 里程计 这个话题,这个话题还有一个不太明白。

msgl.pose.covariance = odom_pose_covariance;

msgl.twist.covariance = odom_twist_covariance;

这两个协方差矩阵具体是怎么定义的,还需要进一步考虑。

将两个cpp文件 和 一个 h 文件放在资源里面,有需要自提。

网盘链接:

链接:https://pan.baidu.com/s/1zUgY3US287KXOQyAWtiYmg 
提取码:spqv 
复制这段内容后打开百度网盘手机App,操作更方便哦

已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页