HiepLM/pnkx_nav_core
1
0
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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
CMakeLists.txt
View File

@ -78,20 +78,20 @@ if (NOT TARGET robot_nav_2d_utils)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Libraries/robot_nav_2d_utils)
endif()
if (NOT TARGET nav_core)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core)
if (NOT TARGET robot_nav_core)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core)
endif()
if (NOT TARGET nav_grid)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/nav_grid)
endif()
if (NOT TARGET nav_core2)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core2)
if (NOT TARGET robot_nav_core2)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core2)
endif()
if (NOT TARGET nav_core_adapter)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/nav_core_adapter)
if (NOT TARGET robot_nav_core_adapter)
add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Cores/robot_nav_core_adapter)
endif()
if (NOT TARGET move_base_core)

8
src/APIs/c_api/src/nav_c_api.cpp
View File

@ -449,12 +449,12 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* ou
try {
robot::move_base_core::BaseNavigation* nav = static_cast<robot::move_base_core::BaseNavigation*>(handle);
robot::move_base_core::NavFeedback cpp_feedback;
if (nav->getFeedback(cpp_feedback)) {
cpp_to_c_nav_feedback(cpp_feedback, out_feedback);
if (nav->getFeedback() != nullptr) {
cpp_to_c_nav_feedback(*nav->getFeedback(), out_feedback);
return true;
}
} else {
return false;
}
} catch (...) {
return false;
}

105
src/Algorithms/Cores/score_algorithm/CMakeLists.txt
View File

@ -1,5 +1,4 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(score_algorithm VERSION 1.0.0 LANGUAGES CXX)
@ -11,50 +10,128 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
set(PACKAGES_DIR robot_nav_2d_msgs robot_nav_2d_utils nav_core nav_core2 geometry_msgs mkt_msgs angles)
# ========================================================
# Find Packages
# ========================================================
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building score_algorithm with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building score_algorithm with Standalone CMake")
endif()
if(BUILDING_WITH_CATKIN)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
robot_nav_2d_msgs
robot_nav_2d_utils
robot_nav_core2
robot_nav_core
robot_cpp
mkt_msgs
angles
)
endif()
## System dependencies
find_package(Boost REQUIRED COMPONENTS filesystem system)
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
## The catkin_package macro generates cmake config files for your package
catkin_package(
INCLUDE_DIRS include
LIBRARIES score_algorithm
CATKIN_DEPENDS robot_nav_2d_msgs robot_nav_2d_utils robot_nav_core robot_nav_core2 robot_cpp mkt_msgs
)
endif()
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tạo thư viện shared (.so)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# ========================================================
# Build
# ========================================================
# Tạo thư viện shared (.so)
add_library(score_algorithm src/score_algorithm.cpp)
target_link_libraries(score_algorithm
# Link libraries
if(BUILDING_WITH_CATKIN)
target_link_libraries(score_algorithm
PUBLIC
${PACKAGES_DIR}
robot_cpp
${catkin_LIBRARIES}
PRIVATE
Boost::filesystem
Boost::system
)
else()
# Standalone mode: link dependencies directly
target_link_libraries(score_algorithm
PUBLIC
robot_nav_2d_msgs
robot_nav_2d_utils
robot_nav_core2
robot_nav_core2
robot_cpp
mkt_msgs
angles
PRIVATE
Boost::filesystem
Boost::system
)
endif()
set_target_properties(score_algorithm PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
## Add cmake target dependencies
if(BUILDING_WITH_CATKIN)
add_dependencies(score_algorithm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
target_include_directories(score_algorithm PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
# Cài đặt header files
install(DIRECTORY include/score_algorithm
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# ========================================================
# Installation
# ========================================================
# Cài đặt library
# Export target trong mọi trường hợp để các target khác có thể export và phụ thuộc vào nó
install(TARGETS score_algorithm
EXPORT score_algorithm-targets
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT score_algorithm-targets
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (chỉ cho standalone mode)
install(DIRECTORY include/score_algorithm
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Export targets (chỉ cho standalone mode)
install(EXPORT score_algorithm-targets
FILE score_algorithm-targets.cmake
DESTINATION lib/cmake/score_algorithm)
else()
# Khi build với Catkin, vẫn cần export để các target khác có thể export
install(EXPORT score_algorithm-targets
FILE score_algorithm-targets.cmake
DESTINATION lib/cmake/score_algorithm)
endif()
# Tùy chọn build
option(BUILD_SHARED_LIBS "Build shared libraries" ON)

6
src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h
View File

@ -7,9 +7,9 @@
#include <robot_nav_2d_msgs/Path2D.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <nav_core2/common.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
#include <robot_nav_core2/common.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot_geometry_msgs/PoseArray.h>

17
src/Algorithms/Cores/score_algorithm/package.xml
View File

@ -19,8 +19,19 @@
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<build_depend>robot_nav_core</build_depend>
<build_depend>robot_nav_core2</build_depend>
<build_depend>robot_cpp</build_depend>
<build_depend>mkt_msgs</build_depend>
<build_depend>angles</build_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<run_depend>robot_nav_core</run_depend>
<run_depend>robot_nav_core2</run_depend>
<run_depend>robot_cpp</run_depend>
<run_depend>mkt_msgs</run_depend>
<run_depend>angles</run_depend>
</package>

48
src/Algorithms/Libraries/angles/CMakeLists.txt
View File

@ -1,10 +1,37 @@
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 angles with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building angles with Standalone CMake")
endif()
project(angles VERSION 1.0.0 LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED)
catkin_package(
INCLUDE_DIRS include
# Header-only library; keep LIBRARIES for visibility when exporting
LIBRARIES angles
)
endif()
# Create interface library (header-only)
add_library(angles INTERFACE)
@ -14,18 +41,27 @@ target_include_directories(angles INTERFACE
$<INSTALL_INTERFACE:include>
)
# Install headers
install(DIRECTORY include/angles
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
if(BUILDING_WITH_CATKIN)
add_dependencies(angles ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
# ========================================================
# Installation
# ========================================================
# Install target
install(TARGETS angles
EXPORT angles-targets
INCLUDES DESTINATION include
)
if(NOT BUILDING_WITH_CATKIN)
# Install headers (standalone)
install(DIRECTORY include/angles
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
endif()
# Export targets
install(EXPORT angles-targets
FILE angles-targets.cmake

4
src/Algorithms/Libraries/angles/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>

50
src/Algorithms/Libraries/kalman/CMakeLists.txt
View File

@ -1,4 +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 kalman with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building kalman with Standalone CMake")
endif()
project(kalman VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
@ -10,15 +24,30 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# ========================================================
# Find dependencies
# ========================================================
find_package(Eigen3 REQUIRED)
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED)
catkin_package(
INCLUDE_DIRS include
LIBRARIES kalman
DEPENDS Eigen3
)
endif()
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIRS}
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# Tìm tất cả file source
file(GLOB SOURCES "src/kalman.cpp")
file(GLOB HEADERS "include/kalman/kalman.h")
@ -30,6 +59,7 @@ add_library(kalman SHARED ${SOURCES} ${HEADERS})
target_link_libraries(kalman
PUBLIC
Eigen3::Eigen
$<$<BOOL:${BUILDING_WITH_CATKIN}>:${catkin_LIBRARIES}>
)
set_target_properties(kalman PROPERTIES
@ -44,6 +74,10 @@ target_include_directories(kalman
${EIGEN3_INCLUDE_DIRS}
)
if(BUILDING_WITH_CATKIN)
add_dependencies(kalman ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
# Tạo executable cho test
add_executable(kalman_node src/kalman-test.cpp)
target_link_libraries(kalman_node
@ -56,11 +90,9 @@ set_target_properties(kalman_node PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
)
# Cài đặt header files
install(DIRECTORY include/kalman
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
# ========================================================
# Installation
# ========================================================
# Cài đặt library
install(TARGETS kalman
@ -75,6 +107,14 @@ install(TARGETS kalman_node
RUNTIME DESTINATION bin
)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/kalman
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
endif()
# Export targets
install(EXPORT kalman-targets
FILE kalman-targets.cmake

4
src/Algorithms/Libraries/kalman/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>

69
src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt
View File

@ -1,4 +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 mkt_algorithm with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building mkt_algorithm with Standalone CMake")
endif()
project(mkt_algorithm VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
@ -10,16 +24,38 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# ========================================================
# Find dependencies
# ========================================================
find_package(Boost REQUIRED COMPONENTS system)
find_package(Eigen3 REQUIRED)
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
score_algorithm
robot_nav_2d_msgs
robot_nav_2d_utils
kalman
angles
nav_grid
robot_costmap_2d
robot_sensor_msgs
robot_std_msgs
robot_cpp
)
endif()
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
${EIGEN3_INCLUDE_DIRS}
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# Dependencies packages
set(PACKAGES_DIR
geometry_msgs
@ -49,13 +85,26 @@ file(GLOB DIFF_HEADERS
add_library(mkt_algorithm_diff SHARED ${DIFF_SOURCES} ${DIFF_HEADERS})
# Link libraries
target_link_libraries(mkt_algorithm_diff
if(BUILDING_WITH_CATKIN)
target_link_libraries(mkt_algorithm_diff
PUBLIC
${catkin_LIBRARIES}
Boost::system
Eigen3::Eigen
)
else()
target_link_libraries(mkt_algorithm_diff
PUBLIC
${PACKAGES_DIR}
robot_cpp
Boost::system
Eigen3::Eigen
)
)
endif()
if(BUILDING_WITH_CATKIN)
add_dependencies(mkt_algorithm_diff ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
set_target_properties(mkt_algorithm_diff PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@ -69,11 +118,9 @@ target_include_directories(mkt_algorithm_diff
${EIGEN3_INCLUDE_DIRS}
)
# Cài đặt header files
install(DIRECTORY include/mkt_algorithm
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
# ========================================================
# Installation
# ========================================================
# Cài đặt library
install(TARGETS mkt_algorithm_diff
@ -83,6 +130,14 @@ install(TARGETS mkt_algorithm_diff
RUNTIME DESTINATION bin
)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/mkt_algorithm
DESTINATION include
FILES_MATCHING PATTERN "*.h"
)
endif()
# Cài đặt plugins.xml (nếu cần cho pluginlib)
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml)
install(FILES plugins.xml

2
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h
View File

@ -27,7 +27,7 @@ public:
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

2
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h
View File

@ -25,7 +25,7 @@ public:
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

2
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h
View File

@ -24,7 +24,7 @@ public:
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

2
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h
View File

@ -25,7 +25,7 @@ public:
* @brief Initialize parameters as needed
* @param nh NodeHandle to read parameters from
*/
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) override;
virtual void initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) override;
/**
* @brief Prior to evaluating any trajectories, look at contextual information constant across all trajectories

4
src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h
View File

@ -8,8 +8,8 @@
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_geometry_msgs/PointStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <nav_core2/exceptions.h>
#include <nav_core2/costmap.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_core2/costmap.h>
#include <nav_grid/coordinate_conversion.h>
#include <angles/angles.h>
#include <tf3/exceptions.h>

26
src/Algorithms/Libraries/mkt_algorithm/package.xml
View File

@ -18,9 +18,27 @@
<url type="website">http://www.ros.org/wiki/mkt_algorithm</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>score_algorithm</build_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<build_depend>kalman</build_depend>
<build_depend>angles</build_depend>
<build_depend>nav_grid</build_depend>
<build_depend>robot_costmap_2d</build_depend>
<build_depend>robot_sensor_msgs</build_depend>
<build_depend>robot_std_msgs</build_depend>
<build_depend>robot_cpp</build_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>score_algorithm</run_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<run_depend>kalman</run_depend>
<run_depend>angles</run_depend>
<run_depend>nav_grid</run_depend>
<run_depend>robot_costmap_2d</run_depend>
<run_depend>robot_sensor_msgs</run_depend>
<run_depend>robot_std_msgs</run_depend>
<run_depend>robot_cpp</run_depend>
</package>

2
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp
View File

@ -6,7 +6,7 @@
namespace mkt_algorithm
{
void GoStraight::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
void GoStraight::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
{
nh_ = robot::NodeHandle(nh, name);
name_ = name;

8
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp
View File

@ -11,7 +11,7 @@
namespace mkt_algorithm
{
void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
void Bicycle::initialize(const robot::NodeHandle& nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
{
nh_priv_ = robot::NodeHandle(nh, name);
nh_ = robot::NodeHandle("~");
@ -257,7 +257,7 @@ bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const r
const robot_nav_2d_msgs::Path2D &global_plan, robot_geometry_msgs::Pose2D &sub_goal,
double &target_direction)
{
const nav_core2::Costmap &costmap = *costmap_;
const robot_nav_core2::Costmap &costmap = *costmap_;
const nav_grid::NavGridInfo &info = costmap.getInfo();
if (global_plan.poses.empty())
@ -355,7 +355,7 @@ bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const r
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) // chuyển tọa độ từ đơn vị mét sang pixel
// && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// && costmap(map_x, map_y) != robot_nav_core2::Costmap::NO_INFORMATION)
// {
// Still on the costmap. Continue.
double distance = fabs(robot_nav_2d_utils::poseDistance(plan[i], robot_pose)); // Tính khoảng cách từ vị trí của robot đến pose đang xét
@ -394,7 +394,7 @@ bool Bicycle::getGoalPose(const robot_geometry_msgs::Pose2D &robot_pose, const r
// double g_x = plan[i].x;
// double g_y = plan[i].y;
// unsigned int map_x, map_y;
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != nav_core2::Costmap::NO_INFORMATION)
// if (worldToGridBounded(info, g_x, g_y, map_x, map_y) && costmap(map_x, map_y) != robot_nav_core2::Costmap::NO_INFORMATION)
// {
// // Still on the costmap. Continue.
// last_valid_index = i;

2
src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp
View File

@ -6,7 +6,7 @@
namespace mkt_algorithm
{
void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, nav_core2::Costmap::Ptr costmap)
void RotateToGoal::initialize(const robot::NodeHandle & nh, std::string name, TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap)
{
nh_ = robot::NodeHandle(nh, name);
name_ = name;

6
src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp
View File

@ -709,7 +709,7 @@ mkt_algorithm::diff::PredictiveTrajectory::getLookAheadPoint(const robot_nav_2d_
{
if (global_plan.poses.empty())
{
throw nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
throw robot_nav_core2::PlannerTFException("The global plan passed to the local planner is empty.");
}
if ((unsigned)global_plan.poses.size() < 2)
@ -790,7 +790,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::pruneGlobalPlan(TFListenerPtr tf
robot_nav_2d_msgs::Pose2DStamped robot;
if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot))
{
throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
}
double dist_thresh_sq = dist_behind_robot * dist_behind_robot;
@ -855,7 +855,7 @@ bool mkt_algorithm::diff::PredictiveTrajectory::transformGlobalPlan(
robot_nav_2d_msgs::Pose2DStamped robot_pose;
if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
{
throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
}
// we'll discard points on the plan that are outside the local costmap

63
src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt
View File

@ -1,5 +1,4 @@
cmake_minimum_required(VERSION 3.10)
# Tên dự án
project(mkt_msgs VERSION 1.0.0 LANGUAGES CXX)
@ -7,6 +6,25 @@ project(mkt_msgs VERSION 1.0.0 LANGUAGES CXX)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
# ========================================================
# Find Packages
# ========================================================
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building mkt_msgs with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building mkt_msgs with Standalone CMake")
endif()
if(BUILDING_WITH_CATKIN)
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
robot_nav_2d_msgs
geometry_msgs
)
endif()
# Cho phép các project khác include được header của mkt_msgs
set(mkt_msgs_INCLUDE_DIRS
${CMAKE_CURRENT_SOURCE_DIR}/include
@ -17,9 +35,34 @@ include_directories(
${PROJECT_SOURCE_DIR}/include
)
# Tạo INTERFACE library (header-only)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
## The catkin_package macro generates cmake config files for your package
catkin_package(
INCLUDE_DIRS include
# Header-only library so LIBRARIES is optional; keep for visibility
LIBRARIES mkt_msgs
CATKIN_DEPENDS robot_nav_2d_msgs geometry_msgs
)
endif()
# ========================================================
# Build (header-only INTERFACE)
# ========================================================
add_library(mkt_msgs INTERFACE)
target_link_libraries(mkt_msgs INTERFACE robot_nav_2d_msgs geometry_msgs)
if(BUILDING_WITH_CATKIN)
target_link_libraries(mkt_msgs INTERFACE ${catkin_LIBRARIES})
add_dependencies(mkt_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
else()
target_link_libraries(mkt_msgs INTERFACE robot_nav_2d_msgs geometry_msgs)
endif()
# Set include directories
target_include_directories(mkt_msgs
@ -28,10 +71,9 @@ target_include_directories(mkt_msgs
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/mkt_msgs
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# ========================================================
# Installation
# ========================================================
install(TARGETS mkt_msgs
EXPORT mkt_msgs-targets
@ -39,7 +81,12 @@ install(TARGETS mkt_msgs
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
# Cài đặt header files (cả catkin và standalone)
install(DIRECTORY include/mkt_msgs
DESTINATION include
FILES_MATCHING PATTERN "*.h")
# Export targets (cho cả hai chế độ)
install(EXPORT mkt_msgs-targets
FILE mkt_msgs-targets.cmake
DESTINATION lib/cmake/mkt_msgs)

7
src/Algorithms/Libraries/mkt_msgs/package.xml
View File

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

94
src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt
View File

@ -1,4 +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 mkt_plugins with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building mkt_plugins with Standalone CMake")
endif()
project(mkt_plugins VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
@ -11,9 +25,27 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# ========================================================
# Find dependencies
# ========================================================
# Find system dependencies
find_package(Boost REQUIRED COMPONENTS system)
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
score_algorithm
robot_nav_2d_msgs
robot_nav_2d_utils
robot_nav_core2
geometry_msgs
robot_nav_msgs
robot_std_msgs
robot_sensor_msgs
angles
)
endif()
# Flags biên dịch
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
@ -21,17 +53,33 @@ 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")
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
LIBRARIES mkt_plugins_goal_checker mkt_plugins_standard_traj_generator
CATKIN_DEPENDS score_algorithm robot_nav_2d_msgs robot_nav_2d_utils robot_nav_core2 geometry_msgs robot_nav_msgs robot_std_msgs robot_sensor_msgs angles
)
endif()
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# Dependencies packages (internal libraries)
set(PACKAGES_DIR
score_algorithm
robot_nav_2d_msgs
robot_nav_2d_utils
nav_core2
robot_nav_core2
geometry_msgs
robot_nav_msgs
robot_std_msgs
@ -50,10 +98,18 @@ add_library(${PROJECT_NAME}_goal_checker SHARED
src/equation_line.cpp
)
target_link_libraries(${PROJECT_NAME}_goal_checker
# Link libraries
if(BUILDING_WITH_CATKIN)
target_link_libraries(${PROJECT_NAME}_goal_checker
PUBLIC ${catkin_LIBRARIES}
PRIVATE Boost::boost
)
else()
target_link_libraries(${PROJECT_NAME}_goal_checker
PUBLIC ${PACKAGES_DIR}
PRIVATE Boost::boost
)
)
endif()
set_target_properties(${PROJECT_NAME}_goal_checker PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@ -73,10 +129,17 @@ add_library(${PROJECT_NAME}_standard_traj_generator SHARED
src/limited_accel_generator.cpp
)
target_link_libraries(${PROJECT_NAME}_standard_traj_generator
if(BUILDING_WITH_CATKIN)
target_link_libraries(${PROJECT_NAME}_standard_traj_generator
PUBLIC ${catkin_LIBRARIES}
PRIVATE Boost::boost
)
else()
target_link_libraries(${PROJECT_NAME}_standard_traj_generator
PUBLIC ${PACKAGES_DIR}
PRIVATE Boost::boost
)
)
endif()
set_target_properties(${PROJECT_NAME}_standard_traj_generator PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@ -88,17 +151,15 @@ target_include_directories(${PROJECT_NAME}_standard_traj_generator
$<INSTALL_INTERFACE:include>
)
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME}_goal_checker ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME}_standard_traj_generator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
#############
## Install ##
#############
# Cài đặt header files
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
# Cài đặt libraries
install(TARGETS
${PROJECT_NAME}_standard_traj_generator
@ -109,6 +170,15 @@ install(TARGETS
RUNTIME DESTINATION bin
)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
endif()
# Export targets
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake

27
src/Algorithms/Libraries/mkt_plugins/package.xml
View File

@ -19,8 +19,31 @@
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<build_depend>score_algorithm</build_depend>
<run_depend>score_algorithm</run_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<build_depend>robot_nav_core2</build_depend>
<run_depend>robot_nav_core2</run_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>
<build_depend>robot_nav_msgs</build_depend>
<run_depend>robot_nav_msgs</run_depend>
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>robot_sensor_msgs</build_depend>
<run_depend>robot_sensor_msgs</run_depend>
<build_depend>angles</build_depend>
<run_depend>angles</run_depend>
</package>

2
src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp
View File

@ -2,7 +2,7 @@
#include <mkt_plugins/limited_accel_generator.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <vector>
namespace mkt_plugins

2
src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp
View File

@ -2,7 +2,7 @@
#include <mkt_plugins/xy_theta_iterator.h>
#include <robot_nav_2d_utils/parameters.h>
#include <boost/dll/alias.hpp>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <algorithm>
#include <cmath>
#include <memory>

2
src/Algorithms/Packages/global_planners/custom_planner

@ -1 +1 @@
Subproject commit 60e9c5673f8fd646d628f843ef73e71d1d9b2a17
Subproject commit 0b01c22019b66c905d6d67611d333742e1e37a55

2
src/Algorithms/Packages/global_planners/dock_planner

@ -1 +1 @@
Subproject commit b5a1b7b6d8367b5812b32f2ca51bbed6e2a4c99a
Subproject commit 22aa83fe5a66f5c090bcb8c1fafb41cfe777ebc1

83
src/Algorithms/Packages/global_planners/two_points_planner/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 two_points_planner with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building two_points_planner with Standalone CMake")
endif()
# Tên dự án
project(two_points_planner VERSION 1.0.0 LANGUAGES CXX)
@ -13,14 +26,47 @@ 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
robot_costmap_2d
robot_nav_core
geometry_msgs
robot_nav_msgs
robot_std_msgs
tf3
robot_tf3_geometry_msgs
robot_cpp
)
endif()
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
LIBRARIES two_points_planner
CATKIN_DEPENDS robot_costmap_2d robot_nav_core geometry_msgs robot_nav_msgs robot_std_msgs tf3 robot_tf3_geometry_msgs robot_cpp
)
endif()
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# Tìm tất cả file source
file(GLOB SOURCES "src/two_points_planner.cpp")
file(GLOB HEADERS "include/two_points_planner/*.h")
@ -28,7 +74,7 @@ file(GLOB HEADERS "include/two_points_planner/*.h")
# Dependencies packages (internal libraries)
set(PACKAGES_DIR
robot_costmap_2d
nav_core
robot_nav_core
geometry_msgs
robot_nav_msgs
robot_std_msgs
@ -40,11 +86,23 @@ set(PACKAGES_DIR
add_library(two_points_planner SHARED ${SOURCES} ${HEADERS})
# Link libraries
target_link_libraries(two_points_planner
if(BUILDING_WITH_CATKIN)
target_link_libraries(two_points_planner
PUBLIC ${catkin_LIBRARIES}
PRIVATE Boost::filesystem
PRIVATE Boost::system
)
else()
target_link_libraries(two_points_planner
PUBLIC ${PACKAGES_DIR}
PUBLIC robot_cpp
PRIVATE Boost::boost
)
)
endif()
if(BUILDING_WITH_CATKIN)
add_dependencies(two_points_planner ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
set_target_properties(two_points_planner PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@ -57,12 +115,9 @@ target_include_directories(two_points_planner
$<INSTALL_INTERFACE:include>
)
# Cài đặt header files
install(DIRECTORY include/two_points_planner
DESTINATION include
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
# ========================================================
# Installation
# ========================================================
# Cài đặt library
install(TARGETS two_points_planner
@ -71,6 +126,16 @@ install(TARGETS two_points_planner
ARCHIVE DESTINATION lib
RUNTIME DESTINATION bin
)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/two_points_planner
DESTINATION include
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
endif()
# Export targets
install(EXPORT two_points_planner-targets
FILE two_points_planner-targets.cmake

6
src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h
View File

@ -5,7 +5,7 @@
#include <iostream>
#include <vector>
#include <robot_geometry_msgs/PoseStamped.h>
#include <nav_core/base_global_planner.h>
#include <robot_nav_core/base_global_planner.h>
#include <robot_costmap_2d/costmap_2d_robot.h>
#include <robot/node_handle.h>
@ -16,7 +16,7 @@ namespace two_points_planner
* @class TwoPointsPlanner
* @brief Plugin-based flexible global_planner
*/
class TwoPointsPlanner : public nav_core::BaseGlobalPlanner
class TwoPointsPlanner : public robot_nav_core::BaseGlobalPlanner
{
public:
/**
@ -56,7 +56,7 @@ public:
* @brief Create a new TwoPointsPlanner instance
* @return A pointer to the new TwoPointsPlanner instance
*/
static nav_core::BaseGlobalPlanner::Ptr create();
static robot_nav_core::BaseGlobalPlanner::Ptr create();
protected:

24
src/Algorithms/Packages/global_planners/two_points_planner/package.xml
View File

@ -19,8 +19,28 @@
<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>robot_nav_core</build_depend>
<run_depend>robot_nav_core</run_depend>
<build_depend>geometry_msgs</build_depend>
<run_depend>geometry_msgs</run_depend>
<build_depend>robot_nav_msgs</build_depend>
<run_depend>robot_nav_msgs</run_depend>
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>tf3</build_depend>
<run_depend>tf3</run_depend>
<build_depend>robot_tf3_geometry_msgs</build_depend>
<run_depend>robot_tf3_geometry_msgs</run_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend>
</package>

2
src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp
View File

@ -211,7 +211,7 @@ namespace two_points_planner
return true;
}
nav_core::BaseGlobalPlanner::Ptr TwoPointsPlanner::create()
robot_nav_core::BaseGlobalPlanner::Ptr TwoPointsPlanner::create()
{
return std::make_shared<TwoPointsPlanner>();
}

100
src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt
View File

@ -1,4 +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 pnkx_local_planner with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building pnkx_local_planner with Standalone CMake")
endif()
project(pnkx_local_planner VERSION 1.0.0 LANGUAGES CXX)
# Chuẩn C++
@ -11,9 +25,33 @@ if(NOT CMAKE_BUILD_TYPE)
set(CMAKE_BUILD_TYPE Release)
endif()
# ========================================================
# Find dependencies
# ========================================================
# Find system dependencies
find_package(Boost REQUIRED COMPONENTS system)
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
robot_nav_msgs
robot_std_msgs
robot_sensor_msgs
robot_visualization_msgs
robot_nav_2d_utils
robot_nav_core2
mkt_msgs
score_algorithm
robot_costmap_2d
tf3
robot_tf3_geometry_msgs
robot_tf3_sensor_msgs
robot_cpp
angles
)
endif()
# Flags biên dịch
# Warning flags - disabled to suppress warnings during build
# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -Wextra -Wpedantic")
@ -21,11 +59,27 @@ 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")
# ========================================================
# Catkin specific configuration
# ========================================================
if(BUILDING_WITH_CATKIN)
catkin_package(
INCLUDE_DIRS include
LIBRARIES pnkx_local_planner pnkx_local_planner_utils
CATKIN_DEPENDS geometry_msgs robot_nav_msgs robot_std_msgs robot_sensor_msgs robot_visualization_msgs robot_nav_2d_utils robot_nav_core2 mkt_msgs score_algorithm robot_costmap_2d tf3 robot_tf3_geometry_msgs robot_tf3_sensor_msgs robot_cpp angles
)
endif()
# Thư mục include
include_directories(
${PROJECT_SOURCE_DIR}/include
)
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
# Dependencies packages (internal libraries)
set(PACKAGES_DIR
geometry_msgs
@ -34,7 +88,7 @@ set(PACKAGES_DIR
robot_sensor_msgs
robot_visualization_msgs
robot_nav_2d_utils
nav_core2
robot_nav_core2
mkt_msgs
score_algorithm
robot_costmap_2d
@ -51,10 +105,17 @@ add_library(${PROJECT_NAME}_utils SHARED
)
# Link libraries cho utils
target_link_libraries(${PROJECT_NAME}_utils
if(BUILDING_WITH_CATKIN)
target_link_libraries(${PROJECT_NAME}_utils
PUBLIC ${catkin_LIBRARIES}
PRIVATE Boost::boost
)
else()
target_link_libraries(${PROJECT_NAME}_utils
PUBLIC ${PACKAGES_DIR}
PRIVATE Boost::boost
)
)
endif()
set_target_properties(${PROJECT_NAME}_utils PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@ -76,11 +137,19 @@ add_library(${PROJECT_NAME} SHARED
)
# Link libraries cho thư viện chính
target_link_libraries(${PROJECT_NAME}
if(BUILDING_WITH_CATKIN)
target_link_libraries(${PROJECT_NAME}
PUBLIC ${PROJECT_NAME}_utils
PUBLIC ${catkin_LIBRARIES}
PRIVATE Boost::boost
)
else()
target_link_libraries(${PROJECT_NAME}
PUBLIC ${PROJECT_NAME}_utils
PUBLIC ${PACKAGES_DIR}
PRIVATE Boost::boost
)
)
endif()
set_target_properties(${PROJECT_NAME} PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
@ -96,6 +165,11 @@ target_include_directories(${PROJECT_NAME}
# Compile options
target_compile_options(${PROJECT_NAME} PUBLIC "-Wno-terminate")
if(BUILDING_WITH_CATKIN)
add_dependencies(${PROJECT_NAME}_utils ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
endif()
# Tùy chọn build
option(BUILD_SHARED_LIBS "Build shared libraries" ON)
option(BUILD_TESTS "Build test programs" OFF)
@ -104,13 +178,6 @@ option(BUILD_TESTS "Build test programs" OFF)
## Install ##
#############
# Cài đặt header files
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
# Cài đặt libraries
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
EXPORT ${PROJECT_NAME}-targets
@ -119,6 +186,15 @@ install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils
RUNTIME DESTINATION bin
)
if(NOT BUILDING_WITH_CATKIN)
# Cài đặt header files (standalone)
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION include/${PROJECT_NAME}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
endif()
# Export targets
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake

18
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h
View File

@ -1,7 +1,7 @@
#ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
#define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_
#include <nav_core/base_global_planner.h>
#include <robot_nav_core2/base_global_planner.h>
#include <pnkx_local_planner/pnkx_local_planner.h>
namespace pnkx_local_planner
@ -22,7 +22,7 @@ namespace pnkx_local_planner
virtual ~PNKXDockingLocalPlanner();
/**
* @brief nav_core2 initialization
* @brief robot_nav_core2 initialization
* @param name Namespace for this planner
* @param tf TFListener pointer
* @param costmap_robot Costmap pointer
@ -31,7 +31,7 @@ namespace pnkx_local_planner
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
/**
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
* @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
@ -46,7 +46,7 @@ namespace pnkx_local_planner
const robot_nav_2d_msgs::Twist2D &velocity) override;
/**
* @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
* @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
*
* The pose that it checks against is the last pose in the current global plan.
* The calculation is delegated to the goal_checker plugin.
@ -61,11 +61,11 @@ namespace pnkx_local_planner
* @brief Create a new instance of the PNKXDockingLocalPlanner
* @return A shared pointer to the new instance
*/
static nav_core2::LocalPlanner::Ptr create();
static robot_nav_core2::LocalPlanner::Ptr create();
protected:
/**
* @brief nav_core2 initialization other
* @brief robot_nav_core2 initialization other
*/
void initializeOthers() override;
@ -75,7 +75,7 @@ namespace pnkx_local_planner
void getMaker();
/**
* @brief nav_core2 reset other
* @brief robot_nav_core2 reset other
*/
void reset() override;
@ -137,7 +137,7 @@ namespace pnkx_local_planner
std::string docking_planner_name_;
std::string docking_nav_name_;
nav_core::BaseGlobalPlanner::Ptr docking_planner_;
robot_nav_core2::BaseGlobalPlanner::Ptr docking_planner_;
score_algorithm::ScoreAlgorithm::Ptr docking_nav_;
robot_geometry_msgs::Vector3 linear_;
@ -158,7 +158,7 @@ namespace pnkx_local_planner
TFListenerPtr tf_;
robot_costmap_2d::Costmap2DROBOT *costmap_robot_;
score_algorithm::TrajectoryGenerator::Ptr traj_generator_;
std::function<nav_core::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
std::function<robot_nav_core2::BaseGlobalPlanner::Ptr()> docking_planner_loader_;
std::function<score_algorithm::ScoreAlgorithm::Ptr()> docking_nav_loader_;
robot::WallTimer detected_timeout_wt_, delayed_wt_;

10
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h
View File

@ -20,7 +20,7 @@ namespace pnkx_local_planner
virtual ~PNKXGoStraightLocalPlanner();
/**
* @brief nav_core2 initialization
* @brief robot_nav_core2 initialization
* @param name Namespace for this planner
* @param tf TFListener pointer
* @param costmap_robot Costmap pointer
@ -29,7 +29,7 @@ namespace pnkx_local_planner
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
/**
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
* @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
@ -44,7 +44,7 @@ namespace pnkx_local_planner
const robot_nav_2d_msgs::Twist2D &velocity) override;
/**
* @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
* @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
*
* The pose that it checks against is the last pose in the current global plan.
* The calculation is delegated to the goal_checker plugin.
@ -59,11 +59,11 @@ namespace pnkx_local_planner
* @brief Create a new instance of the PNKXGoStraightLocalPlanner
* @return A shared pointer to the new instance
*/
static nav_core2::LocalPlanner::Ptr create();
static robot_nav_core2::LocalPlanner::Ptr create();
protected:
/**
* @brief nav_core2 reset other
* @brief robot_nav_core2 reset other
*/
virtual void reset() override;;

20
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h
View File

@ -3,7 +3,7 @@
#include <robot/node_handle.h>
#include <robot/console.h>
#include <nav_core2/local_planner.h>
#include <robot_nav_core2/local_planner.h>
#include <score_algorithm/trajectory_generator.h>
#include <score_algorithm/score_algorithm.h>
#include <score_algorithm/goal_checker.h>
@ -16,7 +16,7 @@ namespace pnkx_local_planner
* @class PNKXLocalPlanner
* @brief Plugin-based flexible local_planner
*/
class PNKXLocalPlanner : public nav_core2::LocalPlanner
class PNKXLocalPlanner : public robot_nav_core2::LocalPlanner
{
public:
/**
@ -27,7 +27,7 @@ namespace pnkx_local_planner
virtual ~PNKXLocalPlanner();
/**
* @brief nav_core2 initialization
* @brief robot_nav_core2 initialization
* @param name Namespace for this planner
* @param tf TFListener pointer
* @param costmap_robot Costmap pointer
@ -36,19 +36,19 @@ namespace pnkx_local_planner
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
/**
* @brief nav_core2 setGoalPose - Sets the global goal pose
* @brief robot_nav_core2 setGoalPose - Sets the global goal pose
* @param goal_pose The Goal Pose
*/
void setGoalPose(const robot_nav_2d_msgs::Pose2DStamped &goal_pose) override;
/**
* @brief nav_core2 setPlan - Sets the global plan
* @brief robot_nav_core2 setPlan - Sets the global plan
* @param path The global plan
*/
void setPlan(const robot_nav_2d_msgs::Path2D &path) override;
/**
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
* @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
@ -99,7 +99,7 @@ namespace pnkx_local_planner
virtual bool islock() override;
/**
* @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
* @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
*
* The pose that it checks against is the last pose in the current global plan.
* The calculation is delegated to the goal_checker plugin.
@ -114,7 +114,7 @@ namespace pnkx_local_planner
* @brief Create a new PNKXLocalPlanner
* @return A shared pointer to the new PNKXLocalPlanner
*/
static nav_core2::LocalPlanner::Ptr create();
static robot_nav_core2::LocalPlanner::Ptr create();
protected:
/**
@ -131,12 +131,12 @@ namespace pnkx_local_planner
*/
virtual void unlock();
/**
* @brief nav_core2 initialization other
* @brief robot_nav_core2 initialization other
*/
virtual void initializeOthers();
/**
* @brief nav_core2 reset other
* @brief robot_nav_core2 reset other
*/
virtual void reset();

10
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h
View File

@ -20,7 +20,7 @@ namespace pnkx_local_planner
virtual ~PNKXRotateLocalPlanner();
/**
* @brief nav_core2 initialization
* @brief robot_nav_core2 initialization
* @param name Namespace for this planner
* @param tf TFListener pointer
* @param costmap_robot Costmap pointer
@ -29,7 +29,7 @@ namespace pnkx_local_planner
TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) override;
/**
* @brief nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
* @brief robot_nav_core2 computeVelocityCommands - calculates the best command given the current pose and velocity
*
* It is presumed that the global plan is already set.
*
@ -44,7 +44,7 @@ namespace pnkx_local_planner
const robot_nav_2d_msgs::Twist2D &velocity) override;
/**
* @brief nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
* @brief robot_nav_core2 isGoalReached - Check whether the robot has reached its goal, given the current pose & velocity.
*
* The pose that it checks against is the last pose in the current global plan.
* The calculation is delegated to the goal_checker plugin.
@ -59,11 +59,11 @@ namespace pnkx_local_planner
* @brief Create a new instance of the PNKXRotateLocalPlanner
* @return A shared pointer to the new instance
*/
static nav_core2::LocalPlanner::Ptr create();
static robot_nav_core2::LocalPlanner::Ptr create();
protected:
/**
* @brief nav_core2 reset other
* @brief robot_nav_core2 reset other
*/
virtual void reset() override;

4
src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h
View File

@ -5,8 +5,8 @@
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <robot_nav_2d_utils/conversions.h>
#include <nav_core2/common.h>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/common.h>
#include <robot_nav_core2/exceptions.h>
#include <tf3/buffer_core.h>
#include <tf3/convert.h>

45
src/Algorithms/Packages/local_planners/pnkx_local_planner/package.xml
View File

@ -19,8 +19,49 @@
<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_nav_msgs</build_depend>
<run_depend>robot_nav_msgs</run_depend>
<build_depend>robot_std_msgs</build_depend>
<run_depend>robot_std_msgs</run_depend>
<build_depend>robot_sensor_msgs</build_depend>
<run_depend>robot_sensor_msgs</run_depend>
<build_depend>robot_visualization_msgs</build_depend>
<run_depend>robot_visualization_msgs</run_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<build_depend>robot_nav_core2</build_depend>
<run_depend>robot_nav_core2</run_depend>
<build_depend>mkt_msgs</build_depend>
<run_depend>mkt_msgs</run_depend>
<build_depend>score_algorithm</build_depend>
<run_depend>score_algorithm</run_depend>
<build_depend>robot_costmap_2d</build_depend>
<run_depend>robot_costmap_2d</run_depend>
<build_depend>tf3</build_depend>
<run_depend>tf3</run_depend>
<build_depend>robot_tf3_geometry_msgs</build_depend>
<run_depend>robot_tf3_geometry_msgs</run_depend>
<build_depend>robot_tf3_sensor_msgs</build_depend>
<run_depend>robot_tf3_sensor_msgs</run_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_cpp</run_depend>
<build_depend>angles</build_depend>
<run_depend>angles</run_depend>
</package>

48
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp
View File

@ -1,5 +1,5 @@
#include <pnkx_local_planner/pnkx_docking_local_planner.h>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
@ -51,7 +51,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
info_.origin_y = costmap_->getOriginY();
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.");
nh_ = robot::NodeHandle("~");
parent_ = parent;
@ -65,7 +65,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
traj_generator_ = traj_gen_loader_();
@ -89,7 +89,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
nav_algorithm_ = algorithm_loader_();
@ -110,7 +110,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("pnkx_local_planner::RotateToGoalDiff"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
rotate_algorithm_ = algorithm_loader_();
@ -132,7 +132,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::initialize(robot::NodeHandle &
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("dwb_plugins::SimpleGoalChecker"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
goal_checker_ = goal_checker_loader_();
@ -262,7 +262,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
info_.origin_y = costmap_->getOriginY();
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.");
}
// Update time stamp of goal pose
@ -279,9 +279,9 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
robot::log_warning_at(__FILE__, __LINE__, "Transform global plan is failed");
}
catch(const nav_core2::LocalPlannerException& e)
catch(const robot_nav_core2::LocalPlannerException& e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
double x_direction, y_direction, theta_direction;
@ -290,7 +290,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
// else
// ROS_INFO_THROTTLE(0.2, "chieu %f %f %f", x_direction, y_direction, theta_direction);
@ -329,7 +329,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
}
if (!dkpl_.front()->docking_nav_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
{
throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
}
}
@ -339,7 +339,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::prepare(const robot_nav_2d_msg
{
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
{
throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
}
// else
@ -366,10 +366,10 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXDockingLocalPlanner::c
cmd_vel = this->ScoreAlgorithm(pose, velocity);
return cmd_vel;
}
catch (const nav_core2::PlannerException &e)
catch (const robot_nav_core2::PlannerException &e)
{
robot::log_warning_at(__FILE__, __LINE__, "%s", e.what());
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
}
@ -499,7 +499,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
}
catch (const std::exception &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
}
else if (!dkpl_.front()->docking_planner_ && dkpl_.front()->docking_nav_)
@ -523,7 +523,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::dockingHanlde(const robot_nav_
}
catch (const std::exception &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
}
}
@ -585,8 +585,8 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
{
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
docking_planner_loader_ = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
docking_planner_loader_ = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
path_file_so, docking_planner_name_, boost::dll::load_mode::append_decorations);
docking_planner_ = docking_planner_loader_();
if (!docking_planner_)
@ -608,7 +608,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
{
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
docking_nav_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
path_file_so, docking_nav_name_, boost::dll::load_mode::append_decorations);
docking_nav_ = docking_nav_loader_();
@ -634,7 +634,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::initialize(rob
std::stringstream re;
re << "Can not get 'maker_goal_frame' in " << name_;
robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
throw nav_core2::LocalPlannerException(re.str());
throw robot_nav_core2::LocalPlannerException(re.str());
}
nh_priv_.param("vel_x", linear_.x, 0.5);
@ -691,7 +691,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getMakerGoal(r
std::stringstream re;
re << "No detected " << maker_goal_frame_;
robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
throw nav_core2::LocalPlannerException(re.str());
throw robot_nav_core2::LocalPlannerException(re.str());
}
}
return false;
@ -724,7 +724,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::geLocalGoal(ro
std::stringstream re;
re << "No detected " << maker_goal_frame_;
robot::log_info_at(__FILE__, __LINE__, "%s", re.str().c_str());
throw nav_core2::LocalPlannerException(re.str());
throw robot_nav_core2::LocalPlannerException(re.str());
}
}
return false;
@ -739,7 +739,7 @@ bool pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::getLocalPath(
if (!docking_planner_->makePlan(start, goal, docking_plan))
{
throw nav_core2::LocalPlannerException("Making plan from goal maker is failed");
throw robot_nav_core2::LocalPlannerException("Making plan from goal maker is failed");
return false;
}
else
@ -763,7 +763,7 @@ void pnkx_local_planner::PNKXDockingLocalPlanner::DockingPlanner::delayCb(const
robot::log_warning_at(__FILE__, __LINE__, "%s delay end %f", name_.c_str(), robot::Time::now().toSec());
}
nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXDockingLocalPlanner::create()
robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXDockingLocalPlanner::create()
{
return std::make_shared<PNKXDockingLocalPlanner>();
}

26
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp
View File

@ -1,4 +1,4 @@
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
@ -47,7 +47,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
info_.origin_y = costmap_->getOriginY();
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.");
parent_ = parent;
planner_nh_ = robot::NodeHandle(parent_, name);
@ -61,7 +61,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
traj_generator_ = traj_gen_loader_();
@ -84,7 +84,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
robot::log_info_at(__FILE__, __LINE__, "Using Algorithm \"%s\"", algorithm_nav_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
nav_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
path_file_so, algorithm_nav_name, boost::dll::load_mode::append_decorations);
nav_algorithm_ = nav_algorithm_loader_();
@ -106,7 +106,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::initialize(robot::NodeHandl
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("score_algorithm::GoalChecker"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
goal_checker_ = goal_checker_loader_();
@ -152,9 +152,9 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXGoStraightLocalPlanner
cmd_vel = this->ScoreAlgorithm(pose, velocity);
return cmd_vel;
}
catch (const nav_core2::PlannerException &e)
catch (const robot_nav_core2::PlannerException &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
}
@ -171,7 +171,7 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
info_.origin_y = costmap_->getOriginY();
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.");
}
// Update time stamp of goal pose
// goal_pose_.header.stamp = pose.header.stamp;
@ -181,17 +181,17 @@ void pnkx_local_planner::PNKXGoStraightLocalPlanner::prepare(const robot_nav_2d_
try
{
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
throw nav_core2::LocalPlannerException("Transform global plan is failed");
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
}
catch(const nav_core2::LocalPlannerException& e)
catch(const robot_nav_core2::LocalPlannerException& e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
double x_direction, y_direction, theta_direction;
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
{
throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
int xytheta_direct[3];
@ -229,7 +229,7 @@ bool pnkx_local_planner::PNKXGoStraightLocalPlanner::isGoalReached(const robot_n
return ret_nav_;
}
nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXGoStraightLocalPlanner::create()
robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXGoStraightLocalPlanner::create()
{
return std::make_shared<pnkx_local_planner::PNKXGoStraightLocalPlanner>();
}

32
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp
View File

@ -1,5 +1,5 @@
#include <pnkx_local_planner/pnkx_local_planner.h>
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
@ -54,7 +54,7 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent,
info_.origin_y = costmap_->getOriginY();
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.");
parent_ = parent;
planner_nh_ = robot::NodeHandle(parent_, name);
@ -221,7 +221,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
info_.origin_y = costmap_->getOriginY();
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.");
}
// Update time stamp of goal pose
@ -242,7 +242,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", nav_algorithm_->getName().c_str());
throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
}
else
@ -250,7 +250,7 @@ void pnkx_local_planner::PNKXLocalPlanner::prepare(const robot_nav_2d_msgs::Pose
if (!rotate_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
{
robot::log_warning_at(__FILE__, __LINE__, "Algorithm \"%s\" failed to prepare", rotate_algorithm_->getName().c_str());
throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
}
@ -274,9 +274,9 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXLocalPlanner::computeV
cmd_vel = this->ScoreAlgorithm(pose, velocity);
return cmd_vel;
}
catch (const nav_core2::PlannerException &e)
catch (const robot_nav_core2::PlannerException &e)
{
throw nav_core2::LocalPlannerException("computing velocity commands has been broken");
throw robot_nav_core2::LocalPlannerException("computing velocity commands has been broken");
return cmd_vel;
}
}
@ -321,11 +321,11 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistLinear(robot_geometry_msgs::V
if (traj_generator_)
return traj_generator_->setTwistLinear(linear);
else
throw nav_core2::LocalPlannerException("Failed to set linear");
throw robot_nav_core2::LocalPlannerException("Failed to set linear");
}
catch (const std::exception &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
return false;
}
}
@ -337,11 +337,11 @@ robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistLinea
if (traj_generator_)
return traj_generator_->getTwistLinear(direct);
else
throw nav_core2::LocalPlannerException("Failed to get linear");
throw robot_nav_core2::LocalPlannerException("Failed to get linear");
}
catch (const std::exception &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
}
@ -352,11 +352,11 @@ bool pnkx_local_planner::PNKXLocalPlanner::setTwistAngular(robot_geometry_msgs::
if (traj_generator_)
return traj_generator_->setTwistAngular(angular);
else
throw nav_core2::LocalPlannerException("Failed to set angular");
throw robot_nav_core2::LocalPlannerException("Failed to set angular");
}
catch (const std::exception &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
return false;
}
}
@ -368,11 +368,11 @@ robot_geometry_msgs::Vector3 pnkx_local_planner::PNKXLocalPlanner::getTwistAngul
if (traj_generator_)
return traj_generator_->getTwistAngular(direct);
else
throw nav_core2::LocalPlannerException("Failed to get angular");
throw robot_nav_core2::LocalPlannerException("Failed to get angular");
}
catch (const std::exception &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
}
@ -434,7 +434,7 @@ robot_nav_2d_msgs::Pose2DStamped pnkx_local_planner::PNKXLocalPlanner::transform
return local_pose;
}
nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXLocalPlanner::create()
robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXLocalPlanner::create()
{
return std::make_shared<PNKXLocalPlanner>();
}

24
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp
View File

@ -1,7 +1,7 @@
#include "pnkx_local_planner/pnkx_rotate_local_planner.h"
#include "pnkx_local_planner/transforms.h"
#include <nav_core2/exceptions.h>
#include <robot_nav_core2/exceptions.h>
#include <robot_nav_2d_utils/conversions.h>
#include <robot_nav_2d_utils/tf_help.h>
#include <robot_nav_2d_utils/path_ops.h>
@ -55,7 +55,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
robot::log_info_at(__FILE__, __LINE__, "Using Trajectory Generator \"%s\"", traj_generator_name.c_str());
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
traj_gen_loader_ = boost::dll::import_alias<score_algorithm::TrajectoryGenerator::Ptr()>(
path_file_so, traj_generator_name, boost::dll::load_mode::append_decorations);
traj_generator_ = traj_gen_loader_();
@ -77,7 +77,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
planner_nh_.param("algorithm_rotate_name", algorithm_rotate_name, std::string("nav_algorithm::Rotate"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
rotate_algorithm_loader_ = boost::dll::import_alias<score_algorithm::ScoreAlgorithm::Ptr()>(
path_file_so, algorithm_rotate_name, boost::dll::load_mode::append_decorations);
rotate_algorithm_ = rotate_algorithm_loader_();
@ -99,7 +99,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::initialize(robot::NodeHandle& p
planner_nh_.param("goal_checker_name", goal_checker_name, std::string("nav_algorithm::GoalChecker"));
try
{
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/src/Algorithms/Packages/local_planners/pnkx_local_planner/libpnkx_local_planner.so";
goal_checker_loader_ = boost::dll::import_alias<score_algorithm::GoalChecker::Ptr()>(
path_file_so, goal_checker_name, boost::dll::load_mode::append_decorations);
goal_checker_ = goal_checker_loader_();
@ -145,7 +145,7 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
info_.origin_y = costmap_->getOriginY();
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.");
}
// Update time stamp of goal pose
@ -156,17 +156,17 @@ void pnkx_local_planner::PNKXRotateLocalPlanner::prepare(const robot_nav_2d_msgs
try
{
if (!pnkx_local_planner::transformGlobalPlan(tf_, global_plan_, local_start_pose, costmap_robot_, costmap_robot_->getGlobalFrameID(), 2.0, transformed_plan_))
throw nav_core2::LocalPlannerException("Transform global plan is failed");
throw robot_nav_core2::LocalPlannerException("Transform global plan is failed");
}
catch(const nav_core2::LocalPlannerException& e)
catch(const robot_nav_core2::LocalPlannerException& e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
double x_direction, y_direction, theta_direction;
if (!nav_algorithm_->prepare(local_start_pose, velocity, local_goal_pose, transformed_plan_, x_direction, y_direction, theta_direction))
{
throw nav_core2::LocalPlannerException("Algorithm failed to prepare");
throw robot_nav_core2::LocalPlannerException("Algorithm failed to prepare");
}
int xytheta_direct[3];
@ -188,9 +188,9 @@ robot_nav_2d_msgs::Twist2DStamped pnkx_local_planner::PNKXRotateLocalPlanner::co
cmd_vel = this->ScoreAlgorithm(pose, velocity);
return cmd_vel;
}
catch (const nav_core2::PlannerException &e)
catch (const robot_nav_core2::PlannerException &e)
{
throw nav_core2::LocalPlannerException(e.what());
throw robot_nav_core2::LocalPlannerException(e.what());
}
}
@ -225,7 +225,7 @@ bool pnkx_local_planner::PNKXRotateLocalPlanner::isGoalReached(const robot_nav_2
return ret_nav_;
}
nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXRotateLocalPlanner::create()
robot_nav_core2::LocalPlanner::Ptr pnkx_local_planner::PNKXRotateLocalPlanner::create()
{
return std::make_shared<pnkx_local_planner::PNKXRotateLocalPlanner>();
}

18
src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp
View File

@ -46,19 +46,19 @@ bool pnkx_local_planner::getPose(TFListenerPtr tf, std::string base_frame_id, st
{
robot::log_warning_throttle(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
return false;
// throw nav_core2::LocalPlannerException(ex.what());
// throw robot_nav_core2::LocalPlannerException(ex.what());
}
catch (tf3::ConnectivityException& ex)
{
robot::log_warning_throttle(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
return false;
// throw nav_core2::LocalPlannerException(ex.what());
// throw robot_nav_core2::LocalPlannerException(ex.what());
}
catch (tf3::ExtrapolationException& ex)
{
robot::log_warning_throttle(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
return false;
// throw nav_core2::LocalPlannerException(ex.what());
// throw robot_nav_core2::LocalPlannerException(ex.what());
}
// check global_pose timeout
if (!global_pose_stamped.header.stamp.isZero() && current_time.toSec() - global_pose_stamped.header.stamp.toSec() > transform_tolerance)
@ -95,15 +95,15 @@ bool pnkx_local_planner::transformGlobalPlan(
if (global_plan.poses.size() == 0)
{
robot::log_warning("Received plan with zero length");
throw nav_core2::PlannerException("Received plan with zero length");
throw robot_nav_core2::PlannerException("Received plan with zero length");
}
transformed_plan.poses.clear();
if (tf == nullptr)
throw nav_core2::PlannerException("TFListenerPtr is null");
throw robot_nav_core2::PlannerException("TFListenerPtr is null");
if (global_plan.poses.empty())
throw nav_core2::PlannerException("Received plan with zero length");
throw robot_nav_core2::PlannerException("Received plan with zero length");
try
{
@ -111,7 +111,7 @@ bool pnkx_local_planner::transformGlobalPlan(
robot_nav_2d_msgs::Pose2DStamped robot_pose;
if (!robot_nav_2d_utils::transformPose(tf, global_plan.header.frame_id, pose, robot_pose))
{
throw nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into global plan's frame");
}
transformed_plan.header.frame_id = costmap->getGlobalFrameID();
@ -139,7 +139,7 @@ bool pnkx_local_planner::transformGlobalPlan(
robot_nav_2d_msgs::Pose2DStamped costmap_pose;
if (!robot_nav_2d_utils::transformPose(tf, transformed_plan.header.frame_id, pose, costmap_pose))
{
throw nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame");
throw robot_nav_core2::PlannerTFException("Unable to transform robot pose into costmap's frame");
}
robot::log_assert(global_plan.poses.size() >= transformed_plan.poses.size(), "Global plan size is less than transformed plan size");
@ -182,7 +182,7 @@ bool pnkx_local_planner::transformGlobalPlan(
if (transformed_plan.poses.size() == 0)
{
throw nav_core2::PlannerTFException("Resulting plan has 0 poses in it.");
throw robot_nav_core2::PlannerTFException("Resulting plan has 0 poses in it.");
}
}
catch (tf3::LookupException& ex)

2
src/Libraries/common_msgs

@ -1 +1 @@
Subproject commit 5c276afb3469a4618e3d2f3523a27bff2f9fb3c8
Subproject commit 1df5ed676fe374c63665d434f4d8b2c9923d41a1

2
src/Libraries/costmap_2d

@ -1 +1 @@
Subproject commit b6733ae04cc2ec79409faf05bbae5f70a3c7fcd2
Subproject commit 80bde38f4d5dbb66bab5a8bd5c2c3faaa870aa9f

2
src/Libraries/data_convert

@ -1 +1 @@
Subproject commit 594f0fe49e40273a7e2e74e44f0d63fa4fe2cfea
Subproject commit d7eed928fbfb26d822fd4c4cb7ab84e3cb46866a

2
src/Libraries/geometry2

@ -1 +1 @@
Subproject commit 1aaeb4c59df4b1888630e26cea5c5edcdd13a46b
Subproject commit e0db8d0e20142df254de7232ada253c9ba4d0a08

2
src/Libraries/laser_geometry

@ -1 +1 @@
Subproject commit 831e4e00d79cbc218cd575aecdb3ce3041a37cb0
Subproject commit 7cb758a986f481ee91afb8be7b407b6329dd61b0

10
src/Libraries/robot_cpp/PLUGIN_LOADER_README.md
View File

@ -41,7 +41,7 @@ robot::PluginLoaderHelper loader;
std::string lib_path = loader.findLibraryPath("CustomPlanner");
if (!lib_path.empty()) {
// Sử dụng với boost::dll::import_alias
auto planner_loader = boost::dll::import_alias<nav_core::BaseGlobalPlanner::Ptr()>(
auto planner_loader = boost::dll::import_alias<robot_nav_core2::BaseGlobalPlanner::Ptr()>(
lib_path, "CustomPlanner", boost::dll::load_mode::append_decorations);
planner_ = planner_loader();
}
@ -53,14 +53,14 @@ Thay vì hardcode đường dẫn:
```cpp
// CŨ (hardcode):
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/libcustom_planner.so";
std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/libcustom_planner.so";
// MỚI (từ config):
robot::PluginLoaderHelper loader;
std::string path_file_so = loader.findLibraryPath(global_planner);
if (path_file_so.empty()) {
// Fallback nếu không tìm thấy
path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/libcustom_planner.so";
path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build/libcustom_planner.so";
}
```
@ -97,7 +97,7 @@ plugin_libraries:
library_path: "/path/to/libpnkx_local_planner.so"
search_paths:
- "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build"
- "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build"
- "/path/to/other/libraries"
```
@ -126,7 +126,7 @@ Helper sẽ tìm library theo thứ tự:
1. Đường dẫn trực tiếp (nếu absolute)
2. `$PNKX_NAV_CORE_CONFIG_DIR/boost_dll_plugins.yaml`
3. `$PNKX_NAV_CORE_DIR/config/boost_dll_plugins.yaml`
4. `/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/config/boost_dll_plugins.yaml`
4. `/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/config/boost_dll_plugins.yaml`
5. `../config/boost_dll_plugins.yaml` (relative paths)
- Symbol name có thể có namespace (ví dụ: `custom_planner::CustomPlanner`), helper sẽ tự động thử tìm cả tên đầy đủ và tên ngắn (`CustomPlanner`).

2
src/Libraries/robot_cpp/src/node_handle.cpp
View File

@ -877,7 +877,7 @@ namespace robot
// Try hardcoded fallback paths
std::vector<std::string> possible_paths = {
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/config",
"/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/config",
"../config",
"../../config",
"./config"};

4
src/Libraries/robot_cpp/src/plugin_loader_helper.cpp
View File

@ -36,7 +36,7 @@ PluginLoaderHelper::PluginLoaderHelper(robot::NodeHandle nh, const std::string&
// Thêm các subdirectories thường dùng
search_paths_.push_back(build_dir + "/src/Algorithms/Packages/global_planners");
search_paths_.push_back(build_dir + "/src/Algorithms/Packages/local_planners");
search_paths_.push_back(build_dir + "/src/Navigations/Cores/nav_core_adapter");
search_paths_.push_back(build_dir + "/src/Navigations/Cores/robot_nav_core2_adapter");
search_paths_.push_back(build_dir + "/src/Libraries/robot_costmap_2d");
}
}
@ -319,7 +319,7 @@ std::string PluginLoaderHelper::getBuildDirectory()
}
// Method 6: Hardcoded fallback
std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build";
std::string fallback = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core2/build";
if (std::filesystem::exists(fallback)) {
return fallback;
}

4
src/Libraries/robot_nav_2d_msgs/CMakeLists.txt
View File

@ -61,8 +61,8 @@ target_link_libraries(robot_nav_2d_msgs
# Add include directories from dependencies explicitly for Catkin build
if(BUILDING_WITH_CATKIN)
# Use relative paths from current source directory
# From robot_nav_2d_msgs (pnkx_nav_core/src/Libraries/robot_nav_2d_msgs)
# to robot_std_msgs (pnkx_nav_core/src/Libraries/common_msgs/robot_std_msgs) is ../common_msgs/robot_std_msgs
# From robot_nav_2d_msgs (pnkx_robot_nav_core2/src/Libraries/robot_nav_2d_msgs)
# to robot_std_msgs (pnkx_robot_nav_core2/src/Libraries/common_msgs/robot_std_msgs) is ../common_msgs/robot_std_msgs
get_filename_component(ROBOT_STD_MSGS_INCLUDE "${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/robot_std_msgs/include" ABSOLUTE)
get_filename_component(GEOMETRY_MSGS_INCLUDE "${CMAKE_CURRENT_SOURCE_DIR}/../common_msgs/robot_geometry_msgs/include" ABSOLUTE)
target_include_directories(robot_nav_2d_msgs INTERFACE

4
src/Libraries/robot_nav_2d_msgs/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>

105
src/Libraries/robot_nav_2d_utils/CMakeLists.txt
View File

@ -27,11 +27,20 @@ else()
endif()
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED COMPONENTS robot_xmlrpcpp)
find_package(catkin REQUIRED COMPONENTS
robot_nav_2d_msgs
robot_geometry_msgs
robot_nav_msgs
nav_grid
robot_nav_core2
tf3
robot_tf3_geometry_msgs
robot_cpp
)
catkin_package(
INCLUDE_DIRS include
LIBRARIES conversions path_ops polygons bounds tf_help robot_nav_2d_utils
LIBRARIES ${PROJECT_NAME}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help ${PROJECT_NAME}
# CATKIN_DEPENDS không cần vì dependencies không phải Catkin packages
DEPENDS console_bridge Boost
)
@ -42,19 +51,19 @@ find_package(console_bridge REQUIRED)
find_package(Boost REQUIRED COMPONENTS system thread)
# Libraries
add_library(conversions src/conversions.cpp)
target_include_directories(conversions
add_library(${PROJECT_NAME}_conversions src/conversions.cpp)
target_include_directories(${PROJECT_NAME}_conversions
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(conversions
target_link_libraries(${PROJECT_NAME}_conversions
PUBLIC
robot_nav_2d_msgs
robot_geometry_msgs
robot_nav_msgs
nav_grid
nav_core2
robot_nav_core2
robot_cpp
PRIVATE
console_bridge::console_bridge
@ -62,121 +71,123 @@ target_link_libraries(conversions
Boost::thread
)
set_target_properties(conversions PROPERTIES
set_target_properties(${PROJECT_NAME}_conversions PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
add_library(path_ops src/path_ops.cpp)
target_include_directories(path_ops
add_library(${PROJECT_NAME}_path_ops src/path_ops.cpp)
target_include_directories(${PROJECT_NAME}_path_ops
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(path_ops
target_link_libraries(${PROJECT_NAME}_path_ops
PUBLIC
robot_nav_2d_msgs
robot_geometry_msgs
robot_cpp
)
set_target_properties(path_ops PROPERTIES
set_target_properties(${PROJECT_NAME}_path_ops PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
add_library(polygons src/polygons.cpp src/footprint.cpp)
target_include_directories(polygons
add_library(${PROJECT_NAME}_polygons src/polygons.cpp src/footprint.cpp)
target_include_directories(${PROJECT_NAME}_polygons
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
if(robot_xmlrpcpp_FOUND)
target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
target_link_libraries(polygons
target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME}_polygons
PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
PRIVATE ${robot_xmlrpcpp_LIBRARIES})
elseif(XmlRpcCpp_FOUND)
target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
target_link_libraries(polygons
target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME}_polygons
PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
PRIVATE ${XmlRpcCpp_LIBRARIES})
else()
target_include_directories(polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
target_link_libraries(polygons
target_include_directories(${PROJECT_NAME}_polygons PRIVATE ${robot_xmlrpcpp_INCLUDE_DIRS})
target_link_libraries(${PROJECT_NAME}_polygons
PUBLIC robot_nav_2d_msgs robot_geometry_msgs robot_cpp
PRIVATE ${robot_xmlrpcpp_LIBRARIES})
endif()
set_target_properties(polygons PROPERTIES
set_target_properties(${PROJECT_NAME}_polygons PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
add_library(bounds src/bounds.cpp)
target_include_directories(bounds
add_library(${PROJECT_NAME}_bounds src/bounds.cpp)
target_include_directories(${PROJECT_NAME}_bounds
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(bounds
target_link_libraries(${PROJECT_NAME}_bounds
PUBLIC
nav_grid
nav_core2
robot_nav_core2
robot_cpp
)
set_target_properties(bounds PROPERTIES
set_target_properties(${PROJECT_NAME}_bounds PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
add_library(tf_help src/tf_help.cpp)
target_include_directories(tf_help
add_library(${PROJECT_NAME}_tf_help src/tf_help.cpp)
target_include_directories(${PROJECT_NAME}_tf_help
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(tf_help
target_link_libraries(${PROJECT_NAME}_tf_help
PUBLIC
robot_nav_2d_msgs
robot_geometry_msgs
nav_core2
robot_nav_msgs
robot_nav_core2
tf3
robot_tf3_geometry_msgs
robot_cpp
)
set_target_properties(tf_help PROPERTIES
set_target_properties(${PROJECT_NAME}_tf_help PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
# Create an INTERFACE library that represents all robot_nav_2d_utils libraries
add_library(robot_nav_2d_utils INTERFACE)
target_include_directories(robot_nav_2d_utils
add_library(${PROJECT_NAME} INTERFACE)
target_include_directories(${PROJECT_NAME}
INTERFACE
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(robot_nav_2d_utils
target_link_libraries(${PROJECT_NAME}
INTERFACE
conversions
path_ops
polygons
bounds
tf_help
${PROJECT_NAME}_conversions
${PROJECT_NAME}_path_ops
${PROJECT_NAME}_polygons
${PROJECT_NAME}_bounds
${PROJECT_NAME}_tf_help
robot_cpp
)
# Install header files
if(NOT BUILDING_WITH_CATKIN)
install(DIRECTORY include/robot_nav_2d_utils
install(DIRECTORY include/${PROJECT_NAME}
DESTINATION include
FILES_MATCHING PATTERN "*.h")
@ -187,23 +198,23 @@ if(NOT BUILDING_WITH_CATKIN)
endif()
# Install targets
install(TARGETS robot_nav_2d_utils conversions path_ops polygons bounds tf_help
EXPORT robot_nav_2d_utils-targets
install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_conversions ${PROJECT_NAME}_path_ops ${PROJECT_NAME}_polygons ${PROJECT_NAME}_bounds ${PROJECT_NAME}_tf_help
EXPORT ${PROJECT_NAME}-targets
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
# Export targets
install(EXPORT robot_nav_2d_utils-targets
FILE robot_nav_2d_utils-targets.cmake
NAMESPACE robot_nav_2d_utils::
DESTINATION lib/cmake/robot_nav_2d_utils)
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake
NAMESPACE ${PROJECT_NAME}::
DESTINATION lib/cmake/${PROJECT_NAME})
# Print configuration info
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Libraries: conversions, path_ops, polygons, bounds, tf_help")
message(STATUS "Dependencies: robot_nav_2d_msgs, robot_geometry_msgs, robot_nav_msgs, nav_grid, nav_core2, tf3, console_bridge, Boost")
message(STATUS "Libraries: ${PROJECT_NAME}_conversions, ${PROJECT_NAME}_path_ops, ${PROJECT_NAME}_polygons, ${PROJECT_NAME}_bounds, ${PROJECT_NAME}_tf_help")
message(STATUS "Dependencies: robot_nav_2d_msgs, robot_geometry_msgs, robot_nav_msgs, nav_grid, robot_nav_core2, tf3, console_bridge, Boost")
message(STATUS "=================================")

4
src/Libraries/robot_nav_2d_utils/README.md
View File

@ -1,6 +1,6 @@
# robot_nav_2d_utils
A handful of useful utility functions for nav_core2 packages.
* Bounds - Utilities for `nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds.
A handful of useful utility functions for robot_nav_core2 packages.
* Bounds - Utilities for `robot_nav_core2::Bounds` objects interacting with other messages/types and for dividing a `UIntBounds` into multiple smaller bounds.
* [Conversions](doc/Conversions.md) - Tools for converting between `robot_nav_2d_msgs` and other types.
* OdomSubscriber - subscribes to the standard `robot_nav_msgs::Odometry` message and provides access to the velocity component as a `robot_nav_2d_msgs::Twist`
* Parameters - a couple ROS parameter patterns

2
src/Libraries/robot_nav_2d_utils/doc/Conversions.md
View File

@ -51,4 +51,4 @@ Also, `robot_nav_msgs::Path posesToPath(std::vector<robot_geometry_msgs::PoseSta
## Bounds Transformations
| to `robot_nav_2d_msgs` | from `robot_nav_2d_msgs` |
| -- | -- |
|`robot_nav_2d_msgs::UIntBounds toMsg(nav_core2::UIntBounds)`|`nav_core2::UIntBounds fromMsg(robot_nav_2d_msgs::UIntBounds)`|
|`robot_nav_2d_msgs::UIntBounds toMsg(robot_nav_core2::UIntBounds)`|`robot_nav_core2::UIntBounds fromMsg(robot_nav_2d_msgs::UIntBounds)`|

4
src/Libraries/robot_nav_2d_utils/doc/PluginMux.md
View File

@ -2,7 +2,7 @@
# Plugin Mux
`PluginMux` is an organizer for switching between multiple different plugins of the same type. It may be easiest to see how it operates through an example. Let's say we have multiple global planners we would like to use at different times, with only one being active at a given time.
This means we have multiple `pluginlib` plugins to load that extend the `BaseGlobalPlanner` interface from the `nav_core` package. We define multiple namespaces in the `global_planner_namespaces` and load each of them. Here's an example parameter config.
This means we have multiple `pluginlib` plugins to load that extend the `BaseGlobalPlanner` interface from the `robot_nav_core2` package. We define multiple namespaces in the `global_planner_namespaces` and load each of them. Here's an example parameter config.
```
global_planner_namespaces:
@ -23,7 +23,7 @@ To advertise which plugin is active, we publish the namespace on a latched topic
This configuration is all created by creating a plugin mux with the following parameters (parameter names shown for convenience):
```
PluginMux(plugin_package = "nav_core",
PluginMux(plugin_package = "robot_nav_core2",
plugin_class = "BaseGlobalPlanner",
parameter_name = "global_planner_namespaces",
default_value = "global_planner/GlobalPlanner",

12
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/bounds.h
View File

@ -36,7 +36,7 @@
#define ROBOT_NAV_2D_UTILS_BOUNDS_H
#include <nav_grid/nav_grid_info.h>
#include <nav_core2/bounds.h>
#include <robot_nav_core2/bounds.h>
#include <vector>
/**
@ -50,14 +50,14 @@ namespace robot_nav_2d_utils
* @param info Info for the NavGrid
* @return bounds covering the entire NavGrid
*/
nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info);
robot_nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info);
/**
* @brief return an integral bounds that covers the entire NavGrid
* @param info Info for the NavGrid
* @return bounds covering the entire NavGrid
*/
nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info);
robot_nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info);
/**
* @brief Translate real-valued bounds to uint coordinates based on nav_grid info
@ -65,7 +65,7 @@ namespace robot_nav_2d_utils
* @param bounds floating point bounds object
* @returns corresponding UIntBounds for parameter
*/
nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::Bounds &bounds);
robot_nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::Bounds &bounds);
/**
* @brief Translate uint bounds to real-valued coordinates based on nav_grid info
@ -73,7 +73,7 @@ namespace robot_nav_2d_utils
* @param bounds UIntBounds object
* @returns corresponding floating point bounds for parameter
*/
nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds);
robot_nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::UIntBounds &bounds);
/**
* @brief divide the given bounds up into sub-bounds of roughly equal size
@ -83,7 +83,7 @@ namespace robot_nav_2d_utils
* @return vector of a maximum of n_cols*n_rows nonempty bounds
* @throws std::invalid_argument when n_cols or n_rows is zero
*/
std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds &original_bounds,
std::vector<robot_nav_core2::UIntBounds> divideBounds(const robot_nav_core2::UIntBounds &original_bounds,
unsigned int n_cols, unsigned int n_rows);
} // namespace robot_nav_2d_utils

6
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h
View File

@ -50,7 +50,7 @@
#include <robot_nav_msgs/MapMetaData.h>
#include <robot_nav_msgs/Path.h>
#include <nav_grid/nav_grid.h>
#include <nav_core2/bounds.h>
#include <robot_nav_core2/bounds.h>
// #include <tf/tf.h>
#include <vector>
#include <string>
@ -98,8 +98,8 @@ namespace robot_nav_2d_utils
robot_nav_msgs::MapMetaData infoToInfo(const nav_grid::NavGridInfo &info);
// Bounds Transformations
robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds);
nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg);
robot_nav_2d_msgs::UIntBounds toMsg(const robot_nav_core2::UIntBounds &bounds);
robot_nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg);
} // namespace robot_nav_2d_utils

2
src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h
View File

@ -35,7 +35,7 @@
#ifndef ROBOT_NAV_2D_UTILS_TF_HELP_H
#define ROBOT_NAV_2D_UTILS_TF_HELP_H
#include <nav_core2/common.h>
#include <robot_nav_core2/common.h>
#include <robot_geometry_msgs/PoseStamped.h>
#include <robot_nav_2d_msgs/Pose2DStamped.h>
#include <string>

14
src/Libraries/robot_nav_2d_utils/package.xml
View File

@ -19,7 +19,17 @@
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>robot_xmlrpcpp</build_depend>
<run_depend>robot_xmlrpcpp</run_depend>
<build_depend>robot_nav_2d_msgs</build_depend>
<build_depend>robot_geometry_msgs</build_depend>
<build_depend>robot_nav_msgs</build_depend>
<build_depend>nav_grid</build_depend>
<build_depend>robot_nav_core2</build_depend>
<build_depend>robot_cpp</build_depend>
<run_depend>robot_nav_2d_msgs</run_depend>
<run_depend>robot_geometry_msgs</run_depend>
<run_depend>robot_nav_msgs</run_depend>
<run_depend>nav_grid</run_depend>
<run_depend>robot_nav_core2</run_depend>
<run_depend>robot_cpp</run_depend>
</package>

22
src/Libraries/robot_nav_2d_utils/src/bounds.cpp
View File

@ -40,35 +40,35 @@
namespace robot_nav_2d_utils
{
nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)
robot_nav_core2::Bounds getFullBounds(const nav_grid::NavGridInfo &info)
{
return nav_core2::Bounds(info.origin_x, info.origin_y,
return robot_nav_core2::Bounds(info.origin_x, info.origin_y,
info.origin_x + info.resolution * info.width, info.origin_y + info.resolution * info.height);
}
nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info)
robot_nav_core2::UIntBounds getFullUIntBounds(const nav_grid::NavGridInfo &info)
{
// bounds are inclusive, so we subtract one
return nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1);
return robot_nav_core2::UIntBounds(0, 0, info.width - 1, info.height - 1);
}
nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::Bounds &bounds)
robot_nav_core2::UIntBounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::Bounds &bounds)
{
unsigned int g_min_x, g_min_y, g_max_x, g_max_y;
worldToGridBounded(info, bounds.getMinX(), bounds.getMinY(), g_min_x, g_min_y);
worldToGridBounded(info, bounds.getMaxX(), bounds.getMaxY(), g_max_x, g_max_y);
return nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y);
return robot_nav_core2::UIntBounds(g_min_x, g_min_y, g_max_x, g_max_y);
}
nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const nav_core2::UIntBounds &bounds)
robot_nav_core2::Bounds translateBounds(const nav_grid::NavGridInfo &info, const robot_nav_core2::UIntBounds &bounds)
{
double min_x, min_y, max_x, max_y;
gridToWorld(info, bounds.getMinX(), bounds.getMinY(), min_x, min_y);
gridToWorld(info, bounds.getMaxX(), bounds.getMaxY(), max_x, max_y);
return nav_core2::Bounds(min_x, min_y, max_x, max_y);
return robot_nav_core2::Bounds(min_x, min_y, max_x, max_y);
}
std::vector<nav_core2::UIntBounds> divideBounds(const nav_core2::UIntBounds &original_bounds,
std::vector<robot_nav_core2::UIntBounds> divideBounds(const robot_nav_core2::UIntBounds &original_bounds,
unsigned int n_cols, unsigned int n_rows)
{
if (n_cols * n_rows == 0)
@ -81,7 +81,7 @@ namespace robot_nav_2d_utils
unsigned int small_width = static_cast<unsigned int>(ceil(static_cast<double>(full_width) / n_cols)),
small_height = static_cast<unsigned int>(ceil(static_cast<double>(full_height) / n_rows));
std::vector<nav_core2::UIntBounds> divided;
std::vector<robot_nav_core2::UIntBounds> divided;
for (unsigned int row = 0; row < n_rows; row++)
{
@ -92,7 +92,7 @@ namespace robot_nav_2d_utils
{
unsigned int min_x = original_bounds.getMinX() + small_width * col;
unsigned int max_x = std::min(min_x + small_width - 1, original_bounds.getMaxX());
nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y);
robot_nav_core2::UIntBounds sub(min_x, min_y, max_x, max_y);
if (!sub.isEmpty())
divided.push_back(sub);
}

54
src/Libraries/robot_nav_2d_utils/src/conversions.cpp
View File

@ -126,17 +126,17 @@ namespace robot_nav_2d_utils
return pose;
}
// robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_geometry_msgs::Pose2D& pose2d,
// const std::string& frame, const robot::Time& stamp)
// {
// robot_geometry_msgs::PoseStamped pose;
// pose.header.frame_id = frame;
// pose.header.stamp = stamp;
// pose.pose.position.x = pose2d.x;
// pose.pose.position.y = pose2d.y;
// // pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
// return pose;
// }
robot_geometry_msgs::PoseStamped pose2DToPoseStamped(const robot_geometry_msgs::Pose2D& pose2d,
const std::string& frame, const robot::Time& stamp)
{
robot_geometry_msgs::PoseStamped pose;
pose.header.frame_id = frame;
pose.header.stamp = stamp;
pose.pose.position.x = pose2d.x;
pose.pose.position.y = pose2d.y;
// pose.pose.orientation = tf::createQuaternionMsgFromYaw(pose2d.theta);
return pose;
}
robot_nav_2d_msgs::Path2D pathToPath(const robot_nav_msgs::Path &path)
{
@ -186,19 +186,19 @@ namespace robot_nav_2d_utils
return path;
}
// robot_nav_msgs::Path poses2DToPath(const std::vector<robot_geometry_msgs::Pose2D>& poses,
// const std::string& frame, const robot::Time& stamp)
// {
// robot_nav_msgs::Path path;
// path.poses.resize(poses.size());
// path.header.frame_id = frame;
// path.header.stamp = stamp;
// for (unsigned int i = 0; i < poses.size(); i++)
// {
// path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
// }
// return path;
// }
robot_nav_msgs::Path poses2DToPath(const std::vector<robot_geometry_msgs::Pose2D>& poses,
const std::string& frame, const robot::Time& stamp)
{
robot_nav_msgs::Path path;
path.poses.resize(poses.size());
path.header.frame_id = frame;
path.header.stamp = stamp;
for (unsigned int i = 0; i < poses.size(); i++)
{
path.poses[i] = pose2DToPoseStamped(poses[i], frame, stamp);
}
return path;
}
robot_nav_msgs::Path pathToPath(const robot_nav_2d_msgs::Path2D &path2d)
{
@ -318,7 +318,7 @@ namespace robot_nav_2d_utils
return metadata;
}
robot_nav_2d_msgs::UIntBounds toMsg(const nav_core2::UIntBounds &bounds)
robot_nav_2d_msgs::UIntBounds toMsg(const robot_nav_core2::UIntBounds &bounds)
{
robot_nav_2d_msgs::UIntBounds msg;
msg.min_x = bounds.getMinX();
@ -328,9 +328,9 @@ namespace robot_nav_2d_utils
return msg;
}
nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
robot_nav_core2::UIntBounds fromMsg(const robot_nav_2d_msgs::UIntBounds &msg)
{
return nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
return robot_nav_core2::UIntBounds(msg.min_x, msg.min_y, msg.max_x, msg.max_y);
}
} // namespace robot_nav_2d_utils

2
src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp
View File

@ -37,7 +37,7 @@
#include <vector>
using robot_nav_2d_utils::divideBounds;
using nav_core2::UIntBounds;
using robot_nav_core2::UIntBounds;
/**
* @brief Count the values in a grid.

3
src/Libraries/tf3/package.xml
View File

@ -19,8 +19,5 @@
<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>

2
src/Libraries/voxel_grid

@ -1 +1 @@
Subproject commit b19cec9f4d4823d7cbbd741d60c239753532e4db
Subproject commit 4303737ff92db94dc1bcde2d8a3bef527e76355a

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

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