DuongTD/mission_adapters
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first update

Browse Source
This commit is contained in:
duongtd
2026-06-05 15:41:06 +07:00
commit 5cdacdeebf
14 changed files with 1575 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

18
CHANGELOG.rst Normal file
View File

@@ -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 <https://github.com/zengxiaolei/homing_local_planner/issues/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

264
CMakeLists.txt Normal file
View File

@@ -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
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>
# )
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# 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
# $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>
# )
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# 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()

217
README.md Normal file
View File

@@ -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!** 🚗

11
cfg/stanley_local_planner_params.yaml Normal file
View File

@@ -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

0
include/mission_adapters/adapter.h Normal file
View File

0
include/mission_adapters/event.h Normal file
View File

0
include/mission_adapters/event_processor.h Normal file
View File

279
include/mission_adapters/mission_adapters.h Normal file
View File

@@ -0,0 +1,279 @@
#ifndef MISSION_ADAPTERS_MISSION_ADAPTERS_H
#define MISSION_ADAPTERS_MISSION_ADAPTERS_H
#include <string>
#include <vector>
#include <queue>
#include <memory>
#include <thread>
#include <mutex>
#include <functional>
#include <atomic>
#include <robot/robot.h>
#include <robot_protocol_msgs/Order.h>
#include <robot_geometry_msgs/PoseStamped.h>
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<robot_protocol_msgs::Node> nodes;
std::vector<robot_protocol_msgs::Edge> edges;
std::vector<Action> actions;
};
struct Event
{
EventType type;
int priority;
std::vector<std::shared_ptr<Mission>> 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<Event, std::vector<Event>, 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<std::mutex> lock(mutex_);
return queue_.empty();
}
inline void clear()
{
std::lock_guard<std::mutex> lock(mutex_);
while(!queue_.empty())
{
queue_.pop();
}
}
};
class VDA5050Adapter
{
public:
std::vector<std::shared_ptr<Mission>> convert(const robot_protocol_msgs::Order& order);
};
class GoalAdapter
{
public:
std::vector<std::shared_ptr<Mission>> convert(const robot_geometry_msgs::PoseStamped& goal);
};
class MissionManager
{
public:
void submit(const std::vector<std::shared_ptr<Mission>>& missions);
std::shared_ptr<Mission> 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<std::shared_ptr<Mission>> mission_queue_;
std::shared_ptr<Mission> 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<bool> running_{false};
};
class MissionExecutor
{
public:
using MissionCallback =
std::function<void(const std::shared_ptr<Mission>&)>;
explicit MissionExecutor(MissionManager& manager);
~MissionExecutor();
void start();
void stop();
void setMissionCallback(MissionCallback cb)
{
std::lock_guard<std::mutex> lock(mutex_);
mission_callback_ = std::move(cb);
}
private:
std::shared_ptr<Mission> mission_excute_;
// Mission cuối cùng đã dispatch
std::shared_ptr<Mission> last_dispatched_mission_;
MissionCallback mission_callback_;
MissionManager& mission_manager_;
mutable std::mutex mutex_;
std::thread worker_;
std::atomic<bool> running_{false};
void spin();
};
}
#endif // MISSION_ADAPTERS_MISSION_ADAPTERS_H

0
include/mission_adapters/mission_excutor.h Normal file
View File

0
include/mission_adapters/mission_manager.h Normal file
View File

0
include/mission_adapters/types.h Normal file
View File

50
package.xml Normal file
View File

@@ -0,0 +1,50 @@
<package>
<name>mission_adapters</name>
<version>0.7.10</version>
<description>
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.
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>
<author>Wim Meeussen</author>
<maintainer email="tfoote@osrfoundation.org">Tully Foote</maintainer>
<license>BSD</license>
<url type="website">http://www.ros.org/wiki/mission_adapters</url>
<buildtool_depend version_gte="0.5.68">catkin</buildtool_depend>
<build_depend>libconsole-bridge-dev</build_depend>
<run_depend>libconsole-bridge-dev</run_depend>
<build_depend>robot_costmap_2d</build_depend>
<build_depend>robot_nav_core</build_depend>
<build_depend>robot_nav_core2</build_depend>
<build_depend>robot_nav_msgs</build_depend>
<build_depend>robot_std_msgs</build_depend>
<build_depend>robot_geometry_msgs</build_depend>
<build_depend>robot_cpp</build_depend>
<build_depend>tf3</build_depend>
<build_depend>robot_tf3_geometry_msgs</build_depend>
<build_depend>robot_visualization_msgs</build_depend>
<build_depend>robot_nav_2d_utils</build_depend>
<build_depend>data_convert</build_depend>
<run_depend>robot_costmap_2d</run_depend>
<run_depend>robot_nav_core</run_depend>
<run_depend>robot_nav_core2</run_depend>
<run_depend>robot_nav_msgs</run_depend>
<run_depend>robot_std_msgs</run_depend>
<run_depend>robot_geometry_msgs</run_depend>
<run_depend>robot_cpp</run_depend>
<run_depend>tf3</run_depend>
<run_depend>robot_tf3_geometry_msgs</run_depend>
<run_depend>robot_visualization_msgs</run_depend>
<run_depend>robot_nav_2d_utils</run_depend>
<run_depend>data_convert</run_depend>
</package>

649
src/mission_adapters.cpp Normal file
View File

@@ -0,0 +1,649 @@
#include <algorithm>
#include <mission_adapters/mission_adapters.h>
namespace mission_adapters
{
void EventBus::push(const Event& event)
{
std::lock_guard<std::mutex> lock(mutex_);
queue_.push(event);
cv_.notify_one();
}
bool EventBus::pop(Event& event)
{
std::unique_lock<std::mutex> 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<std::mutex> lock(mutex_);
stop_ = true;
}
cv_.notify_all();
}
void EventBus::reset()
{
std::lock_guard<std::mutex> lock(mutex_);
stop_ = false;
}
std::vector<std::shared_ptr<Mission>>
GoalAdapter::convert(const robot_geometry_msgs::PoseStamped& goal)
{
std::vector<std::shared_ptr<Mission>> missions;
auto mission = std::make_shared<Mission>();
mission->type = MissionType::SIMPLE_GOAL;
mission->priority = 0;
/////////////////////////// Để tạm thời ///////////////////////////
mission->goal = goal;
////////////////////////////////////////////////////////////////////
missions.push_back(mission);
return missions;
}
std::vector<std::shared_ptr<Mission>>
VDA5050Adapter::convert(const robot_protocol_msgs::Order& order)
{
std::vector<std::shared_ptr<Mission>> missions;
std::vector<size_t> 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>();
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>();
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<std::shared_ptr<Mission>>& missions)
{
std::lock_guard<std::mutex> 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<Mission> MissionManager::nextMission()
{
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
current_mission_.reset();
if(mission_queue_.empty())
{
state_ = MissionState::IDLE;
}
else
{
state_ = MissionState::QUEUED;
}
}
void MissionManager::onNavigationFailed()
{
std::lock_guard<std::mutex> lock(mutex_);
state_ = MissionState::FAILED;
current_mission_.reset();
while(!mission_queue_.empty())
{
mission_queue_.pop();
}
}
void MissionManager::cancel()
{
std::lock_guard<std::mutex> lock(mutex_);
current_mission_.reset();
while(!mission_queue_.empty())
{
mission_queue_.pop();
}
state_ = MissionState::CANCELLED;
}
void MissionManager::emergency()
{
std::lock_guard<std::mutex> lock(mutex_);
current_mission_.reset();
while(!mission_queue_.empty())
{
mission_queue_.pop();
}
state_ = MissionState::EMERGENCY;
}
void MissionManager::pause()
{
std::lock_guard<std::mutex> lock(mutex_);
if(state_ == MissionState::RUNNING)
{
state_ = MissionState::PAUSED;
}
}
void MissionManager::resume()
{
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
return state_;
}
bool MissionManager::hasMission() const
{
std::lock_guard<std::mutex> 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<std::mutex> 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<std::mutex> lock(mutex_);
callback = mission_callback_;
}
if(callback)
{
callback(mission);
}
}
}
if(!mission)
{
std::lock_guard<std::mutex> lock(mutex_);
last_dispatched_mission_.reset();
}
rate.sleep();
}
}
}

87
src/robot_control_test.cpp Normal file
View File

@@ -0,0 +1,87 @@
#include <mission_adapters/mission_adapters.h>
#include <move_base_core/navigation.h>
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> 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>& 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;
}