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_
评论 8
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值