ROS1+RealsenseD456读取Aruco码并输出位姿

一、准备步骤

博主用的设备是Ubuntu20.04+realsenseD456+ROS1noetic

首先需要安装相关的ealsense驱动，可以参考我的另一篇博客如何安装Realsense驱动

新建一个终端，调用如下命令检查相机是否可以正常工作：

realsense-viewer

出现如下图像即为成功：

**Aurco二维码生成的网址为： Aurco二维码

二、下载并编译Aruco_ros

**由于ROS1-noetic于2025年5月31日已经停止维护了，所以直接用sudo的命令或者git的命令去下载aruco_ros是不可行的！！

科学上网后，登陆github官网，搜索aruco_ros并找到ros-noetic分支，如下图所示：

随后，直接下载安装包并解压缩，解压缩后的文件夹放在/home/jetson/realsense_ws/src路径下。这里注意修改为自己电脑对应的路径名称。回到realsense_ws工作空间下，先调用：

rm -rf build devel install build_isolated devel_isolated install_isolated

来清除之前的编译文件，如果之间调用catkin build编译过，或者之前用过catkin_make，可能会产生不兼容的问题，所以需要先调用上述命令来清理编译结果。随后，调用catkin_make开始编译，直至成功，成功界面如下：

三、文件修改、识别及输出

参考：Realsense+ROS1+二维码识别

3.1 单二维码识别single.launch

3.1.1 文件修改

对于rs_camera.launch文件，首先需要把这三行全部改为true，这样才能保证有图像话题输出，方便后续订阅。

<launch>
  <arg name="serial_no"           default=""/>
  <arg name="usb_port_id"         default=""/>
  <arg name="device_type"         default=""/>
  <arg name="json_file_path"      default=""/>
  <arg name="camera"              default="camera"/>
  <arg name="tf_prefix"           default="$(arg camera)"/>
  <arg name="external_manager"    default="false"/>
  <arg name="manager"             default="realsense2_camera_manager"/>
  <arg name="output"              default="screen"/>
  <arg name="respawn"              default="false"/>

  <arg name="fisheye_width"       default="-1"/>
  <arg name="fisheye_height"      default="-1"/>
  <arg name="enable_fisheye"      default="false"/>

  <arg name="depth_width"         default="-1"/>
  <arg name="depth_height"        default="-1"/>
  <arg name="enable_depth"        default="true"/>

  <arg name="confidence_width"    default="-1"/>
  <arg name="confidence_height"   default="-1"/>
  <arg name="enable_confidence"   default="true"/>
  <arg name="confidence_fps"      default="-1"/>

  <arg name="infra_width"         default="848"/>
  <arg name="infra_height"        default="480"/>
  <arg name="enable_infra"        default="true"/>
  <arg name="enable_infra1"       default="true"/>
  <arg name="enable_infra2"       default="true"/>
  <arg name="infra_rgb"           default="false"/>

  <arg name="color_width"         default="-1"/>
  <arg name="color_height"        default="-1"/>
  <arg name="enable_color"        default="true"/>

  <arg name="fisheye_fps"         default="-1"/>
  <arg name="depth_fps"           default="-1"/>
  <arg name="infra_fps"           default="30"/>
  <arg name="color_fps"           default="-1"/>
  <arg name="gyro_fps"            default="-1"/>
  <arg name="accel_fps"           default="-1"/>
  <arg name="enable_gyro"         default="false"/>
  <arg name="enable_accel"        default="false"/>

  <arg name="enable_pointcloud"         default="false"/>
  <arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
  <arg name="pointcloud_texture_index"  default="0"/>
  <arg name="allow_no_texture_points"   default="false"/>
  <arg name="ordered_pc"                default="false"/>

  <arg name="enable_sync"               default="false"/>
  <arg name="align_depth"               default="false"/>

  <arg name="publish_tf"                default="true"/>
  <arg name="tf_publish_rate"           default="0"/>

  <arg name="filters"                   default=""/>
  <arg name="clip_distance"             default="-2"/>
  <arg name="linear_accel_cov"          default="0.01"/>
  <arg name="initial_reset"             default="false"/>
  <arg name="reconnect_timeout"         default="6.0"/>
  <arg name="wait_for_device_timeout"   default="-1.0"/>
  <arg name="unite_imu_method"          default=""/>
  <arg name="topic_odom_in"             default="odom_in"/>
  <arg name="calib_odom_file"           default=""/>
  <arg name="publish_odom_tf"           default="true"/>

  <arg name="stereo_module/exposure/1"  default="7500"/>
  <arg name="stereo_module/gain/1"      default="16"/>
  <arg name="stereo_module/exposure/2"  default="1"/>
  <arg name="stereo_module/gain/2"      default="16"/>
  
  

  <group ns="$(arg camera)">
    <include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
      <arg name="tf_prefix"                value="$(arg tf_prefix)"/>
      <arg name="external_manager"         value="$(arg external_manager)"/>
      <arg name="manager"                  value="$(arg manager)"/>
      <arg name="output"                   value="$(arg output)"/>
      <arg name="respawn"                  value="$(arg respawn)"/>
      <arg name="serial_no"                value="$(arg serial_no)"/>
      <arg name="usb_port_id"              value="$(arg usb_port_id)"/>
      <arg name="device_type"              value="$(arg device_type)"/>
      <arg name="json_file_path"           value="$(arg json_file_path)"/>

      <arg name="enable_pointcloud"        value="$(arg enable_pointcloud)"/>
      <arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
      <arg name="pointcloud_texture_index"  value="$(arg pointcloud_texture_index)"/>
      <arg name="enable_sync"              value="$(arg enable_sync)"/>
      <arg name="align_depth"              value="$(arg align_depth)"/>

      <arg name="fisheye_width"            value="$(arg fisheye_width)"/>
      <arg name="fisheye_height"           value="$(arg fisheye_height)"/>
      <arg name="enable_fisheye"           value="$(arg enable_fisheye)"/>

      <arg name="depth_width"              value="$(arg depth_width)"/>
      <arg name="depth_height"             value="$(arg depth_height)"/>
      <arg name="enable_depth"             value="$(arg enable_depth)"/>

      <arg name="confidence_width"         value="$(arg confidence_width)"/>
      <arg name="confidence_height"        value="$(arg confidence_height)"/>
      <arg name="enable_confidence"        value="$(arg enable_confidence)"/>
      <arg name="confidence_fps"           value="$(arg confidence_fps)"/>

      <arg name="color_width"              value="$(arg color_width)"/>
      <arg name="color_height"             value="$(arg color_height)"/>
      <arg name="enable_color"             value="$(arg enable_color)"/>

      <arg name="infra_width"              value="$(arg infra_width)"/>
      <arg name="infra_height"             value="$(arg infra_height)"/>
      <arg name="enable_infra"             value="$(arg enable_infra)"/>
      <arg name="enable_infra1"            value="$(arg enable_infra1)"/>
      <arg name="enable_infra2"            value="$(arg enable_infra2)"/>
      <arg name="infra_rgb"                value="$(arg infra_rgb)"/>

      <arg name="fisheye_fps"              value="$(arg fisheye_fps)"/>
      <arg name="depth_fps"                value="$(arg depth_fps)"/>
      <arg name="infra_fps"                value="$(arg infra_fps)"/>
      <arg name="color_fps"                value="$(arg color_fps)"/>
      <arg name="gyro_fps"                 value="$(arg gyro_fps)"/>
      <arg name="accel_fps"                value="$(arg accel_fps)"/>
      <arg name="enable_gyro"              value="$(arg enable_gyro)"/>
      <arg name="enable_accel"             value="$(arg enable_accel)"/>

      <arg name="publish_tf"               value="$(arg publish_tf)"/>
      <arg name="tf_publish_rate"          value="$(arg tf_publish_rate)"/>

      <arg name="filters"                  value="$(arg filters)"/>
      <arg name="clip_distance"            value="$(arg clip_distance)"/>
      <arg name="linear_accel_cov"         value="$(arg linear_accel_cov)"/>
      <arg name="initial_reset"            value="$(arg initial_reset)"/>
      <arg name="reconnect_timeout"        value="$(arg reconnect_timeout)"/>
      <arg name="wait_for_device_timeout"  value="$(arg wait_for_device_timeout)"/>
      <arg name="unite_imu_method"         value="$(arg unite_imu_method)"/>
      <arg name="topic_odom_in"            value="$(arg topic_odom_in)"/>
      <arg name="calib_odom_file"          value="$(arg calib_odom_file)"/>
      <arg name="publish_odom_tf"          value="$(arg publish_odom_tf)"/>
      <arg name="stereo_module/exposure/1" value="$(arg stereo_module/exposure/1)"/>
      <arg name="stereo_module/gain/1"     value="$(arg stereo_module/gain/1)"/>
      <arg name="stereo_module/exposure/2" value="$(arg stereo_module/exposure/2)"/>
      <arg name="stereo_module/gain/2"     value="$(arg stereo_module/gain/2)"/>

      <arg name="allow_no_texture_points"  value="$(arg allow_no_texture_points)"/>
      <arg name="ordered_pc"               value="$(arg ordered_pc)"/>
      
    </include>
  </group>
</launch>

随后，注意修改aurco工作空间下的single.launch文件（因为我用的single.launch文件来做测试，所以只修改了这个文件，如果要用到该工作空间下的别的文件也要同步修改）

有6个地方着重修改，分别是：

a. <arg name="markerId" default="123"/>，default后面的值需要改成自己生成的Aurco二维码的ID。

b. <arg name="markerSize" default="0.034"/> <!-- in m -->，default后面的值需要改成自己生成的Aurco二维码的size（也是物理边长），单位是m。

c. <arg name="eye" default="infra1"/>，default后面的值需要改成realsense相机发布的话题，注意这里不是整个话题，eye参数只是话题中的某一节，所以default并没有改为整个话题名称。

d. <remap from="/camera_info" to="/camera/$(arg eye)/camera_info" />，default后面的值需要改成相机发布的话题，这里看到c中的参数eye被传过来了，也就解释了为什么eye会那样设置

e. <remap from="/image" to="/camera/$(arg eye)/image_rect_raw" />，

**f. <param name="camera_frame" value="/camera_link"/>，这里default后面一定要改成/camera/link，该link可以融合相机携带的其他传感器

修改后的launch文件如下：

<launch>

    <arg name="markerId"        default="123"/>
    <arg name="markerSize"      default="0.034"/>    <!-- in m -->
    <arg name="eye"             default="infra1"/>
    <arg name="marker_frame"    default="aruco_marker_frame"/>
    <arg name="ref_frame"       default=""/>  <!-- leave empty and the pose will be published wrt param parent_name -->
    <arg name="corner_refinement" default="LINES" /> <!-- NONE, HARRIS, LINES, SUBPIX -->


    <node pkg="aruco_ros" type="single" name="aruco_single">
        <remap from="/camera_info" to="/camera/$(arg eye)/camera_info" />
        <remap from="/image" to="/camera/$(arg eye)/image_rect_raw" />
        <param name="image_is_rectified" value="True"/>
        <param name="marker_size"        value="$(arg markerSize)"/>
        <param name="marker_id"          value="$(arg markerId)"/>
        <param name="reference_frame"    value="$(arg ref_frame)"/>   <!-- frame in which the marker pose will be refered -->
        <param name="camera_frame"       value="/camera_link"/>
        <param name="marker_frame"       value="$(arg marker_frame)" />
        <param name="corner_refinement"  value="$(arg corner_refinement)" />
    </node>

</launch>

以下面两张图为例，说明这里的参数怎么填。打开Aurco二维码生成二维码，Dictionary选Original ArUco，为aruco_ros的默认

第一张图可以看出，a中markerId的值应该是123，b中markerSize应该是34mm，也就是0.034m。第二张图可以看出，c中参数eye可以选infra1（其实也可以infra2或者color），d和e的参数要改成/camera/(arg eye)/camera_info和/camera/(arg eye)/image_rect_raw，最后f的参数要改成/camera/link。

3.1.2 启动对应的launch文件，并打印输出

首先启动roscore，随后调用如下命令启动相机，

roslaunch realsense2_camera rs_camera.launch 

（可选）关闭结构光，即emitter_enabled设置为off

rosrun rqt_reconfigure rqt_reconfigure

启动二维码识别

roslaunch aruco_ros single.launch

可视化结果输出

rosrun image_view image_view image:=/aruco_single/result

结果输出如下：

可以看到对已有的二维码，可以读取其ID号以及二维码的坐标系

最后打印位姿信息，调用如下命令：

rostopic echo /aruco_single/pose

位姿信息输出如下：

左侧终端的右下角即为输出的位姿信息。之所以可以通过单目相机输出该信息，是因为在single.launch文件中传入了实际二维码的size，所以可以调用算法直接输出位姿信息了。该位姿信息是二维码在相机坐标系下的位姿。相机坐标系和realsense自身的定义是一样的，X水平向右，Y竖直向下，Z垂直纸面向里。

3.2 两个二维码识别double.launch

3.2.1 文件修改

rs_camera.launch文件的修改同上一小节，这里主要讲double.launch文件该如何修改。

a. <arg name="marker1Id" default="123"/>

<arg name="marker2Id" default="300"/>，default的值需要改成自己生成的两个二维码的ID

b. <arg name="markerSize" default="0.034"/> <!-- in m -->，二维码大小，单位为米

c. <arg name="eye" default="infra1"/>，realsense左目相机，话题为infra1

d. <arg name="marker1_frame" default="aruco_marker_123" />

<arg name="marker2_frame" default="aruco_marker_300" />，这里修改为对应的二维码的ID值（我自己试的时候，发现只要不是纯数字都行，但是为了可读性，就按照我这里给出的结果来设置吧）

e. <remap from="/camera_info" to="/camera/$(arg eye)/camera_info" />，同single

f. <remap from="/image" to="/camera/$(arg eye)/image_rect_raw" />，同single

g. <param name="parent_name" value="/camera/link"/>，同single

修改后的文件如下：

<launch>

    <arg name="marker1Id"         default="123"/>
    <arg name="marker2Id"         default="300"/>
    <arg name="markerSize"        default="0.034"/>    <!-- in m -->
    <arg name="eye"               default="infra1"/>
    <arg name="dct_normalization" default="True" />
    <arg name="dct_filter_size"   default="2" />
    <arg name="marker1_frame"     default="aruco_marker_123" />
    <arg name="marker2_frame"     default="aruco_marker_300" />


    <node pkg="aruco_ros" type="double" name="aruco_simple">    
        <remap from="/camera_info" to="/camera/$(arg eye)/camera_info" />
        <remap from="/image" to="/camera/$(arg eye)/image_rect_raw" />
        <param name="image_is_rectified" value="True"/>
        <param name="marker_size" value="$(arg markerSize)"/>
        <param name="marker_id1" value="$(arg marker1Id)"/>
        <param name="marker_id2" value="$(arg marker2Id)"/>
        <param name="normalizeImage" value="$(arg dct_normalization)"/>
        <param name="dct_components_to_remove" value="$(arg dct_filter_size)"/>
        <param name="parent_name" value="/camera/link"/>
        <param name="child_name1" value="$(arg marker1_frame)" />
        <param name="child_name2" value="$(arg marker2_frame)" />
    </node>

</launch>

3.2.2 启动对应的launch文件并打印输出

roscore+相机启动同上，从launch文件开始有区别

启动二维码识别

roslaunch aruco_ros double.launch

可视化结果输出（如果第一个命令不行，可以调用第二个，把输出图像换成compressed格式即可）

rosrun image_view image_view image:=/aruco_simple/result

rosrun image_view image_view image:=/aruco_simple/result _image_transport:=compressed

位姿日至输出：

第一个二维码，即123

rostopic echo /aruco_simple/pose

第二个二维码，即300

rostopic echo /aruco_simple/pose2

运行结果如下：

可以看到终端最右边下面这两张图中，最底下是300的输出，上面是123的输出。输出格式依然是二维码在相机坐标系下的位姿，相机坐标系X轴水平向右，Y轴竖直向下，Z轴垂直纸面向里。

3.3 N个二维码识别multi.launch（自建）

N个二维码的识别，本质上是集成了一个multi.launch文件，并且创建不同的工作空间，多次调用single.launch文件。注意，上面的double.launch和这里的multi.launch文件实现的逻辑是不一样的，double可以直接识别两个二维码，multi虽然也可以识别两个二维码，但实际上是基于利用两个single.launch文件来识别。

3.3.1 multi.launch文件创建

multi.launch文件的结构如下（以123、300和215为例）：

<launch>
    <!-- 检测 ID=123 的标记 -->
    <group ns="marker_123">
        <node pkg="aruco_ros" type="single" name="aruco_single" output="screen">
            <remap from="/camera_info" to="/camera/infra1/camera_info" />
            <remap from="/image" to="/camera/infra1/image_rect_raw" />
            <param name="image_is_rectified" value="True"/>
            <param name="marker_size" value="0.034"/>
            <param name="marker_id" value="123"/>
            <param name="reference_frame" value=""/>
            <param name="camera_frame" value="/camera_link"/>
            <param name="marker_frame" value="aruco_marker_123"/>
            <param name="corner_refinement" value="LINES"/>
        </node>
    </group>
    
    <!-- 检测 ID=300 的标记 -->
    <group ns="marker_300">
        <node pkg="aruco_ros" type="single" name="aruco_single" output="screen">
            <remap from="/camera_info" to="/camera/infra1/camera_info" />
            <remap from="/image" to="/camera/infra1/image_rect_raw" />
            <param name="image_is_rectified" value="True"/>
            <param name="marker_size" value="0.034"/>
            <param name="marker_id" value="300"/>
            <param name="reference_frame" value=""/>
            <param name="camera_frame" value="/camera_link"/>
            <param name="marker_frame" value="aruco_marker_300"/>
            <param name="corner_refinement" value="LINES"/>
        </node>
    </group>
    
    <!-- 检测 ID=215 的标记 -->
    <group ns="marker_215">
        <node pkg="aruco_ros" type="single" name="aruco_single" output="screen">
            <remap from="/camera_info" to="/camera/infra1/camera_info" />
            <remap from="/image" to="/camera/infra1/image_rect_raw" />
            <param name="image_is_rectified" value="True"/>
            <param name="marker_size" value="0.034"/>
            <param name="marker_id" value="215"/>
            <param name="reference_frame" value=""/>
            <param name="camera_frame" value="/camera_link"/>
            <param name="marker_frame" value="aruco_marker_215"/>
            <param name="corner_refinement" value="LINES"/>
        </node>
    </group>
    
    <!-- 检测 更多 的标记 -->
</launch>

3.1.2 启动对应的launch文件并打印输出

按照前面的步骤启动相机后，随后调用如下命令启动多个节点

roslaunch aruco_ros multi.launch 

出现下图显示运行成功

调用如下命令进行可视化。因为三个二维码有三个图像话题的输出，所以调用哪一个都可以，这里我调用的是marker_123工作空间下的图像话题：

rosrun image_view image_view image:=/marker_123/aruco_single/result

最后开启多个终端，分别查看不同二维码的位姿信息：

rostopic echo /marker_123/aruco_single/pose
rostopic echo /marker_215/aruco_single/pose
rostopic echo /marker_300/aruco_single/pose

可以看到如下输出：左侧是215，中间是123，右侧是300