diff --git a/ch7_pkg/launch/ch7_1.launch b/ch7_pkg/launch/ch7_1.launch index e927de1..c0da587 100644 --- a/ch7_pkg/launch/ch7_1.launch +++ b/ch7_pkg/launch/ch7_1.launch @@ -15,11 +15,11 @@ - + - + diff --git a/costmap_prohibition_layer/.gitignore b/costmap_prohibition_layer/.gitignore new file mode 100644 index 0000000..f8bcd11 --- /dev/null +++ b/costmap_prohibition_layer/.gitignore @@ -0,0 +1,47 @@ +build/ +bin/ +lib/ +msg_gen/ +srv_gen/ +msg/*Action.msg +msg/*ActionFeedback.msg +msg/*ActionGoal.msg +msg/*ActionResult.msg +msg/*Feedback.msg +msg/*Goal.msg +msg/*Result.msg +msg/_*.py + +# Generated by dynamic reconfigure +*.cfgc +/cfg/cpp/ +/cfg/*.py + +# Ignore generated docs +*.dox +*.wikidoc + +# eclipse stuff +.project +.cproject + +# qcreator stuff +CMakeLists.txt.user + +srv/_*.py +*.pcd +*.pyc +qtcreator-* +*.user + +/planning/cfg +/planning/docs +/planning/src + +*~ + +# Emacs +.#* + +# Catkin custom files +CATKIN_IGNORE diff --git a/costmap_prohibition_layer/.travis.yml b/costmap_prohibition_layer/.travis.yml new file mode 100644 index 0000000..19562b8 --- /dev/null +++ b/costmap_prohibition_layer/.travis.yml @@ -0,0 +1,107 @@ +# Generic .travis.yml file for running continuous integration on Travis-CI with +# any ROS package. +# +# This installs ROS on a clean Travis-CI virtual machine, creates a ROS +# workspace, resolves all listed dependencies, and sets environment variables +# (setup.bash). Then, it compiles the entire ROS workspace (ensuring there are +# no compilation errors), and runs all the tests. If any of the compilation/test +# phases fail, the build is marked as a failure. +# +# We handle two types of package dependencies: +# - packages (ros and otherwise) available through apt-get. These are installed +# using rosdep, based on the information in the ROS package.xml. +# - dependencies that must be checked out from source. These are handled by +# 'wstool', and should be listed in a file named dependencies.rosinstall. +# +# There are two variables you may want to change: +# - ROS_DISTRO (default is indigo). Note that packages must be available for +# ubuntu 14.04 trusty. +# - ROSINSTALL_FILE (default is dependencies.rosinstall inside the repo +# root). This should list all necessary repositories in wstool format (see +# the ros wiki). If the file does not exists then nothing happens. +# +# See the README.md for more information. +# +# Author: Felix Duvallet + +# NOTE: The build lifecycle on Travis.ci is something like this: +# before_install +# install +# before_script +# script +# after_success or after_failure +# after_script +# OPTIONAL before_deploy +# OPTIONAL deploy +# OPTIONAL after_deploy + +################################################################################ + +# Use ubuntu trusty (14.04) with sudo privileges. +dist: trusty +sudo: required +language: + - generic +cache: + - apt + +# Configuration variables. All variables are global now, but this can be used to +# trigger a build matrix for different ROS distributions if desired. +env: + global: + - ROS_CI_DESKTOP="`lsb_release -cs`" # e.g. [precise|trusty|...] + - CI_SOURCE_PATH=$(pwd) + - ROSINSTALL_FILE=$CI_SOURCE_PATH/dependencies.rosinstall + - CATKIN_OPTIONS=$CI_SOURCE_PATH/catkin.options + - ROS_PARALLEL_JOBS='-j8 -l6' + matrix: + - ROS_DISTRO=jade + + +################################################################################ + +# Install system dependencies, namely a very barebones ROS setup. +before_install: + - sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list" + - wget http://packages.ros.org/ros.key -O - | sudo apt-key add - + - sudo apt-get update -qq + - sudo apt-get install -y python-catkin-pkg python-rosdep python-wstool ros-$ROS_DISTRO-catkin + - source /opt/ros/$ROS_DISTRO/setup.bash + # Prepare rosdep to install dependencies. + - sudo rosdep init + - rosdep update + +# Create a catkin workspace with the package under integration. +install: + - mkdir -p ~/catkin_ws/src + - cd ~/catkin_ws/src + - catkin_init_workspace + # Create the devel/setup.bash (run catkin_make with an empty workspace) and + # source it to set the path variables. + - cd ~/catkin_ws + - catkin_make + - source devel/setup.bash + # Add the package under integration to the workspace using a symlink. + - cd ~/catkin_ws/src + - ln -s $CI_SOURCE_PATH . + +# Install all dependencies, using wstool and rosdep. +# wstool looks for a ROSINSTALL_FILE defined in the environment variables. +before_script: + # source dependencies: install using wstool. + - cd ~/catkin_ws/src + - wstool init + - if [[ -f $ROSINSTALL_FILE ]] ; then wstool merge $ROSINSTALL_FILE ; fi + - wstool up + # package depdencies: install using rosdep. + - cd ~/catkin_ws + - rosdep install -y --from-paths src --ignore-src --rosdistro $ROS_DISTRO + +# Compile and test. If the CATKIN_OPTIONS file exists, use it as an argument to +# catkin_make. +script: + - cd ~/catkin_ws + - catkin_make $( [ -f $CATKIN_OPTIONS ] && cat $CATKIN_OPTIONS ) + # Testing: Use both run_tests (to see the output) and test (to error out). + - catkin_make run_tests # This always returns 0, but looks pretty. + - catkin_make test # This will return non-zero if a test fails. diff --git a/costmap_prohibition_layer/CHANGELOG.rst b/costmap_prohibition_layer/CHANGELOG.rst new file mode 100644 index 0000000..87dd510 --- /dev/null +++ b/costmap_prohibition_layer/CHANGELOG.rst @@ -0,0 +1,21 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package costmap_prohibition_layer +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.5 (2017-01-25) +------------------ +* Support a rolling window map + Enable support for rolling window maps + +0.0.4 (2016-10-30) +------------------ +* Enable recognizing Integer values in single points +* restored deleted line + +0.0.3 (2016-10-28) +------------------ +* Install scripts added + +0.0.2 (2016-10-27) +------------------ +* Initial package version diff --git a/costmap_prohibition_layer/CMakeLists.txt b/costmap_prohibition_layer/CMakeLists.txt new file mode 100755 index 0000000..a306a5d --- /dev/null +++ b/costmap_prohibition_layer/CMakeLists.txt @@ -0,0 +1,188 @@ +cmake_minimum_required(VERSION 2.8.3) +project(costmap_prohibition_layer) + +## 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 + costmap_2d + dynamic_reconfigure + roscpp +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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() + +## Enable C++11 support +include(CheckCXXCompilerFlag) +CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11) +CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) +if(COMPILER_SUPPORTS_CXX11) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") +elseif(COMPILER_SUPPORTS_CXX0X) + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") +else() + message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.") +endif() + +################################################ +## 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 run_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 run_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 run_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/CostmapProhibitionLayer.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 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 costmap_prohibition_layer + CATKIN_DEPENDS costmap_2d dynamic_reconfigure + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include) +include_directories(${catkin_INCLUDE_DIRS} include) + +## Declare a C++ library + +add_library(costmap_prohibition_layer src/costmap_prohibition_layer.cpp) +target_link_libraries(costmap_prohibition_layer ${catkin_LIBRARIES}) + +# Dynamic reconfigure: make sure configure headers are built before any node using them +add_dependencies(costmap_prohibition_layer ${PROJECT_NAME}_gencfg) + +############ +## 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 costmap_prohibition_layer + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_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 + costmap_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +install(DIRECTORY + cfg + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + PATTERN ".svn" EXCLUDE +) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_simple_layers.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) + diff --git a/costmap_prohibition_layer/LICENSE b/costmap_prohibition_layer/LICENSE new file mode 100644 index 0000000..861fccb --- /dev/null +++ b/costmap_prohibition_layer/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2016, TU Dortmund - Lehrstuhl RST +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +* Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +* Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +* Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/costmap_prohibition_layer/README.md b/costmap_prohibition_layer/README.md new file mode 100644 index 0000000..701d61e --- /dev/null +++ b/costmap_prohibition_layer/README.md @@ -0,0 +1,6 @@ +# costmap_prohibition_layer +ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration. + +Build status of the *kinetic-devel* branch: +- Travis (Ubuntu Trusty): [![Build Status](https://travis-ci.org/rst-tu-dortmund/costmap_prohibition_layer.svg?branch=kinetic-devel)](https://travis-ci.org/rst-tu-dortmund/costmap_prohibition_layer) + diff --git a/costmap_prohibition_layer/cfg/CostmapProhibitionLayer.cfg b/costmap_prohibition_layer/cfg/CostmapProhibitionLayer.cfg new file mode 100755 index 0000000..11b028c --- /dev/null +++ b/costmap_prohibition_layer/cfg/CostmapProhibitionLayer.cfg @@ -0,0 +1,16 @@ +#!/usr/bin/env python + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +# For integers and doubles: +# Name Type Reconfiguration level +# Description +# Default Min Max + + +gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True) +gen.add("fill_polygons", bool_t, 0, "Whether to fill polygon cells or not", True) + +exit(gen.generate("costmap_prohibition_layer_namespace", "costmap_prohibition_layer_namespace", "CostmapProhibitionLayer")) diff --git a/costmap_prohibition_layer/costmap_plugins.xml b/costmap_prohibition_layer/costmap_plugins.xml new file mode 100755 index 0000000..c1f3fa9 --- /dev/null +++ b/costmap_prohibition_layer/costmap_plugins.xml @@ -0,0 +1,5 @@ + + + ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration. + + diff --git a/costmap_prohibition_layer/include/costmap_prohibition_layer/costmap_prohibition_layer.h b/costmap_prohibition_layer/include/costmap_prohibition_layer/costmap_prohibition_layer.h new file mode 100644 index 0000000..c80a185 --- /dev/null +++ b/costmap_prohibition_layer/include/costmap_prohibition_layer/costmap_prohibition_layer.h @@ -0,0 +1,205 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Stephan Kurzawe + *********************************************************************/ +#ifndef COSTMAP_PROHIBITION_LAYER_H_ +#define COSTMAP_PROHIBITION_LAYER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace costmap_prohibition_layer_namespace +{ + +// point with integer coordinates +struct PointInt +{ + int x; + int y; +}; + +class CostmapProhibitionLayer : public costmap_2d::Layer +{ +public: + + /** + * default constructor + */ + CostmapProhibitionLayer(); + + /** + * destructor + */ + virtual ~CostmapProhibitionLayer(); + /** + * function which get called at initializing the costmap + * define the reconfige callback, get the reoslution + * and read the prohibitions from the ros-parameter server + */ + virtual void onInitialize(); + + /** + * This is called by the LayeredCostmap to poll this plugin + * as to how much of the costmap it needs to update. + * Each layer can increase the size of this bounds. + */ + virtual void updateBounds(double robot_x, double robot_y, double robot_yaw, + double *min_x, double *min_y, double *max_x, double *max_y); + + /** + * function which get called at every cost updating procdure + * of the overlayed costmap. The before readed costs will get + * filled + */ + virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, + int max_i, int max_j); + +private: + + /** + * overlayed reconfigure callback function + */ + void reconfigureCB(CostmapProhibitionLayerConfig& config, uint32_t level); + + /** + * Compute bounds in world coordinates for the current set of points and polygons. + * The result is stored in class members _min_x, _min_y, _max_x and _max_y. + */ + void computeMapBounds(); + + /** + * Set cost in a Costmap2D for a polygon (polygon may be located outside bounds) + * + * @param master_grid reference to the Costmap2D object + * @param polygon polygon defined by a vector of points (in world coordinates) + * @param cost the cost value to be set (0,255) + * @param min_i minimum bound on the horizontal map index/coordinate + * @param min_j minimum bound on the vertical map index/coordinate + * @param max_i maximum bound on the horizontal map index/coordinate + * @param max_j maximum bound on the vertical map index/coordinate + * @param fill_polygon if true, tue cost for the interior of the polygon will be set as well + */ + void setPolygonCost(costmap_2d::Costmap2D &master_grid, const std::vector& polygon, + unsigned char cost, int min_i, int min_j, int max_i, int max_j, bool fill_polygon); + + /** + * Convert polygon (in map coordinates) to a set of cells in the map + * + * @remarks This method is mainly based on Costmap2D::convexFillCells() but accounts + * for a self-implemented polygonOutlineCells() method and allows negative map coordinates + * + * @param polygon polygon defined by a vector of map coordinates + * @param[out] polygon_cells new cells in map coordinates are pushed back on this container + * @param fill if true, the interior of the polygon will be considered as well + */ + void rasterizePolygon(const std::vector& polygon, std::vector& polygon_cells, bool fill); + + /** + * Extract the boundary of a polygon in terms of map cells + * + * @remarks This method is based on Costmap2D::polygonOutlineCells() but accounts + * for a self-implemented raytrace algorithm and allows negative map coordinates + * + * @param polygon polygon defined by a vector of map coordinates + * @param[out] polygon_cells new cells in map coordinates are pushed back on this container + */ + void polygonOutlineCells(const std::vector& polygon, std::vector& polygon_cells); + + /** + * Rasterize line between two map coordinates into a set of cells + * + * @remarks Since Costmap2D::raytraceLine() is based on the size_x and since we want to rasterize + * polygons that might also be located outside map bounds we provide a modified raytrace + * implementation (also Bresenham) based on the integer version presented here: + * http://playtechs.blogspot.de/2007/03/raytracing-on-grid.html + * + * @param x0 line start x-coordinate (map frame) + * @param y0 line start y-coordinate (map frame) + * @param x1 line end x-coordinate (map frame) + * @param y1 line end y-coordinate (map frame) + * @param[out] cells new cells in map coordinates are pushed back on this container + */ + void raytrace(int x0, int y0, int x1, int y1, std::vector& cells); + + + /** + * read the prohibition areas in YAML-Format from the + * ROS parameter server in the namespace of this + * plugin + * e.g. /move_base/global_costmap/prohibition_layer/param + * + * @param nhandle pointer to the ros-Node handle + * @param param name of the parameter where the + * prohibition areas saved in YAML format + * + * @return bool true if the parsing was successful + * false if it wasn't + */ + bool parseProhibitionListFromYaml(ros::NodeHandle* nhandle, const std::string& param); + + /** + * get a geometry_msgs::Point from a YAML-Array + * The z-coordinate get always written to zero! + * + * @param val YAML-array with to point-coordinates (x and y) + * @param point variable where the determined point get saved + * + * @return bool true if the determining was successful + * false if it wasn't + */ + bool getPoint(XmlRpc::XmlRpcValue& val, geometry_msgs::Point& point); + + dynamic_reconfigure::Server* _dsrv; //!< dynamic_reconfigure server for the costmap + std::mutex _data_mutex; //!< mutex for the accessing _prohibition_points and _prohibition_polygons + double _costmap_resolution; //!< resolution of the overlayed costmap to create the thinnest line out of two points + bool _fill_polygons; //!< if true, all cells that are located in the interior of polygons are marked as obstacle as well + std::vector _prohibition_points; //!< vector to save the lonely points in source coordinates + std::vector> _prohibition_polygons; //!< vector to save the polygons (including lines) in source coordinates + double _min_x, _min_y, _max_x, _max_y; //!< cached map bounds +}; +} +#endif diff --git a/costmap_prohibition_layer/package.xml b/costmap_prohibition_layer/package.xml new file mode 100755 index 0000000..04062bb --- /dev/null +++ b/costmap_prohibition_layer/package.xml @@ -0,0 +1,31 @@ + + + costmap_prohibition_layer + 0.0.5 + ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration. + + Stephan Kurzawe + + BSD + + + + + Stephan Kurzawe + + + + catkin + costmap_2d + dynamic_reconfigure + roscpp + costmap_2d + dynamic_reconfigure + roscpp + + + + + + + diff --git a/costmap_prohibition_layer/src/costmap_prohibition_layer.cpp b/costmap_prohibition_layer/src/costmap_prohibition_layer.cpp new file mode 100644 index 0000000..4b298a7 --- /dev/null +++ b/costmap_prohibition_layer/src/costmap_prohibition_layer.cpp @@ -0,0 +1,474 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2016, + * TU Dortmund - Institute of Control Theory and Systems Engineering. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the institute nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Stephan Kurzawe + *********************************************************************/ + +#include +#include + +PLUGINLIB_EXPORT_CLASS(costmap_prohibition_layer_namespace::CostmapProhibitionLayer, costmap_2d::Layer) + +using costmap_2d::LETHAL_OBSTACLE; + +namespace costmap_prohibition_layer_namespace +{ + +CostmapProhibitionLayer::CostmapProhibitionLayer() : _dsrv(NULL) +{ +} + +CostmapProhibitionLayer::~CostmapProhibitionLayer() +{ + if (_dsrv!=NULL) + delete _dsrv; +} + +void CostmapProhibitionLayer::onInitialize() +{ + ros::NodeHandle nh("~/" + name_); + current_ = true; + + _dsrv = new dynamic_reconfigure::Server(nh); + dynamic_reconfigure::Server::CallbackType cb = + boost::bind(&CostmapProhibitionLayer::reconfigureCB, this, _1, _2); + _dsrv->setCallback(cb); + + // get a pointer to the layered costmap and save resolution + costmap_2d::Costmap2D *costmap = layered_costmap_->getCostmap(); + _costmap_resolution = costmap->getResolution(); + + // set initial bounds + _min_x = _min_y = _max_x = _max_y = 0; + + // reading the prohibition areas out of the namespace of this plugin! + // e.g.: "move_base/global_costmap/prohibition_layer/prohibition_areas" + std::string params = "prohibition_areas"; + if (!parseProhibitionListFromYaml(&nh, params)) + ROS_ERROR_STREAM("Reading prohibition areas from '" << nh.getNamespace() << "/" << params << "' failed!"); + + _fill_polygons = true; + nh.param("fill_polygons", _fill_polygons, _fill_polygons); + + // compute map bounds for the current set of prohibition areas. + computeMapBounds(); + + ROS_INFO("CostmapProhibitionLayer initialized."); +} + +void CostmapProhibitionLayer::reconfigureCB(CostmapProhibitionLayerConfig &config, uint32_t level) +{ + enabled_ = config.enabled; + _fill_polygons = config.fill_polygons; +} + + +void CostmapProhibitionLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, + double *min_x, double *min_y, double *max_x, double *max_y) +{ + if (!enabled_) + return; + + std::lock_guard l(_data_mutex); + + if (_prohibition_points.empty() && _prohibition_polygons.empty()) + return; + + *min_x = std::min(*min_x, _min_x); + *min_y = std::min(*min_y, _min_y); + *max_x = std::max(*max_x, _max_x); + *max_y = std::max(*max_y, _max_y); + +} + +void CostmapProhibitionLayer::updateCosts(costmap_2d::Costmap2D &master_grid, int min_i, int min_j, int max_i, int max_j) +{ + if (!enabled_) + return; + + std::lock_guard l(_data_mutex); + + // set costs of polygons + for (int i = 0; i < _prohibition_polygons.size(); ++i) + { + setPolygonCost(master_grid, _prohibition_polygons[i], LETHAL_OBSTACLE, min_i, min_j, max_i, max_j, _fill_polygons); + } + + // set cost of points + for (int i = 0; i < _prohibition_points.size(); ++i) + { + unsigned int mx; + unsigned int my; + if (master_grid.worldToMap(_prohibition_points[i].x, _prohibition_points[i].y, mx, my)) + { + master_grid.setCost(mx, my, LETHAL_OBSTACLE); + } + } +} + +void CostmapProhibitionLayer::computeMapBounds() +{ + std::lock_guard l(_data_mutex); + + // reset bounds + _min_x = _min_y = _max_x = _max_y = 0; + + // iterate polygons + for (int i = 0; i < _prohibition_polygons.size(); ++i) + { + for (int j=0; j < _prohibition_polygons.at(i).size(); ++j) + { + double px = _prohibition_polygons.at(i).at(j).x; + double py = _prohibition_polygons.at(i).at(j).y; + _min_x = std::min(px, _min_x); + _min_y = std::min(py, _min_y); + _max_x = std::max(px, _max_x); + _max_y = std::max(py, _max_y); + } + } + + // iterate points + for (int i = 0; i < _prohibition_points.size(); ++i) + { + double px = _prohibition_points.at(i).x; + double py = _prohibition_points.at(i).y; + _min_x = std::min(px, _min_x); + _min_y = std::min(py, _min_y); + _max_x = std::max(px, _max_x); + _max_y = std::max(py, _max_y); + } +} + + +void CostmapProhibitionLayer::setPolygonCost(costmap_2d::Costmap2D &master_grid, const std::vector& polygon, unsigned char cost, + int min_i, int min_j, int max_i, int max_j, bool fill_polygon) +{ + std::vector map_polygon; + for (unsigned int i = 0; i < polygon.size(); ++i) + { + PointInt loc; + master_grid.worldToMapNoBounds(polygon[i].x, polygon[i].y, loc.x, loc.y); + map_polygon.push_back(loc); + } + + std::vector polygon_cells; + + // get the cells that fill the polygon + rasterizePolygon(map_polygon, polygon_cells, fill_polygon); + + // set the cost of those cells + for (unsigned int i = 0; i < polygon_cells.size(); ++i) + { + int mx = polygon_cells[i].x; + int my = polygon_cells[i].y; + // check if point is outside bounds + if (mx < min_i || mx >= max_i) + continue; + if (my < min_j || my >= max_j) + continue; + master_grid.setCost(mx, my, cost); + } +} + + +void CostmapProhibitionLayer::polygonOutlineCells(const std::vector& polygon, std::vector& polygon_cells) + { + for (unsigned int i = 0; i < polygon.size() - 1; ++i) + { + raytrace(polygon[i].x, polygon[i].y, polygon[i + 1].x, polygon[i + 1].y, polygon_cells); + } + if (!polygon.empty()) + { + unsigned int last_index = polygon.size() - 1; + // we also need to close the polygon by going from the last point to the first + raytrace(polygon[last_index].x, polygon[last_index].y, polygon[0].x, polygon[0].y, polygon_cells); + } + } + +void CostmapProhibitionLayer::raytrace(int x0, int y0, int x1, int y1, std::vector& cells) +{ + int dx = abs(x1 - x0); + int dy = abs(y1 - y0); + PointInt pt; + pt.x = x0; + pt.y = y0; + int n = 1 + dx + dy; + int x_inc = (x1 > x0) ? 1 : -1; + int y_inc = (y1 > y0) ? 1 : -1; + int error = dx - dy; + dx *= 2; + dy *= 2; + + for (; n > 0; --n) + { + cells.push_back(pt); + + if (error > 0) + { + pt.x += x_inc; + error -= dy; + } + else + { + pt.y += y_inc; + error += dx; + } + } +} + + +void CostmapProhibitionLayer::rasterizePolygon(const std::vector& polygon, std::vector& polygon_cells, bool fill) +{ + // this implementation is a slighly modified version of Costmap2D::convexFillCells(...) + + //we need a minimum polygon of a traingle + if(polygon.size() < 3) + return; + + //first get the cells that make up the outline of the polygon + polygonOutlineCells(polygon, polygon_cells); + + if (!fill) + return; + + //quick bubble sort to sort points by x + PointInt swap; + unsigned int i = 0; + while(i < polygon_cells.size() - 1) + { + if(polygon_cells[i].x > polygon_cells[i + 1].x) + { + swap = polygon_cells[i]; + polygon_cells[i] = polygon_cells[i + 1]; + polygon_cells[i + 1] = swap; + + if(i > 0) + --i; + } + else + ++i; + } + + i = 0; + PointInt min_pt; + PointInt max_pt; + int min_x = polygon_cells[0].x; + int max_x = polygon_cells[(int)polygon_cells.size() -1].x; + + //walk through each column and mark cells inside the polygon + for(int x = min_x; x <= max_x; ++x) + { + if(i >= (int)polygon_cells.size() - 1) + break; + + if(polygon_cells[i].y < polygon_cells[i + 1].y) + { + min_pt = polygon_cells[i]; + max_pt = polygon_cells[i + 1]; + } + else + { + min_pt = polygon_cells[i + 1]; + max_pt = polygon_cells[i]; + } + + i += 2; + while(i < polygon_cells.size() && polygon_cells[i].x == x) + { + if(polygon_cells[i].y < min_pt.y) + min_pt = polygon_cells[i]; + else if(polygon_cells[i].y > max_pt.y) + max_pt = polygon_cells[i]; + ++i; + } + + PointInt pt; + //loop though cells in the column + for(int y = min_pt.y; y < max_pt.y; ++y) + { + pt.x = x; + pt.y = y; + polygon_cells.push_back(pt); + } + } + } + +// load prohibition positions out of the rosparam server +bool CostmapProhibitionLayer::parseProhibitionListFromYaml(ros::NodeHandle *nhandle, const std::string ¶m) +{ + std::lock_guard l(_data_mutex); + std::unordered_map map_out; + + XmlRpc::XmlRpcValue param_yaml; + + bool ret_val = true; + + if (nhandle->getParam(param, param_yaml)) + { + if (param_yaml.getType() == XmlRpc::XmlRpcValue::TypeArray) // list of goals + { + for (int i = 0; i < param_yaml.size(); ++i) + { + if (param_yaml[i].getType() == XmlRpc::XmlRpcValue::TypeArray) + { + std::vector vector_to_add; + + /* ************************************** + * differ between points and polygons + * lines get to a polygon with the resolution + * of the costmap + **************************************** */ + + // add a point + if (param_yaml[i].size() == 1) + { + geometry_msgs::Point point; + ret_val = getPoint(param_yaml[i][0], point); + _prohibition_points.push_back(point); + } + // add a line + else if (param_yaml[i].size() == 2) + { + if (param_yaml[i][0].getType() == XmlRpc::XmlRpcValue::TypeDouble || + param_yaml[i][0].getType() == XmlRpc::XmlRpcValue::TypeInt) + { + // add a lonely point + geometry_msgs::Point point; + ret_val = getPoint(param_yaml[i], point); + _prohibition_points.push_back(point); + } + else + { + // add a line! + geometry_msgs::Point point_A; + ret_val = getPoint(param_yaml[i][0], point_A); + vector_to_add.push_back(point_A); + + geometry_msgs::Point point_B; + ret_val = getPoint(param_yaml[i][1], point_B); + vector_to_add.push_back(point_B); + + // calculate the normal vector for AB + geometry_msgs::Point point_N; + point_N.x = point_B.y - point_A.y; + point_N.y = point_A.x - point_B.x; + + // get the absolute value of N to normalize and get + // it to the length of the costmap resolution + double abs_N = sqrt(pow(point_N.x, 2) + pow(point_N.y, 2)); + point_N.x = point_N.x / abs_N * _costmap_resolution; + point_N.y = point_N.y / abs_N * _costmap_resolution; + + // calculate the new points to get a polygon which can be filled + geometry_msgs::Point point; + point.x = point_A.x + point_N.x; + point.y = point_A.y + point_N.y; + vector_to_add.push_back(point); + + point.x = point_B.x + point_N.x; + point.y = point_B.y + point_N.y; + vector_to_add.push_back(point); + + _prohibition_polygons.push_back(vector_to_add); + } + } + // add a point or add a polygon + else if (param_yaml[i].size() >= 3) + { + // add a polygon with any number of points + for (int j = 0; j < param_yaml[i].size(); ++j) + { + geometry_msgs::Point point; + ret_val = getPoint(param_yaml[i][j], point); + vector_to_add.push_back(point); + } + _prohibition_polygons.push_back(vector_to_add); + } + } + else + { + ROS_ERROR_STREAM("Prohibition Layer:" << param << " with index " << i << " is not correct."); + ret_val = false; + } + } + } + else + { + ROS_ERROR_STREAM("Prohibition Layer: " << param << "struct is not correct."); + ret_val = false; + } + } + else + { + ROS_ERROR_STREAM("Prohibition Layer: Cannot read " << param << " from parameter server"); + ret_val = false; + } + return ret_val; +} + +// get a point out of the XML Type into a geometry_msgs::Point +bool CostmapProhibitionLayer::getPoint(XmlRpc::XmlRpcValue &val, geometry_msgs::Point &point) +{ + try + { + // check if there a two values for the coordinate + if (val.getType() == XmlRpc::XmlRpcValue::TypeArray && val.size() == 2) + { + auto convDouble = [](XmlRpc::XmlRpcValue &val) -> double + { + if (val.getType() == XmlRpc::XmlRpcValue::TypeInt) // XmlRpc cannot cast int to double + return int(val); + return val; // if not double, an exception is thrown; + }; + + point.x = convDouble(val[0]); + point.y = convDouble(val[1]); + point.z = 0.0; + return true; + } + else + { + ROS_ERROR_STREAM("Prohibition_Layer: A point has to consist two values!"); + return false; + } + } + catch (const XmlRpc::XmlRpcException &ex) + { + ROS_ERROR_STREAM("Prohibition Layer: Cannot add current point: " << ex.getMessage()); + return false; + } +} + +} // end namespace diff --git a/dodishes_pkg/CMakeLists.txt b/dodishes_pkg/CMakeLists.txt new file mode 100644 index 0000000..5b8ee2f --- /dev/null +++ b/dodishes_pkg/CMakeLists.txt @@ -0,0 +1,219 @@ +cmake_minimum_required(VERSION 3.0.2) +project(dodishes_pkg) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + actionlib + actionlib_msgs + message_generation + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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 +# ) + +add_action_files( + DIRECTORY action + FILES dodishes.action) + +generate_messages( + DEPENDENCIES actionlib_msgs std_msgs ) + +catkin_package( + CATKIN_DEPENDS actionlib_msgs) + +## Generate added messages and services with any dependencies listed here +# generate_messages( +# DEPENDENCIES +# actionlib_msgs# std_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 dodishes_pkg +# CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/dodishes_pkg.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(${PROJECT_NAME} ${${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/dodishes_pkg_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(${PROJECT_NAME}_node +# ${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 +# 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_dodishes_pkg.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) diff --git a/dodishes_pkg/action/dodishes.action b/dodishes_pkg/action/dodishes.action new file mode 100644 index 0000000..9391a84 --- /dev/null +++ b/dodishes_pkg/action/dodishes.action @@ -0,0 +1,8 @@ +# Define the goal +uint32 dishwasher_id # Specify which dishwasher we want to use +--- +# Define the result +uint32 total_dishes_cleaned +--- +# Define a feedback message +float32 percent_complete diff --git a/dodishes_pkg/package.xml b/dodishes_pkg/package.xml new file mode 100644 index 0000000..4e25a60 --- /dev/null +++ b/dodishes_pkg/package.xml @@ -0,0 +1,76 @@ + + + dodishes_pkg + 0.0.0 + The dodishes_pkg package + + + + + joy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + actionlib + actionlib_msgs + message_generation + roscpp + rospy + std_msgs + actionlib + actionlib_msgs + roscpp + rospy + std_msgs + actionlib + actionlib_msgs + roscpp + rospy + std_msgs + message_generation + + + + + + + + diff --git a/my_actionlib_tutorials/CMakeLists.txt b/my_actionlib_tutorials/CMakeLists.txt new file mode 100644 index 0000000..214ecce --- /dev/null +++ b/my_actionlib_tutorials/CMakeLists.txt @@ -0,0 +1,215 @@ +cmake_minimum_required(VERSION 3.0.2) +project(my_actionlib_tutorials) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + actionlib + actionlib_msgs + message_generation + roscpp + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) +add_action_files( + DIRECTORY action + FILES Fibonacci.action) +generate_messages( +DEPENDENCIES actionlib_msgs std_msgs ) +catkin_package( + CATKIN_DEPENDS actionlib_msgs) + +## 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 +# actionlib_msgs# std_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 my_actionlib_tutorials +# CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/my_actionlib_tutorials.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(${PROJECT_NAME} ${${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/my_actionlib_tutorials_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(${PROJECT_NAME}_node +# ${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 +# 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_my_actionlib_tutorials.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) diff --git a/my_actionlib_tutorials/action/Fibonacci.action b/my_actionlib_tutorials/action/Fibonacci.action new file mode 100644 index 0000000..9065708 --- /dev/null +++ b/my_actionlib_tutorials/action/Fibonacci.action @@ -0,0 +1,8 @@ +#goal definition +int32 order +--- +#result definition +int32[] sequence +--- +#feedback +int32[] sequence \ No newline at end of file diff --git a/my_actionlib_tutorials/package.xml b/my_actionlib_tutorials/package.xml new file mode 100644 index 0000000..babeeaf --- /dev/null +++ b/my_actionlib_tutorials/package.xml @@ -0,0 +1,75 @@ + + + my_actionlib_tutorials + 0.0.0 + The my_actionlib_tutorials package + + + + + joy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + actionlib + actionlib_msgs + message_generation + roscpp + rospy + std_msgs + actionlib + actionlib_msgs + roscpp + rospy + std_msgs + actionlib + actionlib_msgs + roscpp + rospy + std_msgs + message_generation + + + + + + + diff --git a/my_actionlib_tutorials/src/fibonacci_server.cpp b/my_actionlib_tutorials/src/fibonacci_server.cpp new file mode 100644 index 0000000..0568a41 --- /dev/null +++ b/my_actionlib_tutorials/src/fibonacci_server.cpp @@ -0,0 +1,55 @@ +1#include + +2#include + +3#include + +4 + +5 class FibonacciAction + +6 { + +7 protected: + +8 + +9 ros::NodeHandle nh ; + +10 actionlib::SimpleActionServer as_; line. Otherwise strange error occurs. + +11 std::string action_name_; + +12 // create messages that are used to published feedback/result + +13 my_actionlib_tutorials::FibonacciFeedback feedback_; + +my_actionlib_tutorials::FibonacciResult result_; 14 + +15 + +16 public: + +17 + +19 + +FibonacciAction(std::string name) : 18 + +as (nh, name, boost::bind(&FibonacciAction::executeCB, this, 1), false), + +action_name_(name) 20 + +twenty one { + +22 as .start(); + +twenty three } + +24 + +25 ~FibonacciAction(void) + +26 { + +27} \ No newline at end of file diff --git a/myrobot_pkg/CMakeLists.txt b/myrobot_pkg/CMakeLists.txt new file mode 100644 index 0000000..48b0bda --- /dev/null +++ b/myrobot_pkg/CMakeLists.txt @@ -0,0 +1,206 @@ +cmake_minimum_required(VERSION 3.0.2) +project(myrobot_pkg) + +## Compile as C++11, supported in ROS Kinetic and newer +# add_compile_options(-std=c++11) + +## 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 + rospy + std_msgs +) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## 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 +# ) + +################################################ +## 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 myrobot_pkg +# CATKIN_DEPENDS roscpp rospy std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( +# include + ${catkin_INCLUDE_DIRS} +) + +## Declare a C++ library +# add_library(${PROJECT_NAME} +# src/${PROJECT_NAME}/myrobot_pkg.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(${PROJECT_NAME} ${${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/myrobot_pkg_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(${PROJECT_NAME}_node +# ${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 +# 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_myrobot_pkg.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) diff --git a/myrobot_pkg/launch/robot.launch b/myrobot_pkg/launch/robot.launch new file mode 100644 index 0000000..bcb5759 --- /dev/null +++ b/myrobot_pkg/launch/robot.launch @@ -0,0 +1,10 @@ + + + + + + + + + + \ No newline at end of file diff --git a/myrobot_pkg/launch/xacro.launch b/myrobot_pkg/launch/xacro.launch new file mode 100644 index 0000000..ead621b --- /dev/null +++ b/myrobot_pkg/launch/xacro.launch @@ -0,0 +1,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/myrobot_pkg/marcos/robot.xacro b/myrobot_pkg/marcos/robot.xacro new file mode 100644 index 0000000..8a70717 --- /dev/null +++ b/myrobot_pkg/marcos/robot.xacro @@ -0,0 +1,220 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Red + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Green + + + + + + + + + + + + Gazebo/Orange + + + + + + + + + + + + + + + + + + + + + Gazebo/Gray + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Gray + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/myrobot_pkg/package.xml b/myrobot_pkg/package.xml new file mode 100644 index 0000000..38016b1 --- /dev/null +++ b/myrobot_pkg/package.xml @@ -0,0 +1,68 @@ + + + myrobot_pkg + 0.0.0 + The myrobot_pkg package + + + + + joy + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + roscpp + rospy + std_msgs + + + + + + + + diff --git a/myrobot_pkg/urdf/07-physics.urdf b/myrobot_pkg/urdf/07-physics.urdf new file mode 100644 index 0000000..a45efe6 --- /dev/null +++ b/myrobot_pkg/urdf/07-physics.urdf @@ -0,0 +1,456 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Red + + + Gazebo/Yellow + + + Gazebo/Yellow + + diff --git a/myrobot_pkg/urdf/dec4.urdf b/myrobot_pkg/urdf/dec4.urdf new file mode 100644 index 0000000..cea1b09 --- /dev/null +++ b/myrobot_pkg/urdf/dec4.urdf @@ -0,0 +1,456 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Red + + + Gazebo/Yellow + + + Gazebo/Yellow + + diff --git a/simple_layers/CMakeLists.txt b/simple_layers/CMakeLists.txt index 98ee7c3..b2f10a9 100644 --- a/simple_layers/CMakeLists.txt +++ b/simple_layers/CMakeLists.txt @@ -121,7 +121,6 @@ include_directories( # include ${catkin_INCLUDE_DIRS} ) -include_directories(include ${catkin_INCLUDE_DIRS}) add_library(simple_layer src/simple_layer.cpp) ## Declare a C++ library # add_library(${PROJECT_NAME} diff --git a/simple_layers/ostmap_plugins.xml b/simple_layers/costmap_plugins.xml similarity index 94% rename from simple_layers/ostmap_plugins.xml rename to simple_layers/costmap_plugins.xml index 692bc43..50ff0f6 100644 --- a/simple_layers/ostmap_plugins.xml +++ b/simple_layers/costmap_plugins.xml @@ -2,4 +2,4 @@ Demo Layer that adds a point 1 meter in front of the robot - + \ No newline at end of file diff --git a/simple_layers/include/simple_layers/simple_layer.h b/simple_layers/include/simple_layers/simple_layer.h index 99fa081..31a37ff 100644 --- a/simple_layers/include/simple_layers/simple_layer.h +++ b/simple_layers/include/simple_layers/simple_layer.h @@ -25,4 +25,4 @@ private: dynamic_reconfigure::Server *dsrv_; }; } -#endif +#endif \ No newline at end of file diff --git a/simple_layers/package.xml b/simple_layers/package.xml index 1769c1f..8d92b57 100644 --- a/simple_layers/package.xml +++ b/simple_layers/package.xml @@ -67,7 +67,7 @@ - - - + + + diff --git a/simple_layers/src/simple_layer.cpp b/simple_layers/src/simple_layer.cpp index ecb74bf..0c6e00b 100644 --- a/simple_layers/src/simple_layer.cpp +++ b/simple_layers/src/simple_layer.cpp @@ -1,4 +1,4 @@ -#include +#include "../include/simple_layers/simple_layer.h" #include PLUGINLIB_EXPORT_CLASS(simple_layer_namespace::SimpleLayer, costmap_2d::Layer) @@ -24,31 +24,38 @@ void SimpleLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_ { enabled_ = config.enabled; } - + void SimpleLayer::updateBounds(double origin_x, double origin_y, double origin_yaw, double* min_x, double* min_y, double* max_x, double* max_y) { if (!enabled_) return; - + mark_x_ = origin_x + cos(origin_yaw); mark_y_ = origin_y + sin(origin_yaw); - + *min_x = std::min(*min_x, mark_x_); *min_y = std::min(*min_y, mark_y_); *max_x = std::max(*max_x, mark_x_); *max_y = std::max(*max_y, mark_y_); } - + void SimpleLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) { if (!enabled_) return; unsigned int mx; unsigned int my; - if(master_grid.worldToMap(mark_x_, mark_y_, mx, my)){ - master_grid.setCost(mx, my, LETHAL_OBSTACLE); - } -} + // master_grid.worldToMap(-1, -1, mx, my); + master_grid.worldToMap(0.9, -0.75, mx, my); + master_grid.setCost(mx, my, LETHAL_OBSTACLE); -} // end namespace + master_grid.worldToMap(0.65, -0.5, mx, my); + master_grid.setCost(mx, my, LETHAL_OBSTACLE); + + master_grid.worldToMap(0.4, -0.25, mx, my); + master_grid.setCost(mx, my, LETHAL_OBSTACLE); + +} + +} // end namespace \ No newline at end of file