Merge pull request #1702 from sktometometo/PR/fetchdev/support-head-l515

[Fetch] Support head l515 (Part1)
jsk-ros-pkg · Jun 26, 2023 · 93f8039 · 93f8039
2 parents 7a2d1ee + b089d92
commit 93f8039
Show file tree

Hide file tree

Showing 15 changed files with 3,768 additions and 34 deletions.
diff --git a/jsk_fetch_robot/jsk_fetch_accessories/head_l515/fetch_head_l515_mount.step b/jsk_fetch_robot/jsk_fetch_accessories/head_l515/fetch_head_l515_mount.step
diff --git a/jsk_fetch_robot/jsk_fetch_accessories/head_l515/fetch_head_l515_mount.stl b/jsk_fetch_robot/jsk_fetch_accessories/head_l515/fetch_head_l515_mount.stl
diff --git a/...etch_robot/jsk_fetch_accessories/head_l515/fetch_head_l515_mount_simplified.stl b/...etch_robot/jsk_fetch_accessories/head_l515/fetch_head_l515_mount_simplified.stl
diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/config.bash b/jsk_fetch_robot/jsk_fetch_startup/config/config.bash
@@ -0,0 +1,7 @@
+# This file is bash configuration for robot.conf
+# This file must be at /var/lib/robot/config.bash
+
+export RS_SERIAL_NO_T265=""
+export RS_SERIAL_NO_D435_FRONTRIGHT=""
+export RS_SERIAL_NO_D435_FRONTLEFT=""
+export RS_SERIAL_NO_L515_HEAD=""
diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/install_supervisor.sh b/jsk_fetch_robot/jsk_fetch_startup/config/install_supervisor.sh
@@ -15,4 +15,9 @@ for file in $(ls ./*.conf); do
     echo "copied $file to /etc/supervisor/conf.d"
 done
 
+# copy config.bash to /var/lib/robo if not exists
+if [ ! -e /var/lib/robot/config.bash ]; then
+    sudo cp $jsk_fetch_startup/config/config.bash /var/lib/robot/config.bash
+fi
+
 set +x
diff --git a/jsk_fetch_robot/jsk_fetch_startup/config/jsk_startup.rviz b/jsk_fetch_robot/jsk_fetch_startup/config/jsk_startup.rviz
@@ -5,7 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded: ~
       Splitter Ratio: 0.4187380373477936
-    Tree Height: 595
+    Tree Height: 1319
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -27,7 +27,7 @@ Panels:
     SyncSource: BaseScan
   - Class: jsk_rviz_plugin/CancelAction
     Name: CancelAction
-    Topic: /move_base
+    Topic: ""
     topics:
       - /move_base
 Preferences:
@@ -65,6 +65,11 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        base_camera_mount:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         base_link:
           Alpha: 1
           Show Axes: false
@@ -80,6 +85,14 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        d435_front_left_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        d435_front_right_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         elbow_flex_link:
           Alpha: 1
           Show Axes: false
@@ -100,6 +113,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        head_box_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         head_camera_depth_frame:
           Alpha: 1
           Show Axes: false
@@ -120,6 +138,11 @@ Visualization Manager:
           Alpha: 1
           Show Axes: false
           Show Trail: false
+        head_l515_mount_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         head_pan_link:
           Alpha: 1
           Show Axes: false
@@ -130,6 +153,10 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        l515_head_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
         l_gripper_finger_link:
           Alpha: 1
           Show Axes: false
@@ -229,7 +256,7 @@ Visualization Manager:
       Head Length: 0.20000000298023224
       Length: 0.30000001192092896
       Line Style: Billboards
-      Line Width: 0.3
+      Line Width: 0.30000001192092896
       Name: NavigationPath
       Offset:
         X: 0
@@ -252,7 +279,7 @@ Visualization Manager:
       Head Length: 0.20000000298023224
       Length: 0.30000001192092896
       Line Style: Billboards
-      Line Width: 0.1
+      Line Width: 0.10000000149011612
       Name: GlobalPath
       Offset:
         X: 0
@@ -432,6 +459,36 @@ Visualization Manager:
       top: 10
       transport hint: raw
       width: 320
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Max Intensity: 4096
+      Min Color: 0; 0; 0
+      Min Intensity: 0
+      Name: HeadL515PointCloud
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /l515_head/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -462,33 +519,35 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 10
+      Distance: 10.471776962280273
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: -0.00829183217138052
+        Y: 0.022624600678682327
+        Z: -0.003759089857339859
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.7897970676422119
+      Pitch: 0.5747973322868347
       Target Frame: base_link
       Value: Orbit (rviz)
-      Yaw: 5.593582630157471
+      Yaw: 1.745403528213501
     Saved: ~
 Window Geometry:
+  CancelAction:
+    collapsed: true
   Displays:
-    collapsed: false
-  Height: 1052
-  Hide Left Dock: false
+    collapsed: true
+  Height: 1865
+  Hide Left Dock: true
   Hide Right Dock: true
-  QMainWindow State: 000000ff00000000fd0000000400000000000001f80000037efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000019b000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037e000000c900fffffffb00000018004b0069006e00650063007400430061006d0065007200610000000373000000160000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e006100720072006f007700430061006d0065007200610000000244000001460000000000000000000000010000010f00000463fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000463000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d006501000000000000045000000000000000000000053f0000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000400000000000001f8000006abfc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f0070006500720074006900650073020000019b000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d000005b2000000c900fffffffb00000018004b0069006e00650063007400430061006d0065007200610000000373000000160000000000000000fb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e006100720072006f007700430061006d0065007200610000000244000001460000000000000000fb0000001800430061006e00630065006c0041006300740069006f006e00000005f5000000f30000007f00ffffff000000010000010f00000463fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000002800000463000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000780000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000003f50000003efc0100000002fb0000000800540069006d00650100000000000003f5000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003f5000006ab00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -497,6 +556,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: true
-  Width: 1853
+  Width: 1013
   X: 67
   Y: 27
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch.launch
@@ -1,12 +1,21 @@
 <launch>
   <!-- add arg launch_teleop: yamaguchi & s-kitagawa (2019/04/19) -->
   <arg name="launch_teleop" default="true" />
+  <arg name="launch_moveit" default="true" />
+
   <arg name="use_base_camera_mount" default="true" />
   <arg name="use_head_box" default="true" />
   <arg name="launch_diagnostics_agg" default="false" />
+  <arg name="use_head_l515" default="true" />
   <arg name="use_realsense" default="true" />
   <arg name="use_fetch_description" default="false" />
 
+  <!-- realsense serial numbers -->
+  <arg name="RS_SERIAL_NO_T265" default="$(optenv RS_SERIAL_NO_T265)" />
+  <arg name="RS_SERIAL_NO_D435_FRONTRIGHT" default="$(optenv RS_SERIAL_NO_D435_FRONTRIGHT)" />
+  <arg name="RS_SERIAL_NO_D435_FRONTLEFT" default="$(optenv RS_SERIAL_NO_D435_FRONTLEFT)" />
+  <arg name="RS_SERIAL_NO_L515_HEAD" default="$(optenv RS_SERIAL_NO_L515_HEAD)" />
+
   <!-- GDB Debug Option -->
   <arg name="debug" default="false"/>
   <arg unless="$(arg debug)" name="launch_prefix" value=""/>
@@ -26,6 +35,10 @@
   <!-- Launch realsense cameras -->
   <include file="$(find jsk_fetch_startup)/launch/fetch_realsense_bringup.launch" if="$(arg use_realsense)">
       <arg name="respawn" value="true" />
+      <arg name="RS_SERIAL_NO_T265" value="$(arg RS_SERIAL_NO_T265)" />
+      <arg name="RS_SERIAL_NO_D435_FRONTRIGHT" value="$(arg RS_SERIAL_NO_D435_FRONTRIGHT)" />
+      <arg name="RS_SERIAL_NO_D435_FRONTLEFT" value="$(arg RS_SERIAL_NO_D435_FRONTLEFT)" />
+      <arg name="RS_SERIAL_NO_L515_HEAD" value="$(arg RS_SERIAL_NO_L515_HEAD)" />
   </include>
 
   <!-- Odometry -->
@@ -123,7 +136,8 @@
   <param name="robot_description"
          command="$(find xacro)/xacro $(find jsk_fetch_startup)/robots/jsk_fetch.urdf.xacro
                   ros_distro:=$(env ROS_DISTRO) use_fetch_description:=$(arg use_fetch_description)
-                  base_camera_mount:=$(arg use_base_camera_mount) head_box:=$(arg use_head_box)" />
+                  base_camera_mount:=$(arg use_base_camera_mount) head_box:=$(arg use_head_box)
+                  head_l515:=$(arg use_head_l515)" />
 
   <!-- Drivers for Base -->
   <node name="robot_driver" launch-prefix="$(arg launch_prefix)" pkg="fetch_drivers" type="robot_driver" output="screen">
@@ -168,6 +182,15 @@
     <arg name="flags" value="-a -b -g -t"/>
   </include>
 
+  <!-- include fetch moveit -->
+  <group if="$(arg launch_moveit)">
+    <include file="$(find fetch_moveit_config)/launch/move_group.launch" />
+    <!-- overwrite fetch.srdf -->
+    <!-- The semantic description that corresponds to the URDF -->
+    <param name="robot_description_semantic"
+	   command="rosrun xacro xacro --inorder $(find jsk_fetch_startup)/launch/moveit/fetch.srdf.xacro head_l515:=$(arg use_head_l515)" />
+  </group>
+
   <!-- Diagnostics Aggregator -->
   <include if="$(arg launch_diagnostics_agg)"
            file="$(find fetch_bringup)/launch/include/aggregator.launch.xml"/>

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_bringup.launch
@@ -1,6 +1,5 @@
 <launch>
   <arg name="hostname" default="fetch15" />
-  <arg name="launch_moveit" default="true" />
   <arg name="launch_teleop" default="true" />
   <arg name="launch_move_base" default="true" />
   <arg name="launch_sound_play" default="true" />
@@ -116,15 +115,6 @@
     <arg name="launch_manager" value="false"/>
   </include>
 
-  <!-- include fetch moveit -->
-  <group if="$(arg launch_moveit)">
-    <include file="$(find fetch_moveit_config)/launch/move_group.launch" />
-    <!-- overwrite fetch.srdf -->
-    <!-- The semantic description that corresponds to the URDF -->
-    <param name="robot_description_semantic"
-           command="rosrun xacro xacro --inorder $(find jsk_fetch_startup)/launch/moveit/fetch.srdf.xacro" />
-  </group>
-
   <!-- teleop -->
   <include file="$(find jsk_fetch_startup)/launch/fetch_teleop.xml"
            if="$(arg launch_teleop)" >

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_gazebo_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_gazebo_bringup.launch
@@ -32,7 +32,6 @@
   </group>
 
   <include file="$(find jsk_fetch_startup)/launch/fetch_bringup.launch">
-    <arg name="launch_moveit" value="$(arg launch_moveit)" />
     <arg name="launch_move_base" value="$(arg launch_move_base)" />
     <arg name="launch_sound_play" value="$(arg launch_sound_play)" />
     <arg name="map_frame" value="$(arg map_frame)" />

diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_realsense_bringup.launch b/jsk_fetch_robot/jsk_fetch_startup/launch/fetch_realsense_bringup.launch
@@ -2,8 +2,13 @@
 
   <arg name="respawn" default="true" />
 
+  <arg name="RS_SERIAL_NO_T265" default="" />
+  <arg name="RS_SERIAL_NO_D435_FRONTRIGHT" default="" />
+  <arg name="RS_SERIAL_NO_D435_FRONTLEFT" default="" />
+  <arg name="RS_SERIAL_NO_L515_HEAD" default="" />
+
   <!-- t265 -->
-  <group ns="t265">
+  <group ns="t265" if="$(eval arg('RS_SERIAL_NO_T265') != '')">
     <rosparam>
       /tracking_module/enable_mapping: false
       /tracking_module/enable_relocalization: false
@@ -13,7 +18,7 @@
     </rosparam>
     <include file="$(find jsk_fetch_startup)/launch/devices/realsense_nodelet.launch.xml">
       <arg name="tf_prefix"                value="t265"/>
-      <arg name="serial_no"                value=""/>
+      <arg name="serial_no"                value="$(arg RS_SERIAL_NO_T265)"/>
       <arg name="usb_port_id"              value=""/>
       <arg name="device_type"              value="t265"/>
       <arg name="json_file_path"           value=""/>
@@ -44,10 +49,10 @@
   </group>
 
   <!-- d435 -->
-  <group ns="d435_front_right">
+  <group ns="d435_front_right" if="$(eval arg('RS_SERIAL_NO_D435_FRONTRIGHT') != '')">
     <include file="$(find jsk_fetch_startup)/launch/devices/realsense_nodelet.launch.xml">
       <arg name="device_type"           value="d435"/>
-      <arg name="serial_no"             value=""/>
+      <arg name="serial_no"             value="$(arg RS_SERIAL_NO_D435_FRONTRIGHT)"/>
       <arg name="tf_prefix"             value="d435_front_right"/>
       <arg name="initial_reset"         value="true"/>
       <arg name="align_depth"           value="true"/>
@@ -61,10 +66,10 @@
       <arg name="respawn"              value="$(arg respawn)"/>
     </include>
   </group>
-  <group ns="d435_front_left">
+  <group ns="d435_front_left" if="$(eval arg('RS_SERIAL_NO_D435_FRONTLEFT') != '')">
     <include file="$(find jsk_fetch_startup)/launch/devices/realsense_nodelet.launch.xml">
       <arg name="device_type"           value="d435"/>
-      <arg name="serial_no"             value=""/>
+      <arg name="serial_no"             value="$(arg RS_SERIAL_NO_D435_FRONTLEFT)"/>
       <arg name="tf_prefix"             value="d435_front_left"/>
       <arg name="initial_reset"         value="true"/>
       <arg name="align_depth"           value="true"/>
@@ -78,5 +83,28 @@
       <arg name="respawn"              value="$(arg respawn)"/>
     </include>
   </group>
+  <group ns="l515_head" if="$(eval arg('RS_SERIAL_NO_L515_HEAD') != '')">
+    <include file="$(find jsk_fetch_startup)/launch/devices/realsense_nodelet.launch.xml">
+      <arg name="device_type"           value="l515"/>
+      <arg name="serial_no"             value="$(arg RS_SERIAL_NO_L515_HEAD)"/>
+      <arg name="tf_prefix"             value="l515_head"/>
+      <arg name="initial_reset"         value="true"/>
+      <arg name="align_depth"           value="true"/>
+      <arg name="filters"               value="pointcloud"/>
+      <arg name="color_width"           value="1920"/>
+      <arg name="color_height"          value="1080"/>
+      <arg name="depth_width"           value="640"/>
+      <arg name="depth_height"          value="480"/>
+      <arg name="clip_distance"         value="-2"/>
+      <arg name="enable_pointcloud"     value="false"/>
+      <arg name="enable_infra"          value="true"/>
+      <arg name="respawn"              value="$(arg respawn)"/>
+    </include>
+    <rosparam>
+        /l515_head/motion_module/global_time_enabled: true
+        /l515_head/l500_depth_sensor/global_time_enabled: true
+        /l515_head/rgb_camera/global_time_enabled: true
+    </rosparam>
+  </group>
 
 </launch>
diff --git a/jsk_fetch_robot/jsk_fetch_startup/launch/moveit/fetch.srdf.xacro b/jsk_fetch_robot/jsk_fetch_startup/launch/moveit/fetch.srdf.xacro
@@ -1,7 +1,12 @@
 <?xml version="1.0" ?>
 <robot name="fetch" xmlns:xacro="http://www.ros.org/wiki/xacro">
+    <xacro:arg name="head_l515" default="true" />
+
     <!-- VIRTUAL JOINT: virtual joint from map to base_link -->
     <!-- this is necessary to add collision objects based on map frame -->
     <virtual_joint name="world_joint" type="planar" parent_frame="map" child_link="base_link" />
     <xacro:include filename="$(find fetch_moveit_config)/config/fetch.srdf" />
+    <xacro:if value="$(arg head_l515)">
+      <xacro:include filename="$(find jsk_fetch_startup)/robots/head_l515.srdf.xacro" />
+    </xacro:if>
 </robot>
diff --git a/jsk_fetch_robot/jsk_fetch_startup/robots/head_l515.srdf.xacro b/jsk_fetch_robot/jsk_fetch_startup/robots/head_l515.srdf.xacro
@@ -0,0 +1,6 @@
+<?xml version="1.0" ?>
+<robot name="head_l515">
+    <!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
+  <disable_collisions link1="head_tilt_link" link2="head_l515_mount_link" reason="Never" />
+  <disable_collisions link1="head_tilt_link" link2="l515_head_link" reason="Never" />
+</robot>