added sources

README.md

 # gazebo-pkgs
+
 A collection of tools and plugins for Gazebo

gazebo_grasp_plugin/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_grasp_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
+  gazebo_ros
+  geometry_msgs
+  roscpp
+  std_msgs
+)
+
+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()
+
+###################################
+## 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 gazebo_grasp_fix
+  CATKIN_DEPENDS gazebo_ros geometry_msgs roscpp std_msgs
+  DEPENDS gazebo 
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+)
+
+link_directories(
+  ${GAZEBO_LIBRARY_DIRS}
+  ${catkin_LIBRARY_DIRS}
+)
+
+## Declare a cpp library
+add_library(gazebo_grasp_fix SHARED src/GazeboGraspFix.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+# add_dependencies(gazebo_grasp_plugin_node gazebo_grasp_plugin_generate_messages_cpp)
+add_dependencies(gazebo_grasp_fix ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+
+target_link_libraries(gazebo_grasp_fix 
+	${GAZEBO_LIBRARIES} 
+	${Boost_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 gazebo_grasp_plugin gazebo_grasp_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_gazebo_grasp_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)

gazebo_grasp_plugin/include/gazebo_grasp_plugin/GazeboGraspFix.h

+#include <boost/bind.hpp>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/physics.hh>
+#include <gazebo/common/common.hh>
+#include <gazebo/transport/TransportTypes.hh>
+#include <stdio.h>
+
+namespace gazebo {
+
+/**
+ * Inspired by gazebo::physics::Gripper, this is our own implementation which does not use deprecated functions any more.
+ *
+ * This is a *model* pluing, so you have to load the model plugin from the robot URDF:
+ *
+ * ```xml
+ *   <gazebo>
+ *       <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
+ *           <palm_link> hand_link_name  </palm_link>
+ *           <gripper_link> finger_index_link_1 </gripper_link>
+ *           <gripper_link> finger_index_link_2 </gripper_link>
+ *           <gripper_link> ... </gripper_link>
+ *       </plugin>
+ *   </gazebo>
+ * ```
+ *
+ * \author Jennifer Buehler
+ */ 
+class GazeboGraspFix : public ModelPlugin {
+public:
+    GazeboGraspFix();
+    GazeboGraspFix(physics::ModelPtr _model);
+    virtual ~GazeboGraspFix();
+    virtual void Init(); 
+    virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
+    void OnUpdate();
+private: 
+
+    void InitValues();
+    void OnContact(const ConstContactsPtr& ptr);
+
+    bool HandleAttach(const std::string& objName);
+    void HandleDetach(const std::string& objName);
+
+    /**
+     * Checks whether any two vectors in the set have an angle greater than minAngleDiff (in rad), and one is at least
+     * lengthRatio (0..1)  of the other in it's length.
+     */
+    bool checkGrip(const std::vector<math::Vector3>& forces, float minAngleDiff, float lengthRatio);
+    
+    physics::ModelPtr model;
+
+    physics::PhysicsEnginePtr physics;
+    physics::WorldPtr world;
+    physics::JointPtr fixedJoint;
+    
+    physics::LinkPtr palmLink;
+    event::ConnectionPtr update_connection;
+    transport::NodePtr node;
+    transport::SubscriberPtr contactSub; //subscriber to contact updates
+    
+    std::map<std::string, physics::CollisionPtr> collisions;
+
+    /**
+     * Helper class to encapsulate a collision information.
+     * Forward declaration here.
+     */
+    class CollidingPoint;
+
+    //Contact forces sorted by object name the gripper collides with, and the link colliding. 
+    //This is a vector summed up over time, sum count is kept in contactSumCnt.
+    std::map<std::string, std::map<std::string, CollidingPoint> > contacts; 
+    boost::mutex mutexContacts; //mutex protects contacts
+
+    //when an object was attached, it had these colliding points.
+    std::map<std::string, std::map<std::string, CollidingPoint> > attachGripContacts; 
+    
+    //records how many update loops (at updateRate) the grip on that object has been recorded 
+    //as "holding". Every loop, if a grip is not recorded, this number decreases. 
+    //When it reaches grip_count_threshold, it will be attached.
+    //The number won't increase above max_grip_count once it has reached that number.
+    std::map<std::string, int> gripCounts; 
+    //maximum record in gripCounts
+    int max_grip_count;    
+    //number of recorded "grips" in the past (in gripCount) which, when it is exceeded, counts
+    //as the object grasped, and when it is lower, as released.
+    int grip_count_threshold;
+
+    //once an object is gripped, the relative position of the collision link surface to the
+    //object is remembered. As soon as this distance changes more than release_tolerance,
+    //the object is released.
+    float release_tolerance;
+
+    bool attached;
+    std::string attachedObjName;    
+
+    //nano seconds between two updates
+    common::Time updateRate;
+
+    //last time OnUpdate() was called
+    common::Time prevUpdateTime;
+};
+
+}

gazebo_grasp_plugin/package.xml

+<?xml version="1.0"?>
+<package>
+  <name>gazebo_grasp_plugin</name>
+  <version>0.0.0</version>
+  <description>
+        Gazebo Model plugin(s) which handle/help grasping in Gazebo.
+  </description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <maintainer email="jennifer.e.buehler@gmail.com">Jennifer Buehler</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>GPLv3</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_grasp_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> -->
+
+
+  <!-- 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>gazebo_ros</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <run_depend>gazebo_ros</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- You can specify that this package is a metapackage here: -->
+    <!-- <metapackage/> -->
+
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

gazebo_state_plugins/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(gazebo_state_plugins)
+
+## 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
+  geometry_msgs
+  roscpp
+  shape_msgs
+  std_msgs
+  gazebo_ros
+  object_msgs
+  eigen_conversions
+)
+
+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()
+
+###################################
+## 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 gazebo_state_plugins gazebo_map_publisher
+  CATKIN_DEPENDS geometry_msgs roscpp shape_msgs std_msgs gazebo_ros
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${GAZEBO_INCLUDE_DIRS}
+)
+
+## Declare a cpp library
+add_library(gazebo_object_info
+   src/GazeboObjectInfo.cpp
+)
+
+add_library(gazebo_map_publisher
+   src/GazeboMapPublisher.cpp
+)
+
+add_library(gazebo_plugin_loader
+    src/GazeboPluginLoader.cpp
+)
+
+## Declare a cpp executable
+add_executable(gazebo_request_object_info test/object_info_request.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(gazebo_request_object_info ${catkin_EXPORTED_TARGETS})
+add_dependencies(gazebo_object_info ${catkin_EXPORTED_TARGETS})
+add_dependencies(gazebo_map_publisher ${catkin_EXPORTED_TARGETS})
+add_dependencies(gazebo_plugin_loader ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(gazebo_object_info
+   ${GAZEBO_LIBRARIES}
+   ${catkin_LIBRARIES}
+)
+target_link_libraries(gazebo_map_publisher
+   ${GAZEBO_LIBRARIES}
+   ${catkin_LIBRARIES}
+)
+target_link_libraries(gazebo_request_object_info
+   ${catkin_LIBRARIES}
+)
+
+target_link_libraries(gazebo_plugin_loader
+   ${GAZEBO_LIBRARIES}
+   ${catkin_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 gazebo_state_plugins gazebo_state_plugins_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_gazebo_state_plugins.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)

gazebo_state_plugins/config/GazeboMapPublisher.yaml

+# the topic onto which to continuously publish
+# the map.
+publish_map_topic: "/gazebo/map"
+
+# frequency to publish the map
+map_pub_frequency: 1
+
+# topic onto which to publish the metadata
+# information of the map. Only will be published
+# if publish_map_topic is not empty.
+map_metadata_topic: "/map_metadata"
+
+# topic onto which to provide a service to
+# request the map.
+request_map_service: "/dynamic_map"
+
+# The frame ID to assign to the map    
+map_frame_id: "/map"
+
+# the name of the robot in Gazebo
+robot_name: "robot"
+

gazebo_state_plugins/config/GazeboObjectInfo.yaml

+# Continuously publish info of gazebo objects?
+# This should be false if another plugin publishes
+# gazebo object info.
+publish_world_objects: false 
+
+# the topic onto which to publish object info of
+# type object_msgs/Object
+world_objects_topic: "/gazebo_objects"
+
+# The topic onto which to publish the service
+# which can be used to request object info    
+request_object_service: "/gazebo_objects/get_info"
+
+# frame of the objects poses that will be published.
+# Should usually be 'world'
+objects_frame_id: "world"

gazebo_state_plugins/config/WorldPlugins.yaml

+# Parameters read by GazeboPluginLoader: contains
+# names and library files of Gazebo World Plugins to be
+# loaded.
+
+# name of world to load the plugins on. 'default' or 
+# empty string should yield the default world.
+world_name: "default"
+
+# names and filenames of plugins to load
+world_plugins:
+    - name: gazebo_object_info
+      file: libgazebo_object_info.so
+    # - name: gazebo_map_publisher
+    #  file: libgazebo_map_publisher.so

gazebo_state_plugins/include/gazebo_state_plugins/GazeboMapPublisher.h

+#ifndef GAZEBO_GAZEBOMAPPUBLISHER_H
+#define GAZEBO_GAZEBOMAPPUBLISHER_H
+
+#include <ros/ros.h>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/Model.hh>
+#include <nav_msgs/GetMap.h>
+#include <nav_msgs/GetMapRequest.h>
+#include <nav_msgs/GetMapResponse.h>
+#include <nav_msgs/OccupancyGrid.h>
+
+namespace gazebo
+{
+
+/**
+ * Publishes a map of the grid in gazebo to a topic that the map is supposed to be published to.
+ *
+ * A service is also provided to request a map (service name can be specified with ros paramter
+ * "request_map_service") of type nav_msgs/GetMap.
+ *
+ * The plugin parameters can be specified in a YAML file,
+ * which needs to be loaded onto the parameter server
+ * under **namespace gazebo_state_plugins**, as follows:
+ *
+ * ```
+ * # the topic onto which to continuously publish
+ * # the map.
+ * publish_map_topic: "/gazebo/map"
+ * 
+ * # frequency to publish the map
+ * map_pub_frequency: 1
+ * 
+ * # topic onto which to publish the metadata
+ * # information of the map. Only will be published
+ * # if publish_map_topic is not empty.
+ * map_metadata_topic: "/map_metadata"
+ * 
+ * # topic onto which to provide a service to
+ * # request the map.
+ * request_map_service: "/dynamic_map"
+ * 
+ * # The frame ID to assign to the map    
+ * map_frame_id: "/map"
+ * 
+ * # the name of the robot in Gazebo
+ * robot_name: "robot"
+ * ```
+ *
+ * \author Jennifer Buehler
+ */
+class GazeboMapPublisher : public WorldPlugin
+{
+public: 
+
+	GazeboMapPublisher();
+  	void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf);
+
+private:
+
+	bool requestMap(nav_msgs::GetMap::Request &req, nav_msgs::GetMap::Response &res); 
+
+	void advertEvent(const ros::TimerEvent& e); 
+
+	void onWorldUpdate(); 
+
+    // forward declaration
+    class CollisionMapRequest;
+
+	//see http://gazebosim.org/wiki/Tutorials/1.4/custom_messages#Collision_Map_Creator_Plugin
+	bool createMap(const CollisionMapRequest &msg, const std::string& map_frame, nav_msgs::OccupancyGrid& map);
+
+	CollisionMapRequest getCollisionRequest();
+
+	nav_msgs::MapMetaData getMetaData(); 
+
+	nav_msgs::OccupancyGrid getMap(); 
+	
+    //map resolution (m/cell)
+	float map_resolution;
+
+    //height of obstacles to consider for generating the map
+	float map_height;
+	double map_offset_x;
+	double map_offset_y;
+	double map_len_x;
+	double map_len_y;
+
+    ros::NodeHandle node;
+
+	std::string MAP_TOPIC;
+	std::string MAP_FRAME;
+	std::string METADATA_TOPIC;
+	std::string REQUEST_MAP_SERVICE;
+	std::string ROBOT_NAME;
+	float MAP_PUB_FREQ;
+	
+	physics::WorldPtr world;
+
+	event::ConnectionPtr update_connection;
+	ros::Publisher map_pub;
+	ros::Publisher meta_pub;
+
+	ros::ServiceServer request_map_srv;
+		
+	ros::Timer publishTimer;
+
+	bool worldChangedSinceLastAdvert; 
+};
+
+}
+#endif  // GAZEBO_GAZEBOMAPPUBLISHER_H

gazebo_state_plugins/include/gazebo_state_plugins/GazeboObjectInfo.h

+#ifndef GAZEBO_GAZEBOOBJECTINFO_H
+#define GAZEBO_GAZEBOOBJECTINFO_H
+
+#include <ros/ros.h>
+#include <gazebo/gazebo.hh>
+#include <gazebo/physics/Model.hh>
+
+#include <object_msgs/Object.h>
+#include <object_msgs/ObjectInfo.h>
+#include <object_msgs/ObjectInfoRequest.h>
+#include <object_msgs/ObjectInfoResponse.h>