跳转至

2.2 显示小车

约 1518 个字 483 行代码 预计阅读时间 11 分钟

一、显示窗口

在car.py中创建Car类，调整代码如下：

car.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
import pygame
import sys

class Car:
    def __init__(self) -> None:
        """初始化窗口并加载显示资源"""
        pygame.init()
        # 设置窗口大小
        self.win = pygame.display.set_mode((800, 600), pygame.RESIZABLE)
        # 设置窗口标题
        pygame.display.set_caption("Car")
        pass

    def run(self):
        """运行模拟器"""
        while True:
            self._check_events()
            self._update_win()

    def _check_events(self):
        """监视键盘、鼠标事件"""
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                sys.exit()

    def _update_win(self):
        """更新窗口显示"""
        color = (150, 150, 150)
        # 给窗口填充背景色
        self.win.fill(color=color)
        # 让最近绘制的窗口可见
        pygame.display.flip()

def main():
    """创建模拟器，并运行"""
    car = Car()
    car.run()


if __name__ == '__main__':
    main()
首先导入pygame模块和sys模块。pygame模块提供了我们绘制窗口的方法，sys模块提供了退出窗口的一些工具。 在__init__方法中，我们首先调用pygame.init()初始化pygame。调用pygame.display.set_mode方法设置了窗口大小，这里设置的尺寸是800x600。set_mode方法返回的是surface对象，在pygame中，surface是定义绘制区域的对象。这里我们将整个窗口的绘制区域赋值给了win变量，可以方便后面的使用。

运行窗口我们调用的run方法。在该方法中，我们使用了一个循环来不断的进行事件检测和窗口绘制。使用pygame.event.get()，你可以获取到一个事件列表，对于这个列表中的事件，你可以进行相应的处理。pygame.QUIT是点击窗口关闭按钮事件。当点击窗口关闭按钮时，我们调用sys.exit()退出整个程序。

_update_win方法中，我们通过surface对象的fill方法，绘制了窗口的背景色。最后，我们调用pygame.display.flip()方法，将绘制的窗口显示出来。

最后，在main方法中初始化Car实例，并调用run()方法，进行窗口显示。

点击【开始调试】按钮，或者按F5，显示界面如下：

pygame显示窗口

二、显示小车

我们的汽车模拟器界面，最终会有多个部件来进行绘制操作。所以，我们需要新建一个python包，将这些部件放到包中，方便管理。 在kitt包中，创建car_part包 在kitt目录上，右键选择【新建文件夹】，输入名称car_part。在car_part包中新建__init__.py的空文件。结构如下：

car_part子包

在car_part中，新建文件model.py，内容如下：

car.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
import pygame
import os
from importlib.resources import files


class Model:
    """
    汽车模型
    """
    def __init__(self, car) -> None:
        self.car = car
        self.win = car.win
        self._load_model_image()

    def _load_model_image(self):
        model_path = "src/kitt/resource/images/model.png"
        # 加载汽车图片
        self.model_image = pygame.image.load(model_path)

    def update(self):
        # 获取图片尺寸
        model_rect = self.model_image.get_rect()
        # 获取窗口尺寸，让图片居中显示
        win_rect = self.win.get_rect()
        model_rect.x = (win_rect.width - model_rect.width) / 2
        model_rect.y = (win_rect.height - model_rect.height) / 2
        # 在窗口的指定位置上绘制图片
        self.win.blit(self.model_image, model_rect)
将汽车模型文件拷贝到resource/images目录中。 model.py中的代码，非常简单。就是加载汽车图片，然后在update方法中，将其显示到主窗口中。这里，为了将图片显示到窗口正中，我们用窗口的宽度减去图片的宽度，然后再除以2,将得到了图片的x的坐标，同理，我们可以计算出图片y坐标。最后，调用surface对象的blit方法，将图片绘制到界面。

调整car.py中的代码，添加from car_part.model import Model导入我们定义的模型绘制模块。然后，在__init__初始化函数中，创建Model实例。最后，在_update_win方法中，调用Model的update方法，绘制汽车模型到窗口中。修改后的car.py代码如下，重点关注标亮部分。

car.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
import pygame
import sys
from car_part.model import Model

class Car:
    def __init__(self) -> None:
        """初始化窗口并加载显示资源"""
        pygame.init()
        # 设置窗口大小
        self.win = pygame.display.set_mode((800, 600), pygame.RESIZABLE)
        # 设置窗口标题
        pygame.display.set_caption("Car")
        self.model = Model(self)
        pass

    def run(self):
        """运行模拟器"""
        while True:
            self._check_events()
            self._update_win()

    def _check_events(self):
        """监视键盘、鼠标事件"""
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                sys.exit()

    def _update_win(self):
        """更新窗口显示"""
        color = (150, 150, 150)
        # 给窗口填充背景色
        self.win.fill(color=color)
        self.model.update()
        # 让最近绘制的窗口可见
        pygame.display.flip()

def main():
    """创建模拟器，并运行"""
    car = Car()
    car.run()


if __name__ == '__main__':
    main()

点击【开始调试】按钮，或者按F5，显示界面如下：

show_model

三、优化显示

汽车模型显示比例太大了，我们可以通过pygame.transform.smoothscale方法，将图片缩放到原来的六分之一，如下代码调整一下（标亮部分）。

model.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
import pygame
import os
from importlib.resources import files


class Model:
    """
    汽车模型
    """
    def __init__(self, car) -> None:
        self.car = car
        self.win = car.win
        self._load_model_image()

    def _load_model_image(self):
        model_path = "src/kitt/resource/images/model.png"
        # 加载汽车图片
        model_image = pygame.image.load(model_path)
        model_rect = model_image.get_rect()
        self.model_image = pygame.transform.smoothscale(
            model_image, (model_rect.width / 6, model_rect.height / 6)
        )

    def update(self):
        # 获取图片尺寸
        model_rect = self.model_image.get_rect()
        # 获取窗口尺寸，让图片居中显示
        win_rect = self.win.get_rect()
        model_rect.x = (win_rect.width - model_rect.width) / 2
        model_rect.y = (win_rect.height - model_rect.height) / 2
        # 在窗口的指定位置上绘制图片
        self.win.blit(self.model_image, model_rect)
效果如下：

modify_model

四、使用ros2命令运行

在终端窗口中，使用如下命令，运行一下我们的模拟器，执行如下命令试试看： 

ros2 run kitt car
报错如下： 
fotianmoyin@fotianmoyin-moon:~/Projects/vscode/kitt_ws$ ros2 run kitt car
Package 'kitt' not found
原因是我们没有编译程序，ros2命令找不到执行包，我们执行如下命令编译一下（注意colcon build命令需要在ros工作空间中执行，我们当前目录是kitt_ws正是本项目的工作空间）： 
colcon build
执行结果如下： 
fotianmoyin@fotianmoyin-moon:~/Projects/vscode/kitt_ws$ colcon build
Starting >>> inters  
Starting >>> kitt
Finished <<< inters [1.28s]                                            
Finished <<< kitt [1.55s]          

Summary: 2 packages finished [2.76s]
编译完成，我们再执行一下运行命令，又报了如下错误： 
fotianmoyin@fotianmoyin-moon:~/Projects/vscode/kitt_ws$ ros2 run kitt car
Package 'kitt' not found
那应该是，我们没有加载我们的程序命令到当前环境，执行以下命令（注意执行命令的当前目录，这里我的当前目录是kitt_ws，所以直接用相对路径就引用到了local_setup.sh）： 
. install/local_setup.sh
再次执行运行命令，报错如下： 
fotianmoyin@fotianmoyin-moon:~/Projects/vscode/kitt_ws$ ros2 run kitt car
pygame 2.1.2 (SDL 2.0.20, Python 3.10.12)
Hello from the pygame community. https://www.pygame.org/contribute.html
Traceback (most recent call last):
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/kitt/car", line 33, in <module>
    sys.exit(load_entry_point('kitt==0.0.0', 'console_scripts', 'car')())
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/kitt/car", line 25, in importlib_load_entry_point
    return next(matches).load()
  File "/usr/lib/python3.10/importlib/metadata/__init__.py", line 171, in load
    module = import_module(match.group('module'))
  File "/usr/lib/python3.10/importlib/__init__.py", line 126, in import_module
    return _bootstrap._gcd_import(name[level:], package, level)
  File "<frozen importlib._bootstrap>", line 1050, in _gcd_import
  File "<frozen importlib._bootstrap>", line 1027, in _find_and_load
  File "<frozen importlib._bootstrap>", line 1006, in _find_and_load_unlocked
  File "<frozen importlib._bootstrap>", line 688, in _load_unlocked
  File "<frozen importlib._bootstrap_external>", line 883, in exec_module
  File "<frozen importlib._bootstrap>", line 241, in _call_with_frames_removed
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/python3.10/site-packages/kitt/car.py", line 3, in <module>
    from car_part.model import Model
ModuleNotFoundError: No module named 'car_part'
[ros2run]: Process exited with failure 1
没有找到car_part包。报这个错的原因是，在ros中，对于包的搜索策略和python不同。在python中，会在当前位置搜索包，但在ros中，并不会搜索当前位置，所以这里就找不到包了。

如果我们将from car_part.model import Model改为from kitt.car_part.model import Model就可以在ros中运行成功了。但是这样，我们就无法在vscode中调试我们正在编写的代码，会非常不方便。

这里，我们可以在launch.json中添加一个启动参数来指示，我们是在vscode中执行的，然后我们将根据这个参数来调整模块引用代码。当存在这个参数时，我们将用不带kitt前缀的引用方式，如果没有这个参数，我们将用有kitt前缀的引用方式。

调整launch.json代码如下，添加一个debug的运行参数，看标亮行：

launch.json
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
{
    // 使用 IntelliSense 了解相关属性。 
    // 悬停以查看现有属性的描述。
    // 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387
    "version": "0.2.0",
    "configurations": [
        {
            "name": "Python 调试程序: 当前文件",
            "type": "debugpy",
            "request": "launch",
            "program": "${file}",
            "console": "integratedTerminal"
        },
        {
            "name": "car",
            "type": "debugpy",
            "request": "launch",
            "program": "${workspaceFolder}/src/kitt/kitt/car.py",
            "args": [
                "--debug"
            ],
            "console": "integratedTerminal",
            "justMyCode": true,
        },
    ]
}
调整car.py代码，添加对debug参数的检查，看标亮行：
car.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
import pygame
import sys
import os
import getopt

def run_as_debug() -> bool:
    """
    是否传递了参数debug
    """
    debug = False
    # 获取文件名（含后缀）
    name = os.path.basename(__file__)
    try:
        """
        options, args = getopt.getopt(args, shortopts, longopts=[])

        参数args：一般是sys.argv[1:]。过滤掉sys.argv[0]，它是执行脚本的名字，不算做命令行参数。
        参数shortopts：短格式分析串。例如："hp:i:"，h后面没有冒号，表示后面不带参数；p和i后面带有冒号，表示后面带参数。
        参数longopts：长格式分析串列表。例如：["help", "ip=", "port="]，help后面没有等号，表示后面不带参数；ip和port后面带等号，表示后面带参数。

        返回值options是以元组为元素的列表，每个元组的形式为：(选项串, 附加参数)，如：('-i', '192.168.0.1')
        返回值args是个列表，其中的元素是那些不含'-'或'--'的参数。
        """
        opts, args = getopt.getopt(sys.argv[1:], "hd:", ["help", "debug"])
        # 处理 返回值options是以元组为元素的列表。
        for opt, arg in opts:
            if opt in ("-h", "--help"):
                print(f"{name} -d")
                print(f"or: {name} --debug")
                sys.exit()
            elif opt in ("-d", "--debug"):
                debug = True
        if debug:
            print("debug模式")

        # 打印 返回值args列表，即其中的元素是那些不含'-'或'--'的参数。
        for i in range(0, len(args)):
            print("参数 %s 为：%s" % (i + 1, args[i]))
    except getopt.GetoptError:
        print(f"Error: {name} -d")
        print(f"   or: {name} --debug")
    return debug

