disable_link_plugin.cpp

Go to the documentation of this file.

#include <roboticsgroup_upatras_gazebo_plugins/disable_link_plugin.h>

namespace gazebo {

    DisableLinkPlugin::DisableLinkPlugin()
    {
        link_.reset();
    }

    DisableLinkPlugin::~DisableLinkPlugin()
    {
    }

    void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf)
    {
        model_ = _parent;
        world_ = model_->GetWorld();

        // Check for link element
        if (!_sdf->HasElement("link")) {
            ROS_ERROR("No link element present. DisableLinkPlugin could not be loaded.");
            return;
        }

        link_name_ = _sdf->GetElement("link")->Get<std::string>();

        // Get pointers to joints
        link_ = model_->GetLink(link_name_);
        if (link_) {
            link_->SetEnabled(false);
            // Output some confirmation
            ROS_INFO_STREAM("DisableLinkPlugin loaded! Link: " << link_name_);
        }
        else
            ROS_ERROR_STREAM("Link " << link_name_ << " not found! DisableLinkPlugin could not be loaded.");
    }

    GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin);

}  // namespace gazebo