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:
std_srvs::Empty::Request unPauseReq;
std_srvs::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
↧