Add more sensors to Gazebo - SITL

Thanks, that would be great!

I’ll take a shot at getting it working using the existing JSON code as a template. I think it’s mostly there as you’ve already done all the socket work. I’m hoping its mainly a case of bolting a JSON encoder / decoder into the plugin and re-plumbing.

For GPS we’ll need changes on the SITL JSON side, and there’s probably a bit more planning required there to settle on a schema for the list of sensors we might want to eventually support. To start with I’ll use what’s there so no ardupilot changes are needed.

I had already success using Gazebo in the past but it was a pain to handle do to some weird reference frame on Gazebo.

First step would be just convert the basic information to comes from the JSON sharing then we would be to add more thing on the JSON

Here’s the first steps towards supporting the JSON interface:

I’m using jsoncpp as it should already be available if you have Gazebo installed (it is one of the dependencies tested in the cmake output when building ardupilot_gazebo). We only need to create a simple JSON document and the library seems sufficient for our requirements. There’s an example building the JSON document to be returned to SITL in the commit.

I’m also very interested in this. I’m currently using Ardupilot+Gazebo for swarming of copters and boats.

The plan is to also add in simulated video streams from the vehicles for additional visual autonomy testing. So far I have inserted a model of a real-life island and have the vehicles flying/driving around it.

1 Like

That sounds interesting!

I’ve been working on improvements to water / wave simulations in Gazebo and boat dynamics here: https://github.com/srmainwaring/asv_wave_sim/tree/feature/fft_waves

I’ve got a backlog of improvements to implement but the plan is to support boats running ArduPilot with ROS companion computers.

1 Like

@rhys, this is great idea. It would be good to:

  1. Maintain the plugins and models on ardupilot official profile. Currently there’s lot of forks on private accounts.
  2. Target to Gazebo 11
  3. Extend sensors/plugins. Lots of excellent work is being done in PX4 and RotorS. In fact integration with RotorS is something I tried in the past (this would give us access to Flightmare simulator)

That’s a good idea. It might be good to have two repos. One for the main SITL interface plugin and custom sensor modules, and another for models, worlds, materials and other media.

I’m running Gazebo 11 on macOS Catalina and the ardupilot_gazebo plugin works well. The main change might be to use the ignition modules as they become stable. I’ve seen a write up on this elsewhere and believe this fork gerkey/ardupilot_gazebo has done much of the work already.

Yep we definitely need two repos! I never merged switgus stuff as there was to many mesh and heavy stuff. It shouldn’t be hard. I can create the repos tomorrow.

Interesting the ignition modifications ! I still have some contact with ROS guys, I can ask them if we should spend some effort converting now. I could be worth

@khancyr I think I’m nearly there with an approach. The latest commit has a standalone C++ example (SocketExample.cc) to connect to SITL, recv the binary packet with the PWMs and return a JSON string containing the state. It’s basically one of the SITL python examples ported to C++ with all the physics stripped out. A few notes:

  • I’m using a copy of the ArduPilot SocketAPM class. There may be better ways of adding a dependency using git submodules, but this was the quickest way to get working and be consistent with SITL and keep the project light.
  • SIM_JSON seems to be fussy about the indentation of the JSON string it receives. I was getting a lot of missed messages until I removed all indentation and newlines from the returned message (except for the leading and training newlines which are required).
  • I’ve added the wind field just to see if it works (it does!)

Next steps:

  • Rework the ArduPilotPlugin to incorporate the approach. I plan to replace the existing Socket code with the ArduPilot version. I’m not a socket programming expert, so if you think that’s a bad idea please let me know (main concern on my side is Windows compatibility).
  • SIM_JSON provides PWM rather than normalised motor speeds, so the conversion will now have to occur in the plugin. I think that’s ok as it means the plugin looks more like a physical device implementing a servo or ESC response.
  • SIM_JSON expects ArduPilot convention quaternions (or Euler angles, but I’ll use quaternions), which will now need to be handled in the plugin.

We got SITL Python examples ??? :no_mouth: :scream:

For now, I don’t think we should be bothering about windows support. We could do it later.
About ArduPilot Socket class, I will ask other about the licensing, we need to be sure that we can use on other project without transfering the GPL licence. Copy past is fine, submodules can be a pain an this is a low management class anyway (maybe they got a socket class in Ignitions libs? To see later)

Yes, Json is quite strict on indentation. We may use JsonSchema in the future to make the exchange more robust !

About Quaterion, it is already handle and the current gazebo plugin is already sending the quaterion, so it should be fine

About PWM, yes, we could do the conversion on Gazebo side. It can be even more convenient as it will allow us to expose the motors/servo/esc configuration with Gazebo parameters.

This saved me a lot of time when trying to figure out why the C++ version wasn’t working:

The repo license file for ardupilot_gazebo is already GPL (although the plugin code is still copyrighted to the Open Source Robotics Foundation and have an Apache License). I think GPL is ok for the plugin if its going to be maintained as part of the ardupilot org set of repos. If the intention is for it to be contributed back to Gazebo plugin then it would need to conform to the licenses used by OSRF.

yep I just saw that I let the apache license on the file and stated that the repo was GPL. Normally, this is fine with apache licence. But adding ArduPilot socket library that is GPL doesn’t seems to be compliant as this would mean that all file should be GPL and Gazebo should be GPL too by linkage and that isn’t possible. I will look for answers.

1 Like

No prob. I’m good with whatever the AP team decides.

@khancyr I’ve posted a PR - it’s definitely WIP but operational. I’ve tested it on a rover model and have SITL up and running with MAVProxy and MissionPlanner. The performance is terrible because of timeouts etc. so fixing that’s the next step - but you can now edit if you wish.

Update 1: profiling of jsoncpp (doesn’t look good… 64% of update time generating json. I’m sure we can do better).

Update 2: replacing jsoncpp with rapidjson

Replacing jsoncpp with rapidjson with some adjustment of the timeout conditions results in a simulation that runs at say 85% - 90% of the previous interface. Further work should get it back to parity, but at the moment the plugin is usable.

Update 3: Profiling of gzserver and ardurover

More detailed analysis of the differences between SITL/SIM_Gazebo and SITL/SIM_JSON.

Baseline: ArduPilotPlugin + SITL/SIM_Gazebo

Gazebo plugin profile:

ArduPilot SITL profile:

Change: ArduPilotPlugin + SITL/SIM_JSON

Gazebo plugin profile:

ArduPilot SITL profile:

Summary:

The main differences as expected arise from the JSON generation and parsing.

On the Gazebo plugin side we could hand roll the JSON string as the message is small and fairly simple, but this would might be fragile and prone to error as more sensors are added.

On the SITL/SIM_JSON side the bulk of the difference seems to arise in the JSON parsing. There may be a benefit in replacing the hand rolled parsing code with a JSON parser, this would certainly be easier to maintain as the JSON schema gets more complicated as additional sensors are added. What is the AP team’s view on adding dependencies to third-party libraries in non-FCU code?

Update 4: reduced socket.recv timeout

With some further tuning of the timeout settings in the plugin ReceiveCommand() function, the new SITL/SIM_JSON interface is now running my land yacht simulation at a comparable Real Time Factor and FPS to the original plugin.

Stephen, did you have a particular Gazebo sensor(s) that you wanted added? I’d be happy to add another sensor to prove out the approach.

Airspeed, if you haven’t already.

A rangefinder would be awesome too.

I’ve developed an anemometer plugin which should be good for airspeed too. I’ll take a shot at getting it integrated. Range and scanner should be a matter of plumbing through data from existing Gazebo sensors.

Saw your post on the discord canberra-uav channel - not sure if tridge’s follow up was for Gazebo but put this together as a bit of fun (and it uncovered an issue with a change in the interface so turned out to be useful for testing). It’s running on Ashvath’s GSoC Lua script in SITL

First draft of sonar rangefinders in ardupilot_gazebo. SITL_JSON currently supports up to 6, there’s no limit on the Gazebo side.

The demo uses a model from this repo:

which is configured with 8 sensors. The car is an Ackermann steering rover with a Lua script mixer. There are other rover models in the same repo using the JSON interface (Quadruped, Sawppy, TurtleBot).

I see that airspeed has just been merged into SITL_JSON ardupilot master so I’ll take a look at that next.

Hi, I’m adding 2 lidars to my quadplane in gazebo/sitl/ros. The work you have done is impressive but It’s not clear to me how I can integrate your work with my setup. What is the most relevant video/ page to visit to help solve this issue?

Hi @Pyotyr_Young, the comments above refer to simple proximity / range sensors rather than lidar so wouldn’t be directly applicable to the problem you’re looking to solve.

If you are already using ROS then one approach might be to use mavros to forward the data from Gazebo. There is a discussion here of the approach Gazebo with LiDAR. I’ve yet not attempted this myself so can’t advise on tips and tricks to get it all working.

Update

@Pyotyr_Young I’ve been looking into getting Ignition to work with ROS2, mavros (ros2) and ArduPilot SITL, and it’s finally in a state where I can offer a few pointers for your quadplane.

The example shown above illustrates a simple skid steer rover equipped with a 360 deg laser scanner controlled by ArduPilot SITL.

The code is available here: GitHub - srmainwaring/ros_ign_rover at feature/mavros2. A word of warning - it is all ROS2, and much of it experimental and targeting macOS rather than linux.

  • The ROS environment is ros2 galactic
  • The model is defined in URDF and spawned into Ignition Gazebo using the ros_ign_gazebo node
  • Pose, odometry and sensor data in Ignition is made available to ROS2 using the ros_ign bridge.
  • Laser scan data is forwarded to SITL by mavros
  • Vehicle control is via ArduPilot/ardupilot_gazebo

The basic set up is that you configure the mavros_extras obstacle plugin to forward sensor_msgs/msg/LaserScan data to mavlink as an OBSTACLE_DISTANCE message. Your vehicle must set PRX_TYPE=2 (mavlink). SITL appears to convert this internally to a DISTANCE_SENSOR message for the 8 sectors required by the avoidance code.

The main outstanding issue with the example is the frame transformation for the lidar data. There does not appear to be any transform carried out in the mavros node responsible for forwarding range data via mavlink (so the proximity graph is showing the obstruction on the wrong side of the rover as the body frame conventions in ArduPilot and Gazebo differ by 180 deg about x = forward). I’ll need to add a filter node to flip the scan data about before passing to mavros.

1 Like