Transform Frame Issue D435

[samim-17]

While following this tutorial, I tried to add d435 camera urdf and added to the existing drone mode. The gazebo.launch is working fine but when I try to launch slam.launch i get the following issue My TF tree is:

and my launch file is: <?xml version="1.0"?>

Please help required !

[matlabbe]

Based on the TF warning, cameracolor frame doesn't exist, which is indeed true looking at your tf tree. That frame is taken from the camera topics, mostly /camera/camera/color/image_raw. Check its header, probably cameracolor is set as frame_id. You should fix the publisher to publish under camera_rgb_optical_frame. Same for all camera topics, make sure they are using the right frame_id.

[samim-17]

Thanks for the reply! my d435 urdf file is below

<?xml version="1.0"?>

<!--
License: Apache 2.0. See LICENSE file in root directory.
Copyright(c) 2017 Intel Corporation. All Rights Reserved

This is the URDF model for the Intel RealSense 430 camera, in it's
aluminum peripherial evaluation case.
-->

<robot name="sensor_d435" xmlns:xacro="http://ros.org/wiki/xacro">
  <!--File includes-->
  <xacro:include filename="$(find realsense2_description)/urdf/_d435.gazebo.xacro"/>

  <xacro:macro name="sensor_d435" params="name:=camera topics_ns:=camera prefix parent *origin publish_pointcloud:=true">
    <xacro:property name="M_PI" value="3.1415926535897931" />

    <!-- The following values are approximate, and the camera node
     publishing TF values with actual calibrated camera extrinsic values -->
    <xacro:property name="d435_cam_depth_to_left_ir_offset" value="0.0"/>
    <xacro:property name="d435_cam_depth_to_right_ir_offset" value="-0.050"/>
    <xacro:property name="d435_cam_depth_to_color_offset" value="0.015"/>

    <!-- The following values model the aluminum peripherial case for the
    D435 camera, with the camera joint represented by the actual 
    peripherial camera tripod mount -->
    <xacro:property name="d435_cam_width" value="0.090"/>
    <xacro:property name="d435_cam_height" value="0.025"/>
    <xacro:property name="d435_cam_depth" value="0.02505"/>
    <xacro:property name="d435_cam_mount_from_center_offset" value="0.0149"/>

    <!-- The following offset is relative the the physical D435 camera peripherial
    camera tripod mount -->
    <xacro:property name="d435_cam_depth_px" value="${d435_cam_mount_from_center_offset}"/>
    <xacro:property name="d435_cam_depth_py" value="0.0175"/>
    <xacro:property name="d435_cam_depth_pz" value="${d435_cam_height/2}"/>

    <material name="${name}_aluminum">
    <color rgba="0.5 0.5 0.5 1"/>
    </material>

    <!-- camera body, with origin at bottom screw mount -->
    <joint name="${name}_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_bottom_screw_frame" />
    </joint>
    <link name="${name}_bottom_screw_frame"/>

    <joint name="${name}_link_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_py} ${d435_cam_depth_pz}" rpy="0 0 0"/>
      <parent link="${name}_bottom_screw_frame"/>
      <child link="${name}_link" />
    </joint>

    <link name="${name}_link">
      <visual>
      <origin xyz="${d435_cam_mount_from_center_offset} ${-d435_cam_depth_py} 0" rpy="${M_PI/2} 0 ${M_PI/2}"/>
        <geometry>
          <!-- <box size="${d435_cam_width} ${d435_cam_height} ${d435_cam_depth}"/> -->
          <mesh filename="package://realsense2_description/meshes/d435.dae" />
          <!--<mesh filename="package://realsense2_description/meshes/d435/d435.dae" />-->
        </geometry>
        <material name="${name}_aluminum"/>
      </visual>
      <collision>
        <origin xyz="0 ${-d435_cam_depth_py} 0" rpy="0 0 0"/>
        <geometry>
        <box size="${d435_cam_depth} ${d435_cam_width} ${d435_cam_height}"/>
        </geometry>
      </collision>
      <inertial>
        <!-- The following are not reliable values, and should not be used for modeling -->
        <mass value="0.072" />
        <origin xyz="0 0 0" />
        <inertia ixx="0.003881243" ixy="0.0" ixz="0.0" iyy="0.000498940" iyz="0.0" izz="0.003879257" />
      </inertial>
    </link>

    <!-- camera depth joints and links -->
    <joint name="${name}_depth_joint" type="fixed">
      <origin xyz="0 0 0" rpy="0 0 0"/>
      <parent link="${name}_link"/>
      <child link="${name}_depth_frame" />
    </joint>
    <link name="${name}_depth_frame"/>

    <joint name="${name}_depth_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_depth_optical_frame" />
    </joint>
    <link name="${name}_depth_optical_frame"/>

    <!-- camera left IR joints and links -->
    <joint name="${name}_left_ir_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_to_left_ir_offset} 0" rpy="0 0 0" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_left_ir_frame" />
    </joint>
    <link name="${name}_left_ir_frame"/>

    <joint name="${name}_left_ir_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_left_ir_frame" />
      <child link="${name}_left_ir_optical_frame" />
    </joint>
    <link name="${name}_left_ir_optical_frame"/>

    <!-- camera right IR joints and links -->
    <joint name="${name}_right_ir_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_to_right_ir_offset} 0" rpy="0 0 0" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_right_ir_frame" />
    </joint>
    <link name="${name}_right_ir_frame"/>

    <joint name="${name}_right_ir_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_right_ir_frame" />
      <child link="${name}_right_ir_optical_frame" />
    </joint>
    <link name="${name}_right_ir_optical_frame"/>

    <!-- camera color joints and links -->
    <joint name="${name}_color_joint" type="fixed">
      <origin xyz="0 ${d435_cam_depth_to_color_offset} 0" rpy="0 0 0" />
      <parent link="${name}_depth_frame" />
      <child link="${name}_color_frame" />
    </joint>
    <link name="${name}_color_frame"/>

    <joint name="${name}_color_optical_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
      <parent link="${name}_color_frame" />
      <child link="${name}_color_optical_frame" />
    </joint>
    <link name="${name}_color_optical_frame"/>

    <!-- Realsense Gazebo Plugin -->
    <xacro:gazebo_d435 camera_name="${name}" reference_link="${name}_link" topics_ns="${topics_ns}" depth_optical_frame="${name}_depth_optical_frame" color_optical_frame="${name}_color_optical_frame" infrared1_optical_frame="${name}_left_ir_optical_frame" infrared2_optical_frame="${name}_right_ir_optical_frame" publish_pointcloud="${publish_pointcloud}"/>

  </xacro:macro>
</robot>

I didn't quite seem to understand where the frame_id is set and how to publish under camer_rgb_optical_frame my camera topics is below

[matlabbe]

What is the frame_id shown when you do:

rostopic echo /camera/camera/color/image_raw/header

[samim-17]

As you mentioned the frame_id is cameracolor

seq: 0 stamp: secs: 11 nsecs: 205000000 frame_id: "cameracolor"

seq: 1 stamp: secs: 11 nsecs: 262000000 frame_id: "cameracolor"