Robotics StackExchange | Archived questions

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

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!

Asked by gazebo_learner on 2017-11-20 19:37:11 UTC

Comments

Answers