Updating Gazebo model's position from SITL

I’ve been wanting to use Gazebo to model a scene and camera that moves with my (simulated) plane while running SITL. I’ve looked through the SwiftGust and khancyr plugins and they use a Gazebo-Zephyr plane. It is also my understanding than when using this plane/frame the flight dynamics is modeled in Gazebo and not in the ArduPlane SITL anymore (I could be wrong about this, my understanding is limited). This isn’t quite what I am wanting.

What I am hoping/wanting to do is keep the FDM in the default ArduPlane SITL and just send the position (and other needed dynamics) to Gazebo so that Gazebo can draw the scene and create a camera image that gets sent out (to a separate application I use to do image processing). Gazebo shouldn’t need to send anything back to the SITL in this use case.

Is there a way to do this; to send some world data from the SITL to Gazebo to update a model’s location in Gazebo?

There are a couple of ways to do this using Ignition Gazebo (I’m not so sure about the legacy versions), but any approach is going to require an amount of work.

One approach would be to use the SITL vehicle dynamics as a physics engine for Ignition. You can implement a custom physics engine plugin for ignition-physics - it’s a non-trivial exercise, but you could add collisions and interact with other objects in the scene if you went down this route.

The alternative option would be to build a custom plugin for either ignition-gui or ignition-gazebo to which you would feed the model pose from SITL and let the plugin update the model pose in the scene. This might be the easier option, as in this case you’d drop the entire physics system from the scene (which can be done in Ignition, because the physics is just another system plugin). There is still an amount of work to do (C++ plugin for the simulation, publishing pose info from SITL) as there is nothing in place that I’m aware of that does this at present.

I did this long time ago : GitHub - khancyr/gazebo_ardu: gazebo visualisation for ArduPilot, not a simulation !

So it takes the drone pos and attitude from mavros topics and move the gazebo object accordingly

1 Like

Nice! We might be able to do something similar using the python bindings for ignition-transport I put together - there wouldn’t be a dependency on ROS then.

There are also python bindings for some of the Gazebo API in the latest version of Ignition Fortress which may provide another means to update the pose.

Yes, doing without ROS would be great

In gazebo there is an option to set pose from CLI with

gz model

but this is not very elegant solution

Bit more investigation:

There is also a service available in Ignition to set the pose. It can be run from the command line with:

ign service -s /world/move_model/set_pose --reqtype ignition.msgs.Pose  --reptype ignition.msgs.Boolean  --timeout 300 --req 'name: "box", position: {x: 0, y: 0, z: 10}'

where move_model is the name of the world and box is the name of the model to move. It turns out that you do need to have the physics system enabled for this to work, but you can mark the model as static so it won’t fall under gravity between updates.

If I add the python bindings for services to the python-ignition project, then this can be run from a script. We could use pymavlink to get the pose information from ArduPilot and I think that’s everything we’d need.

A follow camera would complete the ask, and that is already available in Ignition and it can be repositioned using another service if different viewpoints are required.

Update

It was straightforward to add the Python bindings for the service interface. A script corresponding to the command line above is:

from ignition.msgs.header_pb2 import Header
from ignition.msgs.pose_pb2 import Pose
from ignition.msgs.vector3d_pb2 import Vector3d

from ignition.transport import Node

# create a transport node
node = Node()

# create a pose
vector3d_msg = Vector3d()
vector3d_msg.x = 0.0
vector3d_msg.y = 0.0
vector3d_msg.z = 10.0

pose_msg = Pose()
pose_msg.name = "box"
pose_msg.position.CopyFrom(vector3d_msg)

# submit a request to a service (blocking)
result = node.request(
  service="/world/move_model/set_pose",
  pose_msg,
  timeout=300,
  reptype="ignition.msgs.Boolean")

This works, however there seems to be an issue when the update rate is increased. If the pose update is greater than about 5Hz the GUI locks. It’s not yet clear what is going on in Ignition to cause this - possibly a race condition or something similar. It’s not the service request as that commits the request to a command buffer and returns correctly (i.e. the fact the service request is blocking on the Python script does not seem to be the bottleneck).

I’ve logged an issue with the Ignition team about the rate limits on updating a pose via the service interface - they may be able to suggest an alternative approach.

Update

The issue will be addressed by SceneBroadcaster: only send changed state information for change events by adlarkin · Pull Request #1392 · ignitionrobotics/ign-gazebo · GitHub when it’s merged. This will allow the pose to be updated at 60Hz or greater which matches the rate at which the scene is broadcast to the GUI.

Here’s a clip of Ignition being used as a 3D viewer for ArduPilot SITL.

There is no interpolation or filtering on the GUI side - so the vehicle motion appears a little jerky because of the mavlink stream rate.

SITL command

sim_vehicle.py -v ArduCopter -f quad --map --console

Models and code

Dependencies

  • dronekit-python is used to retrieve mavlink messages from ArduPilot SITL
  • Ignition Garden patched with PR #1392 is used for the viewer and pose service
  • python-ignition contains python bindings for the Ignition services
  • ignition-math python bindings are used to manipulate vectors and quaternions for the pose transform between the ArduPilot and Gazebo coordinate systems
2 Likes