Gazebo | Ignition | Community
Ask Your Question
0

Wait for model plugin to load in world plugin

asked 2019-04-19 07:56:36 -0600

theo gravatar image

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

1 Answer

Sort by ยป oldest newest most voted
0

answered 2020-08-10 04:26:33 -0600

A bit late so it, unfortunately, might not help you anymore but I think the problem you're facing is that you're pausing the simulation which also pauses ROS time and ROS spinning (delivery of messages to callbacks etc.).

In short, everything is paused so your callback will not be called unless you unpause the simulation manually.

edit flag offensive delete link more
Login/Signup to Answer

Question Tools

1 follower

Stats

Asked: 2019-04-19 07:56:36 -0600

Seen: 728 times

Last updated: Apr 19 '19