delete_model freezing issue

asked 2018-01-03 16:11:45 -0500

swordsgnat gravatar image

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 ...
(more)
edit retag flag offensive close merge delete