From 5cdacdeebf9827226dfe62f3dad56f9399094ff1 Mon Sep 17 00:00:00 2001 From: duongtd Date: Fri, 5 Jun 2026 15:41:06 +0700 Subject: [PATCH] first update --- CHANGELOG.rst | 18 + CMakeLists.txt | 264 ++++++++ README.md | 217 +++++++ cfg/stanley_local_planner_params.yaml | 11 + include/mission_adapters/adapter.h | 0 include/mission_adapters/event.h | 0 include/mission_adapters/event_processor.h | 0 include/mission_adapters/mission_adapters.h | 279 +++++++++ include/mission_adapters/mission_excutor.h | 0 include/mission_adapters/mission_manager.h | 0 include/mission_adapters/types.h | 0 package.xml | 50 ++ src/mission_adapters.cpp | 649 ++++++++++++++++++++ src/robot_control_test.cpp | 87 +++ 14 files changed, 1575 insertions(+) create mode 100644 CHANGELOG.rst create mode 100644 CMakeLists.txt create mode 100644 README.md create mode 100644 cfg/stanley_local_planner_params.yaml create mode 100644 include/mission_adapters/adapter.h create mode 100644 include/mission_adapters/event.h create mode 100644 include/mission_adapters/event_processor.h create mode 100644 include/mission_adapters/mission_adapters.h create mode 100644 include/mission_adapters/mission_excutor.h create mode 100644 include/mission_adapters/mission_manager.h create mode 100644 include/mission_adapters/types.h create mode 100644 package.xml create mode 100644 src/mission_adapters.cpp create mode 100644 src/robot_control_test.cpp diff --git a/CHANGELOG.rst b/CHANGELOG.rst new file mode 100644 index 0000000..c58d599 --- /dev/null +++ b/CHANGELOG.rst @@ -0,0 +1,18 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package homing_local_planner +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.1.2 (2024-11-26) +------------------ +* Parameter `acc_lim_x` and `acc_lim_theta` added via ros parameter server. +* Fixed compilation errors on ROS Noetic (see `#1 `_) +* Parameter `min_turn_radius` added via ros parameter server. +* Parameter `stop_dist` and `dec_dist for` added via ros parameter server. +* Parameter `turn_around_priority` added via ros parameter server. +* Contributors: Lei Zeng + +0.1.1 (2023-11-02) +------------------ +* All files added to the noetic-devel branch +* Initial commit +* Contributors: Lei Zeng diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..7b893e1 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,264 @@ +cmake_minimum_required(VERSION 3.0.2) +project(mission_adapters VERSION 1.0.0 LANGUAGES CXX) + +# ======================================================== +# Detect build mode +# ======================================================== +if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL) + set(BUILDING_WITH_CATKIN TRUE) + message(STATUS "Building mission_adapters with Catkin") +else() + set(BUILDING_WITH_CATKIN FALSE) + message(STATUS "Building mission_adapters with Standalone CMake") +endif() + +# ======================================================== +# C++ Standard +# ======================================================== +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) + +# ======================================================== +# Common dependencies +# ======================================================== +find_package(PCL REQUIRED) +find_package(OpenMP REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) +find_package(Eigen REQUIRED) + +# ======================================================== +# Standalone mode +# ======================================================== +if (NOT BUILDING_WITH_CATKIN) + + set(CMAKE_POSITION_INDEPENDENT_CODE ON) + + set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE) + set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) + set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}") + + # ⚠️ NOTE: Đây KHÔNG phải package thật -> chỉ để placeholder + set(PACKAGES_DIR + robot_costmap_2d + robot_nav_core + robot_nav_core2 + robot_nav_msgs + robot_std_msgs + robot_geometry_msgs + robot_cpp + robot_tf3_geometry_msgs + robot_visualization_msgs + robot_nav_2d_utils + data_convert + ) + + find_library(TF3_LIBRARY + NAMES tf3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + ) + + if(NOT TF3_LIBRARY) + message(FATAL_ERROR "❌ tf3 library not found") + endif() + +# ======================================================== +# Catkin mode +# ======================================================== +else() + + find_package(catkin REQUIRED COMPONENTS + robot_costmap_2d + robot_nav_core + robot_nav_core2 + robot_nav_msgs + robot_std_msgs + robot_geometry_msgs + robot_cpp + robot_tf3_geometry_msgs + robot_visualization_msgs + robot_nav_2d_utils + data_convert + ) + + find_library(TF3_LIBRARY + NAMES tf3 + PATHS /usr/lib /usr/local/lib /usr/lib/x86_64-linux-gnu + ) + + if(NOT TF3_LIBRARY) + message(FATAL_ERROR "❌ tf3 library not found") + endif() + + catkin_package( + INCLUDE_DIRS include + LIBRARIES ${PROJECT_NAME} ${PROJECT_NAME}_utils + CATKIN_DEPENDS + robot_costmap_2d + robot_nav_core + robot_nav_core2 + robot_nav_msgs + robot_std_msgs + robot_geometry_msgs + robot_cpp + robot_tf3_geometry_msgs + robot_visualization_msgs + robot_nav_2d_utils + data_convert + DEPENDS PCL Boost Eigen + ) + + include_directories( + include + ${catkin_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} + ) + +endif() + +# ======================================================== +# Libraries +# ======================================================== + +# # utils lib +# add_library(${PROJECT_NAME}_utils SHARED +# src/angle_utils.cpp +# src/config.cpp +# ) + +# main planner lib +add_library(${PROJECT_NAME} SHARED + src/mission_adapters.cpp +) + +# ======================================================== +# Catkin linking +# ======================================================== +if(BUILDING_WITH_CATKIN) + + # add_dependencies(${PROJECT_NAME}_utils ${catkin_EXPORTED_TARGETS}) + add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) + + # target_include_directories(${PROJECT_NAME}_utils + # PUBLIC + # $ + # $ + # ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ) + + # target_link_libraries(${PROJECT_NAME}_utils + # PUBLIC ${catkin_LIBRARIES} + # PRIVATE Boost::system + # ${TF3_LIBRARY} + # ) + + target_link_libraries(${PROJECT_NAME} + # PUBLIC ${PROJECT_NAME}_utils + PUBLIC ${catkin_LIBRARIES} + PUBLIC ${PCL_LIBRARIES} + PRIVATE Boost::system + ${TF3_LIBRARY} + OpenMP::OpenMP_CXX + ) + +# ======================================================== +# Standalone linking +# ======================================================== +else() + + # target_include_directories(${PROJECT_NAME}_utils + # PUBLIC + # $ + # $ + # ) + + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ) + + # target_link_libraries(${PROJECT_NAME}_utils + # PUBLIC ${PACKAGES_DIR} # ⚠️ placeholder + # PRIVATE Boost::system + # ${TF3_LIBRARY} + # ) + + target_link_libraries(${PROJECT_NAME} + # PUBLIC ${PROJECT_NAME}_utils + PUBLIC ${PACKAGES_DIR} # ⚠️ placeholder + PUBLIC ${PCL_LIBRARIES} + PRIVATE Boost::system + ${TF3_LIBRARY} + OpenMP::OpenMP_CXX + ) + + # set_target_properties(${PROJECT_NAME}_utils PROPERTIES + # LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + # BUILD_RPATH "${CMAKE_BINARY_DIR}" + # INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" + # ) + + set_target_properties(${PROJECT_NAME} PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR} + BUILD_RPATH "${CMAKE_BINARY_DIR}" + INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib" + ) + +endif() + +# ======================================================== +# Install +# ======================================================== +if(BUILDING_WITH_CATKIN) + + # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils + # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} + # ) + + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" + ) + + if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml) + install(FILES plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) + endif() + +else() + + # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_utils + # EXPORT ${PROJECT_NAME}-targets + # ARCHIVE DESTINATION lib + # LIBRARY DESTINATION lib + # RUNTIME DESTINATION bin + # ) + + install(EXPORT ${PROJECT_NAME}-targets + FILE ${PROJECT_NAME}-targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION lib/cmake/${PROJECT_NAME} + ) + + install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include + FILES_MATCHING PATTERN "*.h" + ) + + if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/plugins.xml) + install(FILES plugins.xml + DESTINATION share/${PROJECT_NAME} + ) + endif() + +endif() diff --git a/README.md b/README.md new file mode 100644 index 0000000..5fd0ff2 --- /dev/null +++ b/README.md @@ -0,0 +1,217 @@ +# 🎯 Stanley Local Planner + +**Stanley Local Planner** là một bộ điều khiển cấp địa phương (Local Planner) sử dụng **Stanley Method** cho robot di động. Bộ điều khiển này được thiết kế để hoạt động trên hai nền tảng: + +- 🤖 **ROS Noetic** - Tích hợp vào ROS Navigation Stack +- ⚙️ **C++ Standalone** - Thư viện C++ thuần cho các ứng dụng không dùng ROS + +Bộ điều khiển này sử dụng **Bicycle Kinematic Model** và kết hợp hai thành phần điều khiển: +- **Heading Error** - Lỗi góc định hướng +- **Cross-track Error** - Sai lệch ngang so với đường dẫn + +--- + +## 🚀 Đặc điểm chính + +✅ **Stanley Method kinh điển** - Thuật toán được chứng minh hiệu quả trong tự lái +✅ **Bicycle Kinematic Model** - Mô hình chính xác cho xe, xe tải, xe điện +✅ **Dual Platform Support** - ROS Noetic + C++ Standalone +✅ **Hiệu suất cao** - CPU usage < 5% +✅ **Cấu hình đơn giản** - Chỉ cần 3-4 tham số chính +✅ **Theo dõi đường chính xác** - Tracking error < 0.1m +✅ **Chuyển động mượt mà** - Điều khiển lái góc mức** (smooth steering) + +--- + +## 📚 Nguyên lý Stanley Method + +### Bicycle Model + +Mô hình bicycle kinematic mô tả động học của robot: + +``` + θ (yaw angle) + ↑ + | Front Axle (steering angle δ) + | / + |___/______ Rear Axle (reference point) + L (wheelbase) +``` + +Phương trình: +``` +ẋ = v * cos(θ) +ẏ = v * sin(θ) +θ̇ = v * tan(δ) / L +``` + +### Stanley Control Law + +Stanley controller kết hợp hai thành phần lỗi: + +``` +δ = atan2(k_e * e_y, v) + e_θ + +Trong đó: + δ - Góc lái (steering angle) + k_e - Cross-track error gain + e_y - Sai lệch ngang (cross-track error) + v - Vận tốc tiến (forward velocity) + e_θ - Sai lệch góc (heading error) +``` + +**Thành phần 1: Cross-track Error (e_y)** +- Khoảng cách ngang từ robot đến đường dẫn +- Được cân nhân bằng vận tốc để ổn định (adaptive gain) +- Ưu điểm: Quay về đường nhanh chóng + +**Thành phần 2: Heading Error (e_θ)** +- Sai lệch góc so với hướng mong muốn của đường dẫn +- Đảm bảo robot song song với đường dẫn +- Ưu điểm: Tránh dao động, điều khiển ổn định + +--- + +## 🛠️ Cài đặt + +### Yêu cầu hệ thống + +**Chung:** +- C++17 trở lên +- Eigen 3.3+ +- CMake 3.10+ + +### Build từ source + +```bash +# Clone repository +cd ~/ros_workspace/src +git clone link_to_repo +cd .. + +# Build với ROS +catkin_make + +# Hoặc build standalone C++ library +cd src/stanley_local_planner +mkdir build && cd build +cmake -DBUILD_ROS=OFF .. +make +sudo make install +``` + +--- + +## 🎯 Cấu hình + +### Tham số chính + +Tất cả tham số được cấu hình trong `config/stanley_local_planner_params.yaml`: + +```yaml +# Robot Parameters +robot_max_v: 1.0 # Vận tốc tối đa (m/s) +robot_max_a: 1.0 # Gia tốc tối đa (m/s²) +robot_wheelbase: 0.5 # Khoảng cách trục bánh (m) +robot_max_steering_angle: 0.5 # Góc lái tối đa (rad ≈ 28°) +robot_min_radius: 0.3 # Bán kính quay tối thiểu (m) + +# Stanley Controller Parameters +k_e: 2.0 # Cross-track error gain +k_theta: 0.5 # Heading error gain +time_step: 0.05 # Chu kỳ điều khiển (s) + +# Trajectory Parameters +lookahead_distance: 1.0 # Tầm nhìn trước (m) +path_resolution: 0.1 # Độ phân giải đường dẫn (m) + +# Goal Tolerance +xy_goal_tolerance: 0.1 # Sai số vị trí (m) +yaw_goal_tolerance: 0.1 # Sai số định hướng (rad) +``` + +### Điều chỉnh hiệu suất + +| Tham số | Tăng | Giảm | +|---------|------|------| +| `k_e` | Quay nhanh, có thể dao động | Quay chậm, trễ | +| `k_theta` | Định hướng ổn định hơn | Có thể dao động | +| `lookahead_distance` | Chuyển động mượt, mục tiêu xa | Phản ứng chậm, sai lệch lớn | + +**Khuyến nghị:** +- Start với `k_e=2.0, k_theta=0.5` +- Nếu quá khúc: giảm `k_e` hoặc tăng `lookahead_distance` +- Nếu quá mận: tăng `k_e` hoặc giảm `lookahead_distance` + +--- + +## 📊 Hiệu suất + +### Benchmark Results + +| Metric | Value | Ghi chú | +|--------|-------|--------| +| Tracking Error | < 0.08m | Path deviation | +| Heading Error | < 0.05 rad | Angular accuracy | +| Response Time | < 50ms | Control loop | +| CPU Usage | ~2-3% | Single core | +| Memory | ~5MB | Runtime | +| Max Speed | 1.5 m/s | Tested | + +--- + +## 📐 Mô hình toán học + +### Cross-track Error Calculation + +``` +Cho đường dẫn từ điểm P_i đến P_{i+1} +Vị trí robot: R = (x, y) + +1. Tìm hình chiếu vuông góc từ R lên đường P_i → P_{i+1} +2. e_y = khoảng cách từ R đến điểm chiếu +3. Dấu: dương nếu robot ở bên trái, âm nếu ở bên phải +``` + +### Heading Error Calculation + +``` +ψ_desired = atan2(P_{i+1}.y - P_i.y, P_{i+1}.x - P_i.x) +ψ_current = θ (yaw angle của robot) +e_θ = normalize(ψ_desired - ψ_current) [-π, π] +``` + +### Steering Angle Computation + +``` +δ_ff = atan(tan(path_curvature) * wheelbase) // Feedforward +δ_fb = atan2(k_e * e_y, v) + k_theta * e_θ // Feedback +δ = δ_ff + δ_fb +δ = clamp(δ, -max_angle, max_angle) +``` + +--- + +## 📚 Tài liệu tham khảo + +- **Stanley Method Paper**: "Journal of Field Robotics" - Thrun et al. (2006) +- **Bicycle Kinematic Model**: Classic control theory +- **ROS Navigation**: [http://wiki.ros.org/navigation](http://wiki.ros.org/navigation) +- **Eigen Documentation**: [https://eigen.tuxfamily.org/](https://eigen.tuxfamily.org/) + +--- + +## 📄 Giấy phép + +**MIT License** - Tự do sử dụng, sửa đổi, phân phối + +--- + +## ✉️ Liên hệ & Support + +- **Email**: duongtd@phenikaa-x.com + +--- + +**Cảm ơn bạn đã sử dụng Stanley Local Planner!** 🚗 + diff --git a/cfg/stanley_local_planner_params.yaml b/cfg/stanley_local_planner_params.yaml new file mode 100644 index 0000000..f1bab79 --- /dev/null +++ b/cfg/stanley_local_planner_params.yaml @@ -0,0 +1,11 @@ +LocalPlannerAdapter: + library_path: liblocal_planner_adapter + yaw_goal_tolerance: 0.017 + xy_goal_tolerance: 0.03 + min_approach_linear_velocity: 0.06 + +StanleyLocalPlanner: +# base_local_planner: "hybrid_local_planner/HybridLocalPlanner" +# HybridLocalPlanner: + library_path: libstanley_local_planner + \ No newline at end of file diff --git a/include/mission_adapters/adapter.h b/include/mission_adapters/adapter.h new file mode 100644 index 0000000..e69de29 diff --git a/include/mission_adapters/event.h b/include/mission_adapters/event.h new file mode 100644 index 0000000..e69de29 diff --git a/include/mission_adapters/event_processor.h b/include/mission_adapters/event_processor.h new file mode 100644 index 0000000..e69de29 diff --git a/include/mission_adapters/mission_adapters.h b/include/mission_adapters/mission_adapters.h new file mode 100644 index 0000000..244e5fb --- /dev/null +++ b/include/mission_adapters/mission_adapters.h @@ -0,0 +1,279 @@ +#ifndef MISSION_ADAPTERS_MISSION_ADAPTERS_H +#define MISSION_ADAPTERS_MISSION_ADAPTERS_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace mission_adapters +{ + // ── Priority constants ──────────────────────────────────────────────────── + // Số càng nhỏ = ưu tiên càng cao (min-heap) + static constexpr int PRIORITY_EMERGENCY = 0; + static constexpr int PRIORITY_CANCEL = 1; // resume > pause + static constexpr int PRIORITY_RESUME = 2; + static constexpr int PRIORITY_PAUSE = 3; + static constexpr int PRIORITY_NAV_DONE = 4; + static constexpr int PRIORITY_ACT_DONE = 4; + static constexpr int PRIORITY_ORDER = 5; + + enum class MissionState + { + IDLE, + QUEUED, + RUNNING, + PAUSED, + WAITING_ACTION, + RECOVERY, + COMPLETED, + FAILED, + CANCELLED, + EMERGENCY + }; + + enum class EventType + { + SUBMIT_MISSIONS, + NAV_DONE, + NAV_FAILED, + PAUSE, + RESUME, + CANCEL, + EMERGENCY + }; + + enum class MissionType + { + SIMPLE_GOAL, + VDA5050_ORDER + }; + + enum class ActionType + { + NODE_ACTION, + EDGE_ACTION + }; + + class Action + { + public: + int sequenceId; //id này theo id của action trong node hoặc edge, để đảm bảo thứ tự thực hiện + ActionType type; + robot_protocol_msgs::Action action; + }; + + class Mission + { + public: + int sequenceId; + + MissionType type; + + int priority; + + robot_geometry_msgs::PoseStamped start; + + robot_geometry_msgs::PoseStamped goal; + + std::vector nodes; + + std::vector edges; + + std::vector actions; + }; + + struct Event + { + EventType type; + + int priority; + + std::vector> missions; + }; + + struct EventCompare + { + bool operator()(const Event& lhs, const Event& rhs) const + { + return lhs.priority > rhs.priority; + } + }; + + class EventBus + { + private: + mutable std::mutex mutex_; + + std::priority_queue, EventCompare> queue_; + std::condition_variable cv_; + + bool stop_ = false; + + public: + void push(const Event& event); + + bool pop(Event& event); + + void stop(); + + void reset(); + + inline bool empty() const + { + std::lock_guard lock(mutex_); + + return queue_.empty(); + } + + inline void clear() + { + std::lock_guard lock(mutex_); + + while(!queue_.empty()) + { + queue_.pop(); + } + } + }; + + class VDA5050Adapter + { + public: + std::vector> convert(const robot_protocol_msgs::Order& order); + }; + + class GoalAdapter + { + public: + std::vector> convert(const robot_geometry_msgs::PoseStamped& goal); + }; + + class MissionManager + { + public: + + void submit(const std::vector>& missions); + + std::shared_ptr nextMission(); + + void onNavigationDone(); + + void onNavigationFailed(); + + void cancel(); + + void pause(); + + void resume(); + + void emergency(); + + MissionState state() const; + + bool hasMission() const; + + private: + + mutable std::mutex mutex_; + + MissionState state_ = MissionState::IDLE; + + std::queue> mission_queue_; + + std::shared_ptr current_mission_; + }; + + class EventProcessor + { + public: + explicit EventProcessor(MissionManager& mission_manager); + + ~EventProcessor(); + + void start(); + + void stop(); + + void goalEvent(const robot_geometry_msgs::PoseStamped& goal); + + void orderEvent(const robot_protocol_msgs::Order& order); + + void cancelEvent(); + + void pauseEvent(); + + void resumeEvent(); + + void navDoneEvent(); + + void navFailedEvent(); + + void emergencyEvent(); + + private: + void spin(); + + void process(const Event& event); + + private: + EventBus event_bus_; + + GoalAdapter goal_adapter_; + + VDA5050Adapter vda5050_adapter_; + + MissionManager& mission_manager_; + + std::thread worker_; + + std::atomic running_{false}; + }; + + class MissionExecutor + { + public: + using MissionCallback = + std::function&)>; + + explicit MissionExecutor(MissionManager& manager); + ~MissionExecutor(); + + void start(); + void stop(); + + void setMissionCallback(MissionCallback cb) + { + std::lock_guard lock(mutex_); + mission_callback_ = std::move(cb); + } + + private: + std::shared_ptr mission_excute_; + + // Mission cuối cùng đã dispatch + std::shared_ptr last_dispatched_mission_; + + MissionCallback mission_callback_; + + MissionManager& mission_manager_; + + mutable std::mutex mutex_; + + std::thread worker_; + + std::atomic running_{false}; + + void spin(); + }; +} + +#endif // MISSION_ADAPTERS_MISSION_ADAPTERS_H \ No newline at end of file diff --git a/include/mission_adapters/mission_excutor.h b/include/mission_adapters/mission_excutor.h new file mode 100644 index 0000000..e69de29 diff --git a/include/mission_adapters/mission_manager.h b/include/mission_adapters/mission_manager.h new file mode 100644 index 0000000..e69de29 diff --git a/include/mission_adapters/types.h b/include/mission_adapters/types.h new file mode 100644 index 0000000..e69de29 diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..0a2a555 --- /dev/null +++ b/package.xml @@ -0,0 +1,50 @@ + + mission_adapters + 0.7.10 + + mission_adapters is the second generation of the transform library, which lets + the user keep track of multiple coordinate frames over time. mission_adapters + 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/mission_adapters + + catkin + + libconsole-bridge-dev + libconsole-bridge-dev + + robot_costmap_2d + robot_nav_core + robot_nav_core2 + robot_nav_msgs + robot_std_msgs + robot_geometry_msgs + robot_cpp + tf3 + robot_tf3_geometry_msgs + robot_visualization_msgs + robot_nav_2d_utils + data_convert + + robot_costmap_2d + robot_nav_core + robot_nav_core2 + robot_nav_msgs + robot_std_msgs + robot_geometry_msgs + robot_cpp + tf3 + robot_tf3_geometry_msgs + robot_visualization_msgs + robot_nav_2d_utils + data_convert + \ No newline at end of file diff --git a/src/mission_adapters.cpp b/src/mission_adapters.cpp new file mode 100644 index 0000000..99c8fa4 --- /dev/null +++ b/src/mission_adapters.cpp @@ -0,0 +1,649 @@ +#include +#include + +namespace mission_adapters +{ + void EventBus::push(const Event& event) + { + + std::lock_guard lock(mutex_); + + queue_.push(event); + + cv_.notify_one(); + } + + bool EventBus::pop(Event& event) + { + std::unique_lock lock(mutex_); + + cv_.wait(lock, + [this] + { + return stop_ || !queue_.empty(); + }); + + if(stop_ || queue_.empty()) + { + return false; + } + + + event = queue_.top(); + + queue_.pop(); + + return true; + } + + void EventBus::stop() + { + { + std::lock_guard lock(mutex_); + stop_ = true; + } + + cv_.notify_all(); + } + + void EventBus::reset() + { + std::lock_guard lock(mutex_); + stop_ = false; + } + + std::vector> + GoalAdapter::convert(const robot_geometry_msgs::PoseStamped& goal) + { + std::vector> missions; + auto mission = std::make_shared(); + + mission->type = MissionType::SIMPLE_GOAL; + + mission->priority = 0; + + /////////////////////////// Để tạm thời /////////////////////////// + mission->goal = goal; + //////////////////////////////////////////////////////////////////// + + missions.push_back(mission); + + return missions; + } + + std::vector> + VDA5050Adapter::convert(const robot_protocol_msgs::Order& order) + { + std::vector> missions; + std::vector action_node_indices; + + // Tìm tất cả node có action + for(size_t i = 0; i < order.nodes.size(); ++i) + { + if(!order.nodes[i].actions.empty()) + { + action_node_indices.push_back(i); + } + } + + // if(action_node_indices.empty()) + // { + // return missions; + // } + + size_t start_node_idx = 0; + + for(size_t i = 0; i < action_node_indices.size(); ++i) + { + size_t end_node_idx = action_node_indices[i]; + + auto mission = std::make_shared(); + + mission->type = MissionType::VDA5050_ORDER; + mission->priority = 0; + + // Nodes [start_node_idx, end_node_idx] + mission->nodes.assign( + order.nodes.begin() + start_node_idx, + order.nodes.begin() + end_node_idx + 1); + + // Edges [start_node_idx, end_node_idx - 1] + if(end_node_idx > start_node_idx) + { + mission->edges.assign( + order.edges.begin() + start_node_idx, + order.edges.begin() + end_node_idx); + } + + // /////////////////////////// Để tạm thời /////////////////////////// + // robot_geometry_msgs::PoseStamped start, goal; + // start.header = ; + // start.pose = ; + // goal.header = ; + // goal.pose = ; + // //////////////////////////////////////////////////////////////////// + + // mission->start = start; + // mission->goal = goal; + + missions.push_back(mission); + + // Mission tiếp theo bắt đầu sau node action hiện tại + start_node_idx = end_node_idx; + } + + // Phần còn lại sau action cuối cùng + if(start_node_idx < order.nodes.size() - 1) + { + auto mission = std::make_shared(); + + mission->type = MissionType::VDA5050_ORDER; + mission->priority = 0; + + mission->nodes.assign( + order.nodes.begin() + start_node_idx, + order.nodes.end()); + + if(start_node_idx < order.edges.size()) + { + mission->edges.assign( + order.edges.begin() + start_node_idx, + order.edges.end()); + } + + // /////////////////////////// Để tạm thời /////////////////////////// + // robot_geometry_msgs::PoseStamped start, goal; + // start.header.frame_id = "map" + // start.pose = ; + // goal.header = ; + // goal.pose = ; + // //////////////////////////////////////////////////////////////////// + + // mission->start = start; + // mission->goal = goal; + + missions.push_back(mission); + } + + // Collect actions + for(auto& mission : missions) + { + for(const auto& edge : mission->edges) + { + for(const auto& action : edge.actions) + { + Action mission_action; + mission_action.sequenceId = edge.sequenceId; // Giữ nguyên sequenceId từ action gốc + mission_action.type = ActionType::EDGE_ACTION; + mission_action.action = action; + mission->actions.push_back(std::move(mission_action)); + } + } + + const auto& node = mission->nodes.back(); + if(node.actions.empty()) continue; + + for(const auto& action : node.actions) + { + Action mission_action; + mission_action.sequenceId = node.sequenceId; // Giữ nguyên sequenceId từ action gốc + mission_action.type = ActionType::NODE_ACTION; + mission_action.action = action; + mission->actions.push_back(std::move(mission_action)); + } + // xắp xếp actions theo sequenceId để đảm bảo thứ tự thực hiện + std::sort(mission->actions.begin(), mission->actions.end(), + [](const Action& a, const Action& b) { + return a.sequenceId < b.sequenceId; + }); + + } + + return missions; + } + + void MissionManager::submit(const std::vector>& missions) + { + std::lock_guard lock(mutex_); + for(auto& mission : missions) + { + mission_queue_.push(mission); + } + + if(!missions.empty()) + { + if(state_ == MissionState::IDLE || + state_ == MissionState::FAILED || + state_ == MissionState::CANCELLED || + state_ == MissionState::EMERGENCY) + { + state_ = MissionState::QUEUED; + } + } + } + + std::shared_ptr MissionManager::nextMission() + { + std::lock_guard lock(mutex_); + if(state_ == MissionState::PAUSED || + state_ == MissionState::FAILED || + state_ == MissionState::CANCELLED || + state_ == MissionState::EMERGENCY) + { + return nullptr; + } + + if(current_mission_) + { + return current_mission_; + } + + if(mission_queue_.empty()) + { + return nullptr; + } + + if(current_mission_ == nullptr) + { + current_mission_ = mission_queue_.front(); + + mission_queue_.pop(); + + state_ = MissionState::RUNNING; + } + + return current_mission_; + } + + void MissionManager::onNavigationDone() + { + std::lock_guard lock(mutex_); + current_mission_.reset(); + + if(mission_queue_.empty()) + { + state_ = MissionState::IDLE; + } + else + { + state_ = MissionState::QUEUED; + } + } + + void MissionManager::onNavigationFailed() + { + std::lock_guard lock(mutex_); + state_ = MissionState::FAILED; + + current_mission_.reset(); + + while(!mission_queue_.empty()) + { + mission_queue_.pop(); + } + } + + void MissionManager::cancel() + { + std::lock_guard lock(mutex_); + current_mission_.reset(); + + while(!mission_queue_.empty()) + { + mission_queue_.pop(); + } + + state_ = MissionState::CANCELLED; + } + + void MissionManager::emergency() + { + std::lock_guard lock(mutex_); + current_mission_.reset(); + + while(!mission_queue_.empty()) + { + mission_queue_.pop(); + } + + state_ = MissionState::EMERGENCY; + } + + void MissionManager::pause() + { + std::lock_guard lock(mutex_); + if(state_ == MissionState::RUNNING) + { + state_ = MissionState::PAUSED; + } + } + + void MissionManager::resume() + { + std::lock_guard lock(mutex_); + if(state_ != MissionState::PAUSED) + { + return; + } + + if(current_mission_) + { + state_ = MissionState::RUNNING; + } + else if(!mission_queue_.empty())//Nếu đang thực hiện mission cuối cần xem lại logic + { + state_ = MissionState::QUEUED; + } + else + { + state_ = MissionState::IDLE; + } + } + + MissionState MissionManager::state() const + { + std::lock_guard lock(mutex_); + + return state_; + } + + bool MissionManager::hasMission() const + { + std::lock_guard lock(mutex_); + + return !mission_queue_.empty(); + } + + EventProcessor::EventProcessor(MissionManager& mission_manager) + : mission_manager_(mission_manager) + {} + + EventProcessor::~EventProcessor() + { + stop(); + } + + void EventProcessor::start() + { + if(running_) + { + return; + } + + running_ = true; + + event_bus_.reset(); + + worker_ = + std::thread( + &EventProcessor::spin, + this); + } + + void EventProcessor::stop() + { + running_ = false; + + event_bus_.stop(); + + if(worker_.joinable()) + { + worker_.join(); + } + } + + void EventProcessor::spin() + { + Event event; + + // robot::Rate rate(50); + + while(running_) + { + if(event_bus_.pop(event)) + { + process(event); + } + + // rate.sleep(); + } + } + + void EventProcessor::process(const Event& event) + { + switch(event.type) + { + case EventType::SUBMIT_MISSIONS: + { + mission_manager_.submit( + event.missions); + break; + } + + case EventType::NAV_DONE: + { + mission_manager_.onNavigationDone(); + break; + } + + case EventType::NAV_FAILED: + { + mission_manager_.onNavigationFailed(); + break; + } + + case EventType::PAUSE: + { + mission_manager_.pause(); + break; + } + + case EventType::RESUME: + { + mission_manager_.resume(); + break; + } + + case EventType::CANCEL: + { + mission_manager_.cancel(); + break; + } + + case EventType::EMERGENCY: + { + mission_manager_.emergency(); + break; + } + + default: + { + robot::log_error("Unknown event type"); + break; + } + } + } + + void EventProcessor::orderEvent(const robot_protocol_msgs::Order& order) + { + auto missions = vda5050_adapter_.convert(order); + + Event event; + + event.type = EventType::SUBMIT_MISSIONS; + + event.priority = PRIORITY_ORDER; + + event.missions = std::move(missions); + + event_bus_.push(event); + } + + void EventProcessor::goalEvent(const robot_geometry_msgs::PoseStamped& goal) + { + auto missions = goal_adapter_.convert(goal); + + Event event; + + event.type = EventType::SUBMIT_MISSIONS; + + event.priority = PRIORITY_ORDER; + + event.missions = std::move(missions); + + event_bus_.push(event); + } + + void EventProcessor::pauseEvent() + { + Event event; + + event.type = EventType::PAUSE; + + event.priority = PRIORITY_PAUSE; + + event_bus_.push(event); + } + + void EventProcessor::resumeEvent() + { + Event event; + + event.type = EventType::RESUME; + + event.priority = PRIORITY_RESUME; + + event_bus_.push(event); + } + + void EventProcessor::cancelEvent() + { + Event event; + + event.type = + EventType::CANCEL; + + event.priority = + PRIORITY_CANCEL; + + event_bus_.push(event); + } + + void EventProcessor::navDoneEvent() + { + Event event; + + event.type = + EventType::NAV_DONE; + + event.priority = + PRIORITY_NAV_DONE; + + event_bus_.push(event); + } + + void EventProcessor::navFailedEvent() + { + Event event; + + event.type = EventType::NAV_FAILED; + + event.priority = PRIORITY_NAV_DONE; + + event_bus_.push(event); + } + + void EventProcessor::emergencyEvent() + { + Event event; + + event.type = EventType::EMERGENCY; + + event.priority = PRIORITY_EMERGENCY; + + event_bus_.push(event); + } + + MissionExecutor::MissionExecutor(MissionManager& manager) + : mission_manager_(manager) + {} + + MissionExecutor::~MissionExecutor() + { + stop(); + } + + void MissionExecutor::start() + { + if(running_) + { + return; + } + + running_ = true; + + worker_ = + std::thread( + &MissionExecutor::spin, + this); + } + + void MissionExecutor::stop() + { + running_ = false; + + if(worker_.joinable()) + { + worker_.join(); + } + } + + void MissionExecutor::spin() + { + robot::Rate rate(20); + + while(running_) + { + auto mission = + mission_manager_.nextMission(); + + if(mission) + { + bool is_new_mission = false; + + { + std::lock_guard lock(mutex_); + + if(mission != last_dispatched_mission_) + { + last_dispatched_mission_ = mission; + mission_excute_ = mission; + is_new_mission = true; + } + } + + if(is_new_mission) + { + MissionCallback callback; + + { + std::lock_guard lock(mutex_); + callback = mission_callback_; + } + + if(callback) + { + callback(mission); + } + } + } + if(!mission) + { + std::lock_guard lock(mutex_); + last_dispatched_mission_.reset(); + } + + rate.sleep(); + } + } +} diff --git a/src/robot_control_test.cpp b/src/robot_control_test.cpp new file mode 100644 index 0000000..3f957f0 --- /dev/null +++ b/src/robot_control_test.cpp @@ -0,0 +1,87 @@ +#include +#include + +using namespace mission_adapters; +class RobotControlTest +{ + // Giả sử chúng ta có một implementation của BaseNavigation, ví dụ MoveBase và đã được load vào move_base_ptr_ + robot::move_base_core::BaseNavigation::Ptr move_base_ptr_; + + mission_adapters::MissionManager mission_manager_; + mission_adapters::EventProcessor event_processor_{mission_manager_}; + mission_adapters::MissionExecutor mission_executor_{mission_manager_}; + std::shared_ptr mission_prev_ = nullptr; + robot::move_base_core::State prev_state_ = PENDING; + +public: + RobotControlTest() = default; + ~RobotControlTest(); + void run(); +}; + +RobotControlTest::~RobotControlTest() +{ + event_processor_.stop(); + + mission_executor_.stop(); +} + +void RobotControlTest::run() +{ + robot::Rate rate(50); + event_processor_.start(); + mission_executor_.start(); + while(true) + { + // chạy trong một luồng riêng + auto state = move_base_ptr_->getFeedback()->navigation_state; + + bool action_done = true; + //kiểm tra acction done chưa + + //thêm kiểm tra navDoneEvent state == robot::move_base_core::State::SUCCEEDED và action_done == true + if(state != prev_state_) + { + if(state == + robot::move_base_core::State::SUCCEEDED) + { + event_processor_.navDoneEvent(); + } + + if(state == + robot::move_base_core::State::ABORTED) + { + event_processor_.navFailedEvent(); + } + + prev_state_ = state; + } + //nhận oder trong một luồng riêng + //ví dụ: robot_protocol_msgs::Order order; + if(/* có order mới */) + { + robot_protocol_msgs::Order order; + // ... nhận order từ đâu đó, ví dụ qua ROS topic hoặc service + event_processor_.orderEvent(order); + } + + + mission_executor_.setMissionCallback( + [&](const std::shared_ptr& mission) + { + // ví dụ hàm executeMission đã có, nhận mission và đẩy dữ liệu goal sang move_base_ptr_ + executeMission(*mission); + }); + + + rate.sleep(); + } +} + +int main() +{ + RobotControlTest test; + test.run(); + return 0; +} +