睿抗机器人大赛 Oryxbot 机器人 Gazebo 仿真与 Python 控制实现

睿抗机器人大赛中 Oryxbot 机器人的环境搭建与仿真控制流程。内容包括 ROS Noetic 工作空间配置、Gazebo 仿真模型加载与插件设置、AR 码识别测试、自定义二维码物料创建、以及基于 Python 的机械臂抓取与底盘导航主控脚本编写。最后提供了一键启动脚本以整合所有服务节点，实现从视觉定位到任务执行的自动化控制。

性能调优发布于 2026/4/5更新于 2026/9/893 浏览

1、搭建基础环境

mkdir -p ~/robot_ws/src
cd ~/robot_ws/src

将源码包中的以下 4 个文件夹复制到 ~/robot_ws/src 下：

ar_pose
oryxbot_description
relative_move
pid_lib 注意：不要直接把整个仓库文件夹放进去，要剥离出核心功能包。

安装仿真依赖：

# 下载 ar_track_alvar 源码（涉及二维码识别）
cd ~/robot_ws/src
git clone https://gitee.com/reinovo/ar_track_alvar.git

# 安装 ROS Noetic 的常用控制与仿真依赖
sudo apt update
sudo apt install ros-noetic-moveit \
        ros-noetic-ros-control \
        ros-noetic-ros-controllers \
        ros-noetic-gazebo-ros-control \
        ros-noetic-joint-state-controller \
        ros-noetic-effort-controllers \
        ros-noetic-position-controllers \
        ros-noetic-driver-base \
        ros-noetic-ackermann-msgs

以上安装包建议直接复制多行执行，避免单行报错。

配置仿真模型与插件 (Compatibility Fix)

配置模型文件：将压缩包里的 models 内容复制到 ~/.gazebo/models/ 目录下（.gazebo 为隐藏文件，在主目录下按 Ctrl+H 显示）。
配置插件：在源码包 gazebo_plugins 里有两个文件夹：contact_plugin_18-04_g9 (对应 Ubuntu 18.04) 和 contact_plugin_20-04_g11。ROS Noetic 必须使用 contact_plugin_20-04_g11。编写 ~/.bashrc 文件：

export GAZEBO_PLUGIN_PATH=${GAZEBO_PLUGIN_PATH}:/你的实际路径/gazebo_plugins/contact_plugin_20-04_g11

2、编译与设置环境

编译环境：

cd ~/robot_ws
rm -rf build/ devel/
catkin_make

设置环境变量：

cd ~/robot_ws
source devel/setup.bash

每次打开新的终端都要输入来刷新，或者写入到 ~/.bashrc 文件里：source ~/robot_ws/devel/setup.bash，使打开终端自动生效。

3、加载 Gazebo 仿真与测试

启动环境（底盘、移动、Gazebo 世界）：

roslaunch oryxbot_description gazebo.launch

观察与等待：

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

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

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

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

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

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

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

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

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

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

rostopic echo /ar_pose_marker

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

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

4、添加二维码物料

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

首先明确需要两个物料，一个是 ID-3 一个是 ID-9，这里可以参考 ~/robot_ws/src/models/small_marker_charge_pile 文件格式来编写相应的物料，以下以 ID-3 的创建为例，ID-9 也一样。

在 ~/.gazebo/models/ 下创建文件夹 marker_id3。

创建 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 } } } }

这里将 ID-3 和 ID-9 一块声明了，不影响使用。

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

marker_id3/
├── materials
│   ├── scripts
│   │   └── reinovo.material
│   └── textures
│       ├── id3.png
│       └── id9.png
├── model.config
└── model.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 并创建新世界模型

打开 Gazebo 世界文件：暂定仿真运行：左下角有个暂停按钮，点击后物理效果将会取消，便于调整模型位置。
左侧 Insert 插入模型：
- 提前将模型路径添加到 ~/.bashrc 文件里，export 等等。
- 然后选中需要插入的模型，这里以自己定义的 ID-3_marker 物料为例，放入到模型里。
- 选中模型，按 T 调整位置、按 R 调整位姿、按 Esc 退出调整。
- 退出后点击左下角的运行，看看效果。
保存世界文件：点击左上角的 File 文件选择保存或者另存为，这里建议替换到原先加载的世界模型，如果另存为新的 .world 文件，则需要修改 gazebo.launch 文件的参数（新生成的 .world 文件路径）。

6、控制测试

键盘控制小车运动（测试后可关闭，将小车移到初始位置）：

sudo apt install ros-noetic-teleop-twist-keyboard
rosrun teleop_twist_keyboard teleop_twist_keyboard.py

编写主控脚本 (Python)

cd ~/robot_ws/src/oryxbot_description/src/
touch mission_controller.py

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()

缺乏机械臂的控制核心，需要手动启动：
- 启动 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

睿抗机器人大赛 Oryxbot 机器人 Gazebo 仿真与 Python 控制实现

性能调优发布于 2026/4/5更新于 2026/9/893 浏览

# 下载 ar_track_alvar 源码（涉及二维码识别） cd ~/robot_ws/src git clone https://gitee.com/reinovo/ar_track_alvar.git # 安装 ROS Noetic 的常用控制与仿真依赖 sudo apt update sudo apt install ros-noetic-moveit \ ros-noetic-ros-control \ ros-noetic-ros-controllers \ ros-noetic-gazebo-ros-control \ ros-noetic-joint-state-controller \ ros-noetic-effort-controllers \ ros-noetic-position-controllers \ ros-noetic-driver-base \ ros-noetic-ackermann-msgs

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 } } } }

marker_id3/ ├── materials │ ├── scripts │ │ └── reinovo.material │ └── textures │ ├── id3.png │ └── id9.png ├── model.config └── 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>

#!/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()

#!/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

睿抗机器人大赛 Oryxbot 机器人 Gazebo 仿真与 Python 控制实现

1、搭建基础环境

2、编译与设置环境

3、加载 Gazebo 仿真与测试

4、添加二维码物料

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

6、控制测试

7、一键脚本启动

睿抗机器人大赛 Oryxbot 机器人 Gazebo 仿真与 Python 控制实现

1、搭建基础环境

2、编译与设置环境

3、加载 Gazebo 仿真与测试

4、添加二维码物料

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

6、控制测试

7、一键脚本启动

更多推荐文章

相关免费在线工具

更多推荐文章

相关免费在线工具