Skip to content
Snippets Groups Projects
Commit 1bb3837a authored by Johannes Mey's avatar Johannes Mey
Browse files

initial commit

parent 3a66bffe
No related branches found
No related tags found
No related merge requests found
cmake_minimum_required(VERSION 3.0.2)
project(simulation_util)
# Compile as C++14
add_compile_options(-std=c++14)
## 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
roscpp
gazebo_msgs
)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
find_package(SDFormat)
# this is the last resort if the package cannot be found otherwise
#find_package(PkgConfig REQUIRED)
#pkg_check_modules(SDF sdformat 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 exec_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 exec_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
# std_msgs # Or other packages containing 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 exec_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 your 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_zone_utility
CATKIN_DEPENDS gazebo_ros
DEPENDS SDFormat
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
include
${catkin_INCLUDE_DIRS}
${SDFormat_INCLUDE_DIRS}
)
## Declare a C++ library
# add_library(${PROJECT_NAME}
# src/${PROJECT_NAME}/simulation_util.cpp
# )
add_library(gazebo_zone_utility
src/GazeboZoneSpawner.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(gazebo_zone_utility ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/simulation_util_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
target_link_libraries(gazebo_zone_utility
${catkin_LIBRARIES}
${SDFormat_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
# catkin_install_python(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
# install(TARGETS ${PROJECT_NAME}_node
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
# install(TARGETS ${PROJECT_NAME}
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_GLOBAL_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_simulation_util.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 documentation for this package is located [here](http://st.inf.tu-dresden.de/ceti-robots/pkgs/simulation_util.html).
\ No newline at end of file
A ROS package for Gazebo simulation utility libraries.
The full documentation for this package is located [here](http://st.inf.tu-dresden.de/ceti-robots/pkgs/simulation_util.html).
## Provided Libraries
#### gazebo_zone_utility
- A library for displaying safety zones in Gazebo.
\ No newline at end of file
/*! \file GazeboZoneSpawner.cpp
\author Johannes Mey
\date 31.03.20
*/
#ifndef SIMULATION_UTIL_GAZEBOZONESPAWNER_H
#define SIMULATION_UTIL_GAZEBOZONESPAWNER_H
#include <shape_msgs/SolidPrimitive.h>
#include <geometry_msgs/Pose.h>
/*!
* A class for spawning safety zones in the Gazebo simulator.
*/
class GazeboZoneSpawner {
public:
/**
* @brief Spawns a box.
*
* The box is based on a prototype located in the package at `models/box.sdf`:
* ```xml
* <?xml version="1.0" ?>
* <sdf version="1.5">
* <model name="box">
* <static>true</static>
* <link name="link">
* <visual name="visual">
* <material>
* <script>
* <uri>box.material</uri>
* <name>Gazebo/DarkMagentaTransparent</name>
* </script>
* </material>
* <pose>0 0 0 0 0 0</pose>
* <geometry>
* <box>
* <size>1 1 1</size>
* </box>
* </geometry>
* </visual>
* </link>
* </model>
* </sdf>
* ```
*
* Before the box is spawned, its pose and size are adapted to the method parameters.
*
* The color of the box is provided in a material file in the package at `models/box.material`. Alternatively, the
* built-in materials could be used, but due to some bugs in gazebo_ros, the absolute URI would have to be provided,
* e.g., `/usr/share/gazebo-9/media/materials/scripts/gazebo.material`, which would make the package dependent on
* particular versions and installation locations.
*
* @param shape The shape of the collision box. Currently, only `BOX` is supported.
* @param pose The pose of the box. The position of the box is its center.
*/
static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose);
};
#endif //SIMULATION_UTIL_GAZEBOZONESPAWNER_H
/**
\mainpage
\htmlinclude manifest.html
A ROS package for Gazebo simulation utility libraries.
## Provided Libraries
#### gazebo_zone_utility
- A library for displaying safety zones in Gazebo.
*/
material Zone/DarkMagentaTransparent
{
technique
{
pass
{
scene_blend alpha_blend
depth_write off
ambient 0.6 0.0 0.6 1
diffuse 0.6 0.0 0.6 1
texture_unit
{
colour_op_ex source1 src_current src_current 0.6 0 0.6
alpha_op_ex source1 src_manual src_current 0.5
}
}
}
}
<?xml version="1.0" ?>
<sdf version="1.5">
<model name="box">
<static>true</static>
<link name="link">
<visual name="visual">
<material>
<script>
<uri>box.material</uri>
<name>Zone/DarkMagentaTransparent</name>
</script>
</material>
<pose>2 3 0.5 0 0 0</pose>
<geometry>
<box>
<size>1 1 1</size>
</box>
</geometry>
</visual>
</link>
</model>
</sdf>
\ No newline at end of file
<?xml version="1.0"?>
<package format="2">
<name>simulation_util</name>
<version>0.1.0</version>
<description>The simulation_util package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="johannes.mey@tu-dresden.de">Johannes Mey</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>MIT</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<url type="website">http://st.inf.tu-dresden.de/ceti-robots/pkgs/simulation_util.html</url>
<url type="repository">https://git-st.inf.tu-dresden.de/ceti/ros/simulation_util</url>
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<author email="johannes.mey@tu-dresden.de">Johannes Mey</author>
<!-- The *depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
<!-- <depend>roscpp</depend> -->
<!-- Note that this is equivalent to the following: -->
<!-- <build_depend>roscpp</build_depend> -->
<!-- <exec_depend>roscpp</exec_depend> -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use build_export_depend for packages you need in order to build against this package: -->
<!-- <build_export_depend>message_generation</build_export_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use exec_depend for packages you need at runtime: -->
<!-- <exec_depend>message_runtime</exec_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<!-- Use doc_depend for packages you need only for building documentation: -->
<!-- <doc_depend>doxygen</doc_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>sdformat</depend>
<build_depend>gazebo_msgs</build_depend>
<exec_depend>gazebo_ros</exec_depend>
<doc_depend>doxygen</doc_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>
- builder: doxygen
name: C++ API
file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox'
homepage: http://st.inf.tu-dresden.de/ceti-robots/pkgs/simulation_util.html
/*! \file GazeboZoneSpawner.cpp
\author Johannes Mey
\date 31.03.20
*/
#include "GazeboZoneSpawner.h"
#include <sdformat-6.0/sdf/sdf.hh> // for sdf model parsing
#include <gazebo_msgs/SpawnModel.h> // service definition for spawning things in gazebo
#include <ros/ros.h>
#include <ros/package.h>
#include <tf/tf.h>
void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) {
if (shape.type != shape.BOX) {
ROS_ERROR_STREAM("Safety-zone could not be created due to wrong shape-type");
return;
}
sdf::SDFPtr sdf(new sdf::SDF());
sdf::init(sdf);
auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.sdf";
auto materialFileName = "file://" + ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.material";
assert(sdf::readFile(sdfFileName, sdf));
auto visualElement = sdf->Root()->GetElement("model")->GetElement("link")->GetElement("visual");
auto materialUriElement = visualElement->GetElement("material")->GetElement("script")->GetElement("uri");
auto poseElement = visualElement->GetElement("pose");
auto sizeElement = visualElement->GetElement("geometry")->GetElement("box")->GetElement("size");
materialUriElement->Set(materialFileName);
tf::Quaternion rot;
rot.setValue(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
double roll, pitch, yaw;
tf::Matrix3x3(rot).getRPY(roll, pitch, yaw);
std::ostringstream poseStream;
poseStream << pose.position.x << " " << pose.position.y << " " << pose.position.z << " "
<< roll << " " << pitch << " " << yaw;
poseElement->Set(poseStream.str());
ROS_INFO_STREAM("Set pose in SDF file to '" << poseElement->GetValue()->GetAsString() << "'.");
std::ostringstream sizeStream;
sizeStream << shape.dimensions[0] << " " << shape.dimensions[1] << " " << shape.dimensions[2];
sizeElement->Set(sizeStream.str());
ROS_INFO_STREAM("Set size in SDF file to '" << sizeElement->GetValue()->GetAsString() << "'.");
ros::NodeHandle n;
ros::ServiceClient client = n.serviceClient<gazebo_msgs::SpawnModel>("/gazebo/spawn_sdf_model");
gazebo_msgs::SpawnModel srv;
srv.request.robot_namespace = "box space";
// srv.request.initial_pose = pose; // not required here
srv.request.model_name = std::string("box") + poseStream.str() + sizeStream.str();
srv.request.model_xml = sdf->ToString();
// srv.request.reference_frame = ; // if left empty, world is used
if (client.call(srv)) {
if (srv.response.success) {
ROS_INFO_STREAM("Spawned box '" << srv.request.model_name << "'. " << srv.response.status_message);
} else {
ROS_ERROR_STREAM("Failed to spawn box '" << srv.request.model_name << "'. " << srv.response.status_message);
}
} else {
ROS_ERROR("Failed to call service '/gazebo/spawn_sdf_model'");
}
}
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment