关于2025年暑假自主巡航小车主程序的学习笔记
2025年10月23日更新：
注意代码中的代码中的input = raw_input()，这是python2中的写法，在python3中raw_input()这个函数已经被移除
代码：
点击查看代码
#!/usr/bin/env python2
#coding: utf-8
import rospy
import actionlib
from tf_conversions import transformations
from actionlib_msgs.msg import *
from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseWithCovarianceStamped, Twist, Point
from std_msgs.msg import String, Int32
from ar_track_alvar_msgs.msg import AlvarMarkers, AlvarMarker
from TTS_audio.srv import TTS # 导入TTS服务消息类型
from math import pi
import os
time = 0
vision_id = None
find_id = None
ar_id = None
vlm_id=None
nav_end = None
operator = None
image_recognition_result1 = None
image_recognition_result2 = None
calculation_result = None
ar_id_navi = None
class navigation_demo:
def __init__(self):
self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)
self.arrive_pub = rospy.Publisher('/voiceWords',String,queue_size=10)
self.nav_end_sub = rospy.Subscriber('nav_end_topic', Int32, self.nav_end_callback)
self.vlm_id=rospy.Subscriber('/vlm_id_topic', Int32, self.vlm_id_callback)
self.find_sub = rospy.Subscriber('/object_position', Point, self.find_cb);
self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb);
self.operator_sub = rospy.Subscriber('operator_topic', String, self.operator_callback)
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
self.move_base.wait_for_server(rospy.Duration(60))
self.pub = rospy.Publisher("/cmd_vel",Twist,queue_size=1000)
# 初始化TTS服务客户端
rospy.wait_for_service('tts')
self.tts_service = rospy.ServiceProxy('tts', TTS)
def end(self):
global time
msg = Twist()
msg.linear.x = 0.3
msg.linear.y = 0.3
msg.linear.z = 0.0
msg.angular.x = 0.0
msg.angular.y = 0.0
msg.angular.z = 0.0
while(time <= 13):
self.pub.publish(msg)
rospy.sleep(0.1)
time = time + 1
def trigger_photo_and_upload(self):
#rospy.init_node('trigger_photo_and_upload_node', anonymous=True)
rospy.set_param('/top_view_shot_node/im_flag', 1)
rospy.sleep(2)
result = rospy.get_param('/top_view_shot_node/result', None)
rospy.set_param('/top_view_shot_node/im_flag', 255)
def vlm_id_callback(self, msg):
global vlm_id, vision_id
vlm_id = msg.data
rospy.loginfo("收到 vlm_id: %d", vlm_id)
# 检查 vlm_id 是否在有效范围内
if 1 <= vlm_id <= 8:
vision_id = vlm_id
def ar_cb(self, data):
global ar_id,ar_id_navi
for marker in data.markers:
if ar_id_navi == 1:
ar_id = marker.id
rospy.loginfo("收到ar_id: %d", ar_id)
def find_cb(self, data):
global find_id
# 定义区间与 find_id 的映射字典
interval_mapping = {
(10, 19): 1,
(20, 29): 2,
(30, 39): 3,
(40, 49): 4,
(50, 59): 5,
(60, 69): 6,
(70, 79): 7,
(80, 89): 8
}
point_msg = data
find_id = None
for (start, end), id_value in interval_mapping.items():
if start <= point_msg.z <= end:
find_id = id_value
rospy.loginfo("收到find_id: %d", find_id)
break
# 如果没有匹配到任何区间，可根据需求设置默认值，这里可以自行调整
if find_id is None:
find_id = None
def nav_end_callback(self, msg):
global nav_end
nav_end = msg.data
rospy.loginfo("收到 nav_end: %d", nav_end)
# 新增的回调函数，用于处理接收到的operator_topic消息
def operator_callback(self, msg):
global operator
operator = msg.data
rospy.loginfo("收到 operator: %s", operator)
def set_pose(self, p):
if self.move_base is None:
return False
x, y, th = p
pose = PoseWithCovarianceStamped()
pose.header.stamp = rospy.Time.now()
pose.header.frame_id = 'map'
pose.pose.pose.position.x = x
pose.pose.pose.position.y = y
q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi)
pose.pose.pose.orientation.x = q[0]
pose.pose.pose.orientation.y = q[1]
pose.pose.pose.orientation.z = q[2]
pose.pose.pose.orientation.w = q[3]
self.set_pose_pub.publish(pose)
return True
def _done_cb(self, status, result):
rospy.loginfo("navigation done! status:%d result:%s"%(status, result))
def _active_cb(self):
rospy.loginfo("[Navi] navigation has be actived")
def _feedback_cb(self, feedback):
msg = feedback
#rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback)
def goto(self, p):
rospy.loginfo("[Navi] goto %s"%p)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = p[0]
goal.target_pose.pose.position.y = p[1]
q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi)
goal.target_pose.pose.orientation.x = q[0]
goal.target_pose.pose.orientation.y = q[1]
goal.target_pose.pose.orientation.z = q[2]
goal.target_pose.pose.orientation.w = q[3]
self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb)
result = self.move_base.wait_for_result(rospy.Duration(60))
if not result:
self.move_base.cancel_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("reach goal %s succeeded!"%p)
return True
def cancel(self):
self.move_base.cancel_all_goals()
return True
def speak(self, text):
try:
response = self.tts_service(text)
rospy.loginfo("TTS服务返回的音频文件路径: %s", response.audio_file_path)
except rospy.ServiceException as e:
rospy.logerr("调用TTS服务失败: %s", e)
# ---------------------------------------------------------
# 1. 导航
# ---------------------------------------------------------
def navigate_to_target(navi, goals, index):
"""
:param navi: navigation_demo 实例
:param goals: 目标点列表
:param index: 目标点在列表中的下标
:return: 成功与否
"""
if not (0 <= index < len(goals)):
rospy.logwarn("目标索引 %d 越界", index)
return False
navi.goto(goals[index])
return True
# ---------------------------------------------------------
# 2. 触发拍照并等待识别结果
# ---------------------------------------------------------
def wait_and_get_recognition_result(timeout=5.0):
global find_id, ar_id, vision_id
container = [None]
def _try_pick():
for v in (find_id, ar_id, vision_id):
if v is not None:
container[0] = v
return True
return False
# 清空旧数据
find_id = ar_id = vision_id = None
rate = rospy.Rate(20)
start = rospy.Time.now()
while not rospy.is_shutdown() and (rospy.Time.now() - start).to_sec() < timeout:
if _try_pick():
return container[0]
rate.sleep()
rospy.logwarn("等待识别结果超时")
return None
# ---------------------------------------------------------
# 3. 计算
# ---------------------------------------------------------
def calculate_result_from_operator(a, b):
global operator
if operator == '+':
return abs(a + b)
elif operator == '-':
return abs(a - b)
elif operator == '*':
return abs(a * b)
elif operator == '/':
if b == 0:
rospy.logerr("除数不能为 0")
return None
return abs(float(a) / b)
else:
rospy.logerr("未知算符: %s", operator)
return None
# ---------------------------------------------------------
# 4. 根据计算结果再导航
# ---------------------------------------------------------
def navigate_to_result_point(navi, goals, calc_result):
if calc_result is None:
return
idx = int(calc_result)
if 0 <= idx < len(goals):
navi.goto(goals[idx])
navi.speak("已导航到 {} 点".format(idx))
else:
rospy.logwarn("计算结果 %d 越界，无法导航", idx)
def navigate_and_calculate(navi, goals, index1, index2):
global ar_id_navi
# ---------- 阶段 1：导航到第一个目标 ----------
if not navigate_to_target(navi, goals, index1):
return
ar_id_navi = 1
navi.trigger_photo_and_upload()
res1 = wait_and_get_recognition_result()
if res1 is None:
rospy.loginfo("第一次识别结果为空，流程终止")
navi.speak("识识别结果为None")
return
navi.speak("识识别结果为 {}".format(res1))
# ---------- 阶段 2：导航到第二个目标 ----------
ar_id_navi = 0
if not navigate_to_target(navi, goals, index2):
return
ar_id_navi = 1
navi.trigger_photo_and_upload()
res2 = wait_and_get_recognition_result()
if res2 is None:
rospy.loginfo("第二次识别结果为空，流程终止")
navi.speak("识识别结果为None")
return
navi.speak("识识别结果为 {}".format(res2))
# ---------- 阶段 3：计算 ----------
calc = calculate_result_from_operator(res1, res2)
if calc is None:
return
if operator == '+':
navi.speak("计算式: {} 加 {} 等于 {}".format(res1, res2, calc))
elif operator == '-':
navi.speak("计算式: {} 减 {} 等于 {}".format(res1, res2, calc))
elif operator == '*':
navi.speak("计算式: {} 乘 {} 等于 {}".format(res1, res2, calc))
elif operator == '/':
navi.speak("计算式: {} 除 {} 等于 {}".format(res1, res2, calc))
# ---------- 阶段 4：再次导航 ----------
ar_id_navi = 0
navigate_to_result_point(navi, goals, calc)
if __name__ == "__main__":
rospy.init_node('navigation_demo', anonymous=True)
goalListX = rospy.get_param('~goalListX', '2.0, 2.0')
goalListY = rospy.get_param('~goalListY', '2.0, 4.0')
goalListYaw = rospy.get_param('~goalListYaw', '0, 90.0')
# 替换中文逗号为英文逗号
goalListX = goalListX.replace(u'，', ',')
goalListY = goalListY.replace(u'，', ',')
goalListYaw = goalListYaw.replace(u'，', ',')
goals = [[float(x), float(y), float(yaw)] for (x, y, yaw) in zip(goalListX.split(","), goalListY.split(","), goalListYaw.split(","))]
print('Please 1 to continue: ')
input = raw_input()
print(goals)
r = rospy.Rate(1)
# r.sleep(4)
navi = navigation_demo()
if input == '1':
# rospy.loginfo("收到 operator: %s", operator)
# rospy.loginfo("收到 nav_end: %d", nav_end)
# 识别终点位置
publish_audio()
rospy.sleep(18)
arrive_str = "比比赛开始"
navi.speak(arrive_str) # 调用TTS服务
# 区域一导航
navi.trigger_photo_and_upload()
navigate_and_calculate(navi, goals, 0, 10)
navi.goto(goals[18])
# 区域二导航
navigate_and_calculate(navi, goals, 14, 15)
navi.goto(goals[19])
navi.goto(goals[18])
# 区域三导航
navigate_and_calculate(navi, goals, 16, 17)
navi.goto(goals[20])
#终点导航
if nav_end is not None and nav_end >= 0 and nav_end < len(goals):
navi.goto(goals[nav_end])
navi.end()
arrive_str = "已导航到{}号终点，比赛结束".format(nav_end)
navi.speak(arrive_str) # 调用TTS服务
else:
rospy.loginfo("nav_end 值无效，无法导航")
r.sleep()
这两行为python脚本文件的头部声明，第一行叫‘Shebang’，第二行叫‘编码声明’，第一行用于告诉操作系统应该使用哪一个python解释器来运行脚本，第二行用于指定编码方式，告诉解释器自己的编码方式（默认使用ASCII编码读取文件，使用UTF-8编码可以正常处理中文字符）。
#!/usr/bin/env python2的行为：调用env命令，查找名为python2的环境变量，用于解释此python脚本文件。这种方式更加灵活，避免了硬编码。
库的引用：
ROS库：
rospy：ROS的python客户端库，用于与ROS系统进行交互。 功能：初始化节点，发布或订阅话题，调用服务。节点管理、话题通信、服务通信、参数服务器、时间管理、日志记录等。
actionlib：ROS中用于实现动作（Action）通信的库。功能：可以创建一个简单的动作客户端，提供Action通信机制。动作客户端和服务器的实现，目标发送、反馈接收、结果处理和任务取消。
tf_conversions：ROS中的一个工具包。功能：用于处理坐标转换和四元数操作。
这些都是引用的消息类型，也就是数据类型，定义了ROS通信的数据结构，这些都是ROS系统自带的消息类型。
这也是一个消息类型，但是这个是自定义的消息类型，用于语音播报。
python库：
math：python中的关于数学的标准库，引用了pi模块。
os：python中用于与操作系统交互的标准库。功能：用于处理文件和目录、环境变量、进程管理等。
关于消息类型：
点击查看代码
1. String：用于收发字符串
2. MoveBaseAction：用于声明导航动作客户端/服务器接口
3. MoveBaseGoal：用于设置导航终点位姿
4. Path：用于传递全局或局部路径
5. PoseWithCovarianceStamped：用于设定初始位姿或定位输出
6. Twist：用于发送速度指令
7. Point：用于表示空间坐标点
8. Int32：用于传递整数数据
9. AlvarMarkers：用于接收一组 AR 标记检测结果
10. AlvarMarker：并未使用，用于接收单个 AR 标记
11. TTS：自定义消息类型，用于语音播报
函数：
构造函数：
navigation_demo的构造函数，主要是用于初始化ROS发布者、ROS订阅者、ROS动作（Action）客户端和TTS服务客户端。
Publisher模块的使用：rospy.Publisher(topic_name，data_class，queue_size，[latch=False]，[tcp_nodelay=False])
注意：构造函数中的arrive_pub发布者并未使用。
注释：
点击查看代码
self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5)发布带协方差的位姿
self.arrive_pub = rospy.Publisher('/voiceWords',String,queue_size=10)发布TTS文本，但是这里并未使用
self.nav_end_sub = rospy.Subscriber('nav_end_topic', Int32, self.nav_end_callback)接收终点坐标，调用nav_end_callback函数
self.vlm_id=rospy.Subscriber('/vlm_id_topic', Int32, self.vlm_id_callback)接收大模型识别结果，调用vlm_id_callback函数
self.find_sub = rospy.Subscriber('/object_position', Point, self.find_cb);接收模板匹配结果，调用find_cb函数
self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb);接收AR码识别结果，调用ar_cb函数
self.operator_sub = rospy.Subscriber('operator_topic', String, self.operator_callback)接收运算符结果，调用operator_callback函数
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)创建ROS动作（Action）通信客户端，参数为注册名称和所遵循的规则
self.move_base.wait_for_server(rospy.Duration(60))显式等待move_base动作服务器，持续时间60s
self.pub = rospy.Publisher("/cmd_vel",Twist,queue_size=1000)向机器人地盘发布速度信息
rospy.wait_for_service('tts')显式等待tts服务上线
rospy.ServiceProxy('tts', TTS)创建客户端代理
发布者订阅者最小可运行模板：
点击查看代码
发布者：
import rospy
from geometry_msgs.msg import Twist
rospy.init_node('talker')
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10) # 只传3个必传
订阅者：
import rospy
from geometry_msgs.msg import Twist
def cb(msg):
rospy.loginfo('vx=%.2f', msg.linear.x)
rospy.init_node('listener')
rospy.Subscriber('/cmd_vel', Twist, cb) # 只传3个必传
rospy.spin()
终点函数：
由于局部规划器在L形挡板构成的终点拐角处过于保守，机器人将两侧垂直挡板误判为潜在碰撞风险，始终无法驶入拐角内部到达终点。为此，需启用终点强制模式：在保持底层安全监控的前提下，临时绕过局部规划器，直接下发终点方向的电机指令，强制机器人进入L形框内完成终点对齐。
global time全局声明time变量
msg = Twist()实例化Twist消息类型
设置线速度，得益于麦克纳姆轮，机器人可以朝着斜方向进行平移
注意：这里的13并不是13s，而是13次，这个函数看着有一点乱。
改进建议：
点击查看代码
duration = rospy.Duration(1.3) # 想跑 1.3 秒
end_time = rospy.Time.now() + duration
while rospy.Time.now() < end_time:
self.pub.publish(msg)
rospy.sleep(0.1)
触发拍照并上传
通过参数服务器直接设置参数/top_view_shot_node/im_flag为1，从而触发拍照。
这个获取参数的行为没有任何意义，可能是上一次写错了。
通过参数服务器直接设置参数/top_view_shot_node/im_flag为255，进行重置标志。
回调函数
接收msg的data属性，并将它赋值给vlm_id。
打印日志，可视化。
检查合法性，进行最终赋值。
回调函数
ar_id_navi标志位，因为ar码的识别很灵敏，所以需要是指一个标志位即flag，千万不要被变量名所误解，这个变量名就是起的不太好。
回调函数
首先定义了一个映射字典，用于匹配传过来的数据。
这里使用的时data的z属性
下面扩展了一个默认值，使用场景：有些图片的模板匹配概率太低，这是一个保底行为。
回调函数
接收终点信息
回调函数
接收运算符信息
设置位姿函数
这个函数并未使用。
回调函数
这是动作客户端的三个回调函数，服务于send_goal方法，函数名随意，签名匹配就行。分别对行开始前往目标，前往目标过程中的持续回馈，目标终止（成功，失败，取消）
导航到指定位姿函数
首先初始化MoveBaseGoal消息类型，然后往里面填东西。
frame_id框架，固定填map
stamp时间戳，调用rospy中的当前时间
X轴坐标
Y轴坐标
将坐标进行转换（学名叫：quaternion_from_euler从欧拉角生成四元数）
将转换后的数值分别填入
将坐标传到Action动作服务器
定义一个变量result，用于判断导航动作是否完成，返回值无意义，只是用来判断。
持续时间60s这是一个超时时间，按时完成则输出succeeded!，超时则输出Timed out achieving goal
语音播报函数
接收一个字符串参数
接受返回值时顺带执行了函数
导航函数
判断目标点是否合法
前往目标点
获取识别结果
初始化一个列表
定义一个辅助函数用于确定优先级
重置数值，避免数值污染
定义识别最长等待时间，这里写的有一点令人费解
优化：
点击查看代码
timeout = rospy.Duration(5.0) # 5 秒
deadline = rospy.Time.now() + timeout
while rospy.Time.now() < deadline and not rospy.is_shutdown():
if _try_pick():
return container[0]
rospy.sleep(0.05) # 50 ms 轮询，想多短都行
rospy.logwarn("等待识别结果超时")
return None
获取计算结果
注意除法的特殊性即可
根据计算结果导航到指定点
综合
主程序：
初始化节点
获取参数
处理参数
构建目标点列表
比赛硬性要求，必须有这两行
实例化navigation_demo类
分别调用函数（注意：这里的函数调用是有问题的）
执行终点导航，执行终点函数，语音播报，比赛结束
一些英文翻译：Goal目标，Feedback反馈，Result结果，Cancel取消，Status状态，Action动作，tcp_nodelayTCP无延迟，latch锁存器，queue_size队列大小，callback回调，args参数，buff_size缓冲区大小，geometry几何，navigation导航，PoseWithCovarianceStamped带协方差的位姿，TTS（Text To Speech文字转语音），vlm（vision-language-models）视觉语言大模型，duration持续时间（用于表示一段时间长度），linear线性，angular角度，trigger触发，upload上传，interval间隔（区间），mapping映射，stamp邮票（时间戳），frame框架，transformations变换，quaternion_from_euler从欧拉角生成四元数，orientation方向，container容器
浙公网安备 33010602011771号