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

update ros

Browse Source
This commit is contained in:
HiepLM
2026-01-07 09:28:31 +07:00
parent 11b7c4a20d
commit ca9e100bd9
115 changed files with 1934 additions and 987 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h
View File

@@ -312,10 +312,9 @@ namespace move_base_core
/**
* @brief Get the navigation feedback.
* @param feedback Output parameter with the navigation feedback.
* @return True if feedback was successfully retrieved.
* @return Pointer to the navigation feedback.
*/
virtual bool getFeedback(NavFeedback &feedback) = 0;
virtual NavFeedback *getFeedback() = 0;
protected:
// Shared feedback data for navigation status tracking

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

@@ -1,56 +0,0 @@
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 robot_costmap_2d tf3 robot_protocol_msgs)
# 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 "=================================")

65
src/Navigations/Cores/nav_core2/CMakeLists.txt
View File

@@ -1,65 +0,0 @@
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
robot_costmap_2d
tf3
nav_grid
robot_nav_2d_msgs
robot_cpp
)
# 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 "=================================")

26
src/Navigations/Cores/nav_core2/package.xml
View File

@@ -1,26 +0,0 @@
<package>
<name>nav_core2</name>
<version>0.7.10</version>
<description>
nav_core2 is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. nav_core2
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
point in time.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/nav_core2</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
</package>

115
src/Navigations/Cores/nav_core_adapter/CMakeLists.txt
View File

@@ -1,115 +0,0 @@
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 robot_std_msgs nav_core nav_core2 robot_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
robot_cpp
${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>)
# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs.
# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib,
# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
set(_NAV_CORE_ADAPTER_RPATH
"${CMAKE_BINARY_DIR}/src/Navigations/Cores/nav_core_adapter"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
"${CMAKE_BINARY_DIR}/src/Libraries/tf3"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
)
set_target_properties(costmap_adapter PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
set_target_properties(local_planner_adapter PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
set_target_properties(global_planner_adapter PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
# 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
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
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
View File

@@ -1,48 +0,0 @@
# 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 `robot_costmap_2d::Costmap2DROBOT` 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 `Costmap2DROBOT` object. It is not a perfect adaptation, because
* `Costmap2DROBOT` 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.

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

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

2
src/Navigations/Cores/nav_core/CHANGELOG.rst → src/Navigations/Cores/robot_nav_core/CHANGELOG.rst
View File

@@ -1,5 +1,5 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package nav_core
Changelog for package robot_nav_core
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.17.3 (2023-01-10)

120
src/Navigations/Cores/robot_nav_core/CMakeLists.txt Normal file
View File

@@ -0,0 +1,120 @@
cmake_minimum_required(VERSION 3.10)
# ========================================================
# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
# ========================================================
# Detect if building with Catkin
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building robot_nav_core with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building robot_nav_core with Standalone CMake")
endif()
# Tên dự án
project(robot_nav_core VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# ========================================================
# Find dependencies
# ========================================================
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
robot_costmap_2d
tf3
robot_protocol_msgs
)
endif()
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
# Header-only library; keep LIBRARIES for visibility when exporting
LIBRARIES robot_nav_core
CATKIN_DEPENDS robot_costmap_2d tf3 robot_protocol_msgs
)
endif()
# Cho phép các project khác include được header của robot_nav_core
set(${PROJECT_NAME}_INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/include
PARENT_SCOPE
)
include_directories(
${PROJECT_SOURCE_DIR}/include
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# Tạo INTERFACE library (header-only)
add_library(${PROJECT_NAME} INTERFACE)
# Link libraries
if(BUILDING_WITH_CATKIN)
target_link_libraries(${PROJECT_NAME} INTERFACE
${catkin_LIBRARIES}
)
else()
target_link_libraries(${PROJECT_NAME} INTERFACE
robot_costmap_2d
tf3
robot_protocol_msgs
)
endif()
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
# Set include directories
target_include_directories(${PROJECT_NAME}
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# ========================================================
# Installation
# ========================================================
install(TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/${PROJECT_NAME}
DESTINATION include
FILES_MATCHING PATTERN "*.h")
endif()
# Export targets
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake
DESTINATION lib/cmake/${PROJECT_NAME})
# 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 "=================================")

10
src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h → src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_global_planner.h
View File

@@ -34,15 +34,15 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV_CORE_BASE_GLOBAL_PLANNER_H
#define NAV_CORE_BASE_GLOBAL_PLANNER_H
#ifndef ROBOT_NAV_CORE_BASE_GLOBAL_PLANNER_H
#define ROBOT_NAV_CORE_BASE_GLOBAL_PLANNER_H
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_protocol_msgs/Order.h>
#include <memory>
namespace nav_core {
namespace robot_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.
@@ -107,6 +107,6 @@ namespace nav_core {
protected:
BaseGlobalPlanner(){}
};
} // namespace nav_core
} // namespace robot_nav_core
#endif // NAV_CORE_BASE_GLOBAL_PLANNER_H
#endif // ROBOT_NAV_CORE_BASE_GLOBAL_PLANNER_H

12
src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h → src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h
View File

@@ -34,8 +34,8 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV_CORE_BASE_LOCAL_PLANNER_H
#define NAV_CORE_BASE_LOCAL_PLANNER_H
#ifndef ROBOT_NAV_CORE_BASE_LOCAL_PLANNER_H
#define ROBOT_NAV_CORE_BASE_LOCAL_PLANNER_H
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/Twist.h>
@@ -43,7 +43,7 @@
#include <tf3/buffer_core.h>
#include <memory>
namespace nav_core
namespace robot_nav_core
{
/**
* @class BaseLocalPlanner
@@ -61,7 +61,7 @@ namespace nav_core
virtual bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) = 0;
/**
* @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
* @brief Swap object will be injected interjace robot_nav_core::BaseLocalPlanner
* @param planner_name The object name
* @return True if instance object is successed, False otherwise
*/
@@ -148,6 +148,6 @@ namespace nav_core
protected:
BaseLocalPlanner() {}
};
} // namespace nav_core
} // namespace robot_nav_core
#endif // NAV_CORE_BASE_LOCAL_PLANNER_H
#endif // ROBOT_NAV_CORE_BASE_LOCAL_PLANNER_H

10
src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h → src/Navigations/Cores/robot_nav_core/include/robot_nav_core/parameter_magic.h
View File

@@ -35,10 +35,10 @@
* Author: David Lu
*********************************************************************/
#ifndef NAV_CORE_PARAMETER_MAGIC_H
#define NAV_CORE_PARAMETER_MAGIC_H
#ifndef ROBOT_NAV_CORE_PARAMETER_MAGIC_H
#define ROBOT_NAV_CORE_PARAMETER_MAGIC_H
namespace nav_core
namespace robot_nav_core
{
/**
@@ -81,6 +81,6 @@ void warnRenamedParameter(const std::string current_name, const std::string old_
}
}
} // namespace nav_core
} // namespace robot_nav_core
#endif // NAV_CORE_PARAMETER_MAGIC_H
#endif // ROBOT_NAV_CORE_PARAMETER_MAGIC_H

10
src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h → src/Navigations/Cores/robot_nav_core/include/robot_nav_core/recovery_behavior.h
View File

@@ -34,14 +34,14 @@
*
* Author: Eitan Marder-Eppstein
*********************************************************************/
#ifndef NAV_CORE_RECOVERY_BEHAVIOR_H
#define NAV_CORE_RECOVERY_BEHAVIOR_H
#ifndef ROBOT_NAV_CORE_RECOVERY_BEHAVIOR_H
#define ROBOT_NAV_CORE_RECOVERY_BEHAVIOR_H
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <tf3/buffer_core.h>
#include <memory>
namespace nav_core {
namespace robot_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.
@@ -72,6 +72,6 @@ namespace nav_core {
protected:
RecoveryBehavior(){}
};
} // namespace nav_core
} // namespace robot_nav_core
#endif // NAV_CORE_RECOVERY_BEHAVIOR_H
#endif // ROBOT_NAV_CORE_RECOVERY_BEHAVIOR_H

17
src/Navigations/Cores/nav_core/package.xml → src/Navigations/Cores/robot_nav_core/package.xml
View File

@@ -1,9 +1,9 @@
<package>
<name>nav_core</name>
<name>robot_nav_core</name>
<version>0.7.10</version>
<description>
nav_core is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. nav_core
robot_nav_core is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. robot_nav_core
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
@@ -15,12 +15,17 @@
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/nav_core</url>
<url type="website">http://www.ros.org/wiki/robot_nav_core</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<build_depend>robot_costmap_2d</build_depend>
<run_depend>robot_costmap_2d</run_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<build_depend>tf3</build_depend>
<run_depend>tf3</run_depend>
<build_depend>robot_protocol_msgs</build_depend>
<run_depend>robot_protocol_msgs</run_depend>
</package>

138
src/Navigations/Cores/robot_nav_core2/CMakeLists.txt Executable file
View File

@@ -0,0 +1,138 @@
cmake_minimum_required(VERSION 3.10)
# ========================================================
# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
# ========================================================
# Detect if building with Catkin
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building robot_nav_core2 with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building robot_nav_core2 with Standalone CMake")
endif()
# Tên dự án
project(robot_nav_core2 VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# ========================================================
# Find dependencies
# ========================================================
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
robot_costmap_2d
tf3
nav_grid
robot_nav_2d_msgs
robot_cpp
robot_sensor_msgs
robot_std_msgs
geometry_msgs
robot_nav_msgs
robot_map_msgs
robot_laser_geometry
robot_visualization_msgs
robot_voxel_grid
robot_tf3_geometry_msgs
robot_tf3_sensor_msgs
data_convert
robot_xmlrpcpp
)
endif()
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
# Header-only library; keep LIBRARIES for visibility when exporting
LIBRARIES robot_nav_core2
CATKIN_DEPENDS robot_costmap_2d tf3 nav_grid robot_nav_2d_msgs robot_cpp
)
endif()
# Cho phép các project khác include được header của robot_nav_core2
set(robot_nav_core2_INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/include
PARENT_SCOPE
)
include_directories(
${PROJECT_SOURCE_DIR}/include
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# Tìm tất cả header files
file(GLOB HEADERS "include/robot_nav_core2/*.h")
# Tạo INTERFACE library (header-only)
add_library(${PROJECT_NAME} INTERFACE)
# Link libraries
if(BUILDING_WITH_CATKIN)
target_link_libraries(${PROJECT_NAME} INTERFACE
${catkin_LIBRARIES}
)
else()
target_link_libraries(${PROJECT_NAME} INTERFACE
robot_costmap_2d
tf3
nav_grid
robot_nav_2d_msgs
robot_cpp
)
endif()
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
# Set include directories
target_include_directories(${PROJECT_NAME}
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# ========================================================
# Installation
# ========================================================
# Cài đặt target
install(TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}-targets)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/${PROJECT_NAME}
DESTINATION include
FILES_MATCHING PATTERN "*.h")
endif()
# Export targets
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake
NAMESPACE ${PROJECT_NAME}::
DESTINATION lib/cmake/${PROJECT_NAME})
# 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 "=================================")

20
src/Navigations/Cores/nav_core2/README.md → src/Navigations/Cores/robot_nav_core2/README.md
View File

@@ -1,5 +1,5 @@
# 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.
# robot_nav_core2
A replacement interface for [robot_nav_core2](https://github.com/ros-planning/navigation/tree/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_nav_core2/include/robot_nav_core2) that defines basic two dimensional planner interfaces.
There were a few key reasons for creating new interfaces rather than extending the old ones.
* Use `robot_nav_2d_msgs` to eliminate unused data fields
@@ -13,29 +13,29 @@ There were a few key reasons for creating new interfaces rather than extending t
* Initialization also started an update thread, which is also not always needed in testing.
* Using `Costmap2DROBOT` 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
The [`robot_nav_core2::Costmap`](include/robot_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 `Costmap2DROBOT`.
One basic implementation is provided in [`BasicCostmap`](include/robot_nav_core2/basic_costmap.h). You can also use the `robot_nav_core2_adapter::CostmapAdapter` class which implements the `Costmap` interface while allowing you to still use `Costmap2DROBOT`.
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).
Let us compare the old [robot_nav_core2::BaseGlobalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_nav_core2/include/robot_nav_core2/base_global_planner.h) to the new [robot_nav_core2/GlobalPlanner](include/robot_nav_core2/global_planner.h).
| `nav_core` | `nav_core2` | comments |
| `robot_nav_core2` | `robot_nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, robot_costmap_2d::Costmap2DROBOT*)`|`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 robot_geometry_msgs::PoseStamped&, const robot_geometry_msgs::PoseStamped&, std::vector<robot_geometry_msgs::PoseStamped>&)`|`robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped&, const robot_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).
Now let's compare the old [robot_nav_core2::BaseLocalPlanner](https://github.com/ros-planning/navigation/blob/a2837b5a9dc6dd4b4da176fca7d899d6a3722bf8/robot_nav_core2/include/robot_nav_core2/base_local_planner.h) to the new [robot_nav_core2/LocalPlanner](include/robot_nav_core2/local_planner.h).
| `nav_core` | `nav_core2` | comments |
| `robot_nav_core2` | `robot_nav_core2` | comments |
|---|--|---|
|`void initialize(std::string, tf::TransformListener*, robot_costmap_2d::Costmap2DROBOT*)`|`void initialize(const YAML::Node& parent, const std::string&, TFListenerPtr, Costmap::Ptr)`|Uses modern pointers instead of raw pointers|
|(no equivalent)|`void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped&)`|Explicitly set the new goal location, rather than using the last pose of the global plan`
@@ -44,9 +44,9 @@ Now let's compare the old [nav_core::BaseLocalPlanner](https://github.com/ros-pl
|`bool isGoalReached()` | `bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped&, const robot_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.
A hierarchical collection of [exceptions](include/robot_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).
For use in tracking `Costmap` changes and more, this package also provides an implementation of [bounding boxes](include/robot_nav_core2/bounds.h). These are represented with the ranges `[min_x, max_x]` and `[min_y, max_y]` (inclusive).

0
src/Navigations/Cores/nav_core2/doc/exceptions.png → src/Navigations/Cores/robot_nav_core2/doc/exceptions.png
View File

Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 38 KiB

After

Width:  |  Height:  |  Size: 38 KiB

14
src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h → src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/basic_costmap.h
View File

@@ -32,16 +32,16 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_CORE2_BASIC_COSTMAP_H
#define NAV_CORE2_BASIC_COSTMAP_H
#ifndef ROBOT_NAV_CORE2_BASIC_COSTMAP_H
#define ROBOT_NAV_CORE2_BASIC_COSTMAP_H
#include <nav_core2/costmap.h>
#include <robot_nav_core2/costmap.h>
#include <string>
#include <vector>
namespace nav_core2
namespace robot_nav_core2
{
class BasicCostmap : public nav_core2::Costmap
class BasicCostmap : public robot_nav_core2::Costmap
{
public:
// Standard Costmap Interface
@@ -63,6 +63,6 @@ protected:
mutex_t my_mutex_;
std::vector<unsigned char> data_;
};
} // namespace nav_core2
} // namespace robot_nav_core2
#endif // NAV_CORE2_BASIC_COSTMAP_H
#endif // ROBOT_NAV_CORE2_BASIC_COSTMAP_H

10
src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h → src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/bounds.h
View File

@@ -32,14 +32,14 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_CORE2_BOUNDS_H
#define NAV_CORE2_BOUNDS_H
#ifndef ROBOT_NAV_CORE2_BOUNDS_H
#define ROBOT_NAV_CORE2_BOUNDS_H
#include <algorithm>
#include <limits>
#include <string>
namespace nav_core2
namespace robot_nav_core2
{
/**
* @brief Templatized method for checking if a value falls inside a one-dimensional range
@@ -204,6 +204,6 @@ public:
unsigned int getHeight() const { return getDimension(min_y_, max_y_); }
};
} // namespace nav_core2
} // namespace robot_nav_core2
#endif // NAV_CORE2_BOUNDS_H
#endif // ROBOT_NAV_CORE2_BOUNDS_H

6
src/Navigations/Cores/nav_core2/include/nav_core2/common.h → src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/common.h
View File

@@ -31,12 +31,12 @@
* 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
#ifndef ROBOT_NAV_CORE2_COMMON_H
#define ROBOT_NAV_CORE2_COMMON_H
#include <tf3/buffer_core.h>
#include <memory>
using TFListenerPtr = std::shared_ptr<tf3::BufferCore>;
#endif // NAV_CORE2_COMMON_H
#endif // ROBOT_NAV_CORE2_COMMON_H

14
src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h → src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/costmap.h
View File

@@ -32,18 +32,18 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_CORE2_COSTMAP_H
#define NAV_CORE2_COSTMAP_H
#ifndef ROBOT_NAV_CORE2_COSTMAP_H
#define ROBOT_NAV_CORE2_COSTMAP_H
#include <robot/node_handle.h>
#include <nav_grid/nav_grid.h>
#include <nav_core2/common.h>
#include <nav_core2/bounds.h>
#include <robot_nav_core2/common.h>
#include <robot_nav_core2/bounds.h>
#include <boost/thread.hpp>
#include <memory>
#include <string>
namespace nav_core2
namespace robot_nav_core2
{
class Costmap : public nav_grid::NavGrid<unsigned char>
@@ -152,6 +152,6 @@ public:
return UIntBounds();
}
};
} // namespace nav_core2
} // namespace robot_nav_core2
#endif // NAV_CORE2_COSTMAP_H
#endif // ROBOT_NAV_CORE2_COSTMAP_H

12
src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h → src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/exceptions.h
View File

@@ -31,8 +31,8 @@
* 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
#ifndef ROBOT_NAV_CORE2_EXCEPTIONS_H
#define ROBOT_NAV_CORE2_EXCEPTIONS_H
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <stdexcept>
@@ -41,7 +41,7 @@
#include <memory>
/**************************************************
* The nav_core2 Planning Exception Hierarchy!!
* The robot_nav_core2 Planning Exception Hierarchy!!
* (with arbitrary integer result codes)
**************************************************
* NavCore2Exception
@@ -66,7 +66,7 @@
* -1 Unknown
**************************************************/
namespace nav_core2
namespace robot_nav_core2
{
inline std::string poseToString(const robot_nav_2d_msgs::Pose2DStamped& pose)
@@ -316,6 +316,6 @@ public:
: LocalPlannerException(description, result_code) {}
};
} // namespace nav_core2
} // namespace robot_nav_core2
#endif // NAV_CORE2_EXCEPTIONS_H
#endif // ROBOT_NAV_CORE2_EXCEPTIONS_H

14
src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h → src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h
View File

@@ -31,17 +31,17 @@
* 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
#ifndef ROBOT_NAV_CORE2_GLOBAL_PLANNER_H
#define ROBOT_NAV_CORE2_GLOBAL_PLANNER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <robot_nav_core2/common.h>
#include <robot_nav_core2/costmap.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <string>
#include <memory>
namespace nav_core2
namespace robot_nav_core2
{
/**
@@ -80,6 +80,6 @@ public:
virtual robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
const robot_nav_2d_msgs::Pose2DStamped& goal) = 0;
};
} // namespace nav_core2
} // namespace robot_nav_core2
#endif // NAV_CORE2_GLOBAL_PLANNER_H
#endif // ROBOT_NAV_CORE2_GLOBAL_PLANNER_H

14
src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h → src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h
View File

@@ -31,11 +31,11 @@
* 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
#ifndef ROBOT_NAV_CORE2_LOCAL_PLANNER_H
#define ROBOT_NAV_CORE2_LOCAL_PLANNER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <robot_nav_core2/common.h>
#include <robot_nav_core2/costmap.h>
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_msgs/Twist2D.h>
@@ -45,7 +45,7 @@
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <memory>
namespace nav_core2
namespace robot_nav_core2
{
/**
@@ -168,6 +168,6 @@ namespace nav_core2
*/
virtual bool isGoalReached(const robot_nav_2d_msgs::Pose2DStamped &pose, const robot_nav_2d_msgs::Twist2D &velocity) = 0;
};
} // namespace nav_core2
} // namespace robot_nav_core2
#endif // NAV_CORE2_LOCAL_PLANNER_H
#endif // ROBOT_NAV_CORE2_LOCAL_PLANNER_H

23
src/Navigations/Cores/nav_core_adapter/package.xml → src/Navigations/Cores/robot_nav_core2/package.xml
View File

@@ -1,9 +1,9 @@
<package>
<name>nav_core_adapter</name>
<name>robot_nav_core2</name>
<version>0.7.10</version>
<description>
nav_core_adapter is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. nav_core_adapter
robot_nav_core2 is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. robot_nav_core2
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
@@ -15,12 +15,23 @@
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/nav_core_adapter</url>
<url type="website">http://www.ros.org/wiki/robot_nav_core2</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<build_depend>robot_costmap_2d</build_depend>
<run_depend>robot_costmap_2d</run_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<build_depend>tf3</build_depend>
<run_depend>tf3</run_depend>
<build_depend>nav_grid</build_depend>
<run_depend>nav_grid</run_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend>
</package>

6
src/Navigations/Cores/nav_core2/src/basic_costmap.cpp → src/Navigations/Cores/robot_nav_core2/src/basic_costmap.cpp
View File

@@ -32,9 +32,9 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <nav_core2/basic_costmap.h>
#include <robot_nav_core2/basic_costmap.h>
namespace nav_core2
namespace robot_nav_core2
{
void BasicCostmap::reset()
@@ -57,4 +57,4 @@ void BasicCostmap::setValue(const unsigned int x, const unsigned int y, const un
data_[getIndex(x, y)] = value;
}
} // namespace nav_core2
} // namespace robot_nav_core2

6
src/Navigations/Cores/nav_core2/test/bounds_test.cpp → src/Navigations/Cores/robot_nav_core2/test/bounds_test.cpp
View File

@@ -32,10 +32,10 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <nav_core2/bounds.h>
#include <robot_nav_core2/bounds.h>
using nav_core2::Bounds;
using nav_core2::UIntBounds;
using robot_nav_core2::Bounds;
using robot_nav_core2::UIntBounds;
TEST(Bounds, test_bounds_simple)
{

34
src/Navigations/Cores/nav_core2/test/exception_test.cpp → src/Navigations/Cores/robot_nav_core2/test/exception_test.cpp
View File

@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <string>
TEST(Exceptions, direct_code_access)
@@ -41,18 +41,18 @@ TEST(Exceptions, direct_code_access)
// (This version does not create any copies of the exception)
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
}
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::PlannerException& x)
catch (robot_nav_core2::PlannerException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
}
@@ -62,12 +62,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("");
robot_nav_core2::PlannerException e("");
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
{
e = x;
}
@@ -79,28 +79,28 @@ 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;
robot_nav_core2::NavCore2ExceptionPtr e;
try
{
throw nav_core2::GlobalPlannerTimeoutException("test case");
throw robot_nav_core2::GlobalPlannerTimeoutException("test case");
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
{
e = std::current_exception();
}
EXPECT_EQ(nav_core2::getResultCode(e), 12);
EXPECT_EQ(robot_nav_core2::getResultCode(e), 12);
try
{
std::rethrow_exception(e);
}
catch (nav_core2::GlobalPlannerTimeoutException& x)
catch (robot_nav_core2::GlobalPlannerTimeoutException& x)
{
EXPECT_EQ(x.getResultCode(), 12);
SUCCEED();
}
catch (nav_core2::PlannerException& x)
catch (robot_nav_core2::PlannerException& x)
{
FAIL() << "PlannerException caught instead of specific exception";
}
@@ -108,10 +108,10 @@ TEST(Exceptions, rethrow)
TEST(Exceptions, weird_exception)
{
nav_core2::NavCore2ExceptionPtr e;
robot_nav_core2::NavCore2ExceptionPtr e;
// Check what happens with no exception
EXPECT_EQ(nav_core2::getResultCode(e), -1);
EXPECT_EQ(robot_nav_core2::getResultCode(e), -1);
// Check what happens with a non NavCore2Exception
try
@@ -123,7 +123,7 @@ TEST(Exceptions, weird_exception)
e = std::current_exception();
}
EXPECT_EQ(nav_core2::getResultCode(e), -1);
EXPECT_EQ(robot_nav_core2::getResultCode(e), -1);
}
int main(int argc, char **argv)

212
src/Navigations/Cores/robot_nav_core_adapter/CMakeLists.txt Executable file
View File

@@ -0,0 +1,212 @@
cmake_minimum_required(VERSION 3.10)
# ========================================================
# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
# ========================================================
# Detect if building with Catkin
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building robot_nav_core_adapter with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building robot_nav_core_adapter with Standalone CMake")
endif()
# Tên dự án
project(robot_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()
# ========================================================
# Find dependencies
# ========================================================
# Find system dependencies
find_package(Boost REQUIRED COMPONENTS filesystem system)
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
robot_std_msgs
robot_nav_core
robot_nav_core2
robot_nav_2d_utils
robot_cpp
)
endif()
# Dependencies packages (internal libraries)
set(PACKAGES_DIR geometry_msgs robot_std_msgs robot_nav_core robot_nav_core2 robot_nav_2d_utils pthread)
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
LIBRARIES costmap_adapter local_planner_adapter global_planner_adapter
CATKIN_DEPENDS geometry_msgs robot_std_msgs robot_nav_core robot_nav_core2 robot_nav_2d_utils robot_cpp
)
endif()
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# ========================================================
# Build
# ========================================================
# Tạo thư viện shared (.so)
add_library(costmap_adapter src/costmap_adapter.cpp)
# Link libraries
if(BUILDING_WITH_CATKIN)
target_link_libraries(costmap_adapter
PRIVATE ${catkin_LIBRARIES}
)
else()
target_link_libraries(costmap_adapter
PRIVATE ${PACKAGES_DIR}
)
endif()
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)
if(BUILDING_WITH_CATKIN)
target_link_libraries(local_planner_adapter
PRIVATE
Boost::filesystem
Boost::system
dl
costmap_adapter
${catkin_LIBRARIES}
)
else()
target_link_libraries(local_planner_adapter
PRIVATE
Boost::filesystem
Boost::system
dl
costmap_adapter
robot_cpp
${PACKAGES_DIR}
)
endif()
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)
if(BUILDING_WITH_CATKIN)
target_link_libraries(global_planner_adapter
PRIVATE
Boost::filesystem
Boost::system
dl
costmap_adapter
${catkin_LIBRARIES}
)
else()
target_link_libraries(global_planner_adapter
PRIVATE
Boost::filesystem
Boost::system
dl
costmap_adapter
${PACKAGES_DIR}
)
endif()
if(BUILDING_WITH_CATKIN)
add_dependencies(costmap_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(local_planner_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(global_planner_adapter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
target_include_directories(global_planner_adapter PRIVATE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
# Ensure runtime loader prefers in-tree adapter libs over system/ROS libs.
# Without this, dlopen() may resolve libcostmap_adapter.so from /opt/ros/noetic/lib,
# which has a different ABI (Costmap2DROBOT) and causes missing symbols.
set(_ROBOT_NAV_CORE_ADAPTER_RPATH
"${CMAKE_BINARY_DIR}/src/Navigations/Cores/robot_nav_core_adapter"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_costmap_2d"
"${CMAKE_BINARY_DIR}/src/Libraries/tf3"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_time"
"${CMAKE_BINARY_DIR}/src/Libraries/robot_cpp"
)
set_target_properties(costmap_adapter PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
set_target_properties(local_planner_adapter PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
set_target_properties(global_planner_adapter PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
# ========================================================
# Installation
# ========================================================
# Cài đặt library
install(TARGETS global_planner_adapter local_planner_adapter costmap_adapter
EXPORT robot_nav_core_adapter-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/robot_nav_core_adapter
DESTINATION include
FILES_MATCHING PATTERN "*.h")
endif()
# Export targets
install(EXPORT robot_nav_core_adapter-targets
FILE robot_nav_core_adapter-targets.cmake
DESTINATION lib/cmake/robot_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
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -w") # -w suppresses all warnings
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/robot_nav_core_adapter/README.md Executable file
View File

@@ -0,0 +1,48 @@
# robot_nav_core_adapter
This package contains adapters for using `robot_nav_core2` plugins as `robot_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 `robot_costmap_2d::Costmap2DROBOT` as a plugin for the `robot_nav_core2::Costmap` interface.
## Adapter Classes
* Global Planner Adapters
* `GlobalPlannerAdapter` is used for employing a `robot_nav_core2` global planner interface (such as `dlux_global_planner`) as a `robot_nav_core2` plugin, like in `move_base`.
* `GlobalPlannerAdapter2` is used for employing a `robot_nav_core2` global planner interface (such as `navfn`)
as a `robot_nav_core2` plugin, like in `locomotor`.
* Local Planner Adapter
* `LocalPlannerAdapter` is used for employing a `robot_nav_core2` local planner interface (such as `dwb_local_planner`) as a `robot_nav_core2` 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 `robot_nav_core2` interfaces use additional information (like velocity) in the `local_planner` interface than its `robot_nav_core2` counterpart. This information would be ignored by a `robot_nav_core2` planner, so no adapter is provided.
* `CostmapAdapter` provides most of the functionality from `robot_nav_core2::Costmap` and also provides a raw pointer to the `Costmap2DROBOT` object. It is not a perfect adaptation, because
* `Costmap2DROBOT` 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: robot_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.

4
src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg → src/Navigations/Cores/robot_nav_core_adapter/cfg/NavCoreAdapter.cfg
View File

@@ -1,9 +1,9 @@
#!/usr/bin/env python
PACKAGE = 'nav_core_adapter'
PACKAGE = 'robot_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"))
exit(gen.generate(PACKAGE, "robot_nav_core_adapter", "NavCoreAdapter"))

18
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h → src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/costmap_adapter.h
View File

@@ -32,19 +32,19 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
#define NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
#ifndef ROBOT_NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
#define ROBOT_NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
#include <nav_core2/common.h>
#include <nav_core2/costmap.h>
#include <robot_nav_core2/common.h>
#include <robot_nav_core2/costmap.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <string>
namespace nav_core_adapter
namespace robot_nav_core_adapter
{
nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot);
class CostmapAdapter : public nav_core2::Costmap
class CostmapAdapter : public robot_nav_core2::Costmap
{
public:
/**
@@ -61,7 +61,7 @@ public:
// Standard Costmap Interface
void initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) override;
nav_core2::Costmap::mutex_t* getMutex() override;
robot_nav_core2::Costmap::mutex_t* getMutex() override;
// NavGrid Interface
void reset() override;
@@ -80,6 +80,6 @@ protected:
bool needs_destruction_;
};
} // namespace nav_core_adapter
} // namespace robot_nav_core_adapter
#endif // NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H
#endif // ROBOT_NAV_CORE_ADAPTER_COSTMAP_ADAPTER_H

29
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h → src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/global_planner_adapter.h
View File

@@ -32,45 +32,44 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
#define NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
#ifndef ROBOT_NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
#define ROBOT_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 <robot_nav_core/base_global_planner.h>
#include <robot_nav_core2/global_planner.h>
#include <boost/dll/import.hpp>
#include <string>
#include <vector>
namespace nav_core_adapter
namespace robot_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`.
* @brief used for employing a `robot_nav_core2` global planner (such as `navfn`) as a `robot_nav_core2` plugin, like in `locomotor`.
*/
class GlobalPlannerAdapter: public nav_core2::GlobalPlanner
class GlobalPlannerAdapter: public robot_nav_core2::GlobalPlanner
{
public:
GlobalPlannerAdapter();
// Nav Core 2 Global Planner Interface
void initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
robot_nav_2d_msgs::Path2D makePlan(const robot_nav_2d_msgs::Pose2DStamped& start,
const robot_nav_2d_msgs::Pose2DStamped& goal) override;
static nav_core2::GlobalPlanner::Ptr create();
static robot_nav_core2::GlobalPlanner::Ptr create();
protected:
// Plugin handling
boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
nav_core::BaseGlobalPlanner::Ptr planner_;
boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
robot_nav_core::BaseGlobalPlanner::Ptr planner_;
robot_costmap_2d::Costmap2DROBOT* costmap_robot_;
nav_core2::Costmap::Ptr costmap_;
robot_nav_core2::Costmap::Ptr costmap_;
};
} // namespace nav_core_adapter
} // namespace robot_nav_core_adapter
#endif // NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H
#endif // ROBOT_NAV_CORE_ADAPTER_GLOBAL_PLANNER_ADAPTER2_H

32
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h → src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h
View File

@@ -32,20 +32,20 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#ifndef ROBOT_NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#define ROBOT_NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#include <robot/node_handle.h>
#include <nav_core/base_local_planner.h>
#include <nav_core2/local_planner.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <robot_nav_core/base_local_planner.h>
#include <robot_nav_core2/local_planner.h>
#include <robot_nav_core_adapter/costmap_adapter.h>
#include <robot_nav_2d_utils/odom_subscriber.h>
#include <boost/dll/import.hpp>
#include <memory>
#include <string>
#include <vector>
namespace nav_core_adapter
namespace robot_nav_core_adapter
{
struct NavCoreAdapterConfig
{
@@ -58,9 +58,9 @@ namespace nav_core_adapter
/**
* @class LocalPlannerAdapter
* @brief used for employing a nav_core2 local planner (such as dwb) as a nav_core plugin, like in move_base.
* @brief used for employing a robot_nav_core2 local planner (such as dwb) as a robot_nav_core2 plugin, like in move_base.
*/
class LocalPlannerAdapter : public nav_core::BaseLocalPlanner
class LocalPlannerAdapter : public robot_nav_core::BaseLocalPlanner
{
public:
@@ -87,7 +87,7 @@ namespace nav_core_adapter
bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) override;
/**
* @brief Swap object will be injected interjace nav_core::BaseLocalPlanner
* @brief Swap object will be injected interjace robot_nav_core2::BaseLocalPlanner
* @param planner_name The object name
* @return True if instance object is successed, False otherwise
*/
@@ -162,7 +162,7 @@ namespace nav_core_adapter
* @brief Create a new LocalPlannerAdapter
* @return A shared pointer to the new LocalPlannerAdapter
*/
static nav_core::BaseLocalPlanner::Ptr create();
static robot_nav_core::BaseLocalPlanner::Ptr create();
protected:
/**
@@ -186,8 +186,8 @@ namespace nav_core_adapter
std::shared_ptr<robot_nav_2d_utils::OdomSubscriber> odom_sub_;
// Plugin handling
boost::function<nav_core2::LocalPlanner::Ptr()> planner_loader_;
nav_core2::LocalPlanner::Ptr planner_;
boost::function<robot_nav_core2::LocalPlanner::Ptr()> planner_loader_;
robot_nav_core2::LocalPlanner::Ptr planner_;
// Pointer Copies
TFListenerPtr tf_;
@@ -200,11 +200,11 @@ namespace nav_core_adapter
robot::NodeHandle private_nh_;
robot::NodeHandle adapter_nh_;
nav_core_adapter::NavCoreAdapterConfig last_config_;
nav_core_adapter::NavCoreAdapterConfig default_config_;
robot_nav_core_adapter::NavCoreAdapterConfig last_config_;
robot_nav_core_adapter::NavCoreAdapterConfig default_config_;
bool setup_;
};
} // namespace nav_core_adapter
} // namespace robot_nav_core_adapter
#endif // NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H
#endif // ROBOT_NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H

14
src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h → src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/shared_pointers.h
View File

@@ -32,13 +32,13 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef NAV_CORE_ADAPTER_SHARED_POINTERS_H
#define NAV_CORE_ADAPTER_SHARED_POINTERS_H
#ifndef ROBOT_NAV_CORE_ADAPTER_SHARED_POINTERS_H
#define ROBOT_NAV_CORE_ADAPTER_SHARED_POINTERS_H
#include <nav_core2/common.h>
#include <robot_nav_core2/common.h>
#include <memory>
namespace nav_core_adapter
namespace robot_nav_core_adapter
{
template <typename T>
@@ -47,7 +47,7 @@ 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
* @note This is considered bad form, and is only done here for backwards compatibility purposes. The robot_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
@@ -62,6 +62,6 @@ std::shared_ptr<T> createSharedPointerWithNoDelete(T* raw_ptr)
return std::shared_ptr<T>(raw_ptr, null_deleter<T>);
}
} // namespace nav_core_adapter
} // namespace robot_nav_core_adapter
#endif // NAV_CORE_ADAPTER_SHARED_POINTERS_H
#endif // ROBOT_NAV_CORE_ADAPTER_SHARED_POINTERS_H

40
src/Navigations/Cores/robot_nav_core_adapter/package.xml Normal file
View File

@@ -0,0 +1,40 @@
<package>
<name>robot_nav_core_adapter</name>
<version>0.7.10</version>
<description>
robot_nav_core_adapter is the second generation of the transform library, which lets
the user keep track of multiple coordinate frames over time. robot_nav_core_adapter
maintains the relationship between coordinate frames in a tree
structure buffered in time, and lets the user transform points,
vectors, etc between any two coordinate frames at any desired
point in time.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/robot_nav_core_adapter</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>robot_nav_core</build_depend>
<run_depend>robot_nav_core</run_depend>
<build_depend>robot_nav_core2</build_depend>
<run_depend>robot_nav_core2</run_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend>
</package>

18
src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp → src/Navigations/Cores/robot_nav_core_adapter/src/costmap_adapter.cpp
View File

@@ -32,15 +32,15 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_core2/exceptions.h>
#include <robot_nav_core_adapter/costmap_adapter.h>
#include <robot_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)
// PLUGINLIB_EXPORT_CLASS(robot_nav_core_adapter::CostmapAdapter, robot_nav_core2::Costmap)
namespace nav_core_adapter
namespace robot_nav_core_adapter
{
nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot)
@@ -75,10 +75,10 @@ void CostmapAdapter::initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot,
void CostmapAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf)
{
// TODO: Implement this if needed
throw nav_core2::CostmapException("initialize with robot::NodeHandle not implemented");
throw robot_nav_core2::CostmapException("initialize with robot::NodeHandle not implemented");
}
nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()
robot_nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex()
{
return costmap_->getMutex();
}
@@ -92,7 +92,7 @@ void CostmapAdapter::update()
{
info_ = infoFromCostmap(costmap_robot_);
if (!costmap_robot_->isCurrent())
throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow.");
}
void CostmapAdapter::setValue(const unsigned int x, const unsigned int y, const unsigned char& value)
@@ -108,7 +108,7 @@ unsigned char CostmapAdapter::getValue(const unsigned int x, const unsigned int
void CostmapAdapter::setInfo(const nav_grid::NavGridInfo& new_info)
{
throw nav_core2::CostmapException("setInfo not implemented on CostmapAdapter");
throw robot_nav_core2::CostmapException("setInfo not implemented on CostmapAdapter");
}
void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info)
@@ -116,4 +116,4 @@ void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info)
costmap_->updateOrigin(new_info.origin_x, new_info.origin_y);
}
} // namespace nav_core_adapter
} // namespace robot_nav_core_adapter

24
src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp → src/Navigations/Cores/robot_nav_core_adapter/src/global_planner_adapter.cpp
View File

@@ -32,29 +32,29 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <nav_core_adapter/global_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <robot_nav_core_adapter/global_planner_adapter.h>
#include <robot_nav_core_adapter/costmap_adapter.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <boost/dll/alias.hpp>
#include <memory>
#include <string>
#include <vector>
namespace nav_core_adapter
namespace robot_nav_core_adapter
{
GlobalPlannerAdapter::GlobalPlannerAdapter()
// : planner_loader_("nav_core", "nav_core::BaseGlobalPlanner")
// : planner_loader_("robot_nav_core2", "robot_nav_core2::BaseGlobalPlanner")
{
}
/**
* @brief Load the nav_core global planner and initialize it
* @brief Load the robot_nav_core2 global planner and initialize it
*/
void GlobalPlannerAdapter::initialize(const robot::NodeHandle& parent, const std::string& name,
TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
{
costmap_ = costmap;
std::shared_ptr<CostmapAdapter> ptr = std::static_pointer_cast<CostmapAdapter>(costmap);
@@ -72,7 +72,7 @@ namespace nav_core_adapter
// 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()>(
planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
planner_->initialize(planner_name, costmap_robot_);
@@ -87,16 +87,16 @@ namespace nav_core_adapter
bool ret = planner_->makePlan(start3d, goal3d, plan);
if (!ret)
{
throw nav_core2::PlannerException("Generic Global Planner Error");
throw robot_nav_core2::PlannerException("Generic Global Planner Error");
}
return robot_nav_2d_utils::posesToPath2D(plan);
}
nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create()
robot_nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create()
{
return std::make_shared<GlobalPlannerAdapter>();
}
} // namespace nav_core_adapter
} // namespace robot_nav_core_adapter
// register this planner as a GlobalPlanner plugin
BOOST_DLL_ALIAS(nav_core_adapter::GlobalPlannerAdapter::create, create_plugin)
BOOST_DLL_ALIAS(robot_nav_core_adapter::GlobalPlannerAdapter::create, create_plugin)

32
src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp → src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp
View File

@@ -33,20 +33,20 @@
*/
#include <robot/console.h>
#include <nav_core_adapter/local_planner_adapter.h>
#include <nav_core_adapter/costmap_adapter.h>
#include <nav_core_adapter/shared_pointers.h>
#include <robot_nav_core_adapter/local_planner_adapter.h>
#include <robot_nav_core_adapter/costmap_adapter.h>
#include <robot_nav_core_adapter/shared_pointers.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/parameters.h>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <boost/dll/alias.hpp>
#include <boost/dll/import.hpp>
#include <memory>
#include <string>
#include <vector>
namespace nav_core_adapter
namespace robot_nav_core_adapter
{
LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false)
@@ -71,7 +71,7 @@ namespace nav_core_adapter
}
/**
* @brief Load the nav_core2 local planner and initialize it
* @brief Load the robot_nav_core2 local planner and initialize it
*/
void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot)
{
@@ -97,7 +97,7 @@ namespace nav_core_adapter
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(planner_name);
planner_loader_ = boost::dll::import_alias<nav_core2::LocalPlanner::Ptr()>(
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
if (!planner_)
@@ -131,13 +131,13 @@ namespace nav_core_adapter
if (planner_name != last_config_.planner_name)
{
nav_core2::LocalPlanner::Ptr new_planner;
robot_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()>(
planner_loader_ = boost::dll::import_alias<robot_nav_core2::LocalPlanner::Ptr()>(
path_file_so, planner_name, boost::dll::load_mode::append_decorations);
new_planner = planner_loader_();
if (!new_planner)
@@ -163,7 +163,7 @@ namespace nav_core_adapter
}
/**
* @brief Collect the additional information needed by nav_core2 and call the child computeVelocityCommands
* @brief Collect the additional information needed by robot_nav_core2 and call the child computeVelocityCommands
*/
bool LocalPlannerAdapter::computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel)
{
@@ -187,7 +187,7 @@ namespace nav_core_adapter
{
cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity);
}
catch (const nav_core2::PlannerException &e)
catch (const robot_nav_core2::PlannerException &e)
{
std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl;
return false;
@@ -320,7 +320,7 @@ namespace nav_core_adapter
}
/**
* @brief Collect the additional information needed by nav_core2 and call the child isGoalReached
* @brief Collect the additional information needed by robot_nav_core2 and call the child isGoalReached
*/
bool LocalPlannerAdapter::isGoalReached()
{
@@ -363,7 +363,7 @@ namespace nav_core_adapter
}
return true;
}
catch (const nav_core2::PlannerException &e)
catch (const robot_nav_core2::PlannerException &e)
{
std::cerr << "setPlan Exception: " << e.what() << std::endl;
return false;
@@ -394,11 +394,11 @@ namespace nav_core_adapter
return true;
}
nav_core::BaseLocalPlanner::Ptr LocalPlannerAdapter::create()
robot_nav_core::BaseLocalPlanner::Ptr LocalPlannerAdapter::create()
{
return std::make_shared<LocalPlannerAdapter>();
}
} // namespace nav_core_adapter
} // namespace robot_nav_core_adapter
// register this planner as a BaseLocalPlanner plugin
BOOST_DLL_ALIAS(nav_core_adapter::LocalPlannerAdapter::create, LocalPlannerAdapter)
BOOST_DLL_ALIAS(robot_nav_core_adapter::LocalPlannerAdapter::create, LocalPlannerAdapter)

4
src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp → src/Navigations/Cores/robot_nav_core_adapter/test/unload_test.cpp
View File

@@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <gtest/gtest.h>
#include <nav_core_adapter/local_planner_adapter.h>
#include <robot_nav_core_adapter/local_planner_adapter.h>
TEST(LocalPlannerAdapter, unload_local_planner)
@@ -49,7 +49,7 @@ TEST(LocalPlannerAdapter, unload_local_planner)
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();
robot_nav_core_adapter::LocalPlannerAdapter* lpa = new robot_nav_core_adapter::LocalPlannerAdapter();
robot_costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf);
lpa->initialize("lpa", &tf, &costmap_robot);

3
src/Navigations/Cores/robot_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="robot_nav_core_adapter" type="unload_test" />
</launch>

44
src/Navigations/Libraries/nav_grid/CMakeLists.txt
View File

@@ -1,5 +1,18 @@
cmake_minimum_required(VERSION 3.10)
# ========================================================
# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake
# ========================================================
# Detect if building with Catkin
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building nav_grid with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building nav_grid with Standalone CMake")
endif()
project(nav_grid VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
@@ -7,6 +20,25 @@ set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# ========================================================
# Find dependencies
# ========================================================
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED)
endif()
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
LIBRARIES nav_grid
)
endif()
# Tìm tất cả header files
file(GLOB_RECURSE HEADERS "include/nav_grid/*.h")
@@ -20,10 +52,16 @@ target_include_directories(nav_grid
$<INSTALL_INTERFACE:include>
)
if(BUILDING_WITH_CATKIN)
add_dependencies(nav_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
# Cài đặt header files
install(DIRECTORY include/nav_grid
DESTINATION include
FILES_MATCHING PATTERN "*.h")
if(NOT BUILDING_WITH_CATKIN)
install(DIRECTORY include/nav_grid
DESTINATION include
FILES_MATCHING PATTERN "*.h")
endif()
# Cài đặt target
install(TARGETS nav_grid

4
src/Navigations/Libraries/nav_grid/package.xml
View File

@@ -19,8 +19,4 @@
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
</package>

16
src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md
View File

@@ -6,7 +6,7 @@ Package `move_base` có thể được build bằng cả **Catkin** và **Standa
### 1. Build với Catkin (ROS Workspace)
**⚠️ QUAN TRỌNG**: Thư mục gốc của project (`pnkx_nav_core`) có `CMakeLists.txt` ở root,
**⚠️ QUAN TRỌNG**: Thư mục gốc của project (`pnkx_robot_nav_core`) có `CMakeLists.txt` ở root,
nên **KHÔNG THỂ** chạy `catkin_make` trực tiếp trong thư mục đó. Catkin workspace không được
phép có `CMakeLists.txt` ở root.
@@ -16,7 +16,7 @@ phép có `CMakeLists.txt` ở root.
```bash
# Chạy script setup từ thư mục gốc của project
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core
cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core
./setup_catkin_workspace.sh
# Script sẽ tạo workspace và link các packages có package.xml
@@ -34,11 +34,11 @@ mkdir -p ~/pnkx_nav_catkin_ws/src
cd ~/pnkx_nav_catkin_ws/src
# Link package move_base vào workspace
ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Navigations/Packages/move_base move_base
ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core/src/Navigations/Packages/move_base move_base
# Link các dependencies cần thiết (nếu có package.xml)
ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/tf3 tf3
ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/nav_2d_utils nav_2d_utils
ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core/src/Libraries/tf3 tf3
ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core/src/Libraries/nav_2d_utils nav_2d_utils
# Build với catkin
cd ~/pnkx_nav_catkin_ws
@@ -63,7 +63,7 @@ make
**Lưu ý**: Khi build standalone, bạn cần đảm bảo:
- Tất cả dependencies đã được build trước
- CMake có thể tìm thấy các internal packages (move_base_core, nav_core, etc.)
- CMake có thể tìm thấy các internal packages (move_base_core, robot_nav_core, etc.)
- ROS packages (nếu cần) được cài đặt và có trong CMAKE_PREFIX_PATH
## Cách CMakeLists.txt Detect Build Mode
@@ -85,7 +85,7 @@ Ngược lại, sẽ build ở **Standalone CMake mode**.
### Internal Packages (cần build trước):
- move_base_core
- nav_core
- robot_nav_core
- robot_costmap_2d
- xmlrpcpp
- node_handle
@@ -136,7 +136,7 @@ Building move_base with Standalone CMake
- Source ROS setup: `source /opt/ros/noetic/setup.bash`
- Kiểm tra `ROS_PACKAGE_PATH` environment variable
### Lỗi: Internal packages không tìm thấy (move_base_core, nav_core, etc.)
### Lỗi: Internal packages không tìm thấy (move_base_core, robot_nav_core, etc.)
- **Với Catkin**: Các internal packages cần có `package.xml` và được link vào workspace
- **Với Standalone**: Đảm bảo các packages đã được build trước trong project chính

115
src/Navigations/Packages/move_base/CMakeLists.txt
View File

@@ -27,7 +27,22 @@ endif()
# ========================================================
# Find Packages
# ========================================================
find_package(Boost REQUIRED)
find_package(Boost REQUIRED COMPONENTS filesystem system)
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
robot_std_msgs
move_base_core
robot_nav_core
robot_costmap_2d
robot_tf3_sensor_msgs
robot_tf3_geometry_msgs
data_convert
robot_nav_2d_utils
robot_cpp
)
endif()
# ========================================================
@@ -36,6 +51,18 @@ find_package(Boost REQUIRED)
file(GLOB SOURCES "src/move_base.cpp")
file(GLOB HEADERS "include/move_base/move_base.h")
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
LIBRARIES move_base
CATKIN_DEPENDS geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp
)
endif()
# ========================================================
# Include Directories
# ========================================================
@@ -43,6 +70,10 @@ include_directories(
${PROJECT_SOURCE_DIR}/include
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# ========================================================
# RPATH settings: ưu tiên thư viện build tại chỗ
@@ -62,34 +93,45 @@ add_library(move_base SHARED ${SOURCES} ${HEADERS})
# Dependencies and Link Libraries
# ========================================================
# Standalone CMake mode: link all dependencies manually
set(PACKAGES_DIR
geometry_msgs
robot_std_msgs
move_base_core
nav_core
robot_costmap_2d
plugins # Link với plugins library để có StaticLayer typeinfo
yaml-cpp
robot_tf3_sensor_msgs
robot_tf3_geometry_msgs
data_convert
dl
pthread
robot_nav_2d_utils
)
if(BUILDING_WITH_CATKIN)
target_link_libraries(move_base
PUBLIC ${catkin_LIBRARIES}
PRIVATE Boost::filesystem Boost::system
)
else()
# Standalone CMake mode: link all dependencies manually
set(PACKAGES_DIR
geometry_msgs
robot_std_msgs
move_base_core
robot_nav_core
robot_costmap_2d
plugins # Link với plugins library để có StaticLayer typeinfo
yaml-cpp
robot_tf3_sensor_msgs
robot_tf3_geometry_msgs
data_convert
dl
pthread
robot_nav_2d_utils
)
target_link_libraries(move_base
PUBLIC ${PACKAGES_DIR}
PUBLIC robot_cpp
PRIVATE Boost::boost
)
target_link_libraries(move_base
PUBLIC ${PACKAGES_DIR}
PUBLIC robot_cpp
PRIVATE Boost::filesystem Boost::system
)
endif()
# Set RPATH để ưu tiên thư viện build cục bộ
set_target_properties(move_base PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
if(BUILDING_WITH_CATKIN)
add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
# ========================================================
# Include Directories for Target
@@ -100,11 +142,6 @@ target_include_directories(move_base
$<INSTALL_INTERFACE:include>
)
if(BUILDING_WITH_CATKIN)
target_include_directories(move_base
PUBLIC ${catkin_INCLUDE_DIRS}
)
endif()
# ========================================================
# Executable
@@ -112,7 +149,7 @@ endif()
add_executable(move_base_main src/move_base_main.cpp)
target_link_libraries(move_base_main
PRIVATE move_base robot_cpp
PRIVATE Boost::boost
PRIVATE Boost::filesystem Boost::system
)
# Set RPATH for executable to find libraries in build directory first
@@ -125,11 +162,13 @@ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--disable-new-dtags")
# Installation
# ========================================================
# Standalone CMake installation
install(DIRECTORY include/move_base
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
if(NOT BUILDING_WITH_CATKIN)
# Standalone CMake installation
install(DIRECTORY include/move_base
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
endif()
install(TARGETS move_base move_base_main
EXPORT move_base-targets
@@ -138,10 +177,12 @@ install(TARGETS move_base move_base_main
RUNTIME DESTINATION bin
)
install(EXPORT move_base-targets
FILE move_base-targets.cmake
DESTINATION lib/cmake/move_base
)
if(NOT BUILDING_WITH_CATKIN)
install(EXPORT move_base-targets
FILE move_base-targets.cmake
DESTINATION lib/cmake/move_base
)
endif()
# ========================================================

23
src/Navigations/Packages/move_base/include/move_base/move_base.h
View File

@@ -15,9 +15,9 @@
// interfaces headers
#include <move_base_core/navigation.h>
#include <nav_core/base_global_planner.h>
#include <nav_core/base_local_planner.h>
#include <nav_core/recovery_behavior.h>
#include <robot_nav_core/base_global_planner.h>
#include <robot_nav_core/base_local_planner.h>
#include <robot_nav_core/recovery_behavior.h>
// boost headers
#include <boost/thread.hpp>
@@ -206,10 +206,9 @@ namespace move_base
/**
* @brief Get the navigation feedback.
* @param feedback Output parameter with the navigation feedback.
* @return True if feedback was successfully retrieved.
* @return Pointer to the navigation feedback.
*/
virtual bool getFeedback(robot::move_base_core::NavFeedback &feedback) override;
virtual robot::move_base_core::NavFeedback *getFeedback() override;
/**
* @brief Destructor - Cleans up
@@ -309,13 +308,13 @@ namespace move_base
robot::TFListenerPtr tf_;
robot::NodeHandle private_nh_;
boost::function<nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
boost::function<nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
boost::function<nav_core::RecoveryBehavior::Ptr()> recovery_loader_;
boost::function<robot_nav_core::BaseGlobalPlanner::Ptr()> planner_loader_;
boost::function<robot_nav_core::BaseLocalPlanner::Ptr()> controller_loader_;
boost::function<robot_nav_core::RecoveryBehavior::Ptr()> recovery_loader_;
nav_core::BaseLocalPlanner::Ptr tc_;
nav_core::BaseGlobalPlanner::Ptr planner_;
std::vector<nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
robot_nav_core::BaseLocalPlanner::Ptr tc_;
robot_nav_core::BaseGlobalPlanner::Ptr planner_;
std::vector<robot_nav_core::RecoveryBehavior::Ptr> recovery_behaviors_;
robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_;
robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_;

30
src/Navigations/Packages/move_base/package.xml
View File

@@ -19,8 +19,34 @@
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>move_base_core</build_depend>
<run_depend>move_base_core</run_depend>
<build_depend>robot_nav_core</build_depend>
<run_depend>robot_nav_core</run_depend>
<build_depend>robot_costmap_2d</build_depend>
<run_depend>robot_costmap_2d</run_depend>
<build_depend>robot_tf3_sensor_msgs</build_depend>
<run_depend>robot_tf3_sensor_msgs</run_depend>
<build_depend>robot_tf3_geometry_msgs</build_depend>
<run_depend>robot_tf3_geometry_msgs</run_depend>
<build_depend>data_convert</build_depend>
<run_depend>data_convert</run_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend>
</package>

29
src/Navigations/Packages/move_base/src/move_base.cpp
View File

@@ -219,7 +219,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
{
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath("CustomPlanner");
planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
planner_loader_ = boost::dll::import_alias<robot_nav_core::BaseGlobalPlanner::Ptr()>(
path_file_so, global_planner, boost::dll::load_mode::append_decorations);
planner_ = planner_loader_();
@@ -281,7 +281,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf)
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter");
controller_loader_ =
boost::dll::import_alias<nav_core::BaseLocalPlanner::Ptr()>(
boost::dll::import_alias<robot_nav_core::BaseLocalPlanner::Ptr()>(
path_file_so, local_planner, boost::dll::load_mode::append_decorations);
tc_ = controller_loader_();
if (!tc_)
@@ -883,11 +883,11 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node)
// std::string behavior_name = behavior["name"].as<std::string>();
// std::string path_file_so;
// // Load the factory function from the shared library
// auto recovery_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
// auto recovery_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
// path_file_so, behavior_type, boost::dll::load_mode::append_decorations);
// // Create instance using the factory function
// std::shared_ptr<nav_core::RecoveryBehavior> behavior_ptr(recovery_loader());
// std::shared_ptr<robot_nav_core::RecoveryBehavior> behavior_ptr(recovery_loader());
// // shouldn't be possible, but it won't hurt to check
// if (behavior_ptr == nullptr)
@@ -937,9 +937,9 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
try
{
std::string path_file_so ;
auto clear_costmap_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
std::shared_ptr<nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());
std::shared_ptr<robot_nav_core::RecoveryBehavior> cons_clear(clear_costmap_loader());
cons_clear->initialize("conservative_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
recovery_behavior_names_.push_back("conservative_reset");
recovery_behaviors_.push_back(cons_clear);
@@ -950,15 +950,15 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
}
// next, we'll load a recovery behavior to rotate in place
std::shared_ptr<nav_core::RecoveryBehavior> rotate;
std::shared_ptr<robot_nav_core::RecoveryBehavior> rotate;
if (clearing_rotation_allowed_)
{
try
{
std::string path_file_so;
auto rotate_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
auto rotate_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
path_file_so, "rotate_recovery", boost::dll::load_mode::append_decorations);
rotate = std::shared_ptr<nav_core::RecoveryBehavior>(rotate_loader());
rotate = std::shared_ptr<robot_nav_core::RecoveryBehavior>(rotate_loader());
rotate->initialize("rotate_recovery", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
recovery_behavior_names_.push_back("rotate_recovery");
recovery_behaviors_.push_back(rotate);
@@ -973,9 +973,9 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors()
try
{
std::string path_file_so;
auto clear_costmap_loader = boost::dll::import_alias<nav_core::RecoveryBehavior::Ptr()>(
auto clear_costmap_loader = boost::dll::import_alias<robot_nav_core::RecoveryBehavior::Ptr()>(
path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations);
std::shared_ptr<nav_core::RecoveryBehavior> ags_clear(clear_costmap_loader());
std::shared_ptr<robot_nav_core::RecoveryBehavior> ags_clear(clear_costmap_loader());
ags_clear->initialize("aggressive_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_);
recovery_behavior_names_.push_back("aggressive_reset");
recovery_behaviors_.push_back(ags_clear);
@@ -1307,13 +1307,12 @@ bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist)
return true;
}
bool move_base::MoveBase::getFeedback(robot::move_base_core::NavFeedback &feedback)
robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback()
{
if (!nav_feedback_) {
return false;
return nullptr;
}
feedback = *nav_feedback_;
return true;
return nav_feedback_.get();
}
// void wakePlanner(const robot::TimerEvent &event)