Feeding external vision_pose_estimate data with SITL / gazebo / ROS

Hello everyone!

I would like to test fusing my own odometry data into the local position estimation within SITL / gazebo, following this tutorial.

But my pose messages do not seem to arrive anywhere.
Also, there already is vision data published by the simulation although I didn’t set up anything alike and there even is no camera at all.

I’m using ROS, so I launch

  • roslaunch px4 mavros_posix_sitl.launch
  • qgroundcontrol-start.sh

In the mavros_posix_sitl.launch file I’ve set <arg name="est" default="lpe"/>.
In QGroundControl, I’ve set

  • ATT_EXT_HDG_M to 1 for “vision”
  • LPE_FUSION to “fuse vision position” and “fuse vision yaw”.

Without publishing any position messages, it is possible to takeoff.
LPE_VIS_XY and LPE_VIS_Z parameters do effect the flight behavior (shakiness).
Thanks to this thread, I’ve been able to track down where these messages come from (but not really how to avoid them).

On the other hand, if I publish PoseStamped messages to /mavros/vision_pose/pose they are ignored:

  • The iris quadcopter in gazebo does not behave differently.
  • They do not show up in simulator_mavlink.cpp

I also tried publishing these messages using the /clock timestamps.
I get similar results using EKF2.

Am I doing something wrong?

More specifically:
How to disable the built-in vision position?
And more importantly: How do I make my vision position messages count?

I’m quite the PX4 newbie, so any help is appreciated.

OK, so it seems very hard to edit the simulated sensors, but I think I figured that out in the end.

About the unwanted vision position messages:
I’ve been able to get rid of them by commenting out

vision_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + vision_sub_topic_, &GazeboMavlinkInterface::VisionCallback, this);

in Tools/sitl_gazebo/src/gazebo_mavlink_interface.cpp and then compiling by

make posix_sitl_default gazebo

Still this seems a rather silly “solution” to me…

Alternative

I believe that sending these messages is not the default behavior. I may have provoked it by desperately trying to enable some sort of camera. So if you’re having unwanted vision position messages, check if your vehicle’s sdf file contains something like

<plugin name="vision_plugin" filename="libgazebo_vision_plugin.so">
    <robotNamespace></robotNamespace>
    <pub_rate>30</pub_rate>
    <random_walk>1.0</random_walk>
    <noise_density>0.0005</noise_density>
    <corellation_time>60.0</corellation_time>
</plugin>

and try commenting it out.
Unfortunately, sdf files and their apparently automatic generation process (especially in case of iris) is a completely different subject… but e.g. iris_opt_flow.sdf has that section.

@TSC21 I think you were working on this and. I’ll also share the link with a couple more interested parties.