Added DisableLink Model Plugin

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 65ecc51..4cef938 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -17,6 +17,9 @@ include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} ${GAZEBO_INCLUDE
 add_library(gazebo_mimic_joint_plugin src/mimic_joint_plugin.cpp)
 target_link_libraries(gazebo_mimic_joint_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
 
+add_library(gazebo_disable_link_plugin src/disable_link_plugin.cpp)
+target_link_libraries(gazebo_disable_link_plugin ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
+
 catkin_package(
   DEPENDS 
     roscpp 

diff --git a/README.md b/README.md
index 9afb7bd..7d0eb32 100644 (file)
--- a/README.md
+++ b/README.md
@@ -24,9 +24,27 @@ A simple (Model) plugin for Gazebo in order to add to Gazebo the mimic joint fun
 
       A **double** specifying the offset parameter of the mimic joint. Defaults to 0.0.
 
+###DisableLinkPlugin
+
+A simple (Model) plugin for Gazebo that allows you to disable a link in Gazebo's physics engine.
+
+  - *XML Parameters*
+
+    - link (Required)
+
+      A **string** specifying the name of the link to be disable. It should be a valid sdf (not urdf) link.
 
 ###Hoping to add more plugins....
 
+Usage
+------
+
+Standard Gazebo plugin import inside xacro/urdf. Use **libgazebo_** prefix. E.g. if you want to import MimicJointPlugin:
+
+```
+libgazebo_mimic_joint_plugin.so
+```
+
 Notes
 ------
 

diff --git a/include/gazebo_plugins/disable_link_plugin.h b/include/gazebo_plugins/disable_link_plugin.h
new file mode 100644 (file)
index 0000000..6a79b7a
--- /dev/null
+++ b/include/gazebo_plugins/disable_link_plugin.h
@@ -0,0 +1,70 @@
+/**
+Copyright (c) 2014, Konstantinos Chatzilygeroudis
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 
+    in the documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 
+    from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 
+BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 
+OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**/
+
+#ifndef GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN
+#define GAZEBO_PLUGINS_DISABLE_LINK_PLUGIN
+
+// ROS includes
+#include <ros/ros.h>
+
+// Boost includes
+#include <boost/bind.hpp>
+
+// Gazebo includes
+#include <gazebo/common/Plugin.hh>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+
+
+using std::string;
+
+namespace gazebo
+{
+  class DisableLinkPlugin : public ModelPlugin
+  {
+    public:
+      DisableLinkPlugin();
+      ~DisableLinkPlugin();
+
+      void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+      void UpdateChild();
+
+    private:
+      // Parameters
+      string link_name_;
+
+      bool kill_sim;
+
+      // Pointers to the joints
+      physics::LinkPtr link_;
+
+      // Pointer to the model
+      physics::ModelPtr model_;
+
+      // Pointer to the world
+      physics::WorldPtr world_;
+
+  };
+}
+
+#endif
\ No newline at end of file

diff --git a/src/disable_link_plugin.cpp b/src/disable_link_plugin.cpp
new file mode 100644 (file)
index 0000000..1b2785d
--- /dev/null
+++ b/src/disable_link_plugin.cpp
@@ -0,0 +1,61 @@
+/**
+Copyright (c) 2014, Konstantinos Chatzilygeroudis
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer 
+    in the documentation and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived 
+    from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 
+BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 
+OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+**/
+
+#include <gazebo_plugins/disable_link_plugin.h>
+
+using namespace gazebo;
+
+DisableLinkPlugin::DisableLinkPlugin()
+{
+  kill_sim = false;
+
+  link_.reset();
+}
+
+DisableLinkPlugin::~DisableLinkPlugin()
+{
+  kill_sim = true;
+}
+
+void DisableLinkPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf )
+{
+  this->model_ = _parent;
+  this->world_ = this->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);
+  else
+    ROS_WARN("Link %s not found!", link_name_.c_str());
+}
+
+GZ_REGISTER_MODEL_PLUGIN(DisableLinkPlugin);
\ No newline at end of file