if run_as_debug():
    from car_part.model import Model
else:
    from kitt.car_part.model import Model

class Car:
    def __init__(self) -> None:
        """初始化窗口并加载显示资源"""
        pygame.init()
        # 设置窗口大小
        self.win = pygame.display.set_mode((800, 600), pygame.RESIZABLE)
        # 设置窗口标题
        pygame.display.set_caption("Car")
        self.model = Model(self)
        pass

    def run(self):
        """运行模拟器"""
        while True:
            self._check_events()
            self._update_win()

    def _check_events(self):
        """监视键盘、鼠标事件"""
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                sys.exit()

    def _update_win(self):
        """更新窗口显示"""
        color = (150, 150, 150)
        # 给窗口填充背景色
        self.win.fill(color=color)
        self.model.update()
        # 让最近绘制的窗口可见
        pygame.display.flip()

def main():
    """创建模拟器，并运行"""
    car = Car()
    car.run()


if __name__ == '__main__':
    main()
我们再次执行colcon build命令，然后再执行ros2 run命令，一切运行正常。

五、打包模型图片资源到ros包中

我们新建一个终端，注意终端当前目录不是kitt_ws，我们运行ros2 run命令试试（运行前注意加载local_setup.sh到向前环境）。结果如下：

fotianmoyin@fotianmoyin-moon:~$ ros2 run kitt car
Package 'kitt' not found
fotianmoyin@fotianmoyin-moon:~$ . ~/Projects/vscode/kitt_ws/install/local_setup.sh
fotianmoyin@fotianmoyin-moon:~$ ros2 run kitt car
pygame 2.1.2 (SDL 2.0.20, Python 3.10.12)
Hello from the pygame community. https://www.pygame.org/contribute.html
src/kitt/resource/images/model.png
Traceback (most recent call last):
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/kitt/car", line 33, in <module>
    sys.exit(load_entry_point('kitt==0.0.0', 'console_scripts', 'car')())
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/python3.10/site-packages/kitt/car.py", line 83, in main
    car = Car()
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/python3.10/site-packages/kitt/car.py", line 57, in __init__
    self.model = Model(self)
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/python3.10/site-packages/kitt/car_part/model.py", line 13, in __init__
    self._load_model_image()
  File "/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/lib/python3.10/site-packages/kitt/car_part/model.py", line 19, in _load_model_image
    model_image = pygame.image.load(model_path)
FileNotFoundError: No file 'src/kitt/resource/images/model.png' found in working directory '/home/fotianmoyin'.
[ros2run]: Process exited with failure 1
我们发现无法引用到model.png文件了。我们的模型图片应该和模拟器一同发布，这样无论在哪个目录运行，就都能引用到了。 修改setup.py文件，将图片文件添加到share目录中。修改后的setup.py文件如下，注意标亮的行。
setup.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
from setuptools import find_packages, setup

package_name = 'kitt'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        ('share/' + package_name + '/images', ['resource/images/model.png']),#拷贝汽车图片到共享目录
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='fotianmoyin',
    maintainer_email='190045431@qq.com',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
            'car = kitt.car:main'
        ],
    },
)
使用的时候，如果是debug模式，那么我们依然引用本地文件，如果是ros环境运行，那么我们通过ament_index_python.packages包中的get_package_share_directory方法加载share目录的文件。 调整car.py中代码如下，标亮部分：
car.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
import pygame
import sys
import os
import getopt
from ament_index_python.packages import get_package_share_directory

