delete_model() hangs for several mins after repeated additions/deletions of a sdf model (which sometimes entirely vanishes from the scene too) in Gazebo
I've been facing issues with repeatedly adding and deleting the same model into gazebo (for a repetitive training task). The model is that of a simple cylindrical object (a pole) with no joints ... just one link. After a few (~5-10) successful additions/deletions of this model into the scene, subsequent deletemodel() calls start getting stuck for a few minutes each. It hangs even though I can clearly see the model exists in the scene. At this point, the only way to "release" the stuck call is to manually delete the model through the gazebo UI. But, then it keeps getting stuck the same way on all subsequent calls to deletemodel().
Furthermore, if I try to invoke any other service call while the delete_model() call is held up, those calls also sit blocked, seemingly waiting for the previous invocation to return.
In addition, the object I'm adding to the scene sometimes just disappears from the scene as well, though I'm not sure whether the issues share a common cause. Anyway, thought I'd mention that too, just in case. Maybe it's the SDF of the object that isn't correctly setup?
When the delete_model() call hangs, I have consistently seen the following error message on the shell:
[Err] [Joint.cc:712] Joint axis index [0] larger than DOF count [0].
... but I can't quite figure out what it means.
The SDF of the object I'm adding/deleting is below:
<sdf
<a="" href="http://
xmlns:xacro="http://ros.org/wiki/xacro%22" version="1.6">
xmlns:xacro="http://ros.org/wiki/xacro">
<model name="pole">
<link name="pole_link"/>
<inertial>
<mass>2.14885</mass>
<inertia>
<ixx>0.0287722</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0287722</iyy>
<iyz>0</iyz>
<izz>0.00024175</izz>
</inertia>
<pose frame="">0 0 0 0 -0 0</pose>
</inertial>
<pose frame="">0 0 0 0 -0 0</pose>
<gravity>1</gravity>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
<enable_wind>0</enable_wind>
<visual name="visual">
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.015</radius>
<length>0.5</length>
</cylinder>
</geometry>
<material>
<lighting>1</lighting>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Red</name>
</script>
<shader type="pixel">
<normal_map>__default__</normal_map>
</shader>
<ambient>0.3 0.3 0.3 1</ambient>
<diffuse>0.7 0.7 0.7 1</diffuse>
<specular>0.01 0.01 0.01 1</specular>
<emissive>0 0 0 1</emissive>
</material>
<transparency>0</transparency>
<cast_shadows>1</cast_shadows>
</visual>
<collision name="collision">
<laser_retro>0</laser_retro>
<max_contacts>10</max_contacts>
<pose frame="">0 0 0 0 -0 0</pose>
<geometry>
<cylinder>
<radius>0.015</radius>
<length>0.5</length>
</cylinder>
</geometry>
<surface>
<friction>
<ode>
<mu>1</mu>
<mu2>1</mu2>
<fdir1>0 0 0</fdir1>
<slip1>0</slip1>
<slip2>0</slip2>
</ode>
<torsional>
<coefficient>1</coefficient>
<patch_radius>0</patch_radius>
<surface_radius>0</surface_radius>
<use_patch_radius>1</use_patch_radius>
<ode>
<slip>0</slip>
</ode>
</torsional>
</friction>
<bounce>
<restitution_coefficient>0</restitution_coefficient>
<threshold>1e+06</threshold>
</bounce>
<contact>
<collide_without_contact>0</collide_without_contact>
<collide_without_contact_bitmask>1</collide_without_contact_bitmask>
<collide_bitmask>1</collide_bitmask>
<ode>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
<max_vel>0.01</max_vel>
<min_depth>0</min_depth>
</ode>
<bullet>
<split_impulse>1</split_impulse>
<split_impulse_penetration_threshold>-0.01</split_impulse_penetration_threshold>
<soft_cfm>0</soft_cfm>
<soft_erp>0.2</soft_erp>
<kp>1e+13</kp>
<kd>1</kd>
</bullet>
</contact>
</surface>
</collision>
<sensor name="pole_imu_sensor" type="imu">
<always_on>true</always_on>
<update_rate>15</update_rate>
<visualize>true</visualize>
<topic>__default_topic__</topic>
<plugin filename="libgazebo_ros_imu_sensor.so" name="pole_imu_plugin">
<topicname>pole_link_imu_sensor</topicname>
<bodyname>pole_link</bodyname>
<updateratehz>10.0</updateratehz>
<gaussiannoise>0.0</gaussiannoise>
<xyzoffset>0 0 0</xyzoffset>
<rpyoffset>0 0 0</rpyoffset>
<framename>pole_link</framename>
</plugin>
<pose>0 0 0 0 0 0</pose>
</sensor>
<static>0</static>
<allow_auto_disable>1</allow_auto_disable>
</model>
</sdf>
Environment:
- ROS Melodic (to launch gazebo and other ROS components)
- Gazebo 9
- ROS python client (rospy) to invoke the gazebo services
- Ubuntu 18.04
What am I doing wrong above? Would anyone have some clear instructions for a fix (or reasonable workaround) that a Gazebo newbie could follow?
Thanks in advance.
Asked by alikureishy on 2020-04-30 06:16:11 UTC
Answers
I investigated this and I tracked the problem down to a publisher. I have opened an issue.
Unfortunately, once the problem shows up, the Gazebo-ROS API is rendered useless. You cannot spawn or delete models through the relevant services anymore.
You can use the gz
tool though as a workaround.
To delete a model, run: gz model -m model_0 -d
To spawn a model, run: gz model -m model_0 -f <path_to_model>.sdf
For more options, run: gz help model
Asked by nlamprian on 2020-05-02 10:52:34 UTC
Comments
I can confirm I have the same problem. I haven't looked into it yet, so I cannot offer any advice, but I can add some more information. For me, the service gets stuck randomly the first time I call it. And, it does so more when I run gazebo with the GUI than when I run it headless. So this has been a problem for me during development, but luckily enough, it hasn't been much of a problem when I use gazebo headless remotely for automated testing.
Asked by nlamprian on 2020-04-30 12:17:43 UTC
Thanks for the note, @nlamprian. Unfortunately for me, I need to run it with the GUI at the moment. Hopefully someone in the community here can offer some advice about what to do.
Asked by alikureishy on 2020-05-01 02:16:35 UTC