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