Re: implementing rtabmap_drone_example in real drone

Posted by Samim-17 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/implementing-rtabmap-drone-example-in-real-drone-tp10688p10809.html

So I can set Odom/Strategy 8 to get the msckf_vio odometry approach right ?

My question is whether msckf_vio will support my infra1+depth?

I ran msckf_vio using the two ir camera in simulation and got the following

and in terminal I get this

Odom: quality=0, std dev=0.057065m|0.015811rad, update time=0.027484s, delay=0.031000s
[ INFO] [1735316669.258054018, 20.008000000]: Odom: quality=0, std dev=0.058607m|0.015811rad, update time=0.062536s, delay=0.099000s
[ INFO] [1735316669.500543202, 20.138000000]: Odom: quality=0, std dev=0.060890m|0.015811rad, update time=0.028479s, delay=0.096000s

Do I need to use infra1+infra2 for msck_vio? What about the costmap and move_base planning?Or is there another way of remapping slam.launch? If there is can you please provide the launch file.

I am guessing using msckf_vio will bring the /rtabmap/odom hz > 10 and rtabmap will give the dense 3d point cloud. Is it right ?

currently I'm using this launch file

<?xml version="1.0"?>
<launch>
   
    <arg name="localization" default="false"/>
    <arg name="rtabmap_viz"   default="true"/>
    <arg name="ground_truth" default="false"/>

     <arg name="sim"                   default="true"/>
    <arg name="use_global_map"        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="stereo"		value="true"/>
      <arg name="args"              value="$(arg pre_args) --Odom/Strategy 8 --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="left_image_topic" value="/D435_camera/camera/ir2/image_raw" />
      <arg name="right_image_topic" value="/D435_camera/camera/ir/image_raw" />
      <arg name="left_camera_info_topic" value="/D435_camera/camera/ir2/camera_info" />
      <arg name="right_camera_info_topic" value="/D435_camera/camera/ir/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/aligned_depth_to_color/image_raw"/>
      <remap from="depth/camera_info" to="/camera/aligned_depth_to_color/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="OmplPlanner/odometry_topic"      value="/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>
   <node name="empty_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_empty">
       <param name="cell_type" value="0"/>
   </node>
   <node name="marked_voxels_markers" pkg="rtabmap_costmap_plugins" type="voxel_markers" args="voxel_grid:=/move_base/local_costmap/voxel_layer/voxel_grid visualization_marker:=/voxels_marked" />
</launch>

In real drone, I get the following in RTAB gui with Odom/Strategy set to 8. I have done static transform of camera_link to base_link (0 0 0 0 0 0).


the image and features are opposite to each other. and in terminal I get

Odom: quality=0, std dev=0.014701m|0.015811rad, update time=0.003791s, delay=0.024658s
Odom: quality=0, std dev=0.014775m|0.015811rad, update time=0.031933s, delay=0.045303s
 Make sure IMU is published faster than data rate! (last image stamp=1735366937.850737 and last imu stamp received=1735366937.843248). Buffering the image until an imu with same or greater stamp is received.

What can be the issue.
Please guide me through this.