1
第一阶段：velocity_pub → cmd_vel → velocity_sub（模拟器）→ position
2
                                      ↑ set_enabled 服务
3

4
第二阶段：move_client → move_to Action → move_server
5
                                           ↓ cmd_vel
6
                                         模拟器
7
                                           ↓ position
8
                                       move_server

1
# ROS 1 的每个新终端执行；第二行要求 catkin 工作空间已经构建。只影响当前终端。
2
# ROS 1 终端；工作空间首次构建前只执行第一行
3
# 加载 Noetic 的工具和库路径；此时尚未加载自己的包。
4
source /opt/ros/noetic/setup.bash
5
# 加载当前构建产物，使 ROS 能找到新包、接口和可执行入口；不会自动构建。
6
source ~/catkin_ws/devel/setup.bash

1
# ROS 2 的每个新终端执行；第二行要求 colcon 已成功构建。不要与 ROS 1 环境混用。
2
# ROS 2 终端；工作空间首次构建前只执行第一行
3
# 加载 Jazzy 基础环境；后续工作空间会叠加在它之上。
4
source /opt/ros/jazzy/setup.bash
5
# 加载当前构建产物，使 ROS 能找到新包、接口和可执行入口；不会自动构建。
6
source ~/ros2_ws/install/setup.bash

1
linear:  x, y, z   # 线速度，通常以 m/s 表示
2
angular: x, y, z   # 角速度，通常以 rad/s 表示

1
# ROS 1 独立终端 A 执行，保持运行；后续节点在其他终端启动。Ctrl+C 结束。
2
# 启动 ROS Master、参数服务器等基础组件；它负责发现/注册，不代替业务节点。
3
roscore

1
# ROS 1 建包终端执行；命令按顺序执行，最终回到工作空间根目录。
2
# 加载 Noetic 的工具和库路径；此时尚未加载自己的包。
3
source /opt/ros/noetic/setup.bash
4
# 创建列出的目录；-p 允许父目录尚不存在，且目录已存在时不报错。
5
mkdir -p ~/catkin_ws/src
6
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
7
cd ~/catkin_ws/src
8
# 创建 catkin 包骨架；包名后面的名称会写入依赖声明，节点脚本仍需手工保存。
9
catkin_create_pkg ros1_basics rospy roscpp geometry_msgs std_srvs
10
# 创建列出的目录；-p 允许父目录尚不存在，且目录已存在时不报错。
11
mkdir -p ros1_basics/scripts ros1_basics/launch ros1_basics/config
12
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
13
cd ~/catkin_ws
14
# 构建 catkin 工作空间；接口生成失败时先看报错，不要继续启动依赖这些接口的节点。
15
catkin_make
16
# 加载当前构建产物，使 ROS 能找到新包、接口和可执行入口；不会自动构建。
17
source devel/setup.bash

1
#!/usr/bin/env python3
2
# 检查 NaN/无穷等非有限输入，避免异常数值进入速度或位置计算。
3
import math
4
# ROS 1 Python 客户端库：节点、话题、服务、日志和时钟 API。
5
import rospy
6
# Twist 承载速度；PointStamped 额外携带坐标系、时间戳与位置。
7
from geometry_msgs.msg import Twist
8

9

10
# 程序入口负责初始化通信、创建节点、保持运行以及退出时释放资源。
11
def main():
12
    # 向 ROS 1 注册节点名称；必须先初始化，再创建依赖节点身份的通信对象。
13
    rospy.init_node('velocity_pub')
14
    # 这里只在启动时读取一次；以后修改参数服务器/节点参数，不会自动更新这个变量。
15
    speed = float(rospy.get_param('~speed', 0.5))
16
    # 边界处拒绝非法数值及超范围值；不要让它们进入控制计算。
17
    if not math.isfinite(speed) or abs(speed) > 1.0:
18
        raise ValueError('speed must be finite and within [-1.0, 1.0] m/s')
19
    # 创建相对名称的发布者，随 namespace 解析；保留对象供后续 publish 使用。
20
    publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
21
    # 创建 10 Hz 节拍辅助对象，用于下面主动循环的间隔控制。
22
    rate = rospy.Rate(10)
23
    # ROS 1 退出标志置位后结束循环，不把程序写成永不退出的 while True。
24
    while not rospy.is_shutdown():
25
        message = Twist()
26
        # 本例只控制 x 方向线速度，其余分量保持消息默认值零。
27
        message.linear.x = speed
28
        # 序列化并发送消息；调用返回不等于所有订阅者已处理完毕。
29
        publisher.publish(message)
30
        # 等待本轮剩余时间，避免无限高速发布占满 CPU。
31
        rate.sleep()
32

33

34
# 直接执行此文件时调用入口；被别的模块导入时不自动启动节点。
35
if __name__ == '__main__':
36
    try:
37
        main()
38
    # 响应 Ctrl+C/ROS 退出，让程序进入正常清理路径。
39
    except rospy.ROSInterruptException:
40
        pass

1
#!/usr/bin/env python3
2
# 检查 NaN/无穷等非有限输入，避免异常数值进入速度或位置计算。
3
import math
4
# 为可能并发的回调保护共享状态；锁不负责调度回调。
5
import threading
6
# monotonic 用于持续时间与超时，不受系统日期校时影响；sleep 只等待，不更新位置。
7
import time
8
# ROS 1 Python 客户端库：节点、话题、服务、日志和时钟 API。
9
import rospy
10
# Twist 承载速度；PointStamped 额外携带坐标系、时间戳与位置。
11
from geometry_msgs.msg import PointStamped, Twist
12
# 复用标准启停接口：请求 data，响应 success 与 message。
13
from std_srvs.srv import SetBool, SetBoolResponse
14

15

16
# 模拟器持有唯一的位置状态：订阅命令、定时积分、发布反馈；Action 只能通过接口控制它。
17
class VelocityReceiver:
18
    # 构造阶段建立资源和初始状态；构造完成后再由 spin 驱动事件处理。
19
    def __init__(self):
20
        # 默认停用，必须显式调用服务后才执行速度，避免启动即运动。
21
        self.enabled = False
22
        # 模拟器位置只在节点创建时归零；停用、取消任务不会重置它。
23
        self.x = 0.0
24
        self.command = 0.0
25
        # 记录最后一次收到速度命令的墙钟时刻，供 0.5 秒看门狗使用。
26
        self.last_command = 0.0
27
        # 积分起点使用单调墙钟；与消息中的 ROS 时间戳用途不同。
28
        self.last_tick = time.monotonic()
29
        # 保护多个回调共同访问的变量，读取和更新同一组状态都要使用同一把锁。
30
        self.lock = threading.Lock()
