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

created

Browse Source
This commit is contained in:
HiepLM
2025-12-05 11:12:17 +07:00
parent f60cbc2ed3
commit 45d965671e
196 changed files with 41817 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

414
src/Navigations/Cores/move_base_core/.gitignore vendored Executable file
View File

@@ -0,0 +1,414 @@
# ---> VisualStudio
## Ignore Visual Studio temporary files, build results, and
## files generated by popular Visual Studio add-ons.
##
## Get latest from https://github.com/github/gitignore/blob/main/VisualStudio.gitignore
# User-specific files
*.rsuser
*.suo
*.user
*.userosscache
*.sln.docstates
# User-specific files (MonoDevelop/Xamarin Studio)
*.userprefs
# Mono auto generated files
mono_crash.*
# Build results
[Dd]ebug/
[Dd]ebugPublic/
[Rr]elease/
[Rr]eleases/
x64/
x86/
[Ww][Ii][Nn]32/
[Aa][Rr][Mm]/
[Aa][Rr][Mm]64/
bld/
[Bb]in/
[Oo]bj/
[Ll]og/
[Ll]ogs/
# Visual Studio 2015/2017 cache/options directory
.vs/
# Uncomment if you have tasks that create the project's static files in wwwroot
#wwwroot/
# Visual Studio 2017 auto generated files
Generated\ Files/
# MSTest test Results
[Tt]est[Rr]esult*/
[Bb]uild[Ll]og.*
# NUnit
*.VisualState.xml
TestResult.xml
nunit-*.xml
# Build Results of an ATL Project
[Dd]ebugPS/
[Rr]eleasePS/
dlldata.c
# Benchmark Results
BenchmarkDotNet.Artifacts/
# .NET Core
project.lock.json
project.fragment.lock.json
artifacts/
# ASP.NET Scaffolding
ScaffoldingReadMe.txt
# StyleCop
StyleCopReport.xml
# Files built by Visual Studio
*_i.c
*_p.c
*_h.h
*.ilk
*.meta
*.obj
*.iobj
*.pch
*.pdb
*.ipdb
*.pgc
*.pgd
*.rsp
*.sbr
*.tlb
*.tli
*.tlh
*.tmp
*.tmp_proj
*_wpftmp.csproj
*.log
*.tlog
*.vspscc
*.vssscc
.builds
*.pidb
*.svclog
*.scc
# Chutzpah Test files
_Chutzpah*
# Visual C++ cache files
ipch/
*.aps
*.ncb
*.opendb
*.opensdf
*.sdf
*.cachefile
*.VC.db
*.VC.VC.opendb
# Visual Studio profiler
*.psess
*.vsp
*.vspx
*.sap
# Visual Studio Trace Files
*.e2e
# TFS 2012 Local Workspace
$tf/
# Guidance Automation Toolkit
*.gpState
# ReSharper is a .NET coding add-in
_ReSharper*/
*.[Rr]e[Ss]harper
*.DotSettings.user
# TeamCity is a build add-in
_TeamCity*
# DotCover is a Code Coverage Tool
*.dotCover
# AxoCover is a Code Coverage Tool
.axoCover/*
!.axoCover/settings.json
# Coverlet is a free, cross platform Code Coverage Tool
coverage*.json
coverage*.xml
coverage*.info
# Visual Studio code coverage results
*.coverage
*.coveragexml
# NCrunch
_NCrunch_*
.*crunch*.local.xml
nCrunchTemp_*
# MightyMoose
*.mm.*
AutoTest.Net/
# Web workbench (sass)
.sass-cache/
# Installshield output folder
[Ee]xpress/
# DocProject is a documentation generator add-in
DocProject/buildhelp/
DocProject/Help/*.HxT
DocProject/Help/*.HxC
DocProject/Help/*.hhc
DocProject/Help/*.hhk
DocProject/Help/*.hhp
DocProject/Help/Html2
DocProject/Help/html
# Click-Once directory
publish/
# Publish Web Output
*.[Pp]ublish.xml
*.azurePubxml
# Note: Comment the next line if you want to checkin your web deploy settings,
# but database connection strings (with potential passwords) will be unencrypted
*.pubxml
*.publishproj
# Microsoft Azure Web App publish settings. Comment the next line if you want to
# checkin your Azure Web App publish settings, but sensitive information contained
# in these scripts will be unencrypted
PublishScripts/
# NuGet Packages
*.nupkg
# NuGet Symbol Packages
*.snupkg
# The packages folder can be ignored because of Package Restore
**/[Pp]ackages/*
# except build/, which is used as an MSBuild target.
!**/[Pp]ackages/build/
# Uncomment if necessary however generally it will be regenerated when needed
#!**/[Pp]ackages/repositories.config
# NuGet v3's project.json files produces more ignorable files
*.nuget.props
*.nuget.targets
# Microsoft Azure Build Output
csx/
*.build.csdef
# Microsoft Azure Emulator
ecf/
rcf/
# Windows Store app package directories and files
AppPackages/
BundleArtifacts/
Package.StoreAssociation.xml
_pkginfo.txt
*.appx
*.appxbundle
*.appxupload
# Visual Studio cache files
# files ending in .cache can be ignored
*.[Cc]ache
# but keep track of directories ending in .cache
!?*.[Cc]ache/
# Others
ClientBin/
~$*
*~
*.dbmdl
*.dbproj.schemaview
*.jfm
*.pfx
*.publishsettings
orleans.codegen.cs
# Including strong name files can present a security risk
# (https://github.com/github/gitignore/pull/2483#issue-259490424)
#*.snk
# Since there are multiple workflows, uncomment next line to ignore bower_components
# (https://github.com/github/gitignore/pull/1529#issuecomment-104372622)
#bower_components/
# RIA/Silverlight projects
Generated_Code/
# Backup & report files from converting an old project file
# to a newer Visual Studio version. Backup files are not needed,
# because we have git ;-)
_UpgradeReport_Files/
Backup*/
UpgradeLog*.XML
UpgradeLog*.htm
ServiceFabricBackup/
*.rptproj.bak
# SQL Server files
*.mdf
*.ldf
*.ndf
# Business Intelligence projects
*.rdl.data
*.bim.layout
*.bim_*.settings
*.rptproj.rsuser
*- [Bb]ackup.rdl
*- [Bb]ackup ([0-9]).rdl
*- [Bb]ackup ([0-9][0-9]).rdl
# Microsoft Fakes
FakesAssemblies/
# GhostDoc plugin setting file
*.GhostDoc.xml
# Node.js Tools for Visual Studio
.ntvs_analysis.dat
node_modules/
# Visual Studio 6 build log
*.plg
# Visual Studio 6 workspace options file
*.opt
# Visual Studio 6 auto-generated workspace file (contains which files were open etc.)
*.vbw
# Visual Studio 6 auto-generated project file (contains which files were open etc.)
*.vbp
# Visual Studio 6 workspace and project file (working project files containing files to include in project)
*.dsw
*.dsp
# Visual Studio 6 technical files
*.ncb
*.aps
# Visual Studio LightSwitch build output
**/*.HTMLClient/GeneratedArtifacts
**/*.DesktopClient/GeneratedArtifacts
**/*.DesktopClient/ModelManifest.xml
**/*.Server/GeneratedArtifacts
**/*.Server/ModelManifest.xml
_Pvt_Extensions
# Paket dependency manager
.paket/paket.exe
paket-files/
# FAKE - F# Make
.fake/
# CodeRush personal settings
.cr/personal
# Python Tools for Visual Studio (PTVS)
__pycache__/
*.pyc
# Cake - Uncomment if you are using it
# tools/**
# !tools/packages.config
# Tabs Studio
*.tss
# Telerik's JustMock configuration file
*.jmconfig
# BizTalk build output
*.btp.cs
*.btm.cs
*.odx.cs
*.xsd.cs
# OpenCover UI analysis results
OpenCover/
# Azure Stream Analytics local run output
ASALocalRun/
# MSBuild Binary and Structured Log
*.binlog
# NVidia Nsight GPU debugger configuration file
*.nvuser
# MFractors (Xamarin productivity tool) working folder
.mfractor/
# Local History for Visual Studio
.localhistory/
# Visual Studio History (VSHistory) files
.vshistory/
# BeatPulse healthcheck temp database
healthchecksdb
# Backup folder for Package Reference Convert tool in Visual Studio 2017
MigrationBackup/
# Ionide (cross platform F# VS Code tools) working folder
.ionide/
# Fody - auto-generated XML schema
FodyWeavers.xsd
# VS Code files for those working on multiple tools
.vscode/*
!.vscode/settings.json
!.vscode/tasks.json
!.vscode/launch.json
!.vscode/extensions.json
*.code-workspace
# Local History for Visual Studio Code
.history/
# Windows Installer files from build outputs
*.cab
*.msi
*.msix
*.msm
*.msp
# JetBrains Rider
*.sln.iml
# ---> VisualStudioCode
.vscode/*
!.vscode/settings.json
!.vscode/tasks.json
!.vscode/launch.json
!.vscode/extensions.json
!.vscode/*.code-snippets
# Local History for Visual Studio Code
.history/
# Built Visual Studio Code Extensions
*.vsix

60
src/Navigations/Cores/move_base_core/CMakeLists.txt Normal file
View File

@@ -0,0 +1,60 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(move_base_core VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Cho phép các project khác include được header của move_base_core
set(MOVE_BASE_CORE_INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/include
PARENT_SCOPE
)
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tìm tất cả header files
file(GLOB HEADERS "include/move_base_core/*.h")
# Tạo INTERFACE library (header-only)
add_library(move_base_core INTERFACE)
target_link_libraries(move_base_core INTERFACE tf3 robot_time geometry_msgs)
# Set include directories
target_include_directories(move_base_core
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/move_base_core
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Cài đặt target
install(TARGETS move_base_core
EXPORT move_base_core-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT move_base_core-targets
FILE move_base_core-targets.cmake
DESTINATION lib/cmake/move_base_core)
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "=================================")

2
src/Navigations/Cores/move_base_core/README.md Executable file
View File

@@ -0,0 +1,2 @@
# move_base_core

16
src/Navigations/Cores/move_base_core/include/move_base_core/common.h Normal file
View File

@@ -0,0 +1,16 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2025, Locus Robotics
* All rights reserved.
*
*/
#ifndef _MOVE_BASE_CORE_COMMON_H_INCLUDED_
#define _MOVE_BASE_CORE_COMMON_H_INCLUDED_
#include <tf3/buffer_core.h>
#include <memory>
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
#endif // NAV_CORE2_COMMON_H

282
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h Executable file
View File

@@ -0,0 +1,282 @@
#ifndef _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_
#define _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_
#include <memory>
// geometry msgs
#include <geometry_msgs/Twist.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose2D.h>
#include <geometry_msgs/Vector3.h>
// tf
#include <tf3/buffer_core.h>
#include <tf3/utils.h>
#include <tf3/convert.h>
#include <robot/time.h>
#include <move_base_core/common.h>
namespace move_base_core
{
// Navigation states, including planning and controller status
enum class State
{
PENDING,
ACTIVE,
PREEMPTED,
SUCCEEDED,
ABORTED,
REJECTED,
PREEMPTING,
RECALLING,
RECALLED,
LOST,
PLANNING,
CONTROLLING,
CLEARING,
PAUSED
};
// Feedback structure to describe current navigation status
struct NavFeedback
{
State navigation_state; // Current navigation state
std::string feed_back_str; // Description or debug message
geometry_msgs::Pose2D current_pose; // Robot’s current pose in 2D
bool goal_checked; // Whether the current goal is verified
bool is_ready; // Robot is ready for commands
};
/**
* @brief Convert a State enum to its string representation.
* Useful for debugging/logging or displaying in UI.
*
* @param state Enum value of move_base_core::State
* @return std::string The corresponding string, e.g. "PENDING"
*/
inline std::string toString(move_base_core::State state)
{
using move_base_core::State;
switch (state)
{
case State::PENDING:
return "PENDING"; // Chờ xử lý
case State::ACTIVE:
return "ACTIVE"; // Đang hoạt động
case State::PREEMPTED:
return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới
case State::SUCCEEDED:
return "SUCCEEDED"; // Thành công
case State::ABORTED:
return "ABORTED"; // Bị lỗi
case State::REJECTED:
return "REJECTED"; // Từ chối bởi planner hoặc controller
case State::PREEMPTING:
return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu
case State::RECALLING:
return "RECALLING"; // Đang huỷ bỏ nội bộ
case State::RECALLED:
return "RECALLED"; // Đã được thu hồi
case State::LOST:
return "LOST"; // Mất trạng thái
case State::PLANNING:
return "PLANNING"; // Đang lập kế hoạch đường đi
case State::CONTROLLING:
return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan
case State::CLEARING:
return "CLEARING"; // Đang dọn dẹp bản đồ / costmap
case State::PAUSED:
return "PAUSED"; // Tạm dừng
default:
return "UNKNOWN_STATE"; // Không xác định
}
}
/**
* @brief Creates a target pose (PoseStamped) by offsetting a given 2D pose along its heading direction.
*
* This function calculates a new position by moving forward (or backward if negative)
* a certain `offset_distance` from the current position, following the given heading angle (theta).
*
* @param pose The original 2D pose (x, y, theta) in the local plane.
* @param frame_id The coordinate frame in which the output PoseStamped will be expressed.
* @param offset_distance The distance to offset along the heading direction, in meters.
* Positive moves forward, negative moves backward.
* @return A new PoseStamped offset from the input pose, in the given frame.
*/
inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::Pose2D &pose, const std::string &frame_id, double offset_distance)
{
geometry_msgs::PoseStamped goal;
// goal.header.frame_id = frame_id;
// goal.header.stamp = robot::Time::now();
// goal.pose.position.x = pose.x + offset_distance * cos(pose.theta);
// goal.pose.position.y = pose.y + offset_distance * sin(pose.theta);
// tf3::Quaternion q;
// q.setRPY(0, 0, pose.theta);
// goal.pose.orientation = tf3::convert(q);
return goal;
}
/**
* @brief Overloaded version: creates an offset target pose from a given PoseStamped.
*
* This function extracts the 2D position and orientation (yaw) from the given PoseStamped,
* offsets it forward (or backward) along the current heading direction,
* and returns a new PoseStamped in the same frame.
*
* @param pose The original pose with full position and orientation.
* @param offset_distance Distance to offset along the current heading direction (in meters).
* @return A new PoseStamped offset from the original pose.
*/
inline geometry_msgs::PoseStamped offset_goal(const geometry_msgs::PoseStamped &pose, double offset_distance)
{
geometry_msgs::Pose2D pose2d;
pose2d.x = pose.pose.position.x;
pose2d.y = pose.pose.position.y;
// pose2d.theta = tf2::getYaw(pose.pose.orientation);
return offset_goal(pose2d, pose.header.frame_id, offset_distance);
}
/**
* @class BaseNavigation
* @brief Abstract interface for robot navigation modules.
*
* Provides core methods for setting goals, moving, rotating, and handling motion control.
* All navigation logic must implement this interface.
*/
class BaseNavigation
{
public:
using Ptr = std::shared_ptr<BaseNavigation>;
virtual ~BaseNavigation() {}
/**
* @brief Initialize the navigation system.
* @param tf Shared pointer to the TF listener or buffer.
*/
virtual void initialize(TFListenerPtr tf) = 0;
/**
* @brief Set the robot's footprint (outline shape) in the global frame.
* This can be used for planning or collision checking.
*
* @param fprt A vector of points representing the robot's footprint polygon.
* The points should be ordered counter-clockwise.
* Example:
*
^ Y
|
| P3(-0.3, 0.2) P2(0.3, 0.2)
| ●---------------●
| | |
| | Robot | (view Top )
| | |
| ●---------------●
| P4(-0.3, -0.2) P1(0.3, -0.2)
+-------------------------------> X
std::vector<geometry_msgs::Point> footprint;
1. footprint.push_back(make_point(0.3, -0.2));
2. footprint.push_back(make_point(0.3, 0.2));
3. footprint.push_back(make_point(-0.3, 0.2));
4. footprint.push_back(make_point(-0.3, -0.2));
*/
virtual void setRobotFootprint(const std::vector<geometry_msgs::Point> &fprt) = 0;
/**
* @brief Send a goal for the robot to navigate to.
* @param goal Target pose in the global frame.
* @param xy_goal_tolerance Acceptable error in X/Y (meters).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if goal was accepted and sent successfully.
*/
virtual bool moveTo(const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Send a docking goal to a predefined marker (e.g. charger).
* @param maker Marker name or ID.
* @param goal Target pose for docking.
* @param xy_goal_tolerance Acceptable XY error (meters).
* @param yaw_goal_tolerance Acceptable heading error (radians).
* @return True if docking command succeeded.
*/
virtual bool dockTo(const std::string &maker,
const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Move straight toward the target position (X-axis).
* @param goal Target pose.
* @param xy_goal_tolerance Acceptable positional error (meters).
* @return True if command issued successfully.
*/
virtual bool moveStraightTo(const geometry_msgs::PoseStamped &goal,
double xy_goal_tolerance = 0.0) = 0;
/**
* @brief Rotate in place to align with target orientation.
* @param goal Pose containing desired heading (only Z-axis used).
* @param yaw_goal_tolerance Acceptable angular error (radians).
* @return True if rotation command was sent successfully.
*/
virtual bool rotateTo(const geometry_msgs::PoseStamped &goal,
double yaw_goal_tolerance = 0.0) = 0;
/**
* @brief Pause the robot's movement (e.g. during obstacle avoidance).
*/
virtual void pause() = 0;
/**
* @brief Resume motion after a pause.
*/
virtual void resume() = 0;
/**
* @brief Cancel the current goal and stop the robot.
*/
virtual void cancel() = 0;
/**
* @brief Send limited linear velocity command.
* @param linear Linear velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistLinear(const geometry_msgs::Vector3 &linear) = 0;
/**
* @brief Send limited angular velocity command.
* @param angular Angular velocity vector (x, y, z).
* @return True if the command was accepted.
*/
virtual bool setTwistAngular(const geometry_msgs::Vector3 &angular) = 0;
/**
* @brief Get the robot’s pose as a PoseStamped.
* @param pose Output parameter with the robot’s current pose.
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose(geometry_msgs::PoseStamped &pose) = 0;
/**
* @brief Get the robot’s pose as a 2D pose.
* @param pose Output parameter with the robot’s current 2D pose.
* @return True if pose was successfully retrieved.
*/
virtual bool getRobotPose(geometry_msgs::Pose2D &pose) = 0;
// Shared feedback data for navigation status tracking
std::shared_ptr<NavFeedback> nav_feedback_;
protected:
BaseNavigation() = default;
};
} // namespace move_base_core
#endif // _MOVE_BASE_CORE_NAVIGATION_H_INCLUDED_

112
src/Navigations/Cores/nav_core/CHANGELOG.rst Executable file
View File

@@ -0,0 +1,112 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package nav_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.17.3 (2023-01-10)
-------------------
1.17.2 (2022-06-20)
-------------------
1.17.1 (2020-08-27)
-------------------
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
* increase required cmake version
* Contributors: Michael Ferguson
1.16.6 (2020-03-18)
-------------------
1.16.5 (2020-03-15)
-------------------
1.16.4 (2020-03-04)
-------------------
1.16.3 (2019-11-15)
-------------------
1.16.2 (2018-07-31)
-------------------
1.16.1 (2018-07-28)
-------------------
1.16.0 (2018-07-25)
-------------------
* Switch to TF2 `#755 <https://github.com/ros-planning/navigation/issues/755>`_
* unify parameter names between base_local_planner and dwa_local_planner
addresses parts of `#90 <https://github.com/ros-planning/navigation/issues/90>`_
* fix param names of RotateRecovery, closes `#706 <https://github.com/ros-planning/navigation/issues/706>`_
* Contributors: David V. Lu, Michael Ferguson, Vincent Rabaud
1.15.2 (2018-03-22)
-------------------
* Merge pull request `#673 <https://github.com/ros-planning/navigation/issues/673>`_ from ros-planning/email_update_lunar
update maintainer email (lunar)
* 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, Michael Ferguson
1.15.1 (2017-08-14)
-------------------
1.15.0 (2017-08-07)
-------------------
* convert packages to format2
* makePlan overload must return.
* Contributors: Eric Tappan, Mikael Arguedas
1.14.0 (2016-05-20)
-------------------
1.13.1 (2015-10-29)
-------------------
* Merge pull request `#338 <https://github.com/ros-planning/navigation/issues/338>`_ from mikeferguson/planner_review
* fix doxygen, couple style fixes
* Contributors: Michael Ferguson
1.13.0 (2015-03-17)
-------------------
* adding makePlan function with returned cost to base_global_planner
* Contributors: phil0stine
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)
--------------------
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)
-------------------
1.11.4 (2013-09-27)
-------------------
* Package URL Updates

56
src/Navigations/Cores/nav_core/CMakeLists.txt Normal file
View File

@@ -0,0 +1,56 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(nav_core VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Cho phép các project khác include được header của nav_core
set(nav_core_INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/include
PARENT_SCOPE
)
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tạo INTERFACE library (header-only)
add_library(nav_core INTERFACE)
target_link_libraries(nav_core INTERFACE costmap_2d tf3)
# Set include directories
target_include_directories(nav_core
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/nav_core
DESTINATION include
FILES_MATCHING PATTERN "*.h")
install(TARGETS nav_core
EXPORT nav_core-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT nav_core-targets
FILE nav_core-targets.cmake
DESTINATION lib/cmake/nav_core)
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "=================================")

96
src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h Executable file
View File

@@ -0,0 +1,96 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV_CORE_BASE_GLOBAL_PLANNER_H
#define NAV_CORE_BASE_GLOBAL_PLANNER_H
#include <geometry_msgs/PoseStamped.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <memory>
namespace nav_core {
/**
* @class BaseGlobalPlanner
* @brief Provides an interface for global planners used in navigation. All global planners written as plugins for the navigation stack must adhere to this interface.
*/
class BaseGlobalPlanner{
public:
using Ptr = std::shared_ptr<BaseGlobalPlanner>;
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
* @param goal The goal pose
* @param plan The plan... filled by the planner
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan) = 0;
/**
* @brief Given a goal pose in the world, compute a plan
* @param start The start pose
* @param goal The goal pose
* @param plan The plan... filled by the planner
* @param cost The plans calculated cost
* @return True if a valid plan was found, false otherwise
*/
virtual bool makePlan(const geometry_msgs::PoseStamped& start,
const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan,
double& cost)
{
cost = 0;
return makePlan(start, goal, plan);
}
/**
* @brief Initialization function for the BaseGlobalPlanner
* @param name The name of this planner
* @param costmap_robot A pointer to the wrapper of the costmap to use for planning
*/
virtual bool initialize(std::string name, costmap_2d::Costmap2DROBOT* costmap_robot) = 0;
/**
* @brief Virtual destructor for the interface
*/
virtual ~BaseGlobalPlanner(){}
protected:
BaseGlobalPlanner(){}
};
} // namespace nav_core
#endif // NAV_CORE_BASE_GLOBAL_PLANNER_H

153
src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h Executable file
View File

@@ -0,0 +1,153 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2008, Willow Garage, Inc.
* 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 Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV_CORE_BASE_LOCAL_PLANNER_H
#define NAV_CORE_BASE_LOCAL_PLANNER_H
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Twist.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <memory>
namespace nav_core
{
/**
* @class BaseLocalPlanner
* @brief Provides an interface for local planners used in navigation. All local planners written as plugins for the navigation stack must adhere to this interface.
*/
class BaseLocalPlanner
{
public:
using Ptr = std::shared_ptr<BaseLocalPlanner>;
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid velocity command was found, false otherwise
*/
virtual bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) = 0;
/**
* @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
* @param planner_name The object name
* @return True if instance object is successed, False otherwise
*/
virtual bool swapPlanner(std::string planner_name) { return false; }
/**
* @brief Set velocity for x, y asix of the robot
* @param linear the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return false; }
/**
* @brief Get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; }
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Get velocity for x, y asix of the robot
* @return The current velocity
*/
virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
/**
* @brief Check if the kinematic parameters are currently locked
* @return True if the kinematic parameters are locked, false otherwise
*
* This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL.
*/
virtual bool islock() {throw std::runtime_error("Function islock is not Support.");}
/**
* @brief Lock the kinematic parameters to prevent modifications
*/
virtual void lock() {throw std::runtime_error("Function lock is not Support.");}
/**
* @brief Unlock the kinematic parameters to allow modifications
*/
virtual void unlock() {throw std::runtime_error("Function unlock is not Support.");}
/**
* @brief Check if the goal pose has been achieved by the local planner
* @return True if achieved, false otherwise
*/
virtual bool isGoalReached() = 0;
/**
* @brief Set the plan that the local planner is following
* @param plan The plan to pass to the local planner
* @return True if the plan was updated successfully, false otherwise
*/
virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;
/**
* @brief Constructs the local planner
* @param name The name to give this instance of the local planner
* @param tf A pointer to a transform listener
* @param costmap_ros The cost map to use for assigning costs to local plans
*/
virtual void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) = 0;
/**
* @brief Virtual destructor for the interface
*/
virtual ~BaseLocalPlanner() {}
protected:
BaseLocalPlanner() {}
};
} // namespace nav_core
#endif // NAV_CORE_BASE_LOCAL_PLANNER_H

86
src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h Executable file
View File

@@ -0,0 +1,86 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, Open Source Robotics Foundation
* 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 OSRF nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: David Lu
*********************************************************************/
#ifndef NAV_CORE_PARAMETER_MAGIC_H
#define NAV_CORE_PARAMETER_MAGIC_H
namespace nav_core
{
/**
* @brief Load a parameter from one of two namespaces. Complain if it uses the old name.
* @param nh NodeHandle to look for the parameter in
* @param current_name Parameter name that is current, i.e. not deprecated
* @param old_name Deprecated parameter name
* @param default_value If neither parameter is present, return this value
* @return The value of the parameter or the default value
*/
template<class param_t>
param_t loadParameterWithDeprecation(const std::string current_name,
const std::string old_name, const param_t& default_value)
{
param_t value;
if (nh.hasParam(current_name))
{
nh.getParam(current_name, value);
return value;
}
if (nh.hasParam(old_name))
{
ROS_WARN("Parameter %s is deprecated. Please use the name %s instead.", old_name.c_str(), current_name.c_str());
nh.getParam(old_name, value);
return value;
}
return default_value;
}
/**
* @brief Warn if a parameter exists under a deprecated (and unsupported) name.
*
* Parameters loaded exclusively through dynamic reconfigure can't really use loadParamWithDeprecation.
*/
void warnRenamedParameter(const std::string current_name, const std::string old_name)
{
if (nh.hasParam(old_name))
{
ROS_WARN("Parameter %s is deprecated (and will not load properly). Use %s instead.", old_name.c_str(), current_name.c_str());
}
}
} // namespace nav_core
#endif // NAV_CORE_PARAMETER_MAGIC_H

77
src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h Executable file
View File

@@ -0,0 +1,77 @@
/*********************************************************************
*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* 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 Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
#define NAV_CORE_RECOVERY_BEHAVIOR_H
#include <costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <memory>
namespace nav_core {
/**
* @class RecoveryBehavior
* @brief Provides an interface for recovery behaviors used in navigation. All recovery behaviors written as plugins for the navigation stack must adhere to this interface.
*/
class RecoveryBehavior{
public:
using Ptr = std::shared_ptr<RecoveryBehavior>;
/**
* @brief Initialization function for the RecoveryBehavior
* @param tf A pointer to a transform listener
* @param global_costmap A pointer to the global_costmap used by the navigation stack
* @param local_costmap A pointer to the local_costmap used by the navigation stack
*/
virtual void initialize(std::string name, tf3::BufferCore* tf, costmap_2d::Costmap2DROBOT* global_costmap, costmap_2d::Costmap2DROBOT* local_costmap) = 0;
/**
* @brief Runs the RecoveryBehavior
*/
virtual void runBehavior() = 0;
/**
* @brief Virtual destructor for the interface
*/
virtual ~RecoveryBehavior(){}
protected:
RecoveryBehavior(){}
};
} // namespace nav_core
#endif // NAV_CORE_RECOVERY_BEHAVIOR_H

64
src/Navigations/Cores/nav_core2/CMakeLists.txt Executable file
View File

@@ -0,0 +1,64 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(nav_core2 VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# Cho phép các project khác include được header của nav_core2
set(nav_core2_INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/include
PARENT_SCOPE
)
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tìm tất cả header files
file(GLOB HEADERS "include/nav_core2/*.h")
# Tạo INTERFACE library (header-only)
add_library(nav_core2 INTERFACE)
target_link_libraries(nav_core2 INTERFACE
costmap_2d
tf3
nav_grid
nav_2d_msgs
)
# Set include directories
target_include_directories(nav_core2
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/nav_core2
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Cài đặt target
install(TARGETS nav_core2
EXPORT nav_core2-targets)
# Export targets
install(EXPORT nav_core2-targets
FILE nav_core2-targets.cmake
NAMESPACE nav_core2::
DESTINATION lib/cmake/nav_core2)
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "=================================")

52
src/Navigations/Cores/nav_core2/README.md Executable file
View File

@@ -0,0 +1,52 @@
# nav_core2
A replacement interface for [nav_core](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core) that defines basic two dimensional planner interfaces.
There were a few key reasons for creating new interfaces rather than extending the old ones.
* Use `nav_2d_msgs` to eliminate unused data fields
* Use a new `Costmap` interface as a plugin rather that forcing all implementations of the interfaces to use `costmap_2d::Costmap2DROS`.
* Provide more data in the interfaces for easier testing.
* Use Exceptions rather than booleans to provide more information about the types of errors encountered.
## `Costmap`
`costmap_2d::Costmap2DROS` has been a vital part of the navigation stack for years, but it was not without its flaws.
* Initialization required a transform be available from the global frame to the base frame, which was later used to move rolling costmaps around (and a few other things). This made doing simple testing of any planner or other `Costmap2DROS`-based behavior annoying, because transforms had to be set up, often outside of the immediate code that was being tested.
* Initialization also started an update thread, which is also not always needed in testing.
* Using `Costmap2DROS` locked users into a layered costmap based approach, which made some tasks much easier, but didn't give users the freedom to change the implementation.
The [`nav_core2::Costmap`](include/nav_core2/costmap.h) interface extends the `nav_grid::NavGrid<unsigned char>` for abstracting away the data storage and coordinate manipulation, and provides a few other key methods for costmap functioning such as
* a mutex
* a way to potentially track changes to the costmap
* a public `update` method that can be called in whatever thread you please
The `Costmap` can be loaded using `pluginlib`, allowing for arbitrary implementations of underlying update algorithms, include the layered costmap approach.
One basic implementation is provided in [`BasicCostmap`](include/nav_core2/basic_costmap.h). You can also use the `nav_core_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROS`.
Note: One could also imagine the `Costmap` interface being templatized itself like `NavGrid` instead of forcing use of `unsigned char`. While this may be possible, it was decided that it was a bit ambitious and the use of templates would force all of the implementation into headers, and would ultimately obfuscate the operation of algorithms.
## Global Planner
Let us compare the old [nav_core::BaseGlobalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_global_planner.h) to the new [nav_core2/GlobalPlanner](include/nav_core2/global_planner.h).
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers, and provides a TFListener|
|`bool makePlan(const geometry_msgs::PoseStamped&, const geometry_msgs::PoseStamped&, std::vector<geometry_msgs::PoseStamped>&)`|`nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Pose2DStamped&)`|Uses exceptions for errors instead of returning a bool, which frees up the return for the actual path.|
## Local Planner
Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/nav_core/include/nav_core/base_local_planner.h) to the new [nav_core2/LocalPlanner](include/nav_core2/local_planner.h).
| `nav_core` | `nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, tf::TransformListener*, costmap_2d::Costmap2DROS*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|(no equivalent)|`void setGoalPose(const nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
|`bool setPlan(const std::vector<geometry_msgs::PoseStamped>&)`|`setPlan(const nav_2d_msgs::Path2D&)`||
|`bool computeVelocityCommands(geometry_msgs::Twist&)`|`nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)`|Explicitly provides the current pose and velocity for more explicit data control and easier testing. Uses exceptions for errors instead of returning a bool, which frees up the return for the actual command.|
|`bool isGoalReached()` | `bool isGoalReached(const nav_2d_msgs::Pose2DStamped&, const nav_2d_msgs::Twist2D&)` | Explicitly provide the current pose and velocity for more explicit data control and easier testing. |
## Exceptions
A hierarchical collection of [exceptions](include/nav_core2/exceptions.h) is provided to allow for reacting to navigation failures in a more robust and contextual way.
![exception hierarchy tree](doc/exceptions.png)
Each exception has a corresponding integer "result code" that can be used in ROS interfaces where passing the C++ object is infeasible. Note that due to the hierarchy, the result_code will be for the child-most exception. For example, if you throw a `StartBoundsException` which has a corresponding result code of `6`, it could also be seen as a `InvalidStartPoseException`, `GlobalPlannerException`, `PlannerException` or `NavCore2Exception`, all of which would also have the result code of `6`.
## Bounds
For use in tracking `Costmap` changes and more, this package also provides an implementation of [bounding boxes](include/nav_core2/bounds.h). These are represented with the ranges `[min_x, max_x]` and `[min_y, max_y]` (inclusive).

BIN
src/Navigations/Cores/nav_core2/doc/exceptions.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 38 KiB

68
src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h Executable file
View File

@@ -0,0 +1,68 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_CORE2_BASIC_COSTMAP_H
#define NAV_CORE2_BASIC_COSTMAP_H
#include <nav_core2/costmap.h>
#include <string>
#include <vector>
namespace nav_core2
{
class BasicCostmap : public nav_core2::Costmap
{
public:
// Standard Costmap Interface
mutex_t* getMutex() override { return &my_mutex_; }
// NavGrid Interface
void reset() override;
void setValue(const unsigned int x, const unsigned int y, const unsigned char& value) override;
unsigned char getValue(const unsigned int x, const unsigned int y) const override;
void setInfo(const nav_grid::NavGridInfo& new_info) override
{
info_ = new_info;
reset();
}
// Index Conversion
unsigned int getIndex(const unsigned int x, const unsigned int y) const;
protected:
mutex_t my_mutex_;
std::vector<unsigned char> data_;
};
} // namespace nav_core2
#endif // NAV_CORE2_BASIC_COSTMAP_H

209
src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h Executable file
View File

@@ -0,0 +1,209 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_CORE2_BOUNDS_H
#define NAV_CORE2_BOUNDS_H
#include <algorithm>
#include <limits>
#include <string>
namespace nav_core2
{
/**
* @brief Templatized method for checking if a value falls inside a one-dimensional range
* @param value The value to check
* @param min_value The minimum value of the range
* @param max_value The maximum value of the range
* @return True if the value is within the range
*/
template <typename NumericType>
inline bool inRange(const NumericType value, const NumericType min_value, const NumericType max_value)
{
return min_value <= value && value <= max_value;
}
/**
* @class GenericBounds
* @brief Templatized class that represents a two dimensional bounds with ranges [min_x, max_x] [min_y, max_y] inclusive
*/
template <typename NumericType>
struct GenericBounds
{
public:
/**
* @brief Constructor for an empty bounds
*/
GenericBounds()
{
reset();
}
/**
* @brief Constructor for a non-empty initial bounds
* @param x0 Initial min x
* @param y0 Initial min y
* @param x1 Initial max x
* @param y1 Initial max y
*/
GenericBounds(NumericType x0, NumericType y0, NumericType x1, NumericType y1)
: min_x_(x0), min_y_(y0), max_x_(x1), max_y_(y1) {}
/**
* @brief Reset the bounds to be empty
*/
void reset()
{
min_x_ = min_y_ = std::numeric_limits<NumericType>::max();
max_x_ = max_y_ = std::numeric_limits<NumericType>::lowest(); // -max
}
/**
* @brief Update the bounds to include the point (x, y)
*/
void touch(NumericType x, NumericType y)
{
min_x_ = std::min(x, min_x_);
min_y_ = std::min(y, min_y_);
max_x_ = std::max(x, max_x_);
max_y_ = std::max(y, max_y_);
}
/**
* @brief Update the bounds to include points (x0, y0) and (x1, y1)
* @param x0 smaller of two x values
* @param y0 smaller of two y values
* @param x1 larger of two x values
* @param y1 larger of two y values
*/
void update(NumericType x0, NumericType y0, NumericType x1, NumericType y1)
{
min_x_ = std::min(x0, min_x_);
min_y_ = std::min(y0, min_y_);
max_x_ = std::max(x1, max_x_);
max_y_ = std::max(y1, max_y_);
}
/**
* @brief Update the bounds to include the entirety of another bounds object
* @param other Another bounds object
*/
void merge(const GenericBounds<NumericType>& other)
{
update(other.min_x_, other.min_y_, other.max_x_, other.max_y_);
}
/**
* @brief Returns true if the range is empty
*/
bool isEmpty() const
{
return min_x_ > max_x_ && min_y_ > max_y_;
}
/**
* @brief Returns true if the point is inside this range
*/
bool contains(NumericType x, NumericType y) const
{
return inRange(x, min_x_, max_x_) && inRange(y, min_y_, max_y_);
}
/**
* @brief returns true if the two bounds overlap each other
*/
bool overlaps(const GenericBounds<NumericType>& other) const
{
return !isEmpty() && !other.isEmpty()
&& max_y_ >= other.min_y_ && min_y_ <= other.max_y_
&& max_x_ >= other.min_x_ && min_x_ <= other.max_x_;
}
/**
* @brief comparison operator that requires all fields are equal
*/
bool operator==(const GenericBounds<NumericType>& other) const
{
return min_x_ == other.min_x_ && min_y_ == other.min_y_ &&
max_x_ == other.max_x_ && max_y_ == other.max_y_;
}
bool operator!=(const GenericBounds<NumericType>& other) const
{
return !operator==(other);
}
/**
* @brief Returns a string representation of the bounds
*/
std::string toString() const
{
if (!isEmpty())
{
return "(" + std::to_string(min_x_) + "," + std::to_string(min_y_) + "):(" +
std::to_string(max_x_) + "," + std::to_string(max_y_) + ")";
}
else
{
return "(empty bounds)";
}
}
NumericType getMinX() const { return min_x_; }
NumericType getMinY() const { return min_y_; }
NumericType getMaxX() const { return max_x_; }
NumericType getMaxY() const { return max_y_; }
protected:
NumericType min_x_, min_y_, max_x_, max_y_;
};
using Bounds = GenericBounds<double>;
inline unsigned int getDimension(unsigned int min_v, unsigned int max_v)
{
return (min_v > max_v) ? 0 : max_v - min_v + 1;
}
class UIntBounds : public GenericBounds<unsigned int>
{
public:
using GenericBounds<unsigned int>::GenericBounds;
unsigned int getWidth() const { return getDimension(min_x_, max_x_); }
unsigned int getHeight() const { return getDimension(min_y_, max_y_); }
};
} // namespace nav_core2
#endif // NAV_CORE2_BOUNDS_H

42
src/Navigations/Cores/nav_core2/include/nav_core2/common.h Executable 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 NAV_CORE2_COMMON_H
#define NAV_CORE2_COMMON_H
#include <tf3/buffer_core.h>
#include <memory>
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
#endif // NAV_CORE2_COMMON_H

157
src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h Executable file
View File

@@ -0,0 +1,157 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_CORE2_COSTMAP_H
#define NAV_CORE2_COSTMAP_H
#include <nav_grid/nav_grid.h>
#include <nav_core2/common.h>
#include <nav_core2/bounds.h>
#include <boost/thread.hpp>
#include <memory>
#include <string>
#include <yaml-cpp/yaml.h>
namespace nav_core2
{
class Costmap : public nav_grid::NavGrid<unsigned char>
{
public:
static const unsigned char NO_INFORMATION = 255;
static const unsigned char LETHAL_OBSTACLE = 254;
static const unsigned char INSCRIBED_INFLATED_OBSTACLE = 253;
static const unsigned char FREE_SPACE = 0;
using Ptr = std::shared_ptr<Costmap>;
/**
* @brief Virtual Destructor
*/
virtual ~Costmap() {}
/**
* @brief Initialization function for the Costmap
*
* ROS parameters/topics are expected to be in the parent/name namespace.
* It is suggested that all NodeHandles in the costmap use the parent NodeHandle's callback queue.
*
* @param parent NodeHandle to derive other NodeHandles from
* @param name The namespace for the costmap
* @param tf A pointer to a transform listener
*/
virtual void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) {}
inline unsigned char getCost(const unsigned int x, const unsigned int y)
{
return getValue(x, y);
}
inline unsigned char getCost(const nav_grid::Index& index)
{
return getValue(index.x, index.y);
}
inline void setCost(const unsigned int x, const unsigned int y, const unsigned char cost)
{
setValue(x, y, cost);
}
inline void setCost(const nav_grid::Index& index, const unsigned char cost)
{
setValue(index, cost);
}
/**
* @brief Update the values in the costmap
*
* Note that this method can throw CostmapExceptions to indicate problems, like when it would be unsafe to navigate.
* e.g. If the costmap expects laser data at a given rate, but laser data hasn't shown up in a while, this method
* might throw a CostmapDataLagException.
*/
virtual void update() {}
using mutex_t = boost::recursive_mutex;
/**
* @brief Accessor for boost mutex
*/
virtual mutex_t* getMutex() = 0;
/**
* @brief Flag to indicate whether this costmap is able to track how much has changed
*/
virtual bool canTrackChanges() { return false; }
/**
* @brief If canTrackChanges, get the bounding box for how much of the costmap has changed
*
* Rather than querying based on time stamps (which can require an arbitrary amount of storage)
* we instead query based on a namespace. The return bounding box reports how much of the costmap
* has changed since the last time this method was called with a particular namespace. If a namespace
* is new, then it returns a bounding box for the whole costmap. The max values are inclusive.
*
* Example Sequence with a 5x5 costmap: (results listed (min_x,min_y):(max_x, max_y))
* 0) getChangeBounds("A") --> (0,0):(4,4)
* 1) getChangeBounds("B") --> (0,0):(4,4)
* 2) update cell 1, 1
* 3) getChangeBounds("C") --> (0,0):(4,4)
* 4) getChangeBounds("A") --> (1,1):(1,1)
* 5) getChangeBounds("A") --> (empty bounds) (i.e. nothing was updated since last call)
* 6) updateCell 2, 4
* 7) getChangeBounds("A") --> (2,4):(2,4)
* 8) getChangeBounds("B") --> (1,1):(2,4)
*
* @param ns The namespace
* @return Updated bounds
* @throws std::runtime_error If canTrackChanges is false, the returned bounds would be meaningless
*/
virtual UIntBounds getChangeBounds(const std::string& ns)
{
if (!canTrackChanges())
{
throw std::runtime_error("You called 'getChangeBounds()' on a derived Costmap type that is not capable of "
"tracking changes (i.e. canTrackChanges() returns false). You shouldn't do that.");
}
else
{
throw std::runtime_error("You called 'getChangeBounds()' on a derived Costmap type that is capable of tracking "
"changes but has not properly implemented this function. You should yell at the author "
"of the derived Costmap.");
}
return UIntBounds();
}
};
} // namespace nav_core2
#endif // NAV_CORE2_COSTMAP_H

321
src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h Executable file
View File

@@ -0,0 +1,321 @@
/*
* 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 NAV_CORE2_EXCEPTIONS_H
#define NAV_CORE2_EXCEPTIONS_H
#include <nav_2d_msgs/Pose2DStamped.h>
#include <stdexcept>
#include <exception>
#include <string>
#include <memory>
/**************************************************
* The nav_core2 Planning Exception Hierarchy!!
* (with arbitrary integer result codes)
**************************************************
* NavCore2Exception
* 0 CostmapException
* 1 CostmapSafetyException
* 2 CostmapDataLagException
* 3 PlannerException
* 4 GlobalPlannerException
* 5 InvalidStartPoseException
* 6 StartBoundsException
* 7 OccupiedStartException
* 8 InvalidGoalPoseException
* 9 GoalBoundsException
* 10 OccupiedGoalException
* 11 NoGlobalPathException
* 12 GlobalPlannerTimeoutException
* 13 LocalPlannerException
* 14 IllegalTrajectoryException
* 15 NoLegalTrajectoriesException
* 16 PlannerTFException
*
* -1 Unknown
**************************************************/
namespace nav_core2
{
inline std::string poseToString(const nav_2d_msgs::Pose2DStamped& pose)
{
return "(" + std::to_string(pose.pose.x) + ", " + std::to_string(pose.pose.y) + ", " + std::to_string(pose.pose.theta)
+ " : " + pose.header.frame_id + ")";
}
class NavCore2Exception: public std::runtime_error
{
public:
explicit NavCore2Exception(const std::string& description, int result_code)
: std::runtime_error(description), result_code_(result_code) {}
int getResultCode() const { return result_code_; }
protected:
int result_code_;
};
using NavCore2ExceptionPtr = std::exception_ptr;
/**
* @brief Handy function for getting the result code
*/
inline int getResultCode(const NavCore2ExceptionPtr& e_ptr)
{
if (e_ptr == nullptr)
{
return -1;
}
try
{
std::rethrow_exception(e_ptr);
}
catch (const NavCore2Exception& e)
{
return e.getResultCode();
}
catch (...)
{
// Will end up here if current_exception returned a non-NavCore2Exception
return -1;
}
}
/**
* @class CostmapException
* @brief Extensible exception class for all costmap-related problems
*/
class CostmapException: public NavCore2Exception
{
public:
explicit CostmapException(const std::string& description, int result_code = 0)
: NavCore2Exception(description, result_code) {}
};
/**
* @class CostmapSafetyException
* @brief General container for exceptions thrown when the costmap thinks any movement would be unsafe
*/
class CostmapSafetyException: public CostmapException
{
public:
explicit CostmapSafetyException(const std::string& description, int result_code = 1)
: CostmapException(description, result_code) {}
};
/**
* @class CostmapDataLagException
* @brief Indicates costmap is out of date because data in not up to date.
*
* Functions similarly to `!Costmap2DROS::isCurrent()`
*/
class CostmapDataLagException: public CostmapSafetyException
{
public:
explicit CostmapDataLagException(const std::string& description, int result_code = 2)
: CostmapSafetyException(description, result_code) {}
};
/**
* @class PlannerException
* @brief Parent type of all exceptions defined within
*/
class PlannerException: public NavCore2Exception
{
public:
explicit PlannerException(const std::string& description, int result_code = 3)
: NavCore2Exception(description, result_code) {}
};
/**
* @class GlobalPlannerException
* @brief General container for exceptions thrown from the Global Planner
*/
class GlobalPlannerException: public PlannerException
{
public:
explicit GlobalPlannerException(const std::string& description, int result_code = 4)
: PlannerException(description, result_code) {}
};
/**
* @class LocalPlannerException
* @brief General container for exceptions thrown from the Local Planner
*/
class LocalPlannerException: public PlannerException
{
public:
explicit LocalPlannerException(const std::string& description, int result_code = 13)
: PlannerException(description, result_code) {}
};
/**
* @class PlannerTFException
* @brief Thrown when either the global or local planner cannot complete its operation due to TF errors
*/
class PlannerTFException: public PlannerException
{
public:
explicit PlannerTFException(const std::string& description, int result_code = 16)
: PlannerException(description, result_code) {}
};
/**
* @class InvalidStartPoseException
* @brief Exception thrown when there is a problem at the start location for the global planner
*/
class InvalidStartPoseException: public GlobalPlannerException
{
public:
explicit InvalidStartPoseException(const std::string& description, int result_code = 5)
: GlobalPlannerException(description, result_code) {}
InvalidStartPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 5) :
InvalidStartPoseException("The starting pose " + poseToString(pose) + " is " + problem, result_code) {}
};
/**
* @class StartBoundsException
* @brief Exception thrown when the start location of the global planner is out of the expected bounds
*/
class StartBoundsException: public InvalidStartPoseException
{
public:
explicit StartBoundsException(const std::string& description, int result_code = 6)
: InvalidStartPoseException(description, result_code) {}
explicit StartBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidStartPoseException(pose, "out of bounds", 6) {}
};
/**
* @class OccupiedStartException
* @brief Exception thrown when the start location of the global planner is occupied in the costmap
*/
class OccupiedStartException: public InvalidStartPoseException
{
public:
explicit OccupiedStartException(const std::string& description, int result_code = 7)
: InvalidStartPoseException(description, result_code) {}
explicit OccupiedStartException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidStartPoseException(pose, "occupied", 7) {}
};
/**
* @class InvalidGoalPoseException
* @brief Exception thrown when there is a problem at the goal location for the global planner
*/
class InvalidGoalPoseException: public GlobalPlannerException
{
public:
explicit InvalidGoalPoseException(const std::string& description, int result_code = 8)
: GlobalPlannerException(description, result_code) {}
InvalidGoalPoseException(const nav_2d_msgs::Pose2DStamped& pose, const std::string& problem, int result_code = 8) :
GlobalPlannerException("The goal pose " + poseToString(pose) + " is " + problem, result_code) {}
};
/**
* @class GoalBoundsException
* @brief Exception thrown when the goal location of the global planner is out of the expected bounds
*/
class GoalBoundsException: public InvalidGoalPoseException
{
public:
explicit GoalBoundsException(const std::string& description, int result_code = 9)
: InvalidGoalPoseException(description, result_code) {}
explicit GoalBoundsException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidGoalPoseException(pose, "out of bounds", 9) {}
};
/**
* @class OccupiedGoalException
* @brief Exception thrown when the goal location of the global planner is occupied in the costmap
*/
class OccupiedGoalException: public InvalidGoalPoseException
{
public:
explicit OccupiedGoalException(const std::string& description, int result_code = 10)
: InvalidGoalPoseException(description, result_code) {}
explicit OccupiedGoalException(const nav_2d_msgs::Pose2DStamped& pose) :
InvalidGoalPoseException(pose, "occupied", 10) {}
};
/**
* @class NoGlobalPathException
* @brief Exception thrown when the global planner cannot find a path from the start to the goal
*/
class NoGlobalPathException: public GlobalPlannerException
{
public:
explicit NoGlobalPathException(const std::string& description, int result_code = 11)
: GlobalPlannerException(description, result_code) {}
NoGlobalPathException() : GlobalPlannerException("No global path found.") {}
};
/**
* @class GlobalPlannerTimeoutException
* @brief Exception thrown when the global planner has spent too long looking for a path
*/
class GlobalPlannerTimeoutException: public GlobalPlannerException
{
public:
explicit GlobalPlannerTimeoutException(const std::string& description, int result_code = 12)
: GlobalPlannerException(description, result_code) {}
};
/**
* @class IllegalTrajectoryException
* @brief Thrown when one of the critics encountered a fatal error
*/
class IllegalTrajectoryException: public LocalPlannerException
{
public:
IllegalTrajectoryException(const std::string& critic_name, const std::string& description, int result_code = 14)
: LocalPlannerException(description, result_code), critic_name_(critic_name) {}
std::string getCriticName() const { return critic_name_; }
protected:
std::string critic_name_;
};
/**
* @class NoLegalTrajectoriesException
* @brief Thrown when all the trajectories explored are illegal
*/
class NoLegalTrajectoriesException: public LocalPlannerException
{
public:
explicit NoLegalTrajectoriesException(const std::string& description, int result_code = 15)
: LocalPlannerException(description, result_code) {}
};
} // namespace nav_core2
#endif // NAV_CORE2_EXCEPTIONS_H

85
src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h Executable file
View File

@@ -0,0 +1,85 @@
/*
* 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 NAV_CORE2_GLOBAL_PLANNER_H
#define NAV_CORE2_GLOBAL_PLANNER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <string>
#include <memory>
namespace nav_core2
{
/**
* @class GlobalPlanner
* @brief Provides an interface for global planners used in navigation.
*/
class GlobalPlanner
{
public:
using Ptr = std::shared_ptr<GlobalPlanner>;
/**
* @brief Virtual Destructor
*/
virtual ~GlobalPlanner() {}
/**
* @brief Initialization function for the GlobalPlanner
*
* ROS parameters/topics are expected to be in the parent/name namespace.
* It is suggested that all NodeHandles in the planner use the parent NodeHandle's callback queue.
*
* @param parent NodeHandle to derive other NodeHandles from
* @param name The name of this planner
* @param tf A pointer to a transform listener
* @param costmap A pointer to the costmap
*/
virtual void initialize(const YAML::Node& parent, const std::string& name,
TFListenerPtr tf, Costmap::Ptr costmap) = 0;
/**
* @brief Run the global planner to make a plan starting at the start and ending at the goal.
* @param start The starting pose of the robot
* @param goal The goal pose of the robot
* @return The sequence of poses to get from start to goal, if any
*/
virtual nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
const nav_2d_msgs::Pose2DStamped& goal) = 0;
};
} // namespace nav_core2
#endif // NAV_CORE2_GLOBAL_PLANNER_H

173
src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h Executable file
View File

@@ -0,0 +1,173 @@
/*
* 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 NAV_CORE2_LOCAL_PLANNER_H
#define NAV_CORE2_LOCAL_PLANNER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <nav_2d_msgs/Path2D.h>
#include <nav_2d_msgs/Pose2DStamped.h>
#include <nav_2d_msgs/Twist2D.h>
#include <nav_2d_msgs/Twist2DStamped.h>
#include <geometry_msgs/Twist.h>
#include <string>
#include <costmap_2d/costmap_2d_robot.h>
#include <memory>
namespace nav_core2
{
/**
* @class LocalPlanner
* @brief Provides an interface for local planners used in navigation.
*/
class LocalPlanner
{
public:
using Ptr = std::shared_ptr<LocalPlanner>;
/**
* @brief Virtual Destructor
*/
virtual ~LocalPlanner() {}
/**
* @brief Constructs the local planner
*
* ROS parameters/topics are expected to be in the parent/name namespace.
* It is suggested that all NodeHandles in the planner use the parent NodeHandle's callback queue.
*
* @param parent NodeHandle to derive other NodeHandles from
* @param name The name to give this instance of the local planner
* @param tf A pointer to a transform listener
* @param costmap A pointer to the costmap
*/
// virtual void initialize(YAML::Node &parent, const std::string &name,
// TFListenerPtr tf, Costmap::Ptr costmap) = 0;
virtual void initialize(YAML::Node &parent, const std::string &name,
TFListenerPtr tf, costmap_2d::Costmap2DROBOT *costmap) = 0;
/**
* @brief Sets the global goal for this local planner.
* @param goal_pose The Goal Pose
*/
virtual void setGoalPose(const nav_2d_msgs::Pose2DStamped &goal_pose) = 0;
/**
* @brief Sets the global plan for this local planner.
*
* @param path The global plan
*/
virtual void setPlan(const nav_2d_msgs::Path2D &path) = 0;
/**
* @brief Compute the best command given the current pose, velocity and goal
*
* Get the local plan, given an initial pose, velocity and goal pose.
* It is presumed that the global plan is already set.
*
* @param pose Current robot pose
* @param velocity Current robot velocity
* @return The best computed velocity
*/
virtual nav_2d_msgs::Twist2DStamped computeVelocityCommands(const nav_2d_msgs::Pose2DStamped &pose,
const nav_2d_msgs::Twist2D &velocity) = 0;
/**
* @brief set velocity for x, y asix of the robot
* @param linear the velocity command
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) { return true; }
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) { return false; }
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) { throw std::runtime_error("Function getTwistLinear is not Support."); }
/**
* @brief Get velocity for x, y asix of the robot
* @return The current velocity
*/
virtual geometry_msgs::Twist getActualTwist() { throw std::runtime_error("Function getActualTwist is not Support."); }
/**
* @brief Check if the kinematic parameters are currently locked
* @return True if the kinematic parameters are locked, false otherwise
*
* This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL.
*/
virtual bool islock() {throw std::runtime_error("Function islock is not Support.");}
/**
* @brief Lock the kinematic parameters to prevent modifications
*/
virtual void lock() {throw std::runtime_error("Function lock is not Support.");}
/**
* @brief Unlock the kinematic parameters to allow modifications
*/
virtual void unlock() {throw std::runtime_error("Function unlock is not Support.");}
/**
* @brief Check to see whether the robot has reached its goal
*
* This tests whether the robot has fully reached the goal, given the current pose and velocity.
*
* @param query_pose The pose to check, in local costmap coordinates.
* @param velocity Velocity to check
* @return True if the goal conditions have been met
*/
virtual bool isGoalReached(const nav_2d_msgs::Pose2DStamped &pose, const nav_2d_msgs::Twist2D &velocity) = 0;
};
} // namespace nav_core2
#endif // NAV_CORE2_LOCAL_PLANNER_H

60
src/Navigations/Cores/nav_core2/src/basic_costmap.cpp Executable file
View File

@@ -0,0 +1,60 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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.
*/
#include <nav_core2/basic_costmap.h>
namespace nav_core2
{
void BasicCostmap::reset()
{
data_.assign(info_.width * info_.height, this->default_value_);
}
unsigned int BasicCostmap::getIndex(const unsigned int x, const unsigned int y) const
{
return y * info_.width + x;
}
unsigned char BasicCostmap::getValue(const unsigned int x, const unsigned int y) const
{
return data_[getIndex(x, y)];
}
void BasicCostmap::setValue(const unsigned int x, const unsigned int y, const unsigned char& value)
{
data_[getIndex(x, y)] = value;
}
} // namespace nav_core2

124
src/Navigations/Cores/nav_core2/test/bounds_test.cpp Executable file
View File

@@ -0,0 +1,124 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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.
*/
#include <gtest/gtest.h>
#include <nav_core2/bounds.h>
using nav_core2::Bounds;
using nav_core2::UIntBounds;
TEST(Bounds, test_bounds_simple)
{
Bounds b;
EXPECT_TRUE(b.isEmpty());
b.touch(5.0, 6.0);
EXPECT_EQ(5.0, b.getMinX());
EXPECT_EQ(5.0, b.getMinX());
EXPECT_EQ(5.0, b.getMaxX());
EXPECT_EQ(6.0, b.getMinY());
EXPECT_EQ(6.0, b.getMaxY());
EXPECT_TRUE(b.contains(5.0, 6.0));
EXPECT_FALSE(b.contains(5.5, 6.0));
EXPECT_FALSE(b.contains(5.5, 4.0));
EXPECT_FALSE(b.isEmpty());
Bounds b2 = b;
EXPECT_EQ(5.0, b2.getMinX());
EXPECT_EQ(5.0, b2.getMaxX());
EXPECT_EQ(6.0, b2.getMinY());
EXPECT_EQ(6.0, b2.getMaxY());
b.reset();
EXPECT_EQ(5.0, b2.getMinX());
EXPECT_EQ(5.0, b2.getMaxX());
EXPECT_EQ(6.0, b2.getMinY());
EXPECT_EQ(6.0, b2.getMaxY());
EXPECT_FALSE(b.contains(5.0, 6.0));
EXPECT_FALSE(b.contains(5.5, 6.0));
EXPECT_TRUE(b2.contains(5.0, 6.0));
EXPECT_FALSE(b.contains(5.5, 6.0));
EXPECT_TRUE(b.isEmpty());
Bounds b3;
b3.touch(1.0, 5.0);
b3.touch(4.0, 2.0);
EXPECT_TRUE(b3.contains(3.0, 3.0));
EXPECT_FALSE(b3.contains(0.0, 3.0));
EXPECT_FALSE(b3.contains(5.0, 3.0));
EXPECT_FALSE(b3.contains(3.0, 6.0));
EXPECT_FALSE(b3.contains(3.0, 1.0));
}
TEST(Bounds, test_dimensions)
{
UIntBounds empty;
UIntBounds square(0, 0, 5, 5);
UIntBounds rectangle(1, 4, 3, 15);
EXPECT_EQ(empty.getWidth(), 0u);
EXPECT_EQ(empty.getHeight(), 0u);
EXPECT_EQ(square.getWidth(), 6u);
EXPECT_EQ(square.getHeight(), 6u);
EXPECT_EQ(rectangle.getWidth(), 3u);
EXPECT_EQ(rectangle.getHeight(), 12u);
}
TEST(Bounds, test_bounds_overlap)
{
UIntBounds b0(0, 0, 5, 5);
UIntBounds b1(0, 0, 5, 5);
UIntBounds b2(0, 0, 3, 3);
UIntBounds b3(3, 0, 4, 4);
UIntBounds b4(4, 0, 4, 4);
UIntBounds b5(1, 4, 3, 15);
UIntBounds b6(10, 10, 10, 10);
EXPECT_TRUE(b0.overlaps(b0));
EXPECT_TRUE(b0.overlaps(b1));
EXPECT_TRUE(b0.overlaps(b2));
EXPECT_TRUE(b2.overlaps(b0));
EXPECT_TRUE(b0.overlaps(b3));
EXPECT_TRUE(b2.overlaps(b3));
EXPECT_FALSE(b2.overlaps(b4));
EXPECT_TRUE(b0.overlaps(b5));
EXPECT_TRUE(b5.overlaps(b0));
EXPECT_FALSE(b0.overlaps(b6));
EXPECT_FALSE(b6.overlaps(b0));
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

133
src/Navigations/Cores/nav_core2/test/exception_test.cpp Executable file
View File

@@ -0,0 +1,133 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2019, 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.
*/
#include <gtest/gtest.h>
#include <nav_core2/exceptions.h>
#include <string>
TEST(Exceptions, direct_code_access)
{
// Make sure the caught exceptions have the same types as the thrown exception
// (This version does not create any copies of the exception)
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
}
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::PlannerException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
}
}
TEST(Exceptions, indirect_code_access)
{
// Make sure the caught exceptions have the same types as the thrown exception
// (This version copies the exception to a new object with the parent type)
nav_core2::PlannerException e("");
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
e = x;
}
EXPECT_EQ(e.getResultCode(), 12);
}
TEST(Exceptions, rethrow)
{
// This version tests the ability to catch specific exceptions when rethrown
// A copy of the exception is made and rethrown, with the goal being able to catch the specific type
// and not the parent type.
nav_core2::NavCore2ExceptionPtr e;
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
e = std::current_exception();
}
EXPECT_EQ(nav_core2::getResultCode(e), 12);
try
{
std::rethrow_exception(e);
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
SUCCEED();
}
catch (nav_core2::PlannerException& x)
{
FAIL() << "PlannerException caught instead of specific exception";
}
}
TEST(Exceptions, weird_exception)
{
nav_core2::NavCore2ExceptionPtr e;
// Check what happens with no exception
EXPECT_EQ(nav_core2::getResultCode(e), -1);
// Check what happens with a non NavCore2Exception
try
{
std::string().at(1); // this generates an std::out_of_range
}
catch (...)
{
e = std::current_exception();
}
EXPECT_EQ(nav_core2::getResultCode(e), -1);
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

91
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt Executable file
View File

@@ -0,0 +1,91 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(nav_core_adapter VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
set(PACKAGES_DIR geometry_msgs std_msgs nav_core nav_core2 nav_2d_utils pthread)
find_package(Boost REQUIRED COMPONENTS filesystem system)
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tạo thư viện shared (.so)
add_library(costmap_adapter src/costmap_adapter.cpp)
target_link_libraries(costmap_adapter PRIVATE ${PACKAGES_DIR})
target_include_directories(costmap_adapter PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
add_library(local_planner_adapter src/local_planner_adapter.cpp)
target_link_libraries(local_planner_adapter
PRIVATE
Boost::filesystem
Boost::system
dl
costmap_adapter
${PACKAGES_DIR}
)
target_include_directories(local_planner_adapter PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
add_library(global_planner_adapter src/global_planner_adapter.cpp)
target_link_libraries(global_planner_adapter
PRIVATE
Boost::filesystem
Boost::system
dl
costmap_adapter
${PACKAGES_DIR}
)
target_include_directories(global_planner_adapter PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
# Cài đặt header files
install(DIRECTORY include/nav_core_adapter
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Cài đặt library
install(TARGETS global_planner_adapter local_planner_adapter costmap_adapter
EXPORT nav_core_adapter-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT nav_core_adapter-targets
FILE nav_core_adapter-targets.cmake
DESTINATION lib/cmake/nav_core_adapter)
# Tùy chọn build
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
# Flags biên dịch
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS_DEBUG "-g -O0")
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -DNDEBUG")
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "=================================")

48
src/Navigations/Cores/nav_core_adapter/README.md Executable file
View File

@@ -0,0 +1,48 @@
# nav_core_adapter
This package contains adapters for using `nav_core` plugins as `nav_core2` plugins and vice versa (more or less). In general, the adaptation process involves
* Converting between 2d and 3d datatypes.
* Converting between returning false and throwing exceptions on failure.
We also provide an adapter for using a `costmap_2d::Costmap2DROS` as a plugin for the `nav_core2::Costmap` interface.
## Adapter Classes
* Global Planner Adapters
* `GlobalPlannerAdapter` is used for employing a `nav_core2` global planner interface (such as `dlux_global_planner`) as a `nav_core` plugin, like in `move_base`.
* `GlobalPlannerAdapter2` is used for employing a `nav_core` global planner interface (such as `navfn`)
as a `nav_core2` plugin, like in `locomotor`.
* Local Planner Adapter
* `LocalPlannerAdapter` is used for employing a `nav_core2` local planner interface (such as `dwb_local_planner`) as a `nav_core` plugin, like in `move_base`. In addition to the standard adaptation steps listed above, the local planner adapter also uses the costmap to grab the global pose and subscribes to the odometry in order to get the current velocity.
* There is no `LocalPlannerAdapter2`. The `nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `nav_core` counterpart. This information would be ignored by a `nav_core` planner, so no adapter is provided.
* `CostmapAdapter` provides most of the functionality from `nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROS` object. It is not a perfect adaptation, because
* `Costmap2DROS` starts its own update thread and updates on its own schedule, so calling `update()` does not actually cause the costmap to update. It does update some of the metadata though.
* `setInfo` is not implemented.
## Parameter Setup
Let's look at a practical example of how to use `dwb_local_planner` in `move_base`.
If you were using `DWA` you would probably have parameters set up like this:
```
base_local_planner: dwa_local_planner/DWALocalPlanner
DWALocalPlanner:
acc_lim_x: 0.42
...
```
i.e. you specify
* The name of the planner
* A bunch of additional parameters within the planner's namespace
To use the adapter, you have to provide additional information.
```
base_local_planner: nav_core_adapter::LocalPlannerAdapter
LocalPlannerAdapter:
planner_name: dwb_local_planner::DWBLocalPlanner
DWBLocalPlanner:
acc_lim_x: 0.42
...
```
i.e.
* The name of the planner now points at the adapter
* The name of the actual planner loaded into the adapter's namespace
* The planner's parameters still in the planner's namespace.
The process for the global planners is similar.

9
src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg Executable file
View File

@@ -0,0 +1,9 @@
#!/usr/bin/env python
PACKAGE = 'nav_core_adapter'
from dynamic_reconfigure.parameter_generator_catkin import ParameterGenerator, str_t, double_t, bool_t, int_t
gen = ParameterGenerator()
gen.add("planner_name", str_t, 0, "The name of the plugin for the local planner to use with move_base.", "base_local_planner/TrajectoryPlannerROS")
exit(gen.generate(PACKAGE, "nav_core_adapter", "NavCoreAdapter"))

85
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h Executable file
View File

@@ -0,0 +1,85 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
#define NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <costmap_2d/costmap_2d_robot.h>
#include <string>
namespace nav_core_adapter
{
nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot);
class CostmapAdapter : public nav_core2::Costmap
{
public:
/**
* @brief Deconstructor for possibly freeing the costmap_ros_ object
*/
virtual ~CostmapAdapter();
/**
* @brief Initialize from an existing Costmap2DROS object
* @param costmap_ros A Costmap2DROS object
* @param needs_destruction Whether to free the costmap_ros object when this class is destroyed
*/
void initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction = false);
// Standard Costmap Interface
void initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf) override;
nav_core2::Costmap::mutex_t* getMutex() override;
// NavGrid Interface
void reset() override;
void update() override;
void setValue(const unsigned int x, const unsigned int y, const unsigned char& value) override;
unsigned char getValue(const unsigned int x, const unsigned int y) const override;
void setInfo(const nav_grid::NavGridInfo& new_info) override;
void updateInfo(const nav_grid::NavGridInfo& new_info) override;
// Get Costmap Pointer for Backwards Compatibility
costmap_2d::Costmap2DROBOT* getCostmap2DROBOT() const { return costmap_robot_; }
protected:
costmap_2d::Costmap2DROBOT* costmap_robot_;
costmap_2d::Costmap2D* costmap_;
bool needs_destruction_;
};
} // namespace nav_core_adapter
#endif // NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H

76
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h Executable file
View File

@@ -0,0 +1,76 @@
/*
* 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 NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
#define NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
#include <nav_core/base_global_planner.h>
#include <nav_core2/global_planner.h>
// #include <pluginlib/class_loader.h>
#include <boost/dll/import.hpp>
#include <string>
#include <vector>
namespace nav_core_adapter
{
/**
* @class GlobalPlannerAdapter
* @brief used for employing a `nav_core` global planner (such as `navfn`) as a `nav_core2` plugin, like in `locomotor`.
*/
class GlobalPlannerAdapter: public nav_core2::GlobalPlanner
{
public:
GlobalPlannerAdapter();
// Nav Core 2 Global Planner Interface
void initialize(const YAML::Node& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
nav_2d_msgs::Path2D makePlan(const nav_2d_msgs::Pose2DStamped& start,
const nav_2d_msgs::Pose2DStamped& goal) override;
static nav_core2::GlobalPlanner::Ptr create();
protected:
// Plugin handling
boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
nav_core::BaseGlobalPlanner::Ptr planner_;
costmap_2d::Costmap2DROBOT* costmap_robot_;
nav_core2::Costmap::Ptr costmap_;
};
} // namespace nav_core_adapter
#endif // NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H

210
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h Executable file
View File

@@ -0,0 +1,210 @@
/*
* 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 NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#include <nav_core/base_local_planner.h>
#include <nav_core2/local_planner.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_2d_utils/odom_subscriber.h>
#include <boost/dll/import.hpp>
#include <memory>
#include <string>
#include <vector>
namespace nav_core_adapter
{
struct NavCoreAdapterConfig
{
std::string planner_name;
bool use_odom;
bool use_costmap;
bool use_tf;
bool use_dynamic_reconfigure;
};
/**
* @class LocalPlannerAdapter
* @brief used for employing a nav_core2 local planner (such as dwb) as a nav_core plugin, like in move_base.
*/
class LocalPlannerAdapter : public nav_core::BaseLocalPlanner
{
public:
using Ptr = boost::shared_ptr<LocalPlannerAdapter>;
/**
* @brief Constructor
*/
LocalPlannerAdapter();
/**
* @brief Destructor
*/
virtual ~LocalPlannerAdapter();
// Standard ROS Local Planner Interface
void initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot) override;
/**
* @brief Given the current position, orientation, and velocity of the robot, compute velocity commands to send to the base
* @param cmd_vel Will be filled with the velocity command to be passed to the robot base
* @return True if a valid velocity command was found, false otherwise
*/
bool computeVelocityCommands(geometry_msgs::Twist &cmd_vel) override;
/**
* @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
* @param planner_name The object name
* @return True if instance object is successed, False otherwise
*/
virtual bool swapPlanner(std::string planner_name) override;
/**
* @brief set velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual bool setTwistLinear(geometry_msgs::Vector3 linear) override;
/**
* @brief get velocity for x, y asix of the robot
* @param linear The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistLinear(bool direct) override;
/**
* @brief Set velocity theta for z asix of the robot
* @param angular the command velocity
* @return True if set is done, false otherwise
*/
virtual bool setTwistAngular(geometry_msgs::Vector3 angular) override;
/**
* @brief Get velocity theta for z asix of the robot
* @param direct The velocity command
* @return True if set is done, false otherwise
*/
virtual geometry_msgs::Vector3 getTwistAngular(bool direct) override;
/**
* @brief Get actual velocity for x, y asix of the robot
* @return The current velocity
*/
virtual geometry_msgs::Twist getActualTwist() override;
/**
* @brief Check if the kinematic parameters are currently locked
* @return True if the kinematic parameters are locked, false otherwise
*
* This function indicates whether the kinematic parameters (e.g., velocities, accelerations, or direction settings) are in a locked state, preventing modifications. If the parameters are locked during in-place rotation, it could prevent updates to the direction or velocity, potentially causing inconsistencies in motion commands that lead to odometry errors and localization jumps in AMCL.
*/
virtual bool islock() override;
/**
* @brief Lock the kinematic parameters to prevent modifications
*/
virtual void lock() override;
/**
* @brief Unlock the kinematic parameters to allow modifications
*/
virtual void unlock() override;
/**
* @brief Check if the goal pose has been achieved by the local planner
* @return True if achieved, false otherwise
*/
bool isGoalReached() override;
/**
* @brief Set the plan that the local planner is following
* @param plan The plan to pass to the local planner
* @return True if the plan was updated successfully, false otherwise
*/
bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) override;
/**
* @brief Create a new LocalPlannerAdapter
* @return A shared pointer to the new LocalPlannerAdapter
*/
static nav_core::BaseLocalPlanner::Ptr create();
protected:
/**
* @brief Get the robot pose from the costmap and store as Pose2DStamped
*/
bool getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d);
/**
* @brief See if the back of the global plan matches the most recent goal pose
* @return True if the plan has changed
*/
bool hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal);
// The most recent goal pose
nav_2d_msgs::Pose2DStamped last_goal_;
bool has_active_goal_;
/**
* @brief helper class for subscribing to odometry
*/
std::shared_ptr<nav_2d_utils::OdomSubscriber> odom_sub_;
// Plugin handling
boost::function<nav_core2::LocalPlanner::Ptr()> planner_loader_;
nav_core2::LocalPlanner::Ptr planner_;
// Pointer Copies
TFListenerPtr tf_;
std::shared_ptr<CostmapAdapter> costmap_adapter_;
costmap_2d::Costmap2DROBOT *costmap_robot_;
boost::recursive_mutex configuration_mutex_;
// std::shared_ptr<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>> dsrv_;
YAML::Node private_nh_;
YAML::Node adapter_nh_;
// void reconfigureCB(nav_core_adapter::NavCoreAdapterConfig &config, uint32_t level);
nav_core_adapter::NavCoreAdapterConfig last_config_;
nav_core_adapter::NavCoreAdapterConfig default_config_;
bool setup_;
};
} // namespace nav_core_adapter
#endif // NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H

67
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h Executable file
View File

@@ -0,0 +1,67 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_CORE_ADAPTER_SHARED_POINTERS_H
#define NAV_CORE_ADAPTER_SHARED_POINTERS_H
#include <nav_core2/common.h>
#include <memory>
namespace nav_core_adapter
{
template <typename T>
void null_deleter(T* raw_ptr) {}
/**
* @brief Custom Constructor for creating a shared pointer to an existing object that doesn't delete the ptr when done
*
* @note This is considered bad form, and is only done here for backwards compatibility purposes. The nav_core2
* interfaces require shared pointers, but the creation of the shared pointer from a raw pointer takes ownership
* of the object such that when the object containing the shared pointer is freed, the object pointed at by the
* shared pointer is also freed. This presents a problem, for instance, when switching from one planner to another
* if the costmap is freed by one planner.
*
* @param raw_ptr The raw pointer to an object
* @return A Shared pointer pointing at the raw_ptr, but when it is freed, the raw_ptr remains valid
*/
template <typename T>
std::shared_ptr<T> createSharedPointerWithNoDelete(T* raw_ptr)
{
return std::shared_ptr<T>(raw_ptr, null_deleter<T>);
}
} // namespace nav_core_adapter
#endif // NAV_CORE_ADAPTER_SHARED_POINTERS_H

119
src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp Executable file
View File

@@ -0,0 +1,119 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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.
*/
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_core2/exceptions.h>
#include <boost/dll/import.hpp>
// #include <pluginlib/class_list_macros.h>
#include <string>
// PLUGINLIB_EXPORT_CLASS(nav_core_adapter::CostmapAdapter, nav_core2::Costmap)
namespace nav_core_adapter
{
nav_grid::NavGridInfo infoFromCostmap(costmap_2d::Costmap2DROBOT* costmap_robot)
{
nav_grid::NavGridInfo info;
costmap_2d::Costmap2D* costmap = costmap_robot->getCostmap();
info.width = costmap->getSizeInCellsX();
info.height = costmap->getSizeInCellsY();
info.resolution = costmap->getResolution();
info.frame_id = costmap_robot->getGlobalFrameID();
info.origin_x = costmap->getOriginX();
info.origin_y = costmap->getOriginY();
return info;
}
CostmapAdapter::~CostmapAdapter()
{
if (needs_destruction_)
{
delete costmap_robot_;
}
}
void CostmapAdapter::initialize(costmap_2d::Costmap2DROBOT* costmap_robot, bool needs_destruction)
{
costmap_robot_ = costmap_robot;
needs_destruction_ = needs_destruction;
info_ = infoFromCostmap(costmap_robot_);
costmap_ = costmap_robot_->getCostmap();
}
void CostmapAdapter::initialize(const YAML::Node& parent, const std::string& name, TFListenerPtr tf)
{
// TODO: Implement this if needed
throw nav_core2::CostmapException("initialize with YAML::Node not implemented");
}
nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()
{
return costmap_->getMutex();
}
void CostmapAdapter::reset()
{
costmap_robot_->resetLayers();
}
void CostmapAdapter::update()
{
info_ = infoFromCostmap(costmap_robot_);
if (!costmap_robot_->isCurrent())
throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
}
void CostmapAdapter::setValue(const unsigned int x, const unsigned int y, const unsigned char& value)
{
costmap_->setCost(x, y, value);
}
unsigned char CostmapAdapter::getValue(const unsigned int x, const unsigned int y) const
{
unsigned int index = costmap_->getIndex(x, y);
return costmap_->getCharMap()[index];
}
void CostmapAdapter::setInfo(const nav_grid::NavGridInfo& new_info)
{
throw nav_core2::CostmapException("setInfo not implemented on CostmapAdapter");
}
void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info)
{
costmap_->updateOrigin(new_info.origin_x, new_info.origin_y);
}
} // namespace nav_core_adapter

102
src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp Executable file
View File

@@ -0,0 +1,102 @@
/*
* 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.
*/
#include <nav_core_adapter/global_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_core2/exceptions.h>
#include <boost/dll/alias.hpp>
#include <memory>
#include <string>
#include <vector>
namespace nav_core_adapter
{
GlobalPlannerAdapter::GlobalPlannerAdapter()
// : planner_loader_("nav_core", "nav_core::BaseGlobalPlanner")
{
}
/**
* @brief Load the nav_core global planner and initialize it
*/
void GlobalPlannerAdapter::initialize(const YAML::Node& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
{
costmap_ = costmap;
std::shared_ptr<CostmapAdapter> ptr = std::static_pointer_cast<CostmapAdapter>(costmap);
if (!ptr)
{
std::cerr << "GlobalPlannerAdapter can only be used with the CostmapAdapter, not other Costmaps!" << std::endl;
exit(EXIT_FAILURE);
}
costmap_robot_ = ptr->getCostmap2DROBOT();
// YAML::Node planner_nh(parent, name);
std::string planner_name;
// planner_nh.param("planner_name", planner_name, std::string("global_planner/GlobalPlanner"));
// ROS_INFO_NAMED("GlobalPlannerAdapter", "Loading plugin %s", planner_name.c_str());
std::string path_file_so;
planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
planner_->initialize(planner_name, costmap_robot_);
}
nav_2d_msgs::Path2D GlobalPlannerAdapter::makePlan(const nav_2d_msgs::Pose2DStamped& start,
const nav_2d_msgs::Pose2DStamped& goal)
{
geometry_msgs::PoseStamped start3d = nav_2d_utils::pose2DToPoseStamped(start),
goal3d = nav_2d_utils::pose2DToPoseStamped(goal);
std::vector<geometry_msgs::PoseStamped> plan;
bool ret = planner_->makePlan(start3d, goal3d, plan);
if (!ret)
{
throw nav_core2::PlannerException("Generic Global Planner Error");
}
return nav_2d_utils::posesToPath2D(plan);
}
nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create()
{
return std::make_shared<GlobalPlannerAdapter>();
}
} // namespace nav_core_adapter
// register this planner as a GlobalPlanner plugin
BOOST_DLL_ALIAS(nav_core_adapter::GlobalPlannerAdapter::create, create_plugin)

408
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp Executable file
View File

@@ -0,0 +1,408 @@
/*
* 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.
*/
#include <nav_core_adapter/local_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_core_adapter/shared_pointers.h>
#include <nav_2d_utils/conversions.h>
#include <nav_2d_utils/tf_help.h>
#include <nav_2d_utils/parameters.h>
#include <nav_core2/exceptions.h>
#include <boost/dll/alias.hpp>
#include <memory>
#include <string>
#include <vector>
namespace nav_core_adapter
{
LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false)
{
}
LocalPlannerAdapter::~LocalPlannerAdapter()
{
std::cout << "=== LocalPlannerAdapter destructor called ===" << std::endl;
if (planner_)
{
std::cout << "Planner use_count before reset: " << planner_.use_count() << std::endl;
planner_.reset();
std::cout << "Planner reset complete" << std::endl;
}
else
{
std::cout << "Planner already null" << std::endl;
}
std::cout << "=== LocalPlannerAdapter destructor finished ===" << std::endl;
}
/**
* @brief Load the nav_core2 local planner and initialize it
*/
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, costmap_2d::Costmap2DROBOT *costmap_robot)
{
tf_ = createSharedPointerWithNoDelete<tf3::BufferCore>(tf);
costmap_robot_ = costmap_robot;
costmap_adapter_ = std::make_shared<CostmapAdapter>();
costmap_adapter_->initialize(costmap_robot);
YAML::Node nh;
private_nh_ = YAML::Node("~");
adapter_nh_ = YAML::Node("~/" + name);
std::cout << "Adapter namespace: " << adapter_nh_["~"].as<std::string>() << std::endl;
std::string planner_name;
if (adapter_nh_["planner_name"] && adapter_nh_["planner_name"].IsDefined())
{
planner_name = adapter_nh_["planner_name"].as<std::string>();
}
else
{
planner_name = nav_2d_utils::loadParameterWithDeprecation(
adapter_nh_, "planner_name", "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner"));
adapter_nh_["planner_name"] = planner_name;
}
std::cout << "Loading plugin " << planner_name << std::endl;
// planner_ = planner_loader_.createInstance(planner_name);
std::string path_file_so;
planner_loader_ = boost::dll::import_alias<nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
planner_->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(), tf_, costmap_robot_);
// odom_sub_ = std::make_shared<nav_2d_utils::OdomSubscriber>(nh);
// dsrv_ = std::make_shared<dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>>(configuration_mutex_, adapter_nh_);
// dynamic_reconfigure::Server<nav_core_adapter::NavCoreAdapterConfig>::CallbackType cb =
// boost::bind(&LocalPlannerAdapter::reconfigureCB, this, _1, _2);
// dsrv_->setCallback(cb);
}
bool LocalPlannerAdapter::swapPlanner(std::string planner_name)
{
boost::recursive_mutex::scoped_lock l(configuration_mutex_);
if (!setup_)
{
return false;
}
if (planner_name != last_config_.planner_name)
{
nav_core2::LocalPlanner::Ptr new_planner;
try
{
// Tạo planner mới
std::string path_file_so;
planner_loader_ = boost::dll::import_alias<nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
new_planner = planner_loader_();
if (!new_planner)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
tf_, costmap_robot_);
if (!new_planner)
{
std::cerr << "Failed to load planner " << planner_name << std::endl;
exit(EXIT_FAILURE);
}
new_planner->initialize(private_nh_, adapter_nh_["planner_name"].as<std::string>(),
tf_, costmap_robot_);
if (planner_)
planner_.reset();
planner_ = new_planner;
last_config_.planner_name = planner_name;
std::cout << "Loaded new planner: " << planner_name << std::endl;
}
catch (const std::exception &ex)
{
std::cerr << "Failed to load planner " << planner_name << ": " << ex.what() << std::endl;
return false;
}
}
return true;
}
/**
* @brief Collect the additional information needed by nav_core2 and call the child computeVelocityCommands
*/
bool LocalPlannerAdapter::computeVelocityCommands(geometry_msgs::Twist &cmd_vel)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
if (!has_active_goal_)
{
return false;
}
// Get the Pose
nav_2d_msgs::Pose2DStamped pose2d;
if (!getRobotPose(pose2d))
{
return false;
}
// Get the Velocity
nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
nav_2d_msgs::Twist2DStamped cmd_vel_2d;
try
{
cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
}
catch (const nav_core2::PlannerException &e)
{
std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
return false;
}
cmd_vel = nav_2d_utils::twist2Dto3D(cmd_vel_2d.velocity);
return true;
}
bool LocalPlannerAdapter::setTwistLinear(geometry_msgs::Vector3 linear)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
{
if (planner_)
return planner_->setTwistLinear(linear);
else
throw std::runtime_error("Failed to set linear");
}
catch (const std::exception &e)
{
throw std::exception(e);
}
}
geometry_msgs::Vector3 LocalPlannerAdapter::getTwistLinear(bool direct)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
{
if (planner_)
return planner_->getTwistLinear(direct);
else
throw std::runtime_error("Failed to get linear");
}
catch (const std::exception &e)
{
throw std::exception(e);
}
}
bool LocalPlannerAdapter::setTwistAngular(geometry_msgs::Vector3 angular)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
{
if (planner_)
return planner_->setTwistAngular(angular);
else
throw std::runtime_error("Failed to set angular");
}
catch (const std::exception &e)
{
throw std::exception(e);
}
}
geometry_msgs::Vector3 LocalPlannerAdapter::getTwistAngular(bool direct)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
{
if (planner_)
return planner_->getTwistAngular(direct);
else
throw std::runtime_error("Failed to get angular");
}
catch (const std::exception &e)
{
throw std::exception(e);
}
}
geometry_msgs::Twist LocalPlannerAdapter::getActualTwist()
{
if (odom_sub_)
return nav_2d_utils::twist2Dto3D(odom_sub_->getTwist());
else
throw std::runtime_error("Failed to get twist");
}
bool LocalPlannerAdapter::islock()
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
{
if (planner_)
return planner_->islock();
else
throw std::runtime_error("Failed to call funcion islock");
}
catch (const std::exception &e)
{
std::cerr << e.what() << std::endl;
throw std::exception(e);
}
}
void LocalPlannerAdapter::lock()
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
{
if (planner_)
planner_->lock();
else
throw std::runtime_error("Failed to call funcion lock");
}
catch (const std::exception &e)
{
std::cerr << e.what() << std::endl;
throw std::exception(e);
}
}
void LocalPlannerAdapter::unlock()
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
try
{
if (planner_)
planner_->unlock();
else
throw std::runtime_error("Failed to call funcion unlock");
}
catch (const std::exception &e)
{
std::cerr << e.what() << std::endl;
throw std::exception(e);
}
}
/**
* @brief Collect the additional information needed by nav_core2 and call the child isGoalReached
*/
bool LocalPlannerAdapter::isGoalReached()
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
// Get the Pose
nav_2d_msgs::Pose2DStamped pose2d;
if (!getRobotPose(pose2d))
return false;
nav_2d_msgs::Twist2D velocity = odom_sub_->getTwist();
bool ret = planner_->isGoalReached(pose2d, velocity);
if (ret)
{
has_active_goal_ = false;
}
return ret;
}
/**
* @brief Convert from 2d to 3d and call child setPlan
*/
bool LocalPlannerAdapter::setPlan(const std::vector<geometry_msgs::PoseStamped> &orig_global_plan)
{
boost::recursive_mutex::scoped_lock ecl(configuration_mutex_);
nav_2d_msgs::Path2D path = nav_2d_utils::posesToPath2D(orig_global_plan);
try
{
if (path.poses.size() > 0)
{
nav_2d_msgs::Pose2DStamped goal_pose;
goal_pose = path.poses.back();
if (!has_active_goal_ || hasGoalChanged(goal_pose))
{
last_goal_ = goal_pose;
has_active_goal_ = true;
planner_->setGoalPose(goal_pose);
planner_->setPlan(path);
}
}
return true;
}
catch (const nav_core2::PlannerException &e)
{
std::cerr << "setPlan Exception: " << e.what() << std::endl;
return false;
}
}
bool LocalPlannerAdapter::hasGoalChanged(const nav_2d_msgs::Pose2DStamped &new_goal)
{
if (last_goal_.header.frame_id != new_goal.header.frame_id ||
last_goal_.header.stamp.toSec() != new_goal.header.stamp.toSec())
{
return true;
}
return last_goal_.pose.x != new_goal.pose.x || last_goal_.pose.y != new_goal.pose.y ||
last_goal_.pose.theta != new_goal.pose.theta;
}
bool LocalPlannerAdapter::getRobotPose(nav_2d_msgs::Pose2DStamped &pose2d)
{
geometry_msgs::PoseStamped current_pose;
if (!costmap_robot_->getRobotPose(current_pose))
{
std::cout << "Could not get robot pose" << std::endl;
return false;
}
pose2d = nav_2d_utils::poseStampedToPose2D(current_pose);
return true;
}
nav_core::BaseLocalPlanner::Ptr LocalPlannerAdapter::create()
{
return std::make_shared<LocalPlannerAdapter>();
}
} // namespace nav_core_adapter
// register this planner as a BaseLocalPlanner plugin
BOOST_DLL_ALIAS(nav_core_adapter::LocalPlannerAdapter::create, LocalPlannerAdapter)

69
src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp Executable file
View File

@@ -0,0 +1,69 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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.
*/
#include <gtest/gtest.h>
#include <nav_core_adapter/local_planner_adapter.h>
TEST(LocalPlannerAdapter, unload_local_planner)
{
tf2_ros::Buffer tf;
tf.setUsingDedicatedThread(true);
// This empty transform is added to satisfy the constructor of
// Costmap2DROS, which waits for the transform from map to base_link
// to become available.
geometry_msgs::TransformStamped base_rel_map;
base_rel_map.child_frame_id = "base_link";
base_rel_map.header.frame_id = "map";
base_rel_map.transform.rotation.w = 1.0;
tf.setTransform(base_rel_map, "unload", true);
nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter();
costmap_2d::Costmap2DROS costmap_ros("local_costmap", tf);
lpa->initialize("lpa", &tf, &costmap_ros);
delete lpa;
// Simple test to make sure costmap hasn't been deleted
EXPECT_EQ("map", costmap_ros.getGlobalFrameID());
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "unload_test");
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

3
src/Navigations/Cores/nav_core_adapter/test/unload_test.launch Executable file
View File

@@ -0,0 +1,3 @@
<launch>
<test time-limit="10" test-name="unload_test" pkg="nav_core_adapter" type="unload_test" />
</launch>

71
src/Navigations/Libraries/nav_grid/CMakeLists.txt Executable file
View File

@@ -0,0 +1,71 @@
cmake_minimum_required(VERSION 3.10)
project(nav_grid VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# Tìm tất cả header files
file(GLOB_RECURSE HEADERS "include/nav_grid/*.h")
# Tạo INTERFACE library (header-only)
add_library(nav_grid INTERFACE)
# Set include directories
target_include_directories(nav_grid
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/nav_grid
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Cài đặt target
install(TARGETS nav_grid
EXPORT nav_grid-targets
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT nav_grid-targets
FILE nav_grid-targets.cmake
NAMESPACE nav_grid::
DESTINATION lib/cmake/nav_grid)
# Tùy chọn build tests
option(BUILD_TESTING "Build test programs" OFF)
if(BUILD_TESTING)
find_package(GTest REQUIRED)
# Tạo test executable
add_executable(nav_grid_test test/utest.cpp)
target_link_libraries(nav_grid_test
PRIVATE
nav_grid
GTest::GTest
GTest::Main
)
# Thêm test vào CTest
enable_testing()
add_test(NAME nav_grid_test COMMAND nav_grid_test)
endif()
# In thông tin cấu hình
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME} (Header-Only)")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Headers found:")
foreach(hdr ${HEADERS})
message(STATUS " - ${hdr}")
endforeach()
message(STATUS "Build tests: ${BUILD_TESTING}")
message(STATUS "=================================")

39
src/Navigations/Libraries/nav_grid/README.md Executable file
View File

@@ -0,0 +1,39 @@
# nav_grid
Many navigation algorithms rely on the concept of a two dimensional grid being overlaid on the world, with a value being assigned to each grid cell. In the original navigation stack, `Costmap2DROS` associated an `unsigned char` with grid cells, global planners cache distance to the goal as `float`s and local planners would cache various metrics in a grid to quickly calculate the strength of different trajectories.
![nav_grid diagram](doc/nav_grid.png)
## `NavGridInfo`
Where the grid exists in the world is defined by six parameters.
* `width` and `height` (which together define the number of cells in the grid)
* `resolution` which is the dimension of each cell in meters (square cells only)
* `frame_id` which is the TF frame the grid is defined relative to.
* `origin_x` and `origin_y` which define the offset in meters from the root of the TF frame to the minimum corner of the (0, 0) cell.
Together, these components make a [`nav_grid::NavGridInfo`](include/nav_grid/nav_grid_info.h) struct. It evolved from the [`nav_msgs::MapMetaData` message](http://docs.ros.org/melodic/api/nav_msgs/html/msg/MapMetaData.html) but without `map_load_time`, simplified origin geometry and the `frame_id` added. Note: for general efficiency of computation (particularly moving the grid while retaining some of the values) there is no rotation component to the origin. Each grid is locked to the orientation of its TF frame.
The default values are `width=0, height=0, resolution=1.0, frame_id="map", origin_x=0.0, origin_y=0.0`.
## Coordinate Conversion
One of the most common operations is to want to convert between the real world coordinates and the grid coordinates. These operations can be done with a `NavGridInfo` object and the methods in [`coordinate_conversion.h`](include/nav_grid/coordinate_conversion.h). They are derived from methods in [`costmap_2d.h`](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/costmap_2d/include/costmap_2d/costmap_2d.h#L126), but with some key differences (beyond replacing `map` with `grid`).
* `gridToWorld` is the same as `mapToWorld`, as both return the world coordinates of the center of the specified cell.
* `worldToGrid` works like `worldToMapNoBounds`, but it results in either `int` or `double` coordinates depending on the output parameter types. As the result are not bounded by the grid, the results are signed.
* `worldToGridBounded` is a combination of `worldToMap` and `worldToMapEnforceBounds`. It returns a bool for whether the input coordinates are within the grid AND the output coordinates are forced to be within the grid. The output coordinates are therefore `unsigned int`.
* There's also `isWithinGrid` that returns whether a given point is within the grid (i.e. will match the return value of `worldToGridBounded` but saves some of the computation associated with calculating the actual values of the coordinates.
![example coordinate conversion](doc/coords.png)
## `NavGrid<T>`
Of course, we also want to associate a value with each cell in the grid. For that, we define the templatized [`nav_grid::NavGrid<T>`](include/nav_grid/nav_grid.h) abstract class. The template allows for storing arbitrary data types associated with each grid cell. The actual storage mechanism for the data is not part of the base class to allow for possibly more efficient methods. A default implementation where the data is simply stored in row-major order in a one-dimensional vector is provided in [`nav_grid::VectorNavGrid<T>`](include/nav_grid/vector_nav_grid.h>)
The constructor for `NavGrid` takes a default value for each cell which is 0 by default. The grid's initial info matches the default info above, so the grid is initially `0x0`.
The `NavGrid` class provides handy methods for accessing values via their grid indexes. You can use `grid(x, y)` or `grid.getValue(x, y)` to access each value, and use `grid.setValue(x, y, value)` to write each value. There is also the helper class [`nav_grid::Index`](include/nav_grid/index.h) that can be used to store the two coordinates and used in accessing the data as well a la `grid(index)` and `grid.setValue(index, value)`.
There are two methods for changing the `info` associated with the grid: `setInfo` and `updateInfo`. `setInfo` changes the `info` while maintaining the data associated with each grid coordinate. `updateInfo` will change the info but instead maintain the data associated with the world coordinates.
For instance, imagine a 5x5 grid with 0.5 meter resolution with the cell (2, 0) set to red which represents a cell at (1.25, 0.25) in the world. If we change the origin to be 0.5 meters to the right, the grids will have different values according to the method we use. With `setInfo`, cell (2, 0) is still red, but it is associated with a cell at (1.75, 0.25) in the world. With `updateInfo`, the cell at (1.25, 0.25) is still red, but it is now associated with cell (1, 0). The exact mechanism for how this data is preserved is left to the implementing class.
![illustration of grid update](doc/change_info.png)

BIN
src/Navigations/Libraries/nav_grid/doc/change_info.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 9.6 KiB

BIN
src/Navigations/Libraries/nav_grid/doc/coords.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 55 KiB

BIN
src/Navigations/Libraries/nav_grid/doc/nav_grid.png Executable file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 15 KiB

168
src/Navigations/Libraries/nav_grid/include/nav_grid/coordinate_conversion.h Executable file
View File

@@ -0,0 +1,168 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_GRID_COORDINATE_CONVERSION_H
#define NAV_GRID_COORDINATE_CONVERSION_H
#include <nav_grid/nav_grid_info.h>
#include <math.h>
namespace nav_grid
{
/**
* @brief Convert from grid coordinates to world coordinates of the center of the cell
*
* The resulting coordinates are for the center of the grid cell.
*
* @param[in] mx The x grid coordinate
* @param[in] my The y grid coordinate
* @param[out] wx Set to the associated x world coordinate
* @param[out] wy Set to the associated y world coordinate
*/
inline void gridToWorld(const NavGridInfo& info, int mx, int my, double& wx, double& wy)
{
wx = info.origin_x + (mx + 0.5) * info.resolution;
wy = info.origin_y + (my + 0.5) * info.resolution;
}
/**
* @brief Convert from world coordinates to the precise (double) grid coordinates
*
* The results are not rounded, so that the values can be used for locating a position within a cell
*
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @param[out] mx Set to the associated x grid coordinate
* @param[out] my Set to the associated y grid coordinate
*/
inline void worldToGrid(const NavGridInfo& info, double wx, double wy, double& mx, double& my)
{
mx = (wx - info.origin_x) / info.resolution;
my = (wy - info.origin_y) / info.resolution;
}
/**
* @brief Convert from world coordinates to grid coordinates without checking for legal bounds
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @param[out] mx Set to the associated x grid coordinate
* @param[out] my Set to the associated y grid coordinate
* @note The returned grid coordinates <b>are not guaranteed to lie within the grid.</b>
*/
inline void worldToGrid(const NavGridInfo& info, double wx, double wy, int& mx, int& my)
{
double dmx, dmy;
worldToGrid(info, wx, wy, dmx, dmy);
mx = static_cast<int>(floor(dmx));
my = static_cast<int>(floor(dmy));
}
/**
* @brief Convert from world coordinates to grid coordinates
*
* Combined functionality from costmap_2d::worldToMap and costmap_2d::worldToMapEnforceBounds.
* The output parameters are set to grid indexes within the grid, even if the function returns false,
* meaning the coordinates are outside the grid.
*
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @param[out] mx Set to the associated (bounds-enforced) x grid coordinate
* @param[out] my Set to the associated (bounds-enforced) y grid coordinate
* @return True if the input coordinates were within the grid
*/
inline bool worldToGridBounded(const NavGridInfo& info, double wx, double wy, unsigned int& mx, unsigned int& my)
{
double dmx, dmy;
worldToGrid(info, wx, wy, dmx, dmy);
bool valid = true;
if (dmx < 0.0)
{
mx = 0;
valid = false;
}
else if (dmx >= info.width)
{
mx = info.width - 1;
valid = false;
}
else
{
mx = static_cast<unsigned int>(dmx);
}
if (dmy < 0.0)
{
my = 0;
valid = false;
}
else if (dmy >= info.height)
{
my = info.height - 1;
valid = false;
}
else
{
my = static_cast<unsigned int>(dmy);
}
return valid;
}
/**
* @brief Check to see if the world coordinates are within the grid.
*
* This should only be used if the caller does not need the associated grid coordinates. Otherwise it would
* be more efficient to call worldToGridBounded.
*
* @param[in] wx The x world coordinate
* @param[in] wy The y world coordinate
* @return True if the input coordinates were within the grid
*/
inline bool isWithinGrid(const NavGridInfo& info, double wx, double wy)
{
wx -= info.origin_x;
wy -= info.origin_y;
return wx >= 0.0 &&
wy >= 0.0 &&
wx < info.width * info.resolution &&
wy < info.height * info.resolution;
}
} // namespace nav_grid
#endif // NAV_GRID_COORDINATE_CONVERSION_H

99
src/Navigations/Libraries/nav_grid/include/nav_grid/index.h Executable file
View File

@@ -0,0 +1,99 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_GRID_INDEX_H
#define NAV_GRID_INDEX_H
#include <string>
namespace nav_grid
{
/**
* @class GenericIndex
* @brief A simple pair of x/y coordinates
*/
template <typename NumericType>
struct GenericIndex
{
NumericType x, y;
explicit GenericIndex(const NumericType& x = 0, const NumericType& y = 0) : x(x), y(y) {}
/**
* @brief comparison operator that requires equal x and y
*/
bool operator == (const GenericIndex& other) const
{
return x == other.x && y == other.y;
}
bool operator != (const GenericIndex& other) const
{
return !operator==(other);
}
/**
* @brief less than operator so object can be used in sets
*/
bool operator < (const GenericIndex& other) const
{
return x < other.x || (x == other.x && y < other.y);
}
// Derived Comparators
bool operator > (const GenericIndex& other) const { return other < *this; }
bool operator <= (const GenericIndex& other) const { return !(*this > other); }
bool operator >= (const GenericIndex& other) const { return !(*this < other); }
/**
* @brief String representation of this object
*/
std::string toString() const
{
return "(" + std::to_string(x) + ", " + std::to_string(y) + ")";
}
};
template <typename NumericType>
inline std::ostream& operator<<(std::ostream& stream, const GenericIndex<NumericType>& index)
{
stream << index.toString();
return stream;
}
using SignedIndex = GenericIndex<int>;
using Index = GenericIndex<unsigned int>;
} // namespace nav_grid
#endif // NAV_GRID_INDEX_H

157
src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid.h Executable file
View File

@@ -0,0 +1,157 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_GRID_NAV_GRID_H
#define NAV_GRID_NAV_GRID_H
#include <nav_grid/nav_grid_info.h>
#include <nav_grid/index.h>
#include <string>
namespace nav_grid
{
/**
* @class NavGrid
* This class is a spiritual successor to the costmap_2d::Costmap2D class, with the key differences being that
* the datatype and data storage methods are not specified, and the frame_id is specified.
*
* The templatized nature of the class allows you to store whatever you like at each grid location, including
* unsigned chars if emulating Costmap2D or floating point numbers if emulating the grid_map package, or whatever
* else.
*
* The VectorNavGrid class in this package implements this class with a straight-forward single-dimensional vector
* representing the two dimensional grid. Other classes could implement the data storage differently.
*
* Getting data from the grid can be done either through the getValue methods or the parenthetical operators (which call
* getValue internally). Implementing classes must implement getValue.
* x = grid(0, 0) + grid.getValue(0, 1);
*
* Writing data to the grid must be done through the setValue method (which implementing classes must implement)
* grid.setValue(0, 0, x);
*
* You can also use nav_grid::Index objects
* nav_grid::Index index(0, 0);
* x = grid(index) + grid.getValue(index);
* index.y = 3;
* grid.setCost(index, x);
* The Index methods also internally call setValue/getValue
*
* The geometry of the grid is specified by the NavGridInfo. Borrowing an idea from the grid_map package, two
* separate methods are defined for changing the info. setInfo will change the info without changing the grid values.
* updateInfo will change the info while trying to preserve the contents of the grid.
*
* The final component is a collection of methods inspired by Costmap2D for converting coordinates of different types.
*/
template <typename T> class NavGrid
{
public:
explicit NavGrid(const T default_value = T{}) : default_value_(default_value) {}
/**
* @brief Reset the contents of the grid
*/
virtual void reset() = 0;
/**
* @brief get the value of the grid at (x,y)
* @param x[in] Valid x coordinate
* @param y[in] Valid y coordinate
* @return value at (x,y)
*/
virtual T getValue(const unsigned int x, const unsigned int y) const = 0;
/**
* @brief set the value of the grid at (x,y)
* @param x[in] Valid x coordinate
* @param y[in] Valid y coordinate
* @param value[in] New Value
*/
virtual void setValue(const unsigned int x, const unsigned int y, const T& value) = 0;
/**@name Convenience Aliases */
// Note: You may not be able to use these unless your deriving class declares using NavGrid<T>::operator() or
// using NavGrid<T>::getValue
/**@{*/
T getValue(const Index& index) { return getValue(index.x, index.y); }
T operator() (const unsigned int x, const unsigned int y) const { return getValue(x, y); }
T operator() (const Index& index) const { return getValue(index.x, index.y); }
void setValue(const Index& index, const T& value) { setValue(index.x, index.y, value); }
/**@}*/
/**
* @brief Change the info while attempting to keep the values associated with the grid coordinates
* @param[in] new_info New grid info
*/
virtual void setInfo(const NavGridInfo& new_info) = 0;
/**
* @brief Change the info while attempting to keep the values associated with the world coordinates
*
* For example, if the only change to the info is to the origin's x coordinate (increasing by an amount equal to the
* resolution), then all the values should be shifted one grid cell to the left.
*
* @param[in] new_info New grid info
*/
virtual void updateInfo(const NavGridInfo& new_info) { setInfo(new_info); }
inline NavGridInfo getInfo() const { return info_; }
/**
* @brief Set the default value
* @param[in] new_value New Default Value
*/
void setDefaultValue(const T new_value)
{
default_value_ = new_value;
}
/*****************************************************************************************************
* NavGridInfo accessor methods
*****************************************************************************************************/
inline unsigned int getWidth() const { return info_.width; }
inline unsigned int getHeight() const { return info_.height; }
inline double getResolution() const { return info_.resolution; }
inline std::string getFrameId() const { return info_.frame_id; }
inline double getOriginX() const { return info_.origin_x; }
inline double getOriginY() const { return info_.origin_y; }
protected:
NavGridInfo info_;
T default_value_;
};
} // namespace nav_grid
#endif // NAV_GRID_NAV_GRID_H

92
src/Navigations/Libraries/nav_grid/include/nav_grid/nav_grid_info.h Executable file
View File

@@ -0,0 +1,92 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_GRID_NAV_GRID_INFO_H
#define NAV_GRID_NAV_GRID_INFO_H
#include <string>
namespace nav_grid
{
/**
* @struct NavGridInfo
* This class defines a way to discretize a finite section of the world into a grid.
* It contains similar information to the ROS msg nav_msgs/MapMetaData (aka the info field of nav_msgs/OccupancyGrid)
* except the map_load_time is removed, the geometry is simplified from a Pose to xy coordinates, and the frame_id
* is added.
*/
struct NavGridInfo
{
public:
/* All data is publically accessible */
unsigned int width = 0;
unsigned int height = 0;
double resolution = 1.0;
std::string frame_id = "map";
double origin_x = 0.0; ///< The origin defines the coordinates of minimum corner of cell (0,0) in the grid
double origin_y = 0.0;
/**
* @brief comparison operator that requires all fields are equal
*/
bool operator == (const NavGridInfo& info) const
{
return width == info.width && height == info.height && resolution == info.resolution &&
origin_x == info.origin_x && origin_y == info.origin_y && frame_id == info.frame_id;
}
bool operator != (const NavGridInfo& info) const
{
return !operator==(info);
}
/**
* @brief String representation of this object
*/
std::string toString() const
{
return std::to_string(width) + "x" + std::to_string(height) + " (" + std::to_string(resolution) + "res) " +
frame_id + " " + std::to_string(origin_x) + " " + std::to_string(origin_y);
}
};
inline std::ostream& operator<<(std::ostream& stream, const NavGridInfo& info)
{
stream << info.toString();
return stream;
}
} // namespace nav_grid
#endif // NAV_GRID_NAV_GRID_INFO_H

239
src/Navigations/Libraries/nav_grid/include/nav_grid/vector_nav_grid.h Executable file
View File

@@ -0,0 +1,239 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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 NAV_GRID_VECTOR_NAV_GRID_H
#define NAV_GRID_VECTOR_NAV_GRID_H
#include <nav_grid/nav_grid.h>
#include <nav_grid/coordinate_conversion.h>
#include <algorithm>
#include <vector>
namespace nav_grid
{
/**
* @class VectorNavGrid
* A straight-forward implementation of the NavGrid class where the data for cell (x, y) is stored in a std::vector
* with index (y * info.width + x).
*/
template <typename T> class VectorNavGrid : public NavGrid<T>
{
public:
using NavGrid<T>::NavGrid;
/**
* @brief Reset the contents of the grid to the default value
*/
void reset() override
{
data_.assign(this->info_.width * this->info_.height, this->default_value_);
}
/**
* @brief Change the info while attempting to keep the values associated with the grid coordinates
*
* If the width changes, we need to move each row to its new location
*
* If just the height changes, then we can resize the vector without having to move elements
*
* We just overwrite the rest of the grid info
*/
void setInfo(const NavGridInfo& new_info) override
{
if (this->info_.width != new_info.width)
{
std::vector<T> new_vector(new_info.width * new_info.height, this->default_value_);
unsigned int cols_to_move = std::min(this->info_.width, new_info.width);
auto old_it = data_.begin();
auto new_it = new_vector.begin();
unsigned int max_row = std::min(this->info_.height, new_info.height);
for (unsigned int row = 0; row < max_row; row++)
{
std::copy(old_it, old_it + cols_to_move, new_it);
old_it += this->info_.width;
new_it += new_info.width;
}
data_.swap(new_vector);
}
else if (this->info_.height != new_info.height)
{
data_.resize(new_info.width * new_info.height, this->default_value_);
}
this->info_ = new_info;
}
/**
* @brief Update the info while keeping the data geometrically in tact
*
* If the resolution or frame_id changes, reset all the data.
*
* Otherwise, adjust the new_info so the grid stays aligned (The grid's new info will be within a
* resolution-length of the original new_info). Then copy the common values into the new grid.
*
* @param[in] new_info New information to update the grid with
*/
void updateInfo(const NavGridInfo& new_info) override
{
// If the info is the same, make no changes
if (this->info_ == new_info)
{
return;
}
// If the resolution or frame changes, reset the whole grid
if (this->info_.resolution != new_info.resolution || this->info_.frame_id != new_info.frame_id)
{
setInfo(new_info);
return;
}
// project the new origin into the grid
int cell_ox, cell_oy;
worldToGrid(this->info_, new_info.origin_x, new_info.origin_y, cell_ox, cell_oy);
// To save casting from unsigned int to int a bunch of times
int old_size_x = static_cast<int>(this->info_.width);
int old_size_y = static_cast<int>(this->info_.height);
// we need to compute the overlap of the new and existing windows
int lower_left_x = std::min(std::max(cell_ox, 0), old_size_x);
int lower_left_y = std::min(std::max(cell_oy, 0), old_size_y);
int upper_right_x = std::min(std::max(cell_ox + static_cast<int>(new_info.width), 0), old_size_x);
int upper_right_y = std::min(std::max(cell_oy + static_cast<int>(new_info.height), 0), old_size_y);
unsigned int cell_size_x = upper_right_x - lower_left_x;
unsigned int cell_size_y = upper_right_y - lower_left_y;
// we need a vector to store the new contents in the window temporarily
std::vector<T> new_data(new_info.width * new_info.height, this->default_value_);
// compute the starting cell location for copying data back in
int start_x = lower_left_x - cell_ox;
int start_y = lower_left_y - cell_oy;
// now we want to copy the overlapping information into the new vector, but in its new location
// we'll first need to compute the starting points for each vector
auto src_index = data_.begin() + (lower_left_y * old_size_x + lower_left_x);
auto dest_index = new_data.begin() + (start_y * new_info.width + start_x);
// now, we'll copy the source vector into the destination vector
for (unsigned int i = 0; i < cell_size_y; ++i)
{
std::copy(src_index, src_index + cell_size_x, dest_index);
src_index += this->info_.width;
dest_index += new_info.width;
}
data_.swap(new_data);
// update the dimensions
this->info_.width = new_info.width;
this->info_.height = new_info.height;
// update the origin. Recomputed instead of using new_info.origin
// because we want to keep things grid-aligned
this->info_.origin_x += cell_ox * this->info_.resolution;
this->info_.origin_y += cell_oy * this->info_.resolution;
}
void setValue(const unsigned int x, const unsigned int y, const T& value) override
{
data_[getIndex(x, y)] = value;
}
T getValue(const unsigned int x, const unsigned int y) const override
{
return data_[getIndex(x, y)];
}
using NavGrid<T>::operator();
using NavGrid<T>::getValue;
using NavGrid<T>::setValue;
/**
* Overloading the [] operator so that the data can be accessed directly with vector_nav_grid[i]
*/
T operator[] (unsigned int i) const {return data_[i];}
T& operator[] (unsigned int i) {return data_[i];}
/**
* @brief Return the size of the vector. Equivalent to width * height.
* @return size of the vector
*/
unsigned int size() const { return data_.size(); }
/**
* @brief Given two grid coordinates... compute the associated index
* @param[in] mx The x coordinate
* @param[in] my The y coordinate
* @return The associated index
*/
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
return my * this->info_.width + mx;
}
/**
* @brief Given two world coordinates... compute the associated index
* @param[in] mx The x coordinate
* @param[in] my The y coordinate
* @return The associated index
*/
inline unsigned int getIndex(double x, double y) const
{
unsigned int mx, my;
worldToGridBounded(this->info_, x, y, mx, my);
return getIndex(mx, my);
}
/**
* @brief Given an index... compute the associated grid coordinates
* @param[in] index The index
* @param[out] mx Set to the associated x grid coordinate
* @param[out] my Set to the associated y grid coordinate
*/
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
unsigned int w = this->info_.width;
my = index / w;
mx = index - my * w;
}
protected:
std::vector<T> data_;
};
} // namespace nav_grid
#endif // NAV_GRID_VECTOR_NAV_GRID_H

522
src/Navigations/Libraries/nav_grid/test/utest.cpp Executable file
View File

@@ -0,0 +1,522 @@
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2018, 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.
*/
#include <gtest/gtest.h>
#include <nav_grid/vector_nav_grid.h>
#include <nav_grid/coordinate_conversion.h>
#include <algorithm>
TEST(VectorNavGrid, info_equality)
{
nav_grid::NavGridInfo info0;
nav_grid::NavGridInfo info1;
nav_grid::NavGridInfo width_info;
width_info.width = 3;
nav_grid::NavGridInfo height_info;
height_info.height = 3;
nav_grid::NavGridInfo res_info;
res_info.resolution = 3.0;
nav_grid::NavGridInfo frame_info;
frame_info.frame_id = "foobar";
nav_grid::NavGridInfo originx_info;
originx_info.origin_x = 3.0;
nav_grid::NavGridInfo originy_info;
originy_info.origin_y = 3.0;
EXPECT_EQ(info0, info0);
EXPECT_EQ(info0, info1);
EXPECT_NE(info0, width_info);
EXPECT_NE(info0, height_info);
EXPECT_NE(info0, res_info);
EXPECT_NE(info0, frame_info);
EXPECT_NE(info0, originx_info);
EXPECT_NE(info0, originy_info);
}
TEST(VectorNavGrid, basic_test)
{
nav_grid::VectorNavGrid<int> grid(-3);
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
grid.setInfo(info);
EXPECT_EQ(grid(0, 0), -3);
grid.setValue(1, 1, 10);
EXPECT_EQ(grid(0, 0), -3);
EXPECT_EQ(grid(1, 1), 10);
}
TEST(VectorNavGrid, basic_index_test)
{
nav_grid::VectorNavGrid<int> grid(-3);
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
grid.setInfo(info);
nav_grid::Index index0(0, 0), index1(1, 1);
EXPECT_EQ(grid(index0), -3);
grid.setValue(index1, 10);
EXPECT_EQ(grid(index0), -3);
EXPECT_EQ(grid(index1), 10);
EXPECT_EQ(grid(0, 0), -3);
EXPECT_EQ(grid(1, 1), 10);
}
TEST(VectorNavGrid, easy_coordinates_test)
{
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
double wx, wy;
gridToWorld(info, 0, 0, wx, wy);
EXPECT_DOUBLE_EQ(wx, 0.5);
EXPECT_DOUBLE_EQ(wy, 0.5);
gridToWorld(info, 1, 2, wx, wy);
EXPECT_DOUBLE_EQ(wx, 1.5);
EXPECT_DOUBLE_EQ(wy, 2.5);
unsigned int umx, umy;
int mx, my;
double dmx, dmy;
ASSERT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
worldToGrid(info, wx, wy, dmx, dmy);
EXPECT_DOUBLE_EQ(dmx, wx);
EXPECT_DOUBLE_EQ(dmy, wy);
// Invalid Coordinate
wx = 2.5;
EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 2);
EXPECT_EQ(my, 2);
// Border Cases
EXPECT_TRUE(worldToGridBounded(info, 0.0, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 0.25, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 0.75, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 0.9999, wy, umx, umy));
EXPECT_EQ(umx, 0);
EXPECT_TRUE(worldToGridBounded(info, 1.0, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(worldToGridBounded(info, 1.25, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(worldToGridBounded(info, 1.75, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_TRUE(worldToGridBounded(info, 1.9999, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_FALSE(worldToGridBounded(info, 2.0, wy, umx, umy));
EXPECT_EQ(umx, 1);
}
TEST(VectorNavGrid, hard_coordinates_test)
{
nav_grid::NavGridInfo info;
info.width = 2;
info.height = 3;
info.resolution = 0.1;
info.origin_x = -0.2;
info.origin_y = 0.2;
double wx, wy;
gridToWorld(info, 0, 0, wx, wy);
EXPECT_DOUBLE_EQ(wx, -0.15);
EXPECT_DOUBLE_EQ(wy, 0.25);
gridToWorld(info, 1, 2, wx, wy);
EXPECT_DOUBLE_EQ(wx, -0.05);
EXPECT_DOUBLE_EQ(wy, 0.45);
unsigned int umx, umy;
int mx, my;
double dmx, dmy;
EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 1);
EXPECT_EQ(my, 2);
worldToGrid(info, wx, wy, dmx, dmy);
EXPECT_DOUBLE_EQ(dmx, 1.5);
EXPECT_DOUBLE_EQ(dmy, 2.5);
// Invalid Coordinate
wx = 2.5;
EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, 1);
EXPECT_EQ(umy, 2);
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, 27);
EXPECT_EQ(my, 2);
}
TEST(VectorNavGrid, speed_test)
{
nav_grid::NavGridInfo info;
const int N = 1000;
const int EXTRA = 300;
info.width = N;
info.height = N;
double wx, wy;
unsigned int umx, umy;
int mx, my;
double dmx, dmy;
for (int x = -EXTRA; x < N + EXTRA; x++)
{
for (int y = -EXTRA; y < N + EXTRA; y++)
{
gridToWorld(info, x, y, wx, wy);
if (x < 0 || y < 0 || x >= N || y >= N)
{
EXPECT_FALSE(isWithinGrid(info, wx, wy));
EXPECT_FALSE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, std::min(std::max(0, x), N - 1));
EXPECT_EQ(umy, std::min(std::max(0, y), N - 1));
}
else
{
EXPECT_TRUE(isWithinGrid(info, wx, wy));
EXPECT_TRUE(worldToGridBounded(info, wx, wy, umx, umy));
EXPECT_EQ(umx, x);
EXPECT_EQ(umy, y);
}
worldToGrid(info, wx, wy, mx, my);
EXPECT_EQ(mx, x);
EXPECT_EQ(my, y);
worldToGrid(info, wx, wy, dmx, dmy);
EXPECT_DOUBLE_EQ(dmx, x + 0.5);
EXPECT_DOUBLE_EQ(dmy, y + 0.5);
}
}
}
int testGridValue(double x, double y)
{
return static_cast<int>(100 * floor(x) + floor(y));
}
/**
* Initialize Grid Values with values based on the grid/world coordinates
* which are initially the same
*
* x -->
* 000 100 200 300 400 500 600 700 800 900 y
* 001 101 201 301 401 501 601 701 801 901 |
* 002 102 202 302 402 502 602 702 802 902 |
* 003 103 203 303 403 503 603 703 803 903 V
* 004 104 204 304 404 504 604 704 804 904
*/
void initializeTestGrid(nav_grid::VectorNavGrid<int>& grid)
{
grid.setDefaultValue(-10);
nav_grid::NavGridInfo info;
info.width = 10;
info.height = 5;
grid.setInfo(info);
double mx, my;
for (unsigned int j = 0; j < info.height; j++)
{
for (unsigned int i = 0; i < info.width; i++)
{
gridToWorld(info, i, j, mx, my);
grid.setValue(i, j, testGridValue(mx, my));
}
}
}
/**
* Check to make sure all the grid values are now the same based on their grid coordinates
*/
void checkSetGridValues(const nav_grid::VectorNavGrid<int>& grid,
unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
{
for (unsigned int x = 0; x < grid.getWidth(); x++)
{
for (unsigned int y = 0; y < grid.getHeight(); y++)
{
if (x >= x0 && x < x1 && y >= y0 && y < y1)
{
EXPECT_EQ(grid(x, y), testGridValue(x, y)); // testGridValue based on Grid Coordinates
}
else
{
EXPECT_EQ(grid(x, y), -10);
}
}
}
}
/**
* Check to make sure all the grid values are now the same based on their world coordinates
*/
void checkUpdateGridValues(const nav_grid::VectorNavGrid<int>& grid,
unsigned int x0, unsigned int x1, unsigned int y0, unsigned int y1)
{
double mx, my;
for (unsigned int x = 0; x < grid.getWidth(); x++)
{
for (unsigned int y = 0; y < grid.getHeight(); y++)
{
if (x >= x0 && x < x1 && y >= y0 && y < y1)
{
gridToWorld(grid.getInfo(), x, y, mx, my);
EXPECT_EQ(grid(x, y), testGridValue(mx, my)); // testGridValue based on World Coordinates
}
else
{
EXPECT_EQ(grid(x, y), -10);
}
}
}
}
void debugGridValues(const nav_grid::VectorNavGrid<int>& grid)
{
for (unsigned int j = 0; j < grid.getHeight(); j++)
{
for (unsigned int i = 0; i < grid.getWidth(); i++)
{
printf("%d ", grid(i, j));
}
printf("\n");
}
printf("\n");
}
TEST(VectorNavGrid, resizing_grid_with_set)
{
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
decreased_width_info.width = 5;
grid.setInfo(decreased_width_info);
checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_width_info = grid.getInfo();
increased_width_info.width = 9;
grid.setInfo(increased_width_info);
checkSetGridValues(grid, 0, 5, 0, grid.getHeight());
initializeTestGrid(grid);
checkSetGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_height_info = grid.getInfo();
increased_height_info.height = 9;
grid.setInfo(increased_height_info);
checkSetGridValues(grid, 0, grid.getWidth(), 0, 5);
nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
decreased_height_info.height = 4;
grid.setInfo(decreased_height_info);
checkSetGridValues(grid, 0, grid.getWidth(), 0, 4);
}
TEST(VectorNavGrid, resizing_grid_with_update)
{
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo decreased_width_info = grid.getInfo();
decreased_width_info.width = 5;
grid.updateInfo(decreased_width_info);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_width_info = grid.getInfo();
increased_width_info.width = 9;
grid.updateInfo(increased_width_info);
checkUpdateGridValues(grid, 0, 5, 0, grid.getHeight());
initializeTestGrid(grid);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo increased_height_info = grid.getInfo();
increased_height_info.height = 9;
grid.updateInfo(increased_height_info);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 5);
nav_grid::NavGridInfo decreased_height_info = grid.getInfo();
decreased_height_info.height = 4;
grid.updateInfo(decreased_height_info);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, 4);
}
TEST(VectorNavGrid, change_origin)
{
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
nav_grid::NavGridInfo bump_right_info = grid.getInfo();
bump_right_info.origin_x = 3;
grid.updateInfo(bump_right_info);
checkUpdateGridValues(grid, 0, 7, 0, grid.getHeight());
nav_grid::NavGridInfo bump_up_info = grid.getInfo();
bump_up_info.origin_y = 2;
grid.updateInfo(bump_up_info);
checkUpdateGridValues(grid, 0, 7, 0, 3);
nav_grid::NavGridInfo bump_left_info = grid.getInfo();
bump_left_info.origin_x = -1;
grid.updateInfo(bump_left_info);
checkUpdateGridValues(grid, 4, grid.getWidth(), 0, 3);
nav_grid::NavGridInfo bump_down_info = grid.getInfo();
bump_down_info.origin_y = 0;
grid.updateInfo(bump_down_info);
checkUpdateGridValues(grid, 4, grid.getWidth(), 2, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo bump_far_right_info = grid.getInfo();
bump_far_right_info.origin_x = 30;
grid.updateInfo(bump_far_right_info);
checkUpdateGridValues(grid, 0, 0, 0, 0);
}
TEST(VectorNavGrid, combined_changes)
{
// This is not a complete set of possible combined changes, just enough to satisfy my curiousity
nav_grid::VectorNavGrid<int> grid;
initializeTestGrid(grid);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
nav_grid::NavGridInfo info1 = grid.getInfo();
info1.width = 15;
info1.origin_x = -5.0;
grid.updateInfo(info1);
checkUpdateGridValues(grid, 5, grid.getWidth(), 0, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo info2 = grid.getInfo();
info2.width = 17;
info2.origin_x = -5.0;
grid.updateInfo(info2);
checkUpdateGridValues(grid, 5, grid.getWidth() - 2, 0, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo info3 = grid.getInfo();
info3.width = 2;
info3.origin_x = 2.0;
grid.updateInfo(info3);
checkUpdateGridValues(grid, 0, grid.getWidth(), 0, grid.getHeight());
initializeTestGrid(grid);
nav_grid::NavGridInfo info4 = grid.getInfo();
info4.width = 20;
info4.height = 20;
info4.origin_x = -2.0;
info4.origin_y = -5.0;
grid.updateInfo(info4);
checkUpdateGridValues(grid, 2, 12, 5, 10);
}
TEST(Index, comparison_tests)
{
unsigned int N = 5;
for (unsigned int x0 = 0; x0 < N; ++x0)
{
for (unsigned int y0 = 0; y0 < N; ++y0)
{
nav_grid::Index index0(x0, y0);
for (unsigned int x1 = 0; x1 < N; ++x1)
{
for (unsigned int y1 = 0; y1 < N; ++y1)
{
nav_grid::Index index1(x1, y1);
// Check equality and the test for equality that sets use
// See https://stackoverflow.com/a/1114862
if (x0 == x1 && y0 == y1)
{
EXPECT_EQ(index0, index1);
EXPECT_TRUE(!(index0 < index1) && !(index1 < index0));
EXPECT_GE(index0, index1);
EXPECT_LE(index0, index1);
EXPECT_GE(index1, index0);
EXPECT_LE(index1, index0);
}
else
{
EXPECT_NE(index0, index1);
EXPECT_FALSE(!(index0 < index1) && !(index1 < index0));
if (x0 < x1 || (x0 == x1 && y0 < y1))
{
EXPECT_LT(index0, index1);
EXPECT_GT(index1, index0);
EXPECT_LE(index0, index1);
EXPECT_GE(index1, index0);
}
else
{
EXPECT_GT(index0, index1);
EXPECT_LT(index1, index0);
EXPECT_GE(index0, index1);
EXPECT_LE(index1, index0);
}
}
}
}
}
}
}
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}