Px4 Gazebo SITL - ROVIO Feedback to EKF

[NicoMandel]

Hi everyone, Hello @bloesch

I am new to this forum and its topics, so please be gentle with me if I make some mistakes regarding the conventions. First of all I would like to say thank you for providing a very useful open source and nicely compact package, so far it has been a pleasure to work with. As I am delving deeper into the matter, I am reaching the boundaries of my understanding and a few questions arise, so here goes:
I want to get ROVIO running on my PC in a simulation, to later be able to feedback the information into the EKF of the Px4. So far I managed to download ROVIO and its dependencies and build the catkin workspace, as well as run the basic simulation on the Vicon Room 1 Easy Dataset.
Now I am currently at the stage where I want to employ ROVIO into my own project using a launch file similar to the examples provided in this issue and this tutorial.
UPDATE
I have an error running in my terminal now:

[FilterBase::addUpdateMeas] Warning: included measurements at time 0 before safeTime 2.328

and the RVIZ does not update the position estimate, the UAV just seems to be standing on the ground, even though it is moving in the Gazebo environment.

My current launch file launches the following sections: Edited

<launch>

    <!-- Launching the basic Px4 launch file with front-facing camera output -->
    <include file="$(find iris_control)/launch/basic.launch"/>

    <!-- Launch a node from the img_converter package, which remaps from an argument Topic to "GrayImg"-->
    <node pkg="img_converter" name="ImgConv" type="img_converter_python.py"/>
    
    <!-- Launch the rovio node -->
    <node name="image_proc" type="image_proc" pkg="img_proc" output="screen">
        <remap from="/image_raw" to="/iris_fpv_cam/usb_cam/image_raw"/>
        <remap from="/camera_info" to="/iris_fpv_cam/usb_cam/camera_info"/>
    </node>
    <node pkg="rovio" type="rovio_node" name="rovio" output="screen">
        <remap from="/imu0" to="/mavros/imu/data"/>
        <!-- Will need a monocular remapping here -->
        <remap from="/cam0/image_raw" to="/GrayImg"/>
    </node>
</launch>

The basic.launch file loads the example from the px4 developer guide (mavros_posiv_sitl.launch) and sets the model to iris_fpv_cam and loads a custom world (from the Px4/Avoidance worlds, to conform with physics set in the custom empty_world.world)
I have a single forward facing camera with the following outputs on mavros/iris_fpv_cam/usb_cam/camera_info:

header: 
  seq: 219
  stamp: 
    secs: 455
    nsecs: 112000000
  frame_id: "robot_camera_link"
height: 240
width: 320
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [277.191356, 0.0, 320.5, 0.0, 277.191356, 240.5, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [277.191356, 0.0, 320.5, -0.0, 0.0, 277.191356, 240.5, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False

Now comes the part I am a little confused about:

1. What changes do I have to make to the following lines/files for this to work?

<param name="filter_config" value="$(find rovio)/cfg/rovio.info"/>

and

<param name="camera0_config" value="$(find rovio)/cfg/euroc_cam0.yaml"/>

UPDATE
I deleted those lines from my launch file, as seen above

2. Do I have to remap to a monocular topic or will ROVIO work with the RGB topic? Or do I have to do a conversion to the bgr convention from OpenCV?

UPDATE:
I received an error complaining about cv2 conversion between U8C and RGB and implemented a node to convert to GrayScale:

class image_converter:

  def __init__(self, SubTopic, PubTopic):
    self.image_pub = rospy.Publisher(PubTopic,Image,queue_size=1)
    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber(SubTopic,Image,self.callback)

  def callback(self,data):
    try:
        cv_image = self.bridge.imgmsg_to_cv2(data)
        gray = cv2.cvtColor(cv_image, cv2.COLOR_RGB2GRAY)
        rospy.loginfo("Converting and publishing Image message")
        self.image_pub.publish(self.bridge.cv2_to_imgmsg(gray, "mono8"))
    except CvBridgeError as e:
        print(e)
    except rospy.ROSException as e:
        print(e)

3. How do I actually re-input the position estimate into the EKF? I know I have to set some flags and publish to the vision topic according to the Px4 tutorials, but so far I have not been able to find a python tutorial which could help me with setting these flags.

4. What is the significance of the /mavros/cam_imu/sync/cam_imu_stamp topic that has shown up in my rostopic list when running my custom gazebo simulation, I am assuming it helps fix this or this, or is it unrelated?

The specs of my system are:

• Ubuntu 18.04
• ROS Melodic
• Gazebo 9
• Px4 Firmware Git branch v1.9.0-alpha
• default iris_fpv_cam configuration from the Px4 repository

Any help will be greatly appreciated. Thank you for taking the time to read it all, I know it is a lot, sorry for that.

Regards
Nico