31
        # 创建相对名称的发布者，随 namespace 解析；保留对象供后续 publish 使用。
32
        self.state_pub = rospy.Publisher('position', PointStamped, queue_size=10)
33
        # 注册消息到达时执行的函数；此处只是建立订阅，不会主动读取一条消息。
34
        self.subscription = rospy.Subscriber('cmd_vel', Twist, self.on_velocity)
35
        # 注册服务名、类型和回调；客户端请求到来时才执行处理函数。
36
        self.service = rospy.Service('set_enabled', SetBool, self.on_enable)
37
        # 创建周期触发源；周期是调度目标，不保证每次都恰好按时执行。
38
        self.timer = rospy.Timer(rospy.Duration(0.05), self.step)
39

40
    # 服务回调只改变是否执行命令；停用不清空位置，也不会停止其他节点发布。
41
    def on_enable(self, request):
42
        # 在锁内读取/修改共享状态；离开 with 自动释放，避免计算期间被另一回调改写。
43
        with self.lock:
44
            self.enabled = request.data
45
            if not self.enabled:
46
                self.command = 0.0
47
        # ROS 1 回调返回响应对象；字符串说明本次操作后的启用状态。
48
        return SetBoolResponse(success=True, message='enabled' if request.data else 'disabled')
49

50
    # 订阅回调只保存最新命令；真正的位置积分由定时器统一完成。
51
    def on_velocity(self, message):
52
        value = message.linear.x
53
        # 在锁内读取/修改共享状态；离开 with 自动释放，避免计算期间被另一回调改写。
54
        with self.lock:
55
            # 只接受有限且绝对值不超过 1 m/s 的命令，否则按零速度处理。
56
            self.command = value if math.isfinite(value) and abs(value) <= 1.0 else 0.0
57
            # 记录接收时刻；这是命令新鲜度，不是消息自身携带的时间戳。
58
            self.last_command = time.monotonic()
59

60
    # 每个定时周期推进模拟状态并发布位置；没有新命令时也仍发布当前位置。
61
    def step(self, event):
62
        now = time.monotonic()
63
        # 在锁内读取/修改共享状态；离开 with 自动释放，避免计算期间被另一回调改写。
64
        with self.lock:
65
            # 积分步长最大取 0.1 秒，避免调度卡顿后一次跳很远；因此不是精确动力学模拟。
66
            dt = min(now - self.last_tick, 0.1)
67
            self.last_tick = now
68
            # 超过 0.5 秒没有新命令，就认为控制源失联。
69
            fresh = now - self.last_command <= 0.5
70
            # 必须同时满足“启用”和“命令未过期”；否则本周期采用零速度。
71
            velocity = self.command if self.enabled and fresh else 0.0
72
            # 一维运动积分：位置增量 = 速度 × 实际步长；单位为米。
73
            self.x += velocity * dt
74
            x = self.x
75
        state = PointStamped()
76
        # 声明位置属于 map；这里只填写参考系名称，不执行任何坐标变换。
77
        state.header.frame_id = 'map'
78
        # 使用 ROS 时钟标记状态生成时刻；本教程积分仍基于墙钟，保持 use_sim_time=false。
79
        state.header.stamp = rospy.Time.now()
80
        state.point.x = x
81
        # 将当前位置发给观察工具或 Action 服务端，形成可见反馈。
82
        self.state_pub.publish(state)
83

84

85
# 直接执行此文件时调用入口；被别的模块导入时不自动启动节点。
86
if __name__ == '__main__':
87
    # 向 ROS 1 注册节点名称；必须先初始化，再创建依赖节点身份的通信对象。
88
    rospy.init_node('velocity_sub')
89
    receiver = VelocityReceiver()
90
    # 保持节点存活以处理通信；不等于手工重复调用订阅回调。
91
    rospy.spin()

1
# ROS 1 终端执行；先确认前文两个脚本已保存。绝对路径不依赖当前目录。
2
# 赋予脚本执行权限，使 rosrun 能直接启动；这不会运行脚本。
3
chmod +x ~/catkin_ws/src/ros1_basics/scripts/velocity_pub.py
4
# 赋予脚本执行权限，使 rosrun 能直接启动；这不会运行脚本。
5
chmod +x ~/catkin_ws/src/ros1_basics/scripts/velocity_sub.py

1
# ROS 1 发布者终端执行并保持运行；根命名空间下输出 /cmd_vel。
2
# 按“包名 可执行文件”启动节点；持续发布速度，Ctrl+C 停止。
3
rosrun ros1_basics velocity_pub.py

1
# ROS 1 模拟器终端执行并保持运行；初始 disabled，位置不会立即增长。
2
# 启动速度接收/位置模拟节点；用 set_enabled 服务启用后，位置才会更新。
3
rosrun ros1_basics velocity_sub.py

1
# ROS 1 观察终端执行；发布者、模拟器和 Master 应已运行。hz 会持续运行，Ctrl+C 后再执行下一条。
2
# 列出当前可见节点，先确认预期程序确实出现在 ROS 图中。
3
rosnode list
4
# 查看指定节点发布/订阅的话题和服务；用于核对连接关系。
5
rosnode info /velocity_pub
6
# 查询运行中话题的消息类型；输出类型名可交给 rosmsg show。
7
rostopic type /cmd_vel
8
# 显示消息字段结构；这是类型检查，不是读取某一条运行时消息。
9
rosmsg show geometry_msgs/Twist
10
# -n 1 表示收到一条后退出；检查实际数值和字段，不只看话题名。
11
rostopic echo -n 1 /cmd_vel
12
# 测量观察端收到消息的频率，预期接近 10 Hz；它不是硬实时保证。
13
rostopic hz /cmd_vel
14
# 确认服务类型；本例应为 std_srvs/SetBool。
15
rosservice type /set_enabled
16
# 发送一次启用请求；确认成功后观察位置增长，等待约 2 秒再执行停用请求。
17
rosservice call /set_enabled "data: true"
18
# 发送一次停用请求；模拟器位置应保持，速度话题本身仍可能持续发布。
19
rosservice call /set_enabled "data: false"

1
#!/usr/bin/env python3
2
# ROS 1 Python 客户端库：节点、话题、服务、日志和时钟 API。
3
import rospy
4
# 复用标准启停接口：请求 data，响应 success 与 message。
5
from std_srvs.srv import SetBool
6

7
# 向 ROS 1 注册节点名称；必须先初始化，再创建依赖节点身份的通信对象。
8
rospy.init_node('enable_client')
9
try:
10
    # 最多等待 5 秒发现服务；这是等待服务上线，不是等待业务执行完成。
11
    rospy.wait_for_service('set_enabled', timeout=5.0)
