dec 10
This commit is contained in:
parent
27bfcff45c
commit
ffe453d26b
@ -15,11 +15,11 @@
|
||||
<include file="/home/joy/catkin/src/turtlebot3_simulations/turtlebot3_gazebo/launch/turtlebot3_world.launch"/>
|
||||
<!-- Load existing map -->
|
||||
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="/home/joy/catkin/src/ch7_pkg/mymap.yaml" />
|
||||
<node name="map_server" pkg="map_server" type="map_server" args="/home/joy/catkin/src/ch7_pkg/map.yaml" />
|
||||
|
||||
<!-- Publish a static transformation between /odom and /map -->
|
||||
|
||||
<node name="tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map/odom 100" />
|
||||
<node name="tf" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 /map /odom 100" />
|
||||
|
||||
<!-- move_base -->
|
||||
|
||||
|
||||
47
costmap_prohibition_layer/.gitignore
vendored
Normal file
47
costmap_prohibition_layer/.gitignore
vendored
Normal file
@ -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
|
||||
107
costmap_prohibition_layer/.travis.yml
Normal file
107
costmap_prohibition_layer/.travis.yml
Normal file
@ -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 <felixd@gmail.com>
|
||||
|
||||
# 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.
|
||||
21
costmap_prohibition_layer/CHANGELOG.rst
Normal file
21
costmap_prohibition_layer/CHANGELOG.rst
Normal file
@ -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
|
||||
188
costmap_prohibition_layer/CMakeLists.txt
Executable file
188
costmap_prohibition_layer/CMakeLists.txt
Executable file
@ -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)
|
||||
|
||||
29
costmap_prohibition_layer/LICENSE
Normal file
29
costmap_prohibition_layer/LICENSE
Normal file
@ -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.
|
||||
6
costmap_prohibition_layer/README.md
Normal file
6
costmap_prohibition_layer/README.md
Normal file
@ -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): [](https://travis-ci.org/rst-tu-dortmund/costmap_prohibition_layer)
|
||||
|
||||
16
costmap_prohibition_layer/cfg/CostmapProhibitionLayer.cfg
Executable file
16
costmap_prohibition_layer/cfg/CostmapProhibitionLayer.cfg
Executable file
@ -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"))
|
||||
5
costmap_prohibition_layer/costmap_plugins.xml
Executable file
5
costmap_prohibition_layer/costmap_plugins.xml
Executable file
@ -0,0 +1,5 @@
|
||||
<library path="lib/libcostmap_prohibition_layer">
|
||||
<class type="costmap_prohibition_layer_namespace::CostmapProhibitionLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration.</description>
|
||||
</class>
|
||||
</library>
|
||||
@ -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 <XmlRpcValue.h>
|
||||
#include <XmlRpcException.h>
|
||||
#include <tf/transform_datatypes.h>
|
||||
#include <mutex>
|
||||
#include <geometry_msgs/PoseArray.h>
|
||||
#include <stdlib.h>
|
||||
#include <ros/ros.h>
|
||||
#include <costmap_2d/layer.h>
|
||||
#include <costmap_2d/layered_costmap.h>
|
||||
#include <costmap_prohibition_layer/CostmapProhibitionLayerConfig.h>
|
||||
#include <dynamic_reconfigure/server.h>
|
||||
|
||||
#include <unordered_map>
|
||||
|
||||
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<geometry_msgs::Point>& 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<PointInt>& polygon, std::vector<PointInt>& 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<PointInt>& polygon, std::vector<PointInt>& 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<PointInt>& 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<CostmapProhibitionLayerConfig>* _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<geometry_msgs::Point> _prohibition_points; //!< vector to save the lonely points in source coordinates
|
||||
std::vector<std::vector<geometry_msgs::Point>> _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
|
||||
31
costmap_prohibition_layer/package.xml
Executable file
31
costmap_prohibition_layer/package.xml
Executable file
@ -0,0 +1,31 @@
|
||||
<?xml version="1.0"?>
|
||||
<package>
|
||||
<name>costmap_prohibition_layer</name>
|
||||
<version>0.0.5</version>
|
||||
<description>ROS-Package that implements a costmap layer to add prohibited areas to the costmap-2D by a user configuration.</description>
|
||||
|
||||
<maintainer email="stephan.kurzawe@tu-dortmund.de">Stephan Kurzawe</maintainer>
|
||||
|
||||
<license>BSD</license>
|
||||
|
||||
<!-- <url type="website">http://wiki.ros.org/costmap_prohibition_layer</url> -->
|
||||
|
||||
|
||||
<author email="stephan.kurzawe@tu-dortmund.de">Stephan Kurzawe</author>
|
||||
|
||||
|
||||
|
||||
<buildtool_depend>catkin</buildtool_depend>
|
||||
<build_depend>costmap_2d</build_depend>
|
||||
<build_depend>dynamic_reconfigure</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<run_depend>costmap_2d</run_depend>
|
||||
<run_depend>dynamic_reconfigure</run_depend>
|
||||
<run_depend>roscpp</run_depend>
|
||||
|
||||
|
||||
<export>
|
||||
<costmap_2d plugin="${prefix}/costmap_plugins.xml" />
|
||||
</export>
|
||||
|
||||
</package>
|
||||
474
costmap_prohibition_layer/src/costmap_prohibition_layer.cpp
Normal file
474
costmap_prohibition_layer/src/costmap_prohibition_layer.cpp
Normal file
@ -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 <costmap_prohibition_layer/costmap_prohibition_layer.h>
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
|
||||
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<CostmapProhibitionLayerConfig>(nh);
|
||||
dynamic_reconfigure::Server<CostmapProhibitionLayerConfig>::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<std::mutex> 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<std::mutex> 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<std::mutex> 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<geometry_msgs::Point>& polygon, unsigned char cost,
|
||||
int min_i, int min_j, int max_i, int max_j, bool fill_polygon)
|
||||
{
|
||||
std::vector<PointInt> 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<PointInt> 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<PointInt>& polygon, std::vector<PointInt>& 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<PointInt>& 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<PointInt>& polygon, std::vector<PointInt>& 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<std::mutex> l(_data_mutex);
|
||||
std::unordered_map<std::string, geometry_msgs::Pose> 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<geometry_msgs::Point> 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
|
||||
219
dodishes_pkg/CMakeLists.txt
Normal file
219
dodishes_pkg/CMakeLists.txt
Normal file
@ -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)
|
||||
8
dodishes_pkg/action/dodishes.action
Normal file
8
dodishes_pkg/action/dodishes.action
Normal file
@ -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
|
||||
76
dodishes_pkg/package.xml
Normal file
76
dodishes_pkg/package.xml
Normal file
@ -0,0 +1,76 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>dodishes_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The dodishes_pkg package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="joy@todo.todo">joy</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>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/dodishes_pkg</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use 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>
|
||||
<build_depend>actionlib</build_depend>
|
||||
<build_depend>actionlib_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>actionlib</build_export_depend>
|
||||
<build_export_depend>actionlib_msgs</build_export_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>actionlib</exec_depend>
|
||||
<exec_depend>actionlib_msgs</exec_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>message_generation</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
215
my_actionlib_tutorials/CMakeLists.txt
Normal file
215
my_actionlib_tutorials/CMakeLists.txt
Normal file
@ -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)
|
||||
8
my_actionlib_tutorials/action/Fibonacci.action
Normal file
8
my_actionlib_tutorials/action/Fibonacci.action
Normal file
@ -0,0 +1,8 @@
|
||||
#goal definition
|
||||
int32 order
|
||||
---
|
||||
#result definition
|
||||
int32[] sequence
|
||||
---
|
||||
#feedback
|
||||
int32[] sequence
|
||||
75
my_actionlib_tutorials/package.xml
Normal file
75
my_actionlib_tutorials/package.xml
Normal file
@ -0,0 +1,75 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>my_actionlib_tutorials</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The my_actionlib_tutorials package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="joy@todo.todo">joy</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>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/my_actionlib_tutorials</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use 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>
|
||||
<build_depend>actionlib</build_depend>
|
||||
<build_depend>actionlib_msgs</build_depend>
|
||||
<build_depend>message_generation</build_depend>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>actionlib</build_export_depend>
|
||||
<build_export_depend>actionlib_msgs</build_export_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>actionlib</exec_depend>
|
||||
<exec_depend>actionlib_msgs</exec_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>message_generation</exec_depend>
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
55
my_actionlib_tutorials/src/fibonacci_server.cpp
Normal file
55
my_actionlib_tutorials/src/fibonacci_server.cpp
Normal file
@ -0,0 +1,55 @@
|
||||
1#include <ros/ros.h>
|
||||
|
||||
2#include <actionlib/server/simple_action_server.h>
|
||||
|
||||
3#include <my_actionlib_tutorials/FibonacciAction.h>
|
||||
|
||||
4
|
||||
|
||||
5 class FibonacciAction
|
||||
|
||||
6 {
|
||||
|
||||
7 protected:
|
||||
|
||||
8
|
||||
|
||||
9 ros::NodeHandle nh ;
|
||||
|
||||
10 actionlib::SimpleActionServer<my_actionlib_tutorials::FibonacciAction> 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}
|
||||
206
myrobot_pkg/CMakeLists.txt
Normal file
206
myrobot_pkg/CMakeLists.txt
Normal file
@ -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)
|
||||
10
myrobot_pkg/launch/robot.launch
Normal file
10
myrobot_pkg/launch/robot.launch
Normal file
@ -0,0 +1,10 @@
|
||||
<launch>
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="worlds/empty_sky.world"/>
|
||||
</include>
|
||||
|
||||
<!-- <param name="robot_description" command="$(find xacro)/xacro $(find pr2_description)/robots/pr2.urdf.xacro"/> -->
|
||||
<!-- gazebo_ros spawn_model -file /home/joy/catkin/src/myrobot_pkg/urdf/07-physics.urdf -urdf -x 2 -y 0 -z 0 -model myrobot -->
|
||||
<node name="spawn_pr2_node" pkg="gazebo_ros" type="spawn_model" args="-file /home/joy/catkin/src/myrobot_pkg/urdf/07-physics.urdf -urdf -x 2 -y 0 -z 0 -model myrobot"/>
|
||||
<!-- roslaunch urdf_tutorial display.launch model:='$(find myrobot_pkg)/urdf/07-physics.urdf' gui:=True -->
|
||||
</launch>
|
||||
31
myrobot_pkg/launch/xacro.launch
Normal file
31
myrobot_pkg/launch/xacro.launch
Normal file
@ -0,0 +1,31 @@
|
||||
<launch>
|
||||
<arg name="model"/>
|
||||
<arg name="gui" default="true"/>
|
||||
<param name="robot_description" command="$(find xacro)/xacro /home/joy/catkin/src/myrobot_pkg/marcos/robot.xacro"/>
|
||||
<param name="use_gui" value="$(arg gui)"/>
|
||||
<node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
|
||||
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
|
||||
<node name="rviz" pkg="rviz" type="rviz" args="-d $(find urdf_tutorial)/urdf.rviz"/>
|
||||
|
||||
<arg name="x_pos" default="2.0"/>
|
||||
<arg name="y_pos" default="0.5"/>
|
||||
<arg name="z_pos" default="0.1"/>
|
||||
|
||||
<include file="$(find gazebo_ros)/launch/empty_world.launch">
|
||||
<arg name="world_name" value="$(find turtlebot3_gazebo)/worlds/turtlebot3_world.world"/>
|
||||
<arg name="paused" value="false"/>
|
||||
<arg name="use_sim_time" value="true"/>
|
||||
<arg name="headless" value="false"/>
|
||||
<arg name="debug" value="false"/>
|
||||
</include>
|
||||
|
||||
<node name="spawn_urdf"
|
||||
pkg="gazebo_ros"
|
||||
type="spawn_model"
|
||||
args="-urdf -model myrobot
|
||||
-x $(arg x_pos)
|
||||
-y $(arg y_pos)
|
||||
-z $(arg z_pos)
|
||||
-param robot_description"/>
|
||||
|
||||
</launch>
|
||||
220
myrobot_pkg/marcos/robot.xacro
Normal file
220
myrobot_pkg/marcos/robot.xacro
Normal file
@ -0,0 +1,220 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="myrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
|
||||
<xacro:property name="PI" value="3.1415926535897931"/>
|
||||
<xacro:property name="base_link_l" value="0.4" />
|
||||
<xacro:property name="base_link_w" value="0.4" />
|
||||
<xacro:property name="base_link_h" value="0.1" />
|
||||
<xacro:property name="base_link_mass" value="2" />
|
||||
<xacro:property name="wheel_length" value="0.05" />
|
||||
<xacro:property name="wheel_radius" value="0.05" />
|
||||
<xacro:property name="wheel_mass" value="0.2" />
|
||||
<xacro:property name="caster_radius" value="0.025" />
|
||||
<xacro:property name="caster_mass" value="0.05" />
|
||||
<xacro:property name="laser_scanner_base_link_height" value="0.05" />
|
||||
<xacro:property name="laser_scanner_base_link_radius" value="0.1" />
|
||||
<xacro:property name="laser_scanner_base_link_mass" value="0.5" />
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 0.8 1"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
<material name="orange">
|
||||
<color rgba="1 0.65 0 1"/>
|
||||
</material>
|
||||
<material name="red">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
<material name="green">
|
||||
<color rgba="0 1 0 1"/>
|
||||
</material>
|
||||
|
||||
|
||||
<xacro:macro name="cube_inertial" params="mass">
|
||||
<inertial>
|
||||
<mass value="${mass}"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.041" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="cylinder_inertial" params="mass">
|
||||
<inertial>
|
||||
<mass value="${mass}"/>
|
||||
<inertia ixx="0.0001" ixy="0.0" ixz="0.0"
|
||||
iyy="0.0001" iyz="0.0"
|
||||
izz="0.0001" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="sphere_inertial" params="mass">
|
||||
<inertial>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="1.2e-05" ixy="0.0" ixz="0.0"
|
||||
iyy="1.2e-05" iyz="0.0"
|
||||
izz="1.2e-05" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
|
||||
<link name="base_footprint"/>
|
||||
|
||||
<joint name="base_joint" type="fixed">
|
||||
<parent link="base_footprint"/>
|
||||
<child link="base_link"/>
|
||||
<origin xyz="0 0 0.050" rpy="0 0 0"/>
|
||||
</joint>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="${base_link_w} ${base_link_l} ${base_link_h}"/>
|
||||
</geometry>
|
||||
<material name="orange"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="${base_link_w} ${base_link_l} ${base_link_h}"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
<!-- <inertial>
|
||||
<mass value="${base_link_mass}"/>
|
||||
<inertia ixx="0.01" ixy="0.0" ixz="0.0" iyy="0.02" iyz="0.0" izz="0.041"/>
|
||||
</inertial> -->
|
||||
<xacro:cube_inertial mass="${base_link_mass}"/>
|
||||
</link>
|
||||
|
||||
<link name="right_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_length}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
<material name="red"/>
|
||||
<origin rpy="1.57075 0 0" xyz="0 -0.025 0"/>
|
||||
<material name="red"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_length}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
<origin rpy="1.57075 0 0" xyz="0 -0.025 0"/>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="${wheel_mass}"/>
|
||||
</link>
|
||||
|
||||
<gazebo reference="right_wheel">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
|
||||
<joint name="wheel_right_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_wheel"/>
|
||||
<origin xyz="0.0 -0.2 -0.025"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
|
||||
<link name="left_wheel">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_length}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
<material name="green"/>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0.025 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="${wheel_length}" radius="${wheel_radius}"/>
|
||||
</geometry>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0.025 0"/>
|
||||
</collision>
|
||||
<xacro:cylinder_inertial mass="${wheel_mass}"/>
|
||||
</link>
|
||||
|
||||
<gazebo reference="left_wheel">
|
||||
<material>Gazebo/Green</material>
|
||||
</gazebo>
|
||||
|
||||
<joint name="wheel_left_joint" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_wheel"/>
|
||||
<origin xyz="0.0 0.2 -0.025"/>
|
||||
<axis xyz="0 1 0"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Orange</material>
|
||||
</gazebo>
|
||||
|
||||
|
||||
<link name="right_caster">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="${caster_radius}"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="${caster_radius}"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0"/>
|
||||
</collision>
|
||||
<xacro:sphere_inertial mass="${caster_mass}"/>
|
||||
</link>
|
||||
<gazebo reference="right_caster">
|
||||
<material>Gazebo/Gray</material>
|
||||
</gazebo>
|
||||
|
||||
<joint name="base_to_right_caster" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="right_caster"/>
|
||||
<origin xyz="${base_link_w*0.33} ${base_link_l*0.0} -${base_link_h*0.5}"/>
|
||||
</joint>
|
||||
|
||||
|
||||
<link name="left_caster">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="${caster_radius}"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0"/>
|
||||
<material name="gray"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="${caster_radius}"/>
|
||||
</geometry>
|
||||
<origin xyz="0 0 0"/>
|
||||
</collision>
|
||||
<xacro:sphere_inertial mass="${caster_mass}"/>
|
||||
</link>
|
||||
<gazebo reference="left_caster">
|
||||
<material>Gazebo/Gray</material>
|
||||
</gazebo>
|
||||
|
||||
<joint name="base_to_left_caster" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_caster"/>
|
||||
<origin xyz="-${base_link_w*0.33} -${base_link_l*0} -${base_link_h*0.5}"/>
|
||||
</joint>
|
||||
|
||||
<!-- imu -->
|
||||
|
||||
<joint name="imu_joint" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="imu_link"/>
|
||||
<origin xyz="0.0 0 0" rpy="0 0 0"/>
|
||||
</joint>
|
||||
<link name="imu_link"/>
|
||||
|
||||
</robot>
|
||||
68
myrobot_pkg/package.xml
Normal file
68
myrobot_pkg/package.xml
Normal file
@ -0,0 +1,68 @@
|
||||
<?xml version="1.0"?>
|
||||
<package format="2">
|
||||
<name>myrobot_pkg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>The myrobot_pkg package</description>
|
||||
|
||||
<!-- One maintainer tag required, multiple allowed, one person per tag -->
|
||||
<!-- Example: -->
|
||||
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
|
||||
<maintainer email="joy@todo.todo">joy</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>TODO</license>
|
||||
|
||||
|
||||
<!-- Url tags are optional, but multiple are allowed, one per tag -->
|
||||
<!-- Optional attribute type can be: website, bugtracker, or repository -->
|
||||
<!-- Example: -->
|
||||
<!-- <url type="website">http://wiki.ros.org/myrobot_pkg</url> -->
|
||||
|
||||
|
||||
<!-- Author tags are optional, multiple are allowed, one per tag -->
|
||||
<!-- Authors do not have to be maintainers, but could be -->
|
||||
<!-- Example: -->
|
||||
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
|
||||
|
||||
|
||||
<!-- The *depend tags are used to specify dependencies -->
|
||||
<!-- Dependencies can be catkin packages or system dependencies -->
|
||||
<!-- Examples: -->
|
||||
<!-- Use 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>
|
||||
<build_depend>roscpp</build_depend>
|
||||
<build_depend>rospy</build_depend>
|
||||
<build_depend>std_msgs</build_depend>
|
||||
<build_export_depend>roscpp</build_export_depend>
|
||||
<build_export_depend>rospy</build_export_depend>
|
||||
<build_export_depend>std_msgs</build_export_depend>
|
||||
<exec_depend>roscpp</exec_depend>
|
||||
<exec_depend>rospy</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<!-- Other tools can request additional information be placed here -->
|
||||
|
||||
</export>
|
||||
</package>
|
||||
456
myrobot_pkg/urdf/07-physics.urdf
Normal file
456
myrobot_pkg/urdf/07-physics.urdf
Normal file
@ -0,0 +1,456 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot name="physics">
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="1 0 0 1"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
<material name="yellow">
|
||||
<color rgba="1 1 0 1"/>
|
||||
</material>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="1 0.2 2"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="1 0.2 2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
<material name="yellow"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_right_leg" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="1 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.55 0 -1"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
<material name="yellow"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_left_leg" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="1 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.55 0 -1"/>
|
||||
</joint>
|
||||
<!--
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="1 0 0"/>
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="0 0.22 0.25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0 0.2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius="0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius="0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.08 0.08 0.08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
<origin xyz="-0.04 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.08 0.08 0.08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0.1814 0 0.1414"/>
|
||||
</joint> -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<gazebo reference="left_leg">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
<gazebo reference="right_leg">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
</robot>
|
||||
456
myrobot_pkg/urdf/dec4.urdf
Normal file
456
myrobot_pkg/urdf/dec4.urdf
Normal file
@ -0,0 +1,456 @@
|
||||
<?xml version="1.0" ?>
|
||||
<robot name="myrobot" xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<material name="blue">
|
||||
<color rgba="0 0 0.8 1"/>
|
||||
</material>
|
||||
<material name="black">
|
||||
<color rgba="0 0 0 1"/>
|
||||
</material>
|
||||
<material name="white">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
<material name="yellow">
|
||||
<color rgba="1 1 0 1"/>
|
||||
</material>
|
||||
|
||||
<link name="base_link">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="1 1 0.2"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="1 1 0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="right_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
<material name="yellow"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_right_leg" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="1 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="right_leg"/>
|
||||
<origin xyz="0.55 0 -1"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
<material name="yellow"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.4"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
<joint name="base_to_left_leg" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="1 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="-0.55 0 -1"/>
|
||||
</joint>
|
||||
<!--
|
||||
<link name="right_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_base_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="1 0 0"/>
|
||||
<parent link="right_leg"/>
|
||||
<child link="right_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="right_base"/>
|
||||
<child link="right_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_leg">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.6 0.1 0.2"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0" xyz="0 0 -0.3"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="base_to_left_leg" type="fixed">
|
||||
<parent link="base_link"/>
|
||||
<child link="left_leg"/>
|
||||
<origin xyz="0 0.22 0.25"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_base">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.4 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="10"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_base_joint" type="fixed">
|
||||
<parent link="left_leg"/>
|
||||
<child link="left_base"/>
|
||||
<origin xyz="0 0 -0.6"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_front_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_front_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_front_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_back_wheel">
|
||||
<visual>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
<material name="black"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="1.57075 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<cylinder length="0.1" radius="0.035"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_back_wheel_joint" type="continuous">
|
||||
<axis rpy="0 0 0" xyz="0 1 0"/>
|
||||
<parent link="left_base"/>
|
||||
<child link="left_back_wheel"/>
|
||||
<origin rpy="0 0 0" xyz="-0.133333333333 0 -0.085"/>
|
||||
</joint>
|
||||
|
||||
<joint name="gripper_extension" type="prismatic">
|
||||
<parent link="base_link"/>
|
||||
<child link="gripper_pole"/>
|
||||
<limit effort="1000.0" lower="-0.38" upper="0" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.19 0 0.2"/>
|
||||
</joint>
|
||||
|
||||
<link name="gripper_pole">
|
||||
<visual>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius="0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<cylinder length="0.2" radius="0.01"/>
|
||||
</geometry>
|
||||
<origin rpy="0 1.57075 0 " xyz="0.1 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="left_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_gripper">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="left_tip_joint" type="fixed">
|
||||
<parent link="left_gripper"/>
|
||||
<child link="left_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="left_tip">
|
||||
<visual>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="0.0 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_gripper_joint" type="revolute">
|
||||
<axis xyz="0 0 -1"/>
|
||||
<limit effort="1000.0" lower="0.0" upper="0.548" velocity="0.5"/>
|
||||
<origin rpy="0 0 0" xyz="0.2 -0.01 0"/>
|
||||
<parent link="gripper_pole"/>
|
||||
<child link="right_gripper"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_gripper">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0 0 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="right_tip_joint" type="fixed">
|
||||
<parent link="right_gripper"/>
|
||||
<child link="right_tip"/>
|
||||
</joint>
|
||||
|
||||
<link name="right_tip">
|
||||
<visual>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<mesh filename="package://urdf_tutorial/meshes/l_finger_tip.dae"/>
|
||||
</geometry>
|
||||
<origin rpy="-3.1415 0 0" xyz="0.09137 0.00495 0"/>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="0.05"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<link name="head">
|
||||
<visual>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
<material name="white"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<sphere radius="0.2"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="2"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="head_swivel" type="continuous">
|
||||
<parent link="base_link"/>
|
||||
<child link="head"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<origin xyz="0 0 0.3"/>
|
||||
</joint>
|
||||
|
||||
<link name="box">
|
||||
<visual>
|
||||
<geometry>
|
||||
<box size="0.08 0.08 0.08"/>
|
||||
</geometry>
|
||||
<material name="blue"/>
|
||||
<origin xyz="-0.04 0 0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<geometry>
|
||||
<box size="0.08 0.08 0.08"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<inertial>
|
||||
<mass value="1"/>
|
||||
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
|
||||
</inertial>
|
||||
</link>
|
||||
|
||||
<joint name="tobox" type="fixed">
|
||||
<parent link="head"/>
|
||||
<child link="box"/>
|
||||
<origin xyz="0.1814 0 0.1414"/>
|
||||
</joint> -->
|
||||
<gazebo reference="base_link">
|
||||
<material>Gazebo/Red</material>
|
||||
</gazebo>
|
||||
<gazebo reference="left_leg">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
<gazebo reference="right_leg">
|
||||
<material>Gazebo/Yellow</material>
|
||||
</gazebo>
|
||||
</robot>
|
||||
@ -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}
|
||||
|
||||
@ -2,4 +2,4 @@
|
||||
<class type="simple_layer_namespace::SimpleLayer" base_class_type="costmap_2d::Layer">
|
||||
<description>Demo Layer that adds a point 1 meter in front of the robot</description>
|
||||
</class>
|
||||
</library>
|
||||
</library>
|
||||
@ -25,4 +25,4 @@ private:
|
||||
dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
|
||||
};
|
||||
}
|
||||
#endif
|
||||
#endif
|
||||
@ -67,7 +67,7 @@
|
||||
|
||||
|
||||
<!-- The export tag contains other, unspecified, tags -->
|
||||
<export>
|
||||
<costmap_2d plugin="${prefix}/costmap_plugins.xml" />
|
||||
</export>
|
||||
<export>
|
||||
<costmap_2d plugin="${prefix}/costmap_plugins.xml" />
|
||||
</export>
|
||||
</package>
|
||||
|
||||
@ -1,4 +1,4 @@
|
||||
#include<simple_layers/simple_layer.h>
|
||||
#include "../include/simple_layers/simple_layer.h"
|
||||
#include <pluginlib/class_list_macros.h>
|
||||
|
||||
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
|
||||
Loading…
x
Reference in New Issue
Block a user