diff --git a/README.md b/README.md index 9d84e0c60f07da119817f75fadd573f9a0bec1f5..586318b0f618d1fbb9a8409a1b477c3b98c269f9 100644 --- a/README.md +++ b/README.md @@ -4,6 +4,12 @@ This repository contains several plugins that may be handy for experimentation with the Gazebo simulator. Right now, the package contains the following plugins: -* Gripper plugin: plugin for easily grasping objects in Gazebo without physical - contact between the gripper link and the graspable objects. This overcomes the - difficulties that arise from squishing an object between two links. +* Gripper plugin: model plugin for easily grasping objects in Gazebo without + physical contact between the gripper link and the graspable objects. This + overcomes the difficulties that arise from squishing an object between two + links. +* Black hole plugin: model plugin that converts a given model into a + "black hole". This means that if any of a given list of models (the "stars") + touches the "black hole", then the model will be deleted. Useful to dispatch + objects that are no longer required in the simulation (e.g. a can thrown to + the thrash). diff --git a/gazebo_ros_plugins/package.xml b/gazebo_ros_plugins/package.xml index 0e22c731b82d0f6e4afdb9c9ad9959806a628790..bf9ec1763fcacb646966c07907e5d77c34e8d5d0 100644 --- a/gazebo_ros_plugins/package.xml +++ b/gazebo_ros_plugins/package.xml @@ -43,6 +43,7 @@ <buildtool_depend>catkin</buildtool_depend> <run_depend>gz_gripper_plugin</run_depend> + <run_depend>gz_black_hole</run_depend> <!-- The export tag contains other, unspecified, tags --> diff --git a/gz_black_hole/CMakeLists.txt b/gz_black_hole/CMakeLists.txt new file mode 100644 index 0000000000000000000000000000000000000000..dd936ce8c62d7919b760cb97a9dbbd12b5138882 --- /dev/null +++ b/gz_black_hole/CMakeLists.txt @@ -0,0 +1,194 @@ +cmake_minimum_required(VERSION 2.8.3) +project(gz_black_hole) + +## 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_black_hole +# 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_black_hole + src/gz_black_hole.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_black_hole ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Declare a C++ executable +# add_executable(gz_black_hole_node src/gz_black_hole_node.cpp) + +## Add cmake target dependencies of the executable +## same as for the library above +# add_dependencies(gz_black_hole_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Specify libraries to link a library or executable target against +target_link_libraries(gz_black_hole + ${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_black_hole gz_black_hole_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_black_hole.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_black_hole/launch/test.launch b/gz_black_hole/launch/test.launch new file mode 100644 index 0000000000000000000000000000000000000000..6bec36e022254c3cdad7c24a4f50142a8ba060c7 --- /dev/null +++ b/gz_black_hole/launch/test.launch @@ -0,0 +1,22 @@ +<?xml version="1.0" ?> +<launch> + + <!-- these are the arguments you can pass this launch file, for example paused:=true --> + <arg name="paused" default="false"/> + <arg name="use_sim_time" default="true"/> + <arg name="gui" default="true"/> + <arg name="headless" default="false"/> + <arg name="debug" default="false"/> + + <!-- We resume the logic in empty_world.launch, changing only the name of the world to be launched --> + <include file="$(find gazebo_ros)/launch/empty_world.launch"> + <arg name="world_name" value="$(find gz_black_hole)/worlds/test.world"/> + <arg name="debug" value="$(arg debug)" /> + <arg name="gui" value="$(arg gui)" /> + <arg name="paused" value="$(arg paused)"/> + <arg name="use_sim_time" value="$(arg use_sim_time)"/> + <arg name="headless" value="$(arg headless)"/> + </include> + +</launch> + diff --git a/gz_black_hole/package.xml b/gz_black_hole/package.xml new file mode 100644 index 0000000000000000000000000000000000000000..693eea7b72af472f9ad8ec537a58f50c07893ee6 --- /dev/null +++ b/gz_black_hole/package.xml @@ -0,0 +1,63 @@ +<?xml version="1.0"?> +<package> + <name>gz_black_hole</name> + <version>0.0.0</version> + <description>The gz_black_hole 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_black_hole</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_black_hole/src/gz_black_hole.cpp b/gz_black_hole/src/gz_black_hole.cpp new file mode 100644 index 0000000000000000000000000000000000000000..17e9be4aace1b6c60f8f3985d1236b703d4c97eb --- /dev/null +++ b/gz_black_hole/src/gz_black_hole.cpp @@ -0,0 +1,127 @@ +#include <cmath> +#include <boost/bind.hpp> +#include <gazebo_msgs/DeleteModel.h> +#include <gazebo/gazebo.hh> +#include <gazebo/physics/physics.hh> +#include <gazebo/common/common.hh> +#include <gazebo/transport/transport.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 BlackHolePlugin : public ModelPlugin +{ + private: + transport::SubscriberPtr contactSub_; + physics::ModelPtr model_; + physics::ContactManager* contact_manager_; + transport::NodePtr node_; + std::vector<std::string> collision_names_, stars_; + std::string topic_; // contacts topic + + void read_options(sdf::ElementPtr sdf) + { + // Read gripper link + sdf::ElementPtr elem = sdf->GetElement("star"); + + while (elem) + { + std::string star = elem->GetValue()->GetAsString(); + stars_.push_back(star); + elem = elem->GetNextElement("star"); + ROS_INFO("Added star: %s", star.c_str()); + } + } + + /** + * @brief Iterate through all the collisions of the model to get their + * names and store them in collision_names_ + */ + void obtain_collision_names() + { + collision_names_.clear(); + physics::Link_V links = model_->GetLinks(); + for (int i = 0; i < links.size(); ++i) + { + physics::Collision_V collisions = links[i]->GetCollisions(); + for (int j = 0; j < collisions.size(); ++j) + { + collision_names_.push_back(collisions[j]->GetScopedName()); + } + } + } + + bool is_star(physics::ModelPtr model) + { + for (int i = 0; i < stars_.size(); ++i) + { + if (stars_[i] == model->GetName()) return true; + } + return false; + } + + public: + + virtual void Load(physics::ModelPtr parent, sdf::ElementPtr sdf) + { + model_ = parent; + + // get contact manager from physics engine + physics::PhysicsEnginePtr physics = + parent->GetWorld()->GetPhysicsEngine(); + contact_manager_ = physics->GetContactManager(); + read_options(sdf); + obtain_collision_names(); + topic_ = contact_manager_->CreateFilter(model_->GetName() + "_contacts", + collision_names_); + + // init gazebo node and link contact callback + node_.reset(new transport::Node()); + node_->Init(model_->GetWorld()->GetName()); + contactSub_ = node_->Subscribe(topic_, &BlackHolePlugin::OnContact, this); + } + + void OnContact(const ConstContactsPtr& msg) + { + //ROS_INFO("New contact"); + for (int i = 0; i < msg->contact_size(); ++i) + { + physics::CollisionPtr collision1 = boost::dynamic_pointer_cast< + physics::Collision>(model_->GetWorld()->GetEntity( + msg->contact(i).collision1())); + physics::CollisionPtr collision2 = boost::dynamic_pointer_cast< + physics::Collision>(model_->GetWorld()->GetEntity( + msg->contact(i).collision2())); + physics::ModelPtr model1 = collision1->GetModel(); + physics::ModelPtr model2 = collision2->GetModel(); + + // Check if any of the two models involved in the collision is a star. + // If so, delete the star and exit the loop (maximum one deletion per + // callback to avoid deleting the same model twice) + std::string to_delete; + if (is_star(model1)) to_delete = model1->GetName(); + else if (is_star(model2)) to_delete = model2->GetName(); + + if (not to_delete.empty()) + { + ROS_INFO("Deleting model %s", to_delete.c_str()); + transport::requestNoReply(node_, "entity_delete", to_delete); + } + + } + } + + ~BlackHolePlugin() + { + } + +}; + +GZ_REGISTER_MODEL_PLUGIN(BlackHolePlugin) + +} diff --git a/gz_black_hole/worlds/test.world b/gz_black_hole/worlds/test.world new file mode 100644 index 0000000000000000000000000000000000000000..951d3e08695fe0aca299c408f3d9ef3a17532198 --- /dev/null +++ b/gz_black_hole/worlds/test.world @@ -0,0 +1,70 @@ +<?xml version="1.0"?> +<sdf version="1.4"> + <world name="test.world"> + <scene> + <shadows>false</shadows> + </scene> + <include> + <uri>model://ground_plane</uri> + </include> + <include> + <uri>model://sun</uri> + </include> + + <model name="box1"> + <pose>0 0 0.5 1.57 0 0</pose> + <link name="link"> + <collision name="collision"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + </collision> + <visual name="visual"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + <material> + <ambient>0 0.086 0.043 1</ambient> + <diffuse>0 0.75 0.38 1</diffuse> + </material> + </visual> + + </link> + + <plugin filename="libgz_black_hole.so" name="gripper_plugin"> + <star>box2</star> + <star>beer</star> + </plugin> + </model> + + + <model name="box2"> + <pose>0 -1.05 0.5 0 0 0</pose> + <link name="link"> + <collision name="collision"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + </collision> + <visual name="visual"> + <geometry> + <box> + <size>1 1 1</size> + </box> + </geometry> + <material> + <ambient>0.10 0 0.043 1</ambient> + <diffuse>0.87 0 0.38 1</diffuse> + </material> + </visual> + </link> + </model> + + </world> +</sdf>