Device visualization (#14)

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* add 3d visualization

* 完成在main中启动设备可视化

完成在main中启动设备可视化，并输出物料ID：mesh的对应关系resource_model

添加物料模型管理类，遍历物料与resource_model，完成TF数据收集

* 完成TF发布

* 修改模型方向，在yaml中添加变换属性

* 添加物料tf变化时，发送topic到前端

另外修改了物料初始化的方法，防止在tf还未发布时提前建立物料模型与发布话题

* 添加关节发布节点与物料可视化节点进入unilab

* 使用json启动plr与3D模型仿真

* 完成启动OT并联动rviz

* 修复rviz位置问题,

修复rviz位置问题,
在无tf变动时减缓发送频率
在backend中添加物料跟随方法

* fix: running logic

* fix: running logic

* fix: missing ot

* 在main中直接初始化republisher和物料的mesh节点

* 将joint_republisher和resource_mesh_manager添加进 main_slave_run.py中

---------

Co-authored-by: zhangshixiang <@zhangshixiang>
Co-authored-by: wznln <18435084+Xuwznln@users.noreply.github.com>

unilabos/device_mesh/devices/opentrons_liquid_handler/joint_config.json

@@ -0,0 +1,18 @@
+{
+    "first_joint": {
+        "child":"first_link",
+        "axis" : "-y"
+    },
+    "second_joint": {
+        "child":"second_link",
+        "axis" : "-x"
+    },
+    "third_joint": {
+        "child":"third_link",
+        "axis" : "-z"
+    },
+    "fourth_joint": {
+        "child":"fourth_link",
+        "axis" : "-z"
+    }
+}

unilabos/device_mesh/devices/opentrons_liquid_handler/macro_device.xacro

@@ -0,0 +1,210 @@
+<?xml version="1.0" ?>
+
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="opentrons_liquid_handler" 
+  params="parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0 mesh_path:=''">
+
+
+    <joint name="${station_name}${device_name}base_link_joint" type="fixed">
+      <origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
+      <parent link="${parent_link}"/>
+      <child link="${station_name}${device_name}device_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name="${station_name}${device_name}device_link"/>
+    <joint name="${station_name}${device_name}device_link_joint" type="fixed">
+      <origin xyz="-0.11565 0.496 0" rpy="0 0 0" />
+      <parent link="${station_name}${device_name}device_link"/>
+      <child link="${station_name}${device_name}main_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name='${station_name}${device_name}main_link'>
+      <visual>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-0.stl"/>
+        </geometry>
+        <material name="">
+            <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+          </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-0.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    
+    <link name='${station_name}${device_name}first_link'>
+      <visual>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-1.stl"/>
+        </geometry>
+        <material name="">
+            <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+          </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-1.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='${station_name}${device_name}second_link'>
+      <visual>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-2.stl"/>
+        </geometry>
+        <material name="">
+            <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+          </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-2.stl"/>
+        </geometry>
+      </collision>
+    </link>
+    
+    <link name='${station_name}${device_name}third_link'>
+      <visual>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
+        </geometry>
+        <material name="">
+            <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+          </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3a.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <link name='${station_name}${device_name}fourth_link'>
+      <visual>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
+        </geometry>
+        <material name="">
+            <color rgba="0.756862745098039 0.768627450980392 0.752941176470588 1"/>
+          </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/opentrons_liquid_handler/meshes/ot2-3b.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name='${station_name}${device_name}first_joint' type='prismatic'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}first_link"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <axis xyz="0 -1 0"/>
+      <limit effort="-1" lower="-0.2" upper="0.13" velocity="-1"/>
+      <dynamics damping="0.1"/>
+    </joint>
+    <joint name='${station_name}${device_name}second_joint' type='prismatic'>
+      <parent link="${station_name}${device_name}first_link"/>
+      <child link="${station_name}${device_name}second_link"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <axis xyz="-1 0 0"/>
+      <limit effort="-1" lower="-0.15" upper="0.15" velocity="-1"/>
+      <dynamics damping="0.1"/>
+    </joint>
+    <joint name='${station_name}${device_name}third_joint' type='prismatic'>
+      <parent link="${station_name}${device_name}second_link"/>
+      <child link="${station_name}${device_name}third_link"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <axis xyz="0 0 -1"/>
+      <limit effort="-1" lower="0" upper="0.22" velocity="-1"/>
+      <dynamics damping="0.1"/>
+    </joint>
+    <joint name='${station_name}${device_name}fourth_joint' type='prismatic'>
+      <parent link="${station_name}${device_name}second_link"/>
+      <child link="${station_name}${device_name}fourth_link"/>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <axis xyz="0 0 -1"/>
+      <limit effort="-1" lower="0" upper="0.22" velocity="-1"/>
+      <dynamics damping="0.1"/>
+    </joint>
+
+    <link name='${station_name}${device_name}socketTypeGenericSbsFootprint'/>
+
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_10_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.1795 -0.1825 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_7_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.1795 -0.273 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_4_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.1795 -0.3635 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_1_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.1795 -0.454 0.07"/>
+    </joint>
+
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_11_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.312 -0.1825 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_8_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.312 -0.273 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_5_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.312 -0.3635 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_2_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.312 -0.454 0.07"/>
+    </joint>
+
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_9_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.4445 -0.273 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_6_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.4445 -0.3635 0.07"/>
+    </joint>
+    <joint name='${station_name}${device_name}socketTypeGenericSbsFootprint_3_60_1' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeGenericSbsFootprint"/>
+      <origin rpy="0 0 -1.57" xyz="0.4445 -0.454 0.07"/>
+    </joint>
+
+
+    <link name='${station_name}${device_name}socketTypeHEPAModule'/>
+    <joint name='${station_name}${device_name}socketTypeHEPAModule' type='fixed'>
+      <parent link="${station_name}${device_name}main_link"/>
+      <child link="${station_name}${device_name}socketTypeHEPAModule"/>
+      <origin rpy="0 0 0" xyz="0.31 -0.26 0.66"/>
+    </joint>
+  </xacro:macro>
+</robot>

unilabos/device_mesh/devices/opentrons_liquid_handler/param_config.json

@@ -0,0 +1,10 @@
+{
+    "private_param":
+    {
+
+    },
+    "public_param": 
+    {
+
+    }
+}

unilabos/device_mesh/devices/slide_w140/joint_config.json

@@ -0,0 +1,6 @@
+{
+    "slider_joint": {
+        "child":"slider",
+        "axis" : "x"
+    }
+}

unilabos/device_mesh/devices/slide_w140/macro_device.xacro

@@ -0,0 +1,136 @@
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from slide.urdf                     | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) 
+     Commit Version: 1.6.0-4-g7f85cfe  Build Version: 1.6.7995.38578
+     For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:macro name="slide_w140" params="mesh_path:='' length:=0.1 min_d:=0.1 max_d:=0.1 slider_d:=0.14 parent_link:='' station_name:='' device_name:='' x:=0 y:=0 z:=0 r:=0" >
+
+
+    <joint name="${station_name}${device_name}base_link_joint" type="fixed">
+      <origin xyz="${x} ${y} ${z}" rpy="0 0 ${r}" />
+      <parent link="${parent_link}"/>
+      <child link="${station_name}${device_name}device_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name="${station_name}${device_name}device_link"/>
+    <joint name="${station_name}${device_name}device_link_joint" type="fixed">
+      <origin xyz="${-min_d}  0 0" rpy="0 0 0" />
+      <parent link="${station_name}${device_name}device_link"/>
+      <child link="${station_name}${device_name}base_link"/>
+      <axis xyz="0 0 0"/>
+    </joint>
+
+    <link name="${station_name}${device_name}base_link">
+      <inertial>
+        <origin rpy="0 0 0" xyz="-0.00073944 -0.070732 0.035966"/>
+        <mass value="0.88697"/>
+        <inertia ixx="0.0019162" ixy="-2.5282E-11" ixz="6.1083E-07" iyy="0.00066605" iyz="1.4627E-08" izz="0.0019573"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/base_link.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/base_link.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <link name="${station_name}${device_name}slider">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0 -4.7184E-16 0.0095496"/>
+        <mass value="0.27913"/>
+        <inertia ixx="0.00048249" ixy="2.0866E-19" ixz="2.788E-21" iyy="0.00047014" iyz="1.1542E-17" izz="0.0009242"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slider.STL" />
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slider.STL" />
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}slider_joint" type="prismatic">
+      <origin rpy="0 0 0" xyz="${min_d + slider_d/2} 0 0.0475"/>
+      <parent link="${station_name}${device_name}base_link"/>
+      <child link="${station_name}${device_name}slider"/>
+      <axis xyz="1 0 0"/>
+      <limit effort="100" lower="0" upper="${length}" velocity="1"/>
+    </joint>
+    <link name="${station_name}${device_name}length">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.005 -9.3044E-10 0.02803"/>
+        <mass value="0.050532"/>
+        <inertia ixx="8.098E-05" ixy="-1.2574E-19" ixz="3.1207E-20" iyy="4.4152E-06" iyz="-3.1294E-13" izz="7.7407E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/length.STL" scale="${(length + min_d + max_d + slider_d)} 1 1"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}length_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <parent link="${station_name}${device_name}base_link"/>
+      <child link="${station_name}${device_name}length"/>
+    </joint>
+    <link name="${station_name}${device_name}slide_end">
+      <inertial>
+        <origin rpy="0 0 0" xyz="0.003 8.6044E-06 0.035593"/>
+        <mass value="0.055452"/>
+        <inertia ixx="9.9729E-05" ixy="-1.0228E-21" ixz="3.8344E-22" iyy="2.3158E-05" iyz="1.5613E-08" izz="7.6904E-05"/>
+      </inertial>
+      <visual>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slide_end.STL"/>
+        </geometry>
+        <material name="">
+          <color rgba="1 1 1 1"/>
+        </material>
+      </visual>
+      <collision>
+        <origin rpy="0 0 0" xyz="0 0 0"/>
+        <geometry>
+          <mesh filename="file://${mesh_path}/devices/slide_w140/meshes/slide_end.STL"/>
+        </geometry>
+      </collision>
+    </link>
+    <joint name="${station_name}${device_name}slide_end_joint" type="fixed">
+      <origin rpy="0 0 0" xyz="${length + max_d +min_d + slider_d} 0 0"/>
+      <parent link="${station_name}${device_name}base_link"/>
+      <child link="${station_name}${device_name}slide_end"/>
+    </joint>
+
+  </xacro:macro>
+</robot>

unilabos/device_mesh/devices/slide_w140/param_config.json

@@ -0,0 +1,12 @@
+{
+    "private_param":
+    {
+        "min_d": 0.1 ,
+        "max_d": 0.1 ,
+        "slider_d": 0.14 
+    },
+    "public_param": 
+    {
+        "length" :0.1
+    }
+}