def run_as_debug() -> bool:
    """
    是否传递了参数debug
    """
    debug = False
    # 获取文件名（含后缀）
    name = os.path.basename(__file__)
    try:
        """
        options, args = getopt.getopt(args, shortopts, longopts=[])

        参数args：一般是sys.argv[1:]。过滤掉sys.argv[0]，它是执行脚本的名字，不算做命令行参数。
        参数shortopts：短格式分析串。例如："hp:i:"，h后面没有冒号，表示后面不带参数；p和i后面带有冒号，表示后面带参数。
        参数longopts：长格式分析串列表。例如：["help", "ip=", "port="]，help后面没有等号，表示后面不带参数；ip和port后面带等号，表示后面带参数。

        返回值options是以元组为元素的列表，每个元组的形式为：(选项串, 附加参数)，如：('-i', '192.168.0.1')
        返回值args是个列表，其中的元素是那些不含'-'或'--'的参数。
        """
        opts, args = getopt.getopt(sys.argv[1:], "hd:", ["help", "debug"])
        # 处理 返回值options是以元组为元素的列表。
        for opt, arg in opts:
            if opt in ("-h", "--help"):
                print(f"{name} -d")
                print(f"or: {name} --debug")
                sys.exit()
            elif opt in ("-d", "--debug"):
                debug = True
        if debug:
            print("debug模式")

        # 打印 返回值args列表，即其中的元素是那些不含'-'或'--'的参数。
        for i in range(0, len(args)):
            print("参数 %s 为：%s" % (i + 1, args[i]))
    except getopt.GetoptError:
        print(f"Error: {name} -d")
        print(f"   or: {name} --debug")
    return debug

if run_as_debug():
    from car_part.model import Model
else:
    from kitt.car_part.model import Model

class Car:
    def __init__(self) -> None:
        """初始化窗口并加载显示资源"""
        pygame.init()

        if run_as_debug():
            self.share_path = os.path.join(os.path.dirname(__file__), "../resource")
        else:
            self.share_path = get_package_share_directory("kitt")

        # 设置窗口大小
        self.win = pygame.display.set_mode((800, 600), pygame.RESIZABLE)
        # 设置窗口标题
        pygame.display.set_caption("Car")
        self.model = Model(self)
        pass

    def run(self):
        """运行模拟器"""
        while True:
            self._check_events()
            self._update_win()

    def _check_events(self):
        """监视键盘、鼠标事件"""
        for event in pygame.event.get():
            if event.type == pygame.QUIT:
                sys.exit()

    def _update_win(self):
        """更新窗口显示"""
        color = (150, 150, 150)
        # 给窗口填充背景色
        self.win.fill(color=color)
        self.model.update()
        # 让最近绘制的窗口可见
        pygame.display.flip()

def main():
    """创建模拟器，并运行"""
    car = Car()
    car.run()


if __name__ == '__main__':
    main()
调整model.py中，加载图片的路径，修改如下，注意标亮部分：
model.py
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
import pygame
import os
from importlib.resources import files


class Model:
    """
    汽车模型
    """
    def __init__(self, car) -> None:
        self.car = car
        self.win = car.win
        self.share_path = car.share_path
        self._load_model_image()

    def _load_model_image(self):
        model_path = os.path.join(self.share_path, "images/model.png")
        print(model_path)
        # 加载汽车图片
        model_image = pygame.image.load(model_path)
        model_rect = model_image.get_rect()
        self.model_image = pygame.transform.smoothscale(
            model_image, (model_rect.width / 6, model_rect.height / 6)
        )

    def update(self):
        # 获取图片尺寸
        model_rect = self.model_image.get_rect()
        # 获取窗口尺寸，让图片居中显示
        win_rect = self.win.get_rect()
        model_rect.x = (win_rect.width - model_rect.width) / 2
        model_rect.y = (win_rect.height - model_rect.height) / 2
        # 在窗口的指定位置上绘制图片
        self.win.blit(self.model_image, model_rect)
使用ros2 run运行程序，我们看到使用的图片已经是install/kitt/share目录中的了。 
fotianmoyin@fotianmoyin-moon:~/Projects/vscode/kitt_ws$ ros2 run kitt car
pygame 2.1.2 (SDL 2.0.20, Python 3.10.12)
Hello from the pygame community. https://www.pygame.org/contribute.html
/home/fotianmoyin/Projects/vscode/kitt_ws/install/kitt/share/kitt/images/model.png
至此，小车的显示内容就讲完了。

源码下载