diff --git a/gazebo_ros_plugins/CMakeLists.txt b/gazebo_ros_plugins/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..4254a4daf5685d0ebb740a4fe7b3586e3a74b1bb
--- /dev/null
+++ b/gazebo_ros_plugins/CMakeLists.txt
@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_ros_plugins)
+find_package(catkin REQUIRED)
+catkin_metapackage()
diff --git a/gazebo_ros_plugins/package.xml b/gazebo_ros_plugins/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..0e22c731b82d0f6e4afdb9c9ad9959806a628790
--- /dev/null
+++ b/gazebo_ros_plugins/package.xml
@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+<package>
+ <name>gazebo_ros_plugins</name>
+ <version>0.0.0</version>
+ <description>The gazebo_ros_plugins package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="Suarez@todo.todo">Alejandro Suarez</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/gazebo_ros_plugins</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author >Alejandro Suarez</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+
+ <run_depend>gz_gripper_plugin</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <metapackage/>
+ </export>
+</package>
diff --git a/gz_gripper_plugin/CMakeLists.txt b/gz_gripper_plugin/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..dd72578376ffdb6f8a9e5ad7bf3e29f25be29f96
--- /dev/null
+++ b/gz_gripper_plugin/CMakeLists.txt
@@ -0,0 +1,195 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(gz_gripper_plugin)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+ actionlib
+ gazebo_plugins
+ gazebo_ros
+ iri_common_drivers_msgs
+ tf
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+find_package(gazebo REQUIRED)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+## * add a build_depend tag for "message_generation"
+## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+## but can be declared for certainty nonetheless:
+## * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+## * add "message_generation" and every package in MSG_DEP_SET to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * add "message_runtime" and every package in MSG_DEP_SET to
+## catkin_package(CATKIN_DEPENDS ...)
+## * uncomment the add_*_files sections below as needed
+## and list every .msg/.srv/.action file to be processed
+## * uncomment the generate_messages entry below
+## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+# FILES
+# Message1.msg
+# Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+# FILES
+# Service1.srv
+# Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+# FILES
+# Action1.action
+# Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+# DEPENDENCIES
+# iri_common_drivers_msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+## * add "dynamic_reconfigure" to
+## find_package(catkin REQUIRED COMPONENTS ...)
+## * uncomment the "generate_dynamic_reconfigure_options" section below
+## and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+# cfg/DynReconf1.cfg
+# cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+# INCLUDE_DIRS include
+ LIBRARIES gz_gripper_plugin
+# CATKIN_DEPENDS actionlib gazebo_plugins gazebo_ros iri_common_drivers_msgs tf
+# DEPENDS gazebo
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+#include_directories(include)
+
+# TODO: Check names of system library include directories (gazebo)
+include_directories(
+ ${catkin_INCLUDE_DIRS}
+ ${GAZEBO_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+add_library(gz_gripper_plugin
+ src/gz_gripper_plugin.cpp
+)
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(gz_gripper_plugin ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(gz_gripper_plugin_node src/gz_gripper_plugin_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(gz_gripper_plugin_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(gz_gripper_plugin
+ ${catkin_LIBRARIES}
+ ${GAZEBO_LIBRARIES}
+)
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+# scripts/my_python_script
+# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS gz_gripper_plugin gz_gripper_plugin_node
+# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+# FILES_MATCHING PATTERN "*.h"
+# PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+# # myfile1
+# # myfile2
+# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_gz_gripper_plugin.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)
diff --git a/gz_gripper_plugin/package.xml b/gz_gripper_plugin/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..da5a8764522d07a2ae53afe402a043c14672d660
--- /dev/null
+++ b/gz_gripper_plugin/package.xml
@@ -0,0 +1,63 @@
+<?xml version="1.0"?>
+<package>
+ <name>gz_gripper_plugin</name>
+ <version>0.0.0</version>
+ <description>The gz_gripper_plugin package</description>
+
+ <!-- One maintainer tag required, multiple allowed, one person per tag -->
+ <!-- Example: -->
+ <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+ <maintainer email="Suarez@todo.todo">Alejandro Suarez</maintainer>
+
+
+ <!-- One license tag required, multiple allowed, one license per tag -->
+ <!-- Commonly used license strings: -->
+ <!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+ <license>TODO</license>
+
+
+ <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+ <!-- Optional attribute type can be: website, bugtracker, or repository -->
+ <!-- Example: -->
+ <!-- <url type="website">http://wiki.ros.org/gz_gripper_plugin</url> -->
+
+
+ <!-- Author tags are optional, mutiple are allowed, one per tag -->
+ <!-- Authors do not have to be maintianers, but could be -->
+ <!-- Example: -->
+ <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+ <author >Alejandro Suarez</author>
+
+
+ <!-- The *_depend tags are used to specify dependencies -->
+ <!-- Dependencies can be catkin packages or system dependencies -->
+ <!-- Examples: -->
+ <!-- Use build_depend for packages you need at compile time: -->
+ <!-- <build_depend>message_generation</build_depend> -->
+ <!-- Use buildtool_depend for build tool packages: -->
+ <!-- <buildtool_depend>catkin</buildtool_depend> -->
+ <!-- Use run_depend for packages you need at runtime: -->
+ <!-- <run_depend>message_runtime</run_depend> -->
+ <!-- Use test_depend for packages you need only for testing: -->
+ <!-- <test_depend>gtest</test_depend> -->
+ <buildtool_depend>catkin</buildtool_depend>
+ <build_depend>actionlib</build_depend>
+ <build_depend>gazebo</build_depend>
+ <build_depend>gazebo_plugins</build_depend>
+ <build_depend>gazebo_ros</build_depend>
+ <build_depend>iri_common_drivers_msgs</build_depend>
+ <build_depend>tf</build_depend>
+ <run_depend>actionlib</run_depend>
+ <run_depend>gazebo</run_depend>
+ <run_depend>gazebo_plugins</run_depend>
+ <run_depend>gazebo_ros</run_depend>
+ <run_depend>iri_common_drivers_msgs</run_depend>
+ <run_depend>tf</run_depend>
+
+
+ <!-- The export tag contains other, unspecified, tags -->
+ <export>
+ <!-- Other tools can request additional information be placed here -->
+ <gazebo_ros plugin_path="${prefix}/lib" gazebo_media_path="${prefix}" />
+ </export>
+</package>
diff --git a/gz_gripper_plugin/src/gz_gripper_plugin.cpp b/gz_gripper_plugin/src/gz_gripper_plugin.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5d6217ccba5308375a4f225f860db47e404aa76d
--- /dev/null
+++ b/gz_gripper_plugin/src/gz_gripper_plugin.cpp
@@ -0,0 +1,364 @@
+#include <cmath>
+#include <boost/bind.hpp>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <ros/ros.h>
+#include <actionlib/server/simple_action_server.h>
+#include <tf/transform_listener.h>
+#include <iri_common_drivers_msgs/tool_closeAction.h>
+#include <iri_common_drivers_msgs/tool_openAction.h>
+
+namespace gazebo
+{
+
+class GripperPlugin : public ModelPlugin
+{
+ private:
+ ros::NodeHandle* nh_;
+ event::ConnectionPtr updateConnection_;
+ physics::ModelPtr model_;
+ physics::LinkPtr gripper_;
+ physics::LinkPtr attached_graspable_;
+ math::Pose graspable_rel_pose_;
+ bool deactivate_collision_;
+
+ //physics::JointPtr fixed_joint_;
+ bool closed_;
+
+ // Initially the graspable candidate was chosen by means of a RayShape and
+ // the GetIntersection method
+ // physics::RayShapePtr ray_;
+
+ // If a TF transform is not provided, use this:
+ double z_offset_;
+
+ // If a TF transform is provided, use this:
+ std::string gripper_frame_id_;
+ tf::TransformListener* listener_;
+
+ // action servers
+ actionlib::SimpleActionServer<iri_common_drivers_msgs::tool_closeAction>*
+ close_as_;
+ actionlib::SimpleActionServer<iri_common_drivers_msgs::tool_openAction>*
+ open_as_;
+
+ template <typename T>
+ static void set_option(sdf::ElementPtr sdf, const std::string& option,
+ T& value, const T& default_)
+ {
+ // This template does not work for T=std::string
+ if (sdf->HasElement(option))
+ {
+ sdf::ElementPtr elem = sdf->GetElement(option);
+ if (elem->GetValue()->Get(value))
+ {
+ ROS_INFO_STREAM(option << " set to " << value);
+ }
+ else
+ {
+ ROS_ERROR_STREAM(option << ": bad type. Set to default (" <<
+ default_ << ")");
+ value = default_;
+ }
+ }
+ else
+ {
+ value = default_;
+ ROS_WARN_STREAM(option << " set to default (" << default_ << ")");
+ }
+ }
+
+ static bool is_contained(const math::Vector3& point, const math::Box& box)
+ {
+ math::Vector3 center = box.GetCenter();
+ math::Vector3 size_2 = box.GetSize()/2;
+ double delta_x = std::fabs(center.x - point.x);
+ double delta_y = std::fabs(center.y - point.y);
+ double delta_z = std::fabs(center.z - point.z);
+ return delta_x < size_2.x and delta_y < size_2.y and delta_z < size_2.z;
+ }
+
+ void read_options(sdf::ElementPtr sdf)
+ {
+ // Read gripper link
+ sdf::ElementPtr elem;
+
+ elem = sdf->GetElement("gripper_link");
+ if (elem)
+ {
+ std::string gripper_name = elem->GetValue()->GetAsString();
+ ROS_INFO("Gripper link: %s", gripper_name.c_str());
+ gripper_ = model_->GetLink(gripper_name);
+ }
+ else
+ {
+ ROS_ERROR("No option specifying the gripper link");
+ return;
+ }
+
+ if (sdf->HasElement("gripper_tf"))
+ {
+ elem = sdf->GetElement("gripper_tf");
+ listener_ = new tf::TransformListener;
+ gripper_frame_id_ = elem->GetValue()->GetAsString();
+ ROS_INFO("The position of the gripper will be inferred from the %s tf",
+ gripper_frame_id_.c_str());
+ }
+ else
+ {
+ set_option(sdf, "z_offset", z_offset_, 0.5);
+ //set_option(sdf, "z_reach", z_reach_, 1.0);
+ listener_ = NULL;
+ ROS_INFO("The position of the gripper will be inferred from the "
+ "pose of the gripper link (%s) inside Gazebo",
+ gripper_->GetScopedName().c_str());
+ }
+
+ set_option(sdf, "deactivate_collision", deactivate_collision_, true);
+
+ }
+
+ math::Vector3 get_gripper_position() const
+ {
+ math::Vector3 position;
+
+ if (listener_ != NULL)
+ {
+ ROS_INFO("Trying to obtain gripper position through TF...");
+ try
+ {
+ tf::StampedTransform transform;
+ listener_->lookupTransform("/world", gripper_frame_id_, ros::Time(0),
+ transform);
+ position.x = transform.getOrigin().getX();
+ position.y = transform.getOrigin().getY();
+ position.z = transform.getOrigin().getZ();
+ }
+ catch (tf::TransformException& ex)
+ {
+ ROS_ERROR("%s", ex.what());
+ }
+ }
+ else
+ {
+ ROS_INFO("Obtaining gripper position from link...");
+ math::Pose gripper_pose = math::Pose(math::Vector3(0,0,z_offset_),
+ math::Quaternion()) + gripper_->GetWorldPose();
+ position = gripper_pose.pos;
+ }
+
+ ROS_INFO_STREAM("Retrieved position: " << position);
+
+ return position;
+ }
+
+ physics::LinkPtr get_graspable() const
+ {
+ physics::LinkPtr graspable;
+ math::Vector3 gripper_pos = get_gripper_position();
+
+ physics::Model_V all_models = model_->GetWorld()->GetModels();
+ for (int i = 0; i < all_models.size(); ++i)
+ {
+ if (all_models[i] == model_) continue; // Skip this model's links
+
+ physics::Link_V links = all_models[i]->GetLinks();
+ for (int j = 0; j < links.size(); ++j)
+ {
+ if (is_contained(gripper_pos, links[j]->GetBoundingBox()))
+ {
+ graspable = links[j];
+ break;
+ }
+ }
+ }
+
+ if (graspable)
+ {
+ ROS_INFO("Found graspable: %s", graspable->GetScopedName().c_str());
+ }
+ else
+ {
+ ROS_WARN("No graspable found");
+ }
+
+ return graspable;
+
+ // This alternate version uses RayShape. It uses the GetIntersection
+ // method. Sometimes the GetIntersection method crashed (that's why I'm
+ // using the previous iteration).
+
+ //math::Pose start = math::Pose(math::Vector3(0,0,z_offset_-0.005),
+ //math::Quaternion());
+ //start += gripper_->GetWorldPose();
+
+ //math::Pose end = math::Pose(math::Vector3(0,0,0.2),
+ //math::Quaternion());
+ //end += start;
+
+ //ROS_INFO_STREAM("Ray start point: " << start.pos <<
+ //"; ray end point: " << end.pos);
+
+ //ray_->SetPoints(start.pos, end.pos);
+
+ //double dist;
+ //std::string entity_n;
+ //ray_->GetIntersection(dist, entity_n);
+
+ //if (entity_n.empty()) ROS_WARN("No intersection");
+ //else
+ //{
+ //ROS_INFO("Intersection at %fm with entity %s", dist, entity_n.c_str());
+ //physics::CollisionPtr collision =
+ //boost::dynamic_pointer_cast<physics::Collision>(
+ //model_->GetWorld()->GetEntity(entity_n));
+
+ //graspable = collision->GetLink();
+ //}
+ //return graspable;
+ }
+
+ void open_callback(
+ const iri_common_drivers_msgs::tool_openGoalConstPtr& /* goal */)
+ {
+ if (closed_)
+ {
+ //physics::LinkPtr parent, child;
+ //parent = fixed_joint_->GetParent();
+ //child = fixed_joint_->GetChild();
+ if (attached_graspable_)
+ {
+ ROS_INFO("Links %s and %s are connected. Detaching...",
+ gripper_->GetScopedName().c_str(),
+ attached_graspable_->GetScopedName().c_str());
+
+ if (deactivate_collision_)
+ {
+ attached_graspable_->SetCollideMode("all");
+ }
+
+ attached_graspable_.reset();
+ }
+ else
+ {
+ ROS_INFO("No links to detach");
+ }
+ closed_ = false;
+ }
+ else
+ {
+ ROS_WARN("Gripper was already open. Not doing anything.");
+ }
+
+ // No errors expected => always succeed
+ iri_common_drivers_msgs::tool_openResult result;
+ result.successful = true;
+ open_as_->setSucceeded(result);
+ }
+
+ void close_callback(
+ const iri_common_drivers_msgs::tool_closeGoalConstPtr& /* goal */)
+ {
+ if (closed_)
+ {
+ ROS_WARN("Gripper was already closed. Not doing anything.");
+ }
+ else
+ {
+ attached_graspable_ = get_graspable();
+
+ if (attached_graspable_)
+ {
+ ROS_INFO("Attaching %s to %s", gripper_->GetScopedName().c_str(),
+ attached_graspable_->GetScopedName().c_str());
+ graspable_rel_pose_ = attached_graspable_->GetWorldPose() -
+ gripper_->GetWorldPose();
+
+ if (deactivate_collision_)
+ {
+ attached_graspable_->SetCollideMode("none");
+ }
+ }
+ else
+ {
+ ROS_WARN("The gripper has not picked anything");
+ }
+
+ closed_ = true;
+ }
+
+ // No errors expected => always succeed
+ iri_common_drivers_msgs::tool_closeResult result;
+ result.successful = true;
+ close_as_->setSucceeded(result);
+ }
+
+ public:
+ virtual void Load(physics::ModelPtr parent, sdf::ElementPtr sdf)
+ {
+ model_ = parent;
+ read_options(sdf);
+ if (not gripper_)
+ {
+ // The plugin won't work. Skip the remaining initialization steps.
+ ROS_ERROR("Gripper not found. The plugin will not work.");
+ return;
+ }
+
+ //physics::PhysicsEnginePtr physics = parent->GetWorld()->
+ //GetPhysicsEngine();
+ //ray_ = boost::dynamic_pointer_cast<physics::RayShape>(
+ //physics->CreateShape("ray", physics::CollisionPtr()));
+ //fixed_joint_ = physics->CreateJoint("revolute", model_);
+
+ if (not ros::isInitialized())
+ {
+ nh_ = NULL;
+ open_as_ = NULL;
+ close_as_ = NULL;
+ ROS_ERROR("ROS has not been initialized. Cannot load plugin.");
+ return;
+ }
+
+ nh_ = new ros::NodeHandle;
+ open_as_ = new actionlib::SimpleActionServer<
+ iri_common_drivers_msgs::tool_openAction>(*nh_, "gripper/open_tool",
+ boost::bind(&GripperPlugin::open_callback, this, _1), false);
+ close_as_ = new actionlib::SimpleActionServer<
+ iri_common_drivers_msgs::tool_closeAction>(*nh_, "gripper/close_tool",
+ boost::bind(&GripperPlugin::close_callback, this, _1), false);
+
+ open_as_->start();
+ close_as_->start();
+
+ ROS_INFO("Gripper plugin loaded");
+
+ this->updateConnection_ = event::Events::ConnectWorldUpdateBegin(
+ boost::bind(&GripperPlugin::OnUpdate, this, _1));
+ }
+
+ void OnUpdate(const common::UpdateInfo info)
+ {
+ if (attached_graspable_)
+ {
+ attached_graspable_->SetWorldPose(graspable_rel_pose_ +
+ gripper_->GetWorldPose());
+ attached_graspable_->SetWorldTwist(math::Vector3(), math::Vector3());
+ }
+ ros::spinOnce();
+ }
+
+ ~GripperPlugin()
+ {
+ if (nh_ != NULL) delete nh_;
+ if (open_as_ != NULL) delete open_as_;
+ if (close_as_ != NULL) delete close_as_;
+ if (listener_ != NULL) delete listener_;
+ }
+
+};
+
+GZ_REGISTER_MODEL_PLUGIN(GripperPlugin)
+
+}