Commit 431eeff5 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

added sources

parent e3dfd35a
# gazebo-pkgs
A collection of tools and plugins for Gazebo
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)
#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;
};
}
<?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>
This diff is collapsed.
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)
# 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"
# 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"
# 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
#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
#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>