Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
CeTI
ROS
ROS Packages
gazebo-pkgs
Commits
431eeff5
Commit
431eeff5
authored
Feb 25, 2016
by
Jennifer Buehler
Browse files
added sources
parent
e3dfd35a
Changes
30
Expand all
Hide whitespace changes
Inline
Side-by-side
README.md
View file @
431eeff5
# gazebo-pkgs
A collection of tools and plugins for Gazebo
gazebo_grasp_plugin/CMakeLists.txt
0 → 100644
View file @
431eeff5
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
0 → 100644
View file @
431eeff5
#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
0 → 100644
View file @
431eeff5
<?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_grasp_plugin/src/GazeboGraspFix.cpp
0 → 100644
View file @
431eeff5
This diff is collapsed.
Click to expand it.
gazebo_state_plugins/CMakeLists.txt
0 → 100644
View file @
431eeff5
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
0 → 100644
View file @
431eeff5
# 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
0 → 100644
View file @
431eeff5
# 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
0 → 100644
View file @
431eeff5
# 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
0 → 100644
View file @
431eeff5
#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
0 → 100644
View file @
431eeff5
#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>