从拿到OriginBot到现在也玩了好一段时间了，打算基于它做一些自定义的开发，不可避免的会接触到源码，所以打算用“OriginBot源码学习”这个系列来记录和分享我的学习过程。
我是一名机器人小白，只有Python的基础，所以这个系列的文章会更适合新手看。
先体验功能
这是我第一次看硬件类开源项目的源码，但是我觉得应该和看软件开源项目是差不多的。
在真正开始学习源代码之前，一定要先好好玩一玩OriginBot，把它的功能基本都熟悉之后才能比较容易地理解代码。
我也是在玩了两个月的小车，基本把所有的功能都玩了一遍之后有了一些自定义开发的想法之后才开始学习源代码的。
源码学习计划
我会按照官网的功能清单去阅读，如下图，
所以这个系列的内容会参照快速上手 、基础使用和应用功能来写，但是不会是囊括里面的所有内容，只会包含我所需要的部分。
快速上手代码解析
-
功能解释
快速上手部分是向我们展示怎么样让小车能运动起来，比如前进、后退和拐弯这些，可以让我们对小车有一个直观的体验。具体参考： https://www.originbot.org/guide/easy_start/（老实说，第一次组装好小车并且让它动起来还是很开心的）这一部分代码不是很复杂，并且在后面几乎所有功能中需要使用到，所以我觉得这是我学习源码的一个很好的切入点。
-
键盘控制节点分析
官网的教程让我们先启动机器人底盘，再启动键盘控制节点，也就是分别先后运行一下两行命令:
```sh启动机器人人底盘
ros2 launch originbot_bringup originbot.launch.py
启动键盘控制节点
ros2 run teleop_twist_keyboard teleop_twist_keyboard
但是这里要先说键盘控制节点。
**以下内容需要一点ros2的基础知识，如果完全不懂ros2的话建议先去学一下[ros2 21讲](https://book.guyuehome.com/)**
teleop_twist_keyboard 是ros2自带的一个功能，并不是OriginBot自己的代码。（我一开始以为这是OriginBot的代码，找了很久都没有找到源码）
大家可以在一台安装好ros2的ubuntu运行`ros2 topic list`， 会发现只有两个ros2自带的topic，输出如下，
```sh
$ ros2 topic list
/parameter_events
/rosout
这个时候启动另一个命令行session，运行ros2 run teleop_twist_keyboard teleop_twist_keyboard，如下，
$ ros2 run teleop_twist_keyboard teleop_twist_keyboard
This node takes keypresses from the keyboard and publishes them
as Twist messages. It works best with a US keyboard layout.
---------------------------
Moving around:
u i o
j k l
m , .
For Holonomic mode (strafing), hold down the shift key:
---------------------------
U I O
J K L
M < >
t : up (+z)
b : down (-z)
anything else : stop
q/z : increase/decrease max speeds by 10%
w/x : increase/decrease only linear speed by 10%
e/c : increase/decrease only angular speed by 10%
CTRL-C to quit
currently: speed 0.5 turn 1.0
此时在先前的命令行session中再次运行ros2 topic list，如下：
$ ros2 topic list
/cmd_vel
/parameter_events
/rosout
可以发现多了一个cmd_vel
然后在当前命令行执行ros2 topic echo /cmd_vel (这个命令用于打印来自于cmd_vel这个topic的message), 并在先前的teleop_twist_keyboard 命令行session中按一次i键，会观察到ros2 topic echo /cmd_vel 会有如下输出：
$ ros2 topic echo /cmd_vel
linear:
x: 0.5
y: 0.0
z: 0.0
angular:
x: 0.0
y: 0.0
z: 0.0
---
可以看到我们收到了一组linear（线速度）和angular（角速度）ros2 launch originbot_bringup originbot.launch.py 的作用就是把上面的线速度和角速度以合适的形式发送给STM32，然后STM32就会对小车的速度按照接收到的数据进行调整。
- 底盘控制节点解析
前文提到过，通过执行ros2 launch originbot_bringup originbot.launch.py可以启动机器人底盘，这个命令背后实际运行的代码是originbot.launch.py, 为了方便解释代码，我把代码原文贴在下面，以注释的形式记录。
```python
import os
import launch
import launch_ros.actions
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
ld = launch.LaunchDescription([ # launch文件返回的都是一个LaunchDescription
launch.actions.DeclareLaunchArgument(name='use_lidar', default_value='false'),
launch.actions.DeclareLaunchArgument(name='use_camera', default_value='false'),
launch.actions.DeclareLaunchArgument(name='use_imu', default_value='false'),
launch.actions.DeclareLaunchArgument(name='pub_odom', default_value='true'),
launch.actions.IncludeLaunchDescription( # 调用另外一个功能包中的launch文件时使用IncludeLaunchDescription
launch.launch_description_sources.PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('originbot_base'),
'launch/robot.launch.py')),
launch_arguments={
'use_imu': launch.substitutions.LaunchConfiguration('use_imu'),
'pub_odom': launch.substitutions.LaunchConfiguration('pub_odom'),
}.items(),
),
launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('originbot_bringup'),
'launch','ydlidar.launch.py')),
condition=launch.conditions.IfCondition(
launch.substitutions.LaunchConfiguration('use_lidar'))),
launch.actions.IncludeLaunchDescription(
launch.launch_description_sources.PythonLaunchDescriptionSource(
os.path.join(get_package_share_directory('originbot_bringup'),
'launch','camera.launch.py')),
condition=launch.conditions.IfCondition(
launch.substitutions.LaunchConfiguration('use_camera')))
])
return ld
if __name__ == '__main__':
generate_launch_description()
```
代码中三个launch.actions.IncludeLaunchDescription部分调用分别调用了底盘驱动节点、雷达节点和摄像头节点。
而其中的第一个调用了底盘驱动节点，实际上就是调用了originbot_base功能包中的launch文件，路径是：originbot/originbot_base/launch/robot.launch.py 。
代码如下：
import os
import launch
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
def generate_launch_description():
port_name_arg = DeclareLaunchArgument('port_name', default_value='ttyS3',
description='usb bus name, e.g. ttyS3')
# vx_arg 和vth_arg分别是线速度和角速度的校准系数，可以参考:https://www.originbot.org/manual/odom_calibration/
correct_factor_vx_arg = DeclareLaunchArgument('correct_factor_vx',
default_value='0.898',description='correct factor vx, e.g. 0.9')
correct_factor_vth_arg = DeclareLaunchArgument('correct_factor_vth', default_value='0.874',description='correct factor vth, e.g. 0.9')
auto_stop_on_arg = DeclareLaunchArgument('auto_stop_on', default_value='false',description='auto stop if no cmd received, true or false')
use_imu_arg = DeclareLaunchArgument('use_imu', default_value='false',description='if has imu sensor to drive')
pub_odom_arg = DeclareLaunchArgument('pub_odom', default_value='true',description='publish odom to base_footprint tf, true or false')
# 在这里实际上去执行了src目录下的originbot_base.cpp文件了
originbot_base_node = Node(
package='originbot_base',
executable='originbot_base',
output='screen',
emulate_tty=True,
parameters=[{
'port_name': LaunchConfiguration('port_name'),
'correct_factor_vx': LaunchConfiguration('correct_factor_vx'),
'correct_factor_vth': LaunchConfiguration('correct_factor_vth'),
'auto_stop_on': LaunchConfiguration('auto_stop_on'),
'use_imu': LaunchConfiguration('use_imu'),
'pub_odom': LaunchConfiguration('pub_odom'),
}])
# 以下部分代码跟底盘驱动不相关，没有列出来
originbot_base.cpp 里面设计到很多跟STM32交互的代码，我在这里只尝试解释跟运动相关的部分的代码。
首先，会进过下面这行代码，
cmd_vel_subscription_ = this->create_subscription<geometry_msgs::msg::Twist>("cmd_vel", 10, std::bind(&OriginbotBase::cmd_vel_callback, this, _1));
这行代码代码是创建了一个订阅者来订阅Twist消息，也就是前面提到的ros2 run teleop_twist_keyboard teleop_twist_keyboard 命令发出来的消息，实际上是一个线速度和角速度的集合。
接收到到消息之后，再调用cmd_vel_callback函数，这个函数的作用就是解析消息的内容，并以合适的格式发送给STM32。
以下是cmd_vel_callback的代码
void OriginbotBase::cmd_vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)
{
DataFrame cmdFrame;
float leftSpeed = 0.0, rightSpeed = 0.0;
float x_linear = msg->linear.x;
float z_angular = msg->angular.z;
//差分轮运动学模型求解,
leftSpeed = x_linear - z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;
rightSpeed = x_linear + z_angular * ORIGINBOT_WHEEL_TRACK / 2.0;
// 整个下面部分都是把速度按照通信协议封装好后发送给你STM32.
if (leftSpeed < 0)
cmdFrame.data[0] = 0x00;
else
cmdFrame.data[0] = 0xff;
cmdFrame.data[1] = int(abs(leftSpeed) * 1000) & 0xff; //速度值从m/s变为mm/s
cmdFrame.data[2] = (int(abs(leftSpeed) * 1000) >> 8) & 0xff;
if (rightSpeed < 0)
cmdFrame.data[3] = 0x00;
else
cmdFrame.data[3] = 0xff;
cmdFrame.data[4] = int(abs(rightSpeed) * 1000) & 0xff; //速度值从m/s变为mm/s
cmdFrame.data[5] = (int(abs(rightSpeed) * 1000) >> 8) & 0xff;
cmdFrame.check = (cmdFrame.data[0] + cmdFrame.data[1] + cmdFrame.data[2] +
cmdFrame.data[3] + cmdFrame.data[4] + cmdFrame.data[5]) & 0xff;
// 封装速度命令的数据帧
cmdFrame.header = 0x55;
cmdFrame.id = 0x01;
cmdFrame.length = 0x06;
cmdFrame.tail = 0xbb;
// 以下代码省略
// ......
}
通信协议如下图所示：
自定义拓展——用Python实现
上面是对于OriginBot底盘驱动的代码阅读和理解，为了加深印象，我用Python把其中的订阅器重写了一下， 步骤如下：
-
创建功能包
按照先后顺序执行以下命令mkdir originbot_py mkdir src cd src ros2 pkg create --build-type ament_python originbot_base到这里就有一个python格式的originbot_base功能包了
-
编写订阅器代码
在上面的功能包的originbot_base路径创建originbot_base.py, 内容如下：
```python
import time
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
ORIGINBOT_WHEEL_TRACK = 0.11
class TeleopSubscriber(Node):
def __init__(self):
super().__init__('teleop_subscriber')
self.subscription = self.create_subscription(
Twist,
'cmd_vel',
self.teleop_callback,
10)
self.subscription # prevent unused variable warning
def teleop_callback(self, msg):
self.get_logger().info('Linear: [%.2f, %.2f, %.2f], Angular: [%.2f, %.2f, %.2f]' %(msg.linear.x, msg.linear.y, msg.linear.z, msg.angular.x, msg.angular.y, msg.angular.z))
x_linear = msg.linear.x
z_angular = msg.angular.z
leftSpeed = x_linear - z_angular * ORIGINBOT_WHEEL_TRACK / 2.0
rightSpeed = x_linear + z_angular * ORIGINBOT_WHEEL_TRACK / 2.0
self.get_logger().info(f'left speed: {leftSpeed}, right speed: {rightSpeed}')
def main(args=None):
rclpy.init(args=args)
teleop_subscriber = TeleopSubscriber()
rclpy.spin(teleop_subscriber)
teleop_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
```
代码其实很简单，只写了怎么通过订阅者获取到速度信息，并通过差分模型计算出两个轮子的速度，后面还应该按照通信协议进行封装并通过串口通信发送给STM32，不过工业上这一部分还是C++用的多，就不再用Python写了。
-
编译
代码写完了还需要编译才能运行，并以也很简单，在originbot_py路径下执行colcon build就可以了 -
运行和校验
编译正常的话，就可以运行了，
先在另外一个命令行窗口中运行ros2 run teleop_twist_keyboard teleop_twist_keyboard来启动键盘控制节点，再执行如下命令，source ./install/local_setup.bash #初始化环境变量 ros2 run originbot_base originbot_base
此时在键盘控制节点的命令行中给出一些指令，我们用Python写的代码就可以获取到对应的信息了，日志如下：
[INFO] [1678107979.818387100] [teleop_subscriber]: Linear: [0.50, 0.00, 0.00], Angular: [0.00, 0.00, 0.00]
[INFO] [1678107979.819236900] [teleop_subscriber]: left speed: 0.5, right speed: 0.5
[INFO] [1678107993.680281400] [teleop_subscriber]: Linear: [0.00, 0.00, 0.00], Angular: [0.00, 0.00, 1.00]
[INFO] [1678107993.682360600] [teleop_subscriber]: left speed: -0.055, right speed: 0.05
到这里我们的代码就正常运行了。
评论（5）
您还未登录，请登录后发表或查看评论