12
    # 创建 ROS 1 同步服务代理，调用时按服务定义序列化请求。
13
    client = rospy.ServiceProxy('set_enabled', SetBool)
14
    # 发送 data=true 并同步等待响应；适合短操作，不适合长时间运动任务。
15
    response = client(True)
16
    rospy.loginfo('success=%s, message=%s', response.success, response.message)
17
except (rospy.ROSException, rospy.ServiceException) as error:
18
    rospy.logerr('set_enabled failed: %s', error)

1
# ROS 1 观察终端执行；如果仅使用程序内部默认值，服务器里可能没有这个参数。
2
# 读取参数服务器的实际值；程序内部默认值不一定存在于服务器中。
3
rosparam get /velocity_pub/speed

1
# 停止原发布者后，在 ROS 1 发布者终端执行；参数在本例启动时读取。
2
# _speed 是 ROS 1 私有参数赋值，最终位于 /velocity_pub/speed；单位 m/s。
3
rosrun ros1_basics velocity_pub.py _speed:=0.2

1
# 停止旧发布者后运行；订阅者需要使用同样的命名空间和话题映射。
2
# __ns 改节点命名空间；cmd_vel:=velocity_cmd 重接话题，最终为 /uav1/velocity_cmd。
3
rosrun ros1_basics velocity_pub.py __ns:=/uav1 cmd_vel:=velocity_cmd

1
# ROS 1 节点私有参数文件；在 node 内加载，speed 单位为 m/s。
2
speed: 0.5

1
<!-- ROS 1 启动描述根元素；下面列出启动参数与节点。 -->
2
<launch>
3
  <!-- 声明 Launch 参数，命令行可覆盖；它不是自动写入的 ROS 参数。 -->
4
  <arg name="robot" default="uav1"/>
5
  <!-- 将组内节点与相对接口名称放入选定命名空间。 -->
6
  <group ns="$(arg robot)">
7
    <!-- pkg/type 指定程序，name 指定节点名，output=screen 将日志显示到终端。 -->
8
    <node pkg="ros1_basics" type="velocity_pub.py" name="velocity_pub" output="screen">
9
      <!-- 把 YAML 加载到该节点的私有参数空间；文件位置通过 ROS 包查找解析。 -->
10
      <rosparam command="load" file="$(find ros1_basics)/config/velocity.yaml"/>
11
    </node>
12
    <!-- pkg/type 指定程序，name 指定节点名，output=screen 将日志显示到终端。 -->
13
    <node pkg="ros1_basics" type="velocity_sub.py" name="velocity_sub" output="screen"/>
14
  </group>
15
</launch>

1
# ROS 1 启动终端执行并保持运行；先停掉前文手动启动的重复实例。
2
# 加载 launch 并启动整组节点；robot:=uav1 是 Launch 参数，用于组织命名空间。
3
roslaunch ros1_basics demo.launch robot:=uav1

1
catkin_ws/
2
├── src/
3
│   ├── ros1_basics/       # 本文节点和启动文件
4
│   └── robot_msgs/        # 自定义消息、服务和动作
5
├── build/                 # 构建中间结果
6
└── devel/                 # 开发环境和生成接口

1
# 对应 ROS 版本的工作空间根目录执行；rosdep 已初始化并更新索引。此命令会安装依赖。
2
# --from-paths src 扫描包依赖；--ignore-src 跳过源码已有包，-r 遇到错误继续，-y 自动确认安装。
3
rosdep install --from-paths src --ignore-src -r -y

1
# ROS 1 建包终端执行；先有 catkin_ws/src，再创建接口包及三类接口目录。
2
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
3
cd ~/catkin_ws/src
4
# 创建 catkin 包骨架；包名后面的名称会写入依赖声明，节点脚本仍需手工保存。
5
catkin_create_pkg robot_msgs std_msgs actionlib_msgs message_generation message_runtime
6
# 创建列出的目录；-p 允许父目录尚不存在，且目录已存在时不报错。
7
mkdir -p robot_msgs/msg robot_msgs/srv robot_msgs/action

1
# Header 携带参考系和时间戳；坐标值应与 frame_id 一致。
2
# x/y/z 是位置分量，单位米。
3
std_msgs/Header header
4
float64 x
5
float64 y
6
float64 z

1
# 请求部分：待检查的目标坐标，单位米。
2
float64 x
3
float64 y
4
float64 z
5
---
6
# 响应部分：是否合法及说明；定义接口不会自动创建服务端。
7
bool accepted
8
string message

1
# Goal：map 坐标系中的绝对目标 x，不是位移增量。
2
float64 target_x
3
---
4
# Result：是否到达，以及最后一次测得的位置。
5
bool reached
6
float64 final_x
7
---
8
# Feedback：执行过程中不断更新的当前位置。
9
float64 current_x

1
# 声明构建所需的最低 CMake 版本；不代表会自动安装 CMake。
2
cmake_minimum_required(VERSION 3.0.2)
3
# 声明项目/包名，后续 PROJECT_NAME 使用这里的值。
4
project(robot_msgs)
5
# 查找 catkin 及生成消息需要的包；后续多行是本次构建所需组件。
6
find_package(catkin REQUIRED COMPONENTS
7
  std_msgs actionlib_msgs message_generation
8
)
9
# 列出本包需要生成的 .msg 文件，不会自动扫描任意目录。
10
add_message_files(FILES Waypoint.msg)
11
# 列出请求/响应接口定义。
12
add_service_files(FILES CheckTarget.srv)
13
# 列出动作定义；构建时生成目标、结果、反馈等配套类型。
14
add_action_files(FILES MoveTo.action)
15
# 按定义生成 ROS 1 语言接口，依赖包含标准消息与 Action 基础类型。
16
generate_messages(DEPENDENCIES std_msgs actionlib_msgs)
17
# 向使用本包的下游包导出运行期接口依赖。
18
catkin_package(CATKIN_DEPENDS std_msgs actionlib_msgs message_runtime)

1
<!-- ROS 1 构建时生成接口代码需要的依赖。 -->
2
<build_depend>message_generation</build_depend>
3
<!-- ROS 1 使用已生成接口时需要的运行依赖。 -->
4
<exec_depend>message_runtime</exec_depend>

1
<!-- 节点消费自定义 ROS 1 接口，显式声明构建与运行依赖。 -->
2
<depend>robot_msgs</depend>
3
<!-- 节点使用 ROS 1 动作协议客户端/服务端。 -->
4
<depend>actionlib</depend>

