Added custom base, with option to enable/disable it in the urdf

CMakeLists.txt

@@ -69,6 +69,8 @@ dai_add_node_ros2(rgbd_stereo_node src/rgbd_stereo_publisher.cpp)
 
 install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME})
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY meshes DESTINATION share/${PROJECT_NAME})
 
 install(TARGETS
   rgbd_stereo_node

launch/rgbd_stereo.launch.py

@@ -13,7 +13,7 @@
 def generate_launch_description():
     default_rviz = os.path.join(get_package_share_directory('oak_d_camera'),
                                 'rviz', 'rgbd_stereo_pcl.rviz')
-    urdf_launch_dir = os.path.join(get_package_share_directory('depthai_descriptions'), 'launch')
+    urdf_launch_dir = os.path.join(get_package_share_directory('oak_d_camera'), 'launch')
 
 
     camera_model = LaunchConfiguration('camera_model',  default = 'OAK-D-LITE')
@@ -40,6 +40,7 @@ def generate_launch_description():
     use_pointcloud  = LaunchConfiguration('use_pointcloud', default = True)
     pc_color        = LaunchConfiguration('pc_color',       default = True)
     only_rgb        = LaunchConfiguration('only_rgb',       default = False)
+    use_base       = LaunchConfiguration('use_base',       default = False)
 
 
     declare_camera_model_cmd = DeclareLaunchArgument(
@@ -151,7 +152,11 @@ def generate_launch_description():
         'only_rgb',
         default_value=only_rgb,
         description='Use only RGB image.')
-
+
+    declare_use_base_cmd = DeclareLaunchArgument(
+        'use_base',
+        default_value=use_base,
+        description='Use base description.')
 
     urdf_launch = IncludeLaunchDescription(
                             launch_description_sources.PythonLaunchDescriptionSource(
@@ -165,7 +170,8 @@ def generate_launch_description():
                                               'cam_pos_z'   : cam_pos_z,
                                               'cam_roll'    : cam_roll,
                                               'cam_pitch'   : cam_pitch,
-                                              'cam_yaw'     : cam_yaw}.items())
+                                              'cam_yaw'     : cam_yaw,
+                                              'use_base'    : use_base}.items())
 
 
     rgbd_stereo_node = launch_ros.actions.Node(
@@ -275,6 +281,7 @@ def generate_launch_description():
     ld.add_action(declare_use_pointcloud_cmd)
     ld.add_action(declare_point_cloud_color_cmd)
     ld.add_action(declare_only_rgb_cmd)
+    ld.add_action(declare_use_base_cmd)
 
     ld.add_action(rgbd_stereo_node)
     ld.add_action(urdf_launch)

launch/urdf_launch.py

@@ -0,0 +1,178 @@
+import os
+
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.conditions import IfCondition, UnlessCondition
+from launch.substitutions import Command, LaunchConfiguration
+from launch_ros.actions import LoadComposableNodes, Node
+from launch_ros.descriptions import ComposableNode
+
+
+def launch_setup(context, *args, **kwargs):
+    bringup_dir = get_package_share_directory("oak_d_camera")
+    xacro_path = os.path.join(bringup_dir, "urdf", "base_descr.urdf.xacro")
+
+    camera_model = LaunchConfiguration("camera_model", default="OAK-D")
+    tf_prefix = LaunchConfiguration("tf_prefix", default="oak")
+    base_frame = LaunchConfiguration("base_frame", default="oak-d_frame")
+    parent_frame = LaunchConfiguration("parent_frame", default="oak-d-base-frame")
+    cam_pos_x = LaunchConfiguration("cam_pos_x", default="0.0")
+    cam_pos_y = LaunchConfiguration("cam_pos_y", default="0.0")
+    cam_pos_z = LaunchConfiguration("cam_pos_z", default="0.0")
+    cam_roll = LaunchConfiguration("cam_roll", default="1.5708")
+    cam_pitch = LaunchConfiguration("cam_pitch", default="0.0")
+    cam_yaw = LaunchConfiguration("cam_yaw", default="1.5708")
+    namespace = LaunchConfiguration("namespace", default="")
+    rs_compat = LaunchConfiguration("rs_compat", default="false")
+    use_composition = LaunchConfiguration("use_composition", default="false")
+    use_base = LaunchConfiguration("use_base", default="false")
+
+    name = LaunchConfiguration("tf_prefix").perform(context)
+    robot_description = {
+        "robot_description": Command(
+            [
+                "xacro",
+                " ",
+                xacro_path,
+                " ",
+                "camera_name:=",
+                tf_prefix,
+                " ",
+                "camera_model:=",
+                camera_model,
+                " ",
+                "base_frame:=",
+                base_frame,
+                " ",
+                "parent_frame:=",
+                parent_frame,
+                " ",
+                "cam_pos_x:=",
+                cam_pos_x,
+                " ",
+                "cam_pos_y:=",
+                cam_pos_y,
+                " ",
+                "cam_pos_z:=",
+                cam_pos_z,
+                " ",
+                "cam_roll:=",
+                cam_roll,
+                " ",
+                "cam_pitch:=",
+                cam_pitch,
+                " ",
+                "cam_yaw:=",
+                cam_yaw,
+                " ",
+                "rs_compat:=",
+                rs_compat,
+                " ",
+                "use_base:=",
+                use_base,
+            ]
+        )
+    }
+    return [
+        Node(
+            package="robot_state_publisher",
+            condition=UnlessCondition(use_composition),
+            executable="robot_state_publisher",
+            name=name + "_state_publisher",
+            namespace=namespace,
+            parameters=[robot_description],
+        ),
+        LoadComposableNodes(
+            target_container=f"{namespace.perform(context)}/{name}_container",
+            condition=IfCondition(use_composition),
+            composable_node_descriptions=[
+                ComposableNode(
+                    package="robot_state_publisher",
+                    plugin="robot_state_publisher::RobotStatePublisher",
+                    name=name + "_state_publisher",
+                    namespace=namespace,
+                    parameters=[robot_description],
+                )
+            ],
+        ),
+    ]
+
+
+def generate_launch_description():
+    declared_arguments = [
+        DeclareLaunchArgument(
+            "namespace",
+            default_value="",
+            description='Specifies the namespace of the robot state publisher node. Default value will be ""',
+        ),
+        DeclareLaunchArgument(
+            "camera_model",
+            default_value="OAK-D",
+            description="The model of the camera. Using a wrong camera model can disable camera features. Valid models: `OAK-D, OAK-D-LITE`.",
+        ),
+        DeclareLaunchArgument(
+            "tf_prefix",
+            default_value="oak",
+            description="The name of the camera. It can be different from the camera model and it will be used as node `namespace`.",
+        ),
+        DeclareLaunchArgument(
+            "base_frame",
+            default_value="oak-d_frame",
+            description="Name of the base link.",
+        ),
+        DeclareLaunchArgument(
+            "parent_frame",
+            default_value="oak-d-base-frame",
+            description="Name of the parent link from other a robot TF for example that can be connected to the base of the OAK.",
+        ),
+        DeclareLaunchArgument(
+            "cam_pos_x",
+            default_value="0.0",
+            description="Position X of the camera with respect to the base frame.",
+        ),
+        DeclareLaunchArgument(
+            "cam_pos_y",
+            default_value="0.0",
+            description="Position Y of the camera with respect to the base frame.",
+        ),
+        DeclareLaunchArgument(
+            "cam_pos_z",
+            default_value="0.0",
+            description="Position Z of the camera with respect to the base frame.",
+        ),
+        DeclareLaunchArgument(
+            "cam_roll",
+            default_value="0.0",
+            description="Roll orientation of the camera with respect to the base frame.",
+        ),
+        DeclareLaunchArgument(
+            "cam_pitch",
+            default_value="0.0",
+            description="Pitch orientation of the camera with respect to the base frame.",
+        ),
+        DeclareLaunchArgument(
+            "cam_yaw",
+            default_value="0.0",
+            description="Yaw orientation of the camera with respect to the base frame.",
+        ),
+        DeclareLaunchArgument(
+            "use_composition",
+            default_value="false",
+            description="Use composition to start the robot_state_publisher node. Default value will be false",
+        ),
+        DeclareLaunchArgument(
+            "rs_compat",
+            default_value="false",
+            description="Enable RealSense compatibility mode. Default value will be false",
+        ),
+        DeclareLaunchArgument(
+            "use_base",
+            default_value="false",
+            description="Use base frame for the camera. Default value will be false",
+        ),
+    ]
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )

rviz/rgbd_stereo_pcl.rviz

@@ -7,7 +7,7 @@ Panels:
         - /Global Options1
         - /Status1
       Splitter Ratio: 0.5
-    Tree Height: 254
+    Tree Height: 268
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -209,33 +209,33 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz_default_plugins/Orbit
-      Distance: 5.273627281188965
+      Distance: 0.44121259450912476
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Focal Point:
-        X: -0.7700800895690918
-        Y: -0.4642658233642578
-        Z: 1.758968710899353
+        X: -0.008556458167731762
+        Y: -0.01131016667932272
+        Z: 0.033811572939157486
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.6297970414161682
+      Pitch: 0.2847992181777954
       Target Frame: <Fixed Frame>
       Value: Orbit (rviz_default_plugins)
-      Yaw: 3.1803972721099854
+      Yaw: 5.420414447784424
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
   Height: 794
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000274000002bcfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000013e000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb0000001400720069006700680074005f00720065006300740100000183000000b50000001700fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000fb00000012006c006500660074005f0072006500630074010000023e000000bd0000001700ffffff000000010000010f000002bcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002bc000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002d3000002bc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd000000040000000000000274000002cafc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000004e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003400000145000000a900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb0000001400720069006700680074005f0072006500630074010000017e000000ba0000001600fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000fb00000012006c006500660074005f0072006500630074010000023d000000c10000001600ffffff000000010000010f000002cafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000034000002ca0000009100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002d5000002ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:

urdf/base_camera.urdf.xacro

@@ -0,0 +1,47 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="base_oak">
+  <!-- Macro base -->
+  <xacro:macro name="base" params="camera_frame cam_pos_x cam_pos_y cam_pos_z cam_roll cam_pitch cam_yaw">
+    <link name="base_oak">
+
+      <visual>
+        <origin xyz="0 0 0" rpy="1.56 0 -1.56"/>
+        <geometry>
+          <mesh filename="package://oak_d_camera/meshes/oak_dlite_base.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="Gray">
+          <color rgba="0.5 0.5 0.5 1"/>
+        </material>
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="1.56 0 -1.56"/>
+        <geometry>
+          <mesh filename="package://oak_d_camera/meshes/oak_dlite_base.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="base_footprint_joint" type="fixed">
+      <parent link="base_oak"/>
+      <child link="base_footprint"/>
+      <origin xyz="0.055 0.015 -0.06863" rpy="0 0.0508 0"/>
+    </joint>
+
+    <link name="base_footprint">
+        <inertial>
+        <mass value="0.0"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <inertia ixx="0.0" ixy="0.0" ixz="0.0" iyy="0.0" iyz="0.0" izz="0.0"/>
+      </inertial>
+    </link>
+
+    <joint name="footprint_oak_joint" type="fixed">
+      <parent link="base_footprint"/>
+      <child link="${camera_frame}"/>
+      <origin xyz="${cam_pos_x} ${cam_pos_y} ${cam_pos_z + 0.08863}" rpy="${cam_roll} ${- 0.0508 - cam_pitch} ${cam_yaw}"/>
+    </joint>
+
+    <link name="${camera_frame}" />
+  </xacro:macro>
+</robot>

urdf/base_descr.urdf.xacro

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot name="depthai_camera" xmlns:xacro="http://ros.org/wiki/xacro">
+
+	<xacro:arg name="camera_name" default="oak" />
+	<xacro:arg name="camera_model" default="OAK-D" />
+	<xacro:arg name="base_frame" default="oak-d_frame" />
+	<xacro:arg name="parent_frame" default="oak-d-base-frame" />
+	<xacro:arg name="cam_pos_x" default="0.0" />
+	<xacro:arg name="cam_pos_y" default="0.0" />
+	<xacro:arg name="cam_pos_z" default="0.0" />
+	<xacro:arg name="cam_roll" default="0.0" />
+	<xacro:arg name="cam_pitch" default="0.0" />
+	<xacro:arg name="cam_yaw" default="0.0" />
+	<xacro:arg name="rs_compat" default="false" />
+    <xacro:arg name="use_base" default="false" />
+
+	<xacro:include filename="$(find oak_d_camera)/urdf/include/depthai_macro.urdf.xacro" />
+    <xacro:include filename="$(find oak_d_camera)/urdf/base_camera.urdf.xacro" />
+
+    <xacro:if value="$(arg use_base)">
+        <xacro:base camera_frame="$(arg parent_frame)" cam_pos_x="$(arg cam_pos_x)" cam_pos_y="$(arg cam_pos_y)" cam_pos_z="$(arg cam_pos_z)" cam_roll="$(arg cam_roll)" cam_pitch="$(arg cam_pitch)" cam_yaw="$(arg cam_yaw)" />
+    </xacro:if>
+
+    <xacro:unless value="$(arg use_base)">
+        <link name="$(arg parent_frame)" >
+			<pose xyz="$(arg cam_pos_x) $(arg cam_pos_y) $(arg cam_pos_z)" rpy="$(arg cam_roll) $(arg cam_pitch) $(arg cam_yaw)" />
+		</link>
+    </xacro:unless>
+
+	<xacro:depthai_camera camera_name="$(arg camera_name)" parent="$(arg parent_frame)"
+		camera_model="$(arg camera_model)" base_frame="$(arg base_frame)"
+		cam_pos_x="$(arg cam_pos_x)" cam_pos_y="$(arg cam_pos_y)" cam_pos_z="$(arg cam_pos_z)"
+		cam_roll="$(arg cam_roll)" cam_pitch="$(arg cam_pitch)" cam_yaw="$(arg cam_yaw)"
+		rs_compat="$(arg rs_compat)" />
+
+</robot>