rtabmap with PX4 in Gazebo

Posted by lh917 on
URL: http://official-rtab-map-forum.206.s1.nabble.com/rtabmap-with-PX4-in-Gazebo-tp6721.html

Hi, I'm fairly new to rtabmap and trying to implement stereo odometey with a px4 built-in model and iris drone. I followed the tutorial in SetupOnYourRobot with the Stereo B approach. Then I tried to implement vision position fusion with no GPS.

My launch file for rtabmap is:

<launch>
  <arg name="rviz" default="false" />
  <arg name="rtabmapviz" default="true" />

<arg name="pi/2" value="1.5707963267948966" />
<arg name="optical_rotate" value="0 0 0 -$(arg pi/2) 0 -$(arg pi/2)" />
 
 <node pkg="tf" type="static_transform_publisher"  name="base_to_color"
      args="$(arg optical_rotate) iris_0/base_link camera_link 100" />

<!-- Run the ROS package stereo_image_proc -->
<group ns="stereo" >
    <node pkg="stereo_image_proc" type="stereo_image_proc" name="stereo_image_proc"/>

    <!-- Odometry -->
    <node pkg="rtabmap_ros" type="stereo_odometry" name="stereo_odometry" output="screen">
        <remap from="left/image_rect"       to="/stereo/left/image_rect"/>
        <remap from="right/image_rect"      to="/stereo/right/image_rect"/>
        <remap from="left/camera_info"      to="/stereo/left/camera_info"/>
        <remap from="right/camera_info"     to="/stereo/right/camera_info"/>
        <remap from="odom"                  to="/stereo/stereo_odometry"/>

        <param name="frame_id" type="string" value="iris_0/base_link"/>
        <param name="odom_frame_id" type="string" value="odom"/>
        <param name="approx_sync"   type="bool" value="false"/>
        <param name="queue_size"    type="int" value="5"/>

        <param name="Odom/MinInliers" type="string" value="12"/>
        <param name="Odom/RoiRatios"  type="string" value="0.03 0.03 0.04 0.04"/>
    </node>     
</group>



  <group ns="rtabmap">
    <!-- Visual SLAM -->
    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="--delete_db_on_start">
      <param name="subscribe_stereo" type="bool" value="true"/>
     <param name="subscribe_depth"  type="bool" value="false"/>
     <param name="approx_sync"      type="bool" value="false"/>

     <remap from="left/image_rect"   to="/stereo/left/image_rect_color"/>
     <remap from="right/image_rect"  to="/stereo/right/image_rect"/>
     <remap from="left/camera_info"  to="/stereo/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo/right/camera_info"/>

      <remap from="odom" to="/stereo/stereo_odometry"/>

      <param name="frame_id"    type="string" value="iris_0/base_link"/>
      <param name="queue_size"  type="int"    value="40"/>
      <param name="approx_sync" type="bool"   value="false"/>

      <param name="Vis/MinInliers" type="string" value="12"/>
    </node>
  </group>

 
<!-- Visualisation RTAB-Map -->
 <group ns="rtabmap">  
  <!-- Visualisation RTAB-Map -->
  <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
     <param name="subscribe_stereo"    type="bool"   value="true"/>
     <param name="subscribe_odom_info" type="bool"   value="true"/>
     <param name="queue_size"          type="int"    value="10"/>
     <param name="frame_id"            type="string" value="iris_0/base_link"/>
     <remap from="left/image_rect"   to="/stereo/left/image_rect_color"/>
     <remap from="right/image_rect"  to="/stereo/right/image_rect"/>
     <remap from="left/camera_info"  to="/stereo/left/camera_info"/>
     <remap from="right/camera_info" to="/stereo/right/camera_info"/>
     <remap from="odom_info"         to="/stereo/odom_info"/>
     <remap from="odom"              to="/stereo/stereo_odometry"/>
  </node>
</group>

</launch>

I then subscribe to /stereo/stereo_odometry and use a python script to obtain the pose information and add time stamp and send to mavros/vision_pose/pose:

import rospy
from geometry_msgs.msg import PoseStamped
from nav_msgs.msg import Odometry
import math
from pyquaternion import Quaternion
import tf
import sys

vehicle_type = sys.argv[1]
local_pose = PoseStamped()
local_pose.header.frame_id = 'map'

def rtabmap_callback(data):
    local_pose.header.stamp = data.header.stamp
    local_pose.pose = data.pose.pose
    
rospy.init_node('rtabmap_transfer')
rospy.Subscriber("/stereo/stereo_odometry", Odometry, rtabmap_callback)
position_pub = rospy.Publisher(vehicle_type+"_0/mavros/vision_pose/pose", PoseStamped, queue_size=2)
rate = rospy.Rate(30) 

while True:
    local_pose.header.stamp = rospy.Time.now()
    position_pub.publish(local_pose) 
    rate.sleep()

After everything sets up and I launch the simulation, the drone starts to drift and spiral.
I also get this warning:

[ WARN] [1591277980.417315019, 383.544000000]: Odometry: Detected not valid consecutive stamps (previous=383.260000s new=383.260000s). 
New stamp should be always greater than previous stamp. This new data is ignored. This message will appear only once.

Could you help and see where's the problem?