教程三 底盘控制教程

来自Waveshare Wiki
跳转至: 导航搜索

ugv02使用教程

功能包简介

ugv02通过树莓派朝串口发送json消息{"T":13,"X":0.25,"Z":0.3}对机器人进行移动控制。
(T=13为线速度角速度控制指令,X和Z分别是控制机器人移动的线速度m/s及角速度rad/s)
而在ROS中通常由/cmd_vel节点发布Twist消息控制机器人移动,Twist消息类型如下:
Twist消息类型.png

Twist消息无法直接用于控制底盘,所以接下来将编写ros2功能包实现订阅/cmd_vel节点发布的Twist消息。
然后将获取到的线速度和角速度用于构建json消息,然后再通过串口下发给底盘,从而达到控制底盘的目的。

例程

创建功能包

进入工作空间

cd */ugv02/src


创建功能包

ros2 pkg create communication_module --build-type ament_python --dependencies rclpy

编写底盘控制节点

在/ugv02/src/communication_module/communication_module/目录中新建文件ugv02_control.py
在ugv02_control.py中编写代码程序

#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
import serial  
import json  

# 打开串口  
ser = serial.Serial('/dev/ttyAMA0', 115200, timeout=1)  

class CmdVelSubscribe(Node):
    def __init__(self,name):
        super().__init__(name)
        # 创建订阅者
        self.command_subscribe_ = self.create_subscription(Twist,"cmd_vel",self.command_callback,10)
        
    def command_callback(self,msg):
        print("msg.linear.x:  ", msg.linear.x)  
        print("msg.angular.z:  ", msg.angular.z)  
        angular_velocity = msg.angular.z
        # 防止原地转向时速度太小无法转动
        if msg.linear.x == 0:
            if 0<angular_velocity<0.2:
                angular_velocity = 0.2
            elif -0.2<angular_velocity<0:
                angular_velocity = -0.2

        # 构建JSON消息  
        data = {'T': '13', 'X': msg.linear.x, 'Z': angular_velocity}  

        json_data = json.dumps(data).encode('utf-8')  
        
        # 向串口发送JSON消息  
        ser.write(json_data) 

def main(args=None):
    rclpy.init(args=args) # 初始化rclpy
    node = CmdVelSubscribe("robot_control")  # 新建一个节点
    rclpy.spin(node) # 保持节点运行,检测是否收到退出指令(Ctrl+C)
    rclpy.shutdown() # 关闭rclpy
    # 关闭串口  
    ser.close()


在/ugv02/src/communication_module/目录的setup.py中修改entry_points的内容

entry_points={
    console_scripts': [
        "ugv02_control = communication_module.ugv02_control:main"
    ],
},

编译

返回工作空间并编译

cd ~/ugv02
colcon build

运行测试

进入工作空间

cd ~/ugv02
source ./install/setup.bash


给树莓派串口添加权限(每次树莓派重启后都需运行一次,串口才能被调用)

sudo chmod 777 /dev/ttyAMA0


启动底盘控制节点

ros2 run communication_module ugv02_control


启动键盘控制

ros2 run teleop_twist_keyboard teleop_twist_keyboard


如果上面这一句报错找不到功能包,你可以先安装功能包后再次执行
键盘控制功能包安装

sudo apt-get install ros-humble-teleop-twist-keyboard


接下你就可以使用键盘控制机器人移动了
键盘控制各个键介绍:



  U   I     O   
J K L
M < >

K—停止
I、J、<、L—前、左、后、右
q/z : 最大速度增加/减少10%
w/x : 仅线性速度增加10%
e/c : 仅角速度增加10%