睿抗机器人大赛魔力元宝仿真环境搭建与任务控制实战

# 临时生效
cd ~/robot_ws
source devel/setup.bash

# 永久生效（写入 .bashrc）
echo "source ~/robot_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc

3. 加载 Gazebo 仿真与测试

启动环境

在第一个终端执行：

roslaunch oryxbot_description gazebo.launch

等待 Gazebo 界面弹出。首次启动可能需要几分钟下载模型，请耐心等待。

检查场景：应能看到机器人（Oryxbot）、场地围栏、加工台以及贴有 AR 码的二维码图片。

启动底盘导航与对接功能

打开第二个终端，执行：

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

该 Launch 文件会启动 relative_move（直线移动）和 base_pose_adjust（AR 码精准泊车）。执行后终端可能无输出并卡住，这是正常的，说明后台服务已就绪。

启动机械臂抓取功能

打开第三个终端，执行：

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

这会启动 hand_ar（手部视觉识别）和 pick_ar_gazebo（抓取服务接口）。只要不报红色 Error 即为成功。

测试识别能力

在 Gazebo 中拖动机器人到 5 号加工中心正前方约 0.5 米处，让摄像头对准 AR 码。在第四个终端执行：

rostopic echo /ar_pose_marker

如果看到数据疯狂刷屏（显示 ID 和 position），说明视觉识别正常。

4. 添加二维码物料

针对抽取到的任务（如将 ID-3 放置到 5 号台，从 1 号台取回 ID-9），需要自定义物料模型。

参考 ~/robot_ws/src/models/small_marker_charge_pile 的结构来编写。以创建 ID-3 为例：

1. 在 ~/.gazebo/models/ 下创建文件夹 marker_id3。
2. 创建 materials 文件夹，内部结构如下：

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

这里同时声明了 ID-3 和 ID-9 的材质，不影响使用。

3. 创建模型文件 model.sdf 和 model.config。

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><iyy>0.0001</iyy><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>
      <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></surface>
      </collision>
    </link>
    <!-- 其他链接与关节定义略 -->
  </model>
</sdf>

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

1. 打开 Gazebo 世界文件，点击左下角暂停按钮取消物理效果，便于调整模型位置。
2. 左侧 Insert 菜单中选择模型，确保路径已在 .bashrc 中导出。
3. 选中模型，按 T 调整位置，按 R 调整位姿，Esc 退出。
4. 点击运行查看效果，确认无误后保存世界文件（File -> Save As）。若另存为新 .world 文件，需修改 gazebo.launch 中的参数指向新路径。

6. 控制测试

键盘控制小车

安装控制包并测试运动：

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

测试后可关闭，将小车移回初始位置。

编写主控脚本

在 ~/robot_ws/src/oryxbot_description/src/ 下创建 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

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("连接超时！请检查节点是否启动。")
            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 - 0.034
        dy = target_y - (-0.001)
        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
            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 run_task_1(self):
        rospy.loginfo("\n========== [任务一] 上料 (ID-3 -> 5 号台) ==========")
        self.move_chassis(0.755, 1.162)
        self.align_tag(5)
        # 机械臂动作序列略...
        rospy.loginfo(" >>> 返回起点...")
        self.move_chassis(0.034, -0.001)

if __name__ == "__main__":
    cmdr = MissionCommander()
    cmdr.run_task_1()

启动 IK 节点

机械臂控制核心需要手动启动逆运动学节点：

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

执行后终端可能无输出，这是正常的，它在后台等待计算请求。

7. 一键脚本启动

为了方便调试，可以编写一个 Bash 脚本整合所有启动步骤。脚本会自动清理旧进程、加载环境、依次启动 Gazebo、导航服务、机械臂服务及 Python 主控脚本。

#!/bin/bash
cleanup() {
    echo "正在关闭所有进程..."
    kill $(jobs -p) 2>/dev/null
    killall -9 gzserver gzclient rosmaster rosout nodelet 2>/dev/null
    echo ">>> 环境已清理完毕。"
}
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. 启动仿真环境
echo "[2/4] 正在启动仿真环境 (Gazebo)..."
roslaunch oryxbot_description gazebo.launch > /dev/null 2>&1 &
sleep 15

# 4. 启动控制节点
echo "[3/4] 正在启动控制节点 (IK & Pick)..."
roslaunch oryxbot_description ar_base_gazebo.launch > /dev/null 2>&1 &
sleep 5
roslaunch oryxbot_description pick_ar_gazebo.launch > /dev/null 2>&1 &
sleep 5
rosrun oryxbot_description ik_swiftpro > /dev/null 2>&1 &
sleep 3

# 5. 运行 Python 主控脚本
echo "[4/4] 启动任务控制脚本..."
cd ~/robot_ws/src/oryxbot_description/src/
python3 mission_controller.py

echo "任务脚本已结束。"
echo "按 Enter 键关闭仿真并退出..."
read dummy

运行前请确保已在 Gazebo 中将物料方块手动拖拽到小车 Buffer 上，否则抓取逻辑无法执行。