HiepLM/AMR_T800
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

git commit -m "first commit for v2"

Browse Source
This commit is contained in:
HiepLM
2025-12-29 16:21:22 +07:00
commit aa3d832d5c
1807 changed files with 307078 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

208
Localizations/Cores/loc_core/CMakeLists.txt Normal file
View File

@@ -0,0 +1,208 @@
cmake_minimum_required(VERSION 3.0.2)
project(loc_core)
## Compile as C++11, supported in ROS Kinetic and newer
# add_compile_options(-std=c++11)
set_directory_properties(PROPERTIES COMPILE_OPTIONS "-std=c++14")
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
std_msgs
tf
tf2_ros
)
## 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
# geometry_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 loc_core
CATKIN_DEPENDS geometry_msgs std_msgs tf tf2_ros
# 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}/loc_core.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/loc_core_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_loc_core.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)

42
Localizations/Cores/loc_core/include/loc_core/common.h Normal file
View File

@@ -0,0 +1,42 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2017, Locus Robotics
* 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.
*/
#ifndef _LOC_CORE_COMMON_H_INCLUDED_
#define _LOC_CORE_COMMON_H_INCLUDED_
#include <tf2_ros/buffer.h>
#include <memory>
using TFListenerPtr = std::shared_ptr<tf2_ros::Buffer>;
#endif // _LOC_CORE_COMMON_H_INCLUDED_

136
Localizations/Cores/loc_core/include/loc_core/localization.h Normal file
View File

@@ -0,0 +1,136 @@
#ifndef _LOC_CORE_LOCALIZATION_H_INCLUDED_
#define _LOC_CORE_LOCALIZATION_H_INCLUDED_
#include <ros/ros.h>
#include <tf2_ros/buffer.h>
#include <tf/tf.h>
#include <memory>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose2D.h>
#include <loc_core/common.h>
namespace loc_core
{
enum slam_state_en
{
Mapping,
Localization,
Calibrations,
Ready,
Error
};
/**
* @class BaseLocalization
* @brief Provides an interface for locazation
* @brief All package written as plugins for the localization stack must adhere to this interface.
*/
class BaseLocalization
{
public:
using Ptr = std::shared_ptr<BaseLocalization>;
/**
* @brief Virtual destructor for the interface
*/
virtual ~BaseLocalization() {}
/**
* @brief Initialization function for loc base
* @param nh A Node handle
*
*/
virtual void initialize(ros::NodeHandle nh, TFListenerPtr tf) = 0;
/**
* @brief Loading a Activate Map File name
* @param activated_map_filename The Map File name
* @return True if activated_map_filename is exsits, else Fasle
*/
virtual bool loadActivateMapFileName(std::string& activated_map_filename) = 0;
/**
* @brief Saving a Activate Map File name
* @param activated_map_filename The Map File name
* @return True if activated_map_filename is exsits, else Fasle
*/
virtual bool saveActivateMapFileName(const std::string& activated_map_filename) = 0;
/**
* @brief Create any files to a folder of the path
* @param path The path where is be saved any files (png, yaml ...)
* @param map_name The name folder and files
*/
virtual bool createStaticMap(const std::string map_name) = 0;
/**
* @brief Load a map given a path to a yaml file
* @param filename The file name
* @return True/False If load map succesed or not done
*/
virtual bool changeStaticMap(const std::string filename) = 0;
/**
* @brief Load a virtual walls map given a path to a yaml file
* @param filename The file name
* @return True/False If load map succesed or not done
*/
virtual bool changeVirtualWallsMap(const std::string filename) = 0;
/**
* @brief Get the list of map folder in the workingdirection
* @param directories The list map name
*/
virtual void listMapFiles(std::stringstream &directories) = 0;
/**
* @brief Start the mapping process.
* When the state variable 'SlamState' is 'Ready', this function can be called.
* Upon successful invocation, the state variable 'SlamState' will change to 'Mapping'.
* @return True/False Call fucntion is successed/failed.
*/
virtual bool startMapping() = 0;
/**
* @brief Turn off the Localization mode.
* At this point, the robot's coordinate information will not be updated.
* If called successfully, the 'SlamState' variable will change to 'Ready'.
* @return True/False Call fucntion is successed/failed
*/
virtual bool stopMapping() = 0;
/**
* @brief Set the initial position of the robot on the map.
* The condition to call the function is that the 'SlamState' is 'Localization'.
* When the function is called successfully, the 'SlamState' will change to 'Calibrations'.
* When the 'Calibrations' process ends, the 'SlamState' will change back to 'Localization'.
* @return True/False Call fucntion is successed/failed
*/
virtual bool startLocalization() = 0;
/**
* @brief Turn off the Localization mode.
* At this point, the robot's coordinate information will not be updated.
* If called successfully, the 'SlamState' variable will change to 'Ready'.
* @return True/False Call fucntion is successed/failed
*/
virtual bool stopLocalization() = 0;
/**
* @brief Get State of Loc Base
*/
virtual loc_core::slam_state_en getState() = 0;
std::string activated_map_filename_;
std::string working_dir_;
std::string map_topic_;
protected:
/**
* @brief Constructor for the interface
*/
BaseLocalization() {};
}; // class BaseLocalization
} // namespace loc_core
#endif // _LOC_CORE_LOCALIZATION_H_INCLUDED_

71
Localizations/Cores/loc_core/package.xml Normal file
View File

@@ -0,0 +1,71 @@
<?xml version="1.0"?>
<package format="2">
<name>loc_core</name>
<version>0.0.0</version>
<description>The loc_core package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="robotics@todo.todo">robotics</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/loc_core</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>geometry_msgs</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf</build_depend>
<build_depend>tf2_ros</build_depend>
<build_export_depend>geometry_msgs</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>tf</build_export_depend>
<build_export_depend>tf2_ros</build_export_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>tf</exec_depend>
<exec_depend>tf2_ros</exec_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

258
Localizations/Libraries/Ros/amcl/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,258 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package amcl
^^^^^^^^^^^^^^^^^^^^^^^^^^
1.17.3 (2023-01-10)
-------------------
* [AMCL] Add option to force nomotion update after initialpose (`#1226 <https://github.com/ros-planning/navigation/issues/1226>`_)
* Adds a new boolean parameter force_update_after_initialpose. When set to true, an update is forced on the next laser scan callback, such as when the /request_nomotion_update service is called. This often results in an improved robot pose after a manual (not very precise) re-localization - without a need for the robot to move.
* Fixes a bunch of compiler warnings (unused variable now, catching exceptions by value), normalizes how tf exceptions are caught
* [ROS-O] various patches (`#1225 <https://github.com/ros-planning/navigation/issues/1225>`_)
* do not specify obsolete c++11 standard
this breaks with current versions of log4cxx.
* update pluginlib include paths
the non-hpp headers have been deprecated since kinetic
* use lambdas in favor of boost::bind
Using boost's _1 as a global system is deprecated since C++11.
The ROS packages in Debian removed the implicit support for the global symbols,
so this code fails to compile there without the patch.
* Contributors: Michael Görner, Stephan
1.17.2 (2022-06-20)
-------------------
* Update pf.c (`#1161 <https://github.com/ros-planning/navigation/issues/1161>`_)
`#1160 <https://github.com/ros-planning/navigation/issues/1160>`_ AMCL miscalculates orientation covariance for clusters
* Improved Overall readablity (`#1177 <https://github.com/ros-planning/navigation/issues/1177>`_)
* fix crashes in AMCL (`#1152 <https://github.com/ros-planning/navigation/issues/1152>`_)
* fix: catch runtime_error from roscore
* ignore malformed message from laser, otherwise it will crash
* Fixes `#1117 <https://github.com/ros-planning/navigation/issues/1117>`_ (`#1118 <https://github.com/ros-planning/navigation/issues/1118>`_)
* Fixed the risk of divide by zero. (`#1099 <https://github.com/ros-planning/navigation/issues/1099>`_)
* (AMCL) add missing test dep on tf2_py (`#1091 <https://github.com/ros-planning/navigation/issues/1091>`_)
* (AMCL)(Noetic) use robot pose in tests (`#1087 <https://github.com/ros-planning/navigation/issues/1087>`_)
* (amcl) fix missing '#if NEW_UNIFORM_SAMPLING' (`#1079 <https://github.com/ros-planning/navigation/issues/1079>`_)
* Contributors: David V. Lu!!, Matthijs van der Burgh, Noriaki Ando, Supernovae, christofschroeter, easylyou
1.17.1 (2020-08-27)
-------------------
* (AMCL) add resample limit cache [Noetic] (`#1014 <https://github.com/ros-planning/navigation/issues/1014>`_)
* Contributors: Matthijs van der Burgh
1.17.0 (2020-04-02)
-------------------
* Merge pull request `#982 <https://github.com/ros-planning/navigation/issues/982>`_ from ros-planning/noetic_prep
Noetic Migration
* map is not subscriptable in python3
* fix python3 errors in basic_localization.py
* use upstream pykdl
* Contributors: Michael Ferguson
1.16.6 (2020-03-18)
-------------------
1.16.5 (2020-03-15)
-------------------
* [melodic] updated install for better portability. (`#973 <https://github.com/ros-planning/navigation/issues/973>`_)
* Contributors: Sean Yen
1.16.4 (2020-03-04)
-------------------
* Implement selective resampling (`#921 <https://github.com/cobalt-robotics/navigation/issues/921>`_) (`#971 <https://github.com/cobalt-robotics/navigation/issues/971>`_)
Co-authored-by: Adi Vardi <adidasv111@gmail.com>
* Add CLI option to trigger global localization before processing a bagfile (`#816 <https://github.com/cobalt-robotics/navigation/issues/816>`_) (`#970 <https://github.com/cobalt-robotics/navigation/issues/970>`_)
Co-authored-by: alain-m <alain@savioke.com>
* Fix some reconfigure parameters not being applied [amcl]. (`#952 <https://github.com/cobalt-robotics/navigation/issues/952>`_)
* amcl: include missing CMake functions to fix build (`#946 <https://github.com/cobalt-robotics/navigation/issues/946>`_)
* Set proper limits for the z-weights [amcl]. (`#953 <https://github.com/cobalt-robotics/navigation/issues/953>`_)
* Merge pull request `#965 <https://github.com/cobalt-robotics/navigation/issues/965>`_ from nlimpert/nlimpert/fix_missing_cmake_include
Add missing CMake include(CheckSymbolExists) for CMake >= 3.15
* amcl: add missing CMake include(CheckSymbolExists)
Starting with CMake 3.15 an explicit include(CheckSymbolExists)
is required to use the check_symbol_exists macro.
* Contributors: Ben Wolsieffer, Michael Ferguson, Nicolas Limpert, Patrick Chin
1.16.3 (2019-11-15)
-------------------
* Merge branch 'melodic-devel' into layer_clear_area-melodic
* Fix typo in amcl_laser model header (`#918 <https://github.com/ros-planning/navigation/issues/918>`_)
* Merge pull request `#849 <https://github.com/ros-planning/navigation/issues/849>`_ from seanyen/amcl_windows_fix
[Windows][melodic] AMCL Windows build bring up.
* revert unrelated changes.
* AMCL windows build bring up.
* Add HAVE_UNISTD and HAVE_DRAND48 and portable_utils.hpp for better cross compiling.
* Variable length array is not supported in MSVC, conditionally disable it.
* Fix install location for shared lib and executables on Windows.
* Use isfinite for better cross compiling.
* feat: AMCL Diagnostics (`#807 <https://github.com/ros-planning/navigation/issues/807>`_)
Diagnostic task that monitors the estimated standard deviation of the filter.
By: reinzor <reinzor@gmail.com>
* fix typo for parameter beam_skip_error_threshold but bandaged for other users in AMCL (`#790 <https://github.com/ros-planning/navigation/issues/790>`_)
* fix typo but bandage for other users
* Merge pull request `#785 <https://github.com/ros-planning/navigation/issues/785>`_ from mintar/amcl_c++11
amcl: Add compile option C++11
* amcl: Set C++ standard 11 if not set
This is required to build the melodic-devel branch of the navigation
stack on kinetic. Melodic sets CMAKE_CXX_STANDARD=14, but kinetic
doesn't set that variable at all.
* Contributors: Hadi Tabatabaee, Martin Günther, Michael Ferguson, Rein Appeldoorn, Sean Yen, Steven Macenski
1.16.2 (2018-07-31)
-------------------
* Merge pull request `#773 <https://github.com/ros-planning/navigation/issues/773>`_ from ros-planning/packaging_fixes
packaging fixes
* update amcl to have proper depends
* add geometry_msgs
* add tf2_msgs
* fix alphabetical order
* Contributors: Michael Ferguson
1.16.1 (2018-07-28)
-------------------
* Merge pull request `#770 <https://github.com/ros-planning/navigation/issues/770>`_ from ros-planning/fix_debians
Fix debian builds (closes `#769 <https://github.com/ros-planning/navigation/issues/769>`_)
* make AMCL depend on sensor_msgs
previously, amcl depended on TF, which depended on
sensor_msgs.
* Contributors: Michael Ferguson
1.16.0 (2018-07-25)
-------------------
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* Merge pull request `#734 <https://github.com/ros-planning/navigation/issues/734>`_ from ros-planning/melodic_731
AMCL dynamic reconfigure: Extend parameter range (Forward port `#731 <https://github.com/ros-planning/navigation/issues/731>`_)
* Merge pull request `#728 <https://github.com/ros-planning/navigation/issues/728>`_ from ros-planning/melodic_tf2_conversion
switch AMCL to use TF2
* fix swapped odom1/4 in omni model, fixes `#499 <https://github.com/ros-planning/navigation/issues/499>`_
* Merge pull request `#730 <https://github.com/ros-planning/navigation/issues/730>`_ from Glowcloud/melodic-devel
Fix for Potential Memory Leak in AmclNode::reconfigureCB `#729 <https://github.com/ros-planning/navigation/issues/729>`_
* Fix for Potential Memory Leak in AmclNode::reconfigureCB
* switch AMCL to use TF2
* Merge pull request `#727 <https://github.com/ros-planning/navigation/issues/727>`_ from ros-planning/melodic_668
Update laser_model_type enum on AMCL.cfg (Melodic port of `#668 <https://github.com/ros-planning/navigation/issues/668>`_)
* Update laser_model_type enum on AMCL.cfg
Adding likelihood_field_prob laser model option on AMCL.cfg to be able to control dynamic parameters with this laser sensor model.
* Merge pull request `#723 <https://github.com/ros-planning/navigation/issues/723>`_ from moriarty/melodic-buildfarm-errors
Melodic buildfarm errors
* include <memory> for std::shared_ptr
* Merge pull request `#718 <https://github.com/ros-planning/navigation/issues/718>`_ from moriarty/tf2-buffer-ptr
[melodic] tf2_buffer\_ -> tf2_buffer_ptr\_
* [melodic] tf2_buffer\_ -> tf2_buffer_ptr\_
Change required due to changes in upstream dependencies:
`ros/geometry#163 <https://github.com/ros/geometry/issues/163>`_: "Maintain & expose tf2 Buffer in shared_ptr for tf"
fixes `ros-planning/navigation#717 <https://github.com/ros-planning/navigation/issues/717>`_ (for compile errors at least.)
* Contributors: Alexander Moriarty, Glowcloud, Martin Ganeff, Michael Ferguson, Miguel Cordero, Vincent Rabaud, maracuya-robotics
1.15.2 (2018-03-22)
-------------------
* Fix minor typo (`#682 <https://github.com/ros-planning/navigation/issues/682>`_)
This typo caused some confusion because we were searching for a semicolon in our configuration.
* Merge pull request `#677 <https://github.com/ros-planning/navigation/issues/677>`_ from ros-planning/lunar_634
removing recomputation of cluster stats causing assertion error (`#634 <https://github.com/ros-planning/navigation/issues/634>`_)
* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
update maintainer email (lunar)
* Remove Dead Code [Lunar] (`#646 <https://github.com/ros-planning/navigation/issues/646>`_)
* Clean up navfn
* Cleanup amcl
* Merge pull request `#649 <https://github.com/ros-planning/navigation/issues/649>`_ from aaronhoy/lunar_add_ahoy
Add myself as a maintainer.
* Contributors: Aaron Hoy, David V. Lu!!, Michael Ferguson, stevemacenski
1.15.1 (2017-08-14)
-------------------
1.15.0 (2017-08-07)
-------------------
* Reference Issue `#592 <https://github.com/ros-planning/navigation/issues/592>`_ Added warning to AMCL when map is published on ... (`#604 <https://github.com/ros-planning/navigation/issues/604>`_)
* rebase fixups
* convert packages to format2
* recompute cluster stat when force_publication
* Fix CMakeLists + package.xmls (`#548 <https://github.com/ros-planning/navigation/issues/548>`_)
* amcl: fix compilation with gcc v7
* Added deps to amcl costmap_2d move_base (`#512 <https://github.com/ros-planning/navigation/issues/512>`_)
* fix order of parameters (closes `#553 <https://github.com/ros-planning/navigation/issues/553>`_)
* Fix potential string overflow and resource leak
* Contributors: Dmitry Rozhkov, Laurent GEORGE, Martin Günther, Michael Ferguson, Mikael Arguedas, Peter Harliman Liem, mryellow, vik748
1.14.0 (2016-05-20)
-------------------
* Allow AMCL to run from bag file to allow very fast testing.
* Fixes interpretation of a delayed initialpose message (see `#424 <https://github.com/ros-planning/navigation/issues/424>`_).
The tf lookup as it was before this change was very likely to fail as
ros::Time::now() was used to look up a tf without waiting on the tf's
availability. Additionally, the computation of the "new pose" by
multiplying the delta that the robot moved from the initialpose's
timestamp to ros::Time::now() was wrong. That delta has to by multiplied
from the right to the "old pose".
This commit also changes the reference frame to look up this delta to be
the odom frame as this one is supposed to be smooth and therefore the
best reference to get relative robot motion in the robot (base link) frame.
* New unit test for proper interpretation of a delayed initialpose message.
Modifies the set_pose.py script to be able to send an initial pose with
a user defined time stamp at a user defined time. Adds a rostest to
exercise this new option.
This reveals the issues mentioned in `#424 <https://github.com/ros-planning/navigation/issues/424>`_ (the new test fails).
* Contributors: Derek King, Stephan Wirth
1.13.1 (2015-10-29)
-------------------
* adds the set_map service to amcl
* fix pthread_mutex_lock on shutdown
* Contributors: Michael Ferguson, Stephan Wirth
1.13.0 (2015-03-17)
-------------------
* amcl_node will now save latest pose on shutdown
* Contributors: Ian Danforth
1.12.0 (2015-02-04)
-------------------
* update maintainer email
* Contributors: Michael Ferguson
1.11.15 (2015-02-03)
--------------------
1.11.14 (2014-12-05)
--------------------
1.11.13 (2014-10-02)
--------------------
1.11.12 (2014-10-01)
--------------------
* Bug fix to remove particle weights being reset when motion model is updated
* Integrated new sensor model which calculates the observation likelihood in a probabilistic manner
Also includes the option to do beam-skipping (to better handle observations from dynamic obstacles)
* Pose pulled from parameter server when new map received
* Contributors: Steven Kordell, hes3pal
1.11.11 (2014-07-23)
--------------------
1.11.10 (2014-06-25)
--------------------
1.11.9 (2014-06-10)
-------------------
1.11.8 (2014-05-21)
-------------------
1.11.7 (2014-05-21)
-------------------
* removes useless this->z_max = z_max assignment
* Fix warning string.
* Contributors: Jeremiah Via, enriquefernandez
1.11.5 (2014-01-30)
-------------------
* Fix for `#160 <https://github.com/ros-planning/navigation/issues/160>`_
* Download test data from download.ros.org instead of willow
* Change maintainer from Hersh to Lu
1.11.4 (2013-09-27)
-------------------
* Package URL Updates
* amcl_pose and particle cloud are now published latched
* Fixed or commented out failing amcl tests.

184
Localizations/Libraries/Ros/amcl/CMakeLists.txt Normal file
View File

@@ -0,0 +1,184 @@
cmake_minimum_required(VERSION 3.0.2)
project(amcl)
include(CheckIncludeFile)
include(CheckSymbolExists)
find_package(catkin REQUIRED
COMPONENTS
diagnostic_updater
dynamic_reconfigure
geometry_msgs
message_filters
nav_msgs
rosbag
roscpp
sensor_msgs
std_srvs
tf2
tf2_geometry_msgs
tf2_msgs
tf2_ros
)
find_package(Boost REQUIRED)
# dynamic reconfigure
generate_dynamic_reconfigure_options(
cfg/AMCL.cfg
)
catkin_package(
CATKIN_DEPENDS
diagnostic_updater
dynamic_reconfigure
geometry_msgs
nav_msgs
rosbag
roscpp
sensor_msgs
std_srvs
tf2
tf2_msgs
tf2_ros
INCLUDE_DIRS include
LIBRARIES amcl_sensors amcl_map amcl_pf amcl_lib
)
include_directories(include)
include_directories(${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS})
include_directories(src/include)
check_include_file(unistd.h HAVE_UNISTD_H)
if (HAVE_UNISTD_H)
add_definitions(-DHAVE_UNISTD_H)
endif (HAVE_UNISTD_H)
check_symbol_exists(drand48 stdlib.h HAVE_DRAND48)
if (HAVE_DRAND48)
add_definitions(-DHAVE_DRAND48)
endif (HAVE_DRAND48)
add_library(amcl_pf
src/amcl/pf/pf.c
src/amcl/pf/pf_kdtree.c
src/amcl/pf/pf_pdf.c
src/amcl/pf/pf_vector.c
src/amcl/pf/eig3.c
src/amcl/pf/pf_draw.c)
add_library(amcl_map
src/amcl/map/map.c
src/amcl/map/map_cspace.cpp
src/amcl/map/map_range.c
src/amcl/map/map_store.c
src/amcl/map/map_draw.c)
add_library(amcl_sensors
src/amcl/sensors/amcl_sensor.cpp
src/amcl/sensors/amcl_odom.cpp
src/amcl/sensors/amcl_laser.cpp)
target_link_libraries(amcl_sensors amcl_map amcl_pf)
add_library(amcl_lib
src/amcl.cpp
)
add_dependencies(amcl_lib
amcl_sensors
amcl_map
amcl_pf
${${PROJECT_NAME}_EXPORTED_TARGETS}
${catkin_EXPORTED_TARGETS}
)
target_link_libraries(amcl_lib
amcl_sensors
amcl_map
amcl_pf
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
add_executable(amcl src/amcl_node.cpp)
add_dependencies(amcl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_link_libraries(amcl
amcl_lib
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)
install(TARGETS
amcl
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(TARGETS
amcl_sensors amcl_map amcl_pf amcl_lib
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
install(DIRECTORY include/amcl/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
)
install(DIRECTORY examples/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/examples
)
## Configure Tests
if(CATKIN_ENABLE_TESTING)
find_package(rostest REQUIRED)
# Bags
catkin_download_test_data(${PROJECT_NAME}_basic_localization_stage_indexed.bag
http://download.ros.org/data/amcl/basic_localization_stage_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 41fe43af189ec71e5e48eb9ed661a655)
catkin_download_test_data(${PROJECT_NAME}_global_localization_stage_indexed.bag
http://download.ros.org/data/amcl/global_localization_stage_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 752f711cf4f6e8d1d660675e2da096b0)
catkin_download_test_data(${PROJECT_NAME}_small_loop_prf_indexed.bag
http://download.ros.org/data/amcl/small_loop_prf_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 e4ef0fc006872b43f12ed8a7ce7dcd81)
catkin_download_test_data(${PROJECT_NAME}_small_loop_crazy_driving_prg_indexed.bag
http://download.ros.org/data/amcl/small_loop_crazy_driving_prg_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 4a58d1a7962914009d99000d06e5939c)
catkin_download_test_data(${PROJECT_NAME}_texas_greenroom_loop_indexed.bag
http://download.ros.org/data/amcl/texas_greenroom_loop_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 6e3432115cccdca1247f6c807038e13d)
catkin_download_test_data(${PROJECT_NAME}_texas_willow_hallway_loop_indexed.bag
http://download.ros.org/data/amcl/texas_willow_hallway_loop_indexed.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 27deb742fdcd3af44cf446f39f2688a8)
catkin_download_test_data(${PROJECT_NAME}_rosie_localization_stage.bag
http://download.ros.org/data/amcl/rosie_localization_stage.bag
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 3347bf3835724cfa45e958c5c1846066)
# Maps
catkin_download_test_data(${PROJECT_NAME}_willow-full.pgm
http://download.ros.org/data/amcl/willow-full.pgm
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 b84465cdbbfe3e2fb9eb4579e0bcaf0e)
catkin_download_test_data(${PROJECT_NAME}_willow-full-0.05.pgm
http://download.ros.org/data/amcl/willow-full-0.05.pgm
DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION}/test
MD5 b61694296e08965096c5e78611fd9765)
# Tests
add_rostest(test/set_initial_pose.xml)
add_rostest(test/set_initial_pose_delayed.xml)
add_rostest(test/basic_localization_stage.xml)
add_rostest(test/global_localization_stage.xml)
add_rostest(test/small_loop_prf.xml)
add_rostest(test/small_loop_crazy_driving_prg.xml)
add_rostest(test/texas_greenroom_loop.xml)
add_rostest(test/rosie_multilaser.xml)
add_rostest(test/texas_willow_hallway_loop.xml)
endif()

77
Localizations/Libraries/Ros/amcl/cfg/AMCL.cfg Normal file
View File

@@ -0,0 +1,77 @@
#!/usr/bin/env python
PACKAGE = 'amcl'
from math import pi
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, int_t, double_t, str_t, bool_t
gen = ParameterGenerator()
gen.add("min_particles", int_t, 0, "Minimum allowed number of particles.", 100, 0, 1000)
gen.add("max_particles", int_t, 0, "Mamimum allowed number of particles.", 5000, 0, 10000)
gen.add("kld_err", double_t, 0, "Maximum error between the true distribution and the estimated distribution.", .01, 0, 1)
gen.add("kld_z", double_t, 0, "Upper standard normal quantile for (1 - p), where p is the probability that the error on the estimated distribution will be less than kld_err.", .99, 0, 1)
gen.add("update_min_d", double_t, 0, "Translational movement required before performing a filter update.", .2, 0, 5)
gen.add("update_min_a", double_t, 0, "Rotational movement required before performing a filter update.", pi/6, 0, 2*pi)
gen.add("resample_interval", int_t, 0, "Number of filter updates required before resampling.", 2, 0, 20)
gen.add("transform_tolerance", double_t, 0, "Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.", .1, 0, 2)
gen.add("recovery_alpha_slow", double_t, 0, "Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.", 0, 0, .5)
gen.add("recovery_alpha_fast", double_t, 0, "Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.", 0, 0, 1)
gen.add("do_beamskip", bool_t, 0, "When true skips laser scans when a scan doesnt work for a majority of particles", False)
gen.add("beam_skip_distance", double_t, 0, "Distance from a valid map point before scan is considered invalid", 0.5, 0, 2)
gen.add("beam_skip_threshold", double_t, 0, "Ratio of samples for which the scans are valid to consider as valid scan", 0.3, 0, 1)
gen.add("tf_broadcast", bool_t, 0, "When true (the default), publish results via TF. When false, do not.", True)
gen.add("force_update_after_initialpose", bool_t, 0, "When true, force a pose update after a new /initialpose is received.", False)
gen.add("force_update_after_set_map", bool_t, 0, "When true, force a pose update after a new map (and pose) has been set via the /set_map service.", False)
gen.add("gui_publish_rate", double_t, 0, "Maximum rate (Hz) at which scans and paths are published for visualization, -1.0 to disable.", -1, -1, 100)
gen.add("save_pose_rate", double_t, 0, "Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter. -1.0 to disable.", .5, -1, 10)
gen.add("use_map_topic", bool_t, 0, "When set to true, AMCL will subscribe to the map topic rather than making a service call to receive its map.", False)
gen.add("first_map_only", bool_t, 0, "When set to true, AMCL will only use the first map it subscribes to, rather than updating each time a new one is received.", False)
# Laser Model Parameters
gen.add("laser_min_range", double_t, 0, "Minimum scan range to be considered; -1.0 will cause the laser's reported minimum range to be used.", -1, -1, 1000)
gen.add("laser_max_range", double_t, 0, "Maximum scan range to be considered; -1.0 will cause the laser's reported maximum range to be used.", -1, -1, 1000)
gen.add("laser_max_beams", int_t, 0, "How many evenly-spaced beams in each scan to be used when updating the filter.", 30, 0, 250)
gen.add("laser_z_hit", double_t, 0, "Mixture weight for the z_hit part of the model.", .95, 0, 1)
gen.add("laser_z_short", double_t, 0, "Mixture weight for the z_short part of the model.", .1, 0, 1)
gen.add("laser_z_max", double_t, 0, "Mixture weight for the z_max part of the model.", .05, 0, 1)
gen.add("laser_z_rand", double_t, 0, "Mixture weight for the z_rand part of the model.", .05, 0, 1)
gen.add("laser_sigma_hit", double_t, 0, "Standard deviation for Gaussian model used in z_hit part of the model.", .2, 0, 10)
gen.add("laser_lambda_short", double_t, 0, "Exponential decay parameter for z_short part of model.", .1, 0, 10)
gen.add("laser_likelihood_max_dist", double_t, 0, "Maximum distance to do obstacle inflation on map, for use in likelihood_field model.", 2, 0, 20)
lmt = gen.enum([gen.const("beam_const", str_t, "beam", "Use beam laser model"), gen.const("likelihood_field_const", str_t, "likelihood_field", "Use likelihood_field laser model"), gen.const("likelihood_field_prob", str_t, "likelihood_field_prob", "Use likelihood_field_prob laser model")], "Laser Models")
gen.add("laser_model_type", str_t, 0, "Which model to use, either beam, likelihood_field or likelihood_field_prob.", "likelihood_field", edit_method=lmt)
# Odometry Model Parameters
odt = gen.enum([gen.const("diff_const", str_t, "diff", "Use diff odom model"),
gen.const("omni_const", str_t, "omni", "Use omni odom model"),
gen.const("diff_corrected_const", str_t, "diff-corrected", "Use corrected diff odom model"),
gen.const("omni_corrected_const", str_t, "omni-corrected", "Use corrected omni odom model")],
"Odom Models")
gen.add("odom_model_type", str_t, 0, "Which model to use, diff, omni, diff-corrected, or omni-corrected", "diff", edit_method=odt)
gen.add("odom_alpha1", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the rotational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha2", double_t, 0, "Specifies the expected noise in odometry's rotation estimate from the translational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha3", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the translational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha4", double_t, 0, "Specifies the expected noise in odometry's translation estimate from the rotational component of the robot's motion.", .2, 0, 10)
gen.add("odom_alpha5", double_t, 0, "Translation-related noise parameter (only used if model is omni).", .2, 0, 10)
gen.add("odom_frame_id", str_t, 0, "Which frame to use for odometry.", "odom")
gen.add("base_frame_id", str_t, 0, "Which frame to use for the robot base.", "base_link")
gen.add("global_frame_id", str_t, 0, "The name of the coordinate frame published by the localization system.", "map")
gen.add("restore_defaults", bool_t, 0, "Retsore the default configuration", False)
exit(gen.generate(PACKAGE, "amcl_node", "AMCL"))

34
Localizations/Libraries/Ros/amcl/examples/amcl_diff.launch Normal file
View File

@@ -0,0 +1,34 @@
<launch>
<node pkg="amcl" type="amcl" name="amcl" output="screen">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

34
Localizations/Libraries/Ros/amcl/examples/amcl_omni.launch Normal file
View File

@@ -0,0 +1,34 @@
<launch>
<node pkg="amcl" type="amcl" name="amcl">
<!-- Publish scans from best pose at a max of 10 Hz -->
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha5" value="0.1"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<!-- translation std dev, m -->
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<!-- <param name="laser_model_type" value="beam"/> -->
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
</launch>

293
Localizations/Libraries/Ros/amcl/include/amcl/amcl.h Normal file
View File

@@ -0,0 +1,293 @@
#ifndef _AMCL_LIBRARY_H_INCLUDED_
#define _AMCL_LIBRARY_H_INCLUDED_
#include <algorithm>
#include <vector>
#include <map>
#include <cmath>
#include <memory>
#include <boost/bind.hpp>
#include <boost/thread/mutex.hpp>
#include "amcl/map/map.h"
#include "amcl/pf/pf.h"
#include "amcl/sensors/amcl_odom.h"
#include "amcl/sensors/amcl_laser.h"
#include "amcl/portable_utils.hpp"
#include "ros/assert.h"
// roscpp
#include "ros/ros.h"
// Messages that I need
#include "sensor_msgs/LaserScan.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/PoseArray.h"
#include "geometry_msgs/Pose.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "nav_msgs/GetMap.h"
#include "nav_msgs/SetMap.h"
#include "std_srvs/Empty.h"
// For transform support
#include "tf2/LinearMath/Transform.h"
#include "tf2/convert.h"
#include "tf2/utils.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
#include "tf2_ros/buffer.h"
#include "tf2_ros/message_filter.h"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2_ros/transform_listener.h"
#include "message_filters/subscriber.h"
// Dynamic_reconfigure
#include "dynamic_reconfigure/server.h"
#include "amcl/AMCLConfig.h"
// Allows AMCL to run from bag file
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
// For monitoring the estimator
#include <diagnostic_updater/diagnostic_updater.h>
// using namespace amcl;
#define NEW_UNIFORM_SAMPLING 1
// Pose hypothesis
typedef struct
{
// Total weight (weights sum to 1)
double weight;
// Mean of pose esimate
pf_vector_t pf_pose_mean;
// Covariance of pose estimate
pf_matrix_t pf_pose_cov;
} amcl_hyp_t;
static double
normalize(double z)
{
return atan2(sin(z), cos(z));
}
static double
angle_diff(double a, double b)
{
double d1, d2;
a = normalize(a);
b = normalize(b);
d1 = a - b;
d2 = 2 * M_PI - fabs(d1);
if (d1 > 0)
d2 *= -1.0;
if (fabs(d1) < fabs(d2))
return(d1);
else
return(d2);
}
/* This function is only useful to have the whole code work
* with old rosbags that have trailing slashes for their frames
*/
inline
std::string stripSlash(const std::string& in)
{
std::string out = in;
if ((!in.empty()) && (in[0] == '/'))
out.erase(0, 1);
return out;
}
namespace amcl
{
class Amcl
{
public:
Amcl();
Amcl(ros::NodeHandle nh);
virtual ~Amcl();
/**
* @brief Uses TF and LaserScan messages from bag file to drive AMCL instead
* @param in_bag_fn input bagfile
* @param trigger_global_localization whether to trigger global localization
* before starting to process the bagfile
*/
void runFromBag(const std::string& in_bag_fn, bool trigger_global_localization = false);
int process();
void savePoseToServer(geometry_msgs::PoseWithCovarianceStamped &amcl_pose);
bool getInitialized() {return initialized_;}
private:
bool initialized_;
std::shared_ptr<tf2_ros::TransformBroadcaster> tfb_;
std::shared_ptr<tf2_ros::TransformListener> tfl_;
std::shared_ptr<tf2_ros::Buffer> tf_;
bool sent_first_transform_;
tf2::Transform latest_tf_;
bool latest_tf_valid_;
// Pose-generating function used to uniformly distribute particles over
// the map
static pf_vector_t uniformPoseGenerator(void* arg);
#if NEW_UNIFORM_SAMPLING
static std::vector<std::pair<int, int> > free_space_indices;
#endif
// Callbacks
bool globalLocalizationCallback(std_srvs::Empty::Request& req,
std_srvs::Empty::Response& res);
bool nomotionUpdateCallback(std_srvs::Empty::Request& req,
std_srvs::Empty::Response& res);
bool setMapCallback(nav_msgs::SetMap::Request& req,
nav_msgs::SetMap::Response& res);
void initialize(ros::NodeHandle nh);
void laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan);
void initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg);
void handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg);
void mapReceived(const nav_msgs::OccupancyGridConstPtr& msg);
void handleMapMessage(const nav_msgs::OccupancyGrid& msg);
void freeMapDependentMemory();
map_t* convertMap(const nav_msgs::OccupancyGrid& map_msg);
void updatePoseFromServer();
void applyInitialPose();
//parameter for which odom to use
std::string odom_frame_id_;
//paramater to store latest odom pose
geometry_msgs::PoseStamped latest_odom_pose_;
//parameter for which base to use
std::string base_frame_id_;
std::string global_frame_id_;
bool use_map_topic_;
bool first_map_only_;
ros::Duration gui_publish_period;
ros::Time save_pose_last_time;
ros::Duration save_pose_period;
geometry_msgs::PoseWithCovarianceStamped last_published_pose;
map_t* map_;
char* mapdata;
int sx, sy;
double resolution;
message_filters::Subscriber<sensor_msgs::LaserScan>* laser_scan_sub_;
tf2_ros::MessageFilter<sensor_msgs::LaserScan>* laser_scan_filter_;
ros::Subscriber initial_pose_sub_;
std::vector< amcl::AMCLLaser* > lasers_;
std::vector< bool > lasers_update_;
std::map< std::string, int > frame_to_laser_;
// Particle filter
pf_t* pf_;
double pf_err_, pf_z_;
bool pf_init_;
pf_vector_t pf_odom_pose_;
double d_thresh_, a_thresh_;
int resample_interval_;
int resample_count_;
double laser_min_range_;
double laser_max_range_;
//Nomotion update control
bool m_force_update; // used to temporarily let amcl update samples even when no motion occurs...
amcl::AMCLOdom* odom_;
amcl::AMCLLaser* laser_;
ros::Duration cloud_pub_interval;
ros::Time last_cloud_pub_time;
// For slowing play-back when reading directly from a bag file
ros::WallDuration bag_scan_period_;
void requestMap();
// Helper to get odometric pose from transform system
bool getOdomPose(geometry_msgs::PoseStamped& pose,
double& x, double& y, double& yaw,
const ros::Time& t, const std::string& f);
//time for tolerance on the published transform,
//basically defines how long a map->odom transform is good for
ros::Duration transform_tolerance_;
ros::NodeHandle nh_;
ros::NodeHandle private_nh_;
ros::Publisher pose_pub_;
ros::Publisher particlecloud_pub_;
ros::ServiceServer global_loc_srv_;
ros::ServiceServer nomotion_update_srv_; //to let amcl update samples without requiring motion
ros::ServiceServer set_map_srv_;
ros::Subscriber initial_pose_sub_old_;
ros::Subscriber map_sub_;
diagnostic_updater::Updater diagnosic_updater_;
void standardDeviationDiagnostics(diagnostic_updater::DiagnosticStatusWrapper& diagnostic_status);
double std_warn_level_x_;
double std_warn_level_y_;
double std_warn_level_yaw_;
std::string scan_topic_;
std::string map_topic_;
std::string map_service_;
amcl_hyp_t* initial_pose_hyp_;
bool first_map_received_;
bool first_reconfigure_call_;
boost::recursive_mutex configuration_mutex_;
dynamic_reconfigure::Server<amcl::AMCLConfig>* dsrv_;
amcl::AMCLConfig default_config_;
ros::Timer check_laser_timer_;
int max_beams_, min_particles_, max_particles_;
double alpha1_, alpha2_, alpha3_, alpha4_, alpha5_;
double alpha_slow_, alpha_fast_;
double z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_;
//beam skip related params
bool do_beamskip_;
double beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_;
double laser_likelihood_max_dist_;
amcl::odom_model_t odom_model_type_;
double init_pose_[3];
double init_cov_[3];
amcl::laser_model_t laser_model_type_;
bool tf_broadcast_;
bool force_update_after_initialpose_;
bool force_update_after_set_map_;
bool selective_resampling_;
ros::Time last_tfb_time_;
void reconfigureCB(amcl::AMCLConfig& config, uint32_t level);
ros::Time last_laser_received_ts_;
ros::Duration laser_check_interval_;
void checkLaserReceived(const ros::TimerEvent& event);
};
} // namespace amcl
#define USAGE "USAGE: amcl"
#if NEW_UNIFORM_SAMPLING
std::vector<std::pair<int, int> > amcl::Amcl::free_space_indices;
#endif
#endif // _AMCL_LIBRARY_H_INCLUDED_

150
Localizations/Libraries/Ros/amcl/include/amcl/map/map.h Normal file
View File

@@ -0,0 +1,150 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map (grid-based)
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map.h 1713 2003-08-23 04:03:43Z inspectorg $
**************************************************************************/
#ifndef MAP_H
#define MAP_H
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
#endif
// Forward declarations
struct _rtk_fig_t;
// Limits
#define MAP_WIFI_MAX_LEVELS 8
// Description for a single map cell.
typedef struct
{
// Occupancy state (-1 = free, 0 = unknown, +1 = occ)
int occ_state;
// Distance to the nearest occupied cell
double occ_dist;
// Wifi levels
//int wifi_levels[MAP_WIFI_MAX_LEVELS];
} map_cell_t;
// Description for a map
typedef struct
{
// Map origin; the map is a viewport onto a conceptual larger map.
double origin_x, origin_y;
// Map scale (m/cell)
double scale;
// Map dimensions (number of cells)
int size_x, size_y;
// The map data, stored as a grid
map_cell_t *cells;
// Max distance at which we care about obstacles, for constructing
// likelihood field
double max_occ_dist;
} map_t;
/**************************************************************************
* Basic map functions
**************************************************************************/
// Create a new (empty) map
map_t *map_alloc(void);
// Destroy a map
void map_free(map_t *map);
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa);
// Load an occupancy map
int map_load_occ(map_t *map, const char *filename, double scale, int negate);
// Load a wifi signal strength map
//int map_load_wifi(map_t *map, const char *filename, int index);
// Update the cspace distances
void map_update_cspace(map_t *map, double max_occ_dist);
/**************************************************************************
* Range functions
**************************************************************************/
// Extract a single range reading from the map
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range);
/**************************************************************************
* GUI/diagnostic functions
**************************************************************************/
// Draw the occupancy grid
void map_draw_occ(map_t *map, struct _rtk_fig_t *fig);
// Draw the cspace map
void map_draw_cspace(map_t *map, struct _rtk_fig_t *fig);
// Draw a wifi map
void map_draw_wifi(map_t *map, struct _rtk_fig_t *fig, int index);
/**************************************************************************
* Map manipulation macros
**************************************************************************/
// Convert from map index to world coords
#define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale)
#define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale)
// Convert from world coords to map coords
#define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2)
#define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2)
// Test to see if the given map coords lie within the absolute map bounds.
#define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y))
// Compute the cell index for the given map coords.
#define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
#ifdef __cplusplus
}
#endif
#endif

11
Localizations/Libraries/Ros/amcl/include/amcl/pf/eig3.h Normal file
View File

@@ -0,0 +1,11 @@
/* Eigen-decomposition for symmetric 3x3 real matrices.
Public domain, copied from the public domain Java library JAMA. */
#ifndef _eig_h
/* Symmetric matrix A => eigenvectors in columns of V, corresponding
eigenvalues in d. */
void eigen_decomposition(double A[3][3], double V[3][3], double d[3]);
#endif

210
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf.h Normal file
View File

@@ -0,0 +1,210 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Simple particle filter for localization.
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf.h 3293 2005-11-19 08:37:45Z gerkey $
*************************************************************************/
#ifndef PF_H
#define PF_H
#include "pf_vector.h"
#include "pf_kdtree.h"
#ifdef __cplusplus
extern "C" {
#endif
// Forward declarations
struct _pf_t;
struct _rtk_fig_t;
struct _pf_sample_set_t;
// Function prototype for the initialization model; generates a sample pose from
// an appropriate distribution.
typedef pf_vector_t (*pf_init_model_fn_t) (void *init_data);
// Function prototype for the action model; generates a sample pose from
// an appropriate distribution
typedef void (*pf_action_model_fn_t) (void *action_data,
struct _pf_sample_set_t* set);
// Function prototype for the sensor model; determines the probability
// for the given set of sample poses.
typedef double (*pf_sensor_model_fn_t) (void *sensor_data,
struct _pf_sample_set_t* set);
// Information for a single sample
typedef struct
{
// Pose represented by this sample
pf_vector_t pose;
// Weight for this pose
double weight;
} pf_sample_t;
// Information for a cluster of samples
typedef struct
{
// Number of samples
int count;
// Total weight of samples in this cluster
double weight;
// Cluster statistics
pf_vector_t mean;
pf_matrix_t cov;
// Workspace
double m[4], c[2][2];
} pf_cluster_t;
// Information for a set of samples
typedef struct _pf_sample_set_t
{
// The samples
int sample_count;
pf_sample_t *samples;
// A kdtree encoding the histogram
pf_kdtree_t *kdtree;
// Clusters
int cluster_count, cluster_max_count;
pf_cluster_t *clusters;
// Filter statistics
pf_vector_t mean;
pf_matrix_t cov;
int converged;
double n_effective;
} pf_sample_set_t;
// Information for an entire filter
typedef struct _pf_t
{
// This min and max number of samples
int min_samples, max_samples;
// Population size parameters
double pop_err, pop_z;
// Resample limit cache
int *limit_cache;
// The sample sets. We keep two sets and use [current_set]
// to identify the active set.
int current_set;
pf_sample_set_t sets[2];
// Running averages, slow and fast, of likelihood
double w_slow, w_fast;
// Decay rates for running averages
double alpha_slow, alpha_fast;
// Function used to draw random pose samples
pf_init_model_fn_t random_pose_fn;
void *random_pose_data;
double dist_threshold; //distance threshold in each axis over which the pf is considered to not be converged
int converged;
// boolean parameter to enamble/diable selective resampling
int selective_resampling;
} pf_t;
// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void *random_pose_data);
// Free an existing filter
void pf_free(pf_t *pf);
// Initialize the filter using a guassian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov);
// Initialize the filter using some model
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data);
// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data);
// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data);
// Resample the distribution
void pf_update_resample(pf_t *pf);
// set selective resampling parameter
void pf_set_selective_resampling(pf_t *pf, int selective_resampling);
// Compute the CEP statistics (mean and variance).
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var);
// Compute the statistics for a particular cluster. Returns 0 if
// there is no such cluster.
int pf_get_cluster_stats(pf_t *pf, int cluster, double *weight,
pf_vector_t *mean, pf_matrix_t *cov);
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set);
// Display the sample set
void pf_draw_samples(pf_t *pf, struct _rtk_fig_t *fig, int max_samples);
// Draw the histogram (kdtree)
void pf_draw_hist(pf_t *pf, struct _rtk_fig_t *fig);
// Draw the CEP statistics
void pf_draw_cep_stats(pf_t *pf, struct _rtk_fig_t *fig);
// Draw the cluster statistics
void pf_draw_cluster_stats(pf_t *pf, struct _rtk_fig_t *fig);
//calculate if the particle filter has converged -
//and sets the converged flag in the current set and the pf
int pf_update_converged(pf_t *pf);
//sets the current set and pf converged values to zero
void pf_init_converged(pf_t *pf);
void pf_copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b);
#ifdef __cplusplus
}
#endif
#endif

109
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_kdtree.h Normal file
View File

@@ -0,0 +1,109 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: KD tree functions
* Author: Andrew Howard
* Date: 18 Dec 2002
* CVS: $Id: pf_kdtree.h 6532 2008-06-11 02:45:56Z gbiggs $
*************************************************************************/
#ifndef PF_KDTREE_H
#define PF_KDTREE_H
#ifdef INCLUDE_RTKGUI
#include "rtk.h"
#endif
// Info for a node in the tree
typedef struct pf_kdtree_node
{
// Depth in the tree
int leaf, depth;
// Pivot dimension and value
int pivot_dim;
double pivot_value;
// The key for this node
int key[3];
// The value for this node
double value;
// The cluster label (leaf nodes)
int cluster;
// Child nodes
struct pf_kdtree_node *children[2];
} pf_kdtree_node_t;
// A kd tree
typedef struct
{
// Cell size
double size[3];
// The root node of the tree
pf_kdtree_node_t *root;
// The number of nodes in the tree
int node_count, node_max_count;
pf_kdtree_node_t *nodes;
// The number of leaf nodes in the tree
int leaf_count;
} pf_kdtree_t;
// Create a tree
extern pf_kdtree_t *pf_kdtree_alloc(int max_size);
// Destroy a tree
extern void pf_kdtree_free(pf_kdtree_t *self);
// Clear all entries from the tree
extern void pf_kdtree_clear(pf_kdtree_t *self);
// Insert a pose into the tree
extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value);
// Cluster the leaves in the tree
extern void pf_kdtree_cluster(pf_kdtree_t *self);
// Determine the probability estimate for the given pose
extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose);
// Determine the cluster label for the given pose
extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose);
#ifdef INCLUDE_RTKGUI
// Draw the tree
extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig);
#endif
#endif

85
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_pdf.h Normal file
View File

@@ -0,0 +1,85 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Useful pdf functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_pdf.h 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#ifndef PF_PDF_H
#define PF_PDF_H
#include "pf_vector.h"
//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>
#ifdef __cplusplus
extern "C" {
#endif
/**************************************************************************
* Gaussian
*************************************************************************/
// Gaussian PDF info
typedef struct
{
// Mean, covariance and inverse covariance
pf_vector_t x;
pf_matrix_t cx;
//pf_matrix_t cxi;
double cxdet;
// Decomposed covariance matrix (rotation * diagonal)
pf_matrix_t cr;
pf_vector_t cd;
// A random number generator
//gsl_rng *rng;
} pf_pdf_gaussian_t;
// Create a gaussian pdf
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx);
// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf);
// Compute the value of the pdf at some point [z].
//double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t z);
// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
// http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma);
// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf);
#ifdef __cplusplus
}
#endif
#endif

94
Localizations/Libraries/Ros/amcl/include/amcl/pf/pf_vector.h Normal file
View File

@@ -0,0 +1,94 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Vector functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_vector.h 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#ifndef PF_VECTOR_H
#define PF_VECTOR_H
#ifdef __cplusplus
extern "C" {
#endif
#include <stdio.h>
// The basic vector
typedef struct
{
double v[3];
} pf_vector_t;
// The basic matrix
typedef struct
{
double m[3][3];
} pf_matrix_t;
// Return a zero vector
pf_vector_t pf_vector_zero();
// Check for NAN or INF in any component
int pf_vector_finite(pf_vector_t a);
// Print a vector
void pf_vector_fprintf(pf_vector_t s, FILE *file, const char *fmt);
// Simple vector addition
pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b);
// Simple vector subtraction
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b);
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b);
// Transform from global to local coords (a - b)
pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b);
// Return a zero matrix
pf_matrix_t pf_matrix_zero();
// Check for NAN or INF in any component
int pf_matrix_finite(pf_matrix_t a);
// Print a matrix
void pf_matrix_fprintf(pf_matrix_t s, FILE *file, const char *fmt);
// Compute the matrix inverse. Will also return the determinant,
// which should be checked for underflow (indicated singular matrix).
//pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det);
// Decompose a covariance matrix [a] into a rotation matrix [r] and a
// diagonal matrix [d] such that a = r * d * r^T.
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a);
#ifdef __cplusplus
}
#endif
#endif

35
Localizations/Libraries/Ros/amcl/include/amcl/portable_utils.hpp Normal file
View File

@@ -0,0 +1,35 @@
#ifndef PORTABLE_UTILS_H
#define PORTABLE_UTILS_H
#include <stdlib.h>
#ifdef __cplusplus
extern "C" {
#endif
#ifndef HAVE_DRAND48
// Some system (e.g., Windows) doesn't come with drand48(), srand48().
// Use rand, and srand for such system.
// Remove extern declarations to avoid conflicts
// extern double drand48(void); // Change this to static if it should be static
extern double drand48(void)
{
return ((double)rand())/RAND_MAX;
}
// Remove extern declarations to avoid conflicts
// extern double srand48(long int seedval); // Change this to static if it should be static
extern void srand48(long int seedval)
{
srand(seedval);
}
#endif
#ifdef __cplusplus
}
#endif
#endif

156
Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.h Normal file
View File

@@ -0,0 +1,156 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey et al.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: LASER sensor model for AMCL
// Author: Andrew Howard
// Date: 17 Aug 2003
// CVS: $Id: amcl_laser.h 6443 2008-05-15 19:46:11Z gerkey $
//
///////////////////////////////////////////////////////////////////////////
#ifndef AMCL_LASER_H
#define AMCL_LASER_H
#include "amcl_sensor.h"
#include "../map/map.h"
namespace amcl
{
typedef enum
{
LASER_MODEL_BEAM,
LASER_MODEL_LIKELIHOOD_FIELD,
LASER_MODEL_LIKELIHOOD_FIELD_PROB
} laser_model_t;
// Laser sensor data
class AMCLLaserData : public AMCLSensorData
{
public:
AMCLLaserData () {ranges=NULL;};
virtual ~AMCLLaserData() {delete [] ranges;};
// Laser range data (range, bearing tuples)
public: int range_count;
public: double range_max;
public: double (*ranges)[2];
};
// Laseretric sensor model
class AMCLLaser : public AMCLSensor
{
// Default constructor
public: AMCLLaser(size_t max_beams, map_t* map);
public: virtual ~AMCLLaser();
public: void SetModelBeam(double z_hit,
double z_short,
double z_max,
double z_rand,
double sigma_hit,
double lambda_short,
double chi_outlier);
public: void SetModelLikelihoodField(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist);
//a more probabilistically correct model - also with the option to do beam skipping
public: void SetModelLikelihoodFieldProb(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist,
bool do_beamskip,
double beam_skip_distance,
double beam_skip_threshold,
double beam_skip_error_threshold);
// Update the filter based on the sensor model. Returns true if the
// filter has been updated.
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
// Set the laser's pose after construction
public: void SetLaserPose(pf_vector_t& laser_pose)
{this->laser_pose = laser_pose;}
// Determine the probability for the given pose
private: static double BeamModel(AMCLLaserData *data,
pf_sample_set_t* set);
// Determine the probability for the given pose
private: static double LikelihoodFieldModel(AMCLLaserData *data,
pf_sample_set_t* set);
// Determine the probability for the given pose - more probablistic model
private: static double LikelihoodFieldModelProb(AMCLLaserData *data,
pf_sample_set_t* set);
private: void reallocTempData(int max_samples, int max_obs);
private: laser_model_t model_type;
// Current data timestamp
private: double time;
// The laser map
private: map_t *map;
// Laser offset relative to robot
private: pf_vector_t laser_pose;
// Max beams to consider
private: int max_beams;
// Beam skipping parameters (used by LikelihoodFieldModelProb model)
private: bool do_beamskip;
private: double beam_skip_distance;
private: double beam_skip_threshold;
//threshold for the ratio of invalid beams - at which all beams are integrated to the likelihoods
//this would be an error condition
private: double beam_skip_error_threshold;
//temp data that is kept before observations are integrated to each particle (requried for beam skipping)
private: int max_samples;
private: int max_obs;
private: double **temp_obs;
// Laser model params
//
// Mixture params for the components of the model; must sum to 1
private: double z_hit;
private: double z_short;
private: double z_max;
private: double z_rand;
//
// Stddev of Gaussian model for laser hits.
private: double sigma_hit;
// Decay rate of exponential model for short readings.
private: double lambda_short;
// Threshold for outlier rejection (unused)
private: double chi_outlier;
};
}
#endif

54
Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_laser.puml Normal file
View File

@@ -0,0 +1,54 @@
@startuml
class Player {
+ Player()
}
class AMCLLaserData {
+ AMCLLaserData()
+ ~AMCLLaserData()
# range_count: int
# range_max: double
# ranges: double[][]
}
class AMCLLaser {
- model_type: laser_model_t
- time: double
- map: map_t
- laser_pose: pf_vector_t
- max_beams: int
- do_beamskip: bool
- beam_skip_distance: double
- beam_skip_threshold: double
- beam_skip_error_threshold: double
- max_samples: int
- max_obs: int
- temp_obs: double[][]
- z_hit: double
- z_short: double
- z_max: double
- z_rand: double
- sigma_hit: double
- lambda_short: double
- chi_outlier: double
+ AMCLLaser(max_beams: size_t, map: map_t)
+ ~AMCLLaser()
+ SetModelBeam(z_hit: double, z_short: double, z_max: double, z_rand: double, sigma_hit: double, lambda_short: double, chi_outlier: double)
+ SetModelLikelihoodField(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double)
+ SetModelLikelihoodFieldProb(z_hit: double, z_rand: double, sigma_hit: double, max_occ_dist: double, do_beamskip: bool, beam_skip_distance: double, beam_skip_threshold: double, beam_skip_error_threshold: double)
+ UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
+ SetLaserPose(laser_pose: pf_vector_t)
+ BeamModel(data: AMCLLaserData, set: pf_sample_set_t): double
+ LikelihoodFieldModel(data: AMCLLaserData, set: pf_sample_set_t): double
+ LikelihoodFieldModelProb(data: AMCLLaserData, set: pf_sample_set_t): double
+ reallocTempData(max_samples: int, max_obs: int)
}
Player --|> AMCLSensor
AMCLLaserData --|> AMCLSensorData
@enduml

98
Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.h Normal file
View File

@@ -0,0 +1,98 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey et al.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: Odometry sensor model for AMCL
// Author: Andrew Howard
// Date: 17 Aug 2003
// CVS: $Id: amcl_odom.h 4135 2007-08-23 19:58:48Z gerkey $
//
///////////////////////////////////////////////////////////////////////////
#ifndef AMCL_ODOM_H
#define AMCL_ODOM_H
#include "amcl_sensor.h"
#include "../pf/pf_pdf.h"
namespace amcl
{
typedef enum
{
ODOM_MODEL_DIFF,
ODOM_MODEL_OMNI,
ODOM_MODEL_DIFF_CORRECTED,
ODOM_MODEL_OMNI_CORRECTED
} odom_model_t;
// Odometric sensor data
class AMCLOdomData : public AMCLSensorData
{
// Odometric pose
public: pf_vector_t pose;
// Change in odometric pose
public: pf_vector_t delta;
};
// Odometric sensor model
class AMCLOdom : public AMCLSensor
{
// Default constructor
public: AMCLOdom();
public: void SetModelDiff(double alpha1,
double alpha2,
double alpha3,
double alpha4);
public: void SetModelOmni(double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5);
public: void SetModel( odom_model_t type,
double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5 = 0 );
// Update the filter based on the action model. Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
// Current data timestamp
private: double time;
// Model type
private: odom_model_t model_type;
// Drift parameters
private: double alpha1, alpha2, alpha3, alpha4, alpha5;
};
}
#endif

54
Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_odom.puml Normal file
View File

@@ -0,0 +1,54 @@
@startuml
class Player {
+ Player()
}
class AMCLSensor {
- is_action: bool
- pose: pf_vector_t
+ AMCLSensor()
+ ~AMCLSensor()
+ UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+ InitSensor(pf: pf_t, data: AMCLSensorData): bool
+ UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
}
class AMCLSensorData {
- sensor: AMCLSensor
+ AMCLSensorData()
+ ~AMCLSensorData()
}
class AMCLOdomData {
- pose: pf_vector_t
- delta: pf_vector_t
+ AMCLOdomData()
}
class AMCLOdom {
- time: double
- model_type: odom_model_t
- alpha1: double
- alpha2: double
- alpha3: double
- alpha4: double
- alpha5: double
+ AMCLOdom()
+ SetModelDiff(alpha1: double, alpha2: double, alpha3: double, alpha4: double)
+ SetModelOmni(alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double)
+ SetModel(type: odom_model_t, alpha1: double, alpha2: double, alpha3: double, alpha4: double, alpha5: double = 0)
+ UpdateAction(pf: pf_t, data: AMCLSensorData): bool
}
Player --|> AMCLSensor
AMCLSensorData --|> AMCLSensor
AMCLOdomData --|> AMCLSensorData
AMCLOdom --|> AMCLSensor
@enduml

97
Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.h Normal file
View File

@@ -0,0 +1,97 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey et al.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: Adaptive Monte-Carlo localization
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_sensor.h 6443 2008-05-15 19:46:11Z gerkey $
//
///////////////////////////////////////////////////////////////////////////
#ifndef AMCL_SENSOR_H
#define AMCL_SENSOR_H
#include "../pf/pf.h"
namespace amcl
{
// Forward declarations
class AMCLSensorData;
// Base class for all AMCL sensors
class AMCLSensor
{
// Default constructor
public: AMCLSensor();
// Default destructor
public: virtual ~AMCLSensor();
// Update the filter based on the action model. Returns true if the filter
// has been updated.
public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data);
// Initialize the filter based on the sensor model. Returns true if the
// filter has been initialized.
public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data);
// Update the filter based on the sensor model. Returns true if the
// filter has been updated.
public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data);
// Flag is true if this is the action sensor
public: bool is_action;
// Action pose (action sensors only)
public: pf_vector_t pose;
// AMCL Base
//protected: AdaptiveMCL & AMCL;
#ifdef INCLUDE_RTKGUI
// Setup the GUI
public: virtual void SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
// Finalize the GUI
public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig);
// Draw sensor data
public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data);
#endif
};
// Base class for all AMCL sensor measurements
class AMCLSensorData
{
// Pointer to sensor that generated the data
public: AMCLSensor *sensor;
virtual ~AMCLSensorData() {}
// Data timestamp
public: double time;
};
}
#endif

29
Localizations/Libraries/Ros/amcl/include/amcl/sensors/amcl_sensor.puml Normal file
View File

@@ -0,0 +1,29 @@
@startuml
class Player {
+ Player()
}
class AMCLSensor {
- is_action: bool
- pose: pf_vector_t
+ AMCLSensor()
+ ~AMCLSensor()
+ UpdateAction(pf: pf_t, data: AMCLSensorData): bool
+ InitSensor(pf: pf_t, data: AMCLSensorData): bool
+ UpdateSensor(pf: pf_t, data: AMCLSensorData): bool
}
class AMCLSensorData {
- sensor: AMCLSensor
+ AMCLSensorData()
+ ~AMCLSensorData()
}
Player --|> AMCLSensor
AMCLSensorData --|> AMCLSensor
@enduml

47
Localizations/Libraries/Ros/amcl/package.xml Normal file
View File

@@ -0,0 +1,47 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format2.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="2">
<name>amcl</name>
<version>1.17.3</version>
<description>
<p>
amcl is a probabilistic localization system for a robot moving in
2D. It implements the adaptive (or KLD-sampling) Monte Carlo
localization approach (as described by Dieter Fox), which uses a
particle filter to track the pose of a robot against a known map.
</p>
<p>
This node is derived, with thanks, from Andrew Howard's excellent
'amcl' Player driver.
</p>
</description>
<url>http://wiki.ros.org/amcl</url>
<author>Brian P. Gerkey</author>
<author>contradict@gmail.com</author>
<maintainer email="davidvlu@gmail.com">David V. Lu!!</maintainer>
<maintainer email="mfergs7@gmail.com">Michael Ferguson</maintainer>
<maintainer email="ahoy@fetchrobotics.com">Aaron Hoy</maintainer>
<license>LGPL</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_filters</build_depend>
<build_depend>tf2_geometry_msgs</build_depend>
<depend>diagnostic_updater</depend>
<depend>dynamic_reconfigure</depend>
<depend>geometry_msgs</depend>
<depend>nav_msgs</depend>
<depend>rosbag</depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>std_srvs</depend>
<depend>tf2</depend>
<depend>tf2_msgs</depend>
<depend>tf2_ros</depend>
<test_depend>map_server</test_depend>
<test_depend>rostest</test_depend>
<test_depend>python3-pykdl</test_depend>
<test_depend>tf2_py</test_depend>
</package>

1409
Localizations/Libraries/Ros/amcl/src/amcl.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

84
Localizations/Libraries/Ros/amcl/src/amcl/map/map.c Normal file
View File

@@ -0,0 +1,84 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map (grid-based)
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map.c 1713 2003-08-23 04:03:43Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include "amcl/map/map.h"
// Create a new map
map_t *map_alloc(void)
{
map_t *map;
map = (map_t*) malloc(sizeof(map_t));
// Assume we start at (0, 0)
map->origin_x = 0;
map->origin_y = 0;
// Make the size odd
map->size_x = 0;
map->size_y = 0;
map->scale = 0;
// Allocate storage for main map
map->cells = (map_cell_t*) NULL;
return map;
}
// Destroy a map
void map_free(map_t *map)
{
free(map->cells);
free(map);
return;
}
// Get the cell at the given point
map_cell_t *map_get_cell(map_t *map, double ox, double oy, double oa)
{
int i, j;
map_cell_t *cell;
i = MAP_GXWX(map, ox);
j = MAP_GYWY(map, oy);
if (!MAP_VALID(map, i, j))
return NULL;
cell = map->cells + MAP_INDEX(map, i, j);
return cell;
}

177
Localizations/Libraries/Ros/amcl/src/amcl/map/map_cspace.cpp Normal file
View File

@@ -0,0 +1,177 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <queue>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/map/map.h"
#include <ros/ros.h>
class CellData
{
public:
map_t* map_;
unsigned int i_, j_;
unsigned int src_i_, src_j_;
};
class CachedDistanceMap
{
public:
CachedDistanceMap(double scale, double max_dist) :
distances_(NULL), scale_(scale), max_dist_(max_dist)
{
cell_radius_ = max_dist / scale;
distances_ = new double *[cell_radius_+2];
for(int i=0; i<=cell_radius_+1; i++)
{
distances_[i] = new double[cell_radius_+2];
for(int j=0; j<=cell_radius_+1; j++)
{
distances_[i][j] = sqrt(i*i + j*j);
}
}
}
~CachedDistanceMap()
{
if(distances_)
{
for(int i=0; i<=cell_radius_+1; i++)
delete[] distances_[i];
delete[] distances_;
}
}
double** distances_;
double scale_;
double max_dist_;
int cell_radius_;
};
bool operator<(const CellData& a, const CellData& b)
{
return a.map_->cells[MAP_INDEX(a.map_, a.i_, a.j_)].occ_dist > a.map_->cells[MAP_INDEX(b.map_, b.i_, b.j_)].occ_dist;
}
CachedDistanceMap*
get_distance_map(double scale, double max_dist)
{
static CachedDistanceMap* cdm = NULL;
if(!cdm || (cdm->scale_ != scale) || (cdm->max_dist_ != max_dist))
{
if(cdm)
delete cdm;
cdm = new CachedDistanceMap(scale, max_dist);
}
return cdm;
}
void enqueue(map_t* map, int i, int j,
int src_i, int src_j,
std::priority_queue<CellData>& Q,
CachedDistanceMap* cdm,
unsigned char* marked)
{
if(marked[MAP_INDEX(map, i, j)])
return;
int di = abs(i - src_i);
int dj = abs(j - src_j);
double distance = cdm->distances_[di][dj];
if(distance > cdm->cell_radius_)
return;
map->cells[MAP_INDEX(map, i, j)].occ_dist = distance * map->scale;
CellData cell;
cell.map_ = map;
cell.i_ = i;
cell.j_ = j;
cell.src_i_ = src_i;
cell.src_j_ = src_j;
Q.push(cell);
marked[MAP_INDEX(map, i, j)] = 1;
}
// Update the cspace distance values
void map_update_cspace(map_t *map, double max_occ_dist)
{
unsigned char* marked;
std::priority_queue<CellData> Q;
marked = new unsigned char[map->size_x*map->size_y];
memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y);
map->max_occ_dist = max_occ_dist;
CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist);
// Enqueue all the obstacle cells
CellData cell;
cell.map_ = map;
for(int i=0; i<map->size_x; i++)
{
cell.src_i_ = cell.i_ = i;
for(int j=0; j<map->size_y; j++)
{
if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1)
{
map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0;
cell.src_j_ = cell.j_ = j;
marked[MAP_INDEX(map, i, j)] = 1;
Q.push(cell);
}
else
map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist;
}
}
while(!Q.empty())
{
CellData current_cell = Q.top();
if(current_cell.i_ > 0)
enqueue(map, current_cell.i_-1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if(current_cell.j_ > 0)
enqueue(map, current_cell.i_, current_cell.j_-1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.i_ < map->size_x - 1)
enqueue(map, current_cell.i_+1, current_cell.j_,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
if((int)current_cell.j_ < map->size_y - 1)
enqueue(map, current_cell.i_, current_cell.j_+1,
current_cell.src_i_, current_cell.src_j_,
Q, cdm, marked);
Q.pop();
}
delete[] marked;
}

158
Localizations/Libraries/Ros/amcl/src/amcl/map/map_draw.c Normal file
View File

@@ -0,0 +1,158 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Local map GUI functions
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
**************************************************************************/
#ifdef INCLUDE_RTKGUI
#include <errno.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <rtk.h>
#include "amcl/map/map.h"
////////////////////////////////////////////////////////////////////////////
// Draw the occupancy map
void map_draw_occ(map_t *map, rtk_fig_t *fig)
{
int i, j;
int col;
map_cell_t *cell;
uint16_t *image;
uint16_t *pixel;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
// Draw occupancy
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
pixel = image + (j * map->size_x + i);
col = 127 - 127 * cell->occ_state;
*pixel = RTK_RGB16(col, col, col);
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL);
free(image);
return;
}
////////////////////////////////////////////////////////////////////////////
// Draw the cspace map
void map_draw_cspace(map_t *map, rtk_fig_t *fig)
{
int i, j;
int col;
map_cell_t *cell;
uint16_t *image;
uint16_t *pixel;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
// Draw occupancy
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
pixel = image + (j * map->size_x + i);
col = 255 * cell->occ_dist / map->max_occ_dist;
*pixel = RTK_RGB16(col, col, col);
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, NULL);
free(image);
return;
}
////////////////////////////////////////////////////////////////////////////
// Draw a wifi map
void map_draw_wifi(map_t *map, rtk_fig_t *fig, int index)
{
int i, j;
int level, col;
map_cell_t *cell;
uint16_t *image, *mask;
uint16_t *ipix, *mpix;
image = malloc(map->size_x * map->size_y * sizeof(image[0]));
mask = malloc(map->size_x * map->size_y * sizeof(mask[0]));
// Draw wifi levels
for (j = 0; j < map->size_y; j++)
{
for (i = 0; i < map->size_x; i++)
{
cell = map->cells + MAP_INDEX(map, i, j);
ipix = image + (j * map->size_x + i);
mpix = mask + (j * map->size_x + i);
level = cell->wifi_levels[index];
if (cell->occ_state == -1 && level != 0)
{
col = 255 * (100 + level) / 100;
*ipix = RTK_RGB16(col, col, col);
*mpix = 1;
}
else
{
*mpix = 0;
}
}
}
// Draw the entire occupancy map as an image
rtk_fig_image(fig, map->origin_x, map->origin_y, 0,
map->scale, map->size_x, map->size_y, 16, image, mask);
free(mask);
free(image);
return;
}
#endif

120
Localizations/Libraries/Ros/amcl/src/amcl/map/map_range.c Normal file
View File

@@ -0,0 +1,120 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Range routines
* Author: Andrew Howard
* Date: 18 Jan 2003
* CVS: $Id: map_range.c 1347 2003-05-05 06:24:33Z inspectorg $
**************************************************************************/
#include <assert.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include "amcl/map/map.h"
// Extract a single range reading from the map. Unknown cells and/or
// out-of-bound cells are treated as occupied, which makes it easy to
// use Stage bitmap files.
double map_calc_range(map_t *map, double ox, double oy, double oa, double max_range)
{
// Bresenham raytracing
int x0,x1,y0,y1;
int x,y;
int xstep, ystep;
char steep;
int tmp;
int deltax, deltay, error, deltaerr;
x0 = MAP_GXWX(map,ox);
y0 = MAP_GYWY(map,oy);
x1 = MAP_GXWX(map,ox + max_range * cos(oa));
y1 = MAP_GYWY(map,oy + max_range * sin(oa));
if(abs(y1-y0) > abs(x1-x0))
steep = 1;
else
steep = 0;
if(steep)
{
tmp = x0;
x0 = y0;
y0 = tmp;
tmp = x1;
x1 = y1;
y1 = tmp;
}
deltax = abs(x1-x0);
deltay = abs(y1-y0);
error = 0;
deltaerr = deltay;
x = x0;
y = y0;
if(x0 < x1)
xstep = 1;
else
xstep = -1;
if(y0 < y1)
ystep = 1;
else
ystep = -1;
if(steep)
{
if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
else
{
if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
while(x != (x1 + xstep * 1))
{
x += xstep;
error += deltaerr;
if(2*error >= deltax)
{
y += ystep;
error -= deltax;
}
if(steep)
{
if(!MAP_VALID(map,y,x) || map->cells[MAP_INDEX(map,y,x)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
else
{
if(!MAP_VALID(map,x,y) || map->cells[MAP_INDEX(map,x,y)].occ_state > -1)
return sqrt((x-x0)*(x-x0) + (y-y0)*(y-y0)) * map->scale;
}
}
return max_range;
}

215
Localizations/Libraries/Ros/amcl/src/amcl/map/map_store.c Normal file
View File

@@ -0,0 +1,215 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Global map storage functions
* Author: Andrew Howard
* Date: 6 Feb 2003
* CVS: $Id: map_store.c 2951 2005-08-19 00:48:20Z gerkey $
**************************************************************************/
#include <errno.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/map/map.h"
////////////////////////////////////////////////////////////////////////////
// Load an occupancy grid
int map_load_occ(map_t *map, const char *filename, double scale, int negate)
{
FILE *file;
char magic[3];
int i, j;
int ch, occ;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
if ((fscanf(file, "%2s \n", magic) != 1) || (strcmp(magic, "P5") != 0))
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
fclose(file);
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
if(fscanf(file, " %d %d \n %d \n", &width, &height, &depth) != 3)
{
fprintf(stderr, "Failed ot read image dimensions");
return -1;
}
// Allocate space in the map
if (map->cells == NULL)
{
map->scale = scale;
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
//PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
// Black-on-white images
if (!negate)
{
if (ch < depth / 4)
occ = +1;
else if (ch > 3 * depth / 4)
occ = -1;
else
occ = 0;
}
// White-on-black images
else
{
if (ch < depth / 4)
occ = -1;
else if (ch > 3 * depth / 4)
occ = +1;
else
occ = 0;
}
if (!MAP_VALID(map, i, j))
continue;
cell = map->cells + MAP_INDEX(map, i, j);
cell->occ_state = occ;
}
}
fclose(file);
return 0;
}
////////////////////////////////////////////////////////////////////////////
// Load a wifi signal strength map
/*
int map_load_wifi(map_t *map, const char *filename, int index)
{
FILE *file;
char magic[3];
int i, j;
int ch, level;
int width, height, depth;
map_cell_t *cell;
// Open file
file = fopen(filename, "r");
if (file == NULL)
{
fprintf(stderr, "%s: %s\n", strerror(errno), filename);
return -1;
}
// Read ppm header
fscanf(file, "%10s \n", magic);
if (strcmp(magic, "P5") != 0)
{
fprintf(stderr, "incorrect image format; must be PGM/binary");
return -1;
}
// Ignore comments
while ((ch = fgetc(file)) == '#')
while (fgetc(file) != '\n');
ungetc(ch, file);
// Read image dimensions
fscanf(file, " %d %d \n %d \n", &width, &height, &depth);
// Allocate space in the map
if (map->cells == NULL)
{
map->size_x = width;
map->size_y = height;
map->cells = calloc(width * height, sizeof(map->cells[0]));
}
else
{
if (width != map->size_x || height != map->size_y)
{
//PLAYER_ERROR("map dimensions are inconsistent with prior map dimensions");
return -1;
}
}
// Read in the image
for (j = height - 1; j >= 0; j--)
{
for (i = 0; i < width; i++)
{
ch = fgetc(file);
if (!MAP_VALID(map, i, j))
continue;
if (ch == 0)
level = 0;
else
level = ch * 100 / 255 - 100;
cell = map->cells + MAP_INDEX(map, i, j);
cell->wifi_levels[index] = level;
}
}
fclose(file);
return 0;
}
*/

274
Localizations/Libraries/Ros/amcl/src/amcl/pf/eig3.c Normal file
View File

@@ -0,0 +1,274 @@
/* Eigen decomposition code for symmetric 3x3 matrices, copied from the public
domain Java Matrix library JAMA. */
#include <math.h>
#ifndef MAX
#define MAX(a, b) ((a)>(b)?(a):(b))
#endif
#ifdef _MSC_VER
#define n 3
#else
static int n = 3;
#endif
static double hypot2(double x, double y) {
return sqrt(x*x+y*y);
}
// Symmetric Householder reduction to tridiagonal form.
static void tred2(double V[n][n], double d[n], double e[n]) {
// This is derived from the Algol procedures tred2 by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i,j,k;
double f,g,h,hh;
for (j = 0; j < n; j++) {
d[j] = V[n-1][j];
}
// Householder reduction to tridiagonal form.
for (i = n-1; i > 0; i--) {
// Scale to avoid under/overflow.
double scale = 0.0;
double h = 0.0;
for (k = 0; k < i; k++) {
scale = scale + fabs(d[k]);
}
if (scale == 0.0) {
e[i] = d[i-1];
for (j = 0; j < i; j++) {
d[j] = V[i-1][j];
V[i][j] = 0.0;
V[j][i] = 0.0;
}
} else {
// Generate Householder vector.
for (k = 0; k < i; k++) {
d[k] /= scale;
h += d[k] * d[k];
}
f = d[i-1];
g = sqrt(h);
if (f > 0) {
g = -g;
}
e[i] = scale * g;
h = h - f * g;
d[i-1] = f - g;
for (j = 0; j < i; j++) {
e[j] = 0.0;
}
// Apply similarity transformation to remaining columns.
for (j = 0; j < i; j++) {
f = d[j];
V[j][i] = f;
g = e[j] + V[j][j] * f;
for (k = j+1; k <= i-1; k++) {
g += V[k][j] * d[k];
e[k] += V[k][j] * f;
}
e[j] = g;
}
f = 0.0;
for (j = 0; j < i; j++) {
e[j] /= h;
f += e[j] * d[j];
}
hh = f / (h + h);
for (j = 0; j < i; j++) {
e[j] -= hh * d[j];
}
for (j = 0; j < i; j++) {
f = d[j];
g = e[j];
for (k = j; k <= i-1; k++) {
V[k][j] -= (f * e[k] + g * d[k]);
}
d[j] = V[i-1][j];
V[i][j] = 0.0;
}
}
d[i] = h;
}
// Accumulate transformations.
for (i = 0; i < n-1; i++) {
V[n-1][i] = V[i][i];
V[i][i] = 1.0;
h = d[i+1];
if (h != 0.0) {
for (k = 0; k <= i; k++) {
d[k] = V[k][i+1] / h;
}
for (j = 0; j <= i; j++) {
g = 0.0;
for (k = 0; k <= i; k++) {
g += V[k][i+1] * V[k][j];
}
for (k = 0; k <= i; k++) {
V[k][j] -= g * d[k];
}
}
}
for (k = 0; k <= i; k++) {
V[k][i+1] = 0.0;
}
}
for (j = 0; j < n; j++) {
d[j] = V[n-1][j];
V[n-1][j] = 0.0;
}
V[n-1][n-1] = 1.0;
e[0] = 0.0;
}
// Symmetric tridiagonal QL algorithm.
static void tql2(double V[n][n], double d[n], double e[n]) {
// This is derived from the Algol procedures tql2, by
// Bowdler, Martin, Reinsch, and Wilkinson, Handbook for
// Auto. Comp., Vol.ii-Linear Algebra, and the corresponding
// Fortran subroutine in EISPACK.
int i,j,m,l,k;
double g,p,r,dl1,h,f,tst1,eps;
double c,c2,c3,el1,s,s2;
for (i = 1; i < n; i++) {
e[i-1] = e[i];
}
e[n-1] = 0.0;
f = 0.0;
tst1 = 0.0;
eps = pow(2.0,-52.0);
for (l = 0; l < n; l++) {
// Find small subdiagonal element
tst1 = MAX(tst1,fabs(d[l]) + fabs(e[l]));
m = l;
while (m < n) {
if (fabs(e[m]) <= eps*tst1) {
break;
}
m++;
}
// If m == l, d[l] is an eigenvalue,
// otherwise, iterate.
if (m > l) {
int iter = 0;
do {
iter = iter + 1; // (Could check iteration count here.)
// Compute implicit shift
g = d[l];
p = (d[l+1] - g) / (2.0 * e[l]);
r = hypot2(p,1.0);
if (p < 0) {
r = -r;
}
d[l] = e[l] / (p + r);
d[l+1] = e[l] * (p + r);
dl1 = d[l+1];
h = g - d[l];
for (i = l+2; i < n; i++) {
d[i] -= h;
}
f = f + h;
// Implicit QL transformation.
p = d[m];
c = 1.0;
c2 = c;
c3 = c;
el1 = e[l+1];
s = 0.0;
s2 = 0.0;
for (i = m-1; i >= l; i--) {
c3 = c2;
c2 = c;
s2 = s;
g = c * e[i];
h = c * p;
r = hypot2(p,e[i]);
e[i+1] = s * r;
s = e[i] / r;
c = p / r;
p = c * d[i] - s * g;
d[i+1] = h + s * (c * g + s * d[i]);
// Accumulate transformation.
for (k = 0; k < n; k++) {
h = V[k][i+1];
V[k][i+1] = s * V[k][i] + c * h;
V[k][i] = c * V[k][i] - s * h;
}
}
p = -s * s2 * c3 * el1 * e[l] / dl1;
e[l] = s * p;
d[l] = c * p;
// Check for convergence.
} while (fabs(e[l]) > eps*tst1);
}
d[l] = d[l] + f;
e[l] = 0.0;
}
// Sort eigenvalues and corresponding vectors.
for (i = 0; i < n-1; i++) {
k = i;
p = d[i];
for (j = i+1; j < n; j++) {
if (d[j] < p) {
k = j;
p = d[j];
}
}
if (k != i) {
d[k] = d[i];
d[i] = p;
for (j = 0; j < n; j++) {
p = V[j][i];
V[j][i] = V[j][k];
V[j][k] = p;
}
}
}
}
void eigen_decomposition(double A[n][n], double V[n][n], double d[n]) {
int i,j;
double e[n];
for (i = 0; i < n; i++) {
for (j = 0; j < n; j++) {
V[i][j] = A[i][j];
}
}
tred2(V, d, e);
tql2(V, d, e);
}

763
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf.c Normal file
View File

@@ -0,0 +1,763 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Simple particle filter for localization.
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>
#include "amcl/pf/pf.h"
#include "amcl/pf/pf_pdf.h"
#include "amcl/pf/pf_kdtree.h"
#include "amcl/portable_utils.hpp"
// Compute the required number of samples, given that there are k bins
// with samples in them.
static int pf_resample_limit(pf_t *pf, int k);
// Create a new filter
pf_t *pf_alloc(int min_samples, int max_samples,
double alpha_slow, double alpha_fast,
pf_init_model_fn_t random_pose_fn, void *random_pose_data)
{
int i, j;
pf_t *pf;
pf_sample_set_t *set;
pf_sample_t *sample;
srand48(time(NULL));
pf = calloc(1, sizeof(pf_t));
pf->random_pose_fn = random_pose_fn;
pf->random_pose_data = random_pose_data;
pf->min_samples = min_samples;
pf->max_samples = max_samples;
// Control parameters for the population size calculation. [err] is
// the max error between the true distribution and the estimated
// distribution. [z] is the upper standard normal quantile for (1 -
// p), where p is the probability that the error on the estimated
// distrubition will be less than [err].
pf->pop_err = 0.01;
pf->pop_z = 3;
pf->dist_threshold = 0.5;
// Number of leaf nodes is never higher than the max number of samples
pf->limit_cache = calloc(max_samples, sizeof(int));
pf->current_set = 0;
for (j = 0; j < 2; j++)
{
set = pf->sets + j;
set->sample_count = max_samples;
set->samples = calloc(max_samples, sizeof(pf_sample_t));
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->pose.v[0] = 0.0;
sample->pose.v[1] = 0.0;
sample->pose.v[2] = 0.0;
sample->weight = 1.0 / max_samples;
}
// HACK: is 3 times max_samples enough?
set->kdtree = pf_kdtree_alloc(3 * max_samples);
set->cluster_count = 0;
set->cluster_max_count = max_samples;
set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t));
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
}
pf->w_slow = 0.0;
pf->w_fast = 0.0;
pf->alpha_slow = alpha_slow;
pf->alpha_fast = alpha_fast;
//set converged to 0
pf_init_converged(pf);
return pf;
}
// Free an existing filter
void pf_free(pf_t *pf)
{
int i;
free(pf->limit_cache);
for (i = 0; i < 2; i++)
{
free(pf->sets[i].clusters);
pf_kdtree_free(pf->sets[i].kdtree);
free(pf->sets[i].samples);
}
free(pf);
return;
}
// Initialize the filter using a gaussian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
pf_pdf_gaussian_t *pdf;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set->kdtree);
set->sample_count = pf->max_samples;
pdf = pf_pdf_gaussian_alloc(mean, cov);
// Compute the new sample poses
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = pf_pdf_gaussian_sample(pdf);
// Add sample to histogram
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
pf->w_slow = pf->w_fast = 0.0;
pf_pdf_gaussian_free(pdf);
// Re-compute cluster statistics
pf_cluster_stats(pf, set);
//set converged to 0
pf_init_converged(pf);
return;
}
// Initialize the filter using some model
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
set = pf->sets + pf->current_set;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set->kdtree);
set->sample_count = pf->max_samples;
// Compute the new sample poses
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / pf->max_samples;
sample->pose = (*init_fn) (init_data);
// Add sample to histogram
pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
}
pf->w_slow = pf->w_fast = 0.0;
// Re-compute cluster statistics
pf_cluster_stats(pf, set);
//set converged to 0
pf_init_converged(pf);
return;
}
void pf_init_converged(pf_t *pf){
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
set->converged = 0;
pf->converged = 0;
}
int pf_update_converged(pf_t *pf)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
double total;
set = pf->sets + pf->current_set;
double mean_x = 0, mean_y = 0;
for (i = 0; i < set->sample_count; i++){
sample = set->samples + i;
mean_x += sample->pose.v[0];
mean_y += sample->pose.v[1];
}
mean_x /= set->sample_count;
mean_y /= set->sample_count;
for (i = 0; i < set->sample_count; i++){
sample = set->samples + i;
if(fabs(sample->pose.v[0] - mean_x) > pf->dist_threshold ||
fabs(sample->pose.v[1] - mean_y) > pf->dist_threshold){
set->converged = 0;
pf->converged = 0;
return 0;
}
}
set->converged = 1;
pf->converged = 1;
return 1;
}
// Update the filter with some new action
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
{
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
(*action_fn) (action_data, set);
return;
}
#include <float.h>
// Update the filter with some new sensor observation
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
{
int i;
pf_sample_set_t *set;
pf_sample_t *sample;
double total;
set = pf->sets + pf->current_set;
// Compute the sample weights
total = (*sensor_fn) (sensor_data, set);
set->n_effective = 0;
if (total > 0.0)
{
// Normalize weights
double w_avg=0.0;
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
w_avg += sample->weight;
sample->weight /= total;
set->n_effective += sample->weight*sample->weight;
}
// Update running averages of likelihood of samples (Prob Rob p258)
w_avg /= set->sample_count;
if(pf->w_slow == 0.0)
pf->w_slow = w_avg;
else
pf->w_slow += pf->alpha_slow * (w_avg - pf->w_slow);
if(pf->w_fast == 0.0)
pf->w_fast = w_avg;
else
pf->w_fast += pf->alpha_fast * (w_avg - pf->w_fast);
//printf("w_avg: %e slow: %e fast: %e\n",
//w_avg, pf->w_slow, pf->w_fast);
}
else
{
// Handle zero total
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
sample->weight = 1.0 / set->sample_count;
}
}
set->n_effective = 1.0/set->n_effective;
return;
}
// copy set a to set b
void copy_set(pf_sample_set_t* set_a, pf_sample_set_t* set_b)
{
int i;
double total;
pf_sample_t *sample_a, *sample_b;
// Clean set b's kdtree
pf_kdtree_clear(set_b->kdtree);
// Copy samples from set a to create set b
total = 0;
set_b->sample_count = 0;
for(i = 0; i < set_a->sample_count; i++)
{
sample_b = set_b->samples + set_b->sample_count++;
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Copy sample a to sample b
sample_b->pose = sample_a->pose;
sample_b->weight = sample_a->weight;
total += sample_b->weight;
// Add sample to histogram
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
}
// Normalize weights
for (i = 0; i < set_b->sample_count; i++)
{
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
set_b->converged = set_a->converged;
}
// Resample the distribution
void pf_update_resample(pf_t *pf)
{
int i;
double total;
pf_sample_set_t *set_a, *set_b;
pf_sample_t *sample_a, *sample_b;
//double r,c,U;
//int m;
//double count_inv;
double* c;
double w_diff;
set_a = pf->sets + pf->current_set;
set_b = pf->sets + (pf->current_set + 1) % 2;
if (pf->selective_resampling != 0)
{
if (set_a->n_effective > 0.5*(set_a->sample_count))
{
// copy set a to b
copy_set(set_a,set_b);
// Re-compute cluster statistics
pf_cluster_stats(pf, set_b);
// Use the newly created sample set
pf->current_set = (pf->current_set + 1) % 2;
return;
}
}
// Build up cumulative probability table for resampling.
// TODO: Replace this with a more efficient procedure
// (e.g., http://www.network-theory.co.uk/docs/gslref/GeneralDiscreteDistributions.html)
c = (double*)malloc(sizeof(double)*(set_a->sample_count+1));
c[0] = 0.0;
for(i=0;i<set_a->sample_count;i++)
c[i+1] = c[i]+set_a->samples[i].weight;
// Create the kd tree for adaptive sampling
pf_kdtree_clear(set_b->kdtree);
// Draw samples from set a to create set b.
total = 0;
set_b->sample_count = 0;
w_diff = 1.0 - pf->w_fast / pf->w_slow;
if(w_diff < 0.0)
w_diff = 0.0;
//printf("w_diff: %9.6f\n", w_diff);
// Can't (easily) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// Low-variance resampler, taken from Probabilistic Robotics, p110
count_inv = 1.0/set_a->sample_count;
r = drand48() * count_inv;
c = set_a->samples[0].weight;
i = 0;
m = 0;
*/
while(set_b->sample_count < pf->max_samples)
{
sample_b = set_b->samples + set_b->sample_count++;
if(drand48() < w_diff)
sample_b->pose = (pf->random_pose_fn)(pf->random_pose_data);
else
{
// Can't (easily) combine low-variance sampler with KLD adaptive
// sampling, so we'll take the more traditional route.
/*
// Low-variance resampler, taken from Probabilistic Robotics, p110
U = r + m * count_inv;
while(U>c)
{
i++;
// Handle wrap-around by resetting counters and picking a new random
// number
if(i >= set_a->sample_count)
{
r = drand48() * count_inv;
c = set_a->samples[0].weight;
i = 0;
m = 0;
U = r + m * count_inv;
continue;
}
c += set_a->samples[i].weight;
}
m++;
*/
// Naive discrete event sampler
double r;
r = drand48();
for(i=0;i<set_a->sample_count;i++)
{
if((c[i] <= r) && (r < c[i+1]))
break;
}
assert(i<set_a->sample_count);
sample_a = set_a->samples + i;
assert(sample_a->weight > 0);
// Add sample to list
sample_b->pose = sample_a->pose;
}
sample_b->weight = 1.0;
total += sample_b->weight;
// Add sample to histogram
pf_kdtree_insert(set_b->kdtree, sample_b->pose, sample_b->weight);
// See if we have enough samples yet
if (set_b->sample_count > pf_resample_limit(pf, set_b->kdtree->leaf_count))
break;
}
// Reset averages, to avoid spiraling off into complete randomness.
if(w_diff > 0.0)
pf->w_slow = pf->w_fast = 0.0;
//fprintf(stderr, "\n\n");
// Normalize weights
for (i = 0; i < set_b->sample_count; i++)
{
sample_b = set_b->samples + i;
sample_b->weight /= total;
}
// Re-compute cluster statistics
pf_cluster_stats(pf, set_b);
// Use the newly created sample set
pf->current_set = (pf->current_set + 1) % 2;
pf_update_converged(pf);
free(c);
return;
}
// Compute the required number of samples, given that there are k bins
// with samples in them. This is taken directly from Fox et al.
int pf_resample_limit(pf_t *pf, int k)
{
double a, b, c, x;
int n;
// Return max_samples in case k is outside expected range, this shouldn't
// happen, but is added to prevent any runtime errors
if (k < 1 || k > pf->max_samples)
return pf->max_samples;
// Return value if cache is valid, which means value is non-zero positive
if (pf->limit_cache[k-1] > 0)
return pf->limit_cache[k-1];
if (k == 1)
{
pf->limit_cache[k-1] = pf->max_samples;
return pf->max_samples;
}
a = 1;
b = 2 / (9 * ((double) k - 1));
c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z;
x = a - b + c;
n = (int) ceil((k - 1) / (2 * pf->pop_err) * x * x * x);
if (n < pf->min_samples)
{
pf->limit_cache[k-1] = pf->min_samples;
return pf->min_samples;
}
if (n > pf->max_samples)
{
pf->limit_cache[k-1] = pf->max_samples;
return pf->max_samples;
}
pf->limit_cache[k-1] = n;
return n;
}
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{
int i, j, k, cidx;
pf_sample_t *sample;
pf_cluster_t *cluster;
// Workspace
double m[4], c[2][2];
size_t count;
double weight;
// Cluster the samples
pf_kdtree_cluster(set->kdtree);
// Initialize cluster stats
set->cluster_count = 0;
for (i = 0; i < set->cluster_max_count; i++)
{
cluster = set->clusters + i;
cluster->count = 0;
cluster->weight = 0;
cluster->mean = pf_vector_zero();
cluster->cov = pf_matrix_zero();
for (j = 0; j < 4; j++)
cluster->m[j] = 0.0;
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
cluster->c[j][k] = 0.0;
}
// Initialize overall filter stats
count = 0;
weight = 0.0;
set->mean = pf_vector_zero();
set->cov = pf_matrix_zero();
for (j = 0; j < 4; j++)
m[j] = 0.0;
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
c[j][k] = 0.0;
// Compute cluster stats
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
//printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);
// Get the cluster label for this sample
cidx = pf_kdtree_get_cluster(set->kdtree, sample->pose);
assert(cidx >= 0);
if (cidx >= set->cluster_max_count)
continue;
if (cidx + 1 > set->cluster_count)
set->cluster_count = cidx + 1;
cluster = set->clusters + cidx;
cluster->count += 1;
cluster->weight += sample->weight;
count += 1;
weight += sample->weight;
// Compute mean
cluster->m[0] += sample->weight * sample->pose.v[0];
cluster->m[1] += sample->weight * sample->pose.v[1];
cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
cluster->m[3] += sample->weight * sin(sample->pose.v[2]);
m[0] += sample->weight * sample->pose.v[0];
m[1] += sample->weight * sample->pose.v[1];
m[2] += sample->weight * cos(sample->pose.v[2]);
m[3] += sample->weight * sin(sample->pose.v[2]);
// Compute covariance in linear components
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
{
cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
}
}
// Normalize
for (i = 0; i < set->cluster_count; i++)
{
cluster = set->clusters + i;
cluster->mean.v[0] = cluster->m[0] / cluster->weight;
cluster->mean.v[1] = cluster->m[1] / cluster->weight;
cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);
cluster->cov = pf_matrix_zero();
// Covariance in linear components
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
cluster->mean.v[j] * cluster->mean.v[k];
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
cluster->m[3] * cluster->m[3]) / cluster->weight);
//printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
//cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
//pf_matrix_fprintf(cluster->cov, stdout, "%e");
}
assert(fabs(weight) >= DBL_EPSILON);
if (fabs(weight) < DBL_EPSILON)
{
printf("ERROR : divide-by-zero exception : weight is zero\n");
return;
}
// Compute overall filter stats
set->mean.v[0] = m[0] / weight;
set->mean.v[1] = m[1] / weight;
set->mean.v[2] = atan2(m[3], m[2]);
// Covariance in linear components
for (j = 0; j < 2; j++)
for (k = 0; k < 2; k++)
set->cov.m[j][k] = c[j][k] / weight - set->mean.v[j] * set->mean.v[k];
// Covariance in angular components; I think this is the correct
// formula for circular statistics.
set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
return;
}
void pf_set_selective_resampling(pf_t *pf, int selective_resampling)
{
pf->selective_resampling = selective_resampling;
}
// Compute the CEP statistics (mean and variance).
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
{
int i;
double mn, mx, my, mrr;
pf_sample_set_t *set;
pf_sample_t *sample;
set = pf->sets + pf->current_set;
mn = 0.0;
mx = 0.0;
my = 0.0;
mrr = 0.0;
for (i = 0; i < set->sample_count; i++)
{
sample = set->samples + i;
mn += sample->weight;
mx += sample->weight * sample->pose.v[0];
my += sample->weight * sample->pose.v[1];
mrr += sample->weight * sample->pose.v[0] * sample->pose.v[0];
mrr += sample->weight * sample->pose.v[1] * sample->pose.v[1];
}
assert(fabs(mn) >= DBL_EPSILON);
if (fabs(mn) < DBL_EPSILON)
{
printf("ERROR : divide-by-zero exception : mn is zero\n");
return;
}
mean->v[0] = mx / mn;
mean->v[1] = my / mn;
mean->v[2] = 0.0;
*var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
return;
}
// Get the statistics for a particular cluster.
int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight,
pf_vector_t *mean, pf_matrix_t *cov)
{
pf_sample_set_t *set;
pf_cluster_t *cluster;
set = pf->sets + pf->current_set;
if (clabel >= set->cluster_count)
return 0;
cluster = set->clusters + clabel;
*weight = cluster->weight;
*mean = cluster->mean;
*cov = cluster->cov;
return 1;
}

163
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_draw.c Normal file
View File

@@ -0,0 +1,163 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Particle filter; drawing routines
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_draw.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#ifdef INCLUDE_RTKGUI
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include "rtk.h"
#include "pf.h"
#include "pf_pdf.h"
#include "pf_kdtree.h"
// Draw the statistics
void pf_draw_statistics(pf_t *pf, rtk_fig_t *fig);
// Draw the sample set
void pf_draw_samples(pf_t *pf, rtk_fig_t *fig, int max_samples)
{
int i;
double px, py, pa;
pf_sample_set_t *set;
pf_sample_t *sample;
set = pf->sets + pf->current_set;
max_samples = MIN(max_samples, set->sample_count);
for (i = 0; i < max_samples; i++)
{
sample = set->samples + i;
px = sample->pose.v[0];
py = sample->pose.v[1];
pa = sample->pose.v[2];
//printf("%f %f\n", px, py);
rtk_fig_point(fig, px, py);
rtk_fig_arrow(fig, px, py, pa, 0.1, 0.02);
//rtk_fig_rectangle(fig, px, py, 0, 0.1, 0.1, 0);
}
return;
}
// Draw the hitogram (kd tree)
void pf_draw_hist(pf_t *pf, rtk_fig_t *fig)
{
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
rtk_fig_color(fig, 0.0, 0.0, 1.0);
pf_kdtree_draw(set->kdtree, fig);
return;
}
// Draw the CEP statistics
void pf_draw_cep_stats(pf_t *pf, rtk_fig_t *fig)
{
pf_vector_t mean;
double var;
pf_get_cep_stats(pf, &mean, &var);
var = sqrt(var);
rtk_fig_color(fig, 0, 0, 1);
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], mean.v[2], 3 * var, 3 * var, 0);
return;
}
// Draw the cluster statistics
void pf_draw_cluster_stats(pf_t *pf, rtk_fig_t *fig)
{
int i;
pf_cluster_t *cluster;
pf_sample_set_t *set;
pf_vector_t mean;
pf_matrix_t cov;
pf_matrix_t r, d;
double weight, o, d1, d2;
set = pf->sets + pf->current_set;
for (i = 0; i < set->cluster_count; i++)
{
cluster = set->clusters + i;
weight = cluster->weight;
mean = cluster->mean;
cov = cluster->cov;
// Compute unitary representation S = R D R^T
pf_matrix_unitary(&r, &d, cov);
/* Debugging
printf("mean = \n");
pf_vector_fprintf(mean, stdout, "%e");
printf("cov = \n");
pf_matrix_fprintf(cov, stdout, "%e");
printf("r = \n");
pf_matrix_fprintf(r, stdout, "%e");
printf("d = \n");
pf_matrix_fprintf(d, stdout, "%e");
*/
// Compute the orientation of the error ellipse (first eigenvector)
o = atan2(r.m[1][0], r.m[0][0]);
d1 = 6 * sqrt(d.m[0][0]);
d2 = 6 * sqrt(d.m[1][1]);
if (d1 > 1e-3 && d2 > 1e-3)
{
// Draw the error ellipse
rtk_fig_ellipse(fig, mean.v[0], mean.v[1], o, d1, d2, 0);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o, d1);
rtk_fig_line_ex(fig, mean.v[0], mean.v[1], o + M_PI / 2, d2);
}
// Draw a direction indicator
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2], 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] + 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
rtk_fig_arrow(fig, mean.v[0], mean.v[1], mean.v[2] - 3 * sqrt(cov.m[2][2]), 0.50, 0.10);
}
return;
}
#endif

486
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_kdtree.c Normal file
View File

@@ -0,0 +1,486 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: kd-tree functions
* Author: Andrew Howard
* Date: 18 Dec 2002
* CVS: $Id: pf_kdtree.c 7057 2008-10-02 00:44:06Z gbiggs $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include "amcl/pf/pf_vector.h"
#include "amcl/pf/pf_kdtree.h"
// Compare keys to see if they are equal
static int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[]);
// Insert a node into the tree
static pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent,
pf_kdtree_node_t *node, int key[], double value);
// Recursive node search
static pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[]);
// Recursively label nodes in this cluster
static void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth);
// Recursive node printing
//static void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node);
#ifdef INCLUDE_RTKGUI
// Recursively draw nodes
static void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig);
#endif
////////////////////////////////////////////////////////////////////////////////
// Create a tree
pf_kdtree_t *pf_kdtree_alloc(int max_size)
{
pf_kdtree_t *self;
self = calloc(1, sizeof(pf_kdtree_t));
self->size[0] = 0.50;
self->size[1] = 0.50;
self->size[2] = (10 * M_PI / 180);
self->root = NULL;
self->node_count = 0;
self->node_max_count = max_size;
self->nodes = calloc(self->node_max_count, sizeof(pf_kdtree_node_t));
self->leaf_count = 0;
return self;
}
////////////////////////////////////////////////////////////////////////////////
// Destroy a tree
void pf_kdtree_free(pf_kdtree_t *self)
{
free(self->nodes);
free(self);
return;
}
////////////////////////////////////////////////////////////////////////////////
// Clear all entries from the tree
void pf_kdtree_clear(pf_kdtree_t *self)
{
self->root = NULL;
self->leaf_count = 0;
self->node_count = 0;
return;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a pose into the tree.
void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
{
int key[3];
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
self->root = pf_kdtree_insert_node(self, NULL, self->root, key, value);
// Test code
/*
printf("find %d %d %d\n", key[0], key[1], key[2]);
assert(pf_kdtree_find_node(self, self->root, key) != NULL);
pf_kdtree_print_node(self, self->root);
printf("\n");
for (i = 0; i < self->node_count; i++)
{
node = self->nodes + i;
if (node->leaf)
{
printf("find %d %d %d\n", node->key[0], node->key[1], node->key[2]);
assert(pf_kdtree_find_node(self, self->root, node->key) == node);
}
}
printf("\n\n");
*/
return;
}
////////////////////////////////////////////////////////////////////////////////
// Determine the probability estimate for the given pose. TODO: this
// should do a kernel density estimate rather than a simple histogram.
double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose)
{
int key[3];
pf_kdtree_node_t *node;
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
node = pf_kdtree_find_node(self, self->root, key);
if (node == NULL)
return 0.0;
return node->value;
}
////////////////////////////////////////////////////////////////////////////////
// Determine the cluster label for the given pose
int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
{
int key[3];
pf_kdtree_node_t *node;
key[0] = floor(pose.v[0] / self->size[0]);
key[1] = floor(pose.v[1] / self->size[1]);
key[2] = floor(pose.v[2] / self->size[2]);
node = pf_kdtree_find_node(self, self->root, key);
if (node == NULL)
return -1;
return node->cluster;
}
////////////////////////////////////////////////////////////////////////////////
// Compare keys to see if they are equal
int pf_kdtree_equal(pf_kdtree_t *self, int key_a[], int key_b[])
{
//double a, b;
if (key_a[0] != key_b[0])
return 0;
if (key_a[1] != key_b[1])
return 0;
if (key_a[2] != key_b[2])
return 0;
/* TODO: make this work (pivot selection needs fixing, too)
// Normalize angles
a = key_a[2] * self->size[2];
a = atan2(sin(a), cos(a)) / self->size[2];
b = key_b[2] * self->size[2];
b = atan2(sin(b), cos(b)) / self->size[2];
if ((int) a != (int) b)
return 0;
*/
return 1;
}
////////////////////////////////////////////////////////////////////////////////
// Insert a node into the tree
pf_kdtree_node_t *pf_kdtree_insert_node(pf_kdtree_t *self, pf_kdtree_node_t *parent,
pf_kdtree_node_t *node, int key[], double value)
{
int i;
int split, max_split;
// If the node doesnt exist yet...
if (node == NULL)
{
assert(self->node_count < self->node_max_count);
node = self->nodes + self->node_count++;
memset(node, 0, sizeof(pf_kdtree_node_t));
node->leaf = 1;
if (parent == NULL)
node->depth = 0;
else
node->depth = parent->depth + 1;
for (i = 0; i < 3; i++)
node->key[i] = key[i];
node->value = value;
self->leaf_count += 1;
}
// If the node exists, and it is a leaf node...
else if (node->leaf)
{
// If the keys are equal, increment the value
if (pf_kdtree_equal(self, key, node->key))
{
node->value += value;
}
// The keys are not equal, so split this node
else
{
// Find the dimension with the largest variance and do a mean
// split
max_split = 0;
node->pivot_dim = -1;
for (i = 0; i < 3; i++)
{
split = abs(key[i] - node->key[i]);
if (split > max_split)
{
max_split = split;
node->pivot_dim = i;
}
}
assert(node->pivot_dim >= 0);
node->pivot_value = (key[node->pivot_dim] + node->key[node->pivot_dim]) / 2.0;
if (key[node->pivot_dim] < node->pivot_value)
{
node->children[0] = pf_kdtree_insert_node(self, node, NULL, key, value);
node->children[1] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
}
else
{
node->children[0] = pf_kdtree_insert_node(self, node, NULL, node->key, node->value);
node->children[1] = pf_kdtree_insert_node(self, node, NULL, key, value);
}
node->leaf = 0;
self->leaf_count -= 1;
}
}
// If the node exists, and it has children...
else
{
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
if (key[node->pivot_dim] < node->pivot_value)
pf_kdtree_insert_node(self, node, node->children[0], key, value);
else
pf_kdtree_insert_node(self, node, node->children[1], key, value);
}
return node;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node search
pf_kdtree_node_t *pf_kdtree_find_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int key[])
{
if (node->leaf)
{
//printf("find : leaf %p %d %d %d\n", node, node->key[0], node->key[1], node->key[2]);
// If the keys are the same...
if (pf_kdtree_equal(self, key, node->key))
return node;
else
return NULL;
}
else
{
//printf("find : brch %p %d %f\n", node, node->pivot_dim, node->pivot_value);
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
// If the keys are different...
if (key[node->pivot_dim] < node->pivot_value)
return pf_kdtree_find_node(self, node->children[0], key);
else
return pf_kdtree_find_node(self, node->children[1], key);
}
return NULL;
}
////////////////////////////////////////////////////////////////////////////////
// Recursive node printing
/*
void pf_kdtree_print_node(pf_kdtree_t *self, pf_kdtree_node_t *node)
{
if (node->leaf)
{
printf("(%+02d %+02d %+02d)\n", node->key[0], node->key[1], node->key[2]);
printf("%*s", node->depth * 11, "");
}
else
{
printf("(%+02d %+02d %+02d) ", node->key[0], node->key[1], node->key[2]);
pf_kdtree_print_node(self, node->children[0]);
pf_kdtree_print_node(self, node->children[1]);
}
return;
}
*/
////////////////////////////////////////////////////////////////////////////////
// Cluster the leaves in the tree
void pf_kdtree_cluster(pf_kdtree_t *self)
{
int i;
int queue_count, cluster_count;
pf_kdtree_node_t **queue, *node;
queue_count = 0;
queue = calloc(self->node_count, sizeof(queue[0]));
// Put all the leaves in a queue
for (i = 0; i < self->node_count; i++)
{
node = self->nodes + i;
if (node->leaf)
{
node->cluster = -1;
assert(queue_count < self->node_count);
queue[queue_count++] = node;
// TESTING; remove
assert(node == pf_kdtree_find_node(self, self->root, node->key));
}
}
cluster_count = 0;
// Do connected components for each node
while (queue_count > 0)
{
node = queue[--queue_count];
// If this node has already been labelled, skip it
if (node->cluster >= 0)
continue;
// Assign a label to this cluster
node->cluster = cluster_count++;
// Recursively label nodes in this cluster
pf_kdtree_cluster_node(self, node, 0);
}
free(queue);
return;
}
////////////////////////////////////////////////////////////////////////////////
// Recursively label nodes in this cluster
void pf_kdtree_cluster_node(pf_kdtree_t *self, pf_kdtree_node_t *node, int depth)
{
int i;
int nkey[3];
pf_kdtree_node_t *nnode;
for (i = 0; i < 3 * 3 * 3; i++)
{
nkey[0] = node->key[0] + (i / 9) - 1;
nkey[1] = node->key[1] + ((i % 9) / 3) - 1;
nkey[2] = node->key[2] + ((i % 9) % 3) - 1;
nnode = pf_kdtree_find_node(self, self->root, nkey);
if (nnode == NULL)
continue;
assert(nnode->leaf);
// This node already has a label; skip it. The label should be
// consistent, however.
if (nnode->cluster >= 0)
{
assert(nnode->cluster == node->cluster);
continue;
}
// Label this node and recurse
nnode->cluster = node->cluster;
pf_kdtree_cluster_node(self, nnode, depth + 1);
}
return;
}
#ifdef INCLUDE_RTKGUI
////////////////////////////////////////////////////////////////////////////////
// Draw the tree
void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig)
{
if (self->root != NULL)
pf_kdtree_draw_node(self, self->root, fig);
return;
}
////////////////////////////////////////////////////////////////////////////////
// Recursively draw nodes
void pf_kdtree_draw_node(pf_kdtree_t *self, pf_kdtree_node_t *node, rtk_fig_t *fig)
{
double ox, oy;
char text[64];
if (node->leaf)
{
ox = (node->key[0] + 0.5) * self->size[0];
oy = (node->key[1] + 0.5) * self->size[1];
rtk_fig_rectangle(fig, ox, oy, 0.0, self->size[0], self->size[1], 0);
//snprintf(text, sizeof(text), "%0.3f", node->value);
//rtk_fig_text(fig, ox, oy, 0.0, text);
snprintf(text, sizeof(text), "%d", node->cluster);
rtk_fig_text(fig, ox, oy, 0.0, text);
}
else
{
assert(node->children[0] != NULL);
assert(node->children[1] != NULL);
pf_kdtree_draw_node(self, node->children[0], fig);
pf_kdtree_draw_node(self, node->children[1], fig);
}
return;
}
#endif

147
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_pdf.c Normal file
View File

@@ -0,0 +1,147 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Useful pdf functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_pdf.c 6348 2008-04-17 02:53:17Z gerkey $
*************************************************************************/
#include <assert.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
//#include <gsl/gsl_rng.h>
//#include <gsl/gsl_randist.h>
#include "amcl/pf/pf_pdf.h"
#include "amcl/portable_utils.hpp"
// Random number generator seed value
static unsigned int pf_pdf_seed;
/**************************************************************************
* Gaussian
*************************************************************************/
// Create a gaussian pdf
pf_pdf_gaussian_t *pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
{
pf_matrix_t cd;
pf_pdf_gaussian_t *pdf;
pdf = calloc(1, sizeof(pf_pdf_gaussian_t));
pdf->x = x;
pdf->cx = cx;
//pdf->cxi = pf_matrix_inverse(cx, &pdf->cxdet);
// Decompose the convariance matrix into a rotation
// matrix and a diagonal matrix.
pf_matrix_unitary(&pdf->cr, &cd, pdf->cx);
pdf->cd.v[0] = sqrt(cd.m[0][0]);
pdf->cd.v[1] = sqrt(cd.m[1][1]);
pdf->cd.v[2] = sqrt(cd.m[2][2]);
// Initialize the random number generator
//pdf->rng = gsl_rng_alloc(gsl_rng_taus);
//gsl_rng_set(pdf->rng, ++pf_pdf_seed);
srand48(++pf_pdf_seed);
return pdf;
}
// Destroy the pdf
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
{
//gsl_rng_free(pdf->rng);
free(pdf);
return;
}
/*
// Compute the value of the pdf at some point [x].
double pf_pdf_gaussian_value(pf_pdf_gaussian_t *pdf, pf_vector_t x)
{
int i, j;
pf_vector_t z;
double zz, p;
z = pf_vector_sub(x, pdf->x);
zz = 0;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
zz += z.v[i] * pdf->cxi.m[i][j] * z.v[j];
p = 1 / (2 * M_PI * pdf->cxdet) * exp(-zz / 2);
return p;
}
*/
// Generate a sample from the pdf.
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)
{
int i, j;
pf_vector_t r;
pf_vector_t x;
// Generate a random vector
for (i = 0; i < 3; i++)
{
//r.v[i] = gsl_ran_gaussian(pdf->rng, pdf->cd.v[i]);
r.v[i] = pf_ran_gaussian(pdf->cd.v[i]);
}
for (i = 0; i < 3; i++)
{
x.v[i] = pdf->x.v[i];
for (j = 0; j < 3; j++)
x.v[i] += pdf->cr.m[i][j] * r.v[j];
}
return x;
}
// Draw randomly from a zero-mean Gaussian distribution, with standard
// deviation sigma.
// We use the polar form of the Box-Muller transformation, explained here:
// http://www.taygeta.com/random/gaussian.html
double pf_ran_gaussian(double sigma)
{
double x1, x2, w, r;
do
{
do { r = drand48(); } while (r==0.0);
x1 = 2.0 * r - 1.0;
do { r = drand48(); } while (r==0.0);
x2 = 2.0 * r - 1.0;
w = x1*x1 + x2*x2;
} while(w > 1.0 || w==0.0);
return(sigma * x2 * sqrt(-2.0*log(w)/w));
}

276
Localizations/Libraries/Ros/amcl/src/amcl/pf/pf_vector.c Normal file
View File

@@ -0,0 +1,276 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/**************************************************************************
* Desc: Vector functions
* Author: Andrew Howard
* Date: 10 Dec 2002
* CVS: $Id: pf_vector.c 6345 2008-04-17 01:36:39Z gerkey $
*************************************************************************/
#include <math.h>
//#include <gsl/gsl_matrix.h>
//#include <gsl/gsl_eigen.h>
//#include <gsl/gsl_linalg.h>
#include "amcl/pf/pf_vector.h"
#include "amcl/pf/eig3.h"
// Return a zero vector
pf_vector_t pf_vector_zero()
{
pf_vector_t c;
c.v[0] = 0.0;
c.v[1] = 0.0;
c.v[2] = 0.0;
return c;
}
// Check for NAN or INF in any component
int pf_vector_finite(pf_vector_t a)
{
int i;
for (i = 0; i < 3; i++)
if (!isfinite(a.v[i]))
return 0;
return 1;
}
// Print a vector
void pf_vector_fprintf(pf_vector_t a, FILE *file, const char *fmt)
{
int i;
for (i = 0; i < 3; i++)
{
fprintf(file, fmt, a.v[i]);
fprintf(file, " ");
}
fprintf(file, "\n");
return;
}
// Simple vector addition
pf_vector_t pf_vector_add(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = a.v[0] + b.v[0];
c.v[1] = a.v[1] + b.v[1];
c.v[2] = a.v[2] + b.v[2];
return c;
}
// Simple vector subtraction
pf_vector_t pf_vector_sub(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = a.v[0] - b.v[0];
c.v[1] = a.v[1] - b.v[1];
c.v[2] = a.v[2] - b.v[2];
return c;
}
// Transform from local to global coords (a + b)
pf_vector_t pf_vector_coord_add(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = b.v[0] + a.v[0] * cos(b.v[2]) - a.v[1] * sin(b.v[2]);
c.v[1] = b.v[1] + a.v[0] * sin(b.v[2]) + a.v[1] * cos(b.v[2]);
c.v[2] = b.v[2] + a.v[2];
c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
return c;
}
// Transform from global to local coords (a - b)
pf_vector_t pf_vector_coord_sub(pf_vector_t a, pf_vector_t b)
{
pf_vector_t c;
c.v[0] = +(a.v[0] - b.v[0]) * cos(b.v[2]) + (a.v[1] - b.v[1]) * sin(b.v[2]);
c.v[1] = -(a.v[0] - b.v[0]) * sin(b.v[2]) + (a.v[1] - b.v[1]) * cos(b.v[2]);
c.v[2] = a.v[2] - b.v[2];
c.v[2] = atan2(sin(c.v[2]), cos(c.v[2]));
return c;
}
// Return a zero matrix
pf_matrix_t pf_matrix_zero()
{
int i, j;
pf_matrix_t c;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
c.m[i][j] = 0.0;
return c;
}
// Check for NAN or INF in any component
int pf_matrix_finite(pf_matrix_t a)
{
int i, j;
for (i = 0; i < 3; i++)
for (j = 0; j < 3; j++)
if (!isfinite(a.m[i][j]))
return 0;
return 1;
}
// Print a matrix
void pf_matrix_fprintf(pf_matrix_t a, FILE *file, const char *fmt)
{
int i, j;
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
fprintf(file, fmt, a.m[i][j]);
fprintf(file, " ");
}
fprintf(file, "\n");
}
return;
}
/*
// Compute the matrix inverse
pf_matrix_t pf_matrix_inverse(pf_matrix_t a, double *det)
{
double lndet;
int signum;
gsl_permutation *p;
gsl_matrix_view A, Ai;
pf_matrix_t ai;
A = gsl_matrix_view_array((double*) a.m, 3, 3);
Ai = gsl_matrix_view_array((double*) ai.m, 3, 3);
// Do LU decomposition
p = gsl_permutation_alloc(3);
gsl_linalg_LU_decomp(&A.matrix, p, &signum);
// Check for underflow
lndet = gsl_linalg_LU_lndet(&A.matrix);
if (lndet < -1000)
{
//printf("underflow in matrix inverse lndet = %f", lndet);
gsl_matrix_set_zero(&Ai.matrix);
}
else
{
// Compute inverse
gsl_linalg_LU_invert(&A.matrix, p, &Ai.matrix);
}
gsl_permutation_free(p);
if (det)
*det = exp(lndet);
return ai;
}
*/
// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
// matrix [d] such that a = r d r^T.
void pf_matrix_unitary(pf_matrix_t *r, pf_matrix_t *d, pf_matrix_t a)
{
int i, j;
/*
gsl_matrix *aa;
gsl_vector *eval;
gsl_matrix *evec;
gsl_eigen_symmv_workspace *w;
aa = gsl_matrix_alloc(3, 3);
eval = gsl_vector_alloc(3);
evec = gsl_matrix_alloc(3, 3);
*/
double aa[3][3];
double eval[3];
double evec[3][3];
for (i = 0; i < 3; i++)
{
for (j = 0; j < 3; j++)
{
//gsl_matrix_set(aa, i, j, a.m[i][j]);
aa[i][j] = a.m[i][j];
}
}
// Compute eigenvectors/values
/*
w = gsl_eigen_symmv_alloc(3);
gsl_eigen_symmv(aa, eval, evec, w);
gsl_eigen_symmv_free(w);
*/
eigen_decomposition(aa,evec,eval);
*d = pf_matrix_zero();
for (i = 0; i < 3; i++)
{
//d->m[i][i] = gsl_vector_get(eval, i);
d->m[i][i] = eval[i];
for (j = 0; j < 3; j++)
{
//r->m[i][j] = gsl_matrix_get(evec, i, j);
r->m[i][j] = evec[i][j];
}
}
//gsl_matrix_free(evec);
//gsl_vector_free(eval);
//gsl_matrix_free(aa);
return;
}

510
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_laser.cpp Normal file
View File

@@ -0,0 +1,510 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: AMCL laser routines
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_laser.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include <sys/types.h> // required by Darwin
#include <math.h>
#include <stdlib.h>
#include <assert.h>
#ifdef HAVE_UNISTD_H
#include <unistd.h>
#endif
#include "amcl/sensors/amcl_laser.h"
using namespace amcl;
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLLaser::AMCLLaser(size_t max_beams, map_t* map) : AMCLSensor(),
max_samples(0), max_obs(0),
temp_obs(NULL)
{
this->time = 0.0;
this->max_beams = max_beams;
this->map = map;
return;
}
AMCLLaser::~AMCLLaser()
{
if(temp_obs){
for(int k=0; k < max_samples; k++){
delete [] temp_obs[k];
}
delete []temp_obs;
}
}
void
AMCLLaser::SetModelBeam(double z_hit,
double z_short,
double z_max,
double z_rand,
double sigma_hit,
double lambda_short,
double chi_outlier)
{
this->model_type = LASER_MODEL_BEAM;
this->z_hit = z_hit;
this->z_short = z_short;
this->z_max = z_max;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
this->lambda_short = lambda_short;
this->chi_outlier = chi_outlier;
}
void
AMCLLaser::SetModelLikelihoodField(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist)
{
this->model_type = LASER_MODEL_LIKELIHOOD_FIELD;
this->z_hit = z_hit;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
map_update_cspace(this->map, max_occ_dist);
}
void
AMCLLaser::SetModelLikelihoodFieldProb(double z_hit,
double z_rand,
double sigma_hit,
double max_occ_dist,
bool do_beamskip,
double beam_skip_distance,
double beam_skip_threshold,
double beam_skip_error_threshold)
{
this->model_type = LASER_MODEL_LIKELIHOOD_FIELD_PROB;
this->z_hit = z_hit;
this->z_rand = z_rand;
this->sigma_hit = sigma_hit;
this->do_beamskip = do_beamskip;
this->beam_skip_distance = beam_skip_distance;
this->beam_skip_threshold = beam_skip_threshold;
this->beam_skip_error_threshold = beam_skip_error_threshold;
map_update_cspace(this->map, max_occ_dist);
}
////////////////////////////////////////////////////////////////////////////////
// Apply the laser sensor model
bool AMCLLaser::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
if (this->max_beams < 2)
return false;
// Apply the laser sensor model
if(this->model_type == LASER_MODEL_BEAM)
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModel, data);
else if(this->model_type == LASER_MODEL_LIKELIHOOD_FIELD_PROB)
pf_update_sensor(pf, (pf_sensor_model_fn_t) LikelihoodFieldModelProb, data);
else
pf_update_sensor(pf, (pf_sensor_model_fn_t) BeamModel, data);
return true;
}
////////////////////////////////////////////////////////////////////////////////
// Determine the probability for the given pose
double AMCLLaser::BeamModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double map_range;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
step = (data->range_count - 1) / (self->max_beams - 1);
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// Compute the range according to the map
map_range = map_calc_range(self->map, pose.v[0], pose.v[1],
pose.v[2] + obs_bearing, data->range_max);
pz = 0.0;
// Part 1: good, but noisy, hit
z = obs_range - map_range;
pz += self->z_hit * exp(-(z * z) / (2 * self->sigma_hit * self->sigma_hit));
// Part 2: short reading from unexpected obstacle (e.g., a person)
if(z < 0)
pz += self->z_short * self->lambda_short * exp(-self->lambda_short*obs_range);
// Part 3: Failure to detect obstacle, reported as max-range
if(obs_range == data->range_max)
pz += self->z_max * 1.0;
// Part 4: Random measurements
if(obs_range < data->range_max)
pz += self->z_rand * 1.0/data->range_max;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return(total_weight);
}
double AMCLLaser::LikelihoodFieldModel(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
p = 1.0;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
step = (data->range_count - 1) / (self->max_beams - 1);
// Step size must be at least 1
if(step < 1)
step = 1;
for (i = 0; i < data->range_count; i += step)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max)
continue;
// Check for NaN
if(obs_range != obs_range)
continue;
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj))
z = self->map->max_occ_dist;
else
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
// TODO: outlier rejection for short readings
assert(pz <= 1.0);
assert(pz >= 0.0);
// p *= pz;
// here we have an ad-hoc weighting scheme for combining beam probs
// works well, though...
p += pz*pz*pz;
}
sample->weight *= p;
total_weight += sample->weight;
}
return(total_weight);
}
double AMCLLaser::LikelihoodFieldModelProb(AMCLLaserData *data, pf_sample_set_t* set)
{
AMCLLaser *self;
int i, j, step;
double z, pz;
double log_p;
double obs_range, obs_bearing;
double total_weight;
pf_sample_t *sample;
pf_vector_t pose;
pf_vector_t hit;
self = (AMCLLaser*) data->sensor;
total_weight = 0.0;
step = ceil((data->range_count) / static_cast<double>(self->max_beams));
// Step size must be at least 1
if(step < 1)
step = 1;
// Pre-compute a couple of things
double z_hit_denom = 2 * self->sigma_hit * self->sigma_hit;
double z_rand_mult = 1.0/data->range_max;
double max_dist_prob = exp(-(self->map->max_occ_dist * self->map->max_occ_dist) / z_hit_denom);
//Beam skipping - ignores beams for which a majority of particles do not agree with the map
//prevents correct particles from getting down weighted because of unexpected obstacles
//such as humans
bool do_beamskip = self->do_beamskip;
double beam_skip_distance = self->beam_skip_distance;
double beam_skip_threshold = self->beam_skip_threshold;
//we only do beam skipping if the filter has converged
if(do_beamskip && !set->converged){
do_beamskip = false;
}
//we need a count the no of particles for which the beam agreed with the map
int *obs_count = new int[self->max_beams]();
//we also need a mask of which observations to integrate (to decide which beams to integrate to all particles)
bool *obs_mask = new bool[self->max_beams]();
int beam_ind = 0;
//realloc indicates if we need to reallocate the temp data structure needed to do beamskipping
bool realloc = false;
if(do_beamskip){
if(self->max_obs < self->max_beams){
realloc = true;
}
if(self->max_samples < set->sample_count){
realloc = true;
}
if(realloc){
self->reallocTempData(set->sample_count, self->max_beams);
fprintf(stderr, "Reallocing temp weights %d - %d\n", self->max_samples, self->max_obs);
}
}
// Compute the sample weights
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
// Take account of the laser pose relative to the robot
pose = pf_vector_coord_add(self->laser_pose, pose);
log_p = 0;
beam_ind = 0;
for (i = 0; i < data->range_count; i += step, beam_ind++)
{
obs_range = data->ranges[i][0];
obs_bearing = data->ranges[i][1];
// This model ignores max range readings
if(obs_range >= data->range_max){
continue;
}
// Check for NaN
if(obs_range != obs_range){
continue;
}
pz = 0.0;
// Compute the endpoint of the beam
hit.v[0] = pose.v[0] + obs_range * cos(pose.v[2] + obs_bearing);
hit.v[1] = pose.v[1] + obs_range * sin(pose.v[2] + obs_bearing);
// Convert to map grid coords.
int mi, mj;
mi = MAP_GXWX(self->map, hit.v[0]);
mj = MAP_GYWY(self->map, hit.v[1]);
// Part 1: Get distance from the hit to closest obstacle.
// Off-map penalized as max distance
if(!MAP_VALID(self->map, mi, mj)){
pz += self->z_hit * max_dist_prob;
}
else{
z = self->map->cells[MAP_INDEX(self->map,mi,mj)].occ_dist;
if(z < beam_skip_distance){
obs_count[beam_ind] += 1;
}
pz += self->z_hit * exp(-(z * z) / z_hit_denom);
}
// Gaussian model
// NOTE: this should have a normalization of 1/(sqrt(2pi)*sigma)
// Part 2: random measurements
pz += self->z_rand * z_rand_mult;
assert(pz <= 1.0);
assert(pz >= 0.0);
// TODO: outlier rejection for short readings
if(!do_beamskip){
log_p += log(pz);
}
else{
self->temp_obs[j][beam_ind] = pz;
}
}
if(!do_beamskip){
sample->weight *= exp(log_p);
total_weight += sample->weight;
}
}
if(do_beamskip){
int skipped_beam_count = 0;
for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
if((obs_count[beam_ind] / static_cast<double>(set->sample_count)) > beam_skip_threshold){
obs_mask[beam_ind] = true;
}
else{
obs_mask[beam_ind] = false;
skipped_beam_count++;
}
}
//we check if there is at least a critical number of beams that agreed with the map
//otherwise it probably indicates that the filter converged to a wrong solution
//if that's the case we integrate all the beams and hope the filter might converge to
//the right solution
bool error = false;
if(skipped_beam_count >= (beam_ind * self->beam_skip_error_threshold)){
fprintf(stderr, "Over %f%% of the observations were not in the map - pf may have converged to wrong pose - integrating all observations\n", (100 * self->beam_skip_error_threshold));
error = true;
}
for (j = 0; j < set->sample_count; j++)
{
sample = set->samples + j;
pose = sample->pose;
log_p = 0;
for (beam_ind = 0; beam_ind < self->max_beams; beam_ind++){
if(error || obs_mask[beam_ind]){
log_p += log(self->temp_obs[j][beam_ind]);
}
}
sample->weight *= exp(log_p);
total_weight += sample->weight;
}
}
delete [] obs_count;
delete [] obs_mask;
return(total_weight);
}
void AMCLLaser::reallocTempData(int new_max_samples, int new_max_obs){
if(temp_obs){
for(int k=0; k < max_samples; k++){
delete [] temp_obs[k];
}
delete []temp_obs;
}
max_obs = new_max_obs;
max_samples = fmax(max_samples, new_max_samples);
temp_obs = new double*[max_samples]();
for(int k=0; k < max_samples; k++){
temp_obs[k] = new double[max_obs]();
}
}

310
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_odom.cpp Normal file
View File

@@ -0,0 +1,310 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: AMCL odometry routines
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_odom.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include <algorithm>
#include <sys/types.h> // required by Darwin
#include <math.h>
#include "amcl/sensors/amcl_odom.h"
using namespace amcl;
static double
normalize(double z)
{
return atan2(sin(z),cos(z));
}
static double
angle_diff(double a, double b)
{
double d1, d2;
a = normalize(a);
b = normalize(b);
d1 = a-b;
d2 = 2*M_PI - fabs(d1);
if(d1 > 0)
d2 *= -1.0;
if(fabs(d1) < fabs(d2))
return(d1);
else
return(d2);
}
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLOdom::AMCLOdom() : AMCLSensor()
{
this->time = 0.0;
}
void
AMCLOdom::SetModelDiff(double alpha1,
double alpha2,
double alpha3,
double alpha4)
{
this->model_type = ODOM_MODEL_DIFF;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
}
void
AMCLOdom::SetModelOmni(double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5)
{
this->model_type = ODOM_MODEL_OMNI;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
this->alpha5 = alpha5;
}
void
AMCLOdom::SetModel( odom_model_t type,
double alpha1,
double alpha2,
double alpha3,
double alpha4,
double alpha5 )
{
this->model_type = type;
this->alpha1 = alpha1;
this->alpha2 = alpha2;
this->alpha3 = alpha3;
this->alpha4 = alpha4;
this->alpha5 = alpha5;
}
////////////////////////////////////////////////////////////////////////////////
// Apply the action model
bool AMCLOdom::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
AMCLOdomData *ndata;
ndata = (AMCLOdomData*) data;
// Compute the new sample poses
pf_sample_set_t *set;
set = pf->sets + pf->current_set;
pf_vector_t old_pose = pf_vector_sub(ndata->pose, ndata->delta);
switch( this->model_type )
{
case ODOM_MODEL_OMNI:
{
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot = ndata->delta.v[2];
// Precompute a couple of things
double trans_hat_stddev = (alpha3 * (delta_trans*delta_trans) +
alpha1 * (delta_rot*delta_rot));
double rot_hat_stddev = (alpha4 * (delta_rot*delta_rot) +
alpha2 * (delta_trans*delta_trans));
double strafe_hat_stddev = (alpha1 * (delta_rot*delta_rot) +
alpha5 * (delta_trans*delta_trans));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]) + sample->pose.v[2];
double cs_bearing = cos(delta_bearing);
double sn_bearing = sin(delta_bearing);
// Sample pose differences
delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
// Apply sampled update to particle pose
sample->pose.v[0] += (delta_trans_hat * cs_bearing +
delta_strafe_hat * sn_bearing);
sample->pose.v[1] += (delta_trans_hat * sn_bearing -
delta_strafe_hat * cs_bearing);
sample->pose.v[2] += delta_rot_hat ;
}
}
break;
case ODOM_MODEL_DIFF:
{
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
// Sample pose differences
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans));
delta_trans_hat = delta_trans -
pf_ran_gaussian(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise);
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans));
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
break;
case ODOM_MODEL_OMNI_CORRECTED:
{
double delta_trans, delta_rot, delta_bearing;
double delta_trans_hat, delta_rot_hat, delta_strafe_hat;
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot = ndata->delta.v[2];
// Precompute a couple of things
double trans_hat_stddev = sqrt( alpha3 * (delta_trans*delta_trans) +
alpha4 * (delta_rot*delta_rot) );
double rot_hat_stddev = sqrt( alpha1 * (delta_rot*delta_rot) +
alpha2 * (delta_trans*delta_trans) );
double strafe_hat_stddev = sqrt( alpha4 * (delta_rot*delta_rot) +
alpha5 * (delta_trans*delta_trans) );
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
delta_bearing = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]) + sample->pose.v[2];
double cs_bearing = cos(delta_bearing);
double sn_bearing = sin(delta_bearing);
// Sample pose differences
delta_trans_hat = delta_trans + pf_ran_gaussian(trans_hat_stddev);
delta_rot_hat = delta_rot + pf_ran_gaussian(rot_hat_stddev);
delta_strafe_hat = 0 + pf_ran_gaussian(strafe_hat_stddev);
// Apply sampled update to particle pose
sample->pose.v[0] += (delta_trans_hat * cs_bearing +
delta_strafe_hat * sn_bearing);
sample->pose.v[1] += (delta_trans_hat * sn_bearing -
delta_strafe_hat * cs_bearing);
sample->pose.v[2] += delta_rot_hat ;
}
}
break;
case ODOM_MODEL_DIFF_CORRECTED:
{
// Implement sample_motion_odometry (Prob Rob p 136)
double delta_rot1, delta_trans, delta_rot2;
double delta_rot1_hat, delta_trans_hat, delta_rot2_hat;
double delta_rot1_noise, delta_rot2_noise;
// Avoid computing a bearing from two poses that are extremely near each
// other (happens on in-place rotation).
if(sqrt(ndata->delta.v[1]*ndata->delta.v[1] +
ndata->delta.v[0]*ndata->delta.v[0]) < 0.01)
delta_rot1 = 0.0;
else
delta_rot1 = angle_diff(atan2(ndata->delta.v[1], ndata->delta.v[0]),
old_pose.v[2]);
delta_trans = sqrt(ndata->delta.v[0]*ndata->delta.v[0] +
ndata->delta.v[1]*ndata->delta.v[1]);
delta_rot2 = angle_diff(ndata->delta.v[2], delta_rot1);
// We want to treat backward and forward motion symmetrically for the
// noise model to be applied below. The standard model seems to assume
// forward motion.
delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)),
fabs(angle_diff(delta_rot1,M_PI)));
delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)),
fabs(angle_diff(delta_rot2,M_PI)));
for (int i = 0; i < set->sample_count; i++)
{
pf_sample_t* sample = set->samples + i;
// Sample pose differences
delta_rot1_hat = angle_diff(delta_rot1,
pf_ran_gaussian(sqrt(this->alpha1*delta_rot1_noise*delta_rot1_noise +
this->alpha2*delta_trans*delta_trans)));
delta_trans_hat = delta_trans -
pf_ran_gaussian(sqrt(this->alpha3*delta_trans*delta_trans +
this->alpha4*delta_rot1_noise*delta_rot1_noise +
this->alpha4*delta_rot2_noise*delta_rot2_noise));
delta_rot2_hat = angle_diff(delta_rot2,
pf_ran_gaussian(sqrt(this->alpha1*delta_rot2_noise*delta_rot2_noise +
this->alpha2*delta_trans*delta_trans)));
// Apply sampled update to particle pose
sample->pose.v[0] += delta_trans_hat *
cos(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[1] += delta_trans_hat *
sin(sample->pose.v[2] + delta_rot1_hat);
sample->pose.v[2] += delta_rot1_hat + delta_rot2_hat;
}
}
break;
}
return true;
}

95
Localizations/Libraries/Ros/amcl/src/amcl/sensors/amcl_sensor.cpp Normal file
View File

@@ -0,0 +1,95 @@
/*
* Player - One Hell of a Robot Server
* Copyright (C) 2000 Brian Gerkey & Kasper Stoy
* gerkey@usc.edu kaspers@robotics.usc.edu
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
///////////////////////////////////////////////////////////////////////////
//
// Desc: AMCL sensor
// Author: Andrew Howard
// Date: 6 Feb 2003
// CVS: $Id: amcl_sensor.cc 7057 2008-10-02 00:44:06Z gbiggs $
//
///////////////////////////////////////////////////////////////////////////
#include "amcl/sensors/amcl_sensor.h"
using namespace amcl;
////////////////////////////////////////////////////////////////////////////////
// Default constructor
AMCLSensor::AMCLSensor()
{
return;
}
AMCLSensor::~AMCLSensor()
{
}
////////////////////////////////////////////////////////////////////////////////
// Apply the action model
bool AMCLSensor::UpdateAction(pf_t *pf, AMCLSensorData *data)
{
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Initialize the filter
bool AMCLSensor::InitSensor(pf_t *pf, AMCLSensorData *data)
{
return false;
}
////////////////////////////////////////////////////////////////////////////////
// Apply the sensor model
bool AMCLSensor::UpdateSensor(pf_t *pf, AMCLSensorData *data)
{
return false;
}
#ifdef INCLUDE_RTKGUI
////////////////////////////////////////////////////////////////////////////////
// Setup the GUI
void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
{
return;
}
////////////////////////////////////////////////////////////////////////////////
// Shutdown the GUI
void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig)
{
return;
}
////////////////////////////////////////////////////////////////////////////////
// Draw sensor data
void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data)
{
return;
}
#endif

72
Localizations/Libraries/Ros/amcl/src/amcl_node.cpp Normal file
View File

@@ -0,0 +1,72 @@
/*
* Copyright (c) 2008, Willow Garage, Inc.
* All rights reserved.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
/* Author: Brian Gerkey */
// Signal handling
#include <signal.h>
#include <amcl/amcl.h>
boost::shared_ptr<amcl::Amcl> amcl_node_ptr;
void sigintHandler(int sig)
{
// Save latest pose as we're shutting down.
geometry_msgs::PoseWithCovarianceStamped amcl_pose;
amcl_node_ptr->savePoseToServer(amcl_pose);
ros::shutdown();
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "amcl");
ros::NodeHandle nh;
// Override default sigint handler
signal(SIGINT, sigintHandler);
// Make our node available to sigintHandler
amcl_node_ptr.reset(new amcl::Amcl());
if (argc == 1)
{
// run using ROS input
ros::spin();
}
else if ((argc >= 3) && (std::string(argv[1]) == "--run-from-bag"))
{
if (argc == 3)
{
amcl_node_ptr->runFromBag(argv[2]);
}
else if ((argc == 4) && (std::string(argv[3]) == "--global-localization"))
{
amcl_node_ptr->runFromBag(argv[2], true);
}
}
// Without this, our boost locks are not shut down nicely
amcl_node_ptr.reset();
// To quote Morgan, Hooray!
return(0);
}

101
Localizations/Libraries/Ros/amcl/test/basic_localization.py Normal file
View File

@@ -0,0 +1,101 @@
#!/usr/bin/env python
from __future__ import print_function
import sys
import time
from math import fmod, pi
import unittest
import rospy
import rostest
import tf2_py as tf2
import tf2_ros
import PyKDL
from std_srvs.srv import Empty
class TestBasicLocalization(unittest.TestCase):
def setUp(self):
self.tf = None
self.target_x = None
self.target_y = None
self.target_a = None
self.tfBuffer = None
def print_pose_diff(self):
a_curr = self.compute_angle()
a_diff = self.wrap_angle(a_curr - self.target_a)
print('Curr:\t %16.6f %16.6f %16.6f' % (self.tf.translation.x, self.tf.translation.y, a_curr))
print('Target:\t %16.6f %16.6f %16.6f' % (self.target_x, self.target_y, self.target_a))
print('Diff:\t %16.6f %16.6f %16.6f' % (
abs(self.tf.translation.x - self.target_x), abs(self.tf.translation.y - self.target_y), a_diff))
def get_pose(self):
try:
tf_stamped = self.tfBuffer.lookup_transform("map", "base_link", rospy.Time())
self.tf = tf_stamped.transform
self.print_pose_diff()
except (tf2.LookupException, tf2.ExtrapolationException):
pass
@staticmethod
def wrap_angle(angle):
"""
Wrap angle to [-pi, pi)
:param angle: Angle to be wrapped
:return: wrapped angle
"""
angle += pi
while angle < 0:
angle += 2*pi
return fmod(angle, 2*pi) - pi
def compute_angle(self):
rot = self.tf.rotation
a_curr = PyKDL.Rotation.Quaternion(rot.x, rot.y, rot.z, rot.w).GetRPY()[2]
a_diff = self.wrap_angle(a_curr - self.target_a)
return self.target_a + a_diff
def test_basic_localization(self):
global_localization = int(sys.argv[1])
self.target_x = float(sys.argv[2])
self.target_y = float(sys.argv[3])
self.target_a = self.wrap_angle(float(sys.argv[4]))
tolerance_d = float(sys.argv[5])
tolerance_a = float(sys.argv[6])
target_time = float(sys.argv[7])
rospy.init_node('test', anonymous=True)
while rospy.rostime.get_time() == 0.0:
print('Waiting for initial time publication')
time.sleep(0.1)
if global_localization == 1:
print('Waiting for service global_localization')
rospy.wait_for_service('global_localization')
global_localization = rospy.ServiceProxy('global_localization', Empty)
global_localization()
start_time = rospy.rostime.get_time()
self.tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(self.tfBuffer)
while (rospy.rostime.get_time() - start_time) < target_time:
print('Waiting for end time %.6f (current: %.6f)' % (target_time, (rospy.rostime.get_time() - start_time)))
self.get_pose()
time.sleep(0.1)
print("Final pose:")
self.get_pose()
a_curr = self.compute_angle()
self.assertNotEqual(self.tf, None)
self.assertAlmostEqual(self.tf.translation.x, self.target_x, delta=tolerance_d)
self.assertAlmostEqual(self.tf.translation.y, self.target_y, delta=tolerance_d)
self.assertAlmostEqual(a_curr, self.target_a, delta=tolerance_a)
if __name__ == '__main__':
rostest.run('amcl', 'amcl_localization', TestBasicLocalization, sys.argv)

51
Localizations/Libraries/Ros/amcl/test/basic_localization_stage.xml Normal file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="47.443"/>
<param name="initial_pose_y" value="21.421"/>
<param name="initial_pose_a" value="-1.003"/>
</node>
<test time-limit="180" test-name="basic_localization_stage" pkg="amcl"
type="basic_localization.py" args="0 30.329 34.644 3.142 0.75 0.75 90.0"/>
</launch>

48
Localizations/Libraries/Ros/amcl/test/global_localization_stage.xml Normal file
View File

@@ -0,0 +1,48 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/global_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
</node>
<test time-limit="180" test-name="global_localization_stage" pkg="amcl"
type="basic_localization.py" args="1 10.00 9.67 6.21 0.25 0.25 28.0" />
</launch>

51
Localizations/Libraries/Ros/amcl/test/rosie_multilaser.xml Normal file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<!-- Setting pose: 42.378 17.730 1.583
Setting pose: 33.118 34.530 -0.519
103.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/rosie_localization_stage.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="42.378"/>
<param name="initial_pose_y" value="17.730"/>
<param name="initial_pose_a" value="1.583"/>
</node>
<test time-limit="180" test-name="basic_localization_stage_rosie" pkg="amcl"
type="basic_localization.py" args="0 33.12 34.53 -0.52 0.75 0.4 103.5"/>
</launch>

54
Localizations/Libraries/Ros/amcl/test/set_initial_pose.xml Normal file
View File

@@ -0,0 +1,54 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<!--
<param name="initial_pose_x" value="47.443"/>
<param name="initial_pose_y" value="21.421"/>
<param name="initial_pose_a" value="-1.003"/>
-->
</node>
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
type="basic_localization.py" args="0 30.33 34.64 3.14 0.75 0.75 90.0"/>
</launch>

58
Localizations/Libraries/Ros/amcl/test/set_initial_pose_delayed.xml Normal file
View File

@@ -0,0 +1,58 @@
<?xml version="1.0" ?>
<!-- setting pose: 47.943 21.421 -0.503
setting pose: 30.329 34.644 3.142
117.5s -->
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-d 5 -r 1 --clock --hz 10 $(find amcl)/test/basic_localization_stage_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node name="pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003"/>
<!-- The bagfile starts at 34.6s. We try to send the same initial pose as above with stamp 34.8 at 5 seconds
into the bagfile (at 39.6s). AMCL should add the robot motion from 34.8 to 39.6 to that initial pose
before using it, so there should be no mis-localization caused by this. -->
<node name="delayed_pose_setter" pkg="amcl" type="set_pose.py" args="47.443 21.421 -1.003 34.8 39.6"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<!--
<param name="initial_pose_x" value="47.443"/>
<param name="initial_pose_y" value="21.421"/>
<param name="initial_pose_a" value="-1.003"/>
-->
</node>
<test time-limit="180" test-name="set_initial_pose" pkg="amcl"
type="basic_localization.py" args="0 30.33 34.64 3.14 0.75 0.75 90.0"/>
</launch>

46
Localizations/Libraries/Ros/amcl/test/set_pose.py Normal file
View File

@@ -0,0 +1,46 @@
#!/usr/bin/env python
import rospy
import math
import PyKDL
from geometry_msgs.msg import PoseWithCovarianceStamped
class PoseSetter(rospy.SubscribeListener):
def __init__(self, pose, stamp, publish_time):
self.pose = pose
self.stamp = stamp
self.publish_time = publish_time
def peer_subscribe(self, topic_name, topic_publish, peer_publish):
p = PoseWithCovarianceStamped()
p.header.frame_id = "map"
p.header.stamp = self.stamp
p.pose.pose.position.x = self.pose[0]
p.pose.pose.position.y = self.pose[1]
(p.pose.pose.orientation.x,
p.pose.pose.orientation.y,
p.pose.pose.orientation.z,
p.pose.pose.orientation.w) = PyKDL.Rotation.RPY(0, 0, self.pose[2]).GetQuaternion()
p.pose.covariance[6*0+0] = 0.5 * 0.5
p.pose.covariance[6*1+1] = 0.5 * 0.5
p.pose.covariance[6*3+3] = math.pi/12.0 * math.pi/12.0
# wait for the desired publish time
while rospy.get_rostime() < self.publish_time:
rospy.sleep(0.01)
peer_publish(p)
if __name__ == '__main__':
pose = list(map(float, rospy.myargv()[1:4]))
t_stamp = rospy.Time()
t_publish = rospy.Time()
if len(rospy.myargv()) > 4:
t_stamp = rospy.Time.from_sec(float(rospy.myargv()[4]))
if len(rospy.myargv()) > 5:
t_publish = rospy.Time.from_sec(float(rospy.myargv()[5]))
rospy.init_node('pose_setter', anonymous=True)
rospy.loginfo("Going to publish pose {} with stamp {} at {}".format(pose, t_stamp.to_sec(), t_publish.to_sec()))
pub = rospy.Publisher("initialpose", PoseWithCovarianceStamped, PoseSetter(pose, stamp=t_stamp, publish_time=t_publish), queue_size=1)
rospy.spin()

51
Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg.xml Normal file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_crazy_driving_prg_indexed.bag"/>
<node name="map_sever" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="14.016"/>
<param name="initial_pose_y" value="25.7500"/>
<param name="initial_pose_a" value="-3.0"/>
<param name="initial_cov_xx" value="0.1"/>
<param name="initial_cov_yy" value="0.1"/>
<param name="initial_cov_aa" value="0.1"/>
</node>
<test time-limit="180" test-name="small_loop_crazy_driving_prg" pkg="amcl"
type="basic_localization.py" args="0 14.05 28.91 4.28 0.75 0.75 90.0"/>
</launch>

51
Localizations/Libraries/Ros/amcl/test/small_loop_crazy_driving_prg_corrected.xml Normal file
View File

@@ -0,0 +1,51 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_crazy_driving_prg_indexed.bag"/>
<node name="map_sever" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni-corrected"/>
<param name="odom_alpha1" value="0.005"/>
<param name="odom_alpha2" value="0.005"/>
<param name="odom_alpha3" value="0.01"/>
<param name="odom_alpha4" value="0.005"/>
<param name="odom_alpha5" value="0.003"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="14.016"/>
<param name="initial_pose_y" value="25.7500"/>
<param name="initial_pose_a" value="-3.0"/>
<param name="initial_cov_xx" value="0.1"/>
<param name="initial_cov_yy" value="0.1"/>
<param name="initial_cov_aa" value="0.1"/>
</node>
<test time-limit="180" test-name="small_loop_crazy_driving_prg" pkg="amcl"
type="basic_localization.py" args="0 14.05 28.91 4.28 0.75 0.75 90.0"/>
</launch>

48
Localizations/Libraries/Ros/amcl/test/small_loop_prf.xml Normal file
View File

@@ -0,0 +1,48 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 0.0 -d 5 -r 1 --clock --hz 10 $(find amcl)/test/small_loop_prf_indexed.bag"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full.pgm 0.1"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2" />
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="omni"/>
<param name="odom_alpha1" value="0.2"/>
<param name="odom_alpha2" value="0.2"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.2"/>
<param name="odom_alpha5" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_short" value="0.05"/>
<param name="laser_z_max" value="0.05"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.2"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_lambda_short" value="0.1"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="transform_tolerance" value="0.1"/>
<param name="recovery_alpha_slow" value="0.0"/>
<param name="recovery_alpha_fast" value="0.0"/>
<param name="initial_pose_x" value="14.099"/>
<param name="initial_pose_y" value="21.063"/>
<param name="initial_pose_a" value="-0.006"/>
</node>
<test time-limit="180" test-name="basic_localization_small_loop_prf" pkg="amcl"
type="basic_localization.py" args="0 13.94 23.02 4.72 0.75 0.75 66.0"/>
</launch>

41
Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop.xml Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_greenroom_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha1" value="0.1"/>
<param name="odom_alpha2" value="0.3"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="14.049"/>
<param name="initial_pose_y" value="24.234"/>
<param name="initial_pose_a" value="-1.517"/>
</node>
<test time-limit="180" test-name="texas_greenroom_loop" pkg="amcl"
type="basic_localization.py" args="0 13.87 24.07 4.65 0.75 0.75 89.0"/>
</launch>

41
Localizations/Libraries/Ros/amcl/test/texas_greenroom_loop_corrected.xml Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_greenroom_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha1" value="0.04"/>
<param name="odom_alpha2" value="0.6"/>
<param name="odom_alpha3" value="0.3"/>
<param name="odom_alpha4" value="0.04"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="14.049"/>
<param name="initial_pose_y" value="24.234"/>
<param name="initial_pose_a" value="-1.517"/>
</node>
<test time-limit="180" test-name="texas_greenroom_loop_corrected" pkg="amcl"
type="basic_localization.py" args="0 13.87 24.07 4.65 0.75 0.75 89.0"/>
</launch>

41
Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop.xml Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_willow_hallway_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff"/>
<param name="odom_alpha1" value="0.1"/>
<param name="odom_alpha2" value="0.3"/>
<param name="odom_alpha3" value="0.8"/>
<param name="odom_alpha4" value="0.1"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="31.872"/>
<param name="initial_pose_y" value="31.557"/>
<param name="initial_pose_a" value="0.0"/>
</node>
<test time-limit="350" test-name="texas_willow_hallway_loop" pkg="amcl"
type="basic_localization.py" args="0 32.36 31.44 6.18 0.75 0.75 250.0"/>
</launch>

41
Localizations/Libraries/Ros/amcl/test/texas_willow_hallway_loop_corrected.xml Normal file
View File

@@ -0,0 +1,41 @@
<?xml version="1.0" ?>
<launch>
<param name="/use_sim_time" value="true"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(find amcl)/test/willow-full-0.05.pgm 0.05"/>
<node name="rosbag" pkg="rosbag" type="play"
args="-s 15.0 -d 1 -r 1 --clock --hz 10 $(find amcl)/test/texas_willow_hallway_loop_indexed.bag"/>
<node pkg="amcl" type="amcl" name="amcl" respawn="false" output="screen">
<remap from="scan" to="base_scan" />
<param name="transform_tolerance" value="0.2"/>
<param name="gui_publish_rate" value="10.0"/>
<param name="save_pose_rate" value="0.5"/>
<param name="laser_max_beams" value="30"/>
<param name="min_particles" value="500"/>
<param name="max_particles" value="5000"/>
<param name="kld_err" value="0.05"/>
<param name="kld_z" value="0.99"/>
<param name="odom_model_type" value="diff-corrected"/>
<param name="odom_alpha1" value="0.04"/>
<param name="odom_alpha2" value="0.6"/>
<param name="odom_alpha3" value="0.3"/>
<param name="odom_alpha4" value="0.04"/>
<param name="laser_z_hit" value="0.5"/>
<param name="laser_z_rand" value="0.5"/>
<param name="laser_sigma_hit" value="0.25"/>
<param name="laser_max_range" value="5.0"/>
<param name="laser_model_type" value="likelihood_field"/>
<param name="laser_likelihood_max_dist" value="2.0"/>
<param name="update_min_d" value="0.2"/>
<param name="update_min_a" value="0.5"/>
<param name="odom_frame_id" value="odom_combined"/>
<param name="resample_interval" value="1"/>
<param name="initial_pose_x" value="31.872"/>
<param name="initial_pose_y" value="31.557"/>
<param name="initial_pose_a" value="0.0"/>
</node>
<test time-limit="350" test-name="texas_willow_hallway_loop_corrected" pkg="amcl"
type="basic_localization.py" args="0 32.36 31.44 6.18 0.75 0.75 250.0"/>
</launch>

10
Localizations/Libraries/Ros/hector_slam/.gitignore vendored Normal file
View File

@@ -0,0 +1,10 @@
bin/
build/
lib/
msg_gen/
srv_gen/
hector_mapping/src/hector_mapping/
hector_nav_msgs/src/hector_nav_msgs/
.idea/
.vscode/
cmake-build-*/

3
Localizations/Libraries/Ros/hector_slam/README.txt Normal file
View File

@@ -0,0 +1,3 @@
# hector_slam
See the ROS Wiki for documentation: http://wiki.ros.org/hector_slam

50
Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,50 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_compressed_map_transport
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
* Fix out of bounds access
Fixes `#52 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/52>`_ and `#70 <https://github.com/tu-darmstadt-ros-pkg/hector_slam/issues/70>`_
* Contributors: Stefan Fabian
0.5.1 (2021-01-15)
------------------
0.5.0 (2020-12-17)
------------------
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Bump CMake version to avoid CMP0048 warning
* Contributors: Marius Schnaubelt, Stefan Fabian
0.4.1 (2020-05-15)
------------------
0.3.6 (2019-10-31)
------------------
0.3.5 (2016-06-24)
------------------
* Use the FindEigen3.cmake module provided by Eigen
* Contributors: Johannes Meyer
0.3.4 (2015-11-07)
------------------
0.3.3 (2014-06-15)
------------------
* fixed cmake find for eigen in indigo
* fixed libopencv-dev rosdep key
* Contributors: Alexander Stumpf, Johannes Meyer
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-10-09)
------------------
* added changelogs
0.3.0 (2013-08-08)
------------------
* catkinized hector_slam

40
Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/CMakeLists.txt Normal file
View File

@@ -0,0 +1,40 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_compressed_map_transport)
find_package(catkin REQUIRED COMPONENTS cv_bridge geometry_msgs hector_map_tools image_transport nav_msgs sensor_msgs)
find_package(Eigen3 REQUIRED)
find_package(OpenCV REQUIRED)
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs sensor_msgs
)
###########
## Build ##
###########
include_directories(
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
)
add_executable(map_to_image_node src/map_to_image_node.cpp)
target_link_libraries(map_to_image_node
${catkin_LIBRARIES}
${OpenCV_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
install(TARGETS map_to_image_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

65
Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/package.xml Normal file
View File

@@ -0,0 +1,65 @@
<?xml version="1.0"?>
<package>
<name>hector_compressed_map_transport</name>
<version>0.5.2</version>
<description>
hector_compressed_map_transport provides means for transporting compressed map data through the use of image_transport.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</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>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">http://ros.org/wiki/hector_compressed_map_transport</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>cv_bridge</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>hector_map_tools</build_depend>
<build_depend>image_transport</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>eigen</build_depend>
<run_depend>cv_bridge</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>hector_map_tools</run_depend>
<run_depend>image_transport</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>eigen</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

270
Localizations/Libraries/Ros/hector_slam/hector_compressed_map_transport/src/map_to_image_node.cpp Normal file
View File

@@ -0,0 +1,270 @@
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#include "ros/ros.h"
#include <nav_msgs/GetMap.h>
#include <geometry_msgs/Quaternion.h>
#include <geometry_msgs/PoseStamped.h>
#include <sensor_msgs/image_encodings.h>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <Eigen/Geometry>
#include <hector_map_tools/HectorMapTools.h>
using namespace std;
/**
* @brief This node provides occupancy grid maps as images via image_transport, so the transmission consumes less bandwidth.
* The provided code is a incomplete proof of concept.
*/
class MapAsImageProvider
{
public:
MapAsImageProvider()
: pn_("~")
{
image_transport_ = new image_transport::ImageTransport(n_);
image_transport_publisher_full_ = image_transport_->advertise("map_image/full", 1);
image_transport_publisher_tile_ = image_transport_->advertise("map_image/tile", 1);
pose_sub_ = n_.subscribe("pose", 1, &MapAsImageProvider::poseCallback, this);
map_sub_ = n_.subscribe("map", 1, &MapAsImageProvider::mapCallback, this);
//Which frame_id makes sense?
cv_img_full_.header.frame_id = "map_image";
cv_img_full_.encoding = sensor_msgs::image_encodings::MONO8;
cv_img_tile_.header.frame_id = "map_image";
cv_img_tile_.encoding = sensor_msgs::image_encodings::MONO8;
//Fixed cell width for tile based image, use dynamic_reconfigure for this later
p_size_tiled_map_image_x_ = 64;
p_size_tiled_map_image_y_ = 64;
ROS_INFO("Map to Image node started.");
}
~MapAsImageProvider()
{
delete image_transport_;
}
//We assume the robot position is available as a PoseStamped here (querying tf would be the more general option)
void poseCallback(const geometry_msgs::PoseStampedConstPtr& pose)
{
pose_ptr_ = pose;
}
//The map->image conversion runs every time a new map is received at the moment
void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
{
int size_x = map->info.width;
int size_y = map->info.height;
if ((size_x < 3) || (size_y < 3) ){
ROS_INFO("Map size is only x: %d, y: %d . Not running map to image conversion", size_x, size_y);
return;
}
// Only if someone is subscribed to it, do work and publish full map image
if (image_transport_publisher_full_.getNumSubscribers() > 0){
cv::Mat* map_mat = &cv_img_full_.image;
// resize cv image if it doesn't have the same dimensions as the map
if ( (map_mat->rows != size_y) && (map_mat->cols != size_x)){
*map_mat = cv::Mat(size_y, size_x, CV_8U);
}
const std::vector<int8_t>& map_data (map->data);
unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
//We have to flip around the y axis, y for image starts at the top and y for map at the bottom
int size_y_rev = size_y-1;
for (int y = size_y_rev; y >= 0; --y){
int idx_map_y = size_x * (size_y_rev - y);
int idx_img_y = size_x * y;
for (int x = 0; x < size_x; ++x){
int idx = idx_img_y + x;
switch (map_data[idx_map_y + x])
{
case -1:
map_mat_data_p[idx] = 127;
break;
case 0:
map_mat_data_p[idx] = 255;
break;
case 100:
map_mat_data_p[idx] = 0;
break;
}
}
}
image_transport_publisher_full_.publish(cv_img_full_.toImageMsg());
}
// Only if someone is subscribed to it, do work and publish tile-based map image Also check if pose_ptr_ is valid
if ((image_transport_publisher_tile_.getNumSubscribers() > 0) && (pose_ptr_)){
world_map_transformer_.setTransforms(*map);
Eigen::Vector2f rob_position_world (pose_ptr_->pose.position.x, pose_ptr_->pose.position.y);
Eigen::Vector2f rob_position_map (world_map_transformer_.getC2Coords(rob_position_world));
Eigen::Vector2i rob_position_mapi (rob_position_map.cast<int>());
Eigen::Vector2i tile_size_lower_halfi (p_size_tiled_map_image_x_ / 2, p_size_tiled_map_image_y_ / 2);
Eigen::Vector2i min_coords_map (rob_position_mapi - tile_size_lower_halfi);
//Clamp to lower map coords
if (min_coords_map[0] < 0){
min_coords_map[0] = 0;
}
if (min_coords_map[1] < 0){
min_coords_map[1] = 0;
}
Eigen::Vector2i max_coords_map (min_coords_map + Eigen::Vector2i(p_size_tiled_map_image_x_,p_size_tiled_map_image_y_));
//Clamp to upper map coords
if (max_coords_map[0] > size_x){
int diff = max_coords_map[0] - size_x;
min_coords_map[0] -= diff;
max_coords_map[0] = size_x;
}
if (max_coords_map[1] > size_y){
int diff = max_coords_map[1] - size_y;
min_coords_map[1] -= diff;
max_coords_map[1] = size_y;
}
//Clamp lower again (in case the map is smaller than the selected visualization window)
if (min_coords_map[0] < 0){
min_coords_map[0] = 0;
}
if (min_coords_map[1] < 0){
min_coords_map[1] = 0;
}
Eigen::Vector2i actual_map_dimensions(max_coords_map - min_coords_map);
cv::Mat* map_mat = &cv_img_tile_.image;
// resize cv image if it doesn't have the same dimensions as the selected visualization window
if ( (map_mat->rows != actual_map_dimensions[0]) || (map_mat->cols != actual_map_dimensions[1])){
*map_mat = cv::Mat(actual_map_dimensions[0], actual_map_dimensions[1], CV_8U);
}
const std::vector<int8_t>& map_data (map->data);
unsigned char *map_mat_data_p=(unsigned char*) map_mat->data;
//We have to flip around the y axis, y for image starts at the top and y for map at the bottom
int y_img = max_coords_map[1]-1;
for (int y = min_coords_map[1]; y < max_coords_map[1];++y){
int idx_map_y = y_img-- * size_x;
int idx_img_y = (y-min_coords_map[1]) * actual_map_dimensions.x();
for (int x = min_coords_map[0]; x < max_coords_map[0];++x){
int img_index = idx_img_y + (x-min_coords_map[0]);
switch (map_data[idx_map_y+x])
{
case 0:
map_mat_data_p[img_index] = 255;
break;
case -1:
map_mat_data_p[img_index] = 127;
break;
case 100:
map_mat_data_p[img_index] = 0;
break;
}
}
}
image_transport_publisher_tile_.publish(cv_img_tile_.toImageMsg());
}
}
ros::Subscriber map_sub_;
ros::Subscriber pose_sub_;
image_transport::Publisher image_transport_publisher_full_;
image_transport::Publisher image_transport_publisher_tile_;
image_transport::ImageTransport* image_transport_;
geometry_msgs::PoseStampedConstPtr pose_ptr_;
cv_bridge::CvImage cv_img_full_;
cv_bridge::CvImage cv_img_tile_;
ros::NodeHandle n_;
ros::NodeHandle pn_;
int p_size_tiled_map_image_x_;
int p_size_tiled_map_image_y_;
HectorMapTools::CoordinateTransformer<float> world_map_transformer_;
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_to_image_node");
MapAsImageProvider map_image_provider;
ros::spin();
return 0;
}

68
Localizations/Libraries/Ros/hector_slam/hector_geotiff/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,68 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_geotiff
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
* Refactored hector_geotiff dependencies.
* Contributors: Stefan Fabian
0.5.1 (2021-01-15)
------------------
* Fixed "SEVERE WARNING" by pluginloader when killing geotiff node.
Some minor cleanup.
* Contributors: Stefan Fabian
0.5.0 (2020-12-17)
------------------
* Added missing dependency for Qt5 cmake.
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Qt5 support for hector geotiff on headless systems.
* Updated platform args. Test on robot.
* Experiments with platform argument.
* Renamed depends for (hopefully soon) rosdep compatibility.
* Moved to Qt5.
* Contributors: Stefan Fabian
0.4.1 (2020-05-15)
------------------
0.3.6 (2019-10-31)
------------------
* Update geotiff draw interface to support different shapes
* Contributors: Stefan Kohlbrecher
0.3.5 (2016-06-24)
------------------
* Use the FindEigen3.cmake module provided by Eigen
* hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths
* Contributors: Dorothea Koert, Johannes Meyer
0.3.4 (2015-11-07)
------------------
* Removes trailing spaces and fixes indents
* Contributors: YuK_Ota
0.3.3 (2014-06-15)
------------------
* fixed cmake find for eigen in indigo
* added launchfile to restart geotiff node
* Contributors: Alexander Stumpf
0.3.2 (2014-03-30)
------------------
* Add TrajectoryMapWriter to geotiff_mapper.launch per default
* Add arguments to launch files for specifying geotiff file path
* Contributors: Stefan Kohlbrecher
0.3.1 (2013-10-09)
------------------
* added missing install rule for launch files
* moved header files from include/geotiff_writer to include/hector_geotiff
* fixed warnings for deprecated pluginlib method/macro calls
* added changelogs
0.3.0 (2013-08-08)
------------------
* catkinized hector_slam

63
Localizations/Libraries/Ros/hector_slam/hector_geotiff/CMakeLists.txt Normal file
View File

@@ -0,0 +1,63 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_geotiff)
set(CMAKE_CXX_STANDARD 14)
find_package(catkin REQUIRED COMPONENTS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs)
find_package(Qt5 COMPONENTS Widgets REQUIRED)
find_package(Eigen3 REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES geotiff_writer
CATKIN_DEPENDS hector_map_tools hector_nav_msgs nav_msgs pluginlib roscpp std_msgs
DEPENDS EIGEN3 Qt5Widgets
)
###########
## Build ##
###########
include_directories(
include
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${Qt5Widgets_INCLUDE_DIRS}
)
add_library(geotiff_writer src/geotiff_writer/geotiff_writer.cpp)
target_link_libraries(geotiff_writer ${catkin_LIBRARIES} Qt5::Widgets stdc++fs)
add_dependencies(geotiff_writer ${catkin_EXPORTED_TARGETS})
add_executable(geotiff_saver src/geotiff_saver.cpp)
target_link_libraries(geotiff_saver geotiff_writer)
add_dependencies(geotiff_saver ${catkin_EXPORTED_TARGETS})
add_executable(geotiff_node src/geotiff_node.cpp)
target_link_libraries(geotiff_node geotiff_writer)
add_dependencies(geotiff_node ${catkin_EXPORTED_TARGETS})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
install(TARGETS geotiff_writer geotiff_saver geotiff_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
install(DIRECTORY fonts
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

203
Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/LICENSE.txt Normal file
View File

@@ -0,0 +1,203 @@
License for Roboto-Regular.ttf:
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

BIN
Localizations/Libraries/Ros/hector_slam/hector_geotiff/fonts/Roboto-Regular.ttf Normal file
View File

Binary file not shown.

139
Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/geotiff_writer.h Normal file
View File

@@ -0,0 +1,139 @@
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#ifndef _GEOTIFFWRITER_H__
#define _GEOTIFFWRITER_H__
#include "map_writer_interface.h"
#include <Eigen/Geometry>
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include <QImage>
#include <QApplication>
#include <QFont>
#include <hector_map_tools/HectorMapTools.h>
#if __cplusplus < 201703L
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#else
#include <filesystem>
namespace fs = std::filesystem;
#endif
namespace hector_geotiff{
class GeotiffWriter : public MapWriterInterface
{
public:
explicit GeotiffWriter(bool useCheckerboardCacheIn = false);
virtual ~GeotiffWriter();
//setUsePrecalcGrid(bool usePrecalc, const Eigen::Vector2f& size);
void setMapFileName(const std::string& mapFileName);
void setMapFilePath(const std::string& mapFilePath);
void setUseUtcTimeSuffix(bool useSuffix);
void setupImageSize();
bool setupTransforms(const nav_msgs::OccupancyGrid& map);
void drawBackgroundCheckerboard();
void drawMap(const nav_msgs::OccupancyGrid& map, bool draw_explored_space_grid = true);
void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape);
inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points){
drawPath(start, points, 120,0,240);
}
void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points, int color_r, int color_g, int color_b);
void drawCoords();
std::string getBasePathAndFileName() const;
void writeGeotiffImage(bool completed);
protected:
void transformPainterToImgCoords(QPainter& painter);
void drawCross(QPainter& painter, const Eigen::Vector2f& coords);
void drawArrow(QPainter& painter);
void drawCoordSystem(QPainter& painter);
float resolution = std::numeric_limits<float>::quiet_NaN();
Eigen::Vector2f origin;
int resolutionFactor = 3;
float resolutionFactorf = std::numeric_limits<float>::quiet_NaN();
bool useCheckerboardCache;
bool use_utc_time_suffix_;
float pixelsPerMapMeter = std::numeric_limits<float>::quiet_NaN();
float pixelsPerGeoTiffMeter = std::numeric_limits<float>::quiet_NaN();
Eigen::Vector2i minCoordsMap;
Eigen::Vector2i maxCoordsMap;
Eigen::Vector2i sizeMap;
Eigen::Vector2f sizeMapf;
Eigen::Vector2f rightBottomMarginMeters;
Eigen::Vector2f rightBottomMarginPixelsf;
Eigen::Vector2i rightBottomMarginPixels;
Eigen::Vector2f leftTopMarginMeters;
Eigen::Vector2f totalMeters;
Eigen::Vector2i geoTiffSizePixels;
Eigen::Vector2f mapOrigInGeotiff;
Eigen::Vector2f mapEndInGeotiff;
std::string map_file_name_;
std::string map_file_path_;
QImage image;
QImage checkerboard_cache;
QApplication* app;
QString font_family_;
QFont map_draw_font_;
HectorMapTools::CoordinateTransformer<float> world_map_transformer_;
HectorMapTools::CoordinateTransformer<float> map_geo_transformer_;
HectorMapTools::CoordinateTransformer<float> world_geo_transformer_;
nav_msgs::MapMetaData cached_map_meta_data_;
};
}
#endif

64
Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_interface.h Normal file
View File

@@ -0,0 +1,64 @@
//=================================================================================================
// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#ifndef _MAPWRITERINTERFACE_H__
#define _MAPWRITERINTERFACE_H__
#include <vector>
#include <Eigen/Core>
namespace hector_geotiff{
enum Shape {
SHAPE_CIRCLE,
SHAPE_DIAMOND
};
class MapWriterInterface{
public:
struct Color {
Color(unsigned int r, unsigned int g, unsigned int b) : r(r), g(g), b(b) {}
unsigned int r,g,b;
};
bool completed_map_ = false;
virtual std::string getBasePathAndFileName() const = 0;
virtual void drawObjectOfInterest(const Eigen::Vector2f& coords, const std::string& txt, const Color& color, const Shape& shape = SHAPE_CIRCLE) = 0;
//virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points) = 0;
inline virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points){
drawPath(start, points, 120,0,240);
}
virtual void drawPath(const Eigen::Vector3f& start, const std::vector<Eigen::Vector2f>& points, int color_r, int color_g, int color_b) = 0;
};
}
#endif

47
Localizations/Libraries/Ros/hector_slam/hector_geotiff/include/hector_geotiff/map_writer_plugin_interface.h Normal file
View File

@@ -0,0 +1,47 @@
//=================================================================================================
// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#ifndef _MAPWRITERPLUGININTERFACE_H__
#define _MAPWRITERPLUGININTERFACE_H__
#include "map_writer_interface.h"
namespace hector_geotiff{
class MapWriterPluginInterface{
public:
virtual void initialize(const std::string& name) = 0;
virtual void draw(MapWriterInterface* map_writer_interface) = 0;
};
} //namespace hector_geotiff
#endif

50
Localizations/Libraries/Ros/hector_slam/hector_geotiff/package.xml Normal file
View File

@@ -0,0 +1,50 @@
<?xml version="1.0"?>
<package format="2">
<name>hector_geotiff</name>
<version>0.5.2</version>
<description>
hector_geotiff provides a node that can be used to save occupancy grid map, robot trajectory and object of interest data to RoboCup Rescue compliant GeoTiff images.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</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>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<url type="website">http://ros.org/wiki/hector_geotiff</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>qtbase5-dev</build_depend>
<build_export_depend>qtbase5-dev</build_export_depend>
<exec_depend>qt5-image-formats-plugins</exec_depend>
<depend>hector_map_tools</depend>
<depend>hector_nav_msgs</depend>
<depend>libqt5-widgets</depend>
<depend>nav_msgs</depend>
<depend>pluginlib</depend>
<depend>roscpp</depend>
<depend>std_msgs</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

325
Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_node.cpp Normal file
View File

@@ -0,0 +1,325 @@
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#include "hector_geotiff/geotiff_writer.h"
#include "hector_geotiff/map_writer_plugin_interface.h"
#include <cstdio>
#include <ros/ros.h>
#include <ros/console.h>
#include <pluginlib/class_loader.h>
#include <memory>
#include <boost/algorithm/string.hpp>
#include <geometry_msgs/Quaternion.h>
#include <nav_msgs/GetMap.h>
#include <std_msgs/String.h>
#include <hector_nav_msgs/GetRobotTrajectory.h>
#include <QApplication>
using namespace std;
namespace hector_geotiff{
/**
* @brief Map generation node.
*/
class MapGenerator
{
public:
MapGenerator()
: geotiff_writer_(false)
, pn_("~")
, running_saved_map_num_(0)
{
pn_.param("map_file_path", p_map_file_path_, std::string("."));
geotiff_writer_.setMapFilePath(p_map_file_path_);
geotiff_writer_.setUseUtcTimeSuffix(true);
pn_.param("map_file_base_name", p_map_file_base_name_, std::string());
pn_.param("draw_background_checkerboard", p_draw_background_checkerboard_, true);
pn_.param("draw_free_space_grid", p_draw_free_space_grid_, true);
sys_cmd_sub_ = n_.subscribe("syscommand", 1, &MapGenerator::sysCmdCallback, this);
pn_.param("use_map_topic", use_map_topic_,false);
if(!use_map_topic_) map_service_client_ = n_.serviceClient<nav_msgs::GetMap>("map");
//object_service_client_ = n_.serviceClient<worldmodel_msgs::GetObjectModel>("worldmodel/get_object_model");
path_service_client_ = n_.serviceClient<hector_nav_msgs::GetRobotTrajectory>("trajectory");
double p_geotiff_save_period = 0.0;
pn_.param("geotiff_save_period", p_geotiff_save_period, 0.0);
if(p_geotiff_save_period > 0.0){
//ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, false);
//publish_trajectory_timer_ = private_nh.createTimer(ros::Duration(1.0 / p_trajectory_publish_rate_), &PathContainer::publishTrajectoryTimerCallback, this, false);
map_save_timer_ = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, this, false );
}
pn_.param("plugins", p_plugin_list_, std::string(""));
std::vector<std::string> plugin_list;
boost::algorithm::split(plugin_list, p_plugin_list_, boost::is_any_of("\t "));
//We always have at least one element containing "" in the string list
if ((plugin_list.size() > 0) && (plugin_list[0].length() > 0)){
plugin_loader_ = std::make_unique<pluginlib::ClassLoader<hector_geotiff::MapWriterPluginInterface>>("hector_geotiff", "hector_geotiff::MapWriterPluginInterface");
for (size_t i = 0; i < plugin_list.size(); ++i){
try
{
boost::shared_ptr<hector_geotiff::MapWriterPluginInterface> tmp (plugin_loader_->createInstance(plugin_list[i]));
tmp->initialize(plugin_loader_->getName(plugin_list[i]));
plugin_vector_.push_back(tmp);
}
catch(pluginlib::PluginlibException& ex)
{
ROS_ERROR("The plugin failed to load for some reason. Error: %s", ex.what());
}
}
}else{
ROS_INFO("No plugins loaded for geotiff node");
}
ROS_INFO("Geotiff node started");
}
~MapGenerator() = default;
void writeGeotiff(bool completed)
{
ros::Time start_time (ros::Time::now());
std::stringstream ssStream;
bool received_map = false;
boost::shared_ptr<const nav_msgs::OccupancyGrid> map;
nav_msgs::GetMap srv_map;
if (use_map_topic_)
{
map = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>("map",ros::Duration(4));
if (map != nullptr) received_map = true;
}
else
{
received_map = map_service_client_.call(srv_map);
map = boost::make_shared<nav_msgs::OccupancyGrid>(srv_map.response.map);
}
if (received_map)
{
ROS_INFO("GeotiffNode: Map service called successfully");
std::string map_file_name = p_map_file_base_name_;
std::string competition_name;
std::string team_name;
std::string mission_name;
std::string postfix;
if (n_.getParamCached("/competition", competition_name) && !competition_name.empty()) map_file_name = map_file_name + "_" + competition_name;
if (n_.getParamCached("/team", team_name) && !team_name.empty()) map_file_name = map_file_name + "_" + team_name;
if (n_.getParamCached("/mission", mission_name) && !mission_name.empty()) map_file_name = map_file_name + "_" + mission_name;
if (pn_.getParamCached("map_file_postfix", postfix) && !postfix.empty()) map_file_name = map_file_name + "_" + postfix;
if (map_file_name.substr(0, 1) == "_") map_file_name = map_file_name.substr(1);
if (map_file_name.empty()) map_file_name = "GeoTiffMap";
geotiff_writer_.setMapFileName(map_file_name);
bool transformSuccess = geotiff_writer_.setupTransforms(*map);
if(!transformSuccess){
ROS_INFO("Couldn't set map transform");
return;
}
geotiff_writer_.setupImageSize();
if (p_draw_background_checkerboard_){
geotiff_writer_.drawBackgroundCheckerboard();
}
geotiff_writer_.drawMap(*map, p_draw_free_space_grid_);
geotiff_writer_.drawCoords();
geotiff_writer_.completed_map_ = completed;
//ROS_INFO("Sum: %ld", (long int)srv.response.sum);
}
else
{
ROS_ERROR("Failed to call map service");
return;
}
ROS_INFO("Writing geotiff plugins");
for (size_t i = 0; i < plugin_vector_.size(); ++i){
plugin_vector_[i]->draw(&geotiff_writer_);
}
ROS_INFO("Writing geotiff");
/**
* No Victims for now, first agree on a common standard for representation
*/
/*
if (req_object_model_){
worldmodel_msgs::GetObjectModel srv_objects;
if (object_service_client_.call(srv_objects))
{
ROS_INFO("GeotiffNode: Object service called successfully");
const worldmodel_msgs::ObjectModel& objects_model (srv_objects.response.model);
size_t size = objects_model.objects.size();
unsigned int victim_num = 1;
for (size_t i = 0; i < size; ++i){
const worldmodel_msgs::Object& object (objects_model.objects[i]);
if (object.state.state == worldmodel_msgs::ObjectState::CONFIRMED){
geotiff_writer_.drawVictim(Eigen::Vector2f(object.pose.pose.position.x,object.pose.pose.position.y),victim_num);
victim_num++;
}
}
}
else
{
ROS_ERROR("Failed to call objects service");
}
}
*/
/*
hector_nav_msgs::GetRobotTrajectory srv_path;
if (path_service_client_.call(srv_path))
{
ROS_INFO("GeotiffNode: Path service called successfully");
std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
size_t size = traj_vector.size();
std::vector<Eigen::Vector2f> pointVec;
pointVec.resize(size);
for (size_t i = 0; i < size; ++i){
const geometry_msgs::PoseStamped& pose (traj_vector[i]);
pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
}
if (size > 0){
//Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
geotiff_writer_.drawPath(startVec, pointVec);
}
}
else
{
ROS_ERROR("Failed to call path service");
}
*/
geotiff_writer_.writeGeotiffImage(completed);
running_saved_map_num_++;
ros::Duration elapsed_time (ros::Time::now() - start_time);
ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec());
}
void timerSaveGeotiffCallback(const ros::TimerEvent& e)
{
this->writeGeotiff(false);
}
void sysCmdCallback(const std_msgs::String& sys_cmd)
{
if (sys_cmd.data != "savegeotiff"){
return;
}
this->writeGeotiff(true);
}
std::string p_map_file_path_;
std::string p_map_file_base_name_;
std::string p_plugin_list_;
bool p_draw_background_checkerboard_;
bool p_draw_free_space_grid_;
bool use_map_topic_;
//double p_geotiff_save_period_;
ros::NodeHandle n_;
ros::NodeHandle pn_;
ros::ServiceClient map_service_client_;// = n.serviceClient<beginner_tutorials::AddTwoInts>("add_two_ints");
ros::ServiceClient object_service_client_;
ros::ServiceClient path_service_client_;
ros::Subscriber sys_cmd_sub_;
std::unique_ptr<pluginlib::ClassLoader<hector_geotiff::MapWriterPluginInterface>> plugin_loader_;
std::vector<boost::shared_ptr<hector_geotiff::MapWriterPluginInterface> > plugin_vector_;
GeotiffWriter geotiff_writer_;
ros::Timer map_save_timer_;
unsigned int running_saved_map_num_;
std::string start_dir_;
};
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "geotiff_node");
hector_geotiff::MapGenerator mg;
//ros::NodeHandle pn_;
//double p_geotiff_save_period = 60.0f;
//pn_.param("geotiff_save_period", p_geotiff_save_period, 60.0);
//ros::Timer timer = pn_.createTimer(ros::Duration(p_geotiff_save_period), &MapGenerator::timerSaveGeotiffCallback, &mg, false);
ros::spin();
return 0;
}

121
Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_saver.cpp Normal file
View File

@@ -0,0 +1,121 @@
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#include "hector_geotiff/geotiff_writer.h"
#include <cstdio>
#include <ros/ros.h>
#include <ros/console.h>
#include <nav_msgs/GetMap.h>
#include <QApplication>
using namespace std;
namespace hector_geotiff{
/**
* @brief Map generation node.
*/
class MapGenerator
{
public:
MapGenerator(const std::string& mapname) : mapname_(mapname)
{
ros::NodeHandle n;
ROS_INFO("Waiting for the map");
map_sub_ = n.subscribe("map", 1, &MapGenerator::mapCallback, this);
}
void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
{
ros::Time start_time (ros::Time::now());
geotiff_writer.setMapFileName(mapname_);
geotiff_writer.setupTransforms(*map);
geotiff_writer.drawBackgroundCheckerboard();
geotiff_writer.drawMap(*map);
geotiff_writer.drawCoords();
geotiff_writer.writeGeotiffImage(true);
ros::Duration elapsed_time (ros::Time::now() - start_time);
ROS_INFO("GeoTiff created in %f seconds", elapsed_time.toSec());
}
GeotiffWriter geotiff_writer;
std::string mapname_;
ros::Subscriber map_sub_;
};
}
#define USAGE "Usage: \n" \
" geotiff_saver -h\n"\
" geotiff_saver [-f <mapname>] [ROS remapping args]"
int main(int argc, char** argv)
{
ros::init(argc, argv, "map_saver");
std::string mapname = "map";
for(int i=1; i<argc; i++)
{
if(!strcmp(argv[i], "-h"))
{
puts(USAGE);
return 0;
}
else if(!strcmp(argv[i], "-f"))
{
if(++i < argc)
mapname = argv[i];
else
{
puts(USAGE);
return 1;
}
}
else
{
puts(USAGE);
return 1;
}
}
//GeotiffWriter geotiff_writer;
//geotiff_writer.setMapName("test");
hector_geotiff::MapGenerator mg(mapname);
ros::spin();
return 0;
}

714
Localizations/Libraries/Ros/hector_slam/hector_geotiff/src/geotiff_writer/geotiff_writer.cpp Normal file
View File

@@ -0,0 +1,714 @@
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#include "hector_geotiff/geotiff_writer.h"
#include <ros/console.h>
#include <QFile>
#include <QImageWriter>
#include <QPainter>
//#include <QtCore/QDateTime>
#include <QTime>
#include <QTextStream>
#include <QFontDatabase>
#include <ros/package.h>
#if __cplusplus < 201703L
#include <experimental/filesystem>
namespace fs = std::experimental::filesystem;
#else
#include <filesystem>
namespace fs = std::filesystem;
#endif
namespace hector_geotiff
{
GeotiffWriter::GeotiffWriter( bool useCheckerboardCacheIn )
: useCheckerboardCache( useCheckerboardCacheIn )
, use_utc_time_suffix_( true )
{
cached_map_meta_data_.height = -1;
cached_map_meta_data_.width = -1;
cached_map_meta_data_.resolution = -1.0f;
int fake_argc = 3;
char *fake_argv[3] = { new char[15], new char[10], new char[10] };
strcpy( fake_argv[0], "geotiff_writer" );
strcpy( fake_argv[1], "-platform" );
strcpy( fake_argv[2], "offscreen" ); // Set the env QT_DEBUG_PLUGINS to 1 to see available platforms
ROS_INFO("Creating application with offscreen platform.");
//Create a QApplication cause otherwise drawing text will crash
app = new QApplication( fake_argc, fake_argv );
delete[] fake_argv[0];
delete[] fake_argv[1];
delete[] fake_argv[2];
ROS_INFO("Created application");
std::string font_path = ros::package::getPath( "hector_geotiff" ) + "/fonts/Roboto-Regular.ttf";
int id = QFontDatabase::addApplicationFont( QString::fromStdString( font_path ));
font_family_ = QFontDatabase::applicationFontFamilies( id ).at( 0 );
map_file_name_ = "";
map_file_path_ = "";
}
GeotiffWriter::~GeotiffWriter()
{
delete app;
}
void GeotiffWriter::setMapFileName( const std::string &mapFileName )
{
map_file_name_ = mapFileName;
if ( use_utc_time_suffix_ )
{
//QDateTime now (QDateTime::currentDateTimeUtc());
//std::string current_time_string = now.toString(Qt::ISODate).toStdString();
QTime now( QTime::currentTime());
std::string current_time_string = now.toString( Qt::ISODate ).toStdString();
map_file_name_ += "_" + current_time_string;
}
}
void GeotiffWriter::setMapFilePath( const std::string &mapFilePath )
{
map_file_path_ = mapFilePath;
}
void GeotiffWriter::setUseUtcTimeSuffix( bool useSuffix )
{
use_utc_time_suffix_ = useSuffix;
}
bool GeotiffWriter::setupTransforms( const nav_msgs::OccupancyGrid &map )
{
resolution = static_cast<float>(map.info.resolution);
origin = Eigen::Vector2f( map.info.origin.position.x, map.info.origin.position.y );
resolutionFactor = 3;
resolutionFactorf = static_cast<float>(resolutionFactor);
pixelsPerMapMeter = 1.0f / map.info.resolution;
pixelsPerGeoTiffMeter = pixelsPerMapMeter * static_cast<float>(resolutionFactor);
minCoordsMap = Eigen::Vector2i::Zero();
maxCoordsMap = Eigen::Vector2i( map.info.width, map.info.height );
if ( !HectorMapTools::getMapExtends( map, minCoordsMap, maxCoordsMap ))
{
ROS_INFO( "Cannot determine map extends!" );
return false;
}
sizeMap = Eigen::Vector2i( maxCoordsMap - minCoordsMap );
sizeMapf = ((maxCoordsMap - minCoordsMap).cast<float>());
rightBottomMarginMeters = Eigen::Vector2f( 1.0f, 1.0f );
rightBottomMarginPixelsf = Eigen::Vector2f( rightBottomMarginMeters.array() * pixelsPerGeoTiffMeter );
rightBottomMarginPixels = ((rightBottomMarginPixelsf.array() + 0.5f).cast<int>());
leftTopMarginMeters = Eigen::Vector2f( 3.0f, 3.0f );
totalMeters = (rightBottomMarginMeters + sizeMapf * map.info.resolution + leftTopMarginMeters);
//std::cout << "\n" << totalMeters;
totalMeters.x() = ceil( totalMeters.x());
totalMeters.y() = ceil( totalMeters.y());
//std::cout << "\n" << totalMeters;
geoTiffSizePixels = ((totalMeters.array() * pixelsPerGeoTiffMeter).cast<int>());
mapOrigInGeotiff = (rightBottomMarginPixelsf);
mapEndInGeotiff = (rightBottomMarginPixelsf + sizeMapf * resolutionFactorf);
//std::cout << "\n mapOrig\n" << mapOrigInGeotiff;
//std::cout << "\n mapOrig\n" << mapEndInGeotiff;
world_map_transformer_.setTransforms( map );
map_geo_transformer_.setTransformsBetweenCoordSystems( mapOrigInGeotiff, mapEndInGeotiff, minCoordsMap.cast<float>(),
maxCoordsMap.cast<float>());
/*
Eigen::Vector2f temp_zero_map_g (map_geo_transformer_.getC2Coords(Eigen::Vector2f::Zero()));
Eigen::Vector2f temp_zero_map_g_floor (floor(temp_zero_map_g.x()), floor(temp_zero_map_g.x()));
Eigen::Vector2f diff (temp_zero_map_g - temp_zero_map_g_floor);
map*/
Eigen::Vector2f p1_w( Eigen::Vector2f::Zero());
Eigen::Vector2f p2_w( Eigen::Vector2f( 100.0f, 100.0f ));
Eigen::Vector2f p1_m( world_map_transformer_.getC2Coords( p1_w ));
Eigen::Vector2f p2_m( world_map_transformer_.getC2Coords( p2_w ));
Eigen::Vector2f p1_g( map_geo_transformer_.getC2Coords( p1_m ));
Eigen::Vector2f p2_g( map_geo_transformer_.getC2Coords( p2_m ));
world_geo_transformer_.setTransformsBetweenCoordSystems( p1_g, p2_g, p1_w, p2_w );
map_draw_font_ = QFont( font_family_ );
map_draw_font_.setPixelSize( 6 * resolutionFactor );
if ( useCheckerboardCache )
{
if ((cached_map_meta_data_.height != map.info.height) ||
(cached_map_meta_data_.width != map.info.width) ||
(cached_map_meta_data_.resolution = map.info.resolution))
{
cached_map_meta_data_ = map.info;
Eigen::Vector2f img_size( Eigen::Vector2f( map.info.width, map.info.height ) * resolutionFactorf +
(rightBottomMarginMeters + leftTopMarginMeters) * pixelsPerGeoTiffMeter );
checkerboard_cache = QImage( img_size.y(), img_size.x(), QImage::Format_RGB32 );
QPainter qPainter( &image );
transformPainterToImgCoords( qPainter );
QBrush c1 = QBrush( QColor( 226, 226, 227 ));
QBrush c2 = QBrush( QColor( 237, 237, 238 ));
QRectF background_grid_tile( 0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter );
int xMaxGeo = geoTiffSizePixels[0];
int yMaxGeo = geoTiffSizePixels[1];
for ( int y = 0; y < yMaxGeo; ++y )
{
for ( int x = 0; x < xMaxGeo; ++x )
{
//std::cout << "\n" << x << " " << y;
if ((x + y) % 2 == 0 )
{
//qPainter.fillRect(background_grid_tile, c1);
qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
pixelsPerGeoTiffMeter, c1 );
}
else
{
//qPainter.fillRect(background_grid_tile, c2);
qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
pixelsPerGeoTiffMeter, c2 );
}
//background_grid_tile.moveTo(QPointF(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter));
}
}
}
}
return true;
}
void GeotiffWriter::setupImageSize()
{
bool painter_rotate = true;
int xMaxGeo = geoTiffSizePixels[0];
int yMaxGeo = geoTiffSizePixels[1];
if ( !useCheckerboardCache )
{
if ( painter_rotate )
{
image = QImage( yMaxGeo, xMaxGeo, QImage::Format_RGB32 );
}
else
{
image = QImage( xMaxGeo, yMaxGeo, QImage::Format_RGB32 );
}
QPainter qPainter( &image );
QBrush grey = QBrush( QColor( 128, 128, 128 ));
qPainter.fillRect( image.rect(), grey );
}
}
void GeotiffWriter::drawBackgroundCheckerboard()
{
int xMaxGeo = geoTiffSizePixels[0];
int yMaxGeo = geoTiffSizePixels[1];
bool painter_rotate = true;
if ( !useCheckerboardCache )
{
QPainter qPainter( &image );
if ( painter_rotate )
{
transformPainterToImgCoords( qPainter );
}
//*********************** Background checkerboard pattern **********************
QBrush c1 = QBrush( QColor( 226, 226, 227 ));
QBrush c2 = QBrush( QColor( 237, 237, 238 ));
QRectF background_grid_tile( 0.0f, 0.0f, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter );
for ( int y = 0; y < yMaxGeo; ++y )
{
for ( int x = 0; x < xMaxGeo; ++x )
{
//std::cout << "\n" << x << " " << y;
if ((x + y) % 2 == 0 )
{
//qPainter.fillRect(background_grid_tile, c1);
qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
pixelsPerGeoTiffMeter, c1 );
}
else
{
//qPainter.fillRect(background_grid_tile, c2);
qPainter.fillRect( static_cast<float>(x) * pixelsPerGeoTiffMeter,
static_cast<float>(y) * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter,
pixelsPerGeoTiffMeter, c2 );
}
//background_grid_tile.moveTo(QPointF(static_cast<float>(x)*pixelsPerGeoTiffMeter,static_cast<float>(y)*pixelsPerGeoTiffMeter));
}
}
}
else
{
image = checkerboard_cache.copy( 0, 0, geoTiffSizePixels[0], geoTiffSizePixels[1] );
}
}
void GeotiffWriter::drawMap( const nav_msgs::OccupancyGrid &map, bool draw_explored_space_grid )
{
QPainter qPainter( &image );
transformPainterToImgCoords( qPainter );
//this->drawCoordSystem(qPainter);
QRectF map_cell_grid_tile( 0.0f, 0.0f, resolutionFactor, resolutionFactor );
QBrush occupied_brush( QColor( 0, 40, 120 ));
QBrush free_brush( QColor( 255, 255, 255 ));
QBrush explored_space_grid_brush( QColor( 190, 190, 191 ));
int width = map.info.width;
float explored_space_grid_resolution_pixels = pixelsPerGeoTiffMeter * 0.5f;
float yGeo = 0.0f;
float currYLimit = 0.0f;
bool drawY = false;
for ( int y = minCoordsMap[1]; y < maxCoordsMap[1]; ++y )
{
float xGeo = 0.0f;
if ( yGeo >= currYLimit )
{
drawY = true;
}
float currXLimit = 0.0f;
bool drawX = false;
for ( int x = minCoordsMap[0]; x < maxCoordsMap[0]; ++x )
{
unsigned int i = y * width + x;
int8_t data = map.data[i];
if ( xGeo >= currXLimit )
{
drawX = true;
}
if ( data == 0 )
{
Eigen::Vector2f coords( mapOrigInGeotiff + (Eigen::Vector2f( xGeo, yGeo )));
qPainter.fillRect( coords[0], coords[1], resolutionFactorf, resolutionFactorf, free_brush );
if ( draw_explored_space_grid )
{
if ( drawY )
{
qPainter.fillRect( coords[0], mapOrigInGeotiff.y() + currYLimit, resolutionFactorf, 1.0f,
explored_space_grid_brush );
}
if ( drawX )
{
qPainter.fillRect( mapOrigInGeotiff.x() + currXLimit, coords[1], 1.0f, resolutionFactorf,
explored_space_grid_brush );
}
}
}
else if ( data == 100 )
{
qPainter.fillRect( mapOrigInGeotiff.x() + xGeo, mapOrigInGeotiff.y() + yGeo, resolutionFactorf,
resolutionFactorf, occupied_brush );
}
if ( drawX )
{
currXLimit += explored_space_grid_resolution_pixels;
drawX = false;
}
xGeo += resolutionFactorf;
}
if ( drawY )
{
drawY = false;
currYLimit += explored_space_grid_resolution_pixels;
}
yGeo += resolutionFactorf;
}
}
void GeotiffWriter::drawObjectOfInterest( const Eigen::Vector2f &coords, const std::string &txt, const Color &color,
const Shape &shape )
{
QPainter qPainter( &image );
transformPainterToImgCoords( qPainter );
//qPainter.setPen(ellipse_pen_);
Eigen::Vector2f map_coords( world_map_transformer_.getC2Coords( coords ));
Eigen::Vector2f coords_g( world_geo_transformer_.getC2Coords( coords ));
qPainter.translate( coords_g[0], coords_g[1] );
qPainter.rotate( 90 );
qPainter.setRenderHint( QPainter::Antialiasing, true );
float radius = pixelsPerGeoTiffMeter * 0.175f;
QRectF shape_rect( -radius, -radius, radius * 2.0f, radius * 2.0f );
qPainter.save();
QBrush tmpBrush( QColor( color.r, color.g, color.b ));
QPen tmpPen( Qt::NoPen );
qPainter.setBrush( tmpBrush );
qPainter.setPen( tmpPen );
if ( shape == SHAPE_CIRCLE )
{
qPainter.drawEllipse( shape_rect );
}
else if ( shape == SHAPE_DIAMOND )
{
qPainter.rotate( 45 );
qPainter.drawRect( shape_rect );
}
qPainter.restore();
QString tmp( txt.c_str());
//tmp.setNum(number);
if ( tmp.length() < 2 )
{
qPainter.setFont( map_draw_font_ );
}
else
{
QFont tmp_font( font_family_ );
tmp_font.setPixelSize( 3 * resolutionFactor );
qPainter.setFont( tmp_font );
}
qPainter.setPen( Qt::white );
qPainter.scale( -1.0, 1.0 );
qPainter.drawText( shape_rect, Qt::AlignCenter, tmp );
}
void GeotiffWriter::drawPath( const Eigen::Vector3f &start, const std::vector<Eigen::Vector2f> &points, int color_r,
int color_g, int color_b )
{
QPainter qPainter( &image );
transformPainterToImgCoords( qPainter );
Eigen::Vector2f start_geo( world_geo_transformer_.getC2Coords( start.head<2>()));
size_t size = points.size();
QPolygonF polygon;
polygon.reserve( size );
polygon.push_back( QPointF( start_geo.x(), start_geo.y()));
for ( size_t i = 0; i < size; ++i )
{
const Eigen::Vector2f vec( world_geo_transformer_.getC2Coords( points[i] ));
polygon.push_back( QPointF( vec.x(), vec.y()));
}
QPen pen( qPainter.pen());
pen.setColor( QColor( color_r, color_g, color_b ));
pen.setWidth( 3 );
qPainter.setPen( pen );
//qPainter.setPen(QColor(120,0,240));
qPainter.drawPolyline( polygon );
qPainter.save();
qPainter.translate( start_geo.x(), start_geo.y());
qPainter.rotate( start.z());
qPainter.setRenderHint( QPainter::Antialiasing, true );
drawArrow( qPainter );
//drawCoordSystem(qPainter);
qPainter.restore();
}
std::string GeotiffWriter::getBasePathAndFileName() const
{
return std::string( map_file_path_ + "/" + map_file_name_ );
}
void GeotiffWriter::writeGeotiffImage(bool completed)
{
//Only works with recent Qt versions
//QDateTime now (QDateTime::currentDateTimeUtc());
//std::string current_time_string = now.toString(Qt::ISODate).toStdString();
std::string complete_file_string;
if (completed) {
complete_file_string = map_file_path_ + "/" + map_file_name_;
}
else {
auto t = std::time(nullptr);
auto tm = *std::localtime(&t);
std::stringstream start_ss;
start_ss << std::put_time(&tm, "%Y-%m-%d");
complete_file_string = map_file_path_ + "/autosave";
std::error_code error;
if(!fs::exists(complete_file_string.c_str())) {
fs::create_directory(complete_file_string.c_str(), error);
if (error) {
ROS_ERROR("Can't create autosave folder");
return;
}
}
complete_file_string += "/" + start_ss.str();
if(!fs::exists(complete_file_string.c_str())) {
fs::create_directory(complete_file_string.c_str(), error);
if (error) {
ROS_ERROR("Can't create folder in autosave");
return;
}
}
complete_file_string += "/" + map_file_name_;
}
QImageWriter imageWriter( QString::fromStdString( complete_file_string + ".tif" ));
imageWriter.setCompression( 1 );
bool success = imageWriter.write( image );
std::string tfw_file_name( complete_file_string + ".tfw" );
QFile tfwFile( QString::fromStdString( tfw_file_name ));
tfwFile.open( QIODevice::WriteOnly );
QTextStream out( &tfwFile );
float resolution_geo = resolution / resolutionFactorf;
QString resolution_string;
resolution_string.setNum( resolution_geo, 'f', 10 );
//positive x resolution
out << resolution_string << "\n";
QString zero_string;
zero_string.setNum( 0.0f, 'f', 10 );
//rotation, translation
out << zero_string << "\n" << zero_string << "\n";
//negative y resolution
out << "-" << resolution_string << "\n";
QString top_left_string_x;
QString top_left_string_y;
//Eigen::Vector2f zero_map_w = world_map_transformer_.getC1Coords(Eigen::Vector2f::Zero());
Eigen::Vector2f zero_geo_w( world_geo_transformer_.getC1Coords((geoTiffSizePixels.array() + 1).cast<float>()));
top_left_string_x.setNum( -zero_geo_w.y(), 'f', 10 );
top_left_string_y.setNum( zero_geo_w.x(), 'f', 10 );
out << top_left_string_x << "\n" << top_left_string_y << "\n";
tfwFile.close();
if ( !success )
{
ROS_INFO( "Writing image with file %s failed with error %s", complete_file_string.c_str(),
imageWriter.errorString().toStdString().c_str());
}
else
{
ROS_INFO( "Successfully wrote geotiff to %s", complete_file_string.c_str());
}
}
void GeotiffWriter::transformPainterToImgCoords( QPainter &painter )
{
painter.rotate( -90 );
painter.translate( -geoTiffSizePixels.x(), geoTiffSizePixels.y());
painter.scale( 1.0, -1.0 );
}
void GeotiffWriter::drawCoords()
{
QPainter qPainter( &image );
qPainter.setFont( map_draw_font_ );
float arrowOffset = pixelsPerGeoTiffMeter * 0.15f;
// MAP ORIENTATION
qPainter.setPen( QColor( 0, 50, 140 ));
qPainter.drawLine( pixelsPerGeoTiffMeter / 2, pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter / 2,
2.0f * pixelsPerGeoTiffMeter );
qPainter.drawLine( pixelsPerGeoTiffMeter * 2 / 5, pixelsPerGeoTiffMeter - 1, pixelsPerGeoTiffMeter * 3 / 5,
pixelsPerGeoTiffMeter - 1 );
qPainter.drawLine( pixelsPerGeoTiffMeter * 2 / 5, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter * 3 / 5,
2 * pixelsPerGeoTiffMeter );
qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter,
2 * pixelsPerGeoTiffMeter );
qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset,
2 * pixelsPerGeoTiffMeter - arrowOffset );
qPainter.drawLine( pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter + arrowOffset,
2 * pixelsPerGeoTiffMeter + arrowOffset );
qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter,
2 * pixelsPerGeoTiffMeter );
qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter + arrowOffset,
pixelsPerGeoTiffMeter + arrowOffset );
qPainter.drawLine( 2 * pixelsPerGeoTiffMeter, pixelsPerGeoTiffMeter, 2 * pixelsPerGeoTiffMeter - arrowOffset,
pixelsPerGeoTiffMeter + arrowOffset );
qPainter.drawText( 0.6 * pixelsPerGeoTiffMeter, 1.6 * pixelsPerGeoTiffMeter, QString( "1m" ));
qPainter.drawText( 2.2 * pixelsPerGeoTiffMeter, 1.1 * pixelsPerGeoTiffMeter, QString( "x" ));
qPainter.drawText( 1.2 * pixelsPerGeoTiffMeter, 1.8 * pixelsPerGeoTiffMeter, QString( "y" ));
qPainter.drawText( 0.5f * pixelsPerGeoTiffMeter, 0.75f * pixelsPerGeoTiffMeter,
QString((map_file_name_ + ".tif").c_str()));
}
void GeotiffWriter::drawCross( QPainter &painter, const Eigen::Vector2f &coords )
{
painter.drawLine( QPointF( coords[0] - 1.0f, coords[1] ), QPointF( coords[0] + 1.0f, coords[1] ));
painter.drawLine( QPointF( coords[0], coords[1] - 1.0f ), QPointF( coords[0], coords[1] + 1.0f ));
}
void GeotiffWriter::drawArrow( QPainter &painter )
{
float tip_distance = pixelsPerGeoTiffMeter * 0.3f;
QPolygonF polygon;
polygon << QPointF( tip_distance, 0.0f ) << QPointF( -tip_distance * 0.5f, -tip_distance * 0.5f )
<< QPointF( 0.0f, 0.0f ) << QPointF( -tip_distance * 0.5f, tip_distance * 0.5f );
painter.save();
QBrush tmpBrush( QColor( 255, 200, 0 ));
QPen tmpPen( Qt::NoPen );
painter.setBrush( tmpBrush );
painter.setPen( tmpPen );
painter.drawPolygon( polygon );
painter.restore();
}
void GeotiffWriter::drawCoordSystem( QPainter &painter )
{
painter.save();
QPointF zero_point( 0.0f, 0.0f );
QPointF x_point( pixelsPerGeoTiffMeter, 0.0f );
QPointF y_point( 0.0f, pixelsPerGeoTiffMeter );
QPen tmp = painter.pen();
tmp.setWidth( 5 );
tmp.setColor( QColor( 255.0, 0.0, 0.0 ));
//painter.setPen(QPen::setWidth(5));
painter.setPen( tmp );
painter.drawLine( zero_point, x_point );
tmp.setColor( QColor( 0, 255, 0 ));
painter.setPen( tmp );
painter.drawLine( zero_point, y_point );
painter.restore();
}
}

15
Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,15 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_geotiff_launch
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
0.5.1 (2021-01-15)
------------------
0.5.0 (2020-12-17)
------------------
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Contributors: Stefan Fabian

15
Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/CMakeLists.txt Normal file
View File

@@ -0,0 +1,15 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_geotiff_launch)
find_package(catkin REQUIRED)
catkin_package()
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})

28
Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper.launch Normal file
View File

@@ -0,0 +1,28 @@
<?xml version="1.0"?>
<launch>
<arg name="trajectory_source_frame_name" default="/base_link"/>
<arg name="trajectory_update_rate" default="4"/>
<arg name="trajectory_publish_rate" default="0.25"/>
<arg name="map_file_path" default="$(find hector_geotiff)/maps"/>
<arg name="map_file_base_name" default="hector_slam_map"/>
<node pkg="hector_trajectory_server" type="hector_trajectory_server" name="hector_trajectory_server" output="screen">
<param name="target_frame_name" type="string" value="/map" />
<param name="source_frame_name" type="string" value="$(arg trajectory_source_frame_name)" />
<param name="trajectory_update_rate" type="double" value="$(arg trajectory_update_rate)" />
<param name="trajectory_publish_rate" type="double" value="$(arg trajectory_publish_rate)" />
</node>
<node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
<remap from="map" to="/dynamic_map" />
<param name="map_file_path" type="string" value="$(arg map_file_path)" />
<param name="map_file_base_name" type="string" value="$(arg map_file_base_name)" />
<param name="geotiff_save_period" type="double" value="0" />
<param name="draw_background_checkerboard" type="bool" value="true" />
<param name="draw_free_space_grid" type="bool" value="true" />
<param name="plugins" type="string" value="hector_geotiff_plugins/TrajectoryMapWriter" />
</node>
</launch>

19
Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/launch/geotiff_mapper_only.launch Normal file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0"?>
<launch>
<node pkg="hector_geotiff" type="geotiff_node" name="hector_geotiff_node" output="screen" launch-prefix="nice -n 15">
<remap from="map" to="scanmatcher_map" />
<param name="map_file_path" type="string" value="$(find hector_geotiff)/maps" />
<param name="map_file_base_name" type="string" value="" />
<param name="geotiff_save_period" type="double" value="45" />
<param name="plugins" type="string" value="hector_geotiff_plugins/TrajectoryMapWriter hector_worldmodel_geotiff_plugins/QRCodeMapWriter hector_worldmodel_geotiff_plugins/VictimMapWriter" />
<param name="VictimMapWriter/draw_all_objects" value="false" />
<param name="VictimMapWriter/class_id" value="victim" />
<param name="QRCodeMapWriter/draw_all_objects" value="true" />
<param name="QRCodeMapWriter/class_id" value="qrcode" />
</node>
</launch>

19
Localizations/Libraries/Ros/hector_slam/hector_geotiff_launch/package.xml Normal file
View File

@@ -0,0 +1,19 @@
<?xml version="1.0"?>
<package format="2">
<name>hector_geotiff_launch</name>
<version>0.5.2</version>
<description>Contains launch files for the hector_geotiff mapper.</description>
<maintainer email="fabian@sim.tu-darmstadt.de">Stefan Fabian</maintainer>
<license>BSD</license>
<url type="website">http://ros.org/wiki/hector_geotiff</url>
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>hector_geotiff</exec_depend>
<exec_depend>hector_geotiff_plugins</exec_depend>
<exec_depend>hector_trajectory_server</exec_depend>
</package>

51
Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,51 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_geotiff_plugins
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
0.5.1 (2021-01-15)
------------------
* Fixed "SEVERE WARNING" by pluginloader when killing geotiff node.
Some minor cleanup.
* Contributors: Stefan Fabian
0.5.0 (2020-12-17)
------------------
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Bump CMake version to avoid CMP0048 warning
* Contributors: Marius Schnaubelt, Stefan Fabian
0.4.1 (2020-05-15)
------------------
0.3.6 (2019-10-31)
------------------
0.3.5 (2016-06-24)
------------------
* hector_geotiff/hector_geotiff_plugins: added possibility to specify Color of robot path in the geotiff file in order to allow multiple color robot paths
* Contributors: Dorothea Koert
0.3.4 (2015-11-07)
------------------
0.3.3 (2014-06-15)
------------------
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-10-09)
------------------
* readded PLUGINLIB_DECLARE_CLASS macro for fuerte compatibility
* use hector_geotiff_plugins/TrajectoryMapWriter as legacy lookup name for the trajectory geotiff plugin to not break old launch files
* fixed warnings for deprecated pluginlib method/macro calls
* added changelogs
* added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them
0.3.0 (2013-08-08)
------------------
* catkinized hector_slam

44
Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/CMakeLists.txt Normal file
View File

@@ -0,0 +1,44 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_geotiff_plugins)
find_package(catkin REQUIRED COMPONENTS hector_geotiff hector_nav_msgs)
###################################
## catkin specific configuration ##
###################################
catkin_package(
CATKIN_DEPENDS hector_geotiff hector_nav_msgs
)
###########
## Build ##
###########
include_directories(
${catkin_INCLUDE_DIRS}
)
add_library(hector_geotiff_plugins src/trajectory_geotiff_plugin.cpp)
add_dependencies(hector_geotiff_plugins ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_geotiff_plugins
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
install(TARGETS hector_geotiff_plugins
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(FILES
hector_geotiff_plugins.xml
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

7
Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/hector_geotiff_plugins.xml Normal file
View File

@@ -0,0 +1,7 @@
<library path="lib/libhector_geotiff_plugins">
<class name="hector_geotiff_plugins/TrajectoryMapWriter" type="hector_geotiff_plugins::TrajectoryMapWriter" base_class_type="hector_geotiff::MapWriterPluginInterface">
<description>
This is a MapWriter plugin that draws the robot's trajectory in the map.
</description>
</class>
</library>

56
Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/package.xml Normal file
View File

@@ -0,0 +1,56 @@
<?xml version="1.0"?>
<package>
<name>hector_geotiff_plugins</name>
<version>0.5.2</version>
<description>
hector_geotiff_plugins contains plugins that extend geotiff maps generated by hector_geotiff.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</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>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<url type="website">http://ros.org/wiki/hector_geotiff_plugins</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<author>Gregor Gebhardt</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>hector_geotiff</build_depend>
<build_depend>hector_nav_msgs</build_depend>
<run_depend>hector_geotiff</run_depend>
<run_depend>hector_nav_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
<hector_geotiff plugin="${prefix}/hector_geotiff_plugins.xml" />
</export>
</package>

123
Localizations/Libraries/Ros/hector_slam/hector_geotiff_plugins/src/trajectory_geotiff_plugin.cpp Normal file
View File

@@ -0,0 +1,123 @@
//=================================================================================================
// Copyright (c) 2012, Gregor Gebhardt, TU Darmstadt
// 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 Flight Systems and Automatic Control group,
// TU Darmstadt, 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 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.
//=================================================================================================
#include <hector_geotiff/map_writer_interface.h>
#include <hector_geotiff/map_writer_plugin_interface.h>
#include <ros/ros.h>
#include <hector_nav_msgs/GetRobotTrajectory.h>
#include <fstream>
namespace hector_geotiff_plugins
{
using namespace hector_geotiff;
class TrajectoryMapWriter : public MapWriterPluginInterface
{
public:
TrajectoryMapWriter();
virtual ~TrajectoryMapWriter();
virtual void initialize(const std::string& name);
virtual void draw(MapWriterInterface *interface);
protected:
ros::NodeHandle nh_;
ros::ServiceClient service_client_;
bool initialized_;
std::string name_;
bool draw_all_objects_;
std::string class_id_;
int path_color_r_;
int path_color_g_;
int path_color_b_;
};
TrajectoryMapWriter::TrajectoryMapWriter()
: initialized_(false)
{}
TrajectoryMapWriter::~TrajectoryMapWriter()
{}
void TrajectoryMapWriter::initialize(const std::string& name)
{
ros::NodeHandle plugin_nh("~/" + name);
std::string service_name_;
plugin_nh.param("service_name", service_name_, std::string("trajectory"));
plugin_nh.param("path_color_r", path_color_r_, 120);
plugin_nh.param("path_color_g", path_color_g_, 0);
plugin_nh.param("path_color_b", path_color_b_, 240);
service_client_ = nh_.serviceClient<hector_nav_msgs::GetRobotTrajectory>(service_name_);
initialized_ = true;
this->name_ = name;
ROS_INFO_NAMED(name_, "Successfully initialized hector_geotiff MapWriter plugin %s.", name_.c_str());
}
void TrajectoryMapWriter::draw(MapWriterInterface *interface)
{
if(!initialized_) return;
hector_nav_msgs::GetRobotTrajectory srv_path;
if (!service_client_.call(srv_path)) {
ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str());
return;
}
std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);
size_t size = traj_vector.size();
std::vector<Eigen::Vector2f> pointVec;
pointVec.resize(size);
for (size_t i = 0; i < size; ++i){
const geometry_msgs::PoseStamped& pose (traj_vector[i]);
pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
}
if (size > 0){
//Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
interface->drawPath(startVec, pointVec, path_color_r_, path_color_g_, path_color_b_);
}
}
} // namespace
//register this planner as a MapWriterPluginInterface plugin
#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(hector_geotiff_plugins::TrajectoryMapWriter, hector_geotiff::MapWriterPluginInterface)

45
Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,45 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_imu_attitude_to_tf
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
0.5.1 (2021-01-15)
------------------
0.5.0 (2020-12-17)
------------------
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Bump CMake version to avoid CMP0048 warning
* Contributors: Marius Schnaubelt, Stefan Fabian
0.4.1 (2020-05-15)
------------------
0.3.6 (2019-10-31)
------------------
0.3.5 (2016-06-24)
------------------
0.3.4 (2015-11-07)
------------------
* hector_imu_attitude_to_tf: fixed default values of the base_frame and base_stabilized_frame parameters (fix #20)
* Contributors: Johannes Meyer
0.3.3 (2014-06-15)
------------------
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-10-09)
------------------
* added missing install rule for launch files
* added changelogs
0.3.0 (2013-08-08)
------------------
* catkinized hector_slam

32
Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/CMakeLists.txt Normal file
View File

@@ -0,0 +1,32 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_imu_attitude_to_tf)
find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs tf)
catkin_package(
CATKIN_DEPENDS roscpp sensor_msgs tf
)
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(imu_attitude_to_tf_node src/imu_attitude_to_tf_node.cpp)
target_link_libraries(imu_attitude_to_tf_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
install(TARGETS imu_attitude_to_tf_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

7
Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/launch/example.launch Normal file
View File

@@ -0,0 +1,7 @@
<launch>
<node pkg="hector_imu_attitude_to_tf" type="imu_attitude_to_tf_node" name="imu_attitude_to_tf_node" output="screen">
<remap from="imu_topic" to="thumper_imu" />
<param name="base_stabilized_frame" type="string" value="base_stabilized" />
<param name="base_frame" type="string" value="base_footprint" />
</node>
</launch>

55
Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/package.xml Normal file
View File

@@ -0,0 +1,55 @@
<?xml version="1.0"?>
<package format="2">
<name>hector_imu_attitude_to_tf</name>
<version>0.5.2</version>
<description>
hector_imu_attitude_to_tf is a lightweight node that can be used to publish the roll/pitch attitude angles reported via a imu message to tf.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</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>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">http://ros.org/wiki/hector_imu_attitude_to_tf</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>sensor_msgs</depend>
<depend>tf</depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

84
Localizations/Libraries/Ros/hector_slam/hector_imu_attitude_to_tf/src/imu_attitude_to_tf_node.cpp Normal file
View File

@@ -0,0 +1,84 @@
//=================================================================================================
// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include "sensor_msgs/Imu.h"
std::string p_base_stabilized_frame_;
std::string p_base_frame_;
tf::TransformBroadcaster* tfB_;
tf::StampedTransform transform_;
tf::Quaternion tmp_;
#ifndef TF_MATRIX3x3_H
typedef btScalar tfScalar;
namespace tf { typedef btMatrix3x3 Matrix3x3; }
#endif
void imuMsgCallback(const sensor_msgs::Imu& imu_msg)
{
tf::quaternionMsgToTF(imu_msg.orientation, tmp_);
tfScalar yaw, pitch, roll;
tf::Matrix3x3(tmp_).getRPY(roll, pitch, yaw);
tmp_.setRPY(roll, pitch, 0.0);
transform_.setRotation(tmp_);
transform_.stamp_ = imu_msg.header.stamp;
tfB_->sendTransform(transform_);
}
int main(int argc, char **argv) {
ros::init(argc, argv, ROS_PACKAGE_NAME);
ros::NodeHandle n;
ros::NodeHandle pn("~");
pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
pn.param("base_frame", p_base_frame_, std::string("base_link"));
tfB_ = new tf::TransformBroadcaster();
transform_.getOrigin().setX(0.0);
transform_.getOrigin().setY(0.0);
transform_.getOrigin().setZ(0.0);
transform_.frame_id_ = p_base_stabilized_frame_;
transform_.child_frame_id_ = p_base_frame_;
ros::Subscriber imu_subscriber = n.subscribe("imu_topic", 10, imuMsgCallback);
ros::spin();
delete tfB_;
return 0;
}

41
Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,41 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_imu_tools
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
0.5.1 (2021-01-15)
------------------
0.5.0 (2020-12-17)
------------------
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Bump CMake version to avoid CMP0048 warning
* Contributors: Marius Schnaubelt, Stefan Fabian
0.4.1 (2020-05-15)
------------------
0.3.6 (2019-10-31)
------------------
0.3.5 (2016-06-24)
------------------
0.3.4 (2015-11-07)
------------------
* Fix sim setup
* remap for bertl setup
* Contributors: Florian Kunz, kohlbrecher
0.3.3 (2014-06-15)
------------------
* hector_imu_tools: Basics of simple height etimation
* hector_imu_tools: Add tf publishers in hector_imu_tools
* hector_imu_tools: Also write out fake odometry
* hector_imu_tools: Fix typo
* hector_imu_tools: Prevent race conditions in slam, formatting
* hector_imu_tools: Small executable for generating a IMU message out of a (2d) pose and roll/pitch imu message
* Contributors: Stefan Kohlbrecher

39
Localizations/Libraries/Ros/hector_slam/hector_imu_tools/CMakeLists.txt Normal file
View File

@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_imu_tools)
find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs geometry_msgs nav_msgs tf)
catkin_package(
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp sensor_msgs tf
)
###########
## Build ##
###########
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(pose_and_orientation_to_imu_node src/pose_and_orientation_to_imu_node.cpp)
target_link_libraries(pose_and_orientation_to_imu_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
install(TARGETS pose_and_orientation_to_imu_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
install(DIRECTORY launch/
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch/
)

10
Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/bertl_robot.launch Normal file
View File

@@ -0,0 +1,10 @@
<?xml version="1.0"?>
<launch>
<node pkg="hector_imu_tools" type="pose_and_orientation_to_imu_node" name="pose_and_orientation_to_imu_node" output="screen">
<remap from="/imu" to="/imu_quat" />
<remap from="/fused_imu" to="/imu_in" />
<remap from="/pose" to="/slam_out_pose" />
<!--<remap from="/state" to="/state_imu" />-->
</node>
</launch>

9
Localizations/Libraries/Ros/hector_slam/hector_imu_tools/launch/jasmine_robot.launch Normal file
View File

@@ -0,0 +1,9 @@
<?xml version="1.0"?>
<launch>
<node pkg="hector_imu_tools" type="pose_and_orientation_to_imu_node" name="pose_and_orientation_to_imu_node" output="screen">
<remap from="/imu" to="/imu_quat" />
<remap from="/fused_imu" to="/imu_in" />
<remap from="/pose" to="/slam_out_pose" />
</node>
</launch>

62
Localizations/Libraries/Ros/hector_slam/hector_imu_tools/package.xml Normal file
View File

@@ -0,0 +1,62 @@
<?xml version="1.0"?>
<package>
<name>hector_imu_tools</name>
<version>0.5.2</version>
<description>
hector_imu_tools provides some tools for processing IMU messages
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</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>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">http://ros.org/wiki/hector_imu_attitude_to_tf</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>tf</build_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>sensor_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>tf</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>sensor_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

190
Localizations/Libraries/Ros/hector_slam/hector_imu_tools/src/pose_and_orientation_to_imu_node.cpp Normal file
View File

@@ -0,0 +1,190 @@
//=================================================================================================
// Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#include "ros/ros.h"
#include "tf/transform_broadcaster.h"
#include <sensor_msgs/Imu.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Odometry.h>
std::string p_map_frame_;
std::string p_base_footprint_frame_;
std::string p_base_stabilized_frame_;
std::string p_base_frame_;
tf::TransformBroadcaster* tfB_;
tf::StampedTransform transform_;
tf::Quaternion robot_pose_quaternion_;
tf::Point robot_pose_position_;
tf::Transform robot_pose_transform_;
tf::Quaternion tmp_;
tf::Quaternion orientation_quaternion_;
sensor_msgs::ImuConstPtr last_imu_msg_;
sensor_msgs::Imu fused_imu_msg_;
nav_msgs::Odometry odom_msg_;
geometry_msgs::PoseStampedConstPtr last_pose_msg_;
ros::Publisher fused_imu_publisher_;
ros::Publisher odometry_publisher_;
size_t callback_count_;
#ifndef TF_MATRIX3x3_H
typedef btScalar tfScalar;
namespace tf { typedef btMatrix3x3 Matrix3x3; }
#endif
void imuMsgCallback(const sensor_msgs::Imu::ConstPtr& imu_msg)
{
callback_count_++;
tf::quaternionMsgToTF(imu_msg->orientation, tmp_);
tfScalar imu_yaw, imu_pitch, imu_roll;
tf::Matrix3x3(tmp_).getRPY(imu_roll, imu_pitch, imu_yaw);
tf::Transform transform;
transform.setIdentity();
tf::Quaternion quat;
quat.setRPY(imu_roll, imu_pitch, 0.0);
if (true){
transform.setRotation(quat);
tfB_->sendTransform(tf::StampedTransform(transform, imu_msg->header.stamp, p_base_stabilized_frame_, p_base_frame_));
}
tfScalar pose_yaw, pose_pitch, pose_roll;
if (last_pose_msg_ != 0){
tf::quaternionMsgToTF(last_pose_msg_->pose.orientation, tmp_);
tf::Matrix3x3(tmp_).getRPY(pose_roll, pose_pitch, pose_yaw);
}else{
pose_yaw = 0.0;
}
orientation_quaternion_.setRPY(imu_roll, imu_pitch, pose_yaw);
fused_imu_msg_.header.stamp = imu_msg->header.stamp;
fused_imu_msg_.orientation.x = orientation_quaternion_.getX();
fused_imu_msg_.orientation.y = orientation_quaternion_.getY();
fused_imu_msg_.orientation.z = orientation_quaternion_.getZ();
fused_imu_msg_.orientation.w = orientation_quaternion_.getW();
fused_imu_publisher_.publish(fused_imu_msg_);
//If no pose message received, yaw is set to 0.
//@TODO: Check for timestamp of pose and disable sending if too old
if (last_pose_msg_ != 0){
if ( (callback_count_ % 5) == 0){
odom_msg_.header.stamp = imu_msg->header.stamp;
odom_msg_.pose.pose.orientation = fused_imu_msg_.orientation;
odom_msg_.pose.pose.position = last_pose_msg_->pose.position;
odometry_publisher_.publish(odom_msg_);
}
}
}
void poseMsgCallback(const geometry_msgs::PoseStamped::ConstPtr& pose_msg)
{
std::vector<tf::StampedTransform> transforms;
transforms.resize(2);
tf::quaternionMsgToTF(pose_msg->pose.orientation, robot_pose_quaternion_);
tf::pointMsgToTF(pose_msg->pose.position, robot_pose_position_);
robot_pose_transform_.setRotation(robot_pose_quaternion_);
robot_pose_transform_.setOrigin(robot_pose_position_);
tf::Transform height_transform;
height_transform.setIdentity();
height_transform.setOrigin(tf::Vector3(0.0, 0.0, 0.0));
transforms[0] = tf::StampedTransform(robot_pose_transform_, pose_msg->header.stamp, p_map_frame_, p_base_footprint_frame_);
transforms[1] = tf::StampedTransform(height_transform, pose_msg->header.stamp, p_base_footprint_frame_, p_base_stabilized_frame_);
tfB_->sendTransform(transforms);
// Perform simple estimation of vehicle altitude based on orientation
if (last_pose_msg_){
tf::Vector3 plane_normal = tf::Matrix3x3(orientation_quaternion_) * tf::Vector3(0.0, 0.0, 1.0);
tf::Vector3 last_position;
tf::pointMsgToTF(last_pose_msg_->pose.position, last_position);
double height_difference =
(-plane_normal.getX() * (robot_pose_position_.getX() - last_position.getX())
-plane_normal.getY() * (robot_pose_position_.getY() - last_position.getY())
+plane_normal.getZ() * last_position.getZ()) / last_position.getZ();
}
last_pose_msg_ = pose_msg;
}
int main(int argc, char **argv) {
ros::init(argc, argv, ROS_PACKAGE_NAME);
ros::NodeHandle n;
ros::NodeHandle pn("~");
pn.param("map_frame", p_map_frame_, std::string("map"));
pn.param("base_footprint_frame", p_base_footprint_frame_, std::string("base_footprint"));
pn.param("base_stabilized_frame", p_base_stabilized_frame_, std::string("base_stabilized"));
pn.param("base_frame", p_base_frame_, std::string("base_link"));
fused_imu_msg_.header.frame_id = p_base_stabilized_frame_;
odom_msg_.header.frame_id = "map";
tfB_ = new tf::TransformBroadcaster();
fused_imu_publisher_ = n.advertise<sensor_msgs::Imu>("/fused_imu",1,false);
odometry_publisher_ = n.advertise<nav_msgs::Odometry>("/state", 1, false);
ros::Subscriber imu_subscriber = n.subscribe("/imu", 10, imuMsgCallback);
ros::Subscriber pose_subscriber = n.subscribe("/pose", 10, poseMsgCallback);
callback_count_ = 0;
ros::spin();
delete tfB_;
return 0;
}

43
Localizations/Libraries/Ros/hector_slam/hector_map_server/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,43 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_map_server
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
0.5.1 (2021-01-15)
------------------
0.5.0 (2020-12-17)
------------------
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Bump CMake version to avoid CMP0048 warning
* Contributors: Marius Schnaubelt, Stefan Fabian
0.4.1 (2020-05-15)
------------------
0.3.6 (2019-10-31)
------------------
0.3.5 (2016-06-24)
------------------
0.3.4 (2015-11-07)
------------------
0.3.3 (2014-06-15)
------------------
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-10-09)
------------------
* added changelogs
* added cmake dependencies to catkin exported targets to force message targets to be built before any cpp target using them
0.3.0 (2013-08-08)
------------------
* catkinized hector_slam

35
Localizations/Libraries/Ros/hector_slam/hector_map_server/CMakeLists.txt Normal file
View File

@@ -0,0 +1,35 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_map_server)
find_package(catkin REQUIRED COMPONENTS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf)
catkin_package(
CATKIN_DEPENDS roscpp hector_map_tools hector_marker_drawing hector_nav_msgs nav_msgs tf
)
###########
## Build ##
###########
include_directories(
${catkin_INCLUDE_DIRS}
)
add_executable(hector_map_server src/hector_map_server.cpp)
add_dependencies(hector_map_server ${catkin_EXPORTED_TARGETS})
target_link_libraries(hector_map_server
${catkin_LIBRARIES}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
install(TARGETS hector_map_server
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

63
Localizations/Libraries/Ros/hector_slam/hector_map_server/package.xml Normal file
View File

@@ -0,0 +1,63 @@
<?xml version="1.0"?>
<package>
<name>hector_map_server</name>
<version>0.5.2</version>
<description>
hector_map_server provides a service for retrieving the map, as well as for raycasting based obstacle queries (finds next obstacle in the map, given start and endpoint
in any tf coordinate frame).
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</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>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<url type="website">http://ros.org/wiki/hector_map_server</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>hector_map_tools</build_depend>
<build_depend>hector_marker_drawing</build_depend>
<build_depend>hector_nav_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>tf</build_depend>
<run_depend>roscpp</run_depend>
<run_depend>hector_map_tools</run_depend>
<run_depend>hector_marker_drawing</run_depend>
<run_depend>hector_nav_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>tf</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

331
Localizations/Libraries/Ros/hector_slam/hector_map_server/src/hector_map_server.cpp Normal file
View File

@@ -0,0 +1,331 @@
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, Johannes Meyer TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#include <stdio.h>
#include <stdlib.h>
//#include <libgen.h>
//#include <fstream>
#include "ros/ros.h"
#include "ros/console.h"
#include "nav_msgs/MapMetaData.h"
#include "nav_msgs/OccupancyGrid.h"
#include <boost/thread.hpp>
#include <tf/transform_listener.h>
#include "nav_msgs/GetMap.h"
#include "hector_marker_drawing/HectorDrawings.h"
#include "hector_map_tools/HectorMapTools.h"
#include "hector_nav_msgs/GetDistanceToObstacle.h"
#include "hector_nav_msgs/GetSearchPosition.h"
class OccupancyGridContainer
{
public:
OccupancyGridContainer(std::string sub_topic, std::string prefix, ros::NodeHandle& nh, HectorDrawings* drawing_provider, tf::TransformListener* tf_)
: drawing_provider_(drawing_provider)
, tf_(tf_)
{
std::string service_name = "map";
map_service_ = nh.advertiseService(service_name, &OccupancyGridContainer::mapServiceCallback, this);
ros::NodeHandle pnh("~");
std::string lookup_service_name = "get_distance_to_obstacle";
dist_lookup_service_ = pnh.advertiseService(lookup_service_name, &OccupancyGridContainer::lookupServiceCallback, this);
std::string get_search_pos_service_name = "get_search_position";
get_search_pos_service_ = pnh.advertiseService(get_search_pos_service_name, &OccupancyGridContainer::getSearchPosServiceCallback, this);
map_sub_ = nh.subscribe("map", 1, &OccupancyGridContainer::mapCallback, this);
}
~OccupancyGridContainer()
{}
bool mapServiceCallback(nav_msgs::GetMap::Request &req,
nav_msgs::GetMap::Response &res )
{
ROS_INFO("hector_map_server map service called");
if (!map_ptr_){
ROS_INFO("map_server has no map yet, no map service available");
return false;
}
res.map = *map_ptr_;
return true;
}
bool lookupServiceCallback(hector_nav_msgs::GetDistanceToObstacle::Request &req,
hector_nav_msgs::GetDistanceToObstacle::Response &res )
{
//ROS_INFO("hector_map_server lookup service called");
if (!map_ptr_){
ROS_INFO("map_server has no map yet, no lookup service available");
return false;
}
tf::StampedTransform stamped_pose;
try{
tf_->waitForTransform(map_ptr_->header.frame_id,req.point.header.frame_id, req.point.header.stamp, ros::Duration(1.0));
tf_->lookupTransform(map_ptr_->header.frame_id, req.point.header.frame_id, req.point.header.stamp, stamped_pose);
tf::Point v2_tf;
tf::pointMsgToTF(req.point.point,v2_tf);
tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0);
tf::Vector3 v2 = stamped_pose * v2_tf;
tf::Vector3 diff = v2 - v1;
v2 = v1 + diff / tf::Vector3(diff.x(), diff.y(), 0.0).length() * 5.0f;
Eigen::Vector2f start(v1.x(),v1.y());
Eigen::Vector2f end(v2.x(),v2.y());
Eigen::Vector2f hit_world;
//float dist = dist_meas_.getDist(start,end, &hit_world);
float dist = dist_meas_.getDist(start,end,&hit_world);
if (dist >=0.0f){
tf::Vector3 diff (v2-v1);
float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f));
res.distance = dist/cos(angle);
}else{
res.distance = -1.0f;
}
//debug drawing
if (false){
float cube_scale = map_ptr_->info.resolution;
drawing_provider_->setColor(1.0, 0.0, 0.0);
drawing_provider_->setScale(static_cast<double>(cube_scale));
drawing_provider_->drawPoint(start);
drawing_provider_->setColor(0.0, 1.0, 0.0);
drawing_provider_->drawPoint(end);
if (dist >= 0.0f){
drawing_provider_->setColor(0.0, 0.0, 1.0);
drawing_provider_->drawPoint(hit_world);
}
drawing_provider_->sendAndResetData();
}
return true;
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform failed in lookup distance service call: %s",e.what());
}
return false;
}
bool getSearchPosServiceCallback(hector_nav_msgs::GetSearchPosition::Request &req,
hector_nav_msgs::GetSearchPosition::Response &res )
{
if (!map_ptr_){
ROS_INFO("map_server has no map yet, no get best search pos service available");
return false;
}
try{
tf::Stamped<tf::Pose> ooi_pose, transformed_pose, search_position;
tf::poseStampedMsgToTF(req.ooi_pose, ooi_pose);
tf_->waitForTransform(map_ptr_->header.frame_id, req.ooi_pose.header.frame_id, req.ooi_pose.header.stamp, ros::Duration(1.0));
tf_->transformPose(map_ptr_->header.frame_id, ooi_pose, transformed_pose);
tf::Vector3 direction(-req.distance, 0.0, 0.0);
search_position = transformed_pose;
search_position.setOrigin(transformed_pose.getOrigin() + transformed_pose.getBasis() * direction);
tf::poseStampedTFToMsg(search_position, res.search_pose);
return true;
// //tf::Point v2_tf;
// //tf::pointMsgToTF(req.point.point,v2_tf);
// tf::Vector3 v1 = stamped_pose * tf::Vector3(0.0, 0.0, 0.0);
// //warning: 3D!
// tf::Vector3 v2 = stamped_pose * tf::Vector3(-1.0, 0.0, 0.0);
// tf::Vector3 dir = v2-v1;
// Eigen::Vector2f dir_2d (dir.x(), dir.y());
// dir_2d.normalize();
// Eigen::Vector2f searchPos (Eigen::Vector2f(v1.x(),v1.y()) + (dir_2d*0.5f));
// //copy original pose message but set translation
// res.search_pose.pose = ooi_pose.pose;
// res.search_pose.pose.position.x = searchPos.x();
// res.search_pose.pose.position.y = searchPos.y();
//return true;
//Eigen::Vector2f ooi_pos(v1.x(),v1.y());
//Eigen::Vector2f sample_point_pos(v2.x(),v2.y());
//float dist_from_target = dist_meas_.getDist(ooi_pos,sample_point_pos);
//float dist_from_sample_point = dist_meas_.getDist(sample_point_pos, ooi_pos);
/*
if (dist >=0.0f){
tf::Vector3 diff (v2-v1);
float angle = diff.angle(tf::Vector3(diff.x(),diff.y(),0.0f));
res.distance = dist/cos(angle);
//debug drawing
if (true){
float cube_scale = map_ptr_->info.resolution;
drawing_provider_->setColor(1.0, 0.0, 0.0);
drawing_provider_->setScale(static_cast<double>(cube_scale));
drawing_provider_->drawPoint(start);
drawing_provider_->drawPoint(end);
if (dist >= 0.0f){
drawing_provider_->setColor(0.0, 0.0, 1.0);
drawing_provider_->drawPoint(hit_world);
}
drawing_provider_->sendAndResetData();
}
}
*/
//return true;
} catch(tf::TransformException e){
ROS_ERROR("Transform failed in getSearchPosition service call: %s",e.what());
}
return false;
}
void mapCallback(const nav_msgs::OccupancyGridConstPtr& map)
{
map_ptr_ = map;
dist_meas_.setMap(map_ptr_);
}
//Services
ros::ServiceServer map_service_;
ros::ServiceServer dist_lookup_service_;
ros::ServiceServer get_search_pos_service_;
//Subscriber
ros::Subscriber map_sub_;
HectorMapTools::DistanceMeasurementProvider dist_meas_;
HectorDrawings* drawing_provider_;
tf::TransformListener* tf_;
//nav_msgs::MapMetaData meta_data_message_;
nav_msgs::GetMap::Response map_resp_;
nav_msgs::OccupancyGridConstPtr map_ptr_;
};
class HectorMapServer
{
public:
/** Trivial constructor */
HectorMapServer(ros::NodeHandle& private_nh)
{
std::string frame_id;
hector_drawings_ = new HectorDrawings();
hector_drawings_->setNamespace("map_server");
mapContainer = new OccupancyGridContainer("map", "" ,private_nh, hector_drawings_,&tf_);
}
~HectorMapServer()
{
delete mapContainer;
delete hector_drawings_;
}
public:
OccupancyGridContainer* mapContainer;
private:
HectorDrawings* hector_drawings_;
tf::TransformListener tf_;
};
int main(int argc, char **argv)
{
ros::init(argc, argv, "hector_map_server");
ros::NodeHandle nh;
HectorMapServer ms(nh);
ros::spin();
return 0;
}

51
Localizations/Libraries/Ros/hector_slam/hector_map_tools/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,51 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package hector_map_tools
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
0.5.2 (2021-04-08)
------------------
0.5.1 (2021-01-15)
------------------
0.5.0 (2020-12-17)
------------------
* Moved hector_geotiff launch files to separate package to solve cyclic dependency.
Clean up for noetic release.
* Bump CMake version to avoid CMP0048 warning
* Contributors: Marius Schnaubelt, Stefan Fabian
0.4.1 (2020-05-15)
------------------
0.3.6 (2019-10-31)
------------------
* hector_map_tools: Use the FindEigen3.cmake module provided by Eigen
This patch applies the recommendation from http://wiki.ros.org/jade/Migration and removes the
dependency from package cmake_modules (unless your installation of Eigen3 does not provide a
cmake config).
Same as 1251d9dc20854f48da116eed25780c103a5bd003, but package hector_map_tools was not updated
back then.
* Contributors: Johannes Meyer
0.3.5 (2016-06-24)
------------------
0.3.4 (2015-11-07)
------------------
* -Fix severe bug when not using square size maps (would results in completely wrong obstacle distances and coordinates)
* Contributors: Stefan Kohlbrecher
0.3.3 (2014-06-15)
------------------
0.3.2 (2014-03-30)
------------------
0.3.1 (2013-10-09)
------------------
* added changelogs
0.3.0 (2013-08-08)
------------------
* catkinized hector_slam

25
Localizations/Libraries/Ros/hector_slam/hector_map_tools/CMakeLists.txt Normal file
View File

@@ -0,0 +1,25 @@
cmake_minimum_required(VERSION 3.0.2)
project(hector_map_tools)
find_package(catkin REQUIRED COMPONENTS nav_msgs)
find_package(Eigen3 REQUIRED)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS nav_msgs
DEPENDS EIGEN3
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)

293
Localizations/Libraries/Ros/hector_slam/hector_map_tools/include/hector_map_tools/HectorMapTools.h Normal file
View File

@@ -0,0 +1,293 @@
//=================================================================================================
// Copyright (c) 2011, Stefan Kohlbrecher, TU Darmstadt
// 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 Simulation, Systems Optimization and Robotics
// group, TU Darmstadt 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 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.
//=================================================================================================
#ifndef __HectorMapTools_h_
#define __HectorMapTools_h_
#include <nav_msgs/OccupancyGrid.h>
#include <nav_msgs/MapMetaData.h>
#include<Eigen/Core>
class HectorMapTools{
public:
template<typename ConcreteScalar>
class CoordinateTransformer{
public:
CoordinateTransformer()
{
}
CoordinateTransformer(const nav_msgs::OccupancyGridConstPtr map)
{
this->setTransforms(*map);
}
void setTransforms(const nav_msgs::OccupancyGrid& map)
{
this->setTransforms(map.info);
}
void setTransforms(const nav_msgs::MapMetaData& meta)
{
origo_ = (Eigen::Matrix<ConcreteScalar, 2, 1>(static_cast<ConcreteScalar>(meta.origin.position.x),static_cast<ConcreteScalar>(meta.origin.position.y)));
scale_ = (static_cast<ConcreteScalar>(meta.resolution));
inv_scale_ = (static_cast<ConcreteScalar>(1.0f / meta.resolution));
}
void setTransformsBetweenCoordSystems(const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS1, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS1,
const Eigen::Matrix<ConcreteScalar, 2, 1>& origoCS2, const Eigen::Matrix<ConcreteScalar, 2, 1>& endCS2)
{
Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_x_factors = getLinearTransform(Eigen::Vector2f(origoCS1[0], endCS1[0]), Eigen::Vector2f(origoCS2[0], endCS2[0]));
Eigen::Matrix<ConcreteScalar, 2, 1> map_t_geotiff_y_factors = getLinearTransform(Eigen::Vector2f(origoCS1[1], endCS1[1]), Eigen::Vector2f(origoCS2[1], endCS2[1]));
//std::cout << "\n geo_x: \n" << map_t_geotiff_x_factors << "\n geo_y: \n" << map_t_geotiff_y_factors << "\n";
origo_.x() = map_t_geotiff_x_factors[1];
origo_.y() = map_t_geotiff_y_factors[1];
scale_ = map_t_geotiff_x_factors[0];
inv_scale_ = 1.0 / scale_;
//std::cout << "\nscale " << scale_ << "\n";
}
Eigen::Matrix<ConcreteScalar, 2, 1> getC1Coords(const Eigen::Matrix<ConcreteScalar, 2, 1>& mapCoords) const
{
return origo_ + (mapCoords * scale_);
}
Eigen::Matrix<ConcreteScalar, 2, 1> getC2Coords(const Eigen::Matrix<ConcreteScalar, 2, 1>& worldCoords) const
{
return ((worldCoords - origo_) * inv_scale_);
}
ConcreteScalar getC1Scale(float c2_scale) const
{
return scale_ * c2_scale;
}
ConcreteScalar getC2Scale(float c1_scale) const
{
return inv_scale_ * c1_scale;
}
protected:
Eigen::Matrix<ConcreteScalar, 2, 1> getLinearTransform(const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem1, const Eigen::Matrix<ConcreteScalar, 2, 1>& coordSystem2)
{
ConcreteScalar scaling = (coordSystem2[0] - coordSystem2[1]) / (coordSystem1[0] - coordSystem1[1]);
ConcreteScalar translation = coordSystem2[0] - coordSystem1[0] * scaling;
return Eigen::Vector2f (scaling, translation);
}
Eigen::Matrix<ConcreteScalar, 2, 1> origo_;
ConcreteScalar scale_;
ConcreteScalar inv_scale_;
};
class DistanceMeasurementProvider
{
public:
DistanceMeasurementProvider()
{
}
void setMap(const nav_msgs::OccupancyGridConstPtr map)
{
map_ptr_ = map;
world_map_transformer_.setTransforms(*map_ptr_);
}
float getDist(const Eigen::Vector2f& begin_world, const Eigen::Vector2f& end_world, Eigen::Vector2f* hitCoords = 0)
{
Eigen::Vector2i end_point_map;
Eigen::Vector2i begin_map (world_map_transformer_.getC2Coords(begin_world).cast<int>());
Eigen::Vector2i end_map (world_map_transformer_.getC2Coords(end_world).cast<int>());
float dist = checkOccupancyBresenhami(begin_map, end_map, &end_point_map);
if (hitCoords != 0){
*hitCoords = world_map_transformer_.getC1Coords(end_point_map.cast<float>());
}
return world_map_transformer_.getC1Scale(dist);
}
inline float checkOccupancyBresenhami( const Eigen::Vector2i& beginMap, const Eigen::Vector2i& endMap, Eigen::Vector2i* hitCoords = 0, unsigned int max_length = UINT_MAX){
int x0 = beginMap[0];
int y0 = beginMap[1];
int sizeX = map_ptr_->info.width;
int sizeY = map_ptr_->info.height;
//check if beam start point is inside map, cancel update if this is not the case
if ((x0 < 0) || (x0 >= sizeX) || (y0 < 0) || (y0 >= sizeY)) {
return -1.0f;
}
int x1 = endMap[0];
int y1 = endMap[1];
//std::cout << " x: "<< x1 << " y: " << y1 << " length: " << length << " ";
//check if beam end point is inside map, cancel update if this is not the case
if ((x1 < 0) || (x1 >= sizeX) || (y1 < 0) || (y1 >= sizeY)) {
return -1.0f;
}
int dx = x1 - x0;
int dy = y1 - y0;
unsigned int abs_dx = abs(dx);
unsigned int abs_dy = abs(dy);
int offset_dx = dx > 0 ? 1 : -1;
int offset_dy = (dy > 0 ? 1 : -1) * sizeX;
unsigned int startOffset = beginMap.y() * sizeX + beginMap.x();
int end_offset;
//if x is dominant
if(abs_dx >= abs_dy){
int error_y = abs_dx / 2;
end_offset = bresenham2D(abs_dx, abs_dy, error_y, offset_dx, offset_dy, startOffset,5000);
}else{
//otherwise y is dominant
int error_x = abs_dy / 2;
end_offset = bresenham2D(abs_dy, abs_dx, error_x, offset_dy, offset_dx, startOffset,5000);
}
if (end_offset != -1){
Eigen::Vector2i end_coords(end_offset % sizeX, end_offset / sizeX);
int distMap = ((beginMap - end_coords).cast<float>()).norm();
if (hitCoords != 0){
*hitCoords = end_coords;
}
return distMap;
}
return -1.0f;
//unsigned int endOffset = endMap.y() * sizeX + endMap.x();
//this->bresenhamCellOcc(endOffset);
}
inline int bresenham2D(unsigned int abs_da, unsigned int abs_db, int error_b, int offset_a, int offset_b,
unsigned int offset, unsigned int max_length){
unsigned int end = std::min(max_length, abs_da);
const std::vector<int8_t>& data = map_ptr_->data;
for(unsigned int i = 0; i < end; ++i){
if (data[offset] == 100){
return static_cast<int>(offset);
}
offset += offset_a;
error_b += abs_db;
if((unsigned int)error_b >= abs_da){
offset += offset_b;
error_b -= abs_da;
}
}
return -1;
//at(offset);
}
protected:
CoordinateTransformer<float> world_map_transformer_;
nav_msgs::OccupancyGridConstPtr map_ptr_;
};
static bool getMapExtends(const nav_msgs::OccupancyGrid& map, Eigen::Vector2i& topLeft, Eigen::Vector2i& bottomRight)
{
int lowerStart = -1;
int upperStart = 10000000;
int xMaxTemp = lowerStart;
int yMaxTemp = lowerStart;
int xMinTemp = upperStart;
int yMinTemp = upperStart;
int width = map.info.width;
int height = map.info.height;
for (int y = 0; y < height; ++y){
for (int x = 0; x < width; ++x){
if ( map.data[x+y*width] != -1){
if (x > xMaxTemp) {
xMaxTemp = x;
}
if (x < xMinTemp) {
xMinTemp = x;
}
if (y > yMaxTemp) {
yMaxTemp = y;
}
if (y < yMinTemp) {
yMinTemp = y;
}
}
}
}
if ((xMaxTemp != lowerStart) &&
(yMaxTemp != lowerStart) &&
(xMinTemp != upperStart) &&
(yMinTemp != upperStart)) {
topLeft = Eigen::Vector2i(xMinTemp,yMinTemp);
bottomRight = Eigen::Vector2i(xMaxTemp+1, yMaxTemp+1);
return true;
} else {
return false;
}
};
};
#endif

55
Localizations/Libraries/Ros/hector_slam/hector_map_tools/package.xml Normal file
View File

@@ -0,0 +1,55 @@
<?xml version="1.0"?>
<package>
<name>hector_map_tools</name>
<version>0.5.2</version>
<description>
hector_map_tools contains some functions related to accessing information from OccupancyGridMap maps.
Currently consists of a single header.
</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</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>BSD</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<url type="website">http://ros.org/wiki/hector_map_tools</url>
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>eigen</build_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>eigen</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- You can specify that this package is a metapackage here: -->
<!-- <metapackage/> -->
<!-- Other tools can request additional information be placed here -->
</export>
</package>

Some files were not shown because too many files have changed in this diff Show More