1
# 保存接口定义和构建配置后执行；生成类型后，其他运行终端也需重新 source。
2
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
3
cd ~/catkin_ws
4
# 构建 catkin 工作空间；接口生成失败时先看报错，不要继续启动依赖这些接口的节点。
5
catkin_make
6
# 加载当前构建产物，使 ROS 能找到新包、接口和可执行入口；不会自动构建。
7
source devel/setup.bash
8
# 显示消息字段结构；这是类型检查，不是读取某一条运行时消息。
9
rosmsg show robot_msgs/Waypoint
10
# 显示服务请求与响应字段，--- 分隔两部分。
11
rossrv show robot_msgs/CheckTarget
12
# 显示消息字段结构；这是类型检查，不是读取某一条运行时消息。
13
rosmsg show robot_msgs/MoveToGoal

1
# ROS 1 发布终端执行并保持运行；观察终端运行 rostopic echo /waypoint。
2
# -r 1 表示每秒发布一次；YAML 给出字段值，header 仅指定 frame，未提供有效采样时间。
3
rostopic pub -r 1 /waypoint robot_msgs/Waypoint \
4
  "{header: {frame_id: 'map'}, x: 1.0, y: 2.0, z: 3.0}"

1
# 干净的 ROS 2 建包终端执行；只创建包骨架，不会自动生成本教程的节点代码。
2
# 加载 Jazzy 基础环境；后续工作空间会叠加在它之上。
3
source /opt/ros/jazzy/setup.bash
4
# 创建列出的目录；-p 允许父目录尚不存在，且目录已存在时不报错。
5
mkdir -p ~/ros2_ws/src
6
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
7
cd ~/ros2_ws/src
8
# --build-type 指定包构建方式；--dependencies 写入依赖声明，不能代替环境依赖检查。
9
ros2 pkg create --build-type ament_python --license Apache-2.0 ros2_basics \
10
  --dependencies rclpy geometry_msgs std_srvs

1
ros2_basics/
2
├── package.xml
3
├── setup.py
4
├── setup.cfg
5
├── resource/ros2_basics
6
└── ros2_basics/
7
    └── __init__.py

1
# 检查 NaN/无穷等非有限输入，避免异常数值进入速度或位置计算。
2
import math
3
# ROS 2 Python 客户端库；回调需要通过 executor/spin 获得执行机会。
4
import rclpy
5
# Node 封装节点身份、参数以及发布/订阅等 ROS 资源。
6
from rclpy.node import Node
7
# Twist 承载速度；PointStamped 额外携带坐标系、时间戳与位置。
8
from geometry_msgs.msg import Twist
9

10

11
# 把发布器与定时器的生命周期绑定到节点对象，便于统一管理。
12
class VelocityPublisher(Node):
13
    # 构造阶段建立资源和初始状态；构造完成后再由 spin 驱动事件处理。
14
    def __init__(self):
15
        # 创建具有指定默认名称的 ROS 2 节点；名称仍可通过启动参数重映射。
16
        super().__init__('velocity_pub')
17
        # 声明浮点参数及默认值；命令行的 -p speed:=... 可在启动时覆盖。
18
        self.declare_parameter('speed', 0.5)
19
        # 这里只在启动时读取一次；以后修改参数服务器/节点参数，不会自动更新这个变量。
20
        self.speed = self.get_parameter('speed').value
21
        # 边界处拒绝非法数值及超范围值；不要让它们进入控制计算。
22
        if not math.isfinite(self.speed) or abs(self.speed) > 1.0:
23
            raise ValueError('speed must be finite and within [-1.0, 1.0] m/s')
24
        # 创建相对名称的发布者，随 namespace 解析；保留对象供后续 publish 使用。
25
        self.publisher = self.create_publisher(Twist, 'cmd_vel', 10)
26
        # 创建周期触发源；周期是调度目标，不保证每次都恰好按时执行。
27
        self.timer = self.create_timer(0.1, self.publish_velocity)
28

29
    # 定时器回调只构造并发布一条消息，不在这里再启动循环。
30
    def publish_velocity(self):
31
        message = Twist()
32
        # 本例只控制 x 方向线速度，其余分量保持消息默认值零。
33
        message.linear.x = self.speed
34
        # 序列化并发送消息；调用返回不等于所有订阅者已处理完毕。
35
        self.publisher.publish(message)
36

37

38
# 程序入口负责初始化通信、创建节点、保持运行以及退出时释放资源。
39
def main(args=None):
40
    # 初始化 ROS 2 上下文并处理传入的 ROS 命令行参数。
41
    rclpy.init(args=args)
42
    node = None
43
    try:
44
        node = VelocityPublisher()
45
        # 由执行器处理已就绪的订阅、服务、定时器等事件，直到退出。
46
        rclpy.spin(node)
47
    # 响应 Ctrl+C/ROS 退出，让程序进入正常清理路径。
48
    except KeyboardInterrupt:
49
        pass
50
    # 无论正常完成还是发生异常，都执行本分支的资源/状态清理。
51
    finally:
52
        if node is not None:
53
            # 释放节点及其通信资源；不要只结束循环而遗留节点对象。
54
            node.destroy_node()
55
        # 上下文仍有效才调用后续通信/关闭操作，避免重复 shutdown。
56
        if rclpy.ok():
57
            # 关闭 ROS 2 通信上下文；不是对真实机器人的停止确认。
58
            rclpy.shutdown()
59

60

61
# 直接执行此文件时调用入口；被别的模块导入时不自动启动节点。
62
if __name__ == '__main__':
63
    main()

1
# 检查 NaN/无穷等非有限输入，避免异常数值进入速度或位置计算。
2
import math
3
# monotonic 用于持续时间与超时，不受系统日期校时影响；sleep 只等待，不更新位置。
4
import time
5
# ROS 2 Python 客户端库；回调需要通过 executor/spin 获得执行机会。
6
import rclpy
7
# Node 封装节点身份、参数以及发布/订阅等 ROS 资源。
8
from rclpy.node import Node
9
# Twist 承载速度；PointStamped 额外携带坐标系、时间戳与位置。
10
from geometry_msgs.msg import PointStamped, Twist
11
# 复用标准启停接口：请求 data，响应 success 与 message。
12
from std_srvs.srv import SetBool
13

14

15
# 模拟器持有唯一的位置状态：订阅命令、定时积分、发布反馈；Action 只能通过接口控制它。
16
class VelocityReceiver(Node):
17
    # 构造阶段建立资源和初始状态；构造完成后再由 spin 驱动事件处理。
18
    def __init__(self):
19
        # 创建具有指定默认名称的 ROS 2 节点；名称仍可通过启动参数重映射。
20
        super().__init__('velocity_sub')
21
        # 默认停用，必须显式调用服务后才执行速度，避免启动即运动。
22
        self.enabled = False
23
        # 模拟器位置只在节点创建时归零；停用、取消任务不会重置它。
