Robotics StackExchange | Archived questions

why does RemoveSensor() on sonar sensor during Load() cause a crash?

Hi,

I'm writing a ModelPlugin to run alongside a model specified in SDF (Gazebo 7.0). The SDF includes a snippet for a sonar sensor and a camera sensor, and I want to delete them in the plugin (optionally) to eliminate their CPU cost. Calling SetActive(false) on them disables them, but they still use CPU.

In ModelPlugin::Load(), I can call SensorManager::RemoveSensor() on the camera sensor, and everything works as expected - the camera goes away, and the CPU usage is reduced. I want to do the same to the sonar sensor, but if I call SensorManager::RemoveSensor() on that one, I get a crash shortly afterwards that says:

gzserver: /usr/include/boost/smart_ptr/shared_ptr.hpp:648: typename boost::detail::sp_member_access<T>::type boost::shared_ptr<T>::operator->() const [with T = gazebo::physics::SurfaceParams; typename boost::detail::sp_member_access<T>::type = gazebo::physics::SurfaceParams*]: Assertion 'px != 0' failed.

I am guessing this is happening because before the Load() call on my plugin, the sensor has registered itself with the physics engine in some way - the camera presumably does not need to do this. I've reviewed the source code for SonarSensor and see that there is a connect/disconnect function, but I can't see where I could recover the token in order to call disconnect() on it, even if that isn't barking up the wrong tree.

So - can anyone please tell me either why this crashes, or - more directly - what do I have to do to shut down the SonarSensor gracefully and allow me to RemoveSensor() it?

Thanks, ben

Asked by benjmitch on 2018-10-31 11:49:19 UTC

Comments

How did you measure the CPU usage when they are disabled: SetActive(false)? The SetActive(false) function should eliminate most or all of a sensor's processing.

Asked by nkoenig on 2018-10-31 12:25:12 UTC

Just monitoring externally using top/atop. Tried it just this minute - CPU @ 70% with nine inactive sonars; @ 35% when they are not present in the SDF. Sonar is implemented using contact detection, right? So perhaps this is still running in the background, which would explain the CPU use and also why I can't safely Remove() them... guess guess?

Asked by benjmitch on 2018-10-31 12:36:05 UTC

21 cameras gives me 130% CPU enabled, 98% CPU SetActive(false), 72% CPU RemoveSensor()'d. Similar pattern, except that I /can/ safely do RemoveSensor(), that works just fine.

Asked by benjmitch on 2018-10-31 12:42:57 UTC

Is it possible to post or link to your code and SDF file(s)?

Asked by nkoenig on 2018-10-31 15:10:02 UTC

Here's a link to a minimal example: run make in this folder to generate the error https://we.tl/t-G2YMhZx52k

Asked by benjmitch on 2018-10-31 15:10:46 UTC

Posted new link, old one will expire soon [ https://we.tl/t-bJaqf24w5f ]

Asked by benjmitch on 2018-11-06 06:25:40 UTC

Answers

Thanks for the link to your code. One problem I noticed is the plugin (libgaz.cpp) iterates over all robots during load. This is a problem because the libgaz plugin is loaded by each robot instance. Since libgaz is a model plugin, you should only touch elements that belong to the model. This means you should remove the for loop that looks like:

//  for each robot
//for (char robot='a'; robot<'k'; robot++)
{

You can then get a sensor using:

  //  get sensor
  stringstream ss_name;
  ss_name << "default::" << _parent->GetScopedName() << "::main_link::cam" << i;
  sensor = sensors::SensorManager::Instance()->GetSensor(ss_name.str());

And finally, remove a sensor using:

    sensors::SensorManager::Instance()->RemoveSensor(sensor->GAZEBO_SCOPED_NAME());

I tried the above on Gazebo9, and it seemed to work.

Asked by nkoenig on 2018-11-06 11:06:53 UTC

Comments

Thanks. And sorry about that, built in a bit of a rush. Apologies, whilst this still crashes in v7.0.0, it does not in v7.14.0. Sorry, before today I didn't appreciate that the main Ubuntu repo is not up to date with bug fixes - your response led me to investigate and I'm now on 7.14. I'll go back to my original code and see if 7.14 also eliminates the unexpected CPU use and report back.

Asked by benjmitch on 2018-11-06 12:56:05 UTC

Spoke too soon - original error still there using original code. A minor adjustment to the SDF causes the minimal test to crash (see README). In addition, minimal test on either v7.14.0 or v9.0.0 (a) displays an (incorrect) visualisation of sonar sensors that have been RemoveSensor()d, (b) at exit generates multiple errors of the form "Unable to remove sensor[...sonar1] because it does not exist". Suggesting I haven't cleanly removed them. Do you see the same? [ https://we.tl/t-nkMxxxqMmy ]

Asked by benjmitch on 2018-11-06 14:49:38 UTC

Yes, there is a problem with the visualization system when a sonar sensor (probably any sensor) is removed during load. However, the sonar is actually removed in simulation (at least no data is produced). On exit, the Link object which held the sensor tries to also remove the sensor. This causes those error messages, because you have already removed the sensor. You can ignore those error messages.

Asked by nkoenig on 2018-11-07 10:36:03 UTC

Thanks, that explains those two things. However, it still crashes when using RemoveSensor() in the minimal example, if you make the small change to the SDF indicated in the README. Did you try that out?

Asked by benjmitch on 2018-11-07 11:14:24 UTC

I haven't tried the new crash. Maybe you should submit an issue for this crash on https://bitbucket.org/osrf/gazebo/issues/new

Asked by nkoenig on 2018-11-09 15:04:11 UTC

Thanks, I have done so. I will come back and answer this question if the issue is confirmed.

Asked by benjmitch on 2018-11-12 19:10:58 UTC