ros_contorl/devices/sim/gazebo/urdf/arm.sim.sensors.xacro

<?xml version="1.0"?>
<!--
  =============================================================================
  外挂传感器：眼在手上的相机 + 双指触觉。

  分工（按"装在哪"划分，不是按品牌）：
      D405  眼在手上（eye-in-hand）—— 装在法兰上，跟着手走 → 属于**机器人**，
            所以在这个文件里，跟着 URDF 一起展开
      D435  眼在手外（eye-to-hand）—— 装在工作台支架上，俯视台面 → 属于**场景**，
            所以在 worlds/assembly.world 里（每台相机只出一份定义，不重复）

  为什么 D435 不能挂在 URDF 的 world link 上（实测踩过）：
      挂了之后整台相机在转 SDF 时凭空消失。sdformat 会丢弃无质量的根 link，
      连带它整棵子树；而它**退出码 0、不报任何错**，只是 SDF 里再也找不到
      d435 的 frame 和 sensor，启动后话题永远没数据。
      另一个理由：渲染系统插件必须是 world 级的，相机和插件放一起最不容易错配。

  =============================================================================
  和真机的对应关系（重要）
  =============================================================================
  真机上是 RealSense，出 RGB + 深度。仿真用 rgbd_camera 同时给两者，
  话题名对齐 ros_realsense，应用层（跟踪、粗定位）才能不加分支地两边通用：
      真机 / 仿真：  /d405/color/image_raw   + /d405/depth/image_rect_raw
  换相机型号时只改这里的尺寸/内参，应用层不用动。

  ============================================================================
  插件文件名
  ============================================================================
  Fortress 的传感器系统插件是 libignition-gazebo6-sensors-system.so，
  名称 ignition::gazebo::systems::Sensors。实测确认过：
      ls $CONDA_PREFIX/lib/libignition-gazebo6-sensors-system.so
  换到更新版本（Harmonic+）后名字会变成 libgz-sim*-sensors-system.so，
  且插件名去掉 ignition:: 前缀 —— 那时要改这里。
  =============================================================================
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="arm_sim_sensors"
    params="
      prefix
      sensors_file:=''
      d405_enable:=true
      tactile_enable:=true
      d405_x:=0.0
      d405_y:=-0.06
      d405_z:=0.05
      d405_roll:=0.0
      d405_pitch:=0.0
      d405_yaw:=0.0
      image_width:=640
      image_height:=480
      image_hz:=30
    ">

    <!-- =======================================================================
         传感器系统插件放在 **world 文件**里，不放这里
         =======================================================================
         渲染是所有传感器共用的世界级能力，而且 D435 在 world 里（见下）。
         插件与相机分处两地的话，很容易出现"插件在模型里、相机在世界里"
         这种错配 —— 症状是话题永远没数据且不报错。
         所以统一由 worlds/assembly.world 声明一次，机器人上的 D405
         和触觉也由它一并渲染。
         ======================================================================= -->

    <!-- =======================================================================
         RealSense D405 —— 眼在手上
         =======================================================================
         位姿默认值说明（法兰坐标系，z 朝法兰外侧）：
             z=+0.05  相机略微伸出法兰，避免看到夹爪本体
             y=-0.06  偏置到一侧，与真机上的支架安装位置一致
         这些值**不是**真机的标定结果，只是仿真里的合理起始位。
         仿真里"手眼关系"本来就精确已知（我们直接定义了相机的安装位姿），
         所以不需要跑手眼标定 —— 但保留了可调位姿，方便模拟"标定不准"的情况，
         验证跟踪算法对误差的容忍度。
         ======================================================================= -->
    <xacro:if value="${d405_enable}">
      <link name="${prefix}d405_link"/>

      <joint name="${prefix}d405_joint" type="fixed">
        <parent link="${prefix}flange"/>
        <child link="${prefix}d405_link"/>
        <origin xyz="${d405_x} ${d405_y} ${d405_z}"
                rpy="${d405_roll} ${d405_pitch} ${d405_yaw}"/>
      </joint>

      <gazebo reference="${prefix}d405_link">
        <sensor name="d405_rgbd" type="rgbd_camera">
          <update_rate>${image_hz}</update_rate>
          <always_on>true</always_on>
          <!-- visualize=false：headless 下没有 GUI，开着只会白耗渲染资源 -->
          <visualize>false</visualize>
          <camera name="d405">
            <!-- 水平视场角。D405 的实际值约 87°(H) × 58°(V)，这里取近似 -->
            <horizontal_fov>1.5184</horizontal_fov>
            <image>
              <width>${image_width}</width>
              <height>${image_height}</height>
              <format>R8G8B8</format>
            </image>
            <clip>
              <!-- 近裁剪面必须小于相机到工件的最近距离，否则工件会被裁掉 -->
              <near>0.02</near>
              <far>10.0</far>
            </clip>
            <depth_camera>
              <clip><near>0.02</near><far>10.0</far></clip>
            </depth_camera>
          </camera>
          <!-- 话题名对齐真机 ros_realsense 的命名，应用层两边通用 -->
          <topic>/d405</topic>
        </sensor>
      </gazebo>
    </xacro:if>

    <!-- =======================================================================
         RealSense D435 —— 眼在手外：**不在这个文件里**
         =======================================================================
         它挂过 world link（URDF 里那个空 link），结果整台相机在转换到 SDF 时
         凭空消失：sdformat 会丢掉无质量的根 link，连带它整棵子树。
         现象极具误导性 —— sdformat 退出码 0、不报任何错，只是 SDF 里
         d435 的 frame 和 sensor 都不存在，启动后话题永远没数据。

         正确位置是 world 文件（worlds/assembly.world）：眼在手外的相机在物理上
         就是装在工作台支架上、不属于机械臂。放那里还有个好处 ——
         渲染系统插件必须是 world 级的，相机和插件在一起，不会再出现
         "插件在模型里、相机在世界里"这种错配。
         ======================================================================= -->

    <!-- =======================================================================
         Orisys 视触觉传感器
         =======================================================================
         真机上是视触觉：一块弹性体 + 内置相机，读出三向力、深度图、力矢量图。
         仿真里没法真的模拟弹性体形变成像，采取的务实方案是：
             用 Gazebo 内置的 contact sensor 取接触点的三向力，
             再由自定义插件（devices/sim/gazebo/sensor_plugins/）把它
             按 Orisys 的 TactileStatus 消息格式发布出去，
             含切向/法向分解，使 P6 的力判据在仿真里可跑。

         这里只挂 contact sensor 与自定义插件的位置占位；
         插件的实现与消息映射在 P5c-5 完成。

         为什么要挂两个（left/right）：
             真机是双指各贴一片，tangential_1/2 + normal 三路力分别来自两片，
             单指的数据无法复现"旋拧时切向力从哪一侧来"这个判据。
         ======================================================================= -->
    <xacro:if value="${tactile_enable}">
      <link name="${prefix}tactile_left_link"/>
      <link name="${prefix}tactile_right_link"/>

      <joint name="${prefix}tactile_left_joint" type="fixed">
        <parent link="${prefix}tcp"/>
        <child link="${prefix}tactile_left_link"/>
        <origin xyz="0 0.035 0.005" rpy="0 0 0"/>
      </joint>
      <joint name="${prefix}tactile_right_joint" type="fixed">
        <parent link="${prefix}tcp"/>
        <child link="${prefix}tactile_right_link"/>
        <origin xyz="0 -0.035 0.005" rpy="0 0 0"/>
      </joint>

      <gazebo reference="${prefix}tactile_left_link">
        <sensor name="tactile_left" type="contact">
          <always_on>true</always_on>
          <update_rate>100</update_rate>
          <!-- 话题名与真机 tactile_node 发布的完全一致：
               /tactile/left/status  (arm_msgs/TactileStatus)
               这样 P6 的状态机在仿真/真机之间不用改一行判断。 -->
          <contact>
            <collision>${prefix}tactile_left_link_collision</collision>
          </contact>
          <topic>/tactile/left/raw</topic>
        </sensor>
      </gazebo>

      <gazebo reference="${prefix}tactile_right_link">
        <sensor name="tactile_right" type="contact">
          <always_on>true</always_on>
          <update_rate>100</update_rate>
          <contact>
            <collision>${prefix}tactile_right_link_collision</collision>
          </contact>
          <topic>/tactile/right/raw</topic>
        </sensor>
      </gazebo>
    </xacro:if>

  </xacro:macro>

</robot>