24
        self.x = 0.0
25
        self.command = 0.0
26
        # 记录最后一次收到速度命令的墙钟时刻，供 0.5 秒看门狗使用。
27
        self.last_command = 0.0
28
        # 积分起点使用单调墙钟；与消息中的 ROS 时间戳用途不同。
29
        self.last_tick = time.monotonic()
30
        # 创建相对名称的发布者，随 namespace 解析；保留对象供后续 publish 使用。
31
        self.state_pub = self.create_publisher(PointStamped, 'position', 10)
32
        # 注册消息到达时执行的函数；此处只是建立订阅，不会主动读取一条消息。
33
        self.subscription = self.create_subscription(
34
            Twist, 'cmd_vel', self.on_velocity, 10)
35
        # 注册服务名、类型和回调；客户端请求到来时才执行处理函数。
36
        self.service = self.create_service(SetBool, 'set_enabled', self.on_enable)
37
        # 创建周期触发源；周期是调度目标，不保证每次都恰好按时执行。
38
        self.timer = self.create_timer(0.05, self.step)
39

40
    # 订阅回调只保存最新命令；真正的位置积分由定时器统一完成。
41
    def on_velocity(self, message):
42
        value = message.linear.x
43
        # 只接受有限且绝对值不超过 1 m/s 的命令，否则按零速度处理。
44
        self.command = value if math.isfinite(value) and abs(value) <= 1.0 else 0.0
45
        # 记录接收时刻；这是命令新鲜度，不是消息自身携带的时间戳。
46
        self.last_command = time.monotonic()
47

48
    # 服务回调只改变是否执行命令；停用不清空位置，也不会停止其他节点发布。
49
    def on_enable(self, request, response):
50
        self.enabled = request.data
51
        if not self.enabled:
52
            self.command = 0.0
53
        # success 表示启停请求已处理，不表示机器人已经到达某个目标。
54
        response.success = True
55
        response.message = 'enabled' if self.enabled else 'disabled'
56
        return response
57

58
    # 每个定时周期推进模拟状态并发布位置；没有新命令时也仍发布当前位置。
59
    def step(self):
60
        now = time.monotonic()
61
        # 积分步长最大取 0.1 秒，避免调度卡顿后一次跳很远；因此不是精确动力学模拟。
62
        dt = min(now - self.last_tick, 0.1)
63
        self.last_tick = now
64
        # 超过 0.5 秒没有新命令，就认为控制源失联。
65
        fresh = now - self.last_command <= 0.5
66
        # 必须同时满足“启用”和“命令未过期”；否则本周期采用零速度。
67
        velocity = self.command if self.enabled and fresh else 0.0
68
        # 一维运动积分：位置增量 = 速度 × 实际步长；单位为米。
69
        self.x += velocity * dt
70
        state = PointStamped()
71
        # 声明位置属于 map；这里只填写参考系名称，不执行任何坐标变换。
72
        state.header.frame_id = 'map'
73
        # 使用 ROS 时钟标记状态生成时刻；本教程积分仍基于墙钟，保持 use_sim_time=false。
74
        state.header.stamp = self.get_clock().now().to_msg()
75
        state.point.x = self.x
76
        # 将当前位置发给观察工具或 Action 服务端，形成可见反馈。
77
        self.state_pub.publish(state)
78

79

80
# 程序入口负责初始化通信、创建节点、保持运行以及退出时释放资源。
81
def main(args=None):
82
    # 初始化 ROS 2 上下文并处理传入的 ROS 命令行参数。
83
    rclpy.init(args=args)
84
    node = VelocityReceiver()
85
    try:
86
        # 由执行器处理已就绪的订阅、服务、定时器等事件，直到退出。
87
        rclpy.spin(node)
88
    # 响应 Ctrl+C/ROS 退出，让程序进入正常清理路径。
89
    except KeyboardInterrupt:
90
        pass
91
    # 无论正常完成还是发生异常，都执行本分支的资源/状态清理。
92
    finally:
93
        # 释放节点及其通信资源；不要只结束循环而遗留节点对象。
94
        node.destroy_node()
95
        # 上下文仍有效才调用后续通信/关闭操作，避免重复 shutdown。
96
        if rclpy.ok():
97
            # 关闭 ROS 2 通信上下文；不是对真实机器人的停止确认。
98
            rclpy.shutdown()
99

100

101
# 直接执行此文件时调用入口；被别的模块导入时不自动启动节点。
102
if __name__ == '__main__':
103
    main()

1
# 入口格式为“命令名 = Python 模块:函数”；ros2 run 通过该入口调用 main。
2
entry_points={
3
    # 入口格式为“命令名 = Python 模块:函数”；ros2 run 通过该入口调用 main。
4
    'console_scripts': [
5
        'velocity_pub = ros2_basics.velocity_pub:main',
6
        'velocity_sub = ros2_basics.velocity_sub:main',
7
    ],
8
},

1
# ROS 2 构建终端执行；保存脚本和入口配置后再构建，最后一条会持续运行发布者。
2
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
3
cd ~/ros2_ws
4
# --packages-select 只选择指定包，依赖需已可用；--symlink-install 尽可能使用符号链接，安装规则变化仍须重建。
5
colcon build --packages-select ros2_basics --symlink-install
6
# 加载当前构建产物，使 ROS 能找到新包、接口和可执行入口；不会自动构建。
7
source install/setup.bash
8
# 运行包内入口；--ros-args -p speed:=0.5 为该节点覆盖浮点参数默认值。
9
ros2 run ros2_basics velocity_pub --ros-args -p speed:=0.5

1
# ROS 2 观察终端执行；发布者和模拟器应已启动。hz 持续运行，Ctrl+C 后执行服务调用。
2
# 列出当前 Domain 内可发现的节点；跨域或环境混用可能导致看不到节点。
3
ros2 node list
4
# 检查节点的发布者、订阅者、服务与动作端点。
5
ros2 node info /velocity_pub
6
# 显示生成后的接口字段，确认类型已被当前终端发现。
7
ros2 interface show geometry_msgs/msg/Twist
8
# --once 收到一条即退出；若指定 Transient Local 与 Reliable，是为了匹配保留样本的发布者。
9
ros2 topic echo /cmd_vel --once
10
# 观察接收频率；此命令持续运行，结束后再执行下一条。
11
ros2 topic hz /cmd_vel
12
# 请求启用，响应 success=true 后观察位置并等待约 2 秒，再执行停用。
13
ros2 service call /set_enabled std_srvs/srv/SetBool "{data: true}"
14
# 请求停用；随后 position 应保持当前位置，不会自动归零。
15
ros2 service call /set_enabled std_srvs/srv/SetBool "{data: false}"
16
# 读取指定节点的参数；本例只在构造阶段复制参数，运行时修改不自动改变业务成员。
17
ros2 param get /velocity_pub speed

1
# ROS 2 Python 客户端库；回调需要通过 executor/spin 获得执行机会。
2
import rclpy
3
# 复用标准启停接口：请求 data，响应 success 与 message。
4
from std_srvs.srv import SetBool
5

6

7
# 程序入口负责初始化通信、创建节点、保持运行以及退出时释放资源。
8
def main():
9
    # 初始化 ROS 2 上下文并处理传入的 ROS 命令行参数。
10
    rclpy.init()
11
    node = rclpy.create_node('enable_client')
12
    try:
13
        client = node.create_client(SetBool, 'set_enabled')
14
        # 最多等待 5 秒发现服务；这是等待服务上线，不是等待业务执行完成。
15
        if not client.wait_for_service(timeout_sec=5.0):
16
            raise RuntimeError('set_enabled server unavailable')
17
        request = SetBool.Request()
18
        request.data = True
19
        # 异步发送请求并立即得到 Future；需要后面的 spin 来接收响应。
20
        future = client.call_async(request)
21
        # 在主流程中处理事件直到该 Future 完成或超时；不要直接照搬到互斥回调内部。
22
        rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
23
        # 检查等待是否真正完成；超时不等于服务端没有执行，也不等于机器人停止。
24
        if not future.done():
25
            raise TimeoutError('set_enabled response timed out')
26
        # Future 已完成后读取响应；服务调用异常也可能在此处传播。
27
        response = future.result()
28
        node.get_logger().info(f'success={response.success}, message={response.message}')
29
    # 无论正常完成还是发生异常，都执行本分支的资源/状态清理。
30
    finally:
31
        # 释放节点及其通信资源；不要只结束循环而遗留节点对象。
32
        node.destroy_node()
33
        # 上下文仍有效才调用后续通信/关闭操作，避免重复 shutdown。
34
        if rclpy.ok():
35
            # 关闭 ROS 2 通信上下文；不是对真实机器人的停止确认。
36
            rclpy.shutdown()
37

38

39
# 直接执行此文件时调用入口；被别的模块导入时不自动启动节点。
40
if __name__ == '__main__':
41
    main()

1
# ROS 2 参数文件：/** 匹配节点名称，ros__parameters 下才是参数值。
2
/**:
3
  ros__parameters:
4
    speed: 0.5

1
# LaunchDescription 收集本次启动要执行的动作，不直接在导入时启动节点。
2
from launch import LaunchDescription
3
# 声明外部可覆盖的启动参数，例如 robot:=uav2。
4
from launch.actions import DeclareLaunchArgument
5
# 启动时才解析参数与路径，避免在脚本加载时把值写死。
6
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
7
# 这里的 Node 是“启动一个节点进程”的 Launch 动作，不是 rclpy.Node 对象。
8
from launch_ros.actions import Node
9
# 查找包安装后的 share 目录，避免硬编码源码目录。
10
from launch_ros.substitutions import FindPackageShare
11

12

13
# ros2 launch 调用此函数取得启动描述，再按描述创建进程。
14
def generate_launch_description():
15
    # 保存启动参数的替换表达式，实际字符串在 launch 执行时解析。
16
    robot = LaunchConfiguration('robot')
17
    # 从安装后的包资源目录拼接 YAML 路径，不依赖运行命令时所在目录。
18
    config = PathJoinSubstitution([
19
        FindPackageShare('ros2_basics'), 'config', 'velocity.yaml'])
20
    # 列表中先声明启动参数，再启动发布者和模拟器。
21
    return LaunchDescription([
22
        DeclareLaunchArgument('robot', default_value='uav1'),
23
        # namespace 隔离接口；参数文件仅传给需要读取 speed 的发布者。
24
        Node(package='ros2_basics', executable='velocity_pub',
25
             name='velocity_pub', namespace=robot, parameters=[config], output='screen'),
26
        # namespace 隔离接口；参数文件仅传给需要读取 speed 的发布者。
27
        Node(package='ros2_basics', executable='velocity_sub',
28
             name='velocity_sub', namespace=robot, output='screen'),
29
    ])

1
# 将资源安装到包的 share 目录；增加/修改此安装规则后需要重新构建。
2
('share/ros2_basics/launch', glob('launch/*.launch.py')),
3
# 将资源安装到包的 share 目录；增加/修改此安装规则后需要重新构建。
4
('share/ros2_basics/config', glob('config/*.yaml')),

1
<!-- 启动 Python Launch 描述需要的运行库。 -->
2
<exec_depend>launch</exec_depend>
3
<!-- 启动 Python Launch 描述需要的运行库。 -->
4
<exec_depend>launch_ros</exec_depend>

1
# ROS 2 启动终端执行并保持运行；启动文件和 YAML 必须已安装到工作空间。
2
# 启动已安装的 Python launch 文件；robot 参数决定该组节点的命名空间。
3
ros2 launch ros2_basics demo.launch.py robot:=uav1

1
# ROS 2 建包终端执行；这是 ament_cmake 接口包，供 Python/C++ 节点共同使用。
2
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
3
cd ~/ros2_ws/src
4
# --build-type 指定包构建方式；--dependencies 写入依赖声明，不能代替环境依赖检查。
5
ros2 pkg create --build-type ament_cmake --license Apache-2.0 robot_interfaces
6
# 创建列出的目录；-p 允许父目录尚不存在，且目录已存在时不报错。
7
mkdir -p robot_interfaces/msg robot_interfaces/srv robot_interfaces/action

1
# 声明构建所需的最低 CMake 版本；不代表会自动安装 CMake。
2
cmake_minimum_required(VERSION 3.8)
3
# 声明项目/包名，后续 PROJECT_NAME 使用这里的值。
4
project(robot_interfaces)
5
# 加载 ROS 2 的 CMake 构建支持。
6
find_package(ament_cmake REQUIRED)
7
# 加载 ROS 2 接口代码生成器，供 Python/C++ 共同使用。
8
find_package(rosidl_default_generators REQUIRED)
9
# 查找 Header 所在的标准消息包。
10
find_package(std_msgs REQUIRED)
11
# 查找 ROS 2 Action 基础协议消息依赖。
12
find_package(action_msgs REQUIRED)
13
# 为列出的 ROS 2 接口生成可导入类型；DEPENDENCIES 对应字段引用的外部包。
14
rosidl_generate_interfaces(${PROJECT_NAME}
15
  "msg/Waypoint.msg"
16
  "srv/CheckTarget.srv"
17
  "action/MoveTo.action"
18
  DEPENDENCIES std_msgs
19
)
20
# 向下游导出使用生成接口所需的运行期依赖。
21
ament_export_dependencies(rosidl_default_runtime)
22
# 完成 ROS 2 包导出，通常放在构建配置末尾。
23
ament_package()

1
<!-- 声明构建/生成工具；运行依赖与它承担不同职责。 -->
2
<buildtool_depend>rosidl_default_generators</buildtool_depend>
3
<!-- 使用生成后的 ROS 2 接口所需运行支持。 -->
4
<exec_depend>rosidl_default_runtime</exec_depend>
5
<!-- 接口引用标准 Header，需要依赖其消息定义。 -->
6
<depend>std_msgs</depend>
7
<!-- ROS 2 Action 基础协议所需类型。 -->
8
<depend>action_msgs</depend>
9
<!-- 将此包归入 ROS 2 接口包组，满足接口生成工具的元数据要求。 -->
10
<member_of_group>rosidl_interface_packages</member_of_group>

1
# ROS 2 工作空间构建终端执行；依赖关系应已写入 package.xml。
2
# 切换执行目录；后面的相对路径、构建命令都以此目录为基准。
3
cd ~/ros2_ws
4
# --packages-up-to 同时构建目标及其工作空间依赖；--symlink-install 尽可能使用符号链接，安装规则变化仍须重建。
5
colcon build --packages-up-to ros2_basics --symlink-install
6
# 加载当前构建产物，使 ROS 能找到新包、接口和可执行入口；不会自动构建。
7
source install/setup.bash
8
# 显示生成后的接口字段，确认类型已被当前终端发现。
9
ros2 interface show robot_interfaces/msg/Waypoint
10
# 显示生成后的接口字段，确认类型已被当前终端发现。
11
ros2 interface show robot_interfaces/srv/CheckTarget
12
# 显示生成后的接口字段，确认类型已被当前终端发现。
13
ros2 interface show robot_interfaces/action/MoveTo

1
# ROS 2 发布终端执行；在另一个已加载接口包的终端 echo /waypoint。
2
# 按 1 Hz 发布自定义消息；类型名含 /msg/，YAML 字段必须与生成定义一致。
3
ros2 topic pub /waypoint robot_interfaces/msg/Waypoint \
4
  "{header: {frame_id: 'map'}, x: 1.0, y: 2.0, z: 3.0}" --rate 1

1
# ROS 2 发布终端执行并保持存活；等第一次发布后，再启动后面的订阅命令。
2
# 每 10 秒发送一条字符串；Transient Local 保留历史样本，Reliable 指定可靠交付策略。
3
ros2 topic pub /mission_note std_msgs/msg/String "{data: 'inspection ready'}" \
4
  --rate 0.1 --qos-durability transient_local --qos-reliability reliable

1
# ROS 2 订阅终端执行；上一条发布器应仍在运行，以验证晚加入者接收保留样本。
2
# --once 收到一条即退出；若指定 Transient Local 与 Reliable，是为了匹配保留样本的发布者。
3
ros2 topic echo /mission_note std_msgs/msg/String \
4
  --qos-durability transient_local --qos-reliability reliable --once
5
# -v 展示端点 QoS 等详细信息，排查有话题却收不到消息的问题。
6
ros2 topic info /mission_note -v

1
# 参与同一 ROS 2 实验的每个终端都在启动节点前执行；数值一致才能按该域发现彼此。
2
# 只改变本终端及其子进程的 Domain；相同系统各终端保持一致，它不是安全认证设置。
3
export ROS_DOMAIN_ID=23

1
客户端发送目标 → 服务端接受或拒绝
2
                    ↓ 接受
3
读取 position → 计算位置误差 → 发布 cmd_vel
4
       ↑                           ↓
5
       └───── 模拟器更新位置 ───────┘
6
                    ↓
7
到达 / 取消 / 状态超时 → 发布零速度 → 返回最终结果

1
#!/usr/bin/env python3
2
# 检查 NaN/无穷等非有限输入，避免异常数值进入速度或位置计算。
3
import math
4
# 为可能并发的回调保护共享状态；锁不负责调度回调。
5
import threading
6
# monotonic 用于持续时间与超时，不受系统日期校时影响；sleep 只等待，不更新位置。
7
import time
8
# ROS 1 Action 客户端与服务端，管理目标、反馈、取消和最终状态。
9
import actionlib
10
# ROS 1 Python 客户端库：节点、话题、服务、日志和时钟 API。
11
import rospy
12
# Twist 承载速度；PointStamped 额外携带坐标系、时间戳与位置。
13
from geometry_msgs.msg import PointStamped, Twist
14
# 从 ROS 1 接口包生成的 Python 类型导入，不是手工编写这些消息类。
15
from robot_msgs.msg import MoveToAction, MoveToFeedback, MoveToResult
16

17

18
# 闭环任务服务端：依据 position 测量计算命令，不能直接修改模拟器的 x。
19
class MoveServer:
20
    # 构造阶段建立资源和初始状态；构造完成后再由 spin 驱动事件处理。
21
    def __init__(self):
22
        # 保护多个回调共同访问的变量，读取和更新同一组状态都要使用同一把锁。
23
        self.lock = threading.Lock()
24
        # None 表示尚无测量，不能把默认 0 当作已经收到的位置。
25
        self.x = None
26
        self.last_state = 0.0
27
        # 创建相对名称的发布者，随 namespace 解析；保留对象供后续 publish 使用。
28
        self.publisher = rospy.Publisher('cmd_vel', Twist, queue_size=10)
29
        # 注册消息到达时执行的函数；此处只是建立订阅，不会主动读取一条消息。
30
        self.subscription = rospy.Subscriber('position', PointStamped, self.on_position)
31
        # 建立动作端点，分别绑定执行/接收/取消行为；这一步不发送运动目标。
32
        self.server = actionlib.SimpleActionServer(
33
            'move_to', MoveToAction, execute_cb=self.execute, auto_start=False)
34
        # ROS 1 使用 auto_start=False，完成对象初始化后再开始接收目标。
35
        self.server.start()
36

37
    # 接收外部位置测量，并记下本机收到消息的时刻，用于检测反馈中断。
38
    def on_position(self, message):
39
        # 只接收约定参考系内的有限位置；不匹配时不更新新鲜度，随后会触发超时。
40
        if message.header.frame_id == 'map' and math.isfinite(message.point.x):
41
            # 在锁内读取/修改共享状态；离开 with 自动释放，避免计算期间被另一回调改写。
42
            with self.lock:
43
                self.x = message.point.x
44
                # 记录最后一次有效反馈的接收时刻；仅代表接收新鲜度，不是网络延迟估计。
45
                self.last_state = time.monotonic()
46

47
    # 耗时任务回调：持续读取反馈，直到到达、取消、状态过期或总体超时。
48
    def execute(self, goal):
49
        # 结果默认未到达；只有反馈证明误差满足阈值时才设置 reached。
50
        result = MoveToResult(reached=False, final_x=0.0)
51
        # 边界处拒绝非法数值及超范围值；不要让它们进入控制计算。
52
        if not math.isfinite(goal.target_x) or abs(goal.target_x) > 10.0:
53
            # 显式进入失败终态，不能以 reached=false 冒充成功状态。
54
            self.server.set_aborted(result, 'target_x must be finite and within [-10, 10]')
55
            return
56
        # 记录整次任务的起点，分别用于首次反馈等待和总体超时。
57
        started = time.monotonic()
58
        # 默认按失败结束，成功/取消必须由后续条件显式确认。
59
        status = 'aborted'
60
        reason = 'shutdown or timeout'
61
        try:
62
            # ROS 1 退出标志置位后结束循环，不把程序写成永不退出的 while True。
63
            while not rospy.is_shutdown():
64
                now = time.monotonic()
65
                # 在锁内读取/修改共享状态；离开 with 自动释放，避免计算期间被另一回调改写。
66
                with self.lock:
67
                    # 一次性取得位置与对应接收时间，防止读到来自不同更新周期的组合。
68
                    x, stamp = self.x, self.last_state
69
                if x is not None:
70
                    result.final_x = x
71
                # 每轮主动检查取消；ROS 1 这里也包含被新目标替代的情况。
72
                if self.server.is_preempt_requested():
73
                    status, reason = 'canceled', 'cancel or replacement requested'
74
                    break
75
                # 启动后先允许最多 2 秒等待第一条有效反馈，不立即把尚未发现消息当成失败。
76
                if x is None and now - started < 2.0:
77
                    # 暂停控制循环约 50 ms；位置仍由独立模拟器积分，其他回调需有机会运行。
78
                    time.sleep(0.05)
79
                    continue
80
                # 无反馈、反馈超过 0.5 秒未更新、或任务超过 60 秒，任一成立就中止。
81
                if x is None or now - stamp > 0.5 or now - started > 60.0:
82
                    reason = 'missing/stale position or movement timeout'
83
                    break
84
                # 目标是 map 中的绝对 x；用目标减测量，而不是在本地给位置加步长。
85
                error = goal.target_x - x
86
                # 发送当前测量作为进度；反馈与最终 Result 是不同消息。
87
                self.server.publish_feedback(MoveToFeedback(current_x=x))
88
                # 反馈误差不超过 2 cm 才认为到达；不是根据运行时间猜测成功。
89
                if abs(error) <= 0.02:
90
                    result.reached = True
91
                    status = 'success'
92
                    break
93
                command = Twist()
94
                # 比例系数 2.0 将位置误差映射为速度，再限幅到 ±0.5 m/s。
95
                command.linear.x = max(-0.5, min(0.5, 2.0 * error))
96
                self.publisher.publish(command)
97
                # 暂停控制循环约 50 ms；位置仍由独立模拟器积分，其他回调需有机会运行。
98
                time.sleep(0.05)
99
        # 无论正常完成还是发生异常，都执行本分支的资源/状态清理。
100
        finally:
101
            # 发送全零速度作为结束命令；真实设备仍需状态确认，本模拟器另有命令超时兜底。
102
            self.publisher.publish(Twist())
103
        if status == 'success':
104
            # 显式进入成功终态；客户端随后可以取得最终结果。
105
            self.server.set_succeeded(result)
106
        elif status == 'canceled':
107
            # 显式标记任务已取消/被替代，区别于仅接收到取消请求。
108
            self.server.set_preempted(result, reason)
109
        else:
110
            # 显式进入失败终态，不能以 reached=false 冒充成功状态。
111
            self.server.set_aborted(result, reason)
112

113

114
# 直接执行此文件时调用入口；被别的模块导入时不自动启动节点。
115
if __name__ == '__main__':
116
    # 向 ROS 1 注册节点名称；必须先初始化，再创建依赖节点身份的通信对象。
117
    rospy.init_node('move_server')
118
    server = MoveServer()
119
    # 保持节点存活以处理通信；不等于手工重复调用订阅回调。
120
    rospy.spin()

1
#!/usr/bin/env python3
2
# ROS 1 Action 客户端与服务端，管理目标、反馈、取消和最终状态。
3
import actionlib
4
# ROS 1 Python 客户端库：节点、话题、服务、日志和时钟 API。
5
import rospy
6
# 从 ROS 1 接口包生成的 Python 类型导入，不是手工编写这些消息类。
7
from robot_msgs.msg import MoveToAction, MoveToGoal
8

9
# 向 ROS 1 注册节点名称；必须先初始化，再创建依赖节点身份的通信对象。
10
rospy.init_node('move_client')
11
# 客户端连接动作名称与类型；此时还未发送目标。
12
client = actionlib.SimpleActionClient('move_to', MoveToAction)
13
# 最多等待 5 秒发现 Action 服务端；服务端不存在时不盲目发送目标。
14
if not client.wait_for_server(rospy.Duration(5.0)):
15
    raise RuntimeError('move_to server unavailable')
16
# 发送 ROS 1 目标，并注册每次反馈到来时执行的显示函数。
17
client.send_goal(
18
    MoveToGoal(target_x=1.0),
19
    feedback_cb=lambda feedback: rospy.loginfo('x=%.2f', feedback.current_x),
20
)
21
# 只在主流程等待最多 5 秒；到期未结束则发起取消，而不是假定任务已停止。
22
if not client.wait_for_result(rospy.Duration(5.0)):
23
    # 向 ROS 1 服务端请求取消，后面仍需要等待其终态反馈。
24
    client.cancel_goal()
25
    # 给服务端额外时间处理取消，随后查看状态与结果。
26
    client.wait_for_result(rospy.Duration(2.0))
27
rospy.loginfo('state=%s, result=%s', client.get_state(), client.get_result())

1
# ROS 1 Action 服务端终端执行；模拟器已启动并启用，固定速度发布者已停止。
2
# 赋予脚本执行权限，使 rosrun 能直接启动；这不会运行脚本。
3
chmod +x ~/catkin_ws/src/ros1_basics/scripts/move_server.py
4
# 赋予脚本执行权限，使 rosrun 能直接启动；这不会运行脚本。
5
chmod +x ~/catkin_ws/src/ros1_basics/scripts/move_client.py
6
# 启动动作服务端并等待目标；此命令不会自行发送目标。
7
rosrun ros1_basics move_server.py