devkit: added camera layer urdf

westonrobot · Jul 30, 2024 · c66b5b4 · c66b5b4
1 parent 0b05c6f
commit c66b5b4
Show file tree

Hide file tree

Showing 5 changed files with 150 additions and 50 deletions.
diff --git a/src/kits/top/mid360_sensor_kit_bringup/launch/sensor_kit.launch.py b/src/kits/top/mid360_sensor_kit_bringup/launch/sensor_kit.launch.py
@@ -1,17 +1,54 @@
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, GroupAction, DeclareLaunchArgument
-from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.actions import GroupAction, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.launch_context import LaunchContext
 from launch_ros.actions import Node, PushRosNamespace
 from launch_ros.substitutions import FindPackageShare
+from launch.some_substitutions_type import SomeSubstitutionsType
+from launch.substitution import Substitution
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
-from launch.conditions import IfCondition
+from launch.utilities import perform_substitutions, normalize_to_list_of_substitutions
+from typing import Dict, Text
 
 import os
 import xacro
 
 
+class Xacro(Substitution):
+    def __init__(self, file_path: SomeSubstitutionsType, mappings: Dict[str, SomeSubstitutionsType] = {}, verbose: bool = False):
+        """Create a TemplateSubstitution."""
+        super().__init__()
+        self.__file_path = normalize_to_list_of_substitutions(file_path)
+        self.__mappings = {key: normalize_to_list_of_substitutions(value) for key, value in mappings.items()}
+        self.__verbose = verbose
+
+    def describe(self) -> Text:
+        """Return a description of this substitution as a string."""
+        return f"Xacro: {self.__file_path}"
+
+    def perform(self, context: LaunchContext) -> Text:
+        """Perform the substitution by returning the string with values substituted."""
+
+        file_path = perform_substitutions(context, self.__file_path)
+        mappings = {key: perform_substitutions(context, value) for key, value in self.__mappings.items()}
+
+        if self.__verbose:
+            print(f"xacro file_path: {file_path}")
+            print(f"xacro mappings: {mappings}")
+        document = xacro.process_file(file_path, mappings=mappings)
+        document_string = document.toprettyxml(indent="  ")
+
+        if self.__verbose:
+            print(f"xacro result: {document_string}")
+        return document_string
+
 def generate_launch_description():
     # --------- Arguments ---------
+    front_camera = LaunchConfiguration("front_camera")
+    rear_camera = LaunchConfiguration("rear_camera")
+    left_camera = LaunchConfiguration("left_camera")
+    right_camera = LaunchConfiguration("right_camera")
+
     declare_use_namespace_arg = DeclareLaunchArgument(
         "use_namespace",
         default_value="false",
@@ -31,27 +68,33 @@ def generate_launch_description():
     )
 
     # --------- Description ---------
-    xacro_file = os.path.join(
-        FindPackageShare("mid360_sensor_kit_bringup").find(
-            "mid360_sensor_kit_bringup"),
-        "urdf/sensor_kit.urdf.xacro"
-    )
-    doc = xacro.process_file(xacro_file)
-    robot_desc = doc.toprettyxml(indent='  ')
-
     load_description = Node(
         package="robot_state_publisher",
         executable="robot_state_publisher",
         name="mid360_sensor_kit_state_publisher",
         output="screen",
-        parameters=[{"robot_description": robot_desc}],
+        parameters=[
+            {"robot_description": Xacro(
+                    file_path=os.path.join(
+                        FindPackageShare("mid360_sensor_kit_bringup").find(
+                            "mid360_sensor_kit_bringup"),
+                        "urdf/sensor_kit.urdf.xacro"
+                    ),
+                    mappings={
+                        'front_camera': front_camera,
+                        'rear_camera': rear_camera,
+                        'left_camera': left_camera,
+                        'right_camera': right_camera
+                    },
+                )
+            }
+        ],
         remappings=[
             ("robot_description", "mid360_sensor_kit_description"),
         ]
     )
 
     # --------- Drivers ---------
-
     imu_bringup = GroupAction([
         Node(
             package="wrp_ros2",
@@ -116,5 +159,5 @@ def generate_launch_description():
         declare_namespace_arg,
         load_description,
         imu_bringup,
-        lidar_bringup
+        lidar_bringup,
     ])
diff --git a/src/kits/top/mid360_sensor_kit_bringup/meshes/realsense_d435.stl b/src/kits/top/mid360_sensor_kit_bringup/meshes/realsense_d435.stl
diff --git a/src/kits/top/mid360_sensor_kit_bringup/urdf/sensor_kit.urdf.xacro b/src/kits/top/mid360_sensor_kit_bringup/urdf/sensor_kit.urdf.xacro
@@ -2,13 +2,14 @@
 <robot name="mid360_sensor_kit" xmlns:xacro="http://www.ros.org/wiki/xacro">
 
   <xacro:include filename="$(find mid360_sensor_kit_bringup)/urdf/sensor_kit_macro.urdf.xacro" />
+  <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
 
   <xacro:property name="M_PI" value="3.1415926535897931" />
 
-  <xacro:arg name="front_camera" default="realsense_d435"/>
-  <xacro:arg name="rear_camera" default="rgb_camera"/>
-  <xacro:arg name="left_camera" default="rgb_camera"/>
-  <xacro:arg name="right_camera" default="rgb_camera"/>
+  <xacro:arg name="front_camera" default=""/>
+  <xacro:arg name="rear_camera" default=""/>
+  <xacro:arg name="left_camera" default=""/>
+  <xacro:arg name="right_camera" default=""/>
 
   <material name="silver">
     <color rgba="0.700 0.700 0.700 1.000"/>
@@ -66,19 +67,19 @@
   </link>
 
   <joint name="perception_kit_base_joint" type="fixed">
-    <origin xyz="0 0 -0.05" rpy="0 0 0" />
+    <origin xyz="0 0 0" rpy="0 0 0" />
     <parent link="top_sensor_kit_link" />
     <child link="perception_kit_base_link" />
   </joint>
 
   <link name="perception_kit_base_link">
     <inertial>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 -0.345" rpy="${M_PI/2} 0 ${M_PI/2}" />
       <mass value="0.0" />
       <inertia ixx="0.0" iyy="0.0" izz="0.0" ixy="0.0" iyz="0.0" ixz="0.0" />
     </inertial>
     <visual>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 -0.345" rpy="${M_PI/2} 0 ${M_PI/2}" />
       <geometry>
         <mesh
           filename="package://mid360_sensor_kit_bringup/meshes/perception_kit_base_link.stl"
@@ -87,7 +88,7 @@
       <material name="silver" />
     </visual>
     <collision>
-      <origin xyz="0 0 0" rpy="0 0 0" />
+      <origin xyz="0 0 -0.345" rpy="${M_PI/2} 0 ${M_PI/2}" />
       <geometry>
         <mesh
           filename="package://mid360_sensor_kit_bringup/meshes/perception_kit_base_link.stl"
@@ -97,19 +98,19 @@
   </link>
 
   <xacro:perception_kit camera_pos="front" camera_type="$(arg front_camera)">
-    <origin xyz="0.12 0 0.025" rpy="0 0 0" />
+    <origin xyz="0.118 0 -0.025" rpy="0 0 0" />
   </xacro:perception_kit>
 
   <xacro:perception_kit camera_pos="rear" camera_type="$(arg rear_camera)">
-    <origin xyz="-0.12 0 0.025" rpy="0 0 ${M_PI}" />
+    <origin xyz="-0.118 0 -0.025" rpy="0 0 ${M_PI}" />
   </xacro:perception_kit>
 
   <xacro:perception_kit camera_pos="left" camera_type="$(arg left_camera)">
-    <origin xyz="0 0.085 0.025" rpy="0 0 ${M_PI/2}" />
+    <origin xyz="0 0.083 -0.025" rpy="0 0 ${M_PI/2}" />
   </xacro:perception_kit>
 
   <xacro:perception_kit camera_pos="right" camera_type="$(arg right_camera)">
-    <origin xyz="0 -0.085 0.025" rpy="0 0 -${M_PI/2}" />
+    <origin xyz="0 -0.083 -0.025" rpy="0 0 -${M_PI/2}" />
   </xacro:perception_kit>
 
 </robot>
diff --git a/src/kits/top/mid360_sensor_kit_bringup/urdf/sensor_kit_macro.urdf.xacro b/src/kits/top/mid360_sensor_kit_bringup/urdf/sensor_kit_macro.urdf.xacro
@@ -5,24 +5,33 @@
 
     <xacro:arg name="use_nominal_extrinsics" default="false"/>
     <xacro:include filename="$(find realsense2_description)/urdf/_d435.urdf.xacro" />
+
     <xacro:property name="type" value="${camera_type}" />
 
-    <xacro:if value="${type in ['realsense_d435']}">
-      <xacro:sensor_d435 parent="perception_kit_base_link" name="camera1" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
-        <origin xyz="0.1 0 0" rpy="0 0 0"/>
-      </xacro:sensor_d435>
-    </xacro:if>
-
-    <xacro:if value="${type in ['rgb_camera']}">
-      <link name="${camera_pos}_rgb_camera_link">
+    <xacro:if value="${type in ['realsense_d435', 'rgb_camera']}">    
+      <link name="${camera_pos}_camera_link">
+        <inertial>
+          <origin xyz="-0.118 0 -0.32" rpy="${M_PI/2} 0 ${M_PI/2}" />
+          <mass value="0.0" />
+          <inertia ixx="0.0" iyy="0.0" izz="0.0" ixy="0.0" iyz="0.0" ixz="0.0" />
+        </inertial>
         <visual>
-          <origin xyz="0 0 0" rpy="0 0 0" />
+          <origin xyz="-0.118 0 -0.32" rpy="${M_PI/2} 0 ${M_PI/2}" />
           <geometry>
             <mesh
               filename="package://mid360_sensor_kit_bringup/meshes/${camera_type}.stl"
               scale="0.001 0.001 0.001" />
           </geometry>
+          <material name="silver" />
         </visual>
+        <collision>
+          <origin xyz="-0.118 0 -0.32" rpy="${M_PI/2} 0 ${M_PI/2}" />
+          <geometry>
+            <mesh
+              filename="package://mid360_sensor_kit_bringup/meshes/${camera_type}.stl"
+              scale="0.001 0.001 0.001" />
+          </geometry>
+        </collision>
       </link>
 
       <joint name="${camera_pos}_camera_joint" type="fixed">
@@ -31,6 +40,12 @@
         <xacro:insert_block name="origin" />
       </joint>
     </xacro:if>
+
+    <xacro:if value="${type in ['realsense_d435']}">
+      <xacro:sensor_d435 parent="${camera_pos}_camera_link" name="${camera_pos}_d435" use_nominal_extrinsics="$(arg use_nominal_extrinsics)">
+        <origin xyz="-0.005 0.006 -0.0085" rpy="0 0 0"/>
+      </xacro:sensor_d435>
+    </xacro:if>
   </xacro:macro>
 
 </robot>
diff --git a/src/wr_devkit_bringup/launch/platform/wr_devkit_platform.launch.py b/src/wr_devkit_bringup/launch/platform/wr_devkit_platform.launch.py
@@ -1,16 +1,16 @@
+import sys
 from launch import LaunchDescription
-from launch.actions import IncludeLaunchDescription, GroupAction, DeclareLaunchArgument
+from launch.actions import IncludeLaunchDescription, GroupAction, DeclareLaunchArgument, LogInfo
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 from launch_xml.launch_description_sources import XMLLaunchDescriptionSource
-from launch_ros.actions import Node, PushRosNamespace, SetParameter
+from launch_ros.actions import Node, SetParameter
 from launch_ros.substitutions import FindPackageShare
-from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution, PythonExpression, TextSubstitution
 from launch.conditions import IfCondition
 
 
 def generate_launch_description():
     robot_model = LaunchConfiguration("robot_model")
-    camera = LaunchConfiguration("camera")
 
     declare_use_namespace_cmd = DeclareLaunchArgument(
         "use_namespace",
@@ -19,7 +19,9 @@ def generate_launch_description():
     )
 
     declare_namespace_cmd = DeclareLaunchArgument(
-        "namespace", default_value="", description="Top-level namespace"
+        "namespace", 
+        default_value="", 
+        description="Top-level namespace"
     )
 
     declare_use_sim_time_cmd = DeclareLaunchArgument(
@@ -34,10 +36,28 @@ def generate_launch_description():
         description="ranger_mini_v2, scout_mini",
     )
 
-    declare_camera_cmd = DeclareLaunchArgument(
-        "camera",
-        default_value="false",
-        description="Launch realsense d435 camera",
+    declare_front_camera_cmd = DeclareLaunchArgument(
+        "front_camera",
+        default_value="",
+        description="Front camera type",
+    )
+
+    declare_rear_camera_cmd = DeclareLaunchArgument(
+        "rear_camera",
+        default_value="",
+        description="Rear camera type",
+    )
+
+    declare_left_camera_cmd = DeclareLaunchArgument(
+        "left_camera",
+        default_value="",
+        description="Left camera type",
+    )
+
+    declare_right_camera_cmd = DeclareLaunchArgument(
+        'right_camera',
+        default_value="",
+        description='Right camera type'
     )
 
     SetParameter(
@@ -133,6 +153,12 @@ def generate_launch_description():
     ])
 
     # --------- Sensor kits ---------
+    # params = dict([aa for aa in [aa.split(':=') for aa in sys.argv] if len(aa) == 2])
+    # camera_type = params.get('camera', []).split(',')
+    # print(camera_type)
+    # for i in camera:
+    #     print(i)
+
     sensor_kit_bringup = GroupAction([
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource([
@@ -141,7 +167,13 @@ def generate_launch_description():
                     "launch",
                     "sensor_kit.launch.py",
                 ])
-            ])
+            ]),
+            launch_arguments={
+                "front_camera": LaunchConfiguration("front_camera", default="none"),
+                "rear_camera": LaunchConfiguration("rear_camera", default="none"),
+                "left_camera": LaunchConfiguration("left_camera", default="none"),
+                "right_camera": LaunchConfiguration("right_camera", default="none")
+            }.items(),
         ),
         IncludeLaunchDescription(
             PythonLaunchDescriptionSource([
@@ -160,12 +192,18 @@ def generate_launch_description():
             PythonLaunchDescriptionSource([
                 PathJoinSubstitution([
                     FindPackageShare("realsense2_camera"),
-                    "examples",
-                    "align_depth",
-                    "rs_align_depth_launch.py",
+                    "launch",
+                    "rs_launch.py",
                 ])
             ]),
-            condition=IfCondition(camera)
+            launch_arguments={
+                "rgb_camera.color_profile": "640,480,15",
+                "depth_module.depth_profile": "640,480,15",
+                "depth_module.infra_profile": "640,480,15",
+                "pointcloud.enable": "true",
+                "pointcloud.stream_filter": "0",
+                "pointcloud.stream_index_filter": "-1"
+            }.items(),
         ),
         # Node(
         #     package='tf2_ros',
@@ -182,7 +220,10 @@ def generate_launch_description():
         declare_namespace_cmd,
         declare_use_sim_time_cmd,
         declare_robot_model_cmd,
-        declare_camera_cmd,
+        declare_front_camera_cmd,
+        declare_rear_camera_cmd,
+        declare_left_camera_cmd,
+        declare_right_camera_cmd,
         robot_base_bringup,
         chassis_bringup,
         sensor_kit_bringup