Gazebo aborts with boost::lock_error when adding a model with a GPU ray sensor
I tried to add a GPU ray sensor to our Hector robot model using ROS hydro and Gazebo 1.8.6 from the ROS package repositories.
This is the SDF description of the sensor:
<sensor name="laser1" type="gpu_ray">
<always_on>true</always_on>
<update_rate>40</update_rate>
<pose>0 0 0 0 0 0</pose>
<visualize>false</visualize>
<ray>
<scan>
<horizontal>
<samples>1081</samples>
<resolution>1</resolution>
<min_angle>135</min_angle>
<max_angle>-135</max_angle>
</horizontal>
</scan>
<range>
<min>0.08</min>
<max>30.0</max>
<resolution>0.01</resolution>
</range>
</ray>
<plugin filename="libgazebo_ros_gpu_laser.so" name="gazebo_ros_laser1_controller">
<gaussianNoise>0.005</gaussianNoise>
<alwaysOn>true</alwaysOn>
<updateRate>40</updateRate>
<topicName>laser1/scan</topicName>
<frameName>laser1_frame</frameName>
</plugin>
</sensor>
When the model is loaded, gazebo crashes with the following error message:
gzserver: /usr/include/boost/thread/pthread/mutex.hpp:61: void boost::mutex::unlock(): Assertion `!pthread_mutex_unlock(&m)' failed.
Aborted (core dumped)
I also tried to add the sensor without the plugin and even without the parameters, but this does not help either. I then compiled gazebo (branch default) and gazebo_ros_pkgs (branch hydro-devel) from source with debug information to be able to create a stack trace. The lock_error seems to happen somewhere in the callback queue of the gazebo_ros_api_plugin:
#0 0x00007f94e6066425 in __GI_raise (sig=<optimized out>) at ../nptl/sysdeps/unix/sysv/linux/raise.c:64
#1 0x00007f94e6069b8b in __GI_abort () at abort.c:91
#2 0x00007f94e66bc69d in __gnu_cxx::__verbose_terminate_handler() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#3 0x00007f94e66ba846 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#4 0x00007f94e66ba873 in std::terminate() () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#5 0x00007f94e66ba96e in __cxa_throw () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#6 0x0000000000475c7a in boost::throw_exception<boost::lock_error> (e=...) at /usr/include/boost/throw_exception.hpp:61
#7 0x00007f94d14c4538 in ros::CallbackQueue::callAvailable(ros::WallDuration) () from /opt/ros/hydro/lib/libroscpp.so
#8 0x00007f94d1931e33 in gazebo::GazeboRosApiPlugin::gazeboQueueThread (this=0x268bce0) at /home/meyer/ros/hydro/gazebo/src/gazebo_ros_pkgs/gazebo_ros/src/gazebo_ros_api_plugin.cpp:205
#9 0x00007f94d19a5c80 in boost::_mfi::mf0<void, gazebo::GazeboRosApiPlugin>::operator() (this=0x27c4328, p=0x268bce0) at /usr/include/boost/bind/mem_fn_template.hpp:49
#10 0x00007f94d19a3708 in boost::_bi::list1<boost::_bi::value<gazebo::GazeboRosApiPlugin*> >::operator()<boost::_mfi::mf0<void, gazebo::GazeboRosApiPlugin>, boost::_bi::list0> (this=0x27c4338, f=..., a=...)
at /usr/include/boost/bind/bind.hpp:253
#11 0x00007f94d1998d89 in boost::_bi::bind_t<void, boost::_mfi::mf0<void, gazebo::GazeboRosApiPlugin>, boost::_bi::list1<boost::_bi::value<gazebo::GazeboRosApiPlugin*> > >::operator() (this=0x27c4328)
at /usr/include/boost/bind/bind_template.hpp:20
#12 0x00007f94d19bf3ba in boost::detail::thread_data<boost::_bi::bind_t<void, boost::_mfi::mf0<void, gazebo::GazeboRosApiPlugin>, boost::_bi::list1<boost::_bi::value<gazebo::GazeboRosApiPlugin*> > > >::run (this=0x27c41a0)
at /usr/include/boost/thread/detail/thread.hpp:61
#13 0x00007f94e6d7ace9 in thread_proxy () from /usr/lib/libboost_thread.so.1.46.1
#14 0x00007f94e7909e9a in start_thread (arg=0x7f94a7fff700) at pthread_create.c:308
#15 0x00007f94e6123ccd in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:112
#16 0x0000000000000000 in ?? ()
I am not sure how this is related to the gpu_ray sensor, but with a standard ray ...
Could you post the backtrace output from all the threads: (gdb) thread apply all bt. The performance of gazebo 1.8.6 is greatly improved. Depending on the world and sensors, it'll run near or faster than real-time.
Unfortunately I cannot reproduce the error at the moment because of issue 748. But I found out that the error is caused by a gzthrow in the Camera.cc file as minangle > maxangle in my config. I copied this over from the normal ray sensor we used in Gazebo 1.5 and it worked fine there. The error message is never printed and the lock error occurs if the server calls the plugins' deconstructors afterwards while the gazeboQueueThread is still running.