From 57b77ac14b144acddccc95cc9dd7e8d4d58728dc Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 13 Jan 2026 14:30:22 +0700 Subject: [PATCH] update --- CMakeLists.txt | 11 + config/costmap_common_params.yaml | 8 +- config/move_base_common_params.yaml | 7 +- src/APIs/c_api/src/nav_c_api.cpp | 2 +- .../include/score_algorithm/goal_checker.h | 3 +- .../include/score_algorithm/score_algorithm.h | 2 +- .../score_algorithm/trajectory_generator.h | 2 +- .../diff/diff_predictive_trajectory.h | 2 +- .../src/diff/diff_go_straight.cpp | 2 +- .../src/diff/diff_predictive_trajectory.cpp | 2 +- .../src/diff/diff_rotate_to_goal.cpp | 2 +- .../mkt_msgs/include/mkt_msgs/Trajectory2D.h | 2 +- .../include/mkt_plugins/equation_line.h | 2 +- .../mkt_plugins/kinematic_parameters.h | 2 +- .../mkt_plugins/one_d_velocity_iterator.h | 2 +- .../include/mkt_plugins/simple_goal_checker.h | 2 +- .../mkt_plugins/standard_traj_generator.h | 2 +- .../include/mkt_plugins/velocity_iterator.h | 2 +- .../mkt_plugins/src/goal_checker.cpp | 2 +- .../src/limited_accel_generator.cpp | 2 +- .../Packages/global_planners/dock_planner | 2 +- .../two_points_planner/two_points_planner.h | 2 +- .../src/two_points_planner.cpp | 4 +- .../pnkx_local_planner/pnkx_local_planner.h | 3 +- .../include/pnkx_local_planner/transforms.h | 2 +- .../src/pnkx_local_planner.cpp | 2 +- .../pnkx_local_planner/src/transforms.cpp | 2 +- src/Libraries/common_msgs | 2 +- src/Libraries/costmap_2d | 2 +- .../include/robot/plugin_loader_helper.h | 2 +- src/Libraries/robot_cpp/include/robot/robot.h | 12 + src/Libraries/robot_cpp/src/node_handle.cpp | 5 + src/Libraries/robot_time | 2 +- .../robot_actionlib_msgs/CMakeLists.txt | 156 +++++ .../include/robot_actionlib_msgs/GoalID.h | 65 ++ .../include/robot_actionlib_msgs/GoalStatus.h | 112 ++++ .../robot_actionlib_msgs/GoalStatusArray.h | 64 ++ .../robot_actionlib_msgs/package.xml | 30 + .../CMakeLists.txt | 142 ++++ .../clear_costmap_recovery.h | 68 ++ .../robot_clear_costmap_recovery/package.xml | 27 + .../src/clear_costmap_recovery.cpp | 337 ++++++++++ .../test/clear_tester.cpp | 95 +++ .../test/clear_tests.launch | 6 + .../test/params.yaml | 13 + .../robot_move_base_msgs/CMakeLists.txt | 156 +++++ .../robot_move_base_msgs/MoveBaseAction.h | 69 ++ .../MoveBaseActionFeedback.h | 68 ++ .../robot_move_base_msgs/MoveBaseActionGoal.h | 69 ++ .../MoveBaseActionResult.h | 68 ++ .../robot_move_base_msgs/MoveBaseFeedback.h | 58 ++ .../robot_move_base_msgs/MoveBaseGoal.h | 57 ++ .../robot_move_base_msgs/MoveBaseResult.h | 39 ++ .../robot_move_base_msgs/RecoveryStatus.h | 71 ++ .../robot_move_base_msgs/package.xml | 30 + .../Packages/move_base/CMakeLists.txt | 7 +- .../move_base/README_ACTION_SERVER.md | 161 ----- .../examples/IMPLEMENTATION_GUIDE.md | 265 ++++++++ .../examples/README_SIMPLE_ACTION_SERVER.md | 220 ++++++ .../examples/action_server_example.cpp | 192 ------ .../examples/simple_action_server_example.cpp | 343 ++++++++++ .../move_base/include/move_base/move_base.h | 23 +- .../include/move_base/simple_action_server.h | 259 +++++++ .../Packages/move_base/package.xml | 3 + .../Packages/move_base/src/convert_data.cpp | 3 +- .../Packages/move_base/src/move_base.cpp | 441 +++++++++++- .../Packages/move_base/src/move_base_main.cpp | 3 +- .../move_base/src/simple_action_server.cpp | 632 ++++++++++++++++++ 68 files changed, 4035 insertions(+), 420 deletions(-) create mode 100644 src/Libraries/robot_cpp/include/robot/robot.h create mode 100644 src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt create mode 100644 src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalID.h create mode 100644 src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatus.h create mode 100644 src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatusArray.h create mode 100644 src/Navigations/Libraries/robot_actionlib_msgs/package.xml create mode 100644 src/Navigations/Libraries/robot_clear_costmap_recovery/CMakeLists.txt create mode 100755 src/Navigations/Libraries/robot_clear_costmap_recovery/include/robot_clear_costmap_recovery/clear_costmap_recovery.h create mode 100644 src/Navigations/Libraries/robot_clear_costmap_recovery/package.xml create mode 100755 src/Navigations/Libraries/robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp create mode 100755 src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tester.cpp create mode 100755 src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tests.launch create mode 100755 src/Navigations/Libraries/robot_clear_costmap_recovery/test/params.yaml create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/CMakeLists.txt create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseAction.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionFeedback.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionGoal.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionResult.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseFeedback.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseGoal.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseResult.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/RecoveryStatus.h create mode 100644 src/Navigations/Libraries/robot_move_base_msgs/package.xml delete mode 100644 src/Navigations/Packages/move_base/README_ACTION_SERVER.md create mode 100644 src/Navigations/Packages/move_base/examples/IMPLEMENTATION_GUIDE.md create mode 100644 src/Navigations/Packages/move_base/examples/README_SIMPLE_ACTION_SERVER.md delete mode 100644 src/Navigations/Packages/move_base/examples/action_server_example.cpp create mode 100644 src/Navigations/Packages/move_base/examples/simple_action_server_example.cpp create mode 100644 src/Navigations/Packages/move_base/include/move_base/simple_action_server.h create mode 100644 src/Navigations/Packages/move_base/src/simple_action_server.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ed3b70c..68ea21d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -138,6 +138,17 @@ if (NOT TARGET pnkx_local_planner) add_subdirectory(${CMAKE_SOURCE_DIR}/src/Algorithms/Packages/local_planners/pnkx_local_planner) endif() +if (NOT TARGET robot_actionlib_msgs) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_actionlib_msgs) +endif() + +if (NOT TARGET robot_move_base_msgs) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_move_base_msgs) +endif() + +if (NOT TARGET robot_clear_costmap_recovery) + add_subdirectory(${CMAKE_SOURCE_DIR}/src/Navigations/Libraries/robot_clear_costmap_recovery) +endif() # 2. Main packages (phụ thuộc vào cores) # message(STATUS "[move_base] Shared library configured") diff --git a/config/costmap_common_params.yaml b/config/costmap_common_params.yaml index f88c096..5619b75 100755 --- a/config/costmap_common_params.yaml +++ b/config/costmap_common_params.yaml @@ -76,8 +76,8 @@ global_costmap: lethal_cost_threshold: 100 obstacles: - observation_sources: ffffff_scan_marking f_scan_clearing b_scan_marking b_scan_clearing - ffffff_scan_marking: + observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing + f_scan_marking: topic: /f_scan data_type: LaserScan clearing: false @@ -131,8 +131,8 @@ local_costmap: lethal_cost_threshold: 100 obstacles: - observation_sources: ffffff_scan_marking f_scan_clearing b_scan_marking b_scan_clearing - ffffff_scan_marking: + observation_sources: f_scan_marking f_scan_clearing b_scan_marking b_scan_clearing + f_scan_marking: topic: /f_scan data_type: LaserScan clearing: false diff --git a/config/move_base_common_params.yaml b/config/move_base_common_params.yaml index 449cad0..b9a0b0a 100755 --- a/config/move_base_common_params.yaml +++ b/config/move_base_common_params.yaml @@ -10,8 +10,8 @@ oscillation_distance: 0.4 ### recovery behaviors recovery_behavior_enabled: true recovery_behaviors: [ - {name: aggressive_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, - {name: conservative_reset, type: clear_costmap_recovery/ClearCostmapRecovery}, + {name: aggressive_reset, type: ClearCostmapRecovery}, + {name: conservative_reset, type: ClearCostmapRecovery}, ] conservative_reset: @@ -21,5 +21,8 @@ conservative_reset: aggressive_reset: reset_distance: 3.0 +ClearCostmapRecovery: + library_path: librobot_clear_costmap_recovery + MoveBase: library_path: libmove_base diff --git a/src/APIs/c_api/src/nav_c_api.cpp b/src/APIs/c_api/src/nav_c_api.cpp index 99bc5aa..bc53e30 100644 --- a/src/APIs/c_api/src/nav_c_api.cpp +++ b/src/APIs/c_api/src/nav_c_api.cpp @@ -12,7 +12,7 @@ #include #include #include -#include +#include // ============================================================================ diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h index ebd0015..3322dee 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/goal_checker.h @@ -1,8 +1,7 @@ #ifndef _SCORE_GOAL_CHECKER_H_INCLUDED__ #define _SCORE_GOAL_CHECKER_H_INCLUDED__ -#include -#include +#include #include #include #include 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 8bc9aeb..5b6a05c 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 @@ -1,7 +1,7 @@ #ifndef _SCORE_ALGORITHM_H_INCLUDED__ #define _SCORE_ALGORITHM_H_INCLUDED__ -#include +#include #include #include #include diff --git a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h index 8ccbb3d..724666d 100755 --- a/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h +++ b/src/Algorithms/Cores/score_algorithm/include/score_algorithm/trajectory_generator.h @@ -1,7 +1,7 @@ #ifndef _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H #define _SCORE_ALGORITHM_TRAJECTORY_GENERATOR_H -#include +#include #include #include #include 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 8cf6c55..8b49fa3 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 @@ -1,7 +1,7 @@ #ifndef _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ #define _NAV_ALGORITHM_DIFF_PREDICTIVE_TRAJECTORY_H_INCLUDED__ -#include +#include #include #include diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp index d5388be..23f5c21 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_go_straight.cpp @@ -1,6 +1,6 @@ #include #include -#include +#include void mkt_algorithm::diff::GoStraight::initialize( robot::NodeHandle &nh, const std::string &name, TFListenerPtr tf, robot_costmap_2d::Costmap2DROBOT *costmap_robot, const score_algorithm::TrajectoryGenerator::Ptr &traj) 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 6f8989b..3055289 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 @@ -1,6 +1,6 @@ #include #include -#include +#include #define LIMIT_VEL_THETA 0.33 diff --git a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp index 61eeb69..5644082 100644 --- a/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp +++ b/src/Algorithms/Libraries/mkt_algorithm/src/diff/diff_rotate_to_goal.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include diff --git a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h index 6d5268f..84ab6e5 100644 --- a/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h +++ b/src/Algorithms/Libraries/mkt_msgs/include/mkt_msgs/Trajectory2D.h @@ -4,7 +4,7 @@ #ifndef MKT_MSGS_MESSAGE_TRAJECTORY2D_H #define MKT_MSGS_MESSAGE_TRAJECTORY2D_H -#include +#include #include #include #include diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h index 8c4a306..31bbeaa 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/equation_line.h @@ -1,6 +1,6 @@ #ifndef __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__ #define __MKT_PLUGINS_EQUATION_LINE_H_INCLUDED__ -#include +#include #include #include diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h index 86064e1..319792a 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/kinematic_parameters.h @@ -1,7 +1,7 @@ #ifndef MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_ #define MKT_PLUGINS_KINEMATIC_PARAMETERS_H_INCLUDED_ -#include +#include #include namespace mkt_plugins diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h index e1014c1..eb6b396 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/one_d_velocity_iterator.h @@ -3,7 +3,7 @@ #include #include -#include +#include namespace mkt_plugins { diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h index 439660b..4acd429 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/simple_goal_checker.h @@ -1,7 +1,7 @@ #ifndef _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__ #define _MKTPLUGINS_SIMPLE_GOAL_CHECKER_H_INCLUDED__ -#include +#include #include namespace mkt_plugins diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h index ee26ac5..9c56b03 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/standard_traj_generator.h @@ -1,7 +1,7 @@ #ifndef MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_ #define MKT_PLUGINS_STANDARD_TRAJ_GENERATOR_H_INCLUDED_ -#include +#include #include #include #include diff --git a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h index 25bbceb..12fbe3c 100644 --- a/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h +++ b/src/Algorithms/Libraries/mkt_plugins/include/mkt_plugins/velocity_iterator.h @@ -1,7 +1,7 @@ #ifndef MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_ #define MKT_PLUGINS_VELOCITY_ITERATOR_H_INCLUDED_ -#include +#include #include #include diff --git a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp index c987633..09fe4c8 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/goal_checker.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include 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 8b16943..03edf58 100644 --- a/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp +++ b/src/Algorithms/Libraries/mkt_plugins/src/limited_accel_generator.cpp @@ -1,4 +1,4 @@ -#include +#include #include #include #include diff --git a/src/Algorithms/Packages/global_planners/dock_planner b/src/Algorithms/Packages/global_planners/dock_planner index c4ae396..8f0cd33 160000 --- a/src/Algorithms/Packages/global_planners/dock_planner +++ b/src/Algorithms/Packages/global_planners/dock_planner @@ -1 +1 @@ -Subproject commit c4ae3961ab21c5a2fc48bdcfa9007369fa2efe4c +Subproject commit 8f0cd33ec717d4c65cc71bb6a9bf1a756fca3a2e 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 b07b780..88ace26 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 @@ -7,7 +7,7 @@ #include #include #include -#include +#include namespace two_points_planner { 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 3a72670..ce49201 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 @@ -1,4 +1,4 @@ -#include +#include #include #include #include @@ -6,8 +6,6 @@ #include #include #include -#include -#include #include #include 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 f2edc38..9a4f0e6 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 @@ -1,8 +1,7 @@ #ifndef _PNKX_LOCAL_PLANNER_H_INCLUDED_ #define _PNKX_LOCAL_PLANNER_H_INCLUDED_ -#include -#include +#include #include #include #include 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 e6653c2..4c7cd12 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 @@ -1,7 +1,7 @@ #ifndef _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__ #define _MKT_LOCAL_PLANNER_TRANSFORMS_CHECKER_H_INCLUDED__ -#include +#include #include #include 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 46578e1..0fcae10 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 @@ -14,7 +14,7 @@ #include #include #include -#include +#include pnkx_local_planner::PNKXLocalPlanner::PNKXLocalPlanner() : initialized_(false) 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 6579875..a95751a 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 @@ -1,7 +1,7 @@ #include #include #include -#include +#include #include #include diff --git a/src/Libraries/common_msgs b/src/Libraries/common_msgs index b8b0528..98543e6 160000 --- a/src/Libraries/common_msgs +++ b/src/Libraries/common_msgs @@ -1 +1 @@ -Subproject commit b8b0528f1e649b93ff07b3458aa01f251b286405 +Subproject commit 98543e6c540f48075f0a6976c83a884524956264 diff --git a/src/Libraries/costmap_2d b/src/Libraries/costmap_2d index 81e7874..9026c03 160000 --- a/src/Libraries/costmap_2d +++ b/src/Libraries/costmap_2d @@ -1 +1 @@ -Subproject commit 81e78742744c0a5b8a8ef298fa254035c41dc1da +Subproject commit 9026c03e1e3bbecdce224958c04147f9e9ae7fd2 diff --git a/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h b/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h index 6cea10b..8a4458d 100644 --- a/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h +++ b/src/Libraries/robot_cpp/include/robot/plugin_loader_helper.h @@ -14,7 +14,7 @@ #include #include #include -#include +#include namespace robot { diff --git a/src/Libraries/robot_cpp/include/robot/robot.h b/src/Libraries/robot_cpp/include/robot/robot.h new file mode 100644 index 0000000..ae57aa0 --- /dev/null +++ b/src/Libraries/robot_cpp/include/robot/robot.h @@ -0,0 +1,12 @@ +#ifndef ROBOT_ROBOT_H +#define ROBOT_ROBOT_H + +#include +#include +#include +#include +#include +#include +#include + +#endif \ No newline at end of file diff --git a/src/Libraries/robot_cpp/src/node_handle.cpp b/src/Libraries/robot_cpp/src/node_handle.cpp index b48b686..31657aa 100644 --- a/src/Libraries/robot_cpp/src/node_handle.cpp +++ b/src/Libraries/robot_cpp/src/node_handle.cpp @@ -1981,6 +1981,11 @@ namespace robot template float NodeHandle::param(const std::string ¶m_name) const; template float NodeHandle::param(const std::string ¶m_name, const float &default_val) const; template bool NodeHandle::param(const std::string ¶m_name, float ¶m_val, const float &default_val) const; + + // Vector instantiations + template std::vector NodeHandle::param>(const std::string ¶m_name) const; + template std::vector NodeHandle::param>(const std::string ¶m_name, const std::vector &default_val) const; + template bool NodeHandle::param>(const std::string ¶m_name, std::vector ¶m_val, const std::vector &default_val) const; // Static member initialization std::string NodeHandle::config_directory_; diff --git a/src/Libraries/robot_time b/src/Libraries/robot_time index 35b77e9..750dc94 160000 --- a/src/Libraries/robot_time +++ b/src/Libraries/robot_time @@ -1 +1 @@ -Subproject commit 35b77e9fa25fe4473c9fb0215f902ea587347859 +Subproject commit 750dc94c61be406937ad49e0f215a4fdce538fc5 diff --git a/src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt b/src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt new file mode 100644 index 0000000..b01d981 --- /dev/null +++ b/src/Navigations/Libraries/robot_actionlib_msgs/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 3.0.2) +project(robot_actionlib_msgs VERSION 1.0.0 LANGUAGES CXX) + +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building robot_actionlib_msgs with Catkin") + +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building robot_actionlib_msgs with Standalone CMake") +endif() + +# C++ Standard - must be set before find_package +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Find dependencies +find_package(Boost REQUIRED) + +if (NOT BUILDING_WITH_CATKIN) + + # Enable Position Independent Code + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + # Cấu hình RPATH để tránh cycle trong runtime search path + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + set(PACKAGES_DIR + robot_std_msgs + utils + robot_time + ) + +else() + +# ======================================================== +# Catkin specific configuration +# ======================================================== + find_package(catkin REQUIRED COMPONENTS + robot_std_msgs + utils + robot_time + ) + + find_package(Boost REQUIRED) + + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS robot_std_msgs utils robot_time + DEPENDS Boost + ) + + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) +endif() + +# Tìm tất cả header files +file(GLOB_RECURSE HEADERS "include/robot_actionlib_msgs/*.h") + +# Tạo INTERFACE library (header-only) +add_library(${PROJECT_NAME} INTERFACE) + +if(BUILDING_WITH_CATKIN) + add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + # Set include directories + target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ + ) + + # Link dependencies (header-only, chỉ cần include paths) + target_link_libraries(${PROJECT_NAME} + INTERFACE + ${catkin_LIBRARIES} + Boost::headers + ) + +else() + + # Set include directories + target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ + ) + + # Link dependencies (header-only, chỉ cần include paths) + target_link_libraries(${PROJECT_NAME} + INTERFACE + ${PACKAGES_DIR} + Boost::headers + ) + +endif() + +if(BUILDING_WITH_CATKIN) + ## Mark libraries for installation + ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + +else() + + install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + # Export targets + install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + + # Print configuration info + 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 "Dependencies: robot_std_msgs, utils, robot_time, Boost") + message(STATUS "=================================") +endif() + diff --git a/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalID.h b/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalID.h new file mode 100644 index 0000000..1bb37e3 --- /dev/null +++ b/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalID.h @@ -0,0 +1,65 @@ +// Generated by gencpp from file actionlib_msgs/GoalID.msg +// DO NOT EDIT! + +#ifndef ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALID_H +#define ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALID_H + +#include +#include +#include + +#include + +namespace robot_actionlib_msgs +{ + template + struct GoalID_ + { + typedef GoalID_ Type; + + GoalID_() + : stamp(), id() + { + } + GoalID_(const ContainerAllocator &_alloc) + : stamp(), id(_alloc) + { + (void)_alloc; + } + + typedef robot::Time _stamp_type; + _stamp_type stamp; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _id_type; + _id_type id; + + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID_> Ptr; + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID_ const> ConstPtr; + + }; // struct GoalID_ + + typedef ::robot_actionlib_msgs::GoalID_> GoalID; + + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID> GoalIDPtr; + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalID const> GoalIDConstPtr; + + // constants requiring out of line definition + + + + template + bool operator==(const ::robot_actionlib_msgs::GoalID_ &lhs, const ::robot_actionlib_msgs::GoalID_ &rhs) + { + return lhs.stamp == rhs.stamp && + lhs.id == rhs.id; + } + + template + bool operator!=(const ::robot_actionlib_msgs::GoalID_ &lhs, const ::robot_actionlib_msgs::GoalID_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_actionlib_msgs + +#endif // ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALID_H diff --git a/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatus.h b/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatus.h new file mode 100644 index 0000000..239bad2 --- /dev/null +++ b/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatus.h @@ -0,0 +1,112 @@ +// Generated by gencpp from file actionlib_msgs/GoalStatus.msg +// DO NOT EDIT! + +#ifndef ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUS_H +#define ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUS_H + +#include +#include +#include +#include + +namespace robot_actionlib_msgs +{ + template + struct GoalStatus_ + { + typedef GoalStatus_ Type; + + GoalStatus_() + : goal_id(), status(0), text() + { + } + GoalStatus_(const ContainerAllocator &_alloc) + : goal_id(_alloc), status(0), text(_alloc) + { + (void)_alloc; + } + + typedef ::robot_actionlib_msgs::GoalID_ _goal_id_type; + _goal_id_type goal_id; + + typedef uint8_t _status_type; + _status_type status; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _text_type; + _text_type text; + +// reducing the odds to have name collisions with Windows.h +#if defined(_WIN32) && defined(PENDING) +#undef PENDING +#endif +#if defined(_WIN32) && defined(ACTIVE) +#undef ACTIVE +#endif +#if defined(_WIN32) && defined(PREEMPTED) +#undef PREEMPTED +#endif +#if defined(_WIN32) && defined(SUCCEEDED) +#undef SUCCEEDED +#endif +#if defined(_WIN32) && defined(ABORTED) +#undef ABORTED +#endif +#if defined(_WIN32) && defined(REJECTED) +#undef REJECTED +#endif +#if defined(_WIN32) && defined(PREEMPTING) +#undef PREEMPTING +#endif +#if defined(_WIN32) && defined(RECALLING) +#undef RECALLING +#endif +#if defined(_WIN32) && defined(RECALLED) +#undef RECALLED +#endif +#if defined(_WIN32) && defined(LOST) +#undef LOST +#endif + + enum + { + PENDING = 0u, + ACTIVE = 1u, + PREEMPTED = 2u, + SUCCEEDED = 3u, + ABORTED = 4u, + REJECTED = 5u, + PREEMPTING = 6u, + RECALLING = 7u, + RECALLED = 8u, + LOST = 9u, + }; + + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus_> Ptr; + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus_ const> ConstPtr; + + }; // struct GoalStatus_ + + typedef ::robot_actionlib_msgs::GoalStatus_> GoalStatus; + + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus> GoalStatusPtr; + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatus const> GoalStatusConstPtr; + + // constants requiring out of line definition + + template + bool operator==(const ::robot_actionlib_msgs::GoalStatus_ &lhs, const ::robot_actionlib_msgs::GoalStatus_ &rhs) + { + return lhs.goal_id == rhs.goal_id && + lhs.status == rhs.status && + lhs.text == rhs.text; + } + + template + bool operator!=(const ::robot_actionlib_msgs::GoalStatus_ &lhs, const ::robot_actionlib_msgs::GoalStatus_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_actionlib_msgs + +#endif // ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUS_H diff --git a/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatusArray.h b/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatusArray.h new file mode 100644 index 0000000..e5591c8 --- /dev/null +++ b/src/Navigations/Libraries/robot_actionlib_msgs/include/robot_actionlib_msgs/GoalStatusArray.h @@ -0,0 +1,64 @@ +// Generated by gencpp from file actionlib_msgs/GoalStatusArray.msg +// DO NOT EDIT! + +#ifndef ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUSARRAY_H +#define ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUSARRAY_H + +#include +#include +#include + +#include +#include + +namespace robot_actionlib_msgs +{ + template + struct GoalStatusArray_ + { + typedef GoalStatusArray_ Type; + + GoalStatusArray_() + : header(), status_list() + { + } + GoalStatusArray_(const ContainerAllocator &_alloc) + : header(_alloc), status_list(_alloc) + { + (void)_alloc; + } + + typedef ::robot_std_msgs::Header_ _header_type; + _header_type header; + + typedef std::vector<::robot_actionlib_msgs::GoalStatus_, typename std::allocator_traits::template rebind_alloc<::robot_actionlib_msgs::GoalStatus_>> _status_list_type; + _status_list_type status_list; + + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray_> Ptr; + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray_ const> ConstPtr; + + }; // struct GoalStatusArray_ + + typedef ::robot_actionlib_msgs::GoalStatusArray_> GoalStatusArray; + + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray> GoalStatusArrayPtr; + typedef boost::shared_ptr<::robot_actionlib_msgs::GoalStatusArray const> GoalStatusArrayConstPtr; + + // constants requiring out of line definition + + template + bool operator==(const ::robot_actionlib_msgs::GoalStatusArray_ &lhs, const ::robot_actionlib_msgs::GoalStatusArray_ &rhs) + { + return lhs.header == rhs.header && + lhs.status_list == rhs.status_list; + } + + template + bool operator!=(const ::robot_actionlib_msgs::GoalStatusArray_ &lhs, const ::robot_actionlib_msgs::GoalStatusArray_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_actionlib_msgs + +#endif // ROBOT_ACTIONLIB_MSGS_MESSAGE_GOALSTATUSARRAY_H diff --git a/src/Navigations/Libraries/robot_actionlib_msgs/package.xml b/src/Navigations/Libraries/robot_actionlib_msgs/package.xml new file mode 100644 index 0000000..720d24b --- /dev/null +++ b/src/Navigations/Libraries/robot_actionlib_msgs/package.xml @@ -0,0 +1,30 @@ + + robot_actionlib_msgs + 0.7.10 + + robot_actionlib_msgs is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. robot_actionlib_msgs + 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_actionlib_msgs + + catkin + + robot_std_msgs + robot_std_msgs + + utils + utils + + robot_time + robot_time + \ No newline at end of file diff --git a/src/Navigations/Libraries/robot_clear_costmap_recovery/CMakeLists.txt b/src/Navigations/Libraries/robot_clear_costmap_recovery/CMakeLists.txt new file mode 100644 index 0000000..ef16f43 --- /dev/null +++ b/src/Navigations/Libraries/robot_clear_costmap_recovery/CMakeLists.txt @@ -0,0 +1,142 @@ +cmake_minimum_required(VERSION 3.0.2) +project(robot_clear_costmap_recovery VERSION 1.0.0 LANGUAGES CXX) + +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building robot_clear_costmap_recovery with Catkin") + +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building robot_clear_costmap_recovery with Standalone CMake") +endif() + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Find dependencies +find_package(console_bridge REQUIRED) +find_package(Boost REQUIRED COMPONENTS system thread) + +if (NOT BUILDING_WITH_CATKIN) + +# Enable Position Independent Code + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + +# Cấu hình RPATH để tránh cycle trong runtime search path + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + set(PACKAGES_DIR + robot_nav_core + robot_costmap_2d + robot_cpp + tf3 + console_bridge + ) + +else() + +# ======================================================== +# Catkin specific configuration +# ======================================================== + find_package(catkin REQUIRED COMPONENTS + robot_nav_core + robot_costmap_2d + robot_cpp + tf3 + ) + +# Find dependencies + find_package(console_bridge REQUIRED) + find_package(Boost REQUIRED COMPONENTS system thread) + + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS robot_cpp robot_nav_core robot_costmap_2d tf3 + DEPENDS console_bridge Boost + ) + + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) +endif() + +# Libraries +add_library(${PROJECT_NAME} src/clear_costmap_recovery.cpp) + +if(BUILDING_WITH_CATKIN) + add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +else() + + target_link_libraries(${PROJECT_NAME} + PUBLIC + ${PACKAGES_DIR} + ${Boost_LIBRARIES} + ) + + set_target_properties(${PROJECT_NAME} PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ) + +endif() + +if(BUILDING_WITH_CATKIN) + ## Mark libraries for installation + ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + PATTERN ".svn" EXCLUDE + ) +else() + + install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + ) + + # Export targets + install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" + PATTERN ".svn" EXCLUDE + ) + + # 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: ${PROJECT_NAME}") + message(STATUS "Dependencies: robot_nav_core, robot_costmap_2d, robot_cpp, tf3, console_bridge, Boost") + message(STATUS "=================================") +endif() diff --git a/src/Navigations/Libraries/robot_clear_costmap_recovery/include/robot_clear_costmap_recovery/clear_costmap_recovery.h b/src/Navigations/Libraries/robot_clear_costmap_recovery/include/robot_clear_costmap_recovery/clear_costmap_recovery.h new file mode 100755 index 0000000..85f834f --- /dev/null +++ b/src/Navigations/Libraries/robot_clear_costmap_recovery/include/robot_clear_costmap_recovery/clear_costmap_recovery.h @@ -0,0 +1,68 @@ +/********************************************************************* +* +* Software License Agreement (BSD License) +* +* +* Author: Kevin Lee +*********************************************************************/ +#ifndef _ROBOT_CLEAR_COSTMAP_RECOVERY_H_ +#define _ROBOT_CLEAR_COSTMAP_RECOVERY_H_ +#include +#include +#include + +#include + +namespace robot_clear_costmap_recovery +{ + /** + * @class ClearCostmapRecovery + * @brief A recovery behavior that reverts the navigation stack's costmaps to the static map outside of a user-specified region. + */ + class ClearCostmapRecovery : public robot_nav_core::RecoveryBehavior { + public: + /** + * @brief Constructor, make sure to call initialize in addition to actually initialize the object + * @param + * @return + */ + ClearCostmapRecovery(); + + /** + * @brief Initialization function for the ClearCostmapRecovery recovery behavior + * @param tf A pointer to a transform listener + * @param global_costmap A pointer to the global_costmap used by the navigation stack + * @param local_costmap A pointer to the local_costmap used by the navigation stack + */ + void initialize(std::string name, tf3::BufferCore* tf, + robot_costmap_2d::Costmap2DROBOT* global_costmap, robot_costmap_2d::Costmap2DROBOT* local_costmap); + + /** + * @brief Run the ClearCostmapRecovery recovery behavior. Reverts the + * costmap to the static map outside of a user-specified window and + * clears unknown space around the robot. + */ + void runBehavior(); + + /** + * @brief Export factory function for the ClearCostmapRecovery recovery behavior + * @return A pointer to the ClearCostmapRecovery recovery behavior + */ + static std::shared_ptr create(); + + private: + void clear(robot_costmap_2d::Costmap2DROBOT* costmap); + void clearMap(boost::shared_ptr costmap, double pose_x, double pose_y); + robot_costmap_2d::Costmap2DROBOT* global_costmap_, *local_costmap_; + std::string name_; + tf3::BufferCore* tf_; + bool initialized_; + bool force_updating_; ///< force costmap update after clearing, so we don't need to wait for update thread + double reset_distance_; + bool invert_area_to_clear_; + std::string affected_maps_; ///< clear only local, global or both costmaps + std::set clearable_layers_; ///< Layer names which will be cleared. + }; +} // namespace robot_clear_costmap_recovery + +#endif // _ROBOT_CLEAR_COSTMAP_RECOVERY_H_ diff --git a/src/Navigations/Libraries/robot_clear_costmap_recovery/package.xml b/src/Navigations/Libraries/robot_clear_costmap_recovery/package.xml new file mode 100644 index 0000000..7f896d0 --- /dev/null +++ b/src/Navigations/Libraries/robot_clear_costmap_recovery/package.xml @@ -0,0 +1,27 @@ + + robot_clear_costmap_recovery + 0.7.10 + + robot_clear_costmap_recovery is a recovery behavior that reverts the navigation stack's + costmaps to the static map outside of a user-specified region. + + Eitan Marder-Eppstein + Tully Foote + BSD + + http://www.ros.org/wiki/robot_clear_costmap_recovery + + catkin + + robot_nav_core + robot_nav_core + + robot_costmap_2d + robot_costmap_2d + + robot_cpp + robot_cpp + + tf3 + tf3 + \ No newline at end of file diff --git a/src/Navigations/Libraries/robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp b/src/Navigations/Libraries/robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp new file mode 100755 index 0000000..2ce3ab7 --- /dev/null +++ b/src/Navigations/Libraries/robot_clear_costmap_recovery/src/clear_costmap_recovery.cpp @@ -0,0 +1,337 @@ +/********************************************************************* + * + * Software License Agreement (BSD License) + * + * Copyright (c) 2009, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * Author: Eitan Marder-Eppstein + *********************************************************************/ +#include +#include +#include +#include + +using robot_costmap_2d::NO_INFORMATION; + +namespace robot_clear_costmap_recovery +{ + ClearCostmapRecovery::ClearCostmapRecovery() : global_costmap_(NULL), local_costmap_(NULL), + tf_(NULL), initialized_(false) {} + + void ClearCostmapRecovery::initialize(std::string name, tf3::BufferCore *tf, + robot_costmap_2d::Costmap2DROBOT *global_costmap, robot_costmap_2d::Costmap2DROBOT *local_costmap) + { + if (!initialized_) + { + name_ = name; + tf_ = tf; + global_costmap_ = global_costmap; + local_costmap_ = local_costmap; + + // get some parameters from the parameter server + robot::NodeHandle private_nh("~/" + name_); + + private_nh.param("reset_distance", reset_distance_, 3.0); + private_nh.param("invert_area_to_clear", invert_area_to_clear_, false); + private_nh.param("force_updating", force_updating_, false); + private_nh.param("affected_maps", affected_maps_, std::string("both")); + if (affected_maps_ != "local" && affected_maps_ != "global" && affected_maps_ != "both") + { + robot::log_warning("Wrong value for affected_maps parameter: '%s'; valid values are 'local', 'global' or 'both'; " + "defaulting to 'both'", + affected_maps_.c_str()); + affected_maps_ = "both"; + } + + std::vector clearable_layers_default, clearable_layers; + clearable_layers_default.push_back(std::string("obstacles")); + private_nh.param("layer_names", clearable_layers, clearable_layers_default); + + for (unsigned i = 0; i < clearable_layers.size(); i++) + { + robot::log_info("Recovery behavior will clear layer '%s'", clearable_layers[i].c_str()); + clearable_layers_.insert(clearable_layers[i]); + } + initialized_ = true; + } + else + { + robot::log_error("You should not call initialize twice on this object, doing nothing "); + } + } + + void ClearCostmapRecovery::runBehavior() + { + robot::log_info("[ClearCostmapRecovery::runBehavior] ===== ENTRY ====="); + robot::log_info("[ClearCostmapRecovery::runBehavior] Name: %s", name_.c_str()); + + // Check initialization + if (!initialized_) + { + robot::log_error("[ClearCostmapRecovery::runBehavior] ERROR: Not initialized!"); + robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (not initialized) ====="); + return; + } + robot::log_debug("[ClearCostmapRecovery::runBehavior] Initialization check: OK"); + + // Check costmaps + if (global_costmap_ == NULL || local_costmap_ == NULL) + { + robot::log_error("[ClearCostmapRecovery::runBehavior] ERROR: Costmaps are NULL!"); + robot::log_error("[ClearCostmapRecovery::runBehavior] global_costmap_: %s", global_costmap_ ? "valid" : "NULL"); + robot::log_error("[ClearCostmapRecovery::runBehavior] local_costmap_: %s", local_costmap_ ? "valid" : "NULL"); + robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (NULL costmaps) ====="); + return; + } + robot::log_debug("[ClearCostmapRecovery::runBehavior] Costmaps check: OK"); + + // Log parameters + robot::log_info("[ClearCostmapRecovery::runBehavior] Parameters:"); + robot::log_info("[ClearCostmapRecovery::runBehavior] - reset_distance: %.2f m", reset_distance_); + robot::log_info("[ClearCostmapRecovery::runBehavior] - invert_area_to_clear: %s", invert_area_to_clear_ ? "true" : "false"); + robot::log_info("[ClearCostmapRecovery::runBehavior] - force_updating: %s", force_updating_ ? "true" : "false"); + robot::log_info("[ClearCostmapRecovery::runBehavior] - affected_maps: %s", affected_maps_.c_str()); + robot::log_info("[ClearCostmapRecovery::runBehavior] - clearable_layers count: %zu", clearable_layers_.size()); + for (const auto& layer : clearable_layers_) + { + robot::log_info("[ClearCostmapRecovery::runBehavior] - layer: '%s'", layer.c_str()); + } + + // Log clearing mode + if (!invert_area_to_clear_) + { + robot::log_warning("[ClearCostmapRecovery::runBehavior] %s is Clearing %s costmap%s outside a square (%.2fm) large centered on the robot.", + name_.c_str(), affected_maps_.c_str(), + affected_maps_ == "both" ? "s" : "", reset_distance_); + } + else + { + robot::log_warning("[ClearCostmapRecovery::runBehavior] %s is Clearing %s costmap%s inside a square (%.2fm) large centered on the robot.", + name_.c_str(), affected_maps_.c_str(), + affected_maps_ == "both" ? "s" : "", reset_distance_); + } + + // Process global costmap + robot::WallTime t0 = robot::WallTime::now(); + if (affected_maps_ == "global" || affected_maps_ == "both") + { + robot::log_info("[ClearCostmapRecovery::runBehavior] Processing GLOBAL costmap..."); + robot::WallTime clear_start = robot::WallTime::now(); + + clear(global_costmap_); + + robot::WallDuration clear_duration = robot::WallTime::now() - clear_start; + robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap clear() completed in %.6f s", clear_duration.toSec()); + + if (force_updating_) + { + robot::log_info("[ClearCostmapRecovery::runBehavior] Force updating global costmap..."); + robot::WallTime update_start = robot::WallTime::now(); + global_costmap_->updateMap(); + robot::WallDuration update_duration = robot::WallTime::now() - update_start; + robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap updateMap() completed in %.6f s", update_duration.toSec()); + } + + robot::WallDuration total_duration = robot::WallTime::now() - t0; + robot::log_info("[ClearCostmapRecovery::runBehavior] Global costmap total processing time: %.6f s", total_duration.toSec()); + robot::log_debug("[ClearCostmapRecovery::runBehavior] Global costmap cleared in %fs", total_duration.toSec()); + } + else + { + robot::log_debug("[ClearCostmapRecovery::runBehavior] Skipping global costmap (affected_maps='%s')", affected_maps_.c_str()); + } + + // Process local costmap + t0 = robot::WallTime::now(); + if (affected_maps_ == "local" || affected_maps_ == "both") + { + robot::log_info("[ClearCostmapRecovery::runBehavior] Processing LOCAL costmap..."); + robot::WallTime clear_start = robot::WallTime::now(); + + clear(local_costmap_); + + robot::WallDuration clear_duration = robot::WallTime::now() - clear_start; + robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap clear() completed in %.6f s", clear_duration.toSec()); + + if (force_updating_) + { + robot::log_info("[ClearCostmapRecovery::runBehavior] Force updating local costmap..."); + robot::WallTime update_start = robot::WallTime::now(); + local_costmap_->updateMap(); + robot::WallDuration update_duration = robot::WallTime::now() - update_start; + robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap updateMap() completed in %.6f s", update_duration.toSec()); + } + + robot::WallDuration total_duration = robot::WallTime::now() - t0; + robot::log_info("[ClearCostmapRecovery::runBehavior] Local costmap total processing time: %.6f s", total_duration.toSec()); + robot::log_debug("[ClearCostmapRecovery::runBehavior] Local costmap cleared in %fs", total_duration.toSec()); + } + else + { + robot::log_debug("[ClearCostmapRecovery::runBehavior] Skipping local costmap (affected_maps='%s')", affected_maps_.c_str()); + } + + robot::log_info("[ClearCostmapRecovery::runBehavior] ===== EXIT (success) ====="); + } + + void ClearCostmapRecovery::clear(robot_costmap_2d::Costmap2DROBOT *costmap) + { + robot::log_debug("[ClearCostmapRecovery::clear] ===== ENTRY ====="); + + std::vector> *plugins = costmap->getLayeredCostmap()->getPlugins(); + robot::log_debug("[ClearCostmapRecovery::clear] Found %zu plugins", plugins ? plugins->size() : 0); + + robot_geometry_msgs::PoseStamped pose; + + if (!costmap->getRobotPose(pose)) + { + robot::log_error("[ClearCostmapRecovery::clear] ERROR: Cannot clear map because pose cannot be retrieved"); + robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT (no pose) ====="); + return; + } + + double x = pose.pose.position.x; + double y = pose.pose.position.y; + robot::log_debug("[ClearCostmapRecovery::clear] Robot pose: x=%.3f, y=%.3f", x, y); + + if (!plugins) + { + robot::log_error("[ClearCostmapRecovery::clear] ERROR: plugins pointer is NULL"); + robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT (NULL plugins) ====="); + return; + } + + int processed_layers = 0; + int skipped_layers = 0; + + for (std::vector>::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) + { + boost::shared_ptr plugin = *pluginp; + if (!plugin) + { + robot::log_warning("[ClearCostmapRecovery::clear] WARNING: Found NULL plugin, skipping"); + skipped_layers++; + continue; + } + + std::string name = plugin->getName(); + std::string original_name = name; + int slash = name.rfind('/'); + if (slash != std::string::npos) + { + name = name.substr(slash + 1); + } + + robot::log_debug("[ClearCostmapRecovery::clear] Processing layer: '%s' (original: '%s')", name.c_str(), original_name.c_str()); + + if (clearable_layers_.count(name) != 0) + { + robot::log_info("[ClearCostmapRecovery::clear] Layer '%s' is in clearable_layers, processing...", name.c_str()); + + // check if the value is convertable + if (!dynamic_cast(plugin.get())) + { + robot::log_error("[ClearCostmapRecovery::clear] ERROR: Layer '%s' is not derived from costmap_2d::CostmapLayer", name.c_str()); + skipped_layers++; + continue; + } + + boost::shared_ptr costmap_layer; + costmap_layer = boost::static_pointer_cast(plugin); + + robot::log_debug("[ClearCostmapRecovery::clear] Calling clearMap for layer '%s' at (%.3f, %.3f)", name.c_str(), x, y); + clearMap(costmap_layer, x, y); + processed_layers++; + robot::log_info("[ClearCostmapRecovery::clear] Successfully cleared layer '%s'", name.c_str()); + } + else + { + robot::log_debug("[ClearCostmapRecovery::clear] Layer '%s' is NOT in clearable_layers, skipping", name.c_str()); + skipped_layers++; + } + } + + robot::log_info("[ClearCostmapRecovery::clear] Summary: processed=%d, skipped=%d, total=%zu", + processed_layers, skipped_layers, plugins->size()); + robot::log_debug("[ClearCostmapRecovery::clear] ===== EXIT ====="); + } + + void ClearCostmapRecovery::clearMap(boost::shared_ptr costmap, + double pose_x, double pose_y) + { + robot::log_debug("[ClearCostmapRecovery::clearMap] ===== ENTRY ====="); + robot::log_debug("[ClearCostmapRecovery::clearMap] Robot pose: (%.3f, %.3f)", pose_x, pose_y); + robot::log_debug("[ClearCostmapRecovery::clearMap] Reset distance: %.3f m", reset_distance_); + robot::log_debug("[ClearCostmapRecovery::clearMap] Invert area: %s", invert_area_to_clear_ ? "true" : "false"); + + boost::unique_lock lock(*(costmap->getMutex())); + robot::log_debug("[ClearCostmapRecovery::clearMap] Mutex acquired"); + + double start_point_x = pose_x - reset_distance_ / 2; + double start_point_y = pose_y - reset_distance_ / 2; + double end_point_x = start_point_x + reset_distance_; + double end_point_y = start_point_y + reset_distance_; + + robot::log_debug("[ClearCostmapRecovery::clearMap] Clear area (world coords):"); + robot::log_debug("[ClearCostmapRecovery::clearMap] start: (%.3f, %.3f)", start_point_x, start_point_y); + robot::log_debug("[ClearCostmapRecovery::clearMap] end: (%.3f, %.3f)", end_point_x, end_point_y); + + int start_x, start_y, end_x, end_y; + costmap->worldToMapNoBounds(start_point_x, start_point_y, start_x, start_y); + costmap->worldToMapNoBounds(end_point_x, end_point_y, end_x, end_y); + + robot::log_debug("[ClearCostmapRecovery::clearMap] Clear area (map coords):"); + robot::log_debug("[ClearCostmapRecovery::clearMap] start: (%d, %d)", start_x, start_y); + robot::log_debug("[ClearCostmapRecovery::clearMap] end: (%d, %d)", end_x, end_y); + + costmap->clearArea(start_x, start_y, end_x, end_y, invert_area_to_clear_); + robot::log_debug("[ClearCostmapRecovery::clearMap] clearArea() called"); + + double ox = costmap->getOriginX(), oy = costmap->getOriginY(); + double width = costmap->getSizeInMetersX(), height = costmap->getSizeInMetersY(); + robot::log_debug("[ClearCostmapRecovery::clearMap] Costmap bounds:"); + robot::log_debug("[ClearCostmapRecovery::clearMap] origin: (%.3f, %.3f)", ox, oy); + robot::log_debug("[ClearCostmapRecovery::clearMap] size: %.3f x %.3f m", width, height); + + costmap->addExtraBounds(ox, oy, ox + width, oy + height); + robot::log_debug("[ClearCostmapRecovery::clearMap] addExtraBounds() called"); + robot::log_debug("[ClearCostmapRecovery::clearMap] ===== EXIT ====="); + return; + } + + std::shared_ptr ClearCostmapRecovery::create() + { + return std::make_shared(); + } + +}; + +BOOST_DLL_ALIAS(robot_clear_costmap_recovery::ClearCostmapRecovery::create, ClearCostmapRecovery) \ No newline at end of file diff --git a/src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tester.cpp b/src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tester.cpp new file mode 100755 index 0000000..fa0e46a --- /dev/null +++ b/src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tester.cpp @@ -0,0 +1,95 @@ +#include +#include +#include + +#include +#include + +tf2_ros::Buffer* transformer; +tf2_ros::TransformListener* tfl; + +using costmap_2d::LETHAL_OBSTACLE; + +void testClearBehavior(std::string name, + double distance, + bool obstacles, + bool static_map, + costmap_2d::Costmap2DROS* global_costmap, + costmap_2d::Costmap2DROS* local_costmap){ + robot_clear_costmap_recovery::ClearCostmapRecovery clear = robot_clear_costmap_recovery::ClearCostmapRecovery(); + + ros::NodeHandle clr("~/" + name); + clr.setParam("reset_distance", distance); + + std::vector clearable_layers; + if(obstacles) + clearable_layers.push_back( std::string("obstacles") ); + if(static_map) + clearable_layers.push_back( std::string("static_map") ); + + clr.setParam("layer_names", clearable_layers); + + clear.initialize(name, transformer, global_costmap, local_costmap); + + clear.runBehavior(); +} + +void testCountLethal(std::string name, double distance, bool obstacles, bool static_map, int global_lethal, int local_lethal=0) +{ + costmap_2d::Costmap2DROS global(name + "/global", *transformer); + costmap_2d::Costmap2DROS local(name + "/local" , *transformer); + boost::shared_ptr olayer; + + std::vector >* plugins = global.getLayeredCostmap()->getPlugins(); + for (std::vector >::iterator pluginp = plugins->begin(); pluginp != plugins->end(); ++pluginp) { + boost::shared_ptr plugin = *pluginp; + if(plugin->getName().find("obstacles")!=std::string::npos){ + olayer = boost::static_pointer_cast(plugin); + addObservation(&(*olayer), 5.0, 0.0, MAX_Z/2, 0, 0, MAX_Z/2); + addObservation(&(*olayer), 0.0, 5.0, MAX_Z/2, 0, 0, MAX_Z/2); + } + } + + global.updateMap(); + local.updateMap(); + olayer->clearStaticObservations(true, true); + + testClearBehavior("clear", distance, obstacles, static_map, &global, &local); + + global.updateMap(); + local.updateMap(); + + printMap(*global.getCostmap()); + ASSERT_EQ(countValues(*global.getCostmap(), LETHAL_OBSTACLE), global_lethal); + ASSERT_EQ(countValues( *local.getCostmap(), LETHAL_OBSTACLE), local_lethal); + +} + +TEST(ClearTester, basicTest){ + testCountLethal("base", 3.0, true, false, 20); +} + +TEST(ClearTester, bigRadiusTest){ + testCountLethal("base", 20.0, true, false, 22); +} + +TEST(ClearTester, clearNoLayersTest){ + testCountLethal("base", 20.0, false, false, 22); +} + +TEST(ClearTester, clearBothTest){ + testCountLethal("base", 3.0, true, true, 0); +} + +TEST(ClearTester, clearBothTest2){ + testCountLethal("base", 12.0, true, true, 6); +} + + +int main(int argc, char** argv){ + ros::init(argc, argv, "clear_tests"); + testing::InitGoogleTest(&argc, argv); + transformer = new tf2_ros::Buffer(ros::Duration(10)); + tfl = new tf2_ros::TransformListener(*transformer); + return RUN_ALL_TESTS(); +} diff --git a/src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tests.launch b/src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tests.launch new file mode 100755 index 0000000..637c207 --- /dev/null +++ b/src/Navigations/Libraries/robot_clear_costmap_recovery/test/clear_tests.launch @@ -0,0 +1,6 @@ + + + + + + diff --git a/src/Navigations/Libraries/robot_clear_costmap_recovery/test/params.yaml b/src/Navigations/Libraries/robot_clear_costmap_recovery/test/params.yaml new file mode 100755 index 0000000..7ebca4e --- /dev/null +++ b/src/Navigations/Libraries/robot_clear_costmap_recovery/test/params.yaml @@ -0,0 +1,13 @@ +base: + global: + plugins: + - {name: static_map, type: "costmap_2d::StaticLayer"} + - {name: obstacles, type: "costmap_2d::ObstacleLayer"} + - {name: inflation, type: "costmap_2d::InflationLayer"} + obstacles: + track_unknown_space: true + local: + plugins: [] + robot_radius: .5 +clear: + clearing_distance: 7.0 diff --git a/src/Navigations/Libraries/robot_move_base_msgs/CMakeLists.txt b/src/Navigations/Libraries/robot_move_base_msgs/CMakeLists.txt new file mode 100644 index 0000000..521ee5a --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/CMakeLists.txt @@ -0,0 +1,156 @@ +cmake_minimum_required(VERSION 3.0.2) +project(robot_move_base_msgs VERSION 1.0.0 LANGUAGES CXX) + +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building robot_move_base_msgs with Catkin") + +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building robot_move_base_msgs with Standalone CMake") +endif() + +# C++ Standard - must be set before find_package +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# Find dependencies +find_package(Boost REQUIRED) + +if (NOT BUILDING_WITH_CATKIN) + + # Enable Position Independent Code + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + # Cấu hình RPATH để tránh cycle trong runtime search path + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + set(PACKAGES_DIR + robot_std_msgs + robot_actionlib_msgs + robot_geometry_msgs + ) + +else() + +# ======================================================== +# Catkin specific configuration +# ======================================================== + find_package(catkin REQUIRED COMPONENTS + robot_std_msgs + robot_actionlib_msgs + robot_geometry_msgs + ) + + find_package(Boost REQUIRED) + + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} + CATKIN_DEPENDS robot_std_msgs robot_actionlib_msgs robot_geometry_msgs + DEPENDS Boost + ) + + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) +endif() + +# Tìm tất cả header files +file(GLOB_RECURSE HEADERS "include/robot_move_base_msgs/*.h") + +# Tạo INTERFACE library (header-only) +add_library(${PROJECT_NAME} INTERFACE) + +if(BUILDING_WITH_CATKIN) + add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + + # Set include directories + target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ + ) + + # Link dependencies (header-only, chỉ cần include paths) + target_link_libraries(${PROJECT_NAME} + INTERFACE + ${catkin_LIBRARIES} + Boost::headers + ) + +else() + + # Set include directories + target_include_directories(${PROJECT_NAME} + INTERFACE + $ + $ + ) + + # Link dependencies (header-only, chỉ cần include paths) + target_link_libraries(${PROJECT_NAME} + INTERFACE + ${PACKAGES_DIR} + Boost::headers + ) + +endif() + +if(BUILDING_WITH_CATKIN) + ## Mark libraries for installation + ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html + install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + +else() + + install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}-targets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + ) + + # Export targets + install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + ## Mark cpp header files for installation + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + PATTERN ".svn" EXCLUDE + ) + + # Print configuration info + 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 "Dependencies: robot_std_msgs, robot_actionlib_msgs, robot_geometry_msgs, Boost") + message(STATUS "=================================") +endif() + diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseAction.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseAction.h new file mode 100644 index 0000000..9dcdfee --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseAction.h @@ -0,0 +1,69 @@ +// Generated by gencpp from file robot_move_base_msgs/MoveBaseAction.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTION_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTION_H + +#include +#include +#include + +#include +#include +#include + +namespace robot_move_base_msgs +{ + template + struct MoveBaseAction_ + { + typedef MoveBaseAction_ Type; + + MoveBaseAction_() + : action_goal(), action_result(), action_feedback() + { + } + MoveBaseAction_(const ContainerAllocator &_alloc) + : action_goal(_alloc), action_result(_alloc), action_feedback(_alloc) + { + (void)_alloc; + } + + typedef ::robot_move_base_msgs::MoveBaseActionGoal_ _action_goal_type; + _action_goal_type action_goal; + + typedef ::robot_move_base_msgs::MoveBaseActionResult_ _action_result_type; + _action_result_type action_result; + + typedef ::robot_move_base_msgs::MoveBaseActionFeedback_ _action_feedback_type; + _action_feedback_type action_feedback; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction_ const> ConstPtr; + + }; // struct MoveBaseAction_ + + typedef ::robot_move_base_msgs::MoveBaseAction_> MoveBaseAction; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction> MoveBaseActionPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseAction const> MoveBaseActionConstPtr; + + // constants requiring out of line definition + + template + bool operator==(const ::robot_move_base_msgs::MoveBaseAction_ &lhs, const ::robot_move_base_msgs::MoveBaseAction_ &rhs) + { + return lhs.action_goal == rhs.action_goal && + lhs.action_result == rhs.action_result && + lhs.action_feedback == rhs.action_feedback; + } + + template + bool operator!=(const ::robot_move_base_msgs::MoveBaseAction_ &lhs, const ::robot_move_base_msgs::MoveBaseAction_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTION_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionFeedback.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionFeedback.h new file mode 100644 index 0000000..b066f75 --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionFeedback.h @@ -0,0 +1,68 @@ +// Generated by gencpp from file robot_move_base_msgs/MoveBaseActionFeedback.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONFEEDBACK_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONFEEDBACK_H + +#include +#include +#include + +#include +#include +#include + +namespace robot_move_base_msgs +{ + template + struct MoveBaseActionFeedback_ + { + typedef MoveBaseActionFeedback_ Type; + + MoveBaseActionFeedback_() + : header(), status(), feedback() + { + } + MoveBaseActionFeedback_(const ContainerAllocator &_alloc) + : header(_alloc), status(_alloc), feedback(_alloc) + { + (void)_alloc; + } + + typedef ::robot_std_msgs::Header_ _header_type; + _header_type header; + + typedef ::robot_actionlib_msgs::GoalStatus_ _status_type; + _status_type status; + + typedef ::robot_move_base_msgs::MoveBaseFeedback_ _feedback_type; + _feedback_type feedback; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback_ const> ConstPtr; + + }; // struct MoveBaseActionFeedback_ + + typedef ::robot_move_base_msgs::MoveBaseActionFeedback_> MoveBaseActionFeedback; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback> MoveBaseActionFeedbackPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionFeedback const> MoveBaseActionFeedbackConstPtr; + + // constants requiring out of line definition + template + bool operator==(const ::robot_move_base_msgs::MoveBaseActionFeedback_ &lhs, const ::robot_move_base_msgs::MoveBaseActionFeedback_ &rhs) + { + return lhs.header == rhs.header && + lhs.status == rhs.status && + lhs.feedback == rhs.feedback; + } + + template + bool operator!=(const ::robot_move_base_msgs::MoveBaseActionFeedback_ &lhs, const ::robot_move_base_msgs::MoveBaseActionFeedback_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONFEEDBACK_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionGoal.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionGoal.h new file mode 100644 index 0000000..2d6608e --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionGoal.h @@ -0,0 +1,69 @@ +// Generated by gencpp from file robot_move_base_msgs/MoveBaseActionGoal.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONGOAL_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONGOAL_H + +#include +#include +#include + +#include +#include +#include + +namespace robot_move_base_msgs +{ + template + struct MoveBaseActionGoal_ + { + typedef MoveBaseActionGoal_ Type; + + MoveBaseActionGoal_() + : header(), goal_id(), goal() + { + } + MoveBaseActionGoal_(const ContainerAllocator &_alloc) + : header(_alloc), goal_id(_alloc), goal(_alloc) + { + (void)_alloc; + } + + typedef ::robot_std_msgs::Header_ _header_type; + _header_type header; + + typedef ::robot_actionlib_msgs::GoalID_ _goal_id_type; + _goal_id_type goal_id; + + typedef ::robot_move_base_msgs::MoveBaseGoal_ _goal_type; + _goal_type goal; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal_ const> ConstPtr; + + }; // struct MoveBaseActionGoal_ + + typedef ::robot_move_base_msgs::MoveBaseActionGoal_> MoveBaseActionGoal; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal> MoveBaseActionGoalPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionGoal const> MoveBaseActionGoalConstPtr; + + // constants requiring out of line definition + + template + bool operator==(const ::robot_move_base_msgs::MoveBaseActionGoal_ &lhs, const ::robot_move_base_msgs::MoveBaseActionGoal_ &rhs) + { + return lhs.header == rhs.header && + lhs.goal_id == rhs.goal_id && + lhs.goal == rhs.goal; + } + + template + bool operator!=(const ::robot_move_base_msgs::MoveBaseActionGoal_ &lhs, const ::robot_move_base_msgs::MoveBaseActionGoal_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONGOAL_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionResult.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionResult.h new file mode 100644 index 0000000..322f62c --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseActionResult.h @@ -0,0 +1,68 @@ +// Generated by gencpp from file robot_move_base_msgs/MoveBaseActionResult.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONRESULT_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONRESULT_H + +#include +#include +#include + +#include +#include +#include + +namespace robot_move_base_msgs +{ + template + struct MoveBaseActionResult_ + { + typedef MoveBaseActionResult_ Type; + + MoveBaseActionResult_() + : header(), status(), result() + { + } + MoveBaseActionResult_(const ContainerAllocator &_alloc) + : header(_alloc), status(_alloc), result(_alloc) + { + (void)_alloc; + } + + typedef ::robot_std_msgs::Header_ _header_type; + _header_type header; + + typedef ::robot_actionlib_msgs::GoalStatus_ _status_type; + _status_type status; + + typedef ::robot_move_base_msgs::MoveBaseResult_ _result_type; + _result_type result; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult_ const> ConstPtr; + + }; // struct MoveBaseActionResult_ + + typedef ::robot_move_base_msgs::MoveBaseActionResult_> MoveBaseActionResult; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult> MoveBaseActionResultPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseActionResult const> MoveBaseActionResultConstPtr; + + // constants requiring out of line definition + template + bool operator==(const ::robot_move_base_msgs::MoveBaseActionResult_ &lhs, const ::robot_move_base_msgs::MoveBaseActionResult_ &rhs) + { + return lhs.header == rhs.header && + lhs.status == rhs.status && + lhs.result == rhs.result; + } + + template + bool operator!=(const ::robot_move_base_msgs::MoveBaseActionResult_ &lhs, const ::robot_move_base_msgs::MoveBaseActionResult_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEACTIONRESULT_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseFeedback.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseFeedback.h new file mode 100644 index 0000000..bd64e29 --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseFeedback.h @@ -0,0 +1,58 @@ +// Generated by gencpp from file robot_move_base_msgs/MoveBaseFeedback.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEFEEDBACK_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEFEEDBACK_H + +#include +#include +#include + +#include + +namespace robot_move_base_msgs +{ + template + struct MoveBaseFeedback_ + { + typedef MoveBaseFeedback_ Type; + + MoveBaseFeedback_() + : base_position() + { + } + MoveBaseFeedback_(const ContainerAllocator &_alloc) + : base_position(_alloc) + { + (void)_alloc; + } + + typedef ::robot_geometry_msgs::PoseStamped_ _base_position_type; + _base_position_type base_position; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback_ const> ConstPtr; + + }; // struct MoveBaseFeedback_ + + typedef ::robot_move_base_msgs::MoveBaseFeedback_> MoveBaseFeedback; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback> MoveBaseFeedbackPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseFeedback const> MoveBaseFeedbackConstPtr; + + // constants requiring out of line definition + template + bool operator==(const ::robot_move_base_msgs::MoveBaseFeedback_ &lhs, const ::robot_move_base_msgs::MoveBaseFeedback_ &rhs) + { + return lhs.base_position == rhs.base_position; + } + + template + bool operator!=(const ::robot_move_base_msgs::MoveBaseFeedback_ &lhs, const ::robot_move_base_msgs::MoveBaseFeedback_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEFEEDBACK_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseGoal.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseGoal.h new file mode 100644 index 0000000..bcdd110 --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseGoal.h @@ -0,0 +1,57 @@ +// Generated by gencpp from file robot_move_base_msgs/MoveBaseGoal.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEGOAL_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEGOAL_H + +#include +#include +#include +#include + +namespace robot_move_base_msgs +{ + template + struct MoveBaseGoal_ + { + typedef MoveBaseGoal_ Type; + + MoveBaseGoal_() + : target_pose() + { + } + MoveBaseGoal_(const ContainerAllocator &_alloc) + : target_pose(_alloc) + { + (void)_alloc; + } + + typedef ::robot_geometry_msgs::PoseStamped_ _target_pose_type; + _target_pose_type target_pose; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal_ const> ConstPtr; + + }; // struct MoveBaseGoal_ + + typedef ::robot_move_base_msgs::MoveBaseGoal_> MoveBaseGoal; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal> MoveBaseGoalPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseGoal const> MoveBaseGoalConstPtr; + + // constants requiring out of line definition + template + bool operator==(const ::robot_move_base_msgs::MoveBaseGoal_ &lhs, const ::robot_move_base_msgs::MoveBaseGoal_ &rhs) + { + return lhs.target_pose == rhs.target_pose; + } + + template + bool operator!=(const ::robot_move_base_msgs::MoveBaseGoal_ &lhs, const ::robot_move_base_msgs::MoveBaseGoal_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASEGOAL_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseResult.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseResult.h new file mode 100644 index 0000000..728a499 --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/MoveBaseResult.h @@ -0,0 +1,39 @@ +// Generated by gencpp from file robot_move_base_msgs/MoveBaseResult.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASERESULT_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASERESULT_H + +#include +#include +#include + +namespace robot_move_base_msgs +{ + template + struct MoveBaseResult_ + { + typedef MoveBaseResult_ Type; + + MoveBaseResult_() + { + } + MoveBaseResult_(const ContainerAllocator &_alloc) + { + (void)_alloc; + } + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult_ const> ConstPtr; + + }; // struct MoveBaseResult_ + + typedef ::robot_move_base_msgs::MoveBaseResult_> MoveBaseResult; + + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult> MoveBaseResultPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::MoveBaseResult const> MoveBaseResultConstPtr; + + // constants requiring out of line definition +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_MOVEBASERESULT_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/RecoveryStatus.h b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/RecoveryStatus.h new file mode 100644 index 0000000..a9c775f --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/include/robot_move_base_msgs/RecoveryStatus.h @@ -0,0 +1,71 @@ +// Generated by gencpp from file robot_move_base_msgs/RecoveryStatus.msg +// DO NOT EDIT! + +#ifndef ROBOT_MOVE_BASE_MSGS_MESSAGE_RECOVERYSTATUS_H +#define ROBOT_MOVE_BASE_MSGS_MESSAGE_RECOVERYSTATUS_H + +#include +#include +#include + +#include + +namespace robot_move_base_msgs +{ + template + struct RecoveryStatus_ + { + typedef RecoveryStatus_ Type; + + RecoveryStatus_() + : pose_stamped(), current_recovery_number(0), total_number_of_recoveries(0), recovery_behavior_name() + { + } + RecoveryStatus_(const ContainerAllocator &_alloc) + : pose_stamped(_alloc), current_recovery_number(0), total_number_of_recoveries(0), recovery_behavior_name(_alloc) + { + (void)_alloc; + } + + typedef ::robot_geometry_msgs::PoseStamped_ _pose_stamped_type; + _pose_stamped_type pose_stamped; + + typedef uint16_t _current_recovery_number_type; + _current_recovery_number_type current_recovery_number; + + typedef uint16_t _total_number_of_recoveries_type; + _total_number_of_recoveries_type total_number_of_recoveries; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _recovery_behavior_name_type; + _recovery_behavior_name_type recovery_behavior_name; + + typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus_> Ptr; + typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus_ const> ConstPtr; + + }; // struct RecoveryStatus_ + + typedef ::robot_move_base_msgs::RecoveryStatus_> RecoveryStatus; + + typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus> RecoveryStatusPtr; + typedef boost::shared_ptr<::robot_move_base_msgs::RecoveryStatus const> RecoveryStatusConstPtr; + + // constants requiring out of line definition + + template + bool operator==(const ::robot_move_base_msgs::RecoveryStatus_ &lhs, const ::robot_move_base_msgs::RecoveryStatus_ &rhs) + { + return lhs.pose_stamped == rhs.pose_stamped && + lhs.current_recovery_number == rhs.current_recovery_number && + lhs.total_number_of_recoveries == rhs.total_number_of_recoveries && + lhs.recovery_behavior_name == rhs.recovery_behavior_name; + } + + template + bool operator!=(const ::robot_move_base_msgs::RecoveryStatus_ &lhs, const ::robot_move_base_msgs::RecoveryStatus_ &rhs) + { + return !(lhs == rhs); + } + +} // namespace robot_move_base_msgs + +#endif // ROBOT_MOVE_BASE_MSGS_MESSAGE_RECOVERYSTATUS_H diff --git a/src/Navigations/Libraries/robot_move_base_msgs/package.xml b/src/Navigations/Libraries/robot_move_base_msgs/package.xml new file mode 100644 index 0000000..df9878e --- /dev/null +++ b/src/Navigations/Libraries/robot_move_base_msgs/package.xml @@ -0,0 +1,30 @@ + + robot_move_base_msgs + 0.7.10 + + robot_move_base_msgs is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. robot_move_base_msgs + 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_move_base_msgs + + catkin + + robot_std_msgs + robot_std_msgs + + robot_actionlib_msgs + robot_actionlib_msgs + + robot_geometry_msgs + robot_geometry_msgs + \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/CMakeLists.txt b/src/Navigations/Packages/move_base/CMakeLists.txt index d46fc98..bb6500e 100644 --- a/src/Navigations/Packages/move_base/CMakeLists.txt +++ b/src/Navigations/Packages/move_base/CMakeLists.txt @@ -42,6 +42,7 @@ if (NOT BUILDING_WITH_CATKIN) data_convert robot_nav_2d_utils robot_cpp + robot_move_base_msgs ) else() @@ -60,12 +61,13 @@ else() data_convert robot_nav_2d_utils robot_cpp + robot_move_base_msgs ) catkin_package( INCLUDE_DIRS include LIBRARIES ${PROJECT_NAME} - 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 + 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 robot_move_base_msgs DEPENDS Boost yaml-cpp ) @@ -82,6 +84,7 @@ endif() add_library(${PROJECT_NAME} SHARED src/move_base.cpp src/convert_data.cpp + src/simple_action_server.cpp ) if(BUILDING_WITH_CATKIN) @@ -201,6 +204,6 @@ else() message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}") message(STATUS "Libraries: ${PROJECT_NAME}") message(STATUS "Executables: ${PROJECT_NAME}_main") - message(STATUS "Dependencies: 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, Boost, yaml-cpp") + message(STATUS "Dependencies: 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, robot_move_base_msgs, Boost, yaml-cpp") message(STATUS "=================================") endif() diff --git a/src/Navigations/Packages/move_base/README_ACTION_SERVER.md b/src/Navigations/Packages/move_base/README_ACTION_SERVER.md deleted file mode 100644 index 4afdd9e..0000000 --- a/src/Navigations/Packages/move_base/README_ACTION_SERVER.md +++ /dev/null @@ -1,161 +0,0 @@ -# MoveBaseActionServer - Hướng dẫn sử dụng - -## Tổng quan - -`MoveBaseActionServer` cung cấp một interface tương tự ROS Action Server, cho phép bạn: -- Gửi goals và nhận feedback -- Theo dõi tiến trình navigation -- Hủy goals (cancel/preempt) -- Nhận kết quả khi hoàn thành - -## So sánh với ROS Action Server - -| ROS Action Server | MoveBaseActionServer | -|-------------------|---------------------| -| `actionlib::ActionServer` | `move_base::MoveBaseActionServer` | -| `as_->acceptNewGoal()` | `action_server.sendGoal()` | -| `as_->setSucceeded()` | `action_server.setSucceeded()` | -| `as_->setAborted()` | `action_server.setAborted()` | -| `as_->setPreempted()` | `action_server.setPreempted()` | -| `as_->publishFeedback()` | Tự động qua callback | -| `as_->isNewGoalAvailable()` | `action_server.isNewGoalAvailable()` | -| `as_->isPreemptRequested()` | `action_server.isPreemptRequested()` | - -## Cách sử dụng cơ bản - -### 1. Khởi tạo - -```cpp -#include - -// Tạo MoveBase instance (như bình thường) -robot::move_base_core::BaseNavigation::Ptr move_base_ptr = ...; -move_base_ptr->initialize(tf); - -// Tạo Action Server -move_base::MoveBaseActionServer action_server(move_base_ptr, 20.0); // 20 Hz -action_server.start(); -``` - -### 2. Thiết lập Callbacks - -```cpp -// Callback khi nhận goal mới -action_server.setGoalCallback([](const move_base::MoveBaseActionGoal& goal) { - std::cout << "New goal: (" << goal.target_pose.pose.position.x - << ", " << goal.target_pose.pose.position.y << ")" << std::endl; -}); - -// Callback cho feedback (gọi định kỳ) -action_server.setFeedbackCallback([](const move_base::MoveBaseActionFeedback& feedback) { - std::cout << "State: " << robot::move_base_core::toString( - feedback.nav_feedback.navigation_state) << std::endl; - std::cout << "Distance to goal: " << feedback.distance_to_goal << " m" << std::endl; -}); - -// Callback khi goal hoàn thành -action_server.setResultCallback([](const move_base::MoveBaseActionResult& result) { - if (result.success) { - std::cout << "Goal succeeded!" << std::endl; - } else { - std::cout << "Goal failed: " << result.message << std::endl; - } -}); - -// Callback khi goal bị preempt -action_server.setPreemptCallback([]() { - std::cout << "Goal was preempted" << std::endl; -}); -``` - -### 3. Gửi Goal - -```cpp -move_base::MoveBaseActionGoal goal; -goal.target_pose.header.frame_id = "map"; -goal.target_pose.header.stamp = robot::Time::now(); -goal.target_pose.pose.position.x = 5.0; -goal.target_pose.pose.position.y = 3.0; -goal.target_pose.pose.position.z = 0.0; - -// Set orientation -tf3::Quaternion q; -q.setRPY(0, 0, 0.5); // yaw = 0.5 radians -goal.target_pose.pose.orientation = tf3::toMsg(q); - -goal.xy_goal_tolerance = 0.2; -goal.yaw_goal_tolerance = 0.1; -goal.goal_id = "goal_1"; - -if (action_server.sendGoal(goal)) { - std::cout << "Goal accepted!" << std::endl; -} else { - std::cout << "Goal rejected!" << std::endl; -} -``` - -### 4. Hủy Goal - -```cpp -if (action_server.cancelGoal()) { - std::cout << "Goal cancelled" << std::endl; -} -``` - -### 5. Kiểm tra trạng thái - -```cpp -// Kiểm tra có goal mới không (cho preemption) -if (action_server.isNewGoalAvailable()) { - move_base::MoveBaseActionGoal new_goal = action_server.acceptNewGoal(); - // Xử lý goal mới -} - -// Kiểm tra có yêu cầu preempt không -if (action_server.isPreemptRequested()) { - // Dừng và cleanup -} -``` - -## Ví dụ đầy đủ - -Xem file `examples/action_server_example.cpp` để có ví dụ hoàn chỉnh. - -## Thread Safety - -- `MoveBaseActionServer` sử dụng mutex để đảm bảo thread-safe -- Control loop chạy trong thread riêng -- Tất cả các method đều thread-safe - -## Lưu ý - -1. **Control Loop**: Action Server tự động chạy control loop ở tần số đã chỉ định (mặc định 20 Hz) -2. **Goal Management**: Chỉ có thể có 1 goal active tại một thời điểm. Goal mới sẽ preempt goal cũ. -3. **Feedback**: Feedback được gửi tự động qua callback, không cần gọi `publishFeedback()` như ROS -4. **Result**: Result được set tự động dựa trên state của MoveBase, hoặc có thể set thủ công - -## Migration từ ROS Action Server - -Nếu bạn đang dùng ROS Action Server, migration rất đơn giản: - -**Trước (ROS):** -```cpp -MoveBaseActionServer *as_; -as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", - [this](auto &goal) { executeCb(goal); }, false); -as_->start(); -``` - -**Sau (MoveBaseActionServer):** -```cpp -move_base::MoveBaseActionServer action_server(move_base_ptr, 20.0); -action_server.setGoalCallback([this](const auto& goal) { - executeCb(goal); -}); -action_server.start(); -``` - -## API Reference - -Xem file `include/move_base/move_base_action_server.h` để có đầy đủ API documentation. - diff --git a/src/Navigations/Packages/move_base/examples/IMPLEMENTATION_GUIDE.md b/src/Navigations/Packages/move_base/examples/IMPLEMENTATION_GUIDE.md new file mode 100644 index 0000000..905110b --- /dev/null +++ b/src/Navigations/Packages/move_base/examples/IMPLEMENTATION_GUIDE.md @@ -0,0 +1,265 @@ +# Hướng dẫn triển khai SimpleActionServer trong MoveBase + +## Tổng quan + +Tài liệu này hướng dẫn cách tích hợp `SimpleActionServer` vào class `MoveBase` hiện có. + +## Bước 1: Thêm SimpleActionServer vào MoveBase class + +### Trong `move_base.h`: + +```cpp +#include + +namespace move_base +{ + class MoveBase : public robot::move_base_core::BaseNavigation + { + private: + // Thêm member variable + std::shared_ptr> as_; + + // Thêm method declaration + void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal); + }; +} +``` + +## Bước 2: Khởi tạo trong constructor hoặc initialize() + +### Trong `move_base.cpp` constructor hoặc `initialize()`: + +```cpp +void move_base::MoveBase::initialize(robot::TFListenerPtr tf) +{ + // ... existing initialization code ... + + // Khởi tạo action server + as_ = std::make_shared>( + "move_base", + std::bind(&MoveBase::executeCb, this, std::placeholders::_1), + true // auto_start + ); + + // Setup callbacks (optional) + setupActionServerCallbacks(); + + robot::log_info("[MoveBase] Action server initialized"); +} +``` + +## Bước 3: Setup callbacks (nếu cần publish messages) + +```cpp +void move_base::MoveBase::setupActionServerCallbacks() +{ + if (!as_) + return; + + // Set callback để publish result + // Customize để gửi qua hệ thống communication của bạn + as_->setResultCallback([](const auto& result) { + // TODO: Gửi result qua hệ thống communication của bạn + // your_communication_system.send(result); + robot::log_info("Result: status=%d", result->status.status); + }); + + // Set callback để publish feedback + as_->setFeedbackCallback([](const auto& feedback) { + // TODO: Gửi feedback qua hệ thống communication của bạn + // your_communication_system.send(feedback); + }); + + // Set callback để publish status + as_->setStatusCallback([](const auto& status) { + // TODO: Gửi status qua hệ thống communication của bạn + // your_communication_system.send(status); + }); +} +``` + +## Bước 4: Triển khai executeCb() + +### Trong `move_base.cpp`: + +```cpp +void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal) +{ + robot::log_info("[MoveBase] Execute callback: x=%.2f, y=%.2f", + goal->target_pose.pose.position.x, + goal->target_pose.pose.position.y); + + // Validate quaternion + if (!isQuaternionValid(goal->target_pose.pose.orientation)) + { + robot::log_error("[MoveBase] Invalid quaternion in goal"); + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), + "Invalid quaternion in goal"); + return; + } + + // Convert goal to global frame + robot_geometry_msgs::PoseStamped target_pose = goalToGlobalFrame(goal->target_pose); + + // Send goal to move_base + bool success = moveTo(target_pose, 0.2, 0.2); + + if (!success) + { + robot::log_error("[MoveBase] Failed to send goal"); + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), + "Failed to send goal to move_base"); + return; + } + + // Monitor execution + robot::Rate rate(10.0); + robot::Time last_feedback_time = robot::Time::now(); + + while (as_->isActive() && !as_->isPreemptRequested()) + { + // Get feedback + robot::move_base_core::NavFeedback* feedback = getFeedback(); + + if (feedback) + { + // Check if goal reached + if (feedback->goal_checked && + feedback->navigation_state == robot::move_base_core::State::SUCCEEDED) + { + as_->setSucceeded(robot_move_base_msgs::MoveBaseResult(), + "Goal reached"); + return; + } + + // Check for errors + if (feedback->navigation_state == robot::move_base_core::State::ABORTED) + { + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), + feedback->feed_back_str); + return; + } + + // Publish feedback periodically + robot::Time now = robot::Time::now(); + if ((now - last_feedback_time).toSec() >= 0.5) + { + robot_move_base_msgs::MoveBaseFeedback fb; + fb.base_position.x = feedback->current_pose.x; + fb.base_position.y = feedback->current_pose.y; + fb.base_position.theta = feedback->current_pose.theta; + fb.base_position.header.stamp = robot::Time::now(); + fb.base_position.header.frame_id = global_frame_; + + as_->publishFeedback(fb); + last_feedback_time = now; + } + } + + // Check for preemption + if (as_->isPreemptRequested()) + { + cancel(); + as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), + "Preempted"); + return; + } + + rate.sleep(); + } + + // Handle unexpected exit + if (as_->isPreemptRequested()) + { + cancel(); + as_->setPreempted(robot_move_base_msgs::MoveBaseResult(), "Preempted"); + } + else + { + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), + "Execution ended unexpectedly"); + } +} +``` + +## Bước 5: Xử lý goal và cancel messages từ bên ngoài + +Nhận goal và cancel messages từ hệ thống communication của bạn: + +```cpp +// Trong callback nhận goal từ hệ thống communication của bạn +void move_base::MoveBase::goalCallback( + const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg) +{ + if (as_) + { + as_->processGoal(msg); + } +} + +// Trong callback nhận cancel từ hệ thống communication của bạn +void move_base::MoveBase::cancelCallback( + const robot_actionlib_msgs::GoalIDConstPtr& msg) +{ + if (as_) + { + as_->processCancel(msg); + } +} + +// Ví dụ: Đăng ký với hệ thống communication của bạn +// your_communication_system.onGoal( +// std::bind(&MoveBase::goalCallback, this, std::placeholders::_1)); +// your_communication_system.onCancel( +// std::bind(&MoveBase::cancelCallback, this, std::placeholders::_1)); +``` + +## Bước 6: Cleanup trong destructor + +```cpp +move_base::MoveBase::~MoveBase() +{ + // ... existing cleanup ... + + // Stop action server + if (as_) + { + as_->stop(); + as_.reset(); + } +} +``` + +## Ví dụ hoàn chỉnh + +Xem các file trong thư mục `examples/`: +- `simple_usage_example.cpp` - Ví dụ đơn giản nhất +- `simple_action_server_example.cpp` - Ví dụ đầy đủ +- `move_base_with_action_server_example.cpp` - Ví dụ tích hợp vào MoveBase + +## Lưu ý + +1. **Thread Safety**: `executeCb` chạy trong thread riêng, đảm bảo thread-safe khi truy cập các member variables +2. **State Management**: Action server tự động quản lý state, không cần can thiệp thủ công +3. **Feedback Rate**: Điều chỉnh rate publish feedback phù hợp (khuyến nghị 0.5-1.0 giây) +4. **Error Handling**: Luôn kiểm tra và xử lý các trường hợp lỗi + +## Testing + +Để test action server: + +```cpp +// Tạo goal message +robot_move_base_msgs::MoveBaseActionGoalPtr goal = + boost::make_shared(); +goal->goal_id.id = "test_goal_1"; +goal->goal.target_pose.pose.position.x = 5.0; +goal->goal.target_pose.pose.position.y = 3.0; + +// Gửi goal +move_base_ptr->getActionServer()->processGoal(goal); + +// Kiểm tra status +auto status = move_base_ptr->getActionServer()->getCurrentStatus(); +``` + diff --git a/src/Navigations/Packages/move_base/examples/README_SIMPLE_ACTION_SERVER.md b/src/Navigations/Packages/move_base/examples/README_SIMPLE_ACTION_SERVER.md new file mode 100644 index 0000000..e512922 --- /dev/null +++ b/src/Navigations/Packages/move_base/examples/README_SIMPLE_ACTION_SERVER.md @@ -0,0 +1,220 @@ +# SimpleActionServer - Hướng dẫn sử dụng + +## Tổng quan + +`SimpleActionServer` là một thư viện tương tự ROS actionlib::SimpleActionServer nhưng không phụ thuộc vào ROS. Nó cung cấp interface đơn giản để triển khai action server với các tính năng: + +- Quản lý goal requests +- Hỗ trợ preemption (hủy goal) +- Publish feedback và result thông qua callbacks +- Thread-safe và thread-safe state management +- Tương thích với framework robot hiện có + +## Cấu trúc Files + +- `simple_action_server.h` - Header file với class declaration +- `simple_action_server.cpp` - Implementation file với template code +- `simple_action_server_example.cpp` - Ví dụ triển khai đầy đủ + +## Ví dụ sử dụng cơ bản + +### 1. Khởi tạo SimpleActionServer + +```cpp +#include + +// Tạo action server với execute callback +auto action_server = std::make_shared>( + "move_base", + [this](const auto& goal) { this->executeCb(goal); }, + true // auto_start +); +``` + +### 2. Setup callbacks để publish messages + +```cpp +// Set callback để publish result +action_server->setResultCallback([](const auto& result) { + // Publish result message qua ROS hoặc hệ thống khác + result_pub_.publish(*result); +}); + +// Set callback để publish feedback +action_server->setFeedbackCallback([](const auto& feedback) { + // Publish feedback message + feedback_pub_.publish(*feedback); +}); + +// Set callback để publish status +action_server->setStatusCallback([](const auto& status) { + // Publish status message + status_pub_.publish(*status); +}); +``` + +### 3. Xử lý goal messages từ bên ngoài + +```cpp +// Trong ROS subscriber callback hoặc nơi nhận goal +void goalCallback(const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg) +{ + action_server->processGoal(msg); +} + +// Trong cancel callback +void cancelCallback(const robot_actionlib_msgs::GoalIDConstPtr& msg) +{ + action_server->processCancel(msg); +} +``` + +### 4. Triển khai execute callback + +```cpp +void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal) +{ + robot::log_info("Received goal: x=%.2f, y=%.2f", + goal->target_pose.pose.position.x, + goal->target_pose.pose.position.y); + + // Gửi goal đến move_base + bool success = move_base_ptr_->moveTo(goal->target_pose, 0.2, 0.2); + + if (!success) + { + action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(), + "Failed to send goal"); + return; + } + + // Monitor execution + robot::Rate rate(10.0); + while (action_server_->isActive() && !action_server_->isPreemptRequested()) + { + // Kiểm tra trạng thái + auto* feedback = move_base_ptr_->getFeedback(); + if (feedback->goal_checked && + feedback->navigation_state == robot::move_base_core::State::SUCCEEDED) + { + action_server_->setSucceeded(robot_move_base_msgs::MoveBaseResult(), + "Goal reached"); + return; + } + + // Publish feedback + robot_move_base_msgs::MoveBaseFeedback fb; + fb.base_position.x = feedback->current_pose.x; + fb.base_position.y = feedback->current_pose.y; + action_server_->publishFeedback(fb); + + rate.sleep(); + } + + // Xử lý preemption + if (action_server_->isPreemptRequested()) + { + move_base_ptr_->cancel(); + action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(), + "Preempted"); + } +} +``` + +## Tích hợp vào MoveBase + +### Cách 1: Thêm vào MoveBase class + +```cpp +// Trong move_base.h +#include + +class MoveBase : public robot::move_base_core::BaseNavigation +{ +private: + std::shared_ptr> as_; + + void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal); +}; +``` + +```cpp +// Trong move_base.cpp constructor +MoveBase::MoveBase(robot::TFListenerPtr tf) +{ + // ... existing initialization ... + + // Create action server + as_ = std::make_shared>( + "move_base", + std::bind(&MoveBase::executeCb, this, std::placeholders::_1), + true + ); + + // Setup callbacks + as_->setResultCallback([this](const auto& result) { + // Publish result if needed + }); +} +``` + +### Cách 2: Sử dụng trong wrapper class + +Xem file `simple_action_server_example.cpp` để xem ví dụ đầy đủ. + +## API Reference + +### Constructor +```cpp +SimpleActionServer(const std::string& name, + ExecuteCallback execute_callback, + bool auto_start = true) +``` + +### Main Methods +- `void processGoal(const ActionGoalConstPtr& goal_msg)` - Xử lý goal message +- `void processCancel(const GoalIDConstPtr& cancel_msg)` - Xử lý cancel message +- `GoalConstPtr acceptNewGoal()` - Chấp nhận goal mới +- `bool isNewGoalAvailable()` - Kiểm tra có goal mới +- `bool isPreemptRequested()` - Kiểm tra preemption +- `void setSucceeded(const Result&, const std::string&)` - Đặt thành công +- `void setAborted(const Result&, const std::string&)` - Đặt lỗi +- `void setPreempted(const Result&, const std::string&)` - Đặt preempted +- `void publishFeedback(const Feedback&)` - Publish feedback + +### Callback Setup +- `void setGoalCallback(GoalCallback)` - Callback khi nhận goal +- `void setCancelCallback(CancelCallback)` - Callback khi nhận cancel +- `void setFeedbackCallback(FeedbackCallback)` - Callback để publish feedback +- `void setResultCallback(ResultCallback)` - Callback để publish result +- `void setStatusCallback(StatusCallback)` - Callback để publish status + +## Lưu ý + +1. **Thread Safety**: Tất cả các phương thức đều thread-safe +2. **Callbacks**: Các callbacks được gọi trong context của thread tương ứng +3. **State Management**: Action server tự động quản lý state machine +4. **No ROS Dependency**: Không cần ROS, chỉ cần setup callbacks để publish messages + +## Tích hợp với hệ thống communication khác + +SimpleActionServer không phụ thuộc vào ROS. Bạn có thể tích hợp với bất kỳ hệ thống communication nào bằng cách setup callbacks: + +```cpp +// Setup callbacks để publish messages qua hệ thống của bạn +action_server->setResultCallback([](const auto& result) { + // Gửi result qua hệ thống communication của bạn + // your_communication_system.send(result); +}); + +action_server->setFeedbackCallback([](const auto& feedback) { + // Gửi feedback qua hệ thống communication của bạn + // your_communication_system.send(feedback); +}); + +// Nhận goal messages từ hệ thống của bạn +// your_communication_system.onGoal([&action_server](const auto& msg) { +// action_server->processGoal(msg); +// }); +``` + diff --git a/src/Navigations/Packages/move_base/examples/action_server_example.cpp b/src/Navigations/Packages/move_base/examples/action_server_example.cpp deleted file mode 100644 index 22c3705..0000000 --- a/src/Navigations/Packages/move_base/examples/action_server_example.cpp +++ /dev/null @@ -1,192 +0,0 @@ -/********************************************************************* - * - * Example: Using MoveBaseActionServer - * - * This example demonstrates how to use MoveBaseActionServer - * which provides a ROS Action Server-like interface - *********************************************************************/ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace move_base; - -void goalCallback(const MoveBaseActionGoal& goal) -{ - robot::log_info("[Example] New goal received:"); - robot::log_info(" Position: (%.2f, %.2f)", - goal.target_pose.pose.position.x, - goal.target_pose.pose.position.y); - robot::log_info(" Frame: %s", goal.target_pose.header.frame_id.c_str()); - robot::log_info(" XY Tolerance: %.2f", goal.xy_goal_tolerance); - robot::log_info(" Yaw Tolerance: %.2f", goal.yaw_goal_tolerance); -} - -void feedbackCallback(const MoveBaseActionFeedback& feedback) -{ - const auto& nav_fb = feedback.nav_feedback; - robot::log_info("[Example] Feedback:"); - robot::log_info(" State: %s", - robot::move_base_core::toString(nav_fb.navigation_state).c_str()); - robot::log_info(" Current Pose: (%.2f, %.2f, %.2f)", - nav_fb.current_pose.x, - nav_fb.current_pose.y, - nav_fb.current_pose.theta); - robot::log_info(" Distance to Goal: %.2f m", feedback.distance_to_goal); - if (!nav_fb.feed_back_str.empty()) - { - robot::log_info(" Message: %s", nav_fb.feed_back_str.c_str()); - } -} - -void resultCallback(const MoveBaseActionResult& result) -{ - robot::log_info("[Example] Result received:"); - robot::log_info(" Success: %s", result.success ? "Yes" : "No"); - robot::log_info(" Final State: %s", - robot::move_base_core::toString(result.final_state).c_str()); - robot::log_info(" Message: %s", result.message.c_str()); -} - -void preemptCallback() -{ - robot::log_warning("[Example] Goal was preempted!"); -} - -int main(int argc, char** argv) -{ - try - { - robot::log_info("[Example] Starting MoveBaseActionServer example..."); - - // 1. Create TF buffer - std::shared_ptr tf = std::make_shared(); - - // Set up static transforms (example) - // In real usage, these would come from your localization system - // tf->setTransform(...); - - // 2. Load and initialize MoveBase - robot::log_info("[Example] Loading MoveBase plugin..."); - robot::PluginLoaderHelper loader; - std::string path_file_so = loader.findLibraryPath("MoveBase"); - auto create_plugin = boost::dll::import_alias( - path_file_so, - "MoveBase", - boost::dll::load_mode::append_decorations); - - robot::move_base_core::BaseNavigation::Ptr move_base_ptr = create_plugin(); - if (!move_base_ptr) - { - robot::log_error("[Example] Failed to create MoveBase instance"); - return 1; - } - - robot::log_info("[Example] Initializing MoveBase..."); - move_base_ptr->initialize(tf); - - // Wait for initialization - robot::Rate init_rate(10); - while (!move_base_ptr->getFeedback()->is_ready) - { - init_rate.sleep(); - } - robot::log_info("[Example] MoveBase is ready!"); - - // 3. Create Action Server - robot::log_info("[Example] Creating MoveBaseActionServer..."); - MoveBaseActionServer action_server(move_base_ptr, 20.0); // 20 Hz control loop - - // 4. Set up callbacks - action_server.setGoalCallback(goalCallback); - action_server.setFeedbackCallback(feedbackCallback); - action_server.setResultCallback(resultCallback); - action_server.setPreemptCallback(preemptCallback); - - // 5. Start the action server - action_server.start(); - robot::log_info("[Example] Action server started"); - - // 6. Example: Send a goal - std::this_thread::sleep_for(std::chrono::seconds(1)); - - MoveBaseActionGoal goal; - goal.target_pose.header.frame_id = "map"; - goal.target_pose.header.stamp = robot::Time::now(); - goal.target_pose.pose.position.x = 5.0; - goal.target_pose.pose.position.y = 3.0; - goal.target_pose.pose.position.z = 0.0; - - // Set orientation (yaw = 0.5 radians) - tf3::Quaternion q; - q.setRPY(0, 0, 0.5); - goal.target_pose.pose.orientation = tf3::toMsg(q); - - goal.xy_goal_tolerance = 0.2; - goal.yaw_goal_tolerance = 0.1; - goal.goal_id = "example_goal_1"; - - robot::log_info("[Example] Sending goal..."); - if (action_server.sendGoal(goal)) - { - robot::log_info("[Example] Goal accepted!"); - } - else - { - robot::log_error("[Example] Goal rejected!"); - return 1; - } - - // 7. Monitor progress (in real application, this would be in your main loop) - robot::log_info("[Example] Monitoring navigation progress..."); - robot::Rate monitor_rate(1.0); // Check every second - - int timeout_seconds = 60; - int elapsed_seconds = 0; - - while (action_server.isRunning() && elapsed_seconds < timeout_seconds) - { - // Check if goal is still active - if (!action_server.isNewGoalAvailable() && !action_server.isPreemptRequested()) - { - // Goal is being processed - robot::log_info("[Example] Navigation in progress... (elapsed: %d s)", elapsed_seconds); - } - - monitor_rate.sleep(); - elapsed_seconds++; - } - - // 8. Example: Cancel goal (optional) - // Uncomment to test cancellation: - // robot::log_info("[Example] Cancelling goal..."); - // action_server.cancelGoal(); - - // 9. Cleanup - robot::log_info("[Example] Stopping action server..."); - action_server.stop(); - - robot::log_info("[Example] Example completed successfully"); - return 0; - } - catch (const std::exception& e) - { - robot::log_error("[Example] Exception: %s", e.what()); - return 1; - } - catch (...) - { - robot::log_error("[Example] Unknown exception occurred"); - return 1; - } -} - diff --git a/src/Navigations/Packages/move_base/examples/simple_action_server_example.cpp b/src/Navigations/Packages/move_base/examples/simple_action_server_example.cpp new file mode 100644 index 0000000..f780d18 --- /dev/null +++ b/src/Navigations/Packages/move_base/examples/simple_action_server_example.cpp @@ -0,0 +1,343 @@ +/** + * @file simple_action_server_example.cpp + * @brief Example implementation of SimpleActionServer for MoveBaseAction + * + * This example demonstrates how to use SimpleActionServer to implement + * an action server without ROS dependency. It shows: + * - How to initialize the action server + * - How to set up callbacks for publishing messages + * - How to handle goal execution + * - How to publish feedback and results + */ + +#include +#include + +// Message types +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace move_base +{ + +/** + * @class MoveBaseActionServerExample + * @brief Example class demonstrating SimpleActionServer usage + */ +class MoveBaseActionServerExample +{ +public: + MoveBaseActionServerExample() + : move_base_ptr_(nullptr) + { + // Initialize move_base (your existing navigation system) + // move_base_ptr_ = std::make_shared(tf_); + + // Create SimpleActionServer with execute callback + action_server_ = std::make_shared>( + "move_base", + std::bind(&MoveBaseActionServerExample::executeCb, this, std::placeholders::_1), + true // auto_start + ); + + // Set up callbacks for publishing messages + setupCallbacks(); + + robot::log_info("[MoveBaseActionServerExample] Initialized"); + } + + ~MoveBaseActionServerExample() + { + if (action_server_) + { + action_server_->stop(); + } + } + + /** + * @brief Setup callbacks for SimpleActionServer + * You can customize these callbacks to publish messages to your communication system + */ + void setupCallbacks() + { + if (!action_server_) + return; + + // Set callback for result publishing + // Customize this to send result to your communication system + action_server_->setResultCallback( + [this](const robot_move_base_msgs::MoveBaseActionResultConstPtr& result) + { + // TODO: Publish result to your communication system (e.g., custom message bus, network, etc.) + robot::log_info("[MoveBaseActionServerExample] Result: status=%d, text=%s", + result->status.status, result->status.text.c_str()); + } + ); + + // Set callback for feedback publishing + // Customize this to send feedback to your communication system + action_server_->setFeedbackCallback( + [this](const robot_move_base_msgs::MoveBaseActionFeedbackConstPtr& feedback) + { + // TODO: Publish feedback to your communication system + robot::log_debug("[MoveBaseActionServerExample] Published feedback"); + } + ); + + // Set callback for status publishing + // Customize this to send status to your communication system + action_server_->setStatusCallback( + [this](const robot_actionlib_msgs::GoalStatusArrayConstPtr& status) + { + // TODO: Publish status to your communication system + } + ); + + // Set callback for goal messages (optional - for logging) + action_server_->setGoalCallback( + [this](const robot_move_base_msgs::MoveBaseActionGoalConstPtr& goal) + { + robot::log_info("[MoveBaseActionServerExample] Goal callback: x=%.2f, y=%.2f", + goal->goal.target_pose.pose.position.x, + goal->goal.target_pose.pose.position.y); + } + ); + + // Set callback for cancel messages (optional - for logging) + action_server_->setCancelCallback( + [this](const robot_actionlib_msgs::GoalIDConstPtr& cancel) + { + robot::log_info("[MoveBaseActionServerExample] Cancel callback: id=%s", cancel->id.c_str()); + } + ); + + // Set status publish rate (default is 10 Hz) + action_server_->setStatusPublishRate(10.0); + + robot::log_info("[MoveBaseActionServerExample] Callbacks setup complete"); + } + + /** + * @brief Process goal message from external source + * Call this when you receive a goal message from your communication system + */ + void processGoalMessage(const robot_move_base_msgs::MoveBaseActionGoalConstPtr& msg) + { + if (action_server_) + { + action_server_->processGoal(msg); + } + } + + /** + * @brief Process cancel message from external source + * Call this when you receive a cancel message from your communication system + */ + void processCancelMessage(const robot_actionlib_msgs::GoalIDConstPtr& msg) + { + if (action_server_) + { + action_server_->processCancel(msg); + } + } + + /** + * @brief Execute callback - called when a new goal is received + * This is the main function that processes the goal + */ + void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr& goal) + { + robot::log_info("[MoveBaseActionServerExample] Execute callback started"); + robot::log_info(" Goal: x=%.2f, y=%.2f, frame_id=%s", + goal->target_pose.pose.position.x, + goal->target_pose.pose.position.y, + goal->target_pose.header.frame_id.c_str()); + + // Check if we have a valid move_base instance + if (!move_base_ptr_) + { + robot::log_error("[MoveBaseActionServerExample] move_base_ptr_ is null, aborting goal"); + action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(), + "MoveBase not initialized"); + return; + } + + // Convert goal to PoseStamped + robot_geometry_msgs::PoseStamped target_pose = goal->target_pose; + + // Send goal to move_base + bool success = move_base_ptr_->moveTo(target_pose, 0.2, 0.2); + + if (!success) + { + robot::log_error("[MoveBaseActionServerExample] Failed to send goal to move_base"); + action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(), + "Failed to send goal to move_base"); + return; + } + + // Monitor execution and publish feedback + robot::Rate rate(10.0); // 10 Hz + robot::Time start_time = robot::Time::now(); + robot::Time last_feedback_time = start_time; + + while (action_server_->isActive() && !action_server_->isPreemptRequested()) + { + // Check if goal is reached + if (move_base_ptr_->getFeedback()) + { + auto* feedback_ptr = move_base_ptr_->getFeedback(); + + if (feedback_ptr->goal_checked && + feedback_ptr->navigation_state == robot::move_base_core::State::SUCCEEDED) + { + robot::log_info("[MoveBaseActionServerExample] Goal reached!"); + action_server_->setSucceeded(robot_move_base_msgs::MoveBaseResult(), + "Goal reached successfully"); + return; + } + + // Check for errors + if (feedback_ptr->navigation_state == robot::move_base_core::State::ABORTED) + { + robot::log_error("[MoveBaseActionServerExample] Navigation aborted: %s", + feedback_ptr->feed_back_str.c_str()); + action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(), + feedback_ptr->feed_back_str); + return; + } + + // Publish feedback periodically (every 0.5 seconds) + robot::Time now = robot::Time::now(); + if ((now - last_feedback_time).toSec() >= 0.5) + { + robot_move_base_msgs::MoveBaseFeedback feedback; + feedback.base_position.x = feedback_ptr->current_pose.x; + feedback.base_position.y = feedback_ptr->current_pose.y; + feedback.base_position.theta = feedback_ptr->current_pose.theta; + feedback.base_position.header.stamp = robot::Time::now(); + feedback.base_position.header.frame_id = "map"; + + action_server_->publishFeedback(feedback); + last_feedback_time = now; + } + } + + // Check for preemption + if (action_server_->isPreemptRequested()) + { + robot::log_info("[MoveBaseActionServerExample] Goal preempted"); + move_base_ptr_->cancel(); + action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(), + "Goal preempted by user"); + return; + } + + // Sleep + rate.sleep(); + } + + // If we exit the loop without success, check why + if (action_server_->isPreemptRequested()) + { + robot::log_info("[MoveBaseActionServerExample] Goal was preempted"); + move_base_ptr_->cancel(); + action_server_->setPreempted(robot_move_base_msgs::MoveBaseResult(), + "Goal preempted"); + } + else + { + robot::log_warning("[MoveBaseActionServerExample] Goal execution ended unexpectedly"); + action_server_->setAborted(robot_move_base_msgs::MoveBaseResult(), + "Goal execution ended unexpectedly"); + } + } + + /** + * @brief Get the action server instance + */ + std::shared_ptr> getActionServer() + { + return action_server_; + } + + /** + * @brief Set move_base instance + */ + void setMoveBase(robot::move_base_core::BaseNavigation::Ptr move_base) + { + move_base_ptr_ = move_base; + } + +private: + // Action server + std::shared_ptr> action_server_; + + // Move base instance + robot::move_base_core::BaseNavigation::Ptr move_base_ptr_; +}; + +} // namespace move_base + +/** + * @brief Example main function showing how to use MoveBaseActionServerExample + */ +int main() +{ + try + { + // Create example instance + move_base::MoveBaseActionServerExample example; + + // If you have a move_base instance, set it + // example.setMoveBase(move_base_ptr); + + robot::log_info("[Example] Action server is running. Waiting for goals..."); + + // Example: Create and send a goal message + // In real usage, you would receive this from your communication system + robot_move_base_msgs::MoveBaseActionGoalPtr goal_msg = + boost::make_shared(); + goal_msg->header.stamp = robot::Time::now(); + goal_msg->header.frame_id = "map"; + goal_msg->goal_id.stamp = robot::Time::now(); + goal_msg->goal_id.id = "example_goal_1"; + goal_msg->goal.target_pose.header.stamp = robot::Time::now(); + goal_msg->goal.target_pose.header.frame_id = "map"; + goal_msg->goal.target_pose.pose.position.x = 5.0; + goal_msg->goal.target_pose.pose.position.y = 3.0; + goal_msg->goal.target_pose.pose.position.z = 0.0; + goal_msg->goal.target_pose.pose.orientation.w = 1.0; + + // Process the goal + example.processGoalMessage(goal_msg); + + // Your event loop here + // while (your_system_running()) + // { + // // Receive messages from your communication system + // // Process goals and cancels + // // example.processGoalMessage(goal_msg); + // // example.processCancelMessage(cancel_msg); + // } + + // Keep running for a while + robot::Time::sleep(robot::Duration(10.0)); + } + catch (const std::exception& e) + { + robot::log_error("[Example] Exception: %s", e.what()); + return 1; + } + + return 0; +} + 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 f736a1a..a48e455 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 @@ -7,11 +7,11 @@ #ifndef NAV_MOVE_BASE_ACTION_H_ #define NAV_MOVE_BASE_ACTION_H_ +#include #include #include #include -#include - +#include // interfaces headers #include @@ -28,13 +28,15 @@ #include #include #include +#include +#include +#include -#include -#include -#include namespace move_base { + // typedefs to help us out with the action server so that we don't hace to type so much + typedef move_base::SimpleActionServer MoveBaseActionServer; enum MoveBaseState { @@ -472,7 +474,12 @@ namespace move_base void planThread(); - // void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal); + void executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal); + + /** + * @brief Setup callbacks for the action server + */ + void setupActionServerCallbacks(); bool isQuaternionValid(const robot_geometry_msgs::Quaternion &q); @@ -492,9 +499,11 @@ namespace move_base robot::TFListenerPtr tf_; robot::NodeHandle private_nh_; + MoveBaseActionServer *as_; + boost::function planner_loader_; boost::function controller_loader_; - boost::function recovery_loader_; + std::map> recovery_loaders_; robot_nav_core::BaseLocalPlanner::Ptr tc_; robot_nav_core::BaseGlobalPlanner::Ptr planner_; diff --git a/src/Navigations/Packages/move_base/include/move_base/simple_action_server.h b/src/Navigations/Packages/move_base/include/move_base/simple_action_server.h new file mode 100644 index 0000000..db49f25 --- /dev/null +++ b/src/Navigations/Packages/move_base/include/move_base/simple_action_server.h @@ -0,0 +1,259 @@ +#ifndef MOVE_BASE_SIMPLE_ACTION_SERVER_H_ +#define MOVE_BASE_SIMPLE_ACTION_SERVER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +// Action message types +#include +#include +#include +#include +#include +#include +#include + +// Actionlib message types +#include +#include +#include + +// Robot framework +#include + +namespace move_base +{ + +/** + * @class SimpleActionServer + * @brief A simplified action server implementation similar to ROS actionlib::SimpleActionServer + * + * This class provides a simple interface for implementing action servers without ROS dependency. + * It handles goal requests, preemption, feedback publishing, and result publishing using callback functions. + * + * @tparam ActionSpec The action message type (e.g., robot_move_base_msgs::MoveBaseAction) + */ +template +class SimpleActionServer +{ +public: + // Typedefs for action message types + typedef typename ActionSpec::_action_goal_type ActionGoal; + typedef typename ActionSpec::_action_result_type ActionResult; + typedef typename ActionSpec::_action_feedback_type ActionFeedback; + + typedef typename ActionGoal::_goal_type Goal; + typedef typename ActionResult::_result_type Result; + typedef typename ActionFeedback::_feedback_type Feedback; + + typedef boost::shared_ptr GoalConstPtr; + typedef boost::shared_ptr ResultConstPtr; + typedef boost::shared_ptr FeedbackConstPtr; + typedef typename ActionGoal::ConstPtr ActionGoalConstPtr; + typedef typename ActionResult::ConstPtr ActionResultConstPtr; + typedef typename ActionFeedback::ConstPtr ActionFeedbackConstPtr; + typedef robot_actionlib_msgs::GoalStatusArrayConstPtr GoalStatusArrayConstPtr; + + // Callback function types + typedef std::function GoalCallback; + typedef std::function CancelCallback; + typedef std::function FeedbackCallback; + typedef std::function ResultCallback; + typedef std::function StatusCallback; + typedef std::function ExecuteCallback; + + // Typedefs for non-const pointers + typedef typename ActionResult::Ptr ActionResultPtr; + typedef typename ActionFeedback::Ptr ActionFeedbackPtr; + + /** + * @brief Constructor + * @param name Action name (e.g., "move_base") + * @param execute_callback Callback function to execute when a goal is received + * @param auto_start If true, automatically start the server + */ + SimpleActionServer(const std::string& name, + ExecuteCallback execute_callback, + bool auto_start = true); + + /** + * @brief Destructor + */ + ~SimpleActionServer(); + + /** + * @brief Start the action server + */ + void start(); + + /** + * @brief Stop the action server + */ + void stop(); + + /** + * @brief Set callback for goal messages + * @param callback Function to call when a goal is received + */ + void setGoalCallback(GoalCallback callback); + + /** + * @brief Set callback for cancel messages + * @param callback Function to call when a cancel is received + */ + void setCancelCallback(CancelCallback callback); + + /** + * @brief Set callback for feedback publishing + * @param callback Function to call when feedback needs to be published + */ + void setFeedbackCallback(FeedbackCallback callback); + + /** + * @brief Set callback for result publishing + * @param callback Function to call when result needs to be published + */ + void setResultCallback(ResultCallback callback); + + /** + * @brief Set callback for status publishing + * @param callback Function to call when status needs to be published + */ + void setStatusCallback(StatusCallback callback); + + /** + * @brief Set status publish rate + * @param rate Publishing rate in Hz + */ + void setStatusPublishRate(double rate); + + /** + * @brief Process an incoming goal message + * @param goal_msg The goal message to process + */ + void processGoal(const ActionGoalConstPtr& goal_msg); + + /** + * @brief Process an incoming cancel message + * @param cancel_msg The cancel message to process + */ + void processCancel(const robot_actionlib_msgs::GoalIDConstPtr& cancel_msg); + + /** + * @brief Check if a new goal is available + * @return True if a new goal is available + */ + bool isNewGoalAvailable(); + + /** + * @brief Check if preemption has been requested + * @return True if preemption has been requested + */ + bool isPreemptRequested(); + + /** + * @brief Accept a new goal + * @return The new goal, or nullptr if no new goal is available + */ + GoalConstPtr acceptNewGoal(); + + /** + * @brief Set the action as succeeded + * @param result The result to send + * @param text Success message + */ + void setSucceeded(const Result& result = Result(), const std::string& text = ""); + + /** + * @brief Set the action as aborted + * @param result The result to send + * @param text Error message + */ + void setAborted(const Result& result = Result(), const std::string& text = ""); + + /** + * @brief Set the action as preempted + * @param result The result to send + * @param text Message + */ + void setPreempted(const Result& result = Result(), const std::string& text = ""); + + /** + * @brief Publish feedback + * @param feedback The feedback to publish + */ + void publishFeedback(const Feedback& feedback); + + /** + * @brief Get the current goal + * @return The current goal, or nullptr if no goal is active + */ + GoalConstPtr getCurrentGoal(); + + /** + * @brief Get the current status + * @return The current goal status + */ + robot_actionlib_msgs::GoalStatus getCurrentStatus(); + + /** + * @brief Check if the server is active + * @return True if the server is active + */ + bool isActive(); + + /** + * @brief Get the action name + * @return The action name + */ + std::string getName() const; + +private: + /** + * @brief Execute the goal in a separate thread + */ + void executeGoal(); + + /** + * @brief Thread function to publish status periodically + */ + void statusPublishThread(); + + // Action name + std::string name_; + + // Execute callback + ExecuteCallback execute_callback_; + + // Callbacks for external communication + GoalCallback goal_callback_; + CancelCallback cancel_callback_; + FeedbackCallback feedback_callback_; + ResultCallback result_callback_; + StatusCallback status_callback_; + + // State + boost::mutex state_mutex_; + boost::condition_variable goal_condition_; + bool active_; + bool preempt_requested_; + bool new_goal_available_; + GoalConstPtr goal_; + robot_actionlib_msgs::GoalID current_goal_id_; + robot_actionlib_msgs::GoalStatus current_status_; + double status_publish_rate_; + + // Threads + boost::thread status_thread_; + boost::thread execute_thread_; +}; + +} // namespace move_base +#endif // MOVE_BASE_SIMPLE_ACTION_SERVER_H_ + diff --git a/src/Navigations/Packages/move_base/package.xml b/src/Navigations/Packages/move_base/package.xml index 5b73942..3a8f0bc 100644 --- a/src/Navigations/Packages/move_base/package.xml +++ b/src/Navigations/Packages/move_base/package.xml @@ -48,5 +48,8 @@ robot_cpp robot_cpp + + robot_move_base_msgs + robot_move_base_msgs \ No newline at end of file diff --git a/src/Navigations/Packages/move_base/src/convert_data.cpp b/src/Navigations/Packages/move_base/src/convert_data.cpp index fc6c936..c63f778 100644 --- a/src/Navigations/Packages/move_base/src/convert_data.cpp +++ b/src/Navigations/Packages/move_base/src/convert_data.cpp @@ -1,6 +1,5 @@ #include -#include -#include +#include #include char* move_base::ConvertData::cost_translation_table_ = NULL; diff --git a/src/Navigations/Packages/move_base/src/move_base.cpp b/src/Navigations/Packages/move_base/src/move_base.cpp index 164239d..5fe0cac 100644 --- a/src/Navigations/Packages/move_base/src/move_base.cpp +++ b/src/Navigations/Packages/move_base/src/move_base.cpp @@ -10,13 +10,15 @@ #include #include #include +#include +#include #include #include #include #include #include #include -#include +#include #include #include @@ -49,6 +51,13 @@ move_base::MoveBase::MoveBase(robot::TFListenerPtr tf) move_base::MoveBase::~MoveBase() { + if (as_ != NULL) + { + as_->stop(); + delete as_; + as_ = NULL; + } + recovery_behaviors_.clear(); // Cleanup static maps (shared_ptr tự động cleanup) @@ -113,6 +122,10 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) if (!initialized_) { tf_ = tf; + + as_ = new MoveBaseActionServer("move_base", std::bind(&MoveBase::executeCb, this, std::placeholders::_1), true); + setupActionServerCallbacks(); + // NodeHandle("~") will automatically load YAML files from config directory private_nh_ = robot::NodeHandle("~"); recovery_trigger_ = PLANNING_R; @@ -309,6 +322,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) robot::log_error("[%s:%d]\n ERROR: nav_feedback_ is nullptr", __FILE__, __LINE__); } initialized_ = true; + setup_ = true; robot::log_info("========== End: initialize() - SUCCESS =========="); } else @@ -319,6 +333,7 @@ void move_base::MoveBase::initialize(robot::TFListenerPtr tf) void move_base::MoveBase::swapPlanner(std::string base_global_planner) { + } void move_base::MoveBase::setRobotFootprint(const std::vector &fprt) @@ -349,7 +364,6 @@ void move_base::MoveBase::addStaticMap(const std::string &map_name, robot_nav_ms { it->second = map; } - robot::log_error("[%s:%d] Add StaticMap: %s %x", __FILE__, __LINE__, map_name.c_str(), controller_costmap_robot_ == nullptr); updateGlobalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); updateLocalCostmap(map, robot_costmap_2d::LayerType::STATIC_LAYER, map_name); } @@ -614,6 +628,132 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d return false; } + // Set XY goal tolerance + double final_xy_tolerance; + if (fabs(xy_goal_tolerance) > 0.001) + { + final_xy_tolerance = fabs(xy_goal_tolerance); + this->setXyGoalTolerance(final_xy_tolerance); + } + else + { + final_xy_tolerance = fabs(original_xy_goal_tolerance_); + this->setXyGoalTolerance(final_xy_tolerance); + } + + // Set Yaw goal tolerance + double final_yaw_tolerance; + if (fabs(yaw_goal_tolerance) > 0.001) + { + final_yaw_tolerance = fabs(yaw_goal_tolerance); + this->setYawGoalTolerance(final_yaw_tolerance); + } + else + { + final_yaw_tolerance = fabs(original_yaw_goal_tolerance_); + this->setYawGoalTolerance(final_yaw_tolerance); + } + + // Check pointers + if (!tc_) + { + throw std::runtime_error("Null 'tc_' pointer encountered"); + } + + if (!controller_costmap_robot_) + { + throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); + } + + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); + + // Swap planner + try + { + tc_->swapPlanner(position_planner_name_); + } + catch (const std::exception &e) + { + std::cerr << e.what() << "\n"; + throw std::exception(e); + } + + // Update navigation feedback + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; + nav_feedback_->feed_back_str = std::string("Planning"); + nav_feedback_->goal_checked = false; + } + else + { + robot::log_error("[MoveBase::moveTo] nav_feedback_ pointer is null!"); + lock.unlock(); + return false; + } + + // Reset cancel flag + if (cancel_ctr_) + { + cancel_ctr_ = false; + } + // Check if action server exists + if (!as_) + { + robot::log_error("[MoveBase::moveTo] as_ pointer is null!"); + lock.unlock(); + return false; + } + + try + { + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + robot::log_info("[MoveBase::moveTo] Goal stamp: %ld.%09ld", + action_goal->goal_id.stamp.sec, action_goal->goal_id.stamp.nsec); + robot::log_info("[MoveBase::moveTo] Processing goal through action server..."); + as_->processGoal(action_goal); + robot::log_info("[MoveBase::moveTo] Goal processed successfully by action server"); + } + catch (const std::exception &e) + { + lock.unlock(); + robot::log_error("[MoveBase::moveTo] Exception during processGoal: %s", e.what()); + return false; + } + lock.unlock(); + return true; +} + +bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, + const robot_geometry_msgs::PoseStamped &goal, + double xy_goal_tolerance, double yaw_goal_tolerance) +{ + if (setup_) + { + swapPlanner(default_config_.base_global_planner); + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::REJECTED; + nav_feedback_->feed_back_str = std::string("The system has not been installed yet"); + nav_feedback_->goal_checked = false; + } + return false; + } + if (fabs(xy_goal_tolerance) > 0.001) this->setXyGoalTolerance(fabs(xy_goal_tolerance)); else @@ -623,7 +763,9 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d this->setYawGoalTolerance(fabs(yaw_goal_tolerance)); else this->setYawGoalTolerance(fabs(original_yaw_goal_tolerance_)); + robot::Duration(0.01).sleep(); + robot::log_info("[MoveBase] In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server."); if (!tc_) { throw std::runtime_error("Null 'tc_' pointer encountered"); @@ -640,7 +782,7 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d } catch (const std::exception &e) { - std::cerr << e.what() << "\n"; + robot::log_error("[MoveBase] Exception in swapPlanner: %s", e.what()); throw std::exception(e); } @@ -653,17 +795,25 @@ bool move_base::MoveBase::moveTo(const robot_geometry_msgs::PoseStamped &goal, d if (cancel_ctr_) cancel_ctr_ = false; + + robot_move_base_msgs::MoveBaseActionGoalPtr action_goal = boost::make_shared(); + action_goal->header.stamp = robot::Time::now(); + action_goal->goal.target_pose = goal; + + // Generate unique goal ID using timestamp + robot::Time now = robot::Time::now(); + action_goal->goal_id.stamp = now; + std::ostringstream goal_id_stream; + goal_id_stream << "move_base_goal_" << now.sec << "_" << now.nsec; + action_goal->goal_id.id = goal_id_stream.str(); + + robot::log_info("[MoveBase::moveTo] Generated goal ID: %s", action_goal->goal_id.id.c_str()); + as_->processGoal(action_goal); + lock.unlock(); return true; } -bool move_base::MoveBase::moveTo(const robot_protocol_msgs::Order &msg, - const robot_geometry_msgs::PoseStamped &goal, - double xy_goal_tolerance, double yaw_goal_tolerance) -{ - return false; -} - bool move_base::MoveBase::dockTo(const std::string &maker, const robot_geometry_msgs::PoseStamped &goal, double xy_goal_tolerance, double yaw_goal_tolerance) { @@ -915,12 +1065,43 @@ void move_base::MoveBase::resume() void move_base::MoveBase::cancel() { + robot::log_info("[MoveBase::cancel] ===== ENTRY ====="); + if (!controller_costmap_robot_) { + robot::log_error("[MoveBase::cancel] controller_costmap_robot_ is null!"); throw std::runtime_error("Null 'controller_costmap_robot_' pointer encountered"); } + boost::unique_lock lock(*(controller_costmap_robot_->getCostmap()->getMutex())); cancel_ctr_ = true; + robot::log_info("[MoveBase::cancel] cancel_ctr_ set to true"); + + if (as_) + { + // Get current goal ID from action server to cancel specific goal + // If we want to cancel all goals, use empty string "" + robot_actionlib_msgs::GoalIDPtr msg = boost::make_shared(); + + // Use empty string to cancel current goal (processCancel accepts empty string to cancel all) + msg->id = ""; // Empty string cancels current goal + msg->stamp = robot::Time::now(); + + robot::log_info("[MoveBase::cancel] Sending cancel request to action server"); + robot::log_info("[MoveBase::cancel] Cancel message: id='%s', stamp=%ld.%09ld", + msg->id.c_str(), msg->stamp.sec, msg->stamp.nsec); + + // Convert to ConstPtr for processCancel + robot_actionlib_msgs::GoalIDConstPtr cancel_msg = msg; + as_->processCancel(cancel_msg); + robot::log_info("[MoveBase::cancel] Cancel request processed by action server"); + } + else + { + robot::log_warning("[MoveBase::cancel] Action server (as_) is null - cannot send cancel request"); + } + + robot::log_info("[MoveBase::cancel] ===== EXIT ====="); } bool move_base::MoveBase::getRobotPose(robot_geometry_msgs::PoseStamped &pose) @@ -1102,13 +1283,14 @@ bool move_base::MoveBase::loadRecoveryBehaviors(const robot::NodeHandle &node) // behavior["type"] should be the factory function name (e.g., "create_plugin") std::string behavior_type = behavior["type"].as(); std::string behavior_name = behavior["name"].as(); - std::string path_file_so; + robot::PluginLoaderHelper loader; + std::string path_file_so = loader.findLibraryPath(behavior_type); // Load the factory function from the shared library - auto recovery_loader = boost::dll::import_alias( + recovery_loaders_[behavior_name] = 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_loaders_[behavior_name]()); // shouldn't be possible, but it won't hurt to check if (behavior_ptr == nullptr) @@ -1420,7 +1602,234 @@ void move_base::MoveBase::planThread() } } -// void executeCb(const move_base_msgs::MoveBaseGoalConstPtr &move_base_goal); + + +void move_base::MoveBase::executeCb(const robot_move_base_msgs::MoveBaseGoalConstPtr &move_base_goal) +{ + if (!isQuaternionValid(move_base_goal->target_pose.pose.orientation)) + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED; + nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion"); + } + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); + return; + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE; + nav_feedback_->feed_back_str = std::string("The goal is received"); + robot::log_info("Received goal: x=%.2f, y=%.2f", move_base_goal->target_pose.pose.position.x, move_base_goal->target_pose.pose.position.y); + } + } + + robot_geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose); + publishZeroVelocity(); + // we have a goal so start the planner + boost::unique_lock lock(planner_mutex_); + planner_goal_ = goal; + runPlanner_ = true; + cancel_ctr_ = false; + planner_cond_.notify_one(); + lock.unlock(); + robot::Rate r(controller_frequency_); + if (shutdown_costmaps_) + { + robot::log_debug("[MoveBase] Starting up costmaps that were shut down previously"); + planner_costmap_robot_->start(); + controller_costmap_robot_->start(); + } + // we want to make sure that we reset the last time we had a valid plan and control + last_valid_control_ = robot::Time::now(); + last_valid_plan_ = robot::Time::now(); + last_oscillation_reset_ = robot::Time::now(); + planning_retries_ = 0; + + while(robot::ok()) + { + if (c_freq_change_) + { + robot::log_info("[MoveBase] Setting controller frequency to %.2f", controller_frequency_); + r = robot::Rate(controller_frequency_); + c_freq_change_ = false; + } + if (as_->isPreemptRequested()) + { + if (as_->isNewGoalAvailable()) + { + // if we're active and a new goal is available, we'll accept it, but we won't shut anything down + robot_move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal(); + robot::log_info("[MoveBase] new goal %f %f", new_goal.target_pose.pose.position.x, new_goal.target_pose.pose.position.y); + if (!isQuaternionValid(new_goal.target_pose.pose.orientation)) + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::ABORTED; + nav_feedback_->feed_back_str = std::string("Aborting on goal because it was sent with an invalid quaternion"); + } + as_->setAborted(robot_move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); + return; + } + else + { + if (nav_feedback_) + { + nav_feedback_->navigation_state = robot::move_base_core::State::ACTIVE; + nav_feedback_->feed_back_str = std::string("The new goal is received"); + } + } + + goal = goalToGlobalFrame(new_goal.target_pose); + // we'll make sure that we reset our state for the next execution cycle + recovery_index_ = 0; + state_ = move_base::PLANNING; + if (nav_feedback_) + nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; + // we have a new goal so make sure the planner is awake + lock.lock(); + planner_goal_ = goal; + runPlanner_ = true; + cancel_ctr_ = false; + planner_cond_.notify_one(); + lock.unlock(); + + // publish the goal point to the visualizer + robot::log_debug("[MoveBase] move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y); + // make sure to reset our timeouts and counters + last_valid_control_ = robot::Time::now(); + last_valid_plan_ = robot::Time::now(); + last_oscillation_reset_ = robot::Time::now(); + planning_retries_ = 0; + } + else + { + // if we've been preempted explicitly we need to shut things down + resetState(); + + // notify the ActionServer that we've successfully preempted + robot::log_debug("[MoveBase] Move base preempting the current goal"); + as_->setPreempted(); + if (nav_feedback_) + { + nav_feedback_->feed_back_str = std::string("Move base preempting the current goal."); + nav_feedback_->navigation_state = robot::move_base_core::State::PREEMPTING; + } + // we'll actually return from execute after preempting + return; + } + } + + // we also want to check if we've changed global frames because we need to transform our goal pose + if (goal.header.frame_id != planner_costmap_robot_->getGlobalFrameID()) + { + goal = goalToGlobalFrame(goal); + + // we want to go back to the planning state for the next execution cycle + recovery_index_ = 0; + state_ = move_base::PLANNING; + if (nav_feedback_) + nav_feedback_->navigation_state = robot::move_base_core::State::PLANNING; + + // we have a new goal so make sure the planner is awake + lock.lock(); + planner_goal_ = goal; + runPlanner_ = true; + cancel_ctr_ = false; + planner_cond_.notify_one(); + lock.unlock(); + + // publish the goal point to the visualizer + robot::log_debug("[MoveBase] The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y); + + // make sure to reset our timeouts and counters + last_valid_control_ = robot::Time::now(); + last_valid_plan_ = robot::Time::now(); + last_oscillation_reset_ = robot::Time::now(); + planning_retries_ = 0; + } + + // // for timing that gives real time even in simulation + // ros::WallTime start = ros::WallTime::now(); + + // the real work on pursuing a goal is done here + bool done = executeCycle(goal); + + // if we're done, then we'll return from execute + if (done) + return; + + // check if execution of the goal has completed in some way + // ros::WallDuration t_diff = ros::WallTime::now() - start; + // ROS_DEBUG_NAMED("move_base", "Full control cycle time: %.9f\n", t_diff.toSec()); + + r.sleep(); + } +} + +void move_base::MoveBase::setupActionServerCallbacks() +{ + if (!as_) + { + robot::log_error("[MoveBase] Action server is not initialized"); + return; + } + + // Set callback for result publishing + // Customize this to send result to your communication system + as_->setResultCallback( + [this](const robot_move_base_msgs::MoveBaseActionResultConstPtr& result) + { + // TODO: Publish result to your communication system (e.g., custom message bus, network, etc.) + robot::log_info("[MoveBase] Result: status=%d, text=%s", + result->status.status, result->status.text.c_str()); + } + ); + + // Set callback for feedback publishing + // Customize this to send feedback to your communication system + as_->setFeedbackCallback( + [this](const robot_move_base_msgs::MoveBaseActionFeedbackConstPtr& feedback) + { + // TODO: Publish feedback to your communication system + robot::log_debug("[MoveBase] Published feedback"); + } + ); + + // Set callback for status publishing + // Customize this to send status to your communication system + as_->setStatusCallback( + [this](const robot_actionlib_msgs::GoalStatusArrayConstPtr& status) + { + // TODO: Publish status to your communication system (e.g., custom message bus, network, etc.) + } + ); + + // Set callback for goal messages (optional - for logging) + as_->setGoalCallback( + [this](const robot_move_base_msgs::MoveBaseActionGoalConstPtr& goal) + { + robot::log_info("[MoveBase] Goal callback: x=%.2f, y=%.2f", + goal->goal.target_pose.pose.position.x, + goal->goal.target_pose.pose.position.y); + } + ); + + // Set callback for cancel messages (optional - for logging) + as_->setCancelCallback( + [this](const robot_actionlib_msgs::GoalIDConstPtr& cancel) + { + robot::log_info("[MoveBase] Cancel callback: id=%s", cancel->id.c_str()); + } + ); + + // Set status publish rate (default is 10 Hz) + as_->setStatusPublishRate(10.0); + + robot::log_info("[MoveBase] Callbacks setup complete"); +} bool move_base::MoveBase::isQuaternionValid(const robot_geometry_msgs::Quaternion &q) { @@ -1837,11 +2246,11 @@ bool move_base::MoveBase::executeCycle(robot_geometry_msgs::PoseStamped &goal) // we'll try to clear out space with any user-provided recovery behaviors case move_base::CLEARING: - robot::log_debug("In clearing/recovery state"); + robot::log_debug("[%s:%d] In clearing/recovery state", __FILE__, __LINE__); // we'll invoke whatever recovery behavior we're currently on if they're enabled if (recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()) { - robot::log_debug("Executing behavior %u of %zu", recovery_index_ + 1, recovery_behaviors_.size()); + robot::log_debug("[%s:%d] Executing behavior %u of %zu", __FILE__, __LINE__, recovery_index_ + 1, recovery_behaviors_.size()); // move_base_msgs::RecoveryStatus msg; // msg.pose_stamped = current_position; diff --git a/src/Navigations/Packages/move_base/src/move_base_main.cpp b/src/Navigations/Packages/move_base/src/move_base_main.cpp index c6b4818..e276312 100644 --- a/src/Navigations/Packages/move_base/src/move_base_main.cpp +++ b/src/Navigations/Packages/move_base/src/move_base_main.cpp @@ -29,10 +29,9 @@ #include #include -#include +#include #include #include -#include #include #include diff --git a/src/Navigations/Packages/move_base/src/simple_action_server.cpp b/src/Navigations/Packages/move_base/src/simple_action_server.cpp new file mode 100644 index 0000000..e5f47b6 --- /dev/null +++ b/src/Navigations/Packages/move_base/src/simple_action_server.cpp @@ -0,0 +1,632 @@ +#include + +namespace move_base +{ + +// Constructor +template +SimpleActionServer::SimpleActionServer(const std::string& name, + ExecuteCallback execute_callback, + bool auto_start) + : name_(name), + execute_callback_(execute_callback), + active_(false), + preempt_requested_(false), + new_goal_available_(false), + goal_(nullptr), + status_publish_rate_(10.0) +{ + // Initialize status + current_status_.status = robot_actionlib_msgs::GoalStatus::PENDING; + current_status_.goal_id.stamp = robot::Time::now(); + current_status_.goal_id.id = ""; + current_status_.text = ""; + + if (auto_start) + { + start(); + } + + robot::log_info("[SimpleActionServer] Initialized action server: %s", name_.c_str()); +} + +// Destructor +template +SimpleActionServer::~SimpleActionServer() +{ + stop(); +} + +// Start the action server +template +void SimpleActionServer::start() +{ + boost::lock_guard lock(state_mutex_); + if (!active_) + { + active_ = true; + // Start status publishing thread + status_thread_ = boost::thread(boost::bind(&SimpleActionServer::statusPublishThread, this)); + robot::log_info("[SimpleActionServer] Started action server: %s", name_.c_str()); + } +} + +// Stop the action server +template +void SimpleActionServer::stop() +{ + { + boost::lock_guard lock(state_mutex_); + if (active_) + { + active_ = false; + } + } + + // Notify all waiting threads + goal_condition_.notify_all(); + + if (status_thread_.joinable()) + { + status_thread_.join(); + } + + if (execute_thread_.joinable()) + { + execute_thread_.join(); + } + + robot::log_info("[SimpleActionServer] Stopped action server: %s", name_.c_str()); +} + +// Set callback for goal messages +template +void SimpleActionServer::setGoalCallback(GoalCallback callback) +{ + boost::lock_guard lock(state_mutex_); + goal_callback_ = callback; +} + +// Set callback for cancel messages +template +void SimpleActionServer::setCancelCallback(CancelCallback callback) +{ + boost::lock_guard lock(state_mutex_); + cancel_callback_ = callback; +} + +// Set callback for feedback publishing +template +void SimpleActionServer::setFeedbackCallback(FeedbackCallback callback) +{ + boost::lock_guard lock(state_mutex_); + feedback_callback_ = callback; +} + +// Set callback for result publishing +template +void SimpleActionServer::setResultCallback(ResultCallback callback) +{ + boost::lock_guard lock(state_mutex_); + result_callback_ = callback; +} + +// Set callback for status publishing +template +void SimpleActionServer::setStatusCallback(StatusCallback callback) +{ + boost::lock_guard lock(state_mutex_); + status_callback_ = callback; +} + +// Set status publish rate +template +void SimpleActionServer::setStatusPublishRate(double rate) +{ + boost::lock_guard lock(state_mutex_); + status_publish_rate_ = rate; +} + +// Process an incoming goal message +template +void SimpleActionServer::processGoal(const ActionGoalConstPtr& goal_msg) +{ + boost::lock_guard lock(state_mutex_); + + if (!active_) + { + robot::log_warning("[SimpleActionServer] Received goal but server is not active"); + return; + } + + // Check if we should accept this goal + bool accept_goal = false; + + if (current_status_.status == robot_actionlib_msgs::GoalStatus::PENDING || + current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE || + current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING) + { + // Check if this is a new goal (different ID) + if (goal_msg->goal_id.id != current_goal_id_.id || + goal_msg->goal_id.stamp != current_goal_id_.stamp) + { + // If we have an active goal, preempt it + if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE) + { + preempt_requested_ = true; + current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTING; + robot::log_info("[SimpleActionServer] Preempting current goal for new goal"); + } + + accept_goal = true; + } + } + else + { + // No active goal, accept this one + accept_goal = true; + } + + if (accept_goal) + { + // Store the new goal + goal_ = boost::make_shared(goal_msg->goal); + current_goal_id_ = goal_msg->goal_id; + new_goal_available_ = true; + current_status_.status = robot_actionlib_msgs::GoalStatus::PENDING; + current_status_.goal_id = current_goal_id_; + current_status_.text = ""; + + robot::log_info("[SimpleActionServer] Received new goal, ID: %s", current_goal_id_.id.c_str()); + + // Notify waiting threads + goal_condition_.notify_one(); + + // Start execute thread if not already running + if (execute_callback_) + { + if (!execute_thread_.joinable()) + { + execute_thread_ = boost::thread(boost::bind(&SimpleActionServer::executeGoal, this)); + } + } + + // Call goal callback if set + if (goal_callback_) + { + goal_callback_(goal_msg); + } + } +} + +// Process an incoming cancel message +template +void SimpleActionServer::processCancel(const robot_actionlib_msgs::GoalIDConstPtr& cancel_msg) +{ + robot::log_info("[SimpleActionServer::processCancel] ===== ENTRY ====="); + robot::log_info("[SimpleActionServer::processCancel] Cancel message ID: '%s'", cancel_msg->id.c_str()); + robot::log_info("[SimpleActionServer::processCancel] Cancel message stamp: %ld.%09ld", + cancel_msg->stamp.sec, cancel_msg->stamp.nsec); + + boost::lock_guard lock(state_mutex_); + + robot::log_info("[SimpleActionServer::processCancel] Current goal ID: '%s'", current_goal_id_.id.c_str()); + robot::log_info("[SimpleActionServer::processCancel] Current goal stamp: %ld.%09ld", + current_goal_id_.stamp.sec, current_goal_id_.stamp.nsec); + robot::log_info("[SimpleActionServer::processCancel] Current status: %d", current_status_.status); + + // Check if this cancel request is for the current goal + bool should_cancel = false; + bool is_empty_id = (cancel_msg->id == ""); + bool is_matching_id = (cancel_msg->id == current_goal_id_.id); + bool is_matching_stamp = (cancel_msg->stamp == current_goal_id_.stamp); + + robot::log_debug("[SimpleActionServer::processCancel] Cancel check: empty_id=%s, matching_id=%s, matching_stamp=%s", + is_empty_id ? "true" : "false", + is_matching_id ? "true" : "false", + is_matching_stamp ? "true" : "false"); + + if (is_empty_id || (is_matching_id && is_matching_stamp)) + { + should_cancel = true; + robot::log_info("[SimpleActionServer::processCancel] Cancel request matches current goal - will cancel"); + + if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE) + { + robot::log_info("[SimpleActionServer::processCancel] Goal is ACTIVE - setting preemption"); + preempt_requested_ = true; + current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTING; + robot::log_info("[SimpleActionServer::processCancel] Cancel requested for goal ID: %s", current_goal_id_.id.c_str()); + robot::log_info("[SimpleActionServer::processCancel] Status changed to PREEMPTING"); + } + else if (current_status_.status == robot_actionlib_msgs::GoalStatus::PENDING) + { + robot::log_info("[SimpleActionServer::processCancel] Goal is PENDING - rejecting it"); + // Goal hasn't started yet, just reject it + current_status_.status = robot_actionlib_msgs::GoalStatus::REJECTED; + current_status_.text = "Goal cancelled before execution"; + goal_.reset(); + new_goal_available_ = false; + robot::log_info("[SimpleActionServer::processCancel] Goal cancelled before execution"); + robot::log_info("[SimpleActionServer::processCancel] Status changed to REJECTED"); + } + else + { + robot::log_info("[SimpleActionServer::processCancel] Goal status is %d - no action needed", current_status_.status); + } + } + else + { + robot::log_info("[SimpleActionServer::processCancel] Cancel request does NOT match current goal - ignoring"); + robot::log_info("[SimpleActionServer::processCancel] Cancel ID '%s' != Current ID '%s'", + cancel_msg->id.c_str(), current_goal_id_.id.c_str()); + } + + // Call cancel callback if set + robot::log_debug("[SimpleActionServer::processCancel] Checking cancel_callback_: %s", + cancel_callback_ ? "set" : "null"); + if (cancel_callback_) + { + robot::log_debug("[SimpleActionServer::processCancel] Calling cancel_callback_..."); + try + { + cancel_callback_(cancel_msg); + robot::log_debug("[SimpleActionServer::processCancel] cancel_callback_ executed successfully"); + } + catch (const std::exception& e) + { + robot::log_error("[SimpleActionServer::processCancel] EXCEPTION in cancel_callback_: %s", e.what()); + } + } + + robot::log_info("[SimpleActionServer::processCancel] ===== EXIT ====="); +} + +// Check if a new goal is available +template +bool SimpleActionServer::isNewGoalAvailable() +{ + boost::lock_guard lock(state_mutex_); + return new_goal_available_; +} + +// Check if preemption has been requested +template +bool SimpleActionServer::isPreemptRequested() +{ + boost::lock_guard lock(state_mutex_); + return preempt_requested_; +} + +// Accept a new goal +template +typename SimpleActionServer::GoalConstPtr SimpleActionServer::acceptNewGoal() +{ + robot::log_info("[SimpleActionServer::acceptNewGoal] ===== ENTRY ====="); + boost::lock_guard lock(state_mutex_); + + robot::log_debug("[SimpleActionServer::acceptNewGoal] new_goal_available_: %s", + new_goal_available_ ? "true" : "false"); + robot::log_debug("[SimpleActionServer::acceptNewGoal] goal_ is %s", + goal_ ? "valid" : "null"); + + if (!new_goal_available_) + { + robot::log_warning("[SimpleActionServer::acceptNewGoal] No new goal available - returning null"); + robot::log_info("[SimpleActionServer::acceptNewGoal] ===== EXIT (null) ====="); + return GoalConstPtr(); + } + + // Accept the new goal + GoalConstPtr new_goal = goal_; + goal_.reset(); + new_goal_available_ = false; + preempt_requested_ = false; + + // Update status + current_status_.status = robot_actionlib_msgs::GoalStatus::ACTIVE; + current_status_.goal_id = current_goal_id_; + current_status_.text = ""; + + robot::log_info("[SimpleActionServer::acceptNewGoal] Goal accepted successfully"); + robot::log_info("[SimpleActionServer::acceptNewGoal] Goal ID: %s", current_goal_id_.id.c_str()); + robot::log_info("[SimpleActionServer::acceptNewGoal] Status changed to ACTIVE"); + if (new_goal) + { + robot::log_info("[SimpleActionServer::acceptNewGoal] Goal position: x=%.3f, y=%.3f", + new_goal->target_pose.pose.position.x, + new_goal->target_pose.pose.position.y); + } + robot::log_info("[SimpleActionServer::acceptNewGoal] ===== EXIT (success) ====="); + return new_goal; +} + +// Set the action as succeeded +template +void SimpleActionServer::setSucceeded(const Result& result, const std::string& text) +{ + boost::lock_guard lock(state_mutex_); + + if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE || + current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING) + { + current_status_.status = robot_actionlib_msgs::GoalStatus::SUCCEEDED; + current_status_.text = text; + + // Create and publish result + ActionResultPtr action_result = boost::make_shared(); + action_result->header.stamp = robot::Time::now(); + action_result->status = current_status_; + action_result->result = result; + + // Call result callback if set + if (result_callback_) + { + result_callback_(action_result); + } + + // Clear goal + goal_.reset(); + new_goal_available_ = false; + preempt_requested_ = false; + + robot::log_info("[SimpleActionServer] Goal succeeded: %s", text.c_str()); + } +} + +// Set the action as aborted +template +void SimpleActionServer::setAborted(const Result& result, const std::string& text) +{ + boost::lock_guard lock(state_mutex_); + + if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE || + current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING) + { + current_status_.status = robot_actionlib_msgs::GoalStatus::ABORTED; + current_status_.text = text; + + // Create and publish result + ActionResultPtr action_result = boost::make_shared(); + action_result->header.stamp = robot::Time::now(); + action_result->status = current_status_; + action_result->result = result; + + // Call result callback if set + if (result_callback_) + { + result_callback_(action_result); + } + + // Clear goal + goal_.reset(); + new_goal_available_ = false; + preempt_requested_ = false; + + robot::log_warning("[SimpleActionServer] Goal aborted: %s", text.c_str()); + } +} + +// Set the action as preempted +template +void SimpleActionServer::setPreempted(const Result& result, const std::string& text) +{ + boost::lock_guard lock(state_mutex_); + + if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE || + current_status_.status == robot_actionlib_msgs::GoalStatus::PREEMPTING) + { + current_status_.status = robot_actionlib_msgs::GoalStatus::PREEMPTED; + current_status_.text = text; + + // Create and publish result + ActionResultPtr action_result = boost::make_shared(); + action_result->header.stamp = robot::Time::now(); + action_result->status = current_status_; + action_result->result = result; + + // Call result callback if set + if (result_callback_) + { + result_callback_(action_result); + } + + // Clear goal + goal_.reset(); + new_goal_available_ = false; + preempt_requested_ = false; + + robot::log_info("[SimpleActionServer] Goal preempted: %s", text.c_str()); + } +} + +// Publish feedback +template +void SimpleActionServer::publishFeedback(const Feedback& feedback) +{ + boost::lock_guard lock(state_mutex_); + + if (current_status_.status == robot_actionlib_msgs::GoalStatus::ACTIVE) + { + ActionFeedbackPtr action_feedback = boost::make_shared(); + action_feedback->header.stamp = robot::Time::now(); + action_feedback->status = current_status_; + action_feedback->feedback = feedback; + + // Call feedback callback if set + if (feedback_callback_) + { + feedback_callback_(action_feedback); + } + } +} + +// Get the current goal +template +typename SimpleActionServer::GoalConstPtr SimpleActionServer::getCurrentGoal() +{ + boost::lock_guard lock(state_mutex_); + return goal_; +} + +// Get the current status +template +robot_actionlib_msgs::GoalStatus SimpleActionServer::getCurrentStatus() +{ + boost::lock_guard lock(state_mutex_); + return current_status_; +} + +// Check if the server is active +template +bool SimpleActionServer::isActive() +{ + boost::lock_guard lock(state_mutex_); + return active_; +} + +// Get the action name +template +std::string SimpleActionServer::getName() const +{ + return name_; +} + +// Execute the goal in a separate thread +template +void SimpleActionServer::executeGoal() +{ + robot::log_info("[SimpleActionServer::executeGoal] ===== THREAD STARTED ====="); + robot::log_info("[SimpleActionServer::executeGoal] Action server name: %s", name_.c_str()); + robot::log_debug("[SimpleActionServer::executeGoal] execute_callback_ is %s", + execute_callback_ ? "set" : "null"); + + // Continuously process goals + while (active_) + { + robot::log_debug("[SimpleActionServer::executeGoal] Waiting for new goal..."); + GoalConstPtr goal_to_execute; + + // Wait for a new goal to be available + { + boost::unique_lock lock(state_mutex_); + robot::log_debug("[SimpleActionServer::executeGoal] Mutex lock acquired in executeGoal"); + robot::log_debug("[SimpleActionServer::executeGoal] active_=%s, new_goal_available_=%s", + active_ ? "true" : "false", + new_goal_available_ ? "true" : "false"); + + // Wait until a new goal is available or server is stopped + while (active_ && !new_goal_available_) + { + robot::log_debug("[SimpleActionServer::executeGoal] Waiting on condition variable..."); + goal_condition_.wait(lock); + robot::log_debug("[SimpleActionServer::executeGoal] Woke up from condition wait"); + robot::log_debug("[SimpleActionServer::executeGoal] After wake: active_=%s, new_goal_available_=%s", + active_ ? "true" : "false", + new_goal_available_ ? "true" : "false"); + } + + if (!active_) + { + robot::log_info("[SimpleActionServer::executeGoal] Server is not active - exiting thread"); + break; + } + + // Accept the new goal + if (new_goal_available_) + { + robot::log_info("[SimpleActionServer::executeGoal] New goal available - accepting it..."); + + // Release lock before calling acceptNewGoal() to avoid deadlock + // (acceptNewGoal() will acquire its own lock) + lock.unlock(); + + goal_to_execute = acceptNewGoal(); + + robot::log_info("[SimpleActionServer::executeGoal] Goal accepted: %s", + goal_to_execute ? "valid" : "null"); + } + else + { + robot::log_warning("[SimpleActionServer::executeGoal] Woke up but new_goal_available_ is false!"); + lock.unlock(); + } + } + + // Execute the goal callback + if (goal_to_execute) + { + robot::log_info("[SimpleActionServer::executeGoal] Executing goal callback..."); + robot::log_info("[SimpleActionServer::executeGoal] Goal position: x=%.3f, y=%.3f", + goal_to_execute->target_pose.pose.position.x, + goal_to_execute->target_pose.pose.position.y); + + if (execute_callback_) + { + try + { + robot::log_info("[SimpleActionServer::executeGoal] Calling execute_callback_..."); + execute_callback_(goal_to_execute); + robot::log_info("[SimpleActionServer::executeGoal] execute_callback_ completed"); + } + catch (const std::exception& e) + { + robot::log_error("[SimpleActionServer::executeGoal] EXCEPTION in execute_callback_: %s", e.what()); + } + } + else + { + robot::log_error("[SimpleActionServer::executeGoal] execute_callback_ is null - cannot execute goal!"); + } + } + else + { + robot::log_warning("[SimpleActionServer::executeGoal] goal_to_execute is null - skipping execution"); + } + } + + robot::log_info("[SimpleActionServer::executeGoal] ===== THREAD EXITING ====="); +} + +// Thread function to publish status periodically +template +void SimpleActionServer::statusPublishThread() +{ + double rate = status_publish_rate_; + robot::Duration sleep_duration(1.0 / rate); + + while (active_) + { + { + boost::lock_guard lock(state_mutex_); + + // Create status array + robot_actionlib_msgs::GoalStatusArrayPtr status_array = + boost::make_shared(); + status_array->header.stamp = robot::Time::now(); + status_array->status_list.clear(); + + if (current_status_.status != robot_actionlib_msgs::GoalStatus::PENDING || + !current_goal_id_.id.empty()) + { + status_array->status_list.push_back(current_status_); + } + + // Call status callback if set + if (status_callback_) + { + status_callback_(status_array); + } + } + + // Sleep + robot::Time::sleepUntil(robot::Time::now() + sleep_duration); + } +} + +} // namespace move_base + +// Explicit template instantiation for MoveBaseAction +// This is required because template method definitions are in .cpp file +template class move_base::SimpleActionServer; +