delete_model freezing issue
I'm trying to animate a complex surface in Gazebo as part of a simulation. I have a folder of .stp files representing each successive "frame" of the surface's movement. I have a script written that creates a node that runs code every time /clock
is updated. Every time my desired timestep has passed, I pause the simulation (with the pause_physics
service), delete the old frame of the surface (using the delete_model
service) and then I spawn the new frame of the surface (using the spawn_model
service) before unpausing the simulation (with unpause_physics
).
When I run my simulation headless (GUI-disabled) I get no problems - I can run through all 100+ frames of my animation no problem. When I open up a gzclient, though, it can't make it past frame #17 or so. It registers that it's reached a new timestep and prints that it successfully deleted the old frame, but then stops there and doesn't proceed no matter how long I leave it. There's no error generated - it just hangs up.
Here're the relevant parts of the code:
pausing time:
std_srvs::Empty::Request pauseReq;
std_srvs::Empty::Response pauseResp;
ros::service::waitForService("/gazebo/pause_physics");
bool pauseSuccess = pause_run.call(pauseReq, pauseResp);
if(pauseSuccess)
{
//ROS_INFO_STREAM("Time Pause Success\n");
}
else
{
//ROS_ERROR_STREAM("TIME PAUSE FAILURE\n");
}
Deleting the old surface:
gazebo_msgs::DeleteModel::Request delReq;
gazebo_msgs::DeleteModel::Response delResp;
std::stringstream delNameStream;
delNameStream << "field" << (field_mirror_target_timestep - field_timestep_ratio);
delReq.model_name = delNameStream.str();
ros::service::waitForService("/gazebo/delete_model");
bool delSuccess = delete_old_field.call(delReq, delResp);
if(delSuccess)
{
std::stringstream delStrmG;
delStrmG << "Delete old field successful for: " << delNameStream.str() << "/n"
ROS_INFO_STREAM(delStrmG.str());
}
else
{
std::stringstream delStrmB;
delStrmB << "Delete old field FAILURE for: " << delNameStream.str() << "/n"
ROS_ERROR_STREAM(delStrmB.str());
}
Spawning the new surface:
gazebo_msgs::SpawnModel::Request spawnReq;
gazebo_msgs::SpawnModel::Response spawnResp;
std::stringstream nameStream;
nameStream << "field" << field_mirror_target_timestep;
spawnReq.model_name = nameStream.str();
std::ifstream partOne("/home/nate/.gazebo/models/fields/sdfpt1");
std::ifstream partTwo("/home/nate/.gazebo/models/fields/sdfpt2");
std::ifstream partThree("/home/nate/.gazebo/models/fields/sdfpt3");
std::stringstream sdfStream;
sdfStream << partOne.rdbuf();
if(field_mirror_target_timestep < 100) //i.e., a two-digit number
{
sdfStream << "00";
}
else if(field_mirror_target_timestep < 1000) //i.e., a three-digit number
{
sdfStream << "0";
}
sdfStream << field_mirror_target_timestep << partTwo.rdbuf();
if(field_mirror_target_timestep < 100) //i.e., a two-digit number
{
sdfStream << "00";
}
else if(field_mirror_target_timestep < 1000) //i.e., a three-digit number
{
sdfStream << "0";
}
sdfStream << field_mirror_target_timestep << partThree.rdbuf();
spawnReq.model_xml = sdfStream.str();
spawnReq.robot_namespace = "field";
spawnReq.initial_pose.position.x = 0;
spawnReq.initial_pose.position.y = 0;
spawnReq.initial_pose.position.z = 0;
spawnReq.initial_pose.orientation.x = 0;
spawnReq.initial_pose.orientation.y = 0;
spawnReq.initial_pose.orientation.z = 0;
spawnReq.initial_pose.orientation.w = 1; // equivalent of zero in quaternion
spawnReq.reference_frame = "world";
ros::service::waitForService("/gazebo/spawn_sdf_model");
bool spawnSuccess = spawn_new_field.call(spawnReq, spawnResp);
if(spawnSuccess)
{
std::stringstream spawnStrmG;
spawnStrmG << "" Spawn new field successful for: " << nameStream.str() << "\n";
ROS_INFO_STREAM(spawnStrmG.str());
}
else
{
std::stringstream spawnStrmB;
spawnStrmB << "" Spawn new field FAIL for: " << nameStream.str() << "\n";
ROS_ERROR_STREAM(spawnStrmB.str());
}
ROS_INFO_STREAM(spawnResp.status_message);
}
}
Unpausing Time:
stdsrvs::Empty::Request unPauseReq; stdsrvs::Empty::Response unPauseResp;
ros::service::waitForService("/gazebo/unpause_physics");
bool unPauseSuccess = unpause_run.call(unPauseReq, unPauseResp);
if(unPauseSuccess)
{
//ROS_INFO_STREAM("Time Unpause Success\n");
}
else
{
//ROS_ERROR_STREAM("TIME UNPAUSE FAILURE\n");
}
Does anyone know what might be going on here? It's not directly repeatable - it always gets hung up when the GUI is active, but it ranges between frames like 10 and 20. Again, with the GUI inactive and the exact same code it makes it to the last frames (over 100).
Thanks in advance,
Nate
Asked by swordsgnat on 2018-01-03 17:11:45 UTC
Comments