diff --git a/CMakeLists.txt b/CMakeLists.txt index f45e38a..ed3b70c 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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) diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index b1f0545..99bc5aa 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -449,12 +449,12 @@ extern "C" bool navigation_get_feedback(NavigationHandle handle, NavFeedback* ou try { robot::move_base_core::BaseNavigation* nav = static_cast(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; } - return false; } catch (...) { return false; } diff --git a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt index dd850bb..2adf05e 100644 --- a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt +++ b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt @@ -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 ) + +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 - PUBLIC - ${PACKAGES_DIR} - robot_cpp - PRIVATE - Boost::filesystem - Boost::system - ) +# Link libraries +if(BUILDING_WITH_CATKIN) + target_link_libraries(score_algorithm + PUBLIC + ${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 $ $) -# 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 - FILE score_algorithm-targets.cmake - DESTINATION lib/cmake/score_algorithm) +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) diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h index 2837857..8bc9aeb 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/score_algorithm.h @@ -7,9 +7,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include diff --git a/src/Algorithms/Cores/score_algorithm/package.xml b/src/Algorithms/Cores/score_algorithm/package.xml index ebab52c..f607334 100644 --- a/src/Algorithms/Cores/score_algorithm/package.xml +++ b/src/Algorithms/Cores/score_algorithm/package.xml @@ -19,8 +19,19 @@ catkin - libconsole-bridge-dev - - libconsole-bridge-dev + robot_nav_2d_msgs + robot_nav_2d_utils + robot_nav_core + robot_nav_core2 + robot_cpp + mkt_msgs + angles + robot_nav_2d_msgs + robot_nav_2d_utils + robot_nav_core + robot_nav_core2 + robot_cpp + mkt_msgs + angles \ No newline at end of file diff --git a/src/Algorithms/Libraries/angles/CMakeLists.txt b/src/Algorithms/Libraries/angles/CMakeLists.txt index a60896c..db59f61 100644 --- a/src/Algorithms/Libraries/angles/CMakeLists.txt +++ b/src/Algorithms/Libraries/angles/CMakeLists.txt @@ -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 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 diff --git a/src/Algorithms/Libraries/angles/package.xml b/src/Algorithms/Libraries/angles/package.xml index 2bc4294..9eddac1 100644 --- a/src/Algorithms/Libraries/angles/package.xml +++ b/src/Algorithms/Libraries/angles/package.xml @@ -19,8 +19,4 @@ catkin - libconsole-bridge-dev - - libconsole-bridge-dev - \ No newline at end of file diff --git a/src/Algorithms/Libraries/kalman/CMakeLists.txt b/src/Algorithms/Libraries/kalman/CMakeLists.txt index 27e8e41..e7b8621 100755 --- a/src/Algorithms/Libraries/kalman/CMakeLists.txt +++ b/src/Algorithms/Libraries/kalman/CMakeLists.txt @@ -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 + $<$:${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 diff --git a/src/Algorithms/Libraries/kalman/package.xml b/src/Algorithms/Libraries/kalman/package.xml index 7282140..9391990 100644 --- a/src/Algorithms/Libraries/kalman/package.xml +++ b/src/Algorithms/Libraries/kalman/package.xml @@ -19,8 +19,4 @@ catkin - libconsole-bridge-dev - - libconsole-bridge-dev - \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index bdb358f..89880a3 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -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 - PUBLIC - ${PACKAGES_DIR} - robot_cpp - Boost::system - Eigen3::Eigen -) +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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h index f3fa588..260a595 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/bicycle.h @@ -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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h index ffd165f..0faaa06 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/go_straight.h @@ -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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h index 631c996..62ade0e 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/pta.h @@ -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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h index 599f915..413eb08 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/bicycle/rotate_to_goal.h @@ -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 diff --git a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h index 2a05e3e..8cf6c55 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h +++ b/src/Algorithms/Libraries/mkt_algorithm/include/mkt_algorithm/diff/diff_predictive_trajectory.h @@ -8,8 +8,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include diff --git a/src/Algorithms/Libraries/mkt_algorithm/package.xml b/src/Algorithms/Libraries/mkt_algorithm/package.xml index 22f6eeb..4a9b2bd 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/package.xml +++ b/src/Algorithms/Libraries/mkt_algorithm/package.xml @@ -18,9 +18,27 @@ http://www.ros.org/wiki/mkt_algorithm catkin + 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 - libconsole-bridge-dev - - libconsole-bridge-dev - + 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 \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp index 26f2316..c35abf5 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycel_go_straight.cpp @@ -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; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp index 2c85547..39f63c2 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_pure_pursuit.cpp @@ -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; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp index 0555d4f..e9a7980 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/bicycle/bicycle_rotate_to_goal.cpp @@ -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; diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp index 273e5ec..6f8989b 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_predictive_trajectory.cpp @@ -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 diff --git a/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt b/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt index 99896dc..55bade6 100644 --- a/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_msgs/CMakeLists.txt @@ -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 $ ) -# 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) diff --git a/src/Algorithms/Libraries/mkt_msgs/package.xml b/src/Algorithms/Libraries/mkt_msgs/package.xml index 43197e1..c1573bf 100644 --- a/src/Algorithms/Libraries/mkt_msgs/package.xml +++ b/src/Algorithms/Libraries/mkt_msgs/package.xml @@ -18,9 +18,10 @@ http://www.ros.org/wiki/mkt_msgs catkin + + robot_nav_2d_msgs + geometry_msgs - libconsole-bridge-dev - - libconsole-bridge-dev - + robot_nav_2d_msgs + geometry_msgs \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt index acfe029..0aa2845 100644 --- a/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt @@ -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 - PUBLIC ${PACKAGES_DIR} - PRIVATE Boost::boost -) +# 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 - PUBLIC ${PACKAGES_DIR} - PRIVATE Boost::boost -) +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 $ ) +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 diff --git a/src/Algorithms/Libraries/mkt_plugins/package.xml b/src/Algorithms/Libraries/mkt_plugins/package.xml index 128054b..a82e9a3 100644 --- a/src/Algorithms/Libraries/mkt_plugins/package.xml +++ b/src/Algorithms/Libraries/mkt_plugins/package.xml @@ -19,8 +19,31 @@ catkin - libconsole-bridge-dev + score_algorithm + score_algorithm - libconsole-bridge-dev + robot_nav_2d_msgs + robot_nav_2d_msgs + + robot_nav_2d_utils + robot_nav_2d_utils + + robot_nav_core2 + robot_nav_core2 + + geometry_msgs + geometry_msgs + + robot_nav_msgs + robot_nav_msgs + + robot_std_msgs + robot_std_msgs + + robot_sensor_msgs + robot_sensor_msgs + + angles + angles \ No newline at end of file diff --git a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp index 6bcb4b9..8b16943 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include namespace mkt_plugins diff --git a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp index 60d7570..28ee174 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/standard_traj_generator.cpp @@ -2,7 +2,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/Algorithms/Packages/global_planners/custom_planner b/src/Algorithms/Packages/global_planners/custom_planner index 60e9c56..0b01c22 160000 --- a/src/Algorithms/Packages/global_planners/custom_planner +++ b/src/Algorithms/Packages/global_planners/custom_planner @@ -1 +1 @@ -Subproject commit 60e9c5673f8fd646d628f843ef73e71d1d9b2a17 +Subproject commit 0b01c22019b66c905d6d67611d333742e1e37a55 diff --git a/src/Algorithms/Packages/global_planners/dock_planner b/src/Algorithms/Packages/global_planners/dock_planner index b5a1b7b..22aa83f 160000 --- a/src/Algorithms/Packages/global_planners/dock_planner +++ b/src/Algorithms/Packages/global_planners/dock_planner @@ -1 +1 @@ -Subproject commit b5a1b7b6d8367b5812b32f2ca51bbed6e2a4c99a +Subproject commit 22aa83fe5a66f5c090bcb8c1fafb41cfe777ebc1 diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt index 81d0db2..b1a2c6f 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt @@ -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 - PUBLIC ${PACKAGES_DIR} - PUBLIC robot_cpp - PRIVATE Boost::boost -) +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 $ ) -# 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 diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h b/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h index da06364..b07b780 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h +++ b/src/Algorithms/Packages/global_planners/two_points_planner/include/two_points_planner/two_points_planner.h @@ -5,7 +5,7 @@ #include #include #include -#include +#include #include #include @@ -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: diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/package.xml b/src/Algorithms/Packages/global_planners/two_points_planner/package.xml index 29df275..9f68659 100644 --- a/src/Algorithms/Packages/global_planners/two_points_planner/package.xml +++ b/src/Algorithms/Packages/global_planners/two_points_planner/package.xml @@ -19,8 +19,28 @@ catkin - libconsole-bridge-dev + robot_costmap_2d + robot_costmap_2d - libconsole-bridge-dev + robot_nav_core + robot_nav_core + + geometry_msgs + geometry_msgs + + robot_nav_msgs + robot_nav_msgs + + robot_std_msgs + robot_std_msgs + + tf3 + tf3 + + robot_tf3_geometry_msgs + robot_tf3_geometry_msgs + + robot_cpp + robot_cpp \ No newline at end of file diff --git a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp index 22069eb..3a72670 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp +++ b/src/Algorithms/Packages/global_planners/two_points_planner/src/two_points_planner.cpp @@ -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(); } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt index dfc3424..f073bab 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt @@ -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 - PUBLIC ${PACKAGES_DIR} - PRIVATE Boost::boost -) +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} - PUBLIC ${PROJECT_NAME}_utils - PUBLIC ${PACKAGES_DIR} - PRIVATE Boost::boost -) +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 diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h index 6a9ed18..e9b0f03 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_docking_local_planner.h @@ -1,7 +1,7 @@ #ifndef _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ #define _PNKX_DOCKING_LOCAL_PLANNER_H_INCLUDED_ -#include +#include #include 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 docking_planner_loader_; + std::function docking_planner_loader_; std::function docking_nav_loader_; robot::WallTimer detected_timeout_wt_, delayed_wt_; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h index 72953a4..6ac81c1 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_go_straight_local_planner.h @@ -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;; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h index aef29e6..f2edc38 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_local_planner.h @@ -3,7 +3,7 @@ #include #include -#include +#include #include #include #include @@ -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(); diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h index 1601e0c..8e3cf8a 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/pnkx_rotate_local_planner.h @@ -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; diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h index 13e6407..e6653c2 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/include/pnkx_local_planner/transforms.h @@ -5,8 +5,8 @@ #include #include -#include -#include +#include +#include #include #include diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/package.xml b/src/Algorithms/Packages/local_planners/pnkx_local_planner/package.xml index dd01bd0..cfca858 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/package.xml +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/package.xml @@ -19,8 +19,49 @@ catkin - libconsole-bridge-dev + geometry_msgs + geometry_msgs - libconsole-bridge-dev + robot_nav_msgs + robot_nav_msgs + + robot_std_msgs + robot_std_msgs + + robot_sensor_msgs + robot_sensor_msgs + + robot_visualization_msgs + robot_visualization_msgs + + robot_nav_2d_utils + robot_nav_2d_utils + + robot_nav_core2 + robot_nav_core2 + + mkt_msgs + mkt_msgs + + score_algorithm + score_algorithm + + robot_costmap_2d + robot_costmap_2d + + tf3 + tf3 + + robot_tf3_geometry_msgs + robot_tf3_geometry_msgs + + robot_tf3_sensor_msgs + robot_tf3_sensor_msgs + + robot_cpp + robot_cpp + + angles + angles \ No newline at end of file diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp index 49e0fea..74375cf 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_docking_local_planner.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include #include @@ -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( 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( 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( 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( 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( + 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( 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( 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(); } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp index bfb57bb..c4ad108 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_go_straight_local_planner.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -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( 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( 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( 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(); } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp index 52d0e91..46578e1 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_local_planner.cpp @@ -1,5 +1,5 @@ #include -#include +#include #include #include @@ -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(); } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp index ddd8803..8b728d6 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/pnkx_rotate_local_planner.cpp @@ -1,7 +1,7 @@ #include "pnkx_local_planner/pnkx_rotate_local_planner.h" #include "pnkx_local_planner/transforms.h" -#include +#include #include #include #include @@ -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( 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( 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( 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(); } diff --git a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp index 3ff743f..6579875 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/src/transforms.cpp @@ -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) diff --git a/src/Libraries/common_msgs b/src/Libraries/common_msgs index 5c276af..1df5ed6 160000 --- a/src/Libraries/common_msgs +++ b/src/Libraries/common_msgs @@ -1 +1 @@ -Subproject commit 5c276afb3469a4618e3d2f3523a27bff2f9fb3c8 +Subproject commit 1df5ed676fe374c63665d434f4d8b2c9923d41a1 diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index b6733ae..80bde38 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit b6733ae04cc2ec79409faf05bbae5f70a3c7fcd2 +Subproject commit 80bde38f4d5dbb66bab5a8bd5c2c3faaa870aa9f diff --git a/src/Libraries/data_convert b/src/Libraries/data_convert index 594f0fe..d7eed92 160000 --- a/src/Libraries/data_convert +++ b/src/Libraries/data_convert @@ -1 +1 @@ -Subproject commit 594f0fe49e40273a7e2e74e44f0d63fa4fe2cfea +Subproject commit d7eed928fbfb26d822fd4c4cb7ab84e3cb46866a diff --git a/src/Libraries/geometry2 b/src/Libraries/geometry2 index 1aaeb4c..e0db8d0 160000 --- a/src/Libraries/geometry2 +++ b/src/Libraries/geometry2 @@ -1 +1 @@ -Subproject commit 1aaeb4c59df4b1888630e26cea5c5edcdd13a46b +Subproject commit e0db8d0e20142df254de7232ada253c9ba4d0a08 diff --git a/src/Libraries/laser_geometry b/src/Libraries/laser_geometry index 831e4e0..7cb758a 160000 --- a/src/Libraries/laser_geometry +++ b/src/Libraries/laser_geometry @@ -1 +1 @@ -Subproject commit 831e4e00d79cbc218cd575aecdb3ce3041a37cb0 +Subproject commit 7cb758a986f481ee91afb8be7b407b6329dd61b0 diff --git a/src/Libraries/robot_cpp/PLUGIN_LOADER_README.md b/src/Libraries/robot_cpp/PLUGIN_LOADER_README.md index 83e9730..5232f26 100644 --- a/src/Libraries/robot_cpp/PLUGIN_LOADER_README.md +++ b/src/Libraries/robot_cpp/PLUGIN_LOADER_README.md @@ -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( + auto planner_loader = boost::dll::import_alias( 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`). diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp index 1dfbb4d..b48b686 100644 --- a/src/Libraries/robot_cpp/src/node_handle.cpp +++ b/src/Libraries/robot_cpp/src/node_handle.cpp @@ -877,7 +877,7 @@ namespace robot // Try hardcoded fallback paths std::vector 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"}; diff --git a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp index 6325556..00c90c1 100644 --- a/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp +++ b/src/Libraries/robot_cpp/src/plugin_loader_helper.cpp @@ -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; } diff --git a/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt b/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt index 775ff89..50b06f0 100755 --- a/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt +++ b/src/Libraries/robot_nav_2d_msgs/CMakeLists.txt @@ -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 diff --git a/src/Libraries/robot_nav_2d_msgs/package.xml b/src/Libraries/robot_nav_2d_msgs/package.xml index efb49af..ec19a12 100755 --- a/src/Libraries/robot_nav_2d_msgs/package.xml +++ b/src/Libraries/robot_nav_2d_msgs/package.xml @@ -19,8 +19,4 @@ catkin - libconsole-bridge-dev - - libconsole-bridge-dev - \ No newline at end of file diff --git a/src/Libraries/robot_nav_2d_utils/CMakeLists.txt b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt index f1dc735..bed6cbc 100755 --- a/src/Libraries/robot_nav_2d_utils/CMakeLists.txt +++ b/src/Libraries/robot_nav_2d_utils/CMakeLists.txt @@ -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 $ $ ) -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 $ $ ) -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 $ $ ) 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 $ $ ) -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 $ $ ) -target_link_libraries(tf_help - PUBLIC - robot_nav_2d_msgs - robot_geometry_msgs - nav_core2 - tf3 - robot_cpp +target_link_libraries(${PROJECT_NAME}_tf_help + PUBLIC + robot_nav_2d_msgs + robot_geometry_msgs + 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 $ $ ) -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 "=================================") diff --git a/src/Libraries/robot_nav_2d_utils/README.md b/src/Libraries/robot_nav_2d_utils/README.md index d46106f..7ad4dc3 100755 --- a/src/Libraries/robot_nav_2d_utils/README.md +++ b/src/Libraries/robot_nav_2d_utils/README.md @@ -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 diff --git a/src/Libraries/robot_nav_2d_utils/doc/Conversions.md b/src/Libraries/robot_nav_2d_utils/doc/Conversions.md index 88a9807..6491f41 100755 --- a/src/Libraries/robot_nav_2d_utils/doc/Conversions.md +++ b/src/Libraries/robot_nav_2d_utils/doc/Conversions.md @@ -51,4 +51,4 @@ Also, `robot_nav_msgs::Path posesToPath(std::vector -#include +#include #include /** @@ -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 divideBounds(const nav_core2::UIntBounds &original_bounds, + std::vector divideBounds(const robot_nav_core2::UIntBounds &original_bounds, unsigned int n_cols, unsigned int n_rows); } // namespace robot_nav_2d_utils diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h index 1e2c862..f088ac0 100755 --- a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/conversions.h @@ -50,7 +50,7 @@ #include #include #include -#include +#include // #include #include #include @@ -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 diff --git a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h index 369dd7e..18d14d7 100755 --- a/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h +++ b/src/Libraries/robot_nav_2d_utils/include/robot_nav_2d_utils/tf_help.h @@ -35,7 +35,7 @@ #ifndef ROBOT_NAV_2D_UTILS_TF_HELP_H #define ROBOT_NAV_2D_UTILS_TF_HELP_H -#include +#include #include #include #include diff --git a/src/Libraries/robot_nav_2d_utils/package.xml b/src/Libraries/robot_nav_2d_utils/package.xml index de0118f..2549adb 100755 --- a/src/Libraries/robot_nav_2d_utils/package.xml +++ b/src/Libraries/robot_nav_2d_utils/package.xml @@ -19,7 +19,17 @@ catkin - robot_xmlrpcpp - robot_xmlrpcpp + robot_nav_2d_msgs + robot_geometry_msgs + robot_nav_msgs + nav_grid + robot_nav_core2 + robot_cpp + robot_nav_2d_msgs + robot_geometry_msgs + robot_nav_msgs + nav_grid + robot_nav_core2 + robot_cpp \ No newline at end of file diff --git a/src/Libraries/robot_nav_2d_utils/src/bounds.cpp b/src/Libraries/robot_nav_2d_utils/src/bounds.cpp index 241317e..9c69dee 100755 --- a/src/Libraries/robot_nav_2d_utils/src/bounds.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/bounds.cpp @@ -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 divideBounds(const nav_core2::UIntBounds &original_bounds, + std::vector 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(ceil(static_cast(full_width) / n_cols)), small_height = static_cast(ceil(static_cast(full_height) / n_rows)); - std::vector divided; + std::vector 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); } diff --git a/src/Libraries/robot_nav_2d_utils/src/conversions.cpp b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp index 455c836..70282f7 100755 --- a/src/Libraries/robot_nav_2d_utils/src/conversions.cpp +++ b/src/Libraries/robot_nav_2d_utils/src/conversions.cpp @@ -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& 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& 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 diff --git a/src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp b/src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp index 60755ed..5118951 100755 --- a/src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp +++ b/src/Libraries/robot_nav_2d_utils/test/bounds_test.cpp @@ -37,7 +37,7 @@ #include using robot_nav_2d_utils::divideBounds; -using nav_core2::UIntBounds; +using robot_nav_core2::UIntBounds; /** * @brief Count the values in a grid. diff --git a/src/Libraries/tf3/package.xml b/src/Libraries/tf3/package.xml index 5e4811d..e2e74f8 100644 --- a/src/Libraries/tf3/package.xml +++ b/src/Libraries/tf3/package.xml @@ -19,8 +19,5 @@ catkin - libconsole-bridge-dev - - libconsole-bridge-dev \ No newline at end of file diff --git a/src/Libraries/voxel_grid b/src/Libraries/voxel_grid index b19cec9..4303737 160000 --- a/src/Libraries/voxel_grid +++ b/src/Libraries/voxel_grid @@ -1 +1 @@ -Subproject commit b19cec9f4d4823d7cbbd741d60c239753532e4db +Subproject commit 4303737ff92db94dc1bcde2d8a3bef527e76355a diff --git a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h index 2b6a97e..f0a86ac 100755 --- a/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h +++ b/src/Navigations/Cores/move_base_core/include/move_base_core/navigation.h @@ -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 diff --git a/src/Navigations/Cores/nav_core/CMakeLists.txt b/src/Navigations/Cores/nav_core/CMakeLists.txt deleted file mode 100644 index 89baf5f..0000000 --- a/src/Navigations/Cores/nav_core/CMakeLists.txt +++ /dev/null @@ -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 - $ - $ -) - -# 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 "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core2/CMakeLists.txt b/src/Navigations/Cores/nav_core2/CMakeLists.txt deleted file mode 100755 index b0d75f4..0000000 --- a/src/Navigations/Cores/nav_core2/CMakeLists.txt +++ /dev/null @@ -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 - $ - $ -) - -# 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 "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core2/package.xml b/src/Navigations/Cores/nav_core2/package.xml deleted file mode 100644 index 9fa81fe..0000000 --- a/src/Navigations/Cores/nav_core2/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - nav_core2 - 0.7.10 - - 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. - - Tully Foote - Eitan Marder-Eppstein - Wim Meeussen - Tully Foote - BSD - - http://www.ros.org/wiki/nav_core2 - - catkin - - libconsole-bridge-dev - - libconsole-bridge-dev - - \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt deleted file mode 100755 index 04f9b4a..0000000 --- a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt +++ /dev/null @@ -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 - $ - $) - -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 - $ - $) - -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 - $ - $) - -# 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 "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core_adapter/README.md b/src/Navigations/Cores/nav_core_adapter/README.md deleted file mode 100755 index 069d753..0000000 --- a/src/Navigations/Cores/nav_core_adapter/README.md +++ /dev/null @@ -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. diff --git a/src/Navigations/Cores/nav_core_adapter/test/unload_test.launch b/src/Navigations/Cores/nav_core_adapter/test/unload_test.launch deleted file mode 100755 index c1ddad8..0000000 --- a/src/Navigations/Cores/nav_core_adapter/test/unload_test.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - diff --git a/src/Navigations/Cores/nav_core/CHANGELOG.rst b/src/Navigations/Cores/robot_nav_core/CHANGELOG.rst similarity index 98% rename from src/Navigations/Cores/nav_core/CHANGELOG.rst rename to src/Navigations/Cores/robot_nav_core/CHANGELOG.rst index 1817763..06388d8 100755 --- a/src/Navigations/Cores/nav_core/CHANGELOG.rst +++ b/src/Navigations/Cores/robot_nav_core/CHANGELOG.rst @@ -1,5 +1,5 @@ ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package nav_core +Changelog for package robot_nav_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 1.17.3 (2023-01-10) diff --git a/src/Navigations/Cores/robot_nav_core/CMakeLists.txt b/src/Navigations/Cores/robot_nav_core/CMakeLists.txt new file mode 100644 index 0000000..2063b22 --- /dev/null +++ b/src/Navigations/Cores/robot_nav_core/CMakeLists.txt @@ -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 + $ + $ +) + +# ======================================================== +# 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 "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_global_planner.h similarity index 95% rename from src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h rename to src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_global_planner.h index 536f2bd..d66d168 100755 --- a/src/Navigations/Cores/nav_core/include/nav_core/base_global_planner.h +++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_global_planner.h @@ -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 #include #include #include -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 \ No newline at end of file +#endif // ROBOT_NAV_CORE_BASE_GLOBAL_PLANNER_H \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h similarity index 95% rename from src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h rename to src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h index 8753212..8470a55 100755 --- a/src/Navigations/Cores/nav_core/include/nav_core/base_local_planner.h +++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/base_local_planner.h @@ -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 #include @@ -43,7 +43,7 @@ #include #include -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 diff --git a/src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/parameter_magic.h similarity index 94% rename from src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h rename to src/Navigations/Cores/robot_nav_core/include/robot_nav_core/parameter_magic.h index 79889c1..a5794d1 100755 --- a/src/Navigations/Cores/nav_core/include/nav_core/parameter_magic.h +++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/parameter_magic.h @@ -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 diff --git a/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/recovery_behavior.h similarity index 94% rename from src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h rename to src/Navigations/Cores/robot_nav_core/include/robot_nav_core/recovery_behavior.h index 2616cd6..1e56687 100755 --- a/src/Navigations/Cores/nav_core/include/nav_core/recovery_behavior.h +++ b/src/Navigations/Cores/robot_nav_core/include/robot_nav_core/recovery_behavior.h @@ -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 #include #include -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 diff --git a/src/Navigations/Cores/nav_core/package.xml b/src/Navigations/Cores/robot_nav_core/package.xml similarity index 58% rename from src/Navigations/Cores/nav_core/package.xml rename to src/Navigations/Cores/robot_nav_core/package.xml index 25bab09..55c2805 100644 --- a/src/Navigations/Cores/nav_core/package.xml +++ b/src/Navigations/Cores/robot_nav_core/package.xml @@ -1,9 +1,9 @@ - nav_core + robot_nav_core 0.7.10 - 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 @@ Tully Foote BSD - http://www.ros.org/wiki/nav_core + http://www.ros.org/wiki/robot_nav_core catkin - libconsole-bridge-dev + robot_costmap_2d + robot_costmap_2d - libconsole-bridge-dev + tf3 + tf3 + + robot_protocol_msgs + robot_protocol_msgs \ No newline at end of file diff --git a/src/Navigations/Cores/robot_nav_core2/CMakeLists.txt b/src/Navigations/Cores/robot_nav_core2/CMakeLists.txt new file mode 100755 index 0000000..83eaf01 --- /dev/null +++ b/src/Navigations/Cores/robot_nav_core2/CMakeLists.txt @@ -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 + $ + $ +) + +# ======================================================== +# 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 "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core2/README.md b/src/Navigations/Cores/robot_nav_core2/README.md similarity index 71% rename from src/Navigations/Cores/nav_core2/README.md rename to src/Navigations/Cores/robot_nav_core2/README.md index e45e938..99a4b9e 100755 --- a/src/Navigations/Cores/nav_core2/README.md +++ b/src/Navigations/Cores/robot_nav_core2/README.md @@ -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` 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` 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_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). diff --git a/src/Navigations/Cores/nav_core2/doc/exceptions.png b/src/Navigations/Cores/robot_nav_core2/doc/exceptions.png similarity index 100% rename from src/Navigations/Cores/nav_core2/doc/exceptions.png rename to src/Navigations/Cores/robot_nav_core2/doc/exceptions.png diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/basic_costmap.h similarity index 89% rename from src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h rename to src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/basic_costmap.h index 5018438..ec1208a 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/basic_costmap.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/basic_costmap.h @@ -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 +#include #include #include -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 data_; }; -} // namespace nav_core2 +} // namespace robot_nav_core2 -#endif // NAV_CORE2_BASIC_COSTMAP_H +#endif // ROBOT_NAV_CORE2_BASIC_COSTMAP_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/bounds.h similarity index 97% rename from src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h rename to src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/bounds.h index c3fcbaf..dafeffd 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/bounds.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/bounds.h @@ -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 #include #include -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 diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/common.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/common.h similarity index 94% rename from src/Navigations/Cores/nav_core2/include/nav_core2/common.h rename to src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/common.h index c726589..baeaadc 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/common.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/common.h @@ -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 #include using TFListenerPtr = std::shared_ptr; -#endif // NAV_CORE2_COMMON_H +#endif // ROBOT_NAV_CORE2_COMMON_H diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/costmap.h similarity index 96% rename from src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h rename to src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/costmap.h index 9b0485b..0a68fba 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/costmap.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/costmap.h @@ -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 #include -#include -#include +#include +#include #include #include #include -namespace nav_core2 +namespace robot_nav_core2 { class Costmap : public nav_grid::NavGrid @@ -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 diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/exceptions.h similarity index 97% rename from src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h rename to src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/exceptions.h index bf79037..c707d08 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/exceptions.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/exceptions.h @@ -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 #include @@ -41,7 +41,7 @@ #include /************************************************** - * 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 diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h similarity index 92% rename from src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h rename to src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h index 35a1754..b437c3e 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/global_planner.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/global_planner.h @@ -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 -#include +#include +#include #include #include #include #include -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 diff --git a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h similarity index 96% rename from src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h rename to src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h index 5a80370..9adea92 100755 --- a/src/Navigations/Cores/nav_core2/include/nav_core2/local_planner.h +++ b/src/Navigations/Cores/robot_nav_core2/include/robot_nav_core2/local_planner.h @@ -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 -#include +#include +#include #include #include #include @@ -45,7 +45,7 @@ #include #include -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 diff --git a/src/Navigations/Cores/nav_core_adapter/package.xml b/src/Navigations/Cores/robot_nav_core2/package.xml similarity index 51% rename from src/Navigations/Cores/nav_core_adapter/package.xml rename to src/Navigations/Cores/robot_nav_core2/package.xml index 3e94d23..1689471 100644 --- a/src/Navigations/Cores/nav_core_adapter/package.xml +++ b/src/Navigations/Cores/robot_nav_core2/package.xml @@ -1,9 +1,9 @@ - nav_core_adapter + robot_nav_core2 0.7.10 - 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 @@ Tully Foote BSD - http://www.ros.org/wiki/nav_core_adapter + http://www.ros.org/wiki/robot_nav_core2 catkin - libconsole-bridge-dev + robot_costmap_2d + robot_costmap_2d - libconsole-bridge-dev + tf3 + tf3 + + nav_grid + nav_grid + + robot_nav_2d_msgs + robot_nav_2d_msgs + + robot_cpp + robot_cpp \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core2/src/basic_costmap.cpp b/src/Navigations/Cores/robot_nav_core2/src/basic_costmap.cpp similarity index 95% rename from src/Navigations/Cores/nav_core2/src/basic_costmap.cpp rename to src/Navigations/Cores/robot_nav_core2/src/basic_costmap.cpp index 64a272a..a9b9675 100755 --- a/src/Navigations/Cores/nav_core2/src/basic_costmap.cpp +++ b/src/Navigations/Cores/robot_nav_core2/src/basic_costmap.cpp @@ -32,9 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include +#include -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 diff --git a/src/Navigations/Cores/nav_core2/test/bounds_test.cpp b/src/Navigations/Cores/robot_nav_core2/test/bounds_test.cpp similarity index 97% rename from src/Navigations/Cores/nav_core2/test/bounds_test.cpp rename to src/Navigations/Cores/robot_nav_core2/test/bounds_test.cpp index c9f19fa..0ee095c 100755 --- a/src/Navigations/Cores/nav_core2/test/bounds_test.cpp +++ b/src/Navigations/Cores/robot_nav_core2/test/bounds_test.cpp @@ -32,10 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include -using nav_core2::Bounds; -using nav_core2::UIntBounds; +using robot_nav_core2::Bounds; +using robot_nav_core2::UIntBounds; TEST(Bounds, test_bounds_simple) { diff --git a/src/Navigations/Cores/nav_core2/test/exception_test.cpp b/src/Navigations/Cores/robot_nav_core2/test/exception_test.cpp similarity index 76% rename from src/Navigations/Cores/nav_core2/test/exception_test.cpp rename to src/Navigations/Cores/robot_nav_core2/test/exception_test.cpp index 26c3ef2..995829e 100755 --- a/src/Navigations/Cores/nav_core2/test/exception_test.cpp +++ b/src/Navigations/Cores/robot_nav_core2/test/exception_test.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include #include 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) diff --git a/src/Navigations/Cores/robot_nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/robot_nav_core_adapter/CMakeLists.txt new file mode 100755 index 0000000..f130068 --- /dev/null +++ b/src/Navigations/Cores/robot_nav_core_adapter/CMakeLists.txt @@ -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 + $ + $) + +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 + $ + $) + +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 + $ + $) + +# 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 "=================================") \ No newline at end of file diff --git a/src/Navigations/Cores/robot_nav_core_adapter/README.md b/src/Navigations/Cores/robot_nav_core_adapter/README.md new file mode 100755 index 0000000..44f663b --- /dev/null +++ b/src/Navigations/Cores/robot_nav_core_adapter/README.md @@ -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. diff --git a/src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg b/src/Navigations/Cores/robot_nav_core_adapter/cfg/NavCoreAdapter.cfg similarity index 74% rename from src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg rename to src/Navigations/Cores/robot_nav_core_adapter/cfg/NavCoreAdapter.cfg index 55065e6..9ba9ea2 100755 --- a/src/Navigations/Cores/nav_core_adapter/cfg/NavCoreAdapter.cfg +++ b/src/Navigations/Cores/robot_nav_core_adapter/cfg/NavCoreAdapter.cfg @@ -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")) diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/costmap_adapter.h similarity index 88% rename from src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h rename to src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/costmap_adapter.h index 993f80d..53bfc64 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/costmap_adapter.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/costmap_adapter.h @@ -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 -#include +#include +#include #include #include -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 diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/global_planner_adapter.h similarity index 72% rename from src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h rename to src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/global_planner_adapter.h index 0670d69..3a975c1 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/global_planner_adapter.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/global_planner_adapter.h @@ -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 -#include -// #include +#include +#include #include #include #include -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 planner_loader_; - nav_core::BaseGlobalPlanner::Ptr planner_; + boost::function 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 diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h similarity index 87% rename from src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h rename to src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h index 93168c8..832a786 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/local_planner_adapter.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/local_planner_adapter.h @@ -32,20 +32,20 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H -#define NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H +#ifndef ROBOT_NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H +#define ROBOT_NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H #include -#include -#include -#include +#include +#include +#include #include #include #include #include #include -namespace nav_core_adapter +namespace robot_nav_core_adapter { struct NavCoreAdapterConfig { @@ -58,9 +58,9 @@ namespace nav_core_adapter /** * @class LocalPlannerAdapter - * @brief used for employing a nav_core2 local planner (such as dwb) as a nav_core plugin, like in move_base. + * @brief used for employing a robot_nav_core2 local planner (such as dwb) as a robot_nav_core2 plugin, like in move_base. */ - class LocalPlannerAdapter : public nav_core::BaseLocalPlanner + class LocalPlannerAdapter : public robot_nav_core::BaseLocalPlanner { public: @@ -87,7 +87,7 @@ namespace nav_core_adapter bool computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) override; /** - * @brief Swap object will be injected interjace nav_core::BaseLocalPlanner + * @brief Swap object will be injected interjace robot_nav_core2::BaseLocalPlanner * @param planner_name The object name * @return True if instance object is successed, False otherwise */ @@ -162,7 +162,7 @@ namespace nav_core_adapter * @brief Create a new LocalPlannerAdapter * @return A shared pointer to the new LocalPlannerAdapter */ - static nav_core::BaseLocalPlanner::Ptr create(); + static robot_nav_core::BaseLocalPlanner::Ptr create(); protected: /** @@ -186,8 +186,8 @@ namespace nav_core_adapter std::shared_ptr odom_sub_; // Plugin handling - boost::function planner_loader_; - nav_core2::LocalPlanner::Ptr planner_; + boost::function planner_loader_; + robot_nav_core2::LocalPlanner::Ptr planner_; // Pointer Copies TFListenerPtr tf_; @@ -200,11 +200,11 @@ namespace nav_core_adapter robot::NodeHandle private_nh_; robot::NodeHandle adapter_nh_; - nav_core_adapter::NavCoreAdapterConfig last_config_; - nav_core_adapter::NavCoreAdapterConfig default_config_; + robot_nav_core_adapter::NavCoreAdapterConfig last_config_; + robot_nav_core_adapter::NavCoreAdapterConfig default_config_; bool setup_; }; -} // namespace nav_core_adapter +} // namespace robot_nav_core_adapter -#endif // NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H +#endif // ROBOT_NAV_CORE_ADAPTER_LOCAL_PLANNER_ADAPTER_H diff --git a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/shared_pointers.h similarity index 89% rename from src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h rename to src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/shared_pointers.h index d7b4d52..cc186e4 100755 --- a/src/Navigations/Cores/nav_core_adapter/include/nav_core_adapter/shared_pointers.h +++ b/src/Navigations/Cores/robot_nav_core_adapter/include/robot_nav_core_adapter/shared_pointers.h @@ -32,13 +32,13 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef NAV_CORE_ADAPTER_SHARED_POINTERS_H -#define NAV_CORE_ADAPTER_SHARED_POINTERS_H +#ifndef ROBOT_NAV_CORE_ADAPTER_SHARED_POINTERS_H +#define ROBOT_NAV_CORE_ADAPTER_SHARED_POINTERS_H -#include +#include #include -namespace nav_core_adapter +namespace robot_nav_core_adapter { template @@ -47,7 +47,7 @@ void null_deleter(T* raw_ptr) {} /** * @brief Custom Constructor for creating a shared pointer to an existing object that doesn't delete the ptr when done * - * @note This is considered bad form, and is only done here for backwards compatibility purposes. The nav_core2 + * @note This is considered bad form, and is only done here for backwards compatibility purposes. The robot_nav_core2 * interfaces require shared pointers, but the creation of the shared pointer from a raw pointer takes ownership * of the object such that when the object containing the shared pointer is freed, the object pointed at by the * shared pointer is also freed. This presents a problem, for instance, when switching from one planner to another @@ -62,6 +62,6 @@ std::shared_ptr createSharedPointerWithNoDelete(T* raw_ptr) return std::shared_ptr(raw_ptr, null_deleter); } -} // namespace nav_core_adapter +} // namespace robot_nav_core_adapter -#endif // NAV_CORE_ADAPTER_SHARED_POINTERS_H +#endif // ROBOT_NAV_CORE_ADAPTER_SHARED_POINTERS_H diff --git a/src/Navigations/Cores/robot_nav_core_adapter/package.xml b/src/Navigations/Cores/robot_nav_core_adapter/package.xml new file mode 100644 index 0000000..5ba8da8 --- /dev/null +++ b/src/Navigations/Cores/robot_nav_core_adapter/package.xml @@ -0,0 +1,40 @@ + + robot_nav_core_adapter + 0.7.10 + + robot_nav_core_adapter is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. robot_nav_core_adapter + maintains the relationship between coordinate frames in a tree + structure buffered in time, and lets the user transform points, + vectors, etc between any two coordinate frames at any desired + point in time. + + Tully Foote + Eitan Marder-Eppstein + Wim Meeussen + Tully Foote + BSD + + http://www.ros.org/wiki/robot_nav_core_adapter + + catkin + + geometry_msgs + geometry_msgs + + robot_std_msgs + robot_std_msgs + + robot_nav_core + robot_nav_core + + robot_nav_core2 + robot_nav_core2 + + robot_nav_2d_utils + robot_nav_2d_utils + + robot_cpp + robot_cpp + + \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/costmap_adapter.cpp similarity index 85% rename from src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp rename to src/Navigations/Cores/robot_nav_core_adapter/src/costmap_adapter.cpp index 338e21f..ab9b57e 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/costmap_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/costmap_adapter.cpp @@ -32,15 +32,15 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include +#include #include // #include #include -// PLUGINLIB_EXPORT_CLASS(nav_core_adapter::CostmapAdapter, nav_core2::Costmap) +// PLUGINLIB_EXPORT_CLASS(robot_nav_core_adapter::CostmapAdapter, robot_nav_core2::Costmap) -namespace nav_core_adapter +namespace robot_nav_core_adapter { nav_grid::NavGridInfo infoFromCostmap(robot_costmap_2d::Costmap2DROBOT* costmap_robot) @@ -75,10 +75,10 @@ void CostmapAdapter::initialize(robot_costmap_2d::Costmap2DROBOT* costmap_robot, void CostmapAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, TFListenerPtr tf) { // TODO: Implement this if needed - throw nav_core2::CostmapException("initialize with robot::NodeHandle not implemented"); + throw robot_nav_core2::CostmapException("initialize with robot::NodeHandle not implemented"); } -nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex() +robot_nav_core2::Costmap::mutex_t* CostmapAdapter::getMutex() { return costmap_->getMutex(); } @@ -92,7 +92,7 @@ void CostmapAdapter::update() { info_ = infoFromCostmap(costmap_robot_); if (!costmap_robot_->isCurrent()) - throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); + throw robot_nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); } void CostmapAdapter::setValue(const unsigned int x, const unsigned int y, const unsigned char& value) @@ -108,7 +108,7 @@ unsigned char CostmapAdapter::getValue(const unsigned int x, const unsigned int void CostmapAdapter::setInfo(const nav_grid::NavGridInfo& new_info) { - throw nav_core2::CostmapException("setInfo not implemented on CostmapAdapter"); + throw robot_nav_core2::CostmapException("setInfo not implemented on CostmapAdapter"); } void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info) @@ -116,4 +116,4 @@ void CostmapAdapter::updateInfo(const nav_grid::NavGridInfo& new_info) costmap_->updateOrigin(new_info.origin_x, new_info.origin_y); } -} // namespace nav_core_adapter +} // namespace robot_nav_core_adapter diff --git a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/global_planner_adapter.cpp similarity index 81% rename from src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp rename to src/Navigations/Cores/robot_nav_core_adapter/src/global_planner_adapter.cpp index d1ee5ce..1219eab 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/global_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/global_planner_adapter.cpp @@ -32,29 +32,29 @@ * POSSIBILITY OF SUCH DAMAGE. */ -#include -#include +#include +#include #include #include -#include +#include #include #include #include #include -namespace nav_core_adapter +namespace robot_nav_core_adapter { GlobalPlannerAdapter::GlobalPlannerAdapter() - // : planner_loader_("nav_core", "nav_core::BaseGlobalPlanner") + // : planner_loader_("robot_nav_core2", "robot_nav_core2::BaseGlobalPlanner") { } /** - * @brief Load the nav_core global planner and initialize it + * @brief Load the robot_nav_core2 global planner and initialize it */ void GlobalPlannerAdapter::initialize(const robot::NodeHandle& parent, const std::string& name, - TFListenerPtr tf, nav_core2::Costmap::Ptr costmap) + TFListenerPtr tf, robot_nav_core2::Costmap::Ptr costmap) { costmap_ = costmap; std::shared_ptr ptr = std::static_pointer_cast(costmap); @@ -72,7 +72,7 @@ namespace nav_core_adapter // planner_nh.param("planner_name", planner_name, std::string("global_planner/GlobalPlanner")); // ROS_INFO_NAMED("GlobalPlannerAdapter", "Loading plugin %s", planner_name.c_str()); std::string path_file_so; - planner_loader_ = boost::dll::import_alias( + planner_loader_ = boost::dll::import_alias( path_file_so, planner_name, boost::dll::load_mode::append_decorations); planner_ = planner_loader_(); planner_->initialize(planner_name, costmap_robot_); @@ -87,16 +87,16 @@ namespace nav_core_adapter bool ret = planner_->makePlan(start3d, goal3d, plan); if (!ret) { - throw nav_core2::PlannerException("Generic Global Planner Error"); + throw robot_nav_core2::PlannerException("Generic Global Planner Error"); } return robot_nav_2d_utils::posesToPath2D(plan); } - nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create() + robot_nav_core2::GlobalPlanner::Ptr GlobalPlannerAdapter::create() { return std::make_shared(); } -} // namespace nav_core_adapter +} // namespace robot_nav_core_adapter // register this planner as a GlobalPlanner plugin -BOOST_DLL_ALIAS(nav_core_adapter::GlobalPlannerAdapter::create, create_plugin) +BOOST_DLL_ALIAS(robot_nav_core_adapter::GlobalPlannerAdapter::create, create_plugin) diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp similarity index 91% rename from src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp rename to src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp index fdef733..adab4d2 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/src/local_planner_adapter.cpp @@ -33,20 +33,20 @@ */ #include -#include -#include -#include +#include +#include +#include #include #include #include -#include +#include #include #include #include #include #include -namespace nav_core_adapter +namespace robot_nav_core_adapter { LocalPlannerAdapter::LocalPlannerAdapter() : has_active_goal_(false) @@ -71,7 +71,7 @@ namespace nav_core_adapter } /** - * @brief Load the nav_core2 local planner and initialize it + * @brief Load the robot_nav_core2 local planner and initialize it */ void LocalPlannerAdapter::initialize(std::string name, tf3::BufferCore *tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot) { @@ -97,7 +97,7 @@ namespace nav_core_adapter { robot::PluginLoaderHelper loader; std::string path_file_so = loader.findLibraryPath(planner_name); - planner_loader_ = boost::dll::import_alias( + planner_loader_ = boost::dll::import_alias( path_file_so, planner_name, boost::dll::load_mode::append_decorations); planner_ = planner_loader_(); if (!planner_) @@ -131,13 +131,13 @@ namespace nav_core_adapter if (planner_name != last_config_.planner_name) { - nav_core2::LocalPlanner::Ptr new_planner; + robot_nav_core2::LocalPlanner::Ptr new_planner; try { // Tạo planner mới std::string path_file_so; - planner_loader_ = boost::dll::import_alias( + planner_loader_ = boost::dll::import_alias( path_file_so, planner_name, boost::dll::load_mode::append_decorations); new_planner = planner_loader_(); if (!new_planner) @@ -163,7 +163,7 @@ namespace nav_core_adapter } /** - * @brief Collect the additional information needed by nav_core2 and call the child computeVelocityCommands + * @brief Collect the additional information needed by robot_nav_core2 and call the child computeVelocityCommands */ bool LocalPlannerAdapter::computeVelocityCommands(robot_geometry_msgs::Twist &cmd_vel) { @@ -187,7 +187,7 @@ namespace nav_core_adapter { cmd_vel_2d = planner_->computeVelocityCommands(pose2d, velocity); } - catch (const nav_core2::PlannerException &e) + catch (const robot_nav_core2::PlannerException &e) { std::cerr << "computeVelocityCommands exception: " << e.what() << std::endl; return false; @@ -320,7 +320,7 @@ namespace nav_core_adapter } /** - * @brief Collect the additional information needed by nav_core2 and call the child isGoalReached + * @brief Collect the additional information needed by robot_nav_core2 and call the child isGoalReached */ bool LocalPlannerAdapter::isGoalReached() { @@ -363,7 +363,7 @@ namespace nav_core_adapter } return true; } - catch (const nav_core2::PlannerException &e) + catch (const robot_nav_core2::PlannerException &e) { std::cerr << "setPlan Exception: " << e.what() << std::endl; return false; @@ -394,11 +394,11 @@ namespace nav_core_adapter return true; } - nav_core::BaseLocalPlanner::Ptr LocalPlannerAdapter::create() + robot_nav_core::BaseLocalPlanner::Ptr LocalPlannerAdapter::create() { return std::make_shared(); } -} // namespace nav_core_adapter +} // namespace robot_nav_core_adapter // register this planner as a BaseLocalPlanner plugin -BOOST_DLL_ALIAS(nav_core_adapter::LocalPlannerAdapter::create, LocalPlannerAdapter) \ No newline at end of file +BOOST_DLL_ALIAS(robot_nav_core_adapter::LocalPlannerAdapter::create, LocalPlannerAdapter) \ No newline at end of file diff --git a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp b/src/Navigations/Cores/robot_nav_core_adapter/test/unload_test.cpp similarity index 94% rename from src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp rename to src/Navigations/Cores/robot_nav_core_adapter/test/unload_test.cpp index e63972e..e87fac5 100755 --- a/src/Navigations/Cores/nav_core_adapter/test/unload_test.cpp +++ b/src/Navigations/Cores/robot_nav_core_adapter/test/unload_test.cpp @@ -32,7 +32,7 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include -#include +#include TEST(LocalPlannerAdapter, unload_local_planner) @@ -49,7 +49,7 @@ TEST(LocalPlannerAdapter, unload_local_planner) base_rel_map.transform.rotation.w = 1.0; tf.setTransform(base_rel_map, "unload", true); - nav_core_adapter::LocalPlannerAdapter* lpa = new nav_core_adapter::LocalPlannerAdapter(); + robot_nav_core_adapter::LocalPlannerAdapter* lpa = new robot_nav_core_adapter::LocalPlannerAdapter(); robot_costmap_2d::Costmap2DROBOT costmap_robot("local_costmap", tf); lpa->initialize("lpa", &tf, &costmap_robot); diff --git a/src/Navigations/Cores/robot_nav_core_adapter/test/unload_test.launch b/src/Navigations/Cores/robot_nav_core_adapter/test/unload_test.launch new file mode 100755 index 0000000..1c64734 --- /dev/null +++ b/src/Navigations/Cores/robot_nav_core_adapter/test/unload_test.launch @@ -0,0 +1,3 @@ + + + diff --git a/src/Navigations/Libraries/nav_grid/CMakeLists.txt b/src/Navigations/Libraries/nav_grid/CMakeLists.txt index 8b1058c..1f0384b 100755 --- a/src/Navigations/Libraries/nav_grid/CMakeLists.txt +++ b/src/Navigations/Libraries/nav_grid/CMakeLists.txt @@ -1,5 +1,18 @@ cmake_minimum_required(VERSION 3.10) +# ======================================================== +# Dual-mode CMakeLists.txt: Supports both Catkin and Standalone CMake +# ======================================================== + +# Detect if building with Catkin +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building nav_grid with Catkin") +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building nav_grid with Standalone CMake") +endif() + project(nav_grid VERSION 1.0.0 LANGUAGES CXX) # Chuẩn C++ @@ -7,6 +20,25 @@ set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED ON) set(CMAKE_CXX_EXTENSIONS OFF) +# ======================================================== +# Find dependencies +# ======================================================== + +if(BUILDING_WITH_CATKIN) + find_package(catkin REQUIRED) +endif() + +# ======================================================== +# Catkin specific configuration +# ======================================================== + +if(BUILDING_WITH_CATKIN) + catkin_package( + INCLUDE_DIRS include + LIBRARIES nav_grid + ) +endif() + # Tìm tất cả header files file(GLOB_RECURSE HEADERS "include/nav_grid/*.h") @@ -20,10 +52,16 @@ target_include_directories(nav_grid $ ) +if(BUILDING_WITH_CATKIN) + add_dependencies(nav_grid ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +endif() + # Cài đặt header files -install(DIRECTORY include/nav_grid - DESTINATION include - FILES_MATCHING PATTERN "*.h") +if(NOT BUILDING_WITH_CATKIN) + install(DIRECTORY include/nav_grid + DESTINATION include + FILES_MATCHING PATTERN "*.h") +endif() # Cài đặt target install(TARGETS nav_grid diff --git a/src/Navigations/Libraries/nav_grid/package.xml b/src/Navigations/Libraries/nav_grid/package.xml index 21b3c76..59216ed 100644 --- a/src/Navigations/Libraries/nav_grid/package.xml +++ b/src/Navigations/Libraries/nav_grid/package.xml @@ -19,8 +19,4 @@ catkin - libconsole-bridge-dev - - libconsole-bridge-dev - \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md b/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md index 5bd1167..42a0fdc 100644 --- a/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md +++ b/src/Navigations/Packages/move_base/BUILD_INSTRUCTIONS.md @@ -6,7 +6,7 @@ Package `move_base` có thể được build bằng cả **Catkin** và **Standa ### 1. Build với Catkin (ROS Workspace) -**⚠️ QUAN TRỌNG**: Thư mục gốc của project (`pnkx_nav_core`) có `CMakeLists.txt` ở root, +**⚠️ QUAN TRỌNG**: Thư mục gốc của project (`pnkx_robot_nav_core`) có `CMakeLists.txt` ở root, nên **KHÔNG THỂ** chạy `catkin_make` trực tiếp trong thư mục đó. Catkin workspace không được phép có `CMakeLists.txt` ở root. @@ -16,7 +16,7 @@ phép có `CMakeLists.txt` ở root. ```bash # Chạy script setup từ thư mục gốc của project -cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core +cd /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core ./setup_catkin_workspace.sh # Script sẽ tạo workspace và link các packages có package.xml @@ -34,11 +34,11 @@ mkdir -p ~/pnkx_nav_catkin_ws/src cd ~/pnkx_nav_catkin_ws/src # Link package move_base vào workspace -ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Navigations/Packages/move_base move_base +ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core/src/Navigations/Packages/move_base move_base # Link các dependencies cần thiết (nếu có package.xml) -ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/tf3 tf3 -ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/src/Libraries/nav_2d_utils nav_2d_utils +ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core/src/Libraries/tf3 tf3 +ln -s /home/robotics/AGV/Diff_Wheel_Prj/pnkx_robot_nav_core/src/Libraries/nav_2d_utils nav_2d_utils # Build với catkin cd ~/pnkx_nav_catkin_ws @@ -63,7 +63,7 @@ make **Lưu ý**: Khi build standalone, bạn cần đảm bảo: - Tất cả dependencies đã được build trước -- CMake có thể tìm thấy các internal packages (move_base_core, nav_core, etc.) +- CMake có thể tìm thấy các internal packages (move_base_core, robot_nav_core, etc.) - ROS packages (nếu cần) được cài đặt và có trong CMAKE_PREFIX_PATH ## Cách CMakeLists.txt Detect Build Mode @@ -85,7 +85,7 @@ Ngược lại, sẽ build ở **Standalone CMake mode**. ### Internal Packages (cần build trước): - move_base_core -- nav_core +- robot_nav_core - robot_costmap_2d - xmlrpcpp - node_handle @@ -136,7 +136,7 @@ Building move_base with Standalone CMake - Source ROS setup: `source /opt/ros/noetic/setup.bash` - Kiểm tra `ROS_PACKAGE_PATH` environment variable -### Lỗi: Internal packages không tìm thấy (move_base_core, nav_core, etc.) +### Lỗi: Internal packages không tìm thấy (move_base_core, robot_nav_core, etc.) - **Với Catkin**: Các internal packages cần có `package.xml` và được link vào workspace - **Với Standalone**: Đảm bảo các packages đã được build trước trong project chính diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index 169fca1..1b52695 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -27,7 +27,22 @@ endif() # ======================================================== # Find Packages # ======================================================== -find_package(Boost REQUIRED) +find_package(Boost REQUIRED COMPONENTS filesystem system) + +if(BUILDING_WITH_CATKIN) + find_package(catkin REQUIRED COMPONENTS + geometry_msgs + robot_std_msgs + move_base_core + robot_nav_core + robot_costmap_2d + robot_tf3_sensor_msgs + robot_tf3_geometry_msgs + data_convert + robot_nav_2d_utils + robot_cpp + ) +endif() # ======================================================== @@ -36,6 +51,18 @@ find_package(Boost REQUIRED) file(GLOB SOURCES "src/move_base.cpp") file(GLOB HEADERS "include/move_base/move_base.h") +# ======================================================== +# Catkin specific configuration +# ======================================================== + +if(BUILDING_WITH_CATKIN) + catkin_package( + INCLUDE_DIRS include + LIBRARIES move_base + CATKIN_DEPENDS geometry_msgs robot_std_msgs move_base_core robot_nav_core robot_costmap_2d robot_tf3_sensor_msgs robot_tf3_geometry_msgs data_convert robot_nav_2d_utils robot_cpp + ) +endif() + # ======================================================== # Include Directories # ======================================================== @@ -43,6 +70,10 @@ include_directories( ${PROJECT_SOURCE_DIR}/include ) +if(BUILDING_WITH_CATKIN) + include_directories(${catkin_INCLUDE_DIRS}) +endif() + # ======================================================== # RPATH settings: ưu tiên thư viện build tại chỗ @@ -62,34 +93,45 @@ add_library(move_base SHARED ${SOURCES} ${HEADERS}) # Dependencies and Link Libraries # ======================================================== -# Standalone CMake mode: link all dependencies manually -set(PACKAGES_DIR - geometry_msgs - robot_std_msgs - move_base_core - nav_core - robot_costmap_2d - plugins # Link với plugins library để có StaticLayer typeinfo - yaml-cpp - robot_tf3_sensor_msgs - robot_tf3_geometry_msgs - data_convert - dl - pthread - robot_nav_2d_utils -) +if(BUILDING_WITH_CATKIN) + target_link_libraries(move_base + PUBLIC ${catkin_LIBRARIES} + PRIVATE Boost::filesystem Boost::system + ) +else() + # Standalone CMake mode: link all dependencies manually + set(PACKAGES_DIR + geometry_msgs + robot_std_msgs + move_base_core + robot_nav_core + robot_costmap_2d + plugins # Link với plugins library để có StaticLayer typeinfo + yaml-cpp + robot_tf3_sensor_msgs + robot_tf3_geometry_msgs + data_convert + dl + pthread + robot_nav_2d_utils + ) -target_link_libraries(move_base - PUBLIC ${PACKAGES_DIR} - PUBLIC robot_cpp - PRIVATE Boost::boost -) + target_link_libraries(move_base + PUBLIC ${PACKAGES_DIR} + PUBLIC robot_cpp + PRIVATE Boost::filesystem Boost::system + ) +endif() # Set RPATH để ưu tiên thư viện build cục bộ set_target_properties(move_base PROPERTIES LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} ) +if(BUILDING_WITH_CATKIN) + add_dependencies(move_base ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) +endif() + # ======================================================== # Include Directories for Target @@ -100,11 +142,6 @@ target_include_directories(move_base $ ) -if(BUILDING_WITH_CATKIN) - target_include_directories(move_base - PUBLIC ${catkin_INCLUDE_DIRS} - ) -endif() # ======================================================== # Executable @@ -112,7 +149,7 @@ endif() add_executable(move_base_main src/move_base_main.cpp) target_link_libraries(move_base_main PRIVATE move_base robot_cpp - PRIVATE Boost::boost + PRIVATE Boost::filesystem Boost::system ) # Set RPATH for executable to find libraries in build directory first @@ -125,11 +162,13 @@ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--disable-new-dtags") # Installation # ======================================================== -# Standalone CMake installation -install(DIRECTORY include/move_base - DESTINATION include - FILES_MATCHING PATTERN "*.h" -) +if(NOT BUILDING_WITH_CATKIN) + # Standalone CMake installation + install(DIRECTORY include/move_base + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) +endif() install(TARGETS move_base move_base_main EXPORT move_base-targets @@ -138,10 +177,12 @@ install(TARGETS move_base move_base_main RUNTIME DESTINATION bin ) -install(EXPORT move_base-targets - FILE move_base-targets.cmake - DESTINATION lib/cmake/move_base -) +if(NOT BUILDING_WITH_CATKIN) + install(EXPORT move_base-targets + FILE move_base-targets.cmake + DESTINATION lib/cmake/move_base + ) +endif() # ======================================================== diff --git a/src/Navigations/Packages/move_base/include/move_base/move_base.h b/src/Navigations/Packages/move_base/include/move_base/move_base.h index 8dc31d2..810b532 100644 --- a/src/Navigations/Packages/move_base/include/move_base/move_base.h +++ b/src/Navigations/Packages/move_base/include/move_base/move_base.h @@ -15,9 +15,9 @@ // interfaces headers #include -#include -#include -#include +#include +#include +#include // boost headers #include @@ -206,10 +206,9 @@ namespace move_base /** * @brief Get the navigation feedback. - * @param feedback Output parameter with the navigation feedback. - * @return True if feedback was successfully retrieved. + * @return Pointer to the navigation feedback. */ - virtual bool getFeedback(robot::move_base_core::NavFeedback &feedback) override; + virtual robot::move_base_core::NavFeedback *getFeedback() override; /** * @brief Destructor - Cleans up @@ -309,13 +308,13 @@ namespace move_base robot::TFListenerPtr tf_; robot::NodeHandle private_nh_; - boost::function planner_loader_; - boost::function controller_loader_; - boost::function recovery_loader_; + boost::function planner_loader_; + boost::function controller_loader_; + boost::function recovery_loader_; - nav_core::BaseLocalPlanner::Ptr tc_; - nav_core::BaseGlobalPlanner::Ptr planner_; - std::vector recovery_behaviors_; + robot_nav_core::BaseLocalPlanner::Ptr tc_; + robot_nav_core::BaseGlobalPlanner::Ptr planner_; + std::vector recovery_behaviors_; robot_costmap_2d::Costmap2DROBOT *controller_costmap_robot_; robot_costmap_2d::Costmap2DROBOT *planner_costmap_robot_; diff --git a/src/Navigations/Packages/move_base/package.xml b/src/Navigations/Packages/move_base/package.xml index a6cb84e..5b73942 100644 --- a/src/Navigations/Packages/move_base/package.xml +++ b/src/Navigations/Packages/move_base/package.xml @@ -19,8 +19,34 @@ catkin - libconsole-bridge-dev + geometry_msgs + geometry_msgs - libconsole-bridge-dev + robot_std_msgs + robot_std_msgs + move_base_core + move_base_core + + robot_nav_core + robot_nav_core + + robot_costmap_2d + robot_costmap_2d + + robot_tf3_sensor_msgs + robot_tf3_sensor_msgs + + robot_tf3_geometry_msgs + robot_tf3_geometry_msgs + + data_convert + data_convert + + robot_nav_2d_utils + robot_nav_2d_utils + + robot_cpp + robot_cpp + \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 6ea2044..3836b57 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -219,7 +219,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) { robot::PluginLoaderHelper loader; std::string path_file_so = loader.findLibraryPath("CustomPlanner"); - planner_loader_ = boost::dll::import_alias( + planner_loader_ = boost::dll::import_alias( path_file_so, global_planner, boost::dll::load_mode::append_decorations); planner_ = planner_loader_(); @@ -281,7 +281,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) robot::PluginLoaderHelper loader; std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter"); controller_loader_ = - boost::dll::import_alias( + boost::dll::import_alias( path_file_so, local_planner, boost::dll::load_mode::append_decorations); tc_ = controller_loader_(); if (!tc_) @@ -883,11 +883,11 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) // std::string behavior_name = behavior["name"].as(); // std::string path_file_so; // // Load the factory function from the shared library - // auto recovery_loader = boost::dll::import_alias( + // auto recovery_loader = boost::dll::import_alias( // path_file_so, behavior_type, boost::dll::load_mode::append_decorations); // // Create instance using the factory function - // std::shared_ptr behavior_ptr(recovery_loader()); + // std::shared_ptr behavior_ptr(recovery_loader()); // // shouldn't be possible, but it won't hurt to check // if (behavior_ptr == nullptr) @@ -937,9 +937,9 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors() try { std::string path_file_so ; - auto clear_costmap_loader = boost::dll::import_alias( + auto clear_costmap_loader = boost::dll::import_alias( path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); - std::shared_ptr cons_clear(clear_costmap_loader()); + std::shared_ptr cons_clear(clear_costmap_loader()); cons_clear->initialize("conservative_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_); recovery_behavior_names_.push_back("conservative_reset"); recovery_behaviors_.push_back(cons_clear); @@ -950,15 +950,15 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors() } // next, we'll load a recovery behavior to rotate in place - std::shared_ptr rotate; + std::shared_ptr rotate; if (clearing_rotation_allowed_) { try { std::string path_file_so; - auto rotate_loader = boost::dll::import_alias( + auto rotate_loader = boost::dll::import_alias( path_file_so, "rotate_recovery", boost::dll::load_mode::append_decorations); - rotate = std::shared_ptr(rotate_loader()); + rotate = std::shared_ptr(rotate_loader()); rotate->initialize("rotate_recovery", tf_.get(), planner_costmap_robot_, controller_costmap_robot_); recovery_behavior_names_.push_back("rotate_recovery"); recovery_behaviors_.push_back(rotate); @@ -973,9 +973,9 @@ void move_base::MoveBase::loadDefaultRecoveryBehaviors() try { std::string path_file_so; - auto clear_costmap_loader = boost::dll::import_alias( + auto clear_costmap_loader = boost::dll::import_alias( path_file_so, "clear_costmap_recovery", boost::dll::load_mode::append_decorations); - std::shared_ptr ags_clear(clear_costmap_loader()); + std::shared_ptr ags_clear(clear_costmap_loader()); ags_clear->initialize("aggressive_reset", tf_.get(), planner_costmap_robot_, controller_costmap_robot_); recovery_behavior_names_.push_back("aggressive_reset"); recovery_behaviors_.push_back(ags_clear); @@ -1307,13 +1307,12 @@ bool move_base::MoveBase::getTwist(robot_nav_2d_msgs::Twist2DStamped &twist) return true; } -bool move_base::MoveBase::getFeedback(robot::move_base_core::NavFeedback &feedback) +robot::move_base_core::NavFeedback *move_base::MoveBase::getFeedback() { if (!nav_feedback_) { - return false; + return nullptr; } - feedback = *nav_feedback_; - return true; + return nav_feedback_.get(); } // void wakePlanner(const robot::TimerEvent &event)