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:

<arg name="localization" default="false"/>
<arg name="rtabmap_viz"   default="true"/>
<arg name="ground_truth" default="false"/>

<arg     if="$(arg localization)" name="pre_args" default=""/>
<arg unless="$(arg localization)" name="pre_args" default="-d"/>

<node pkg="nodelet" type="nodelet" name="imu_to_tf" args="standalone rtabmap_util/imu_to_tf">
  <remap from="imu/data" to="/mavros/imu/data"/>
  <param name="fixed_frame_id" value="base_link_stabilized"/>
  <param name="base_frame_id" value="base_link"/>
</node>

<!-- To connect rtabmap planning stuff with move_base below -->
<param name="/rtabmap/rtabmap/use_action_for_goal" value="true"/>
<remap from="/rtabmap/move_base" to="/move_base"/>

<!-- VSLAM -->
<param name="/rtabmap/rtabmap/latch" value="false"/> <!-- For some reason, if we latch grid_map, the global costmap inflation layer will create holes on robot path. To avoid holes, republish grid_map on each update (latching disabled). -->
<include file="$(find rtabmap_launch)/launch/rtabmap.launch">
  <arg name="localization"      value="$(arg localization)"/>
  <arg name="args"              value="$(arg pre_args) --Optimizer/GravitySigma 0.1 --Vis/FeatureType 10 --Kp/DetectorStrategy 10 --Grid/MapFrameProjection true --NormalsSegmentation false --Grid/MaxGroundHeight 1 --Grid/MaxObstacleHeight 1.6 --RGBD/StartAtOrigin true" />
  <arg name="rtabmap_viz"        value="$(arg rtabmap_viz)" />
  <arg name="frame_id"          value="base_link" />
  <arg name="odom_guess_frame_id" value="base_link_stabilized" />
  <arg name="rgb_topic"         value="/camera/camera/color/image_raw" />
  <arg name="depth_topic"       value="/camera/camera/depth/image_raw" />
  <arg name="camera_info_topic" value="/camera/camera/color/camera_info" /> 

  <arg name="imu_topic"         value="/mavros/imu/data"/>
  <arg name="wait_imu_to_init"  value="true"/>
  <arg name="approx_sync"       value="true"/>
  <arg if="$(arg ground_truth)" name="ground_truth_frame_id" value="world"/>
  <arg if="$(arg ground_truth)" name="ground_truth_base_frame_id" value="base_link_gt"/>
</include>

<!-- Costmap -->
<node pkg="nodelet" type="nodelet" name="camera_points_xyz" args="standalone rtabmap_util/point_cloud_xyz">
  <remap from="depth/image"       to="/camera/camera/depth/image_raw"/>
  <remap from="depth/camera_info" to="/camera/camera/depth/camera_info"/>
  <remap from="cloud"             to="camera_cloud" />

  <param name="decimation"  type="double" value="4"/>
  <param name="voxel_size"  type="double" value="0.0"/>
  <param name="approx_sync" type="bool"   value="true"/>
</node> 

<!-- navigation -->  
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
   <remap from="map" to="/rtabmap/grid_map"/>
   <remap from="odom" to="/rtabmap/odom"/>
   <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />  
   <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="global_costmap" />
   <rosparam file="$(find rtabmap_drone_example)/param/costmap_common_params.yaml" command="load" ns="local_costmap" />
   <rosparam file="$(find rtabmap_drone_example)/param/global_costmap_params.yaml" command="load" />
   <rosparam file="$(find rtabmap_drone_example)/param/local_costmap_params.yaml" command="load" />
   <rosparam file="$(find rtabmap_drone_example)/param/base_local_planner_params.yaml" command="load" />
</node>

<!-- joystick -->
<rosparam file="$(find rtabmap_drone_example)/config/joy_config.yaml" />
<node pkg="joy" type="joy_node" name="joy_node">
    <param name="autorepeat_rate" value="5"/>
</node>
<node pkg="teleop_twist_joy" type="teleop_node" name="teleop_node" output="screen">
    <param name="autorepeat_rate" value="5"/>
</node>

<!-- Ground truth -->

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"

[samim-17]

Hello ! I would like to give a little update. I mapped the cameracolor to camera_color_optical_frame using

rosrun tf static_transform_publisher 0 0 0 0 0 0 camera_color_optical_frame cameracolor 100

and secondly mapped base_link_stabilized to odom

rosrun tf static_transform_publisher 0 0 0 0 0 0 map odom 100

my tf tree after these is

after doing these two, I got the RTAB map to visualize as below:

but the demo tutorial for rs200 has the map to odom broadcaster as /rtabmap/rtabmap which is not in my case.
now the costmap map is not showing in rviz and drone cannot go offboard.
Can you verify if the following two transformation are correct and what can I do further ?
slam.launch terminal is showing the following error.

Please kindly check and thanks

[matlabbe]

Remove

rosrun tf static_transform_publisher 0 0 0 0 0 0 map odom 100

that should not be a static transform. It is published by rtabmap node. I don't see any logs from rtabmap node in your terminal, it may be not receiving topics, so map->odom would not be published (and "Loop closure detection" will be empty and no map in rviz). Once rtabmap is receiving correctly the data, it will publish map->odom at 20 Hz.

For the cameracolor issue, that could be fixed by finding the corresponding config in your gazebo simulated camera config, and change it to camera_color_optical_frame directly.