diff --git a/config/costmap_global_params.yaml b/config/costmap_global_params.yaml index c6dea24..5c88dc6 100755 --- a/config/costmap_global_params.yaml +++ b/config/costmap_global_params.yaml @@ -1,5 +1,6 @@ global_costmap: - path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so + library_path: libplugins + robot_base_frame: base_footprint global_frame: map update_frequency: 1.0 publish_frequency: 1.0 diff --git a/config/costmap_local_params.yaml b/config/costmap_local_params.yaml index 48600c5..2e74a01 100755 --- a/config/costmap_local_params.yaml +++ b/config/costmap_local_params.yaml @@ -1,6 +1,7 @@ local_costmap: - path_plugins: /home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Libraries/costmap_2d/libplugins.so + library_path: libplugins global_frame: odom + robot_base_frame: base_footprint update_frequency: 6.0 publish_frequency: 6.0 rolling_window: true diff --git a/config/mprim/custom_global_params.yaml b/config/custom_global_params.yaml similarity index 65% rename from config/mprim/custom_global_params.yaml rename to config/custom_global_params.yaml index 6536a6e..f4eaf44 100644 --- a/config/mprim/custom_global_params.yaml +++ b/config/custom_global_params.yaml @@ -1,5 +1,6 @@ base_global_planner: CustomPlanner CustomPlanner: + library_path: libcustom_planner environment_type: XYThetaLattice planner_type: ARAPlanner allocated_time: 10.0 @@ -9,9 +10,6 @@ CustomPlanner: nominalvel_mpersecs: 0.8 timetoturn45degsinplace_secs: 1.31 # = 0.6 rad/s allow_unknown: true - directory_to_save_paths: "/init/paths" - pathway_filename: "pathway.txt" - current_pose_topic_name: "/amcl_pose" - map_frame_id: "map" - base_frame_id: "base_link" + + diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index 871aa70..449cad0 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -21,4 +21,5 @@ conservative_reset: aggressive_reset: reset_distance: 3.0 - +MoveBase: + library_path: libmove_base diff --git a/config/two_points_global_params.yaml b/config/two_points_global_params.yaml index ca0b60f..99f7480 100644 --- a/config/two_points_global_params.yaml +++ b/config/two_points_global_params.yaml @@ -1,3 +1,4 @@ base_global_planner: TwoPointsPlanner TwoPointsPlanner: + library_path: libtwo_points_planner lethal_obstacle: 20 \ No newline at end of file diff --git a/examples/CSharpExample.cs b/examples/CSharpExample.cs index 8da2079..3d52a77 100644 --- a/examples/CSharpExample.cs +++ b/examples/CSharpExample.cs @@ -287,7 +287,16 @@ namespace NavigationExample return; } - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_link", + if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_footprint", + 0, 0, 0, + 0, 0, 0, 1)) + { + LogError("Failed to inject static TF map -> base_link"); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1)) { diff --git a/examples/NavigationExample/Program.cs b/examples/NavigationExample/Program.cs index 8da2079..3d52a77 100644 --- a/examples/NavigationExample/Program.cs +++ b/examples/NavigationExample/Program.cs @@ -287,7 +287,16 @@ namespace NavigationExample return; } - if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_link", + if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "odom", "base_footprint", + 0, 0, 0, + 0, 0, 0, 1)) + { + LogError("Failed to inject static TF map -> base_link"); + NavigationAPI.tf_listener_destroy(tfHandle); + return; + } + + if (!NavigationAPI.tf_listener_set_static_transform(tfHandle, "base_footprint", "base_link", 0, 0, 0, 0, 0, 0, 1)) { diff --git a/examples/NavigationExample/libnav_c_api.so b/examples/NavigationExample/libnav_c_api.so index bcb1b2e..d7c12b8 100755 Binary files a/examples/NavigationExample/libnav_c_api.so and b/examples/NavigationExample/libnav_c_api.so differ diff --git a/examples/run_example.sh b/examples/run_example.sh index e8466c3..9a04a48 100755 --- a/examples/run_example.sh +++ b/examples/run_example.sh @@ -7,7 +7,7 @@ set -e SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" PROJECT_ROOT="$(cd "$SCRIPT_DIR/.." && pwd)" BUILD_DIR="$PROJECT_ROOT/build" -LIB_DIR="$BUILD_DIR/src/APIs/c_api" +LIB_DIR="$BUILD_DIR" EXAMPLE_DIR="$SCRIPT_DIR" echo "==========================================" diff --git a/src/APIs/c_api/CMakeLists.txt b/src/APIs/c_api/CMakeLists.txt index 5ad0c41..1fd72c7 100644 --- a/src/APIs/c_api/CMakeLists.txt +++ b/src/APIs/c_api/CMakeLists.txt @@ -37,6 +37,10 @@ target_link_libraries(nav_c_api ${PACKAGES_DIR} ) +set_target_properties(nav_c_api PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Set include directories target_include_directories(nav_c_api PUBLIC diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 49734f5..3b506a7 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -12,6 +12,7 @@ #include #include #include +#include // ============================================================================ @@ -227,9 +228,10 @@ extern "C" bool navigation_initialize(NavigationHandle handle, TFListenerHandle printf("[%s:%d]\n Initialize navigation\n", __FILE__, __LINE__); move_base_core::BaseNavigation* nav = static_cast(handle); auto* tf_ptr = static_cast*>(tf_handle); - + robot::PluginLoaderHelper loader; + std::string lib_path = loader.findLibraryPath("MoveBase"); auto create_plugin = boost::dll::import_alias( - "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Packages/move_base/libmove_base.so", + lib_path, "MoveBase", boost::dll::load_mode::append_decorations); diff --git a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt index 54e5b4b..a86045e 100644 --- a/src/Algorithms/Cores/score_algorithm/CMakeLists.txt +++ b/src/Algorithms/Cores/score_algorithm/CMakeLists.txt @@ -29,7 +29,12 @@ target_link_libraries(score_algorithm PRIVATE Boost::filesystem Boost::system + + ) +set_target_properties(score_algorithm PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} ) + target_include_directories(score_algorithm PUBLIC $ $) diff --git a/src/Algorithms/Libraries/kalman/CMakeLists.txt b/src/Algorithms/Libraries/kalman/CMakeLists.txt index b6bb455..27e8e41 100755 --- a/src/Algorithms/Libraries/kalman/CMakeLists.txt +++ b/src/Algorithms/Libraries/kalman/CMakeLists.txt @@ -32,6 +32,10 @@ target_link_libraries(kalman Eigen3::Eigen ) +set_target_properties(kalman PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Set include directories target_include_directories(kalman PUBLIC @@ -48,6 +52,10 @@ target_link_libraries(kalman_node Eigen3::Eigen ) +set_target_properties(kalman_node PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Cài đặt header files install(DIRECTORY include/kalman DESTINATION include diff --git a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt index 40c2f82..521e14c 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_algorithm/CMakeLists.txt @@ -57,6 +57,10 @@ target_link_libraries(mkt_algorithm_diff Eigen3::Eigen ) +set_target_properties(mkt_algorithm_diff PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Set include directories target_include_directories(mkt_algorithm_diff PUBLIC diff --git a/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt index 75ac25c..55f90aa 100644 --- a/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt +++ b/src/Algorithms/Libraries/mkt_plugins/CMakeLists.txt @@ -55,6 +55,10 @@ target_link_libraries(${PROJECT_NAME}_goal_checker PRIVATE Boost::boost ) +set_target_properties(${PROJECT_NAME}_goal_checker PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + target_include_directories(${PROJECT_NAME}_goal_checker PUBLIC $ @@ -74,6 +78,10 @@ target_link_libraries(${PROJECT_NAME}_standard_traj_generator PRIVATE Boost::boost ) +set_target_properties(${PROJECT_NAME}_standard_traj_generator PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + target_include_directories(${PROJECT_NAME}_standard_traj_generator PUBLIC $ 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 8e6c322..fc4011f 100755 --- a/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/global_planners/two_points_planner/CMakeLists.txt @@ -46,6 +46,10 @@ target_link_libraries(two_points_planner PRIVATE Boost::boost ) +set_target_properties(two_points_planner PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Set include directories target_include_directories(two_points_planner PUBLIC 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 eeffc64..e7c472e 100644 --- a/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt +++ b/src/Algorithms/Packages/local_planners/pnkx_local_planner/CMakeLists.txt @@ -56,6 +56,10 @@ target_link_libraries(${PROJECT_NAME}_utils PRIVATE Boost::boost ) +set_target_properties(${PROJECT_NAME}_utils PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Set include directories cho utils target_include_directories(${PROJECT_NAME}_utils PUBLIC @@ -78,6 +82,10 @@ target_link_libraries(${PROJECT_NAME} PRIVATE Boost::boost ) +set_target_properties(${PROJECT_NAME} PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Set include directories cho thư viện chính target_include_directories(${PROJECT_NAME} PUBLIC 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 de194b8..8c129b6 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 @@ -60,7 +60,6 @@ void pnkx_local_planner::PNKXLocalPlanner::initialize(robot::NodeHandle &parent, throw nav_core2::CostmapDataLagException("Costmap2DROBOT is out of date somehow."); parent_ = parent; - robot::log_warning_at(__FILE__, __LINE__, "Parent namespace: %s", parent_.getNamespace().c_str()); planner_nh_ = robot::NodeHandle(parent_, name); // planner_nh_.printRootParams(); this->getParams(planner_nh_); diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index b3be5da..10521c1 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit b3be5da393abee4ef535f68f702cd84c02f3b98b +Subproject commit 10521c1629301d3e81f040213f08de7881bc5e59 diff --git a/src/Libraries/nav_2d_utils/CMakeLists.txt b/src/Libraries/nav_2d_utils/CMakeLists.txt index 38548e1..a89a66d 100755 --- a/src/Libraries/nav_2d_utils/CMakeLists.txt +++ b/src/Libraries/nav_2d_utils/CMakeLists.txt @@ -39,6 +39,10 @@ target_link_libraries(conversions Boost::thread ) +set_target_properties(conversions PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + add_library(path_ops src/path_ops.cpp) target_include_directories(path_ops PUBLIC @@ -58,22 +62,28 @@ target_include_directories(polygons $ $ ) -target_link_libraries(polygons - PUBLIC - nav_2d_msgs - geometry_msgs - robot_cpp - PRIVATE - ${xmlrpcpp_LIBRARIES} -) if(xmlrpcpp_FOUND) target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS}) + target_link_libraries(polygons + PUBLIC nav_2d_msgs geometry_msgs robot_cpp + PRIVATE ${xmlrpcpp_LIBRARIES}) elseif(XmlRpcCpp_FOUND) target_include_directories(polygons PRIVATE ${XmlRpcCpp_INCLUDE_DIRS}) - target_link_libraries(polygons PRIVATE ${XmlRpcCpp_LIBRARIES}) + target_link_libraries(polygons + PUBLIC nav_2d_msgs geometry_msgs robot_cpp + PRIVATE ${XmlRpcCpp_LIBRARIES}) +else() + target_include_directories(polygons PRIVATE ${xmlrpcpp_INCLUDE_DIRS}) + target_link_libraries(polygons + PUBLIC nav_2d_msgs geometry_msgs robot_cpp + PRIVATE ${xmlrpcpp_LIBRARIES}) endif() +set_target_properties(polygons PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + add_library(bounds src/bounds.cpp) target_include_directories(bounds PUBLIC @@ -87,6 +97,10 @@ target_link_libraries(bounds robot_cpp ) +set_target_properties(bounds PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + add_library(tf_help src/tf_help.cpp) target_include_directories(tf_help PUBLIC @@ -102,6 +116,10 @@ target_link_libraries(tf_help robot_cpp ) +set_target_properties(tf_help PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) + # Create an INTERFACE library that represents all nav_2d_utils libraries add_library(nav_2d_utils INTERFACE) target_include_directories(nav_2d_utils diff --git a/src/Libraries/robot_cpp b/src/Libraries/robot_cpp index 7540426..c54064a 160000 --- a/src/Libraries/robot_cpp +++ b/src/Libraries/robot_cpp @@ -1 +1 @@ -Subproject commit 754042654785374e19d614a2d5927362735ed7ca +Subproject commit c54064a3196792b2ad4362fae2b2294aa9b6872c diff --git a/src/Libraries/tf3/CMakeLists.txt b/src/Libraries/tf3/CMakeLists.txt index 65cb09b..ba86e54 100644 --- a/src/Libraries/tf3/CMakeLists.txt +++ b/src/Libraries/tf3/CMakeLists.txt @@ -14,6 +14,9 @@ include_directories(include ${console_bridge_INCLUDE_DIRS}) #CPP Libraries add_library(tf3 src/cache.cpp src/buffer_core.cpp src/static_cache.cpp) target_link_libraries(tf3 ${Boost_LIBRARIES} ${console_bridge_LIBRARIES}) +set_target_properties(tf3 PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) target_include_directories(tf3 PUBLIC $ $) @@ -21,7 +24,9 @@ target_include_directories(tf3 PUBLIC add_executable(simple_tf3_example examples/simple_tf3_example.cpp) target_include_directories(simple_tf3_example PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) target_link_libraries(simple_tf3_example PRIVATE tf3 pthread ${console_bridge_LIBRARIES}) - +set_target_properties(simple_tf3_example PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} +) install(TARGETS tf3 ARCHIVE DESTINATION lib LIBRARY DESTINATION lib 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 8d398d7..cc153b1 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 @@ -9,6 +9,9 @@ #include #include +// robot protocol msgs +#include + // tf #include #include @@ -60,35 +63,35 @@ namespace move_base_core switch (state) { case State::PENDING: - return "PENDING"; // Chờ xử lý + return "PENDING"; // Chờ xử lý case State::ACTIVE: - return "ACTIVE"; // Đang hoạt động + return "ACTIVE"; // Đang hoạt động case State::PREEMPTED: - return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới + return "PREEMPTED"; // Đã bị huỷ bởi yêu cầu mới case State::SUCCEEDED: - return "SUCCEEDED"; // Thành công + return "SUCCEEDED"; // Thành công case State::ABORTED: - return "ABORTED"; // Bị lỗi + return "ABORTED"; // Bị lỗi case State::REJECTED: - return "REJECTED"; // Từ chối bởi planner hoặc controller + return "REJECTED"; // Từ chối bởi planner hoặc controller case State::PREEMPTING: - return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu + return "PREEMPTING"; // Đang huỷ bỏ theo yêu cầu case State::RECALLING: - return "RECALLING"; // Đang huỷ bỏ nội bộ + return "RECALLING"; // Đang huỷ bỏ nội bộ case State::RECALLED: - return "RECALLED"; // Đã được thu hồi + return "RECALLED"; // Đã được thu hồi case State::LOST: - return "LOST"; // Mất trạng thái + return "LOST"; // Mất trạng thái case State::PLANNING: - return "PLANNING"; // Đang lập kế hoạch đường đi + return "PLANNING"; // Đang lập kế hoạch đường đi case State::CONTROLLING: - return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan + return "CONTROLLING"; // Đang điều khiển robot di chuyển theo plan case State::CLEARING: - return "CLEARING"; // Đang dọn dẹp bản đồ / costmap + return "CLEARING"; // Đang dọn dẹp bản đồ / costmap case State::PAUSED: - return "PAUSED"; // Tạm dừng + return "PAUSED"; // Tạm dừng default: - return "UNKNOWN_STATE"; // Không xác định + return "UNKNOWN_STATE"; // Không xác định } } @@ -165,7 +168,7 @@ namespace move_base_core * @param fprt A vector of points representing the robot's footprint polygon. * The points should be ordered counter-clockwise. * Example: - * + * ^ Y | | P3(-0.3, 0.2) P2(0.3, 0.2) @@ -196,6 +199,19 @@ namespace move_base_core double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) = 0; + /** + * @brief Send a goal for the robot to navigate to. + * @param msg Order message. + * @param goal Target pose in the global frame. + * @param xy_goal_tolerance Acceptable error in X/Y (meters). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if goal was accepted and sent successfully. + */ + virtual bool moveTo(const robot_protocol_msgs::Order &msg, + const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) = 0; + /** * @brief Send a docking goal to a predefined marker (e.g. charger). * @param maker Marker name or ID. @@ -209,6 +225,19 @@ namespace move_base_core double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) = 0; + /** + * @brief Send a docking goal to a predefined marker (e.g. charger). + * @param msg Order message. + * @param goal Target pose for docking. + * @param xy_goal_tolerance Acceptable XY error (meters). + * @param yaw_goal_tolerance Acceptable heading error (radians). + * @return True if docking command succeeded. + */ + virtual bool dockTo(const robot_protocol_msgs::Order &msg, + const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) = 0; + /** * @brief Move straight toward the target position (X-axis). * @param goal Target pose. diff --git a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt index de09229..f610875 100755 --- a/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt +++ b/src/Navigations/Cores/nav_core_adapter/CMakeLists.txt @@ -66,19 +66,13 @@ set(_NAV_CORE_ADAPTER_RPATH ) set_target_properties(costmap_adapter PROPERTIES - BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}" - INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}" - LINK_FLAGS "-Wl,--disable-new-dtags" + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} ) set_target_properties(local_planner_adapter PROPERTIES - BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}" - INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}" - LINK_FLAGS "-Wl,--disable-new-dtags" + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} ) set_target_properties(global_planner_adapter PROPERTIES - BUILD_RPATH "${_NAV_CORE_ADAPTER_RPATH}" - INSTALL_RPATH "${_NAV_CORE_ADAPTER_RPATH}" - LINK_FLAGS "-Wl,--disable-new-dtags" + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} ) # Cài đặt header files diff --git a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp index 4555aaf..47a166c 100755 --- a/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp +++ b/src/Navigations/Cores/nav_core_adapter/src/local_planner_adapter.cpp @@ -93,10 +93,9 @@ namespace nav_core_adapter planner_name = nav_2d_utils::searchAndGetParam(adapter_nh_, "position_planner_name", std::string("dwb_local_planner::DWBLocalPlanner")); adapter_nh_.setParam("planner_name", planner_name); } - - 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"; 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"; planner_loader_ = boost::dll::import_alias( path_file_so, planner_name, boost::dll::load_mode::append_decorations); planner_ = planner_loader_(); diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index 0ebc8ea..4b73e49 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -157,9 +157,7 @@ else() # Set RPATH để ưu tiên thư viện build cục bộ set_target_properties(move_base PROPERTIES - BUILD_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp" - INSTALL_RPATH "${CMAKE_BINARY_DIR}/src/Libraries/costmap_2d:${CMAKE_BINARY_DIR}/src/Libraries/node_handle:${CMAKE_BINARY_DIR}/src/Libraries/tf3:${CMAKE_BINARY_DIR}/src/Libraries/robot_time:${CMAKE_BINARY_DIR}/src/Libraries/xmlrpcpp" - LINK_FLAGS "-Wl,--disable-new-dtags" + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} ) 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 778043a..1e699bd 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 @@ -27,6 +27,7 @@ #include #include #include +#include #include #include @@ -94,6 +95,19 @@ namespace move_base double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) override; + /** + * @brief Send a goal for the robot to navigate to. + * @param msg Order message. + * @param goal Target pose in the global frame. + * @param xy_goal_tolerance Acceptable error in X/Y (meters). + * @param yaw_goal_tolerance Acceptable angular error (radians). + * @return True if goal was accepted and sent successfully. + */ + virtual bool moveTo(const robot_protocol_msgs::Order &msg, + const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) override; + /** * @brief Send a docking goal to a predefined marker (e.g. charger). * @param maker Marker name or ID. @@ -107,8 +121,22 @@ namespace move_base double xy_goal_tolerance = 0.0, double yaw_goal_tolerance = 0.0) override; + /** + * @brief Send a docking goal to a predefined marker (e.g. charger). + * @param msg Order message. + * @param goal Target pose for docking. + * @param xy_goal_tolerance Acceptable XY error (meters). + * @param yaw_goal_tolerance Acceptable heading error (radians). + * @return True if docking command succeeded. + */ + virtual bool dockTo(const robot_protocol_msgs::Order &msg, + const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance = 0.0, + double yaw_goal_tolerance = 0.0) override; + /** * @brief Move straight toward the target position (X-axis). + * @param msg Order message. * @param goal Target pose. * @param xy_goal_tolerance Acceptable positional error (meters). * @return True if command issued successfully. @@ -206,22 +234,6 @@ namespace move_base */ void swapPlanner(std::string base_global_planner); - // /** - // * @brief A service call that clears the costmaps of obstacles - // * @param req The service request - // * @param resp The service response - // * @return True if the service call succeeds, false otherwise - // */ - // bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp); - - // /** - // * @brief A service call that can be made when the action is inactive that will return a plan - // * @param req The goal request - // * @param resp The plan request - // * @return True if planning succeeded, false otherwise - // */ - // bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp); - /** * @brief Make a new global plan * @param goal The goal to plan to diff --git a/src/Navigations/Packages/move_base/plugins.xml b/src/Navigations/Packages/move_base/plugins.xml new file mode 100644 index 0000000..6d769c4 --- /dev/null +++ b/src/Navigations/Packages/move_base/plugins.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 0a5d16e..dbf4217 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -15,6 +15,7 @@ #include #include #include +#include move_base::MoveBase::MoveBase() : initialized_(false), @@ -156,34 +157,24 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) // we'll assume the radius of the robot to be consistent with what's specified for the costmaps // From config param double inscribed_radius; - robot::log_info("[%s:%d] inscribed_radius: %f", __FILE__, __LINE__, inscribed_radius); private_nh_.getParam("local_costmap/inscribed_radius", inscribed_radius, 0.2); double circumscribed_radius; private_nh_.getParam("local_costmap/circumscribed_radius", circumscribed_radius, 0.3); - robot::log_info("[%s:%d] circumscribed_radius: %f", __FILE__, __LINE__, circumscribed_radius); inscribed_radius_ = inscribed_radius; circumscribed_radius_ = circumscribed_radius; private_nh_.getParam("clearing_radius", clearing_radius_, 0.0); - robot::log_info("[%s:%d] clearing_radius: %f", __FILE__, __LINE__, clearing_radius_); double conservative_reset_dist; private_nh_.getParam("conservative_reset_dist", conservative_reset_dist, 0.0); - robot::log_info("[%s:%d] conservative_reset_dist: %f", __FILE__, __LINE__, conservative_reset_dist); conservative_reset_dist_ = conservative_reset_dist; - robot::log_info("[%s:%d] shutdown_costmaps: %f", __FILE__, __LINE__, shutdown_costmaps_); private_nh_.getParam("shutdown_costmaps", shutdown_costmaps_, false); - robot::log_info("[%s:%d] clearing_rotation_allowed: %f", __FILE__, __LINE__, clearing_rotation_allowed_); private_nh_.getParam("clearing_rotation_allowed", clearing_rotation_allowed_, true); - robot::log_info("[%s:%d] recovery_behavior_enabled: %f", __FILE__, __LINE__, recovery_behavior_enabled_); private_nh_.getParam("recovery_behavior_enabled", recovery_behavior_enabled_, true); // create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map - robot::log_info("[%s:%d] create the ros wrapper for the planner's costmap..."); try { planner_costmap_robot_ = new costmap_2d::Costmap2DROBOT("global_costmap", *tf_); - robot::log_info("[%s:%d] planner_costmap_robot_->pause()"); planner_costmap_robot_->pause(); - robot::log_info("[%s:%d] planner_costmap_robot_->pause()"); } catch (const std::exception &ex) { @@ -193,10 +184,10 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) // initialize the global planner try { + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath("CustomPlanner"); planner_loader_ = boost::dll::import_alias( - "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Algorithms/Packages/global_planners/two_points_planner/libtwo_points_planner.so", - global_planner, - boost::dll::load_mode::append_decorations); + path_file_so, global_planner, boost::dll::load_mode::append_decorations); planner_ = planner_loader_(); if (!planner_) @@ -214,13 +205,22 @@ void move_base::MoveBase::initialize(TFListenerPtr tf) robot::log_error("[%s:%d]\n EXCEPTION in global planner: %s", __FILE__, __LINE__, ex.what()); throw std::runtime_error("Failed to create the " + global_planner + " planner"); } - // create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map - controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_); - controller_costmap_robot_->pause(); + + try + { + controller_costmap_robot_ = new costmap_2d::Costmap2DROBOT("local_costmap", *tf_); + controller_costmap_robot_->pause(); + } + catch (const std::exception &ex) + { + robot::log_error("[%s:%d]\n EXCEPTION: %s", __FILE__, __LINE__, ex.what()); + throw std::runtime_error("Failed to create the controller_costmap_robot_"); + } // create a local planner try { - std::string path_file_so = "/home/robotics/AGV/Diff_Wheel_Prj/pnkx_nav_core/build/src/Navigations/Cores/nav_core_adapter/liblocal_planner_adapter.so"; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath("LocalPlannerAdapter"); controller_loader_ = boost::dll::import_alias( path_file_so, local_planner, boost::dll::load_mode::append_decorations); @@ -363,15 +363,18 @@ bool move_base::MoveBase::moveTo(const geometry_msgs::PoseStamped &goal, double if (cancel_ctr_) cancel_ctr_ = false; - // move_base_msgs::MoveBaseActionGoal action_goal; - // action_goal.header.stamp = robot::Time::now(); - // action_goal.goal.target_pose = goal; - // action_goal_pub_.publish(action_goal); - lock.unlock(); return true; } +bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, + const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + return false; +} + + bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) { @@ -454,14 +457,17 @@ bool move_base::MoveBase::dockTo(const std::string &maker, const geometry_msgs:: if (cancel_ctr_) cancel_ctr_ = false; - // move_base_msgs::MoveBaseActionGoal action_goal; - // action_goal.header.stamp = robot::Time::now(); - // action_goal.goal.target_pose = goal; - // action_goal_pub_.publish(action_goal); lock.unlock(); return true; } +bool move_base::MoveBase::dockTo(const robot_protocol_msgs::Order &msg, + const geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + return false; +} + bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, double xy_goal_tolerance) { if (setup_) @@ -505,10 +511,6 @@ bool move_base::MoveBase::moveStraightTo(const geometry_msgs::PoseStamped &goal, } if (cancel_ctr_) cancel_ctr_ = false; - // move_base_msgs::MoveBaseActionGoal action_goal; - // action_goal.header.stamp = robot::Time::now(); - // action_goal.goal.target_pose = goal; - // action_goal_pub_.publish(action_goal); lock.unlock(); return true;