From 815a248170b589b82d736278a65d11bfd1ca2d3e Mon Sep 17 00:00:00 2001 From: Johannes Mey <johannes.mey@tu-dresden.de> Date: Mon, 31 Aug 2020 12:25:37 +0200 Subject: [PATCH] cleanup API and add some documentation --- README.md | 7 ++- include/GazeboZoneSpawner.h | 80 +++++++++++++++++++++--- models/{hardbox.sdf => physical_box.sdf} | 0 src/GazeboZoneSpawner.cpp | 6 +- 4 files changed, 82 insertions(+), 11 deletions(-) rename models/{hardbox.sdf => physical_box.sdf} (100%) diff --git a/README.md b/README.md index 4cd93b5..695966b 100644 --- a/README.md +++ b/README.md @@ -5,4 +5,9 @@ The full documentation for this package is located [here](http://st.inf.tu-dresd ## Provided Libraries #### gazebo_zone_utility -- A library for displaying safety zones in Gazebo. \ No newline at end of file +- A library for displaying objects in Gazebo. Currently, only boxes are supported. + - `GazeboZoneSpawner::spawnPrimitive()` spawns a primitive with visual, collision, and (opionally) physical + properties. + - `GazeboZoneSpawner::spawnVisualPrimitive()` also spawns objects, but these are purely visual, such that any + physical object moves straight through them. This is useful for additional visualizations, e.g., to describe + certain zones in the scene and to display paraphysical entities, such as ghosts. \ No newline at end of file diff --git a/include/GazeboZoneSpawner.h b/include/GazeboZoneSpawner.h index a108d50..21df2d5 100644 --- a/include/GazeboZoneSpawner.h +++ b/include/GazeboZoneSpawner.h @@ -10,6 +10,7 @@ #include <shape_msgs/SolidPrimitive.h> #include <geometry_msgs/Pose.h> +#include <std_msgs/ColorRGBA.h> /*! * A class for spawning safety zones in the Gazebo simulator. @@ -19,7 +20,7 @@ class GazeboZoneSpawner { public: /** - * @brief Spawns a box. + * @brief Spawns a purely visual box that ignores physics and is ignored by collision detection. * * The box is based on a prototype located in the package at `models/box.sdf`: * ```xml @@ -49,17 +50,82 @@ public: * * 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. + * Currently, 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. For future extensions, a `color` parameter is + * provided. * + * @param name The name of the box, which must be unique within gazebo. * @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. + * @param color The color of the box. This parameter is currently ignored. */ - static void spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose); + static void spawnVisualPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, std_msgs::ColorRGBA color = std_msgs::ColorRGBA()); - static void spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, double mass = 1.0, bool is_static = false); + /** + * @brief Spawns a box with physical properties. + * + * 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>false</static> + * <link name="link"> + * <pose>2 3 0.5 0 0 0</pose> + * <inertial> + * <mass>1.0</mass> + * <inertia> + * <ixx>0.083</ixx> <!-- for a box: ixx = 0.083 * mass * (y*y + z*z) --> + * <ixy>0.0</ixy> <!-- for a box: ixy = 0 --> + * <ixz>0.0</ixz> <!-- for a box: ixz = 0 --> + * <iyy>0.083</iyy> <!-- for a box: iyy = 0.083 * mass * (x*x + z*z) --> + * <iyz>0.0</iyz> <!-- for a box: iyz = 0 --> + * <izz>0.083</izz> <!-- for a box: izz = 0.083 * mass * (x*x + y*y) --> + * </inertia> + * </inertial> + * <visual name="visual"> + * <material> + * <script> + * <uri>box.material</uri> + * <name>Zone/DarkMagentaTransparent</name> + * </script> + * </material> + * <geometry> + * <box> + * <size>1 1 1</size> + * </box> + * </geometry> + * </visual> + * <collision name="collision"> + * <geometry> + * <box> + * <size>1 1 1</size> + * </box> + * </geometry> + * </collision> + * </link> + * </model> + * </sdf> + * ``` + * + * Before the box is spawned, its pose, inertial matrix, mass, and size are adapted to the method parameters. + * + * Currently, 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. For future extensions, a `color` parameter is + * provided. + * + * @param name The name of the box, which must be unique within gazebo. + * @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. + * @param color The color of the box. This parameter is currently ignored. + * @param mass The weight of the box in kg. Irrelevant, if `is_static` is set to true. + * @param is_static True, if the box should be ignored by physical forces (such as gravity). It is still considered for collisions. + */ + static void spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, std_msgs::ColorRGBA = std_msgs::ColorRGBA(), double mass = 1.0, bool is_static = false); }; diff --git a/models/hardbox.sdf b/models/physical_box.sdf similarity index 100% rename from models/hardbox.sdf rename to models/physical_box.sdf diff --git a/src/GazeboZoneSpawner.cpp b/src/GazeboZoneSpawner.cpp index 265a5d8..378c9f3 100644 --- a/src/GazeboZoneSpawner.cpp +++ b/src/GazeboZoneSpawner.cpp @@ -12,7 +12,7 @@ #include <ros/package.h> #include <tf/tf.h> -void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose) { +void GazeboZoneSpawner::spawnVisualPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, std_msgs::ColorRGBA color) { if (shape.type != shape.BOX) { ROS_ERROR_STREAM("Safety-zone could not be created due to wrong shape-type"); return; @@ -70,7 +70,7 @@ void GazeboZoneSpawner::spawnCollisionBox(shape_msgs::SolidPrimitive shape, geom } } -void GazeboZoneSpawner::spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, double mass, bool is_static) { +void GazeboZoneSpawner::spawnPrimitive(const std::string &name, shape_msgs::SolidPrimitive shape, geometry_msgs::Pose pose, std_msgs::ColorRGBA, double mass, bool is_static) { if (shape.type != shape.BOX) { ROS_ERROR_STREAM("Safety-zone could not be created due to wrong shape-type"); return; @@ -79,7 +79,7 @@ void GazeboZoneSpawner::spawnPrimitive(const std::string &name, shape_msgs::Soli sdf::SDFPtr sdf(new sdf::SDF()); sdf::init(sdf); - auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/hardbox.sdf"; + auto sdfFileName = ros::package::getPath(ROS_PACKAGE_NAME) + "/models/physical_box.sdf"; auto materialFileName = "file://" + ros::package::getPath(ROS_PACKAGE_NAME) + "/models/box.material"; assert(sdf::readFile(sdfFileName, sdf)); -- GitLab