睿抗机器人大赛：ROS 机器人仿真环境搭建与任务控制

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

roslaunch oryxbot_description gazebo.launch

观察与等待：

• 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. 针对抽取到的任务进行物料编辑，这里以'将机器人 Buffer 上 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

.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

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

1. 启动 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