Warnings and strange behaviors while inserting and removing models in a world plugin

asked 2017-11-20 18:37:11 -0600

gazebo_learner gravatar image

I was trying to dynamically insert/remove/update models in the WorldUpdateBegin connection callback function in a world plugin. I wanted to play back a scene of multiple objects moving on a plane: when a certain object appears, I will add it to Gazebo; if it disappears, I want to remove it from Gazebo; otherwise I will update its pose.

Here's what I'm doing in the OnUpdate function (per each update iteration):

    // I got an updated object list from another piece of code
    std::map<std::string, Object> objs;
    // i want to update the Gazebo models based on the object poses
    physics::Model_V models = m_worldPtr->GetModels();
    for(unsigned int j=0; j<models.size(); j++) {
        physics::ModelPtr modelPtr = models[j];
        std::string model_name = modelPtr->GetName();
        // find this model in my object list
        std::map<std::string, Object>::iterator it = objs.find(model_name);
        if( it != objs.end()) {
            modelPtr->SetWorldPose(it->second.pose);
            objs.erase(it);
        } else {
            // I tried both versions but got the same warnings
            m_worldPtr->RemoveModel(modelPtr);
            //modelPtr->Fini();
        }
    }

    for(std::map<std::string, Object>::iterator it = objs.begin(); it != objs.end(); ++it) {
        std::string model_str = generateModelString(it->first, it->second.pose, it->second.size);
        m_worldPtr->InsertModelString(model_str);
    }

When I ran this code, I saw some models appearing at wrong timestamps and bump into others. I also saw the warning messages like this:

[Wrn] [IntrospectionManager.cc:115] Item [data://world/default/model/obj_012?p=vector3d/world_angular_acceleration] already registered
...
[Wrn] [IntrospectionManager.cc:134] Item [data://world/default/model/obj_012/link/link?p=pose3d/world_pose] is not registered

My guess of the problem is that model creation and removal take time, but how do I know if they are safely created or removed? Are there any locking mechanisms? Or are there any better ways of doing this?

Thank you very much!

edit retag flag offensive close merge delete