ROS 机器人仿真环境搭建与任务控制脚本实现

• Gazebo 界面：应该会自动弹出一个 3D 仿真窗口。
• 加载过程：第一次启动可能会卡顿几分钟（因为在下载/加载模型），请耐心等待。
• 检查场景：能看到机器人（Oryxbot），且看到场地围栏和加工台，加工台上贴有 AR 码（黑白相间的二维码图片）。

2. 启动底盘导航与对接功能 (Terminal 2) 这个 Launch 文件会启动两个关键服务：

• relative_move：让底盘走直线（基于里程计）。
• base_pose_adjust：识别底盘摄像头的 AR 码，用于精准泊车。 打开第 2 个终端，执行：

source ~/robot_ws/devel/setup.bash roslaunch oryxbot_description ar_base_gazebo.launch

注意：执行后，终端会停在那里不动（可能会显示 started），这是正常的！它启动的是后台服务节点，不会弹新窗口。

3. 启动机械臂抓取功能 (Terminal 3) 这个 Launch 文件会启动：

• hand_ar：识别手部摄像头的 AR 码，用于抓取定位。
• pick_ar_gazebo：提供'去抓取'的服务接口。 打开第 3 个终端，执行：

source ~/robot_ws/devel/setup.bash roslaunch oryxbot_description pick_ar_gazebo.launch

同样，这里也不会弹新窗口，只要不报红色 Error 就是成功。

4. 测试识别能力： 在 Gazebo 中，手动点击工具栏的'十字箭头'图标，把机器人拖拽到 5 号加工中心（那个台子上贴着 AR 码）的正前方，距离大概 0.5 米，让底盘前方的摄像头正对着码。 然后在终端 4 执行：

rostopic echo /ar_pose_marker

预期结果：你应该能看到数据疯狂刷屏（显示检测到的 ID 和 position）。如果有数据，说明机器人已经能'看懂'AR 码了。

以上就是对其简单功能的测试，针对于抽到的任务需要进行定制修改。

4、添加二维码物料

1. 针对抽取到的任务进行物料编辑，这里以'将机器人 buff 上 Id-3 物料放置到 5#加工中心;将 id-9 物料从 1#加工中心取回。'为例展开说明：

2. 首先明确需要两个物料，一个是 id3 一个是 id9，这里可以参考'/robot_ws/src/models/small_marker_charge_pile'文件格式来编写相应的物料，以下以 id3 的创建为例，id9 也一样。 1> 在/.gazebo/models/下创建文件夹 marker_id3 文件夹。 2> 创建 materials 文件夹用于存储图片和描述如何加载到 gazebo 中，在其目录下创建以下文件：

materials/
├── scripts
│   └── reinovo.material
└── textures
    ├── id3.png
    └── id9.png

以下是 reinovo.material 文件内容：

material reinovo/id3_marker { receive_shadows on technique { pass { ambient 1.0 1.0 1.0 1.0 diffuse 1.0 1.0 1.0 1.0 specular 0.5 0.5 0.5 1.0 lighting on texture_unit { texture id3.png } } } }
material reinovo/id9_marker { receive_shadows on technique { pass { ambient 1.0 1.0 1.0 1.0 diffuse 1.0 1.0 1.0 1.0 specular 0.5 0.5 0.5 1.0 lighting on texture_unit { texture id9.png } } } }

这里将 id3 和 id9 一块声明了，不影响使用。

3. 创建模型文件用来加载二维码图片放到小白块上并添加物理属性：

marker_id3/
├── materials
│   ├── scripts
│   │   └── reinovo.material
│   └── textures
│       ├── id3.png
│       └── id9.png
├── model.config
└── model.sdf

meshes 文件内容不用管，这里 model.sdf 是模型文件，类似于 3D 建模，.config 为配置文件进行版本信息声明于维护，以下为相应的内容，先写.config 文件，其次是.sdf。

model.config:

<?xml version="1.0" ?> <model> <name>id3_marker</name> <version>1.0</version> <sdf version="1.7">model.sdf</sdf> <author> <name></name> <email></email> </author> <description></description> </model>

model.sdf:

<?xml version='1.0'?> <sdf version='1.7'> <model name='marker_id3'> <link name='link_0'> <inertial> <mass>0.1</mass> <inertia> <ixx>0.0001</ixx> <ixy>0</ixy> <ixz>0</ixz> <iyy>0.0001</iyy> <iyz>0</iyz> <izz>0.0001</izz> </inertia> <pose>0 0 0 0 0 0</pose> </inertial> <pose>0 0 0.015 0 0 0</pose> <gravity>1</gravity> <self_collide>0</self_collide> <kinematic>0</kinematic> <visual name='visual'> <geometry> <box> <size>0.03 0.03 0.03</size> </box> </geometry> <material> <script> <uri>file://media/materials/scripts/gazebo.material</uri> <name>Gazebo/White</name> </script> </material> </visual> <collision name='collision'> <geometry> <box> <size>0.03 0.03 0.03</size> </box> </geometry> <surface> <friction> <ode> <mu>100</mu> <mu2>100</mu2> </ode> </friction> <contact> <ode> <kp>10000000.0</kp> <kd>1.0</kd> <min_depth>0.001</min_depth> <max_vel>0.1</max_vel> </ode> </contact> </surface> </collision> </link> <link name='link_1'> <inertial> <mass>0.001</mass> <inertia> <ixx>0.00001</ixx> <ixy>0</ixy> <ixz>0</ixz> <iyy>0.00001</iyy> <iyz>0</iyz> <izz>0.00001</izz> </inertia> </inertial> <pose>0 0 0.0305 0 0 0</pose> <visual name='visual'> <geometry> <box> <size>0.028 0.028 0.001</size> </box> </geometry> <material> <script> <uri>model://small_marker_charge_pile/materials/scripts</uri> <uri>model://small_marker_charge_pile/materials/textures</uri> <name>reinovo/id3_marker</name> </script> </material> </visual> </link> <joint name='link_0_JOINT_0' type='fixed'> <parent>link_0</parent> <child>link_1</child> <pose>0 0 0 0 0 0</pose> </joint> </model> </sdf>

5、添加到 Gazebo 并创建新世界模型

1. 打开 gazebo 世界文件：暂定仿真运行：左下角有个暂停按钮，点击后物理效果将会取消，便于调整模型位置。

2. 左侧 insert 插入模型： 1> 提前将模型路径添加到~/.bashrc 文件里，export 等等。 2> 然后选中需要插入的模型，这里以自己定义的 id3_marker 物料为例，放入到模型里。 3> 选中模型，按 T 调整位置、按 R 调整位姿、按 Esc 退出调整。 4> 退出后点击左下角的运行，看看效果。

3. 保存世界文件：点击左上角的 file 文件选择保存或者另存为，这里建议替换到原先加载的世界模型，如果另存为新的.world 文件，则需要修改 gazebo.launch 文件的参数（新生成的.world 文件路径）。

6、控制测试

1. 键盘控制小车运动（测试后可关闭，将小车移到初始位置）： sudo apt install ros-noetic-teleop-twist-keyboard 安装控制包 执行控制命令：rosrun teleop_twist_keyboard teleop_twist_keyboard.py

2. 编写主控脚本 (Python) cd ~/robot_ws/src/oryxbot_description/src/ touch mission_controller.py gedit mission_controller.py

#!/usr/bin/env python3 # -*- coding: utf-8 -*- import rospy import time from geometry_msgs.msg import Pose, Pose2D from std_srvs.srv import Empty from arm_controller.srv import PickPlace, move from relative_move.srv import SetRelativeMove from ar_pose.srv import Track # ================= 🗺️ 核心坐标与参数配置 ================= # --- 1. 场地关键点 (根据你的标定数据) --- START_X = 0.034 START_Y = -0.001 # 5 号加工中心 (任务一：上料点) STATION_5_X = 0.755 STATION_5_Y = 1.162 STATION_5_ID = 5 # 1 号加工中心 (任务二：取料点) STATION_1_X = 2.221 STATION_1_Y = 2.144 STATION_1_ID = 1 # --- 2. 机械臂动作参数 (单位：mm) --- # Buffer (车上的储物槽) # 设定：左侧放 ID-3，右侧放 ID-9 (为了防止打架) BUFFER_LEFT_X = 0.0 # 前后 BUFFER_LEFT_Y = 180.0 # 左右 (左侧) BUFFER_RIGHT_X = 0.0 BUFFER_RIGHT_Y = -180.0 # 右侧 (如果 buff 够宽) 或者就用同一个位置分先后 BUFFER_Z_LIFT = 100.0 # 抬起高度 BUFFER_Z_GRAB = 45.0 # 抓取高度 # 加工台交互位置 TABLE_EXTEND_X = 300.0 # 伸出距离 TABLE_EXTEND_Y = 0.0 # 正前方 TABLE_Z_DROP = 200.0 # 放置高度 TABLE_Z_HOVER = 240.0 # 悬停高度 # =============================================================== class MissionCommander: def __init__(self): rospy.init_node('mission_commander', anonymous=True) rospy.loginfo(">>> 正在连接机器人底层服务...") try: rospy.wait_for_service('/relative_move', timeout=10) rospy.wait_for_service('/ar_track', timeout=10) rospy.wait_for_service('/pick_ar', timeout=10) rospy.wait_for_service('/goto_position', timeout=10) rospy.wait_for_service('/swiftpro/on', timeout=10) except rospy.ROSException: rospy.logerr("连接超时！请检查 run_mission.sh 是否正确启动了所有节点。") exit(1) self.srv_move_base = rospy.ServiceProxy('/relative_move', SetRelativeMove) self.srv_align = rospy.ServiceProxy('/ar_track', Track) self.srv_pick_ar = rospy.ServiceProxy('/pick_ar', PickPlace) self.srv_arm_move = rospy.ServiceProxy('/goto_position', move) self.srv_pump_on = rospy.ServiceProxy('/swiftpro/on', Empty) self.srv_pump_off = rospy.ServiceProxy('/swiftpro/off', Empty) rospy.loginfo(">>> 系统就绪。") def move_chassis(self, target_x, target_y): """底盘移动 (带避障)""" dx = target_x - START_X dy = target_y - START_Y rospy.loginfo(f"🚗 导航启动 -> 目标:({target_x:.2f}, {target_y:.2f})") try: goal_pose = Pose2D() goal_pose.x = dx goal_pose.y = dy goal_pose.theta = 0.0 # 开启避障：avoid_obstacle=True self.srv_move_base(goal_pose, "odom", True, False) time.sleep(1) except Exception as e: rospy.logerr(f"底盘移动失败：{e}") def align_tag(self, tag_id): """视觉精准泊车""" rospy.loginfo(f"👀 视觉对准 -> ID-{tag_id}") try: self.srv_align(ar_id=tag_id, goal_dist=0.3) time.sleep(1) except Exception as e: rospy.logwarn(f"对准失败 (可能已被挡住): {e}") def move_arm(self, x, y, z): """机械臂简单移动""" p = Pose() p.position.x = float(x) p.position.y = float(y) p.position.z = float(z) self.srv_arm_move(pose=p) time.sleep(0.8) # ================= 任务流程 ================= def run_task_1(self): """任务一：将 Buffer 里的 ID-3 送到 5 号台""" rospy.loginfo("\n========== [任务一] 上料 (ID-3 -> 5 号台) ==========") # 1. 导航 self.move_chassis(STATION_5_X, STATION_5_Y) self.align_tag(STATION_5_ID) # 2. 机械臂动作 (盲抓 Buffer -> 放台子) rospy.loginfo(" >>> 动作：从车上抓取 ID-3 放到台面") self.move_arm(150, 0, 150) # 复位 # 抓取 Buffer 左侧 self.move_arm(BUFFER_LEFT_X, BUFFER_LEFT_Y, BUFFER_Z_LIFT) self.move_arm(BUFFER_LEFT_X, BUFFER_LEFT_Y, BUFFER_Z_GRAB) self.srv_pump_on() time.sleep(0.5) self.move_arm(BUFFER_LEFT_X, BUFFER_LEFT_Y, BUFFER_Z_LIFT) # 放置到台面 self.move_arm(TABLE_EXTEND_X, TABLE_EXTEND_Y, TABLE_Z_HOVER) self.move_arm(TABLE_EXTEND_X, TABLE_EXTEND_Y, TABLE_Z_DROP) self.srv_pump_off() time.sleep(0.5) self.move_arm(150, 0, 150) # 收回 # 3. 返回起点 rospy.loginfo(" >>> 返回起点...") self.move_chassis(START_X, START_Y) def run_task_2_setup(self): """任务二 (准备阶段)：把 ID-9 送到 1 号台 (帮你布置考场)""" rospy.loginfo("\n========== [任务二·准备] 布置 ID-9 到 1 号台 ==========") rospy.loginfo("提示：请确保你已经在 Gazebo 里把 ID-9 方块放到了小车 Buffer 上！") # 1. 导航 self.move_chassis(STATION_1_X, STATION_1_Y) self.align_tag(STATION_1_ID) # 2. 放置动作 (盲抓 Buffer -> 放台子) # 假设 ID-9 也放在 Buffer 左侧 (因为任务一已经把那里的 ID-3 拿走了，空出来了) self.move_arm(150, 0, 150) self.move_arm(BUFFER_LEFT_X, BUFFER_LEFT_Y, BUFFER_Z_LIFT) self.move_arm(BUFFER_LEFT_X, BUFFER_LEFT_Y, BUFFER_Z_GRAB) self.srv_pump_on() time.sleep(0.5) self.move_arm(BUFFER_LEFT_X, BUFFER_LEFT_Y, BUFFER_Z_LIFT) self.move_arm(TABLE_EXTEND_X, TABLE_EXTEND_Y, TABLE_Z_HOVER) self.move_arm(TABLE_EXTEND_X, TABLE_EXTEND_Y, TABLE_Z_DROP) self.srv_pump_off() time.sleep(0.5) self.move_arm(150, 0, 150) # 3. 返回起点 (准备开始正式考试) rospy.loginfo(" >>> 布置完成，返回起点，准备正式取料...") self.move_chassis(START_X, START_Y) time.sleep(2) def run_task_2_execute(self): """任务二 (正式阶段)：把 ID-9 从 1 号台取回来""" rospy.loginfo("\n========== [任务二·正式] 从 1 号台取回 ID-9 ==========") # 1. 导航 self.move_chassis(STATION_1_X, STATION_1_Y) self.align_tag(STATION_1_ID) # 2. 智能抓取 (使用 pick_ar 视觉识别) rospy.loginfo(" >>> 动作：视觉寻找 ID-9 并抓回") # 设定放回目标 (放回 Buffer) buffer_target = Pose() buffer_target.position.x = BUFFER_LEFT_X buffer_target.position.y = BUFFER_LEFT_Y buffer_target.position.z = BUFFER_Z_GRAB + 20 # 稍微高一点 # 调用视觉抓取服务 self.srv_pick_ar(number=9, mode=0, pose=buffer_target) # 3. 返回起点 time.sleep(2) rospy.loginfo(" >>> 取料完成，返回起点...") self.move_chassis(START_X, START_Y) def run(self): # 1. 执行任务一 self.run_task_1() time.sleep(3) # 2. 执行任务二 (按照你的思路：先送过去，再取回来) self.run_task_2_setup() # 阶段 A：送过去 self.run_task_2_execute() # 阶段 B：取回来 rospy.loginfo("\n🏆 所有任务圆满结束！") if __name__ == "__main__": cmdr = MissionCommander() # ⚠️ 重要提示： # 为了让代码能抓到东西，运行脚本前，请在 Gazebo 里 # 手动把 ID-3 和 ID-9 两个方块，拖拽放到小车后斗上！ cmdr.run()

3. 缺乏机械臂的控制核心，需要手动启动：
4. 启动 IK (逆运动学) 节点 这是机械臂的'小脑'，负责把坐标算出角度。

source ~/robot_ws/devel/setup.bash rosrun oryxbot_description ik_swiftpro

(注意：执行后终端可能没有输出并卡住，这是正常的，它在后台等待计算请求。) *以上是整体控制的流程。

7、一键脚本启动*

结合上述内容，编写一键脚本启动文件：

#!/bin/bash # 定义清理函数：脚本退出时自动执行 cleanup() { echo "正在关闭所有进程..." # 杀掉所有后台子进程 kill $(jobs -p) 2>/dev/null # 强制清理 ROS 和 Gazebo 残留 killall -9 gzserver gzclient rosmaster rosout nodelet 2>/dev/null echo ">>> 环境已清理完毕，随时可以再次启动。" } # 捕获退出信号 (Ctrl+C) trap cleanup EXIT echo "==========================================" echo " 课程设计任务一键启动脚本" echo "==========================================" # 1. 环境清理 echo "[1/4] 清理旧环境..." killall -9 gzserver gzclient rosmaster 2>/dev/null sleep 2 # 2. 加载环境变量 source ~/robot_ws/devel/setup.bash # 3. 启动仿真环境 (底盘 + 视觉+Gazebo) echo "[2/4] 正在启动仿真环境 (Gazebo)..." # 注意：使用 nohup 或 & 后台运行，并屏蔽日志输出以免刷屏 roslaunch oryxbot_description gazebo.launch > /dev/null 2>&1 & PID_GAZEBO=$! echo " 等待 Gazebo 加载 (15 秒)..." sleep 15 # 4. 启动任务层 (Pick AR + 机械臂 IK) echo "[3/4] 正在启动控制节点 (IK & Pick)..." echo "启动底盘导航与对接功能..." roslaunch oryxbot_description ar_base_gazebo.launch> /dev/null 2>&1 & sleep 5 echo "启动机械臂抓取功能..." roslaunch oryxbot_description pick_ar_gazebo.launch > /dev/null 2>&1 & sleep 5 echo " 启动 IK (逆运动学) 节点..." rosrun oryxbot_description ik_swiftpro > /dev/null 2>&1 & sleep 3 # 5. 运行 Python 主控脚本 echo "[4/4] 启动任务控制脚本..." echo "================ 日志输出 ================" # 切换到脚本所在目录运行 cd ~/robot_ws/src/oryxbot_description/src/ python3 mission_controller.py # 脚本运行结束后，等待用户按键再退出 echo "==========================================" echo "任务脚本已结束。" echo "按 Enter 键关闭仿真并退出..." read dummy