Revision history [back]

Hi!

I am trying to spawn a model from my world plugin, and that model uses its own plugin. It seems to work fine, however, when I wait for the model to subscribe to my topic, it hangs Gazebo and the model plugin ends up never being loaded... I am trying to use a callback to detect when the model is spawned, but it is called before the plugin actually loads, so it is sort of useless. I have also tried running WaitForConnection() in a thread, but it still hangs Gazebo.... Is there a proper way to do this?

The world plugin:

  namespace gazebo
    {
    RLControl::RLControl(): WorldPlugin()
    {
        ROS_INFO("[*] Created RLControl plugin");
    }

    RLControl::~RLControl() {}

    void RLControl::Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
    {
        // Make sure the ROS node for Gazebo has already been initialized                                                                                    
        if (!ros::isInitialized())
        {
            ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                    << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
            return;
        }

                ROS_INFO("[*] Loading RLControl plugin...");
        this->world = _parent;
        this->node = transport::NodePtr(new transport::Node());
        this->node->Init(this->world->GetName());
        transport::run();
        this->pubVelocity = this->node->Advertise<msgs::Vector3d>("~/aero/velocity");
        this->SpawnDrone(ignition::math::Vector3d(0, 0, 0));
        //std::string topicName = "~/intel_aero/crashed";
        //ROS_INFO("[*] Subscribing to topic ");
        //this->subCrashed = this->node->Subscribe(topicName, &RLControl::SetCrashed, this);
    }

    void RLControl::Train()
    {
        ROS_INFO("[*] Starting RL training (in a thread)...");
        ROS_INFO("[*] Waiting for connection on ~/aero/velocity");
        this->pubVelocity->WaitForConnection();
        this->isCrashed = false;
        msgs::Vector3d velMsg;
        msgs::Set(&velMsg, ignition::math::Vector3d(.9, 0, .5));
        while (true) {
            ROS_INFO("[*] Publishing velocity...");
            this->pubVelocity->Publish(velMsg);
            common::Time::MSleep(1500);
        }
        ROS_INFO("[*] End of training session");
    }

    void RLControl::SpawnDrone(const ignition::math::Vector3d location)
    {
        ROS_INFO("[*] Spawning Intel Aero");
        this->subSpawned = this->node->Subscribe("~/model/info", &RLControl::ModelSpawned, this);
        this->world->SetPaused(true);
        // The filename must be in the GAZEBO_MODEL_PATH environment variable.
        //this->world->InsertModelFile("model://aero_control/models/aero");
        this->world->InsertModelFile("model://aero_control/models/aero_working_standalone");
    }

    void RLControl::ModelSpawned(ConstModelPtr &msg)
    {
        ROS_INFO("[*] Model spawned. Unpausing simulation.");
        this->world->SetPaused(false);
        std::thread training(&RLControl::Train, this);
        ROS_INFO("[*] Training thread running");
        training.join();
    }
}

And the model plugin:

namespace gazebo
{
    AeroControl::AeroControl(): ModelPlugin()
    {
        ROS_INFO("[AERO] Created AeroControl plugin");
    }

    void AeroControl::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        // Make sure the ROS node for Gazebo has already been initialized
        if (!ros::isInitialized())
        {
            ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
                    << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
            return;
        }

        ROS_INFO("[AERO] Loading model ");
        this->model = _parent;
        this->updateConnection = event::Events::ConnectWorldUpdateBegin(
                std::bind(&AeroControl::OnUpdate, this));
        this->node = transport::NodePtr(new transport::Node());
        this->node->Init(this->model->GetWorld()->GetName());
        transport::run();
        std::string topicName = "~/" + this->model->GetName() + "/velocity";
        ROS_INFO("[AERO] Subscribing to topic ");
        std::cout << topicName << std::endl;
        this->subVelocity = this->node->Subscribe(topicName, &AeroControl::SetVelocity, this);
    }

    void AeroControl::OnUpdate()
    {
        // Example: apply a small linear velocity
        //this->model->SetLinearVel(ignition::math::Vector3d(.3, 0, 0));
        //TODO: Publish to crashed
    }

    void AeroControl::SetVelocity(ConstVector3dPtr &msg)
    {
        ROS_INFO("[AERO] Got velocity command");
        this->model->SetLinearVel(msgs::ConvertIgn(*msg));
    }

    bool AeroControl::IsCrashed()
    {
        // TODO
        return false;
    }
}