Servers by jDrones

Add more sensors to Gazebo - SITL

@khancyr I’ve been using Gazebo to test the sailing vehicle code developed by @iampete. The current API has been great for the first round of tests and proved invaluable in getting a wing sail land yacht tuned and running.

For next steps I’d like to feed a wider range of sensor data back to SITL from Gazebo. My immediate requirement is for wind sensor data, but GPS, range finders and joint positions (wheel odometry) would be good to have too.

I’ve made a start here:

and here:

which gets the vehicle apparent wind data into SITL (I’ve bundled the plugins for the sensor in so the example is self contained).

I think a better approach might be to use the JSON interface developed by Peter to replace the current binary packet. That should simplify the release process as currently the plugin and the ardupilot code need to be released in lockstep which I imagine is tricky.

I’m happy to carry on and put this together but would appreciate input from you and Peter as you authored the Gazebo / JSON components. I’m keen to roll the changes out so that I can share some of the models and simulation examples I’ve been developing for rovers and sailing vehicles.

hello,

The JSON road is definitively the right path ! That is why I didn’t push anything on the repo for long time … but I never get the time to get back at it as it would take some time to get it right.

I would be glad to assist you on this.

1 Like

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.

Servers by jDrones