睿抗机器人大赛魔力元宝

1、搭建基础环境

1、以Ros-noetic为例创建工作区间：

1. 创建工作空间目录 mkdir -p ~/catkin_ws/src cd ~/robot_ws/src

2. 这里的关键步骤：请将你上传的源码包中的以下 4 个文件夹复制到 ~/robot_ws/src 下：

- ar_pose # - oryxbot_description # - relative_move # - pid_lib

这里以moliyuanbao/relative_move/src at main · Xk-fly/moliyuanbao我所上传的源码为例

注意：不要直接把整个 xk-fly 文件夹放进去，要剥离出这 5 个核心功能包。

2、安装仿真依赖：

1. 下载 ar_track_alvar 源码 (源码脚本里提到的，这一步必须做，主要是涉及到二维码)

cd ~/catkin_ws/src

git clone https://gitee.com/reinovo/ar_track_alvar.git

2. 安装 ROS Noetic 的常用控制与仿真依赖 (防止报"缺少 controller"或"moveit"错误)

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

以上安装包直接复制多行内，一行会报错

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

1>配置模型文件：

将压缩包里的models内容复制到~/.gazebo/models/目录下（.gazebo为隐藏文件，在主目录下按ctrl+h会显示）

2>配置插件：

1>在源码包 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、编译与设置环境

1、编译环境：

切换工作区间cd ~/robot_ws

清理构建缓存 rm -rf build/ devel/

重新编译 catkin_make

2、设置环境变量：

cd ~/robot_ws

source devel/setup.bash（每次打开新的终端都要输入来刷新）

或者是写入到~/.bashrc文件里:source ~/robot_ws/devel/setup.bash(打开终端自动生效)

3、加载Gazebo仿真与测试

1、# 这一步是启动环境（底盘、移动、Gazebo世界）

roslaunch oryxbot_description gazebo.launch

PS：

观察与等待：

1. Gazebo 界面：应该会自动弹出一个 3D 仿真窗口。

2. 加载过程：第一次启动可能会卡顿几分钟（因为在下载/加载模型），请耐心等待。

3. 检查场景 ：能看到 机器人 （Oryxbot），且看到 场地围栏 和 加工台 （类似课程设计要求图中的布局），加工台上贴有 AR 码（黑白相间的二维码图片）

   

2、启动底盘导航与对接功能 (Terminal 2)

这个 Launch 文件会启动两个关键服务：

• relative_move：让底盘走直线（基于里程计）。

• base_pose_adjust：识别底盘摄像头的 AR 码，用于精准泊车。

1. 打开第 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：提供"去抓取"的服务接口。

1. 打开第 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

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

<?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>退出后点击左下角的运行，看看效果

2、保存世界文件：

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

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