hivecore robot brain
This commit is contained in:
5
.gitignore
vendored
Normal file
5
.gitignore
vendored
Normal file
@@ -0,0 +1,5 @@
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
|
||||
.vscode/
|
||||
28
README.md
28
README.md
@@ -1,2 +1,30 @@
|
||||
# hivecore_robot_brain
|
||||
|
||||
## BT安装
|
||||
```bash
|
||||
sudo apt-get install -y libzmq3-dev #有的环境需要安装zmq
|
||||
|
||||
git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git \
|
||||
cd BehaviorTree.CPP \
|
||||
mkdir build && cd build \
|
||||
cmake .. \
|
||||
make \
|
||||
sudo make install
|
||||
```
|
||||
## SMACC2安装
|
||||
```bash
|
||||
sudo apt-get install -y liblttng-ust-dev #有的环境需要安装
|
||||
|
||||
git clone https://github.com/robosoft-ai/SMACC2.git
|
||||
colcon build --packages-select smacc2_msgs smacc2
|
||||
```
|
||||
|
||||
## 编译
|
||||
```bash
|
||||
./src/scripts/build.sh
|
||||
```
|
||||
|
||||
## 运行
|
||||
```bash
|
||||
./src/scripts/run.sh
|
||||
```
|
||||
|
||||
11
src/brain/.clang-format
Normal file
11
src/brain/.clang-format
Normal file
@@ -0,0 +1,11 @@
|
||||
# Project-wide clang-format inheriting Google style with tweaks
|
||||
BasedOnStyle: Google
|
||||
Language: Cpp
|
||||
ColumnLimit: 110
|
||||
DerivePointerAlignment: false
|
||||
PointerAlignment: Left
|
||||
ReflowComments: true
|
||||
SortIncludes: true
|
||||
IncludeBlocks: Regroup
|
||||
NamespaceIndentation: None
|
||||
AllowShortFunctionsOnASingleLine: Empty
|
||||
17
src/brain/.uncrustify.cfg
Normal file
17
src/brain/.uncrustify.cfg
Normal file
@@ -0,0 +1,17 @@
|
||||
# Minimal uncrustify configuration aligned with Google C++ style tendencies
|
||||
newlines=lf
|
||||
input_tab_size=2
|
||||
output_tab_size=2
|
||||
indent_columns=2
|
||||
indent_with_tabs=0
|
||||
code_width=110
|
||||
sp_after_comma=1
|
||||
sp_after_keyword=1
|
||||
sp_before_sparen=0
|
||||
nl_before_brace_function=1
|
||||
nl_before_brace_class=1
|
||||
nl_before_brace_namespace=1
|
||||
nl_before_brace_control=1
|
||||
nl_after_brace_open=1
|
||||
nl_after_return=1
|
||||
cmt_reflow_mode=1
|
||||
267
src/brain/CMakeLists.txt
Normal file
267
src/brain/CMakeLists.txt
Normal file
@@ -0,0 +1,267 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(brain)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_components REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(smacc2 REQUIRED)
|
||||
find_package(smacc2_msgs REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(behaviortree_cpp REQUIRED)
|
||||
find_package(rclcpp_lifecycle REQUIRED)
|
||||
find_package(Boost REQUIRED COMPONENTS thread)
|
||||
find_package(yaml-cpp REQUIRED)
|
||||
find_package(ament_index_cpp REQUIRED)
|
||||
find_package(Doxygen QUIET)
|
||||
find_package(std_srvs REQUIRED)
|
||||
|
||||
# 创建一个可执行文件，将两个节点编译到一个进程中
|
||||
add_executable(brain_node
|
||||
src/brain_node.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/cerebrum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
|
||||
target_include_directories(brain_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
|
||||
target_compile_features(brain_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
|
||||
ament_target_dependencies(brain_node
|
||||
rclcpp
|
||||
rclcpp_components
|
||||
rclcpp_action
|
||||
behaviortree_cpp
|
||||
interfaces
|
||||
std_msgs
|
||||
smacc2
|
||||
smacc2_msgs
|
||||
rclcpp_lifecycle
|
||||
ament_index_cpp
|
||||
yaml-cpp
|
||||
std_srvs
|
||||
)
|
||||
|
||||
target_link_libraries(brain_node
|
||||
Boost::thread
|
||||
yaml-cpp
|
||||
)
|
||||
|
||||
install(TARGETS brain_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
install(DIRECTORY config/
|
||||
DESTINATION share/${PROJECT_NAME}/config)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
if(NOT DEFINED ENV{SKIP_LINT})
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
endif()
|
||||
find_package(ament_cmake_gtest REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
if(NOT DEFINED ENV{SKIP_LINT})
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
# -------------------------
|
||||
# GoogleTest targets
|
||||
# -------------------------
|
||||
ament_add_gtest(test_action_registry
|
||||
test/test_action_registry.cpp
|
||||
)
|
||||
if(TARGET test_action_registry)
|
||||
ament_target_dependencies(test_action_registry
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
interfaces
|
||||
std_srvs
|
||||
)
|
||||
target_include_directories(test_action_registry PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
endif()
|
||||
|
||||
ament_add_gtest(test_bt_registry
|
||||
test/test_bt_registry.cpp
|
||||
)
|
||||
if(TARGET test_bt_registry)
|
||||
ament_target_dependencies(test_bt_registry
|
||||
rclcpp
|
||||
behaviortree_cpp
|
||||
std_srvs
|
||||
)
|
||||
target_include_directories(test_bt_registry PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
endif()
|
||||
|
||||
ament_add_gtest(test_cerebellum_node
|
||||
test/test_cerebellum_node.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
if(TARGET test_cerebellum_node)
|
||||
target_compile_definitions(test_cerebellum_node PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(test_cerebellum_node
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
behaviortree_cpp
|
||||
smacc2
|
||||
smacc2_msgs
|
||||
ament_index_cpp
|
||||
interfaces
|
||||
std_srvs
|
||||
)
|
||||
target_include_directories(test_cerebellum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
target_link_libraries(test_cerebellum_node Boost::thread)
|
||||
target_link_libraries(test_cerebellum_node yaml-cpp)
|
||||
endif()
|
||||
|
||||
ament_add_gtest(test_cerebrum_node
|
||||
test/test_cerebrum_node.cpp
|
||||
src/cerebrum_node.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
if(TARGET test_cerebrum_node)
|
||||
target_compile_definitions(test_cerebrum_node PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(test_cerebrum_node
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
behaviortree_cpp
|
||||
interfaces
|
||||
smacc2
|
||||
smacc2_msgs
|
||||
ament_index_cpp
|
||||
std_srvs
|
||||
)
|
||||
target_include_directories(test_cerebrum_node PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
target_link_libraries(test_cerebrum_node Boost::thread)
|
||||
target_link_libraries(test_cerebrum_node yaml-cpp)
|
||||
endif()
|
||||
|
||||
ament_add_gtest(test_sm_cerebellum
|
||||
test/test_sm_cerebellum.cpp
|
||||
)
|
||||
if(TARGET test_sm_cerebellum)
|
||||
ament_target_dependencies(test_sm_cerebellum
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
smacc2
|
||||
smacc2_msgs
|
||||
interfaces
|
||||
std_srvs
|
||||
)
|
||||
target_include_directories(test_sm_cerebellum PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
endif()
|
||||
|
||||
# ExecuteBtAction end-to-end single skill test
|
||||
ament_add_gtest(test_execute_bt_action
|
||||
test/test_execute_bt_action.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
if(TARGET test_execute_bt_action)
|
||||
target_compile_definitions(test_execute_bt_action PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(test_execute_bt_action
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
behaviortree_cpp
|
||||
interfaces
|
||||
smacc2
|
||||
smacc2_msgs
|
||||
ament_index_cpp
|
||||
std_srvs
|
||||
)
|
||||
target_include_directories(test_execute_bt_action PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
target_link_libraries(test_execute_bt_action Boost::thread yaml-cpp)
|
||||
endif()
|
||||
|
||||
# Extended ExecuteBtAction scenario tests (multi-skill, failure, RUN interpolation, timeout, stats, cancel)
|
||||
ament_add_gtest(test_execute_bt_action_extended
|
||||
test/test_execute_bt_action_extended.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
if(TARGET test_execute_bt_action_extended)
|
||||
target_compile_definitions(test_execute_bt_action_extended PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(test_execute_bt_action_extended
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
behaviortree_cpp
|
||||
interfaces
|
||||
smacc2
|
||||
smacc2_msgs
|
||||
ament_index_cpp
|
||||
std_srvs
|
||||
)
|
||||
target_include_directories(test_execute_bt_action_extended PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
target_link_libraries(test_execute_bt_action_extended Boost::thread yaml-cpp)
|
||||
endif()
|
||||
|
||||
# Additional isolated ExecuteBtAction scenario tests
|
||||
foreach(extra_test IN ITEMS unsupported run_interp timeout_continue stats cancel)
|
||||
set(test_name test_execute_bt_action_${extra_test})
|
||||
ament_add_gtest(${test_name}
|
||||
test/test_execute_bt_action_${extra_test}.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
if(TARGET ${test_name})
|
||||
target_compile_definitions(${test_name} PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(${test_name}
|
||||
rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
|
||||
target_include_directories(${test_name} PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
target_link_libraries(${test_name} Boost::thread yaml-cpp)
|
||||
endif()
|
||||
endforeach()
|
||||
|
||||
ament_add_gtest(test_cerebrum_utils
|
||||
test/test_cerebrum_utils.cpp
|
||||
src/cerebrum_node.cpp
|
||||
src/cerebellum_node.cpp
|
||||
src/skill_manager.cpp
|
||||
)
|
||||
if(TARGET test_cerebrum_utils)
|
||||
target_compile_definitions(test_cerebrum_utils PRIVATE BRAIN_DISABLE_SM=1)
|
||||
ament_target_dependencies(test_cerebrum_utils
|
||||
rclcpp rclcpp_action behaviortree_cpp interfaces smacc2 smacc2_msgs ament_index_cpp std_srvs)
|
||||
target_include_directories(test_cerebrum_utils PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
target_link_libraries(test_cerebrum_utils Boost::thread yaml-cpp)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# --------------------------------------------------
|
||||
# Doxygen documentation target (optional)
|
||||
# Usage: colcon build --packages-select brain --cmake-args -DBUILD_DOCS=ON
|
||||
# Generated at build/brain/docs/html
|
||||
# --------------------------------------------------
|
||||
option(BUILD_DOCS "Build API documentation with Doxygen" OFF)
|
||||
if(BUILD_DOCS AND DOXYGEN_FOUND)
|
||||
set(DOXYGEN_IN ${CMAKE_CURRENT_SOURCE_DIR}/Doxyfile)
|
||||
if(EXISTS ${DOXYGEN_IN})
|
||||
set(DOXYGEN_OUT ${CMAKE_CURRENT_BINARY_DIR}/Doxyfile.gen)
|
||||
configure_file(${DOXYGEN_IN} ${DOXYGEN_OUT} @ONLY)
|
||||
add_custom_target(brain_docs
|
||||
COMMAND ${DOXYGEN_EXECUTABLE} ${DOXYGEN_OUT}
|
||||
WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
|
||||
COMMENT "Generating API documentation with Doxygen" VERBATIM)
|
||||
endif()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
58
src/brain/Doxyfile
Normal file
58
src/brain/Doxyfile
Normal file
@@ -0,0 +1,58 @@
|
||||
# Doxygen configuration generated for brain package
|
||||
PROJECT_NAME = "brain_cerebrum_node"
|
||||
PROJECT_BRIEF = "Behavior-tree driven skill sequencing node (CerebrumNode)"
|
||||
PROJECT_NUMBER = 1.0
|
||||
OUTPUT_DIRECTORY = docs
|
||||
CREATE_SUBDIRS = NO
|
||||
OUTPUT_LANGUAGE = English
|
||||
OPTIMIZE_OUTPUT_FOR_C = NO
|
||||
OPTIMIZE_OUTPUT_JAVA = NO
|
||||
OPTIMIZE_FOR_FORTRAN = NO
|
||||
OPTIMIZE_OUTPUT_VHDL = NO
|
||||
MARKDOWN_SUPPORT = YES
|
||||
AUTOLINK_SUPPORT = YES
|
||||
BUILTIN_STL_SUPPORT = YES
|
||||
EXTRACT_ALL = YES
|
||||
EXTRACT_PRIVATE = NO
|
||||
EXTRACT_STATIC = NO
|
||||
EXTRACT_ANON_NSPACES = YES
|
||||
EXTRACT_LOCAL_CLASSES = YES
|
||||
FULL_PATH_NAMES = YES
|
||||
STRIP_FROM_PATH =
|
||||
STRIP_FROM_INC_PATH =
|
||||
INPUT = src/brain/include/brain src/brain/src
|
||||
FILE_PATTERNS = *.hpp *.h *.cpp
|
||||
RECURSIVE = YES
|
||||
EXCLUDE = build install log
|
||||
EXCLUDE_SYMLINKS = YES
|
||||
GENERATE_HTML = YES
|
||||
HTML_OUTPUT = html
|
||||
GENERATE_LATEX = NO
|
||||
GENERATE_MAN = NO
|
||||
GENERATE_XML = NO
|
||||
QUIET = NO
|
||||
WARN_IF_UNDOCUMENTED = YES
|
||||
WARN_IF_DOC_ERROR = YES
|
||||
WARN_NO_PARAMDOC = YES
|
||||
INLINE_SOURCES = NO
|
||||
REFERENCED_BY_RELATION = NO
|
||||
REFERENCES_RELATION = NO
|
||||
SOURCE_BROWSER = NO
|
||||
USE_MDFILE_AS_MAINPAGE = README_CEREBRUM_NODE.md
|
||||
GENERATE_TREEVIEW = YES
|
||||
DOT_GRAPH_MAX_NODES = 80
|
||||
HAVE_DOT = YES
|
||||
CALL_GRAPH = NO
|
||||
CALLER_GRAPH = NO
|
||||
CLASS_DIAGRAMS = YES
|
||||
ALPHABETICAL_INDEX = YES
|
||||
SEARCHENGINE = YES
|
||||
GENERATE_TAGFILE =
|
||||
ALIASES += "thread_safety=\par Thread-safety:\n"
|
||||
ALIASES += "warning=\par WARNING:\n"
|
||||
ALIASES += "note=\par Note:\n"
|
||||
MULTILINE_CPP_IS_BRIEF = YES
|
||||
JAVADOC_AUTOBRIEF = YES
|
||||
WARN_IF_UNDOCUMENTED = YES
|
||||
WARN_IF_DOC_ERROR = YES
|
||||
WARN_NO_PARAMDOC = YES
|
||||
615
src/brain/README.md
Normal file
615
src/brain/README.md
Normal file
@@ -0,0 +1,615 @@
|
||||
# brain：大脑 / 小脑 协同执行框架
|
||||
|
||||
## 目录概览
|
||||
|
||||
| 组件 | 角色 | 关键文件 |
|
||||
| ---- | ---- | -------- |
|
||||
| CerebrumNode | 高层行为/技能序列决策 (BT) | `cerebrum_node.cpp` |
|
||||
| CerebellumNode | 技能执行编排 + SMACC2 状态机 | `cerebellum_node.cpp` |
|
||||
| SkillManager | 解析 YAML / 注册技能接口 | `skill_manager.cpp` |
|
||||
| ActionClientRegistry | 多 Action 客户端统一封装 | `action_registry.hpp` |
|
||||
| ActionServerRegistry | 多 Action 服务端统一封装 | `action_registry.hpp` |
|
||||
| BTRegistry | 行为树节点/序列注册工厂 | `bt_registry.hpp` |
|
||||
| Skill Action Traits | 技能动作类型元编程分派 | `skill_action_traits.hpp` |
|
||||
| ExecuteBtAction | 技能序列统一 Action 定义 | `interfaces/action/ExecuteBtAction.action` |
|
||||
| Stats Publisher | 技能统计发布 `/brain/skill_stats` | `cerebellum_node.cpp` |
|
||||
|
||||
## 架构要点（当前版本特性）
|
||||
|
||||
1. 行为树（BehaviorTree.CPP）在 Cerebrum 动态构建：从技能集合挑选（随机或固定序列）生成 `<Sequence>` XML，注册后 tick。
|
||||
2. 每个技能生成一个 BT Action Node（名称形如 `SkillName_H`），其 `on_start` 通过统一客户端向小脑发送 ExecuteBtAction 目标（封装技能名称）。
|
||||
3. 小脑（Cerebellum）收到 ExecuteBtAction 目标后将 action_name 解析为一个或多个技能（现阶段每个 goal 只含 1..N 技能列表），按顺序执行：
|
||||
- 动作型技能：通过 `SkillActionTraits` 匹配具体 Action 类型并同步 `send_and_wait`，支持超时 + 取消；同时推送结构化反馈。
|
||||
- 服务型技能（示例 VisionObjectRecognition）：直接调用并反馈结果。
|
||||
4. 统一统计：累计每个技能成功 / 失败次数 + 序列成功/失败计数，定期日志并以字符串形式发布到 `/brain/skill_stats`。
|
||||
5. 结构化反馈 (ExecuteBtAction.Feedback)：包含阶段(Stage)、当前技能、整体进度(Progress ∈ [0,1])、细节(Detail)。
|
||||
6. 运行阶段 Stage 采用：`START` / `WAIT` / `RUN` / `END`。
|
||||
7. 进度计算：
|
||||
- 序列共有 N 技能；第 i(0-based) 个技能开始时 progress≈ i/N。
|
||||
- WAIT/RUN 阶段在该技能时间窗内插值（RUN 对基于超时的估算 <1.0）。
|
||||
- END 阶段统一发布 (i+1)/N，最后一个技能 END = 1.0。
|
||||
8. 超时/取消：`ActionClientRegistry::send_and_wait` 支持超时；未来可暴露 cancel 调用（当前 RUN 线程轮询完成，不主动取消）。
|
||||
9. 无需为每个技能单独写冗长 if-else，trait 模板自动匹配技能 → Action 类型。
|
||||
10. 严格命名常量集中于 `constants.hpp`，减少硬编码串。
|
||||
|
||||
## BT安装
|
||||
```bash
|
||||
|
||||
sudo apt-get install -y libzmq3-dev #有的环境需要安装zmq
|
||||
|
||||
git clone https://github.com/BehaviorTree/BehaviorTree.CPP.git \
|
||||
cd BehaviorTree.CPP \
|
||||
mkdir build && cd build \
|
||||
cmake .. \
|
||||
make \
|
||||
sudo make install
|
||||
```
|
||||
|
||||
## SMACC2安装
|
||||
```bash
|
||||
|
||||
sudo apt-get install -y liblttng-ust-dev #有的环境需要安装
|
||||
|
||||
git clone https://github.com/robosoft-ai/SMACC2.git
|
||||
colcon build
|
||||
```
|
||||
|
||||
## ExecuteBtAction.action 结构
|
||||
|
||||
```text
|
||||
Goal:
|
||||
string action_name # 逗号/空格/分号分隔技能序列，或单个技能
|
||||
|
||||
Result:
|
||||
bool success
|
||||
string message # 汇总: Seq{A,B}: A(OK)[...], B(FAIL)[...]
|
||||
int32 total_skills
|
||||
int32 succeeded_skills
|
||||
|
||||
Feedback:
|
||||
string stage # START | WAIT | RUN | END
|
||||
string current_skill
|
||||
float32 progress # 0..1 序列整体进展
|
||||
string detail # 附加信息 (attempt= / OK: / FAIL:原因)
|
||||
```
|
||||
|
||||
## 反馈阶段语义
|
||||
|
||||
| Stage | 触发时机 | 说明 |
|
||||
| ----- | -------- | ---- |
|
||||
| START | 技能开始调度 | 记录基础进度基线 (i/N) |
|
||||
| WAIT | 等待对应 Action Server 可用重试 | detail=attempt=k |
|
||||
| RUN | 已发送 / 正在等待动作结束 | 基于超时线性插值进度，不超过 (i+1)/N 的 0.99 |
|
||||
| END | 技能完成（成功/失败） | 最终进度 (i+1)/N + detail=OK/FAIL:... |
|
||||
|
||||
## 技能动作 Traits 分派
|
||||
|
||||
`skill_action_traits.hpp` 定义：
|
||||
|
||||
```cpp
|
||||
using SkillActionTypes = std::tuple<ArmSpaceControl, HandControl, LegControl, VisionGraspObject>;
|
||||
|
||||
// dispatch_skill_action<0, SkillActionTypes>(skillName, topic, registry,...)
|
||||
```
|
||||
|
||||
扩展新技能：
|
||||
|
||||
1. 在 `interfaces/action/` 新增 Action 定义并编译。
|
||||
2. 在 traits 中专门化 `SkillActionTrait<YourAction>`（success/message 适配）。
|
||||
3. 将新 Action 类型加入 `SkillActionTypes` 元组。
|
||||
4. 确保 YAML 中声明该技能及其接口列表（含 `.action` 标识）。
|
||||
|
||||
## 技能注册与 YAML
|
||||
|
||||
`SkillManager` 解析技能文件（默认 `share/brain/config/robot_skills.yaml`），为每个声明的动作接口注册客户端（topic 采用 CamelCase 与 snake_case 双尝试）。
|
||||
|
||||
## 行为树序列构建逻辑
|
||||
|
||||
1. 若参数 `fixed_skill_sequence` 提供 → 使用固定序列。
|
||||
2. 否则随机挑选满足拥有 action 接口的技能（当前示例默认 1 个，可调整）。
|
||||
3. 生成 `<Sequence>` XML 并通过 `BTRegistry` 注册。
|
||||
4. 10ms 定时器 tick root；节点通过 per-node 执行状态表跟踪 in-flight / timeout。
|
||||
|
||||
## 超时与失败策略
|
||||
|
||||
| 维度 | 策略 |
|
||||
| ---- | ---- |
|
||||
| 单技能动作超时 | `ActionClientRegistry::send_and_wait` 等待；超时返回失败 detail=action failed |
|
||||
| Server 不可用 | 最多 8 次 WAIT 重试；失败 detail=Server unavailable after retries |
|
||||
| 序列失败聚合 | 任一失败若参数 `abort_on_failure=true` → 提前中断序列 |
|
||||
| 行为树节点超时 | 节点 `on_running` 判定 elapsed > node_timeout_sec_ → FAIL |
|
||||
|
||||
## 统计发布 `/brain/skill_stats`
|
||||
|
||||
格式：`seq_ok=X,seq_fail=Y;SkillA=ok/fail;SkillB=ok/fail...`
|
||||
（后续可演进为自定义 msg 类型与更丰富指标：平均耗时、成功率窗口等）
|
||||
|
||||
## 关键参数
|
||||
|
||||
| 参数 | 说明 | 默认 |
|
||||
| default_action_timeout_sec | 动作通用超时 | 60 |
|
||||
| vision_grasp_object_timeout_sec | 特定技能超时 | 120 |
|
||||
| skill_timeouts | 覆盖型超时映射 Skill=sec | 空 |
|
||||
| allowed_action_skills | 白名单（逗号/分号/空格） | 空(全允许) |
|
||||
| fixed_skill_sequence | 固定序列 | 空(随机) |
|
||||
| stats_log_interval_sec | 统计日志周期 | 60 |
|
||||
|
||||
## 构建与运行
|
||||
```bash
|
||||
colcon build --packages-select interfaces smacc2_msgs smacc2 brain
|
||||
source install/setup.bash
|
||||
ros2 run brain brain_node
|
||||
```
|
||||
|
||||
## 典型日志观察点
|
||||
| 日志前缀 | 意义 |
|
||||
| -------- | ---- |
|
||||
| [BT][on_start] | 行为树节点开始调度技能 |
|
||||
| [ExecuteBtAction] Dispatch skill= | 小脑收到并调度技能 |
|
||||
| [ExecuteBtAction][feedback] stage=RUN | 正在插值进度 |
|
||||
| [stats] sequences success= | 定期统计输出 |
|
||||
|
||||
## 新增技能快速步骤
|
||||
1. 定义 Action（或 Service）接口并编译。
|
||||
2. 在 YAML 中添加技能条目及接口（`YourSkill.some_pkg/YourAction.action`）。
|
||||
3. traits 中专门化 + 元组加入。
|
||||
4. 运行后自动出现在随机/固定序列候选。
|
||||
|
||||
## 取消支持（Roadmap）
|
||||
目前框架已在 `ActionClientRegistry` 保留 goal handle，将引入：
|
||||
1. RUN 线程检测 cancel 请求（`goal_handle->is_canceling()`）。
|
||||
2. 调用底层 `cancel_goal_async` 或本地标志提前结束并发布 END:FAIL:Canceled。
|
||||
|
||||
## 已完成的重构里程碑
|
||||
| 阶段 | 内容 |
|
||||
| ---- | ---- |
|
||||
| Phase 1 | 统一 ExecuteBtAction 名称/常量、去除 pending 全局状态、拼写修复、事件细分 |
|
||||
| Phase 2 | 超时 + 取消基础、traits 动态分派、统计 topic、结构拆分工具 |
|
||||
| Phase 3 | ExecuteBtAction 结构化反馈 (stage/current_skill/progress/detail) + RUN 阶段插值 |
|
||||
|
||||
1. 自定义统计消息（`brain_msgs/msg/SkillStats.msg`）。
|
||||
2. 反馈中加入“估计剩余时间 / 单技能耗时”。
|
||||
3. 提供远程服务用于动态加载/禁用技能。
|
||||
4. 更精确 goal handle ↔ skill 映射（当前简单扫描 in_flight）。
|
||||
5. 多线程 executor 下互斥完善（现在统计有锁，其它结构可加读写锁）。
|
||||
|
||||
## 兼容性说明
|
||||
本版本修改了 `ExecuteBtAction.action` 字段（Result/Feedback 增强）。外部依赖该 Action 的节点需要重新编译并适配新增字段；旧字段仍保留（success/message），保持最小破坏。
|
||||
|
||||
## 许可证 / 版权
|
||||
示例/内部框架文件保留原版权头，外部依赖库遵循其各自 LICENSE（BehaviorTree.CPP, SMACC2, Boost）。
|
||||
|
||||
## FAQ 精简
|
||||
| 问题 | 可能原因 | 排查 |
|
||||
| ---- | -------- | ---- |
|
||||
| 一直 WAIT | Action Server 未启动或主题命名不匹配 | 检查 topic / `ros2 action list` |
|
||||
| RUN 不出现 | 动作执行极快或在 WAIT 就失败 | 使用真实服务器并延长执行时间 |
|
||||
| progress 不到 1.0 | 未收到 END（节点超时/异常） | 查看 Cerebrum 日志/超时配置 |
|
||||
| 序列提前终止 | `abort_on_failure=true` 且技能失败 | 设为 false 继续剩余技能 |
|
||||
|
||||
---
|
||||
如需进一步扩展（自定义消息、完全取消通道或分布式多进程拆分），可在 Issue / 任务规划中补充需求。
|
||||
|
||||
---
|
||||
## 文档注释规范（Doxygen）
|
||||
|
||||
所有对外（以及大多数非平凡内部）函数需使用统一的 Doxygen 模板块：
|
||||
|
||||
```
|
||||
/**
|
||||
* @brief 使用祈使语气的一句话概要描述。
|
||||
*
|
||||
* 可选的详细说明：为什么需要、输入输出语义、约束/前置条件。
|
||||
* @param foo 参数含义；对显而易见的 getter/setter 可省略。
|
||||
* @param[out] bar 输出参数（仅在函数内被写入时使用）。
|
||||
* @return 返回值语义（void 可省略）。
|
||||
* @thread_safety 并发/线程安全说明（读/写锁策略、无锁、仅主线程等）。
|
||||
* @warning 可能造成误用或隐藏陷阱的情形。
|
||||
* @note 额外提示 / 使用建议。
|
||||
*/
|
||||
```
|
||||
|
||||
编写要点：
|
||||
- 第一行 `@brief` 只写一句话；其后空一行再写补充段落。
|
||||
- 只对“非平凡”参数写说明，避免“@param node 节点对象”这类噪声。
|
||||
- 并发保证统一用 `@thread_safety`（在 `Doxyfile` 里通过 `ALIASES` 高亮）。
|
||||
- 仅当原地修改参数才用 `@param[out]`；否则描述返回对象。
|
||||
- 尽量描述“副作用 / 资源所有权 / 失败条件”，而不是重复类型信息。
|
||||
- 内部复杂 helper 也需要简短说明其不变量或算法思路。
|
||||
- 错误处理策略（返回 false / 抛异常 / 日志警告）尽量写清楚。
|
||||
|
||||
### 代码示例：ExecuteBtAction 反馈主循环
|
||||
|
||||
下列伪代码展示“单技能执行 + 周期性 RUN 反馈”的骨架：
|
||||
|
||||
```cpp
|
||||
// 摘要自 CerebellumNode::ExecuteActionSkill / RunActionSkillWithProgress
|
||||
bool ExecuteActionSkill(skill, topic, spec, detail, index, total, goal_handle) {
|
||||
// START：发布基础进度 (index / total)
|
||||
PublishFeedbackStage(goal_handle, "START", skill, base_progress, "");
|
||||
|
||||
// WAIT：循环等待 Action Server 可用，期间多次发布 WAIT 反馈
|
||||
if (!WaitForActionServer(topic, skill, base_progress, goal_handle, detail)) {
|
||||
PublishFeedbackStage(goal_handle, "END", skill, base_progress, "FAIL:unavailable");
|
||||
return false;
|
||||
}
|
||||
|
||||
// RUN：启动线程执行 dispatch_skill_action(...)；主线程按固定周期插值进度并发 RUN 反馈
|
||||
worker = std::thread([&]{ ok = dispatch_skill_action(...); finished = true; });
|
||||
while (!finished && elapsed < timeout) {
|
||||
progress = interpolate(index, total, elapsed/timeout);
|
||||
PublishFeedbackStage(goal_handle, "RUN", skill, progress, "");
|
||||
sleep(200ms);
|
||||
}
|
||||
worker.join();
|
||||
|
||||
// END：最终发布 OK / FAIL 结论
|
||||
PublishFeedbackStage(goal_handle, "END", skill, final_progress, ok?"OK":"FAIL:..." );
|
||||
return ok;
|
||||
}
|
||||
```
|
||||
|
||||
目的：即便底层具体技能 Action 不提供连续反馈，外部也能得到单调递增的序列级进度，以便 UI / 上层调度展示。
|
||||
|
||||
### 新增已文档化 API 流程
|
||||
1. 修改 / 新增头文件声明（.hpp）。
|
||||
2. 添加上述 Doxygen 注释块（参考现有文件风格）。
|
||||
3. 构建并开启文档：`colcon build --packages-select brain --cmake-args -DBUILD_DOCS=ON`。
|
||||
4. 打开 `build/brain/docs/html/index.html` 检查别名渲染与段落格式。
|
||||
|
||||
### 常见错误
|
||||
- 忘记空行：`@brief` 后没有空行会导致摘要与正文合并。
|
||||
- 重复陈述类型：如 “@param timeout 超时时间（秒）” 可以接受，但不要仅写 “int 超时”。
|
||||
- 忽略线程语义：共享状态更新必须写明锁或原子策略。
|
||||
- 随意换行：超长行适当换行，但保持注释块左右对齐与一致缩进。
|
||||
|
||||
## Doxygen 构建
|
||||
|
||||
启用文档：
|
||||
```bash
|
||||
colcon build --packages-select brain --cmake-args -DBUILD_DOCS=ON
|
||||
```
|
||||
生成目录：`build/brain/docs/html/index.html`。
|
||||
|
||||
最小 GitHub Actions CI 示例：
|
||||
```yaml
|
||||
jobs:
|
||||
docs:
|
||||
runs-on: ubuntu-latest
|
||||
steps:
|
||||
- uses: actions/checkout@v4
|
||||
- name: 安装依赖
|
||||
run: sudo apt-get update && sudo apt-get install -y doxygen graphviz
|
||||
- name: 构建文档
|
||||
run: |
|
||||
colcon build --packages-select brain --cmake-args -DBUILD_DOCS=ON
|
||||
- name: 上传产物
|
||||
uses: actions/upload-artifact@v4
|
||||
with:
|
||||
name: brain-docs
|
||||
path: build/brain/docs/html
|
||||
```
|
||||
|
||||
## CerebrumNode 机制详解
|
||||
|
||||
本节对大脑节点 `CerebrumNode` 的内部结构、行为树调度流程、并发模型与扩展策略做系统阐述，便于快速理解其作为“高层技能序列生成 + 行为树执行协调者”的作用。
|
||||
|
||||
### 1. 角色与目标
|
||||
| 目标 | 说明 |
|
||||
| ---- | ---- |
|
||||
| 序列生成 | 基于参数（固定 / 随机）与白名单筛选技能集合，生成有序技能序列。 |
|
||||
| 行为树封装 | 将技能序列转换为一串 `<SkillName>_H` 的 BT Action 节点 `<Sequence>`。|
|
||||
| 统一下发 | 借助单一 `ExecuteBtAction` 将完整序列（CSV + epoch）一次性发送给 Cerebellum。|
|
||||
| 状态跟踪 | 为每个 BT 节点维护 in_flight / result / progress / 超时。|
|
||||
| 反馈接入 | 监听 ExecuteBtAction 反馈与结果，更新节点执行状态并保持进度单调。|
|
||||
| Epoch 隔离 | 通过 epoch + 技能集合过滤陈旧反馈，避免跨序列污染。|
|
||||
| 动态重建 | 定时或显式服务触发重建行为树（可配置是否允许执行期间重建）。|
|
||||
|
||||
### 2. 关键数据结构
|
||||
| 成员 | 说明 |
|
||||
| ---- | ---- |
|
||||
| `action_registry_` | 统一 ActionClient 注册与发送封装。|
|
||||
| `bt_registry_` | 行为树节点 & 序列工厂注册构建器。|
|
||||
| `skill_manager_` | 解析与缓存技能定义（名称→接口列表等）。|
|
||||
| `per_node_exec_` | `bt_type` → 执行跟踪结构（开始时间、进度、结果码、是否还在运行）。|
|
||||
| `current_sequence_skills_` | 当前选定技能名称有序列表（受互斥锁保护）。|
|
||||
| `active_sequence_` | 已在 BT 中激活的序列标识（“FixedSequence”/“VLMSequence”）。|
|
||||
| `epoch_filter_` | 维护当前 epoch 与技能集合，供反馈过滤。|
|
||||
| `bt_pending_run_` | 标记当前行为树是否需要继续 tick。|
|
||||
| `bt_updating_` | 原子布尔，防止重入的 BT 重建。|
|
||||
|
||||
`PerNodeExecInfo` 关键字段：
|
||||
| 字段 | 用途 |
|
||||
| ---- | ---- |
|
||||
| `in_flight` | 节点是否处于 RUNNING（尚未结束）。|
|
||||
| `result` / `result_code` | 最终布尔与分类结果（SUCCESS/FAILURE/CANCELLED/UNKNOWN）。|
|
||||
| `start_time` | 用于节点级超时判定。|
|
||||
| `last_progress` | 保持反馈进度单调，过滤倒退。|
|
||||
|
||||
### 3. 参数与优先级
|
||||
| 参数 | 影响 |
|
||||
| ---- | ---- |
|
||||
| `skills_file` | 技能 YAML 路径；空则回退默认 share 下文件。|
|
||||
| `node_timeout_sec` | 单个 BT 节点（技能代理）最大运行秒数。|
|
||||
| `allow_bt_rebuild_during_execution` | 执行中是否允许重建（否则跳过定时重建）。|
|
||||
| `allowed_action_skills` | 白名单（逗号/分号/空格）；非空则仅保留匹配技能。|
|
||||
| `fixed_skill_sequence` | 固定序列优先于随机；解析后若非空直接使用。|
|
||||
| `random_skill_pick_count` | 随机模式下最多选择技能数。|
|
||||
|
||||
优先级：固定序列存在 → 忽略随机；白名单仅影响随机候选池；`node_timeout_sec` 对所有 BT 节点统一生效。
|
||||
|
||||
### 4. 生命周期与时序
|
||||
1. 构造：声明参数 → 加载技能文件 → 注册全部技能 BT 节点（`RegisterSkillBtActions`）→ 注册 ExecuteBtAction 客户端（`RegisterActionClient`）→ 启动两个定时器：
|
||||
- `bt_timer_` (10ms)：`ExecuteBehaviorTree()` 持续 tick root（仅在 `bt_pending_run_` 为真）。
|
||||
- `task_timer_` (5s)：`CerebrumTask()` 触发一次序列生成 + 重建流程。
|
||||
2. 重建服务：`/cerebrum/rebuild_now` 外部调用时执行：模型（选序列）→ 取消旧 goal → 重建 BT → 触发新一轮 tick。
|
||||
3. 停止：BT root 返回 SUCCESS/FAILURE 时重置 `bt_pending_run_`。
|
||||
|
||||
### 5. 行为树节点注册机制
|
||||
`RegisterSkillBtActions` 为每个技能创建 `<SkillName>_H` 类型：
|
||||
| 生命周期回调 | 逻辑 |
|
||||
| ------------- | ---- |
|
||||
| `on_start` | 初始化节点跟踪结构；如果是序列第一个节点且 ExecuteBtAction server 可用 → 发送统一 goal。|
|
||||
| `on_running` | 检查节点是否超时（`elapsed > node_timeout_sec_` 则 FAIL）；否则保持 RUNNING。|
|
||||
| `on_halted` | 目前仅日志记录。|
|
||||
|
||||
这样一个 `<Sequence>` 中各节点只在第一次 `on_start` 时触发统一 goal（其余节点等待反馈驱动完成）。
|
||||
|
||||
### 6. 序列生成策略
|
||||
函数 `RunVlmModel()`：
|
||||
1. 若存在 `fixed_skill_sequence_` → 包装为 `FixedSequence` 注册/激活。
|
||||
2. 否则调用 `PickRandomActionSkills(count)`：
|
||||
- 遍历 `skill_manager_->skills()`，过滤无 `.action` 接口技能。
|
||||
- 若白名单非空再过滤非白名单项。
|
||||
- 使用 `std::shuffle` 打乱，截断到 `random_skill_pick_count_`。
|
||||
3. 将结果转为 `BTSequenceSpec` (`MakeSequenceFromSkills`) 并通过 `RegisterBtSequence` + `SetActiveSequence` 激活。
|
||||
|
||||
### 7. CSV 序列与 Goal 构造
|
||||
`RegisterActionClient` 中 Goal builder 回调：
|
||||
| 步骤 | 描述 |
|
||||
| ---- | ---- |
|
||||
| 获取 epoch | `epoch_filter_.epoch()`（当前行为树有效窗口）。|
|
||||
| 加锁复制 `current_sequence_skills_` | 确保与重建互斥。|
|
||||
| 组装 CSV | 以逗号拼接技能名放入 `goal.action_name`。|
|
||||
| 返回 Goal | 统一发送给 Cerebellum 执行。|
|
||||
|
||||
### 8. 反馈与结果处理
|
||||
| 回调 | 作用 |
|
||||
| ---- | ---- |
|
||||
| Feedback | 验证 `epoch_filter_.accept(epoch, skill, bt_pending_run_)`；匹配到 `per_node_exec_` 中对应 skill（通过 `skill_name`）更新 `last_progress`、`result`、`result_code`。|
|
||||
| Result | 汇总成功/失败；为仍 `in_flight` 的节点补完最终结果（保证 BT 节点能完成）。|
|
||||
|
||||
进度单调：若收到 RUN 且 progress < `last_progress`（浮点微差外）则忽略，防倒退。
|
||||
|
||||
### 9. Epoch 过滤机制
|
||||
重建行为树 (`UpdateBehaviorTree`) 时：
|
||||
1. increment epoch。
|
||||
2. 捕获当前序列技能列表并调用 `epoch_filter_.reset_epoch(new_epoch, seq_skills)`。
|
||||
3. 清空 `per_node_exec_`，置 `bt_pending_run_ = true`。
|
||||
|
||||
反馈只要 epoch 或技能名不在集合内 → 丢弃。
|
||||
|
||||
### 10. per_node_exec_ & 超时
|
||||
`on_start` 填充初值；`on_running` 里比较 `now - start_time` 与 `node_timeout_sec_`。到期：设置 `result=false` / `result_code=FAILURE` 返回 BT FAILURE，使 Sequence 终止（或由上层策略处理）。
|
||||
|
||||
### 11. 并发与线程安全
|
||||
| 场景 | 处理 |
|
||||
| ---- | ---- |
|
||||
| 序列列表写入 | `std::lock_guard<std::mutex> lk(exec_mutex_)` 在生成/更新时保护。|
|
||||
| 反馈更新节点状态 | 同一互斥锁保护遍历与写入 `per_node_exec_`。|
|
||||
| 行为树重建重入 | `bt_updating_` 原子布尔 + RAII guard。|
|
||||
| 运行标志 | `bt_pending_run_` 原子控制 tick。|
|
||||
| epoch 计数 | 通过 `epoch_filter_`（内部原子/轻量结构）。|
|
||||
|
||||
### 12. 错误与失败处理
|
||||
| 情况 | 行为 |
|
||||
| ---- | ---- |
|
||||
| ExecuteBtAction server 不可用（首节点） | 立即标记该节点 FAIL，行为树 Sequence 失败。|
|
||||
| 节点超时 | `on_running` 返回 FAILURE。|
|
||||
| 反馈缺失 | 节点会持续 RUNNING 直至超时或动作整体 result 回调补发。|
|
||||
| 重建时旧反馈 | 被 epoch_filter 拦截。|
|
||||
|
||||
### 13. 扩展：新增技能
|
||||
1. 在 YAML 添加技能（含至少一个 `.action` 接口）。
|
||||
2. 编译生成对应接口 Action；被 `SkillManager` 自动发现并注册客户端。
|
||||
3. 重建（定时或服务）后自动出现在随机候选；行为树节点自动可用。无需修改 CerebrumNode 核心代码。
|
||||
|
||||
### 14. 常用辅助函数梳理
|
||||
| 函数 | 作用 |
|
||||
| ---- | ---- |
|
||||
| `PickRandomActionSkills` | 随机挑选动作技能池。|
|
||||
| `MakeSequenceFromSkills` | 技能名 → `BTSequenceSpec`。|
|
||||
| `BuildXmlFromSequence` | 诊断展示生成 XML（调试用）。|
|
||||
| `ParseListString` | 解析参数列表（逗号/空格/分号）。|
|
||||
| `ParseResultDetail` | detail 字符串 → `ExecResultCode`。|
|
||||
| `CancelActiveExecuteBtGoal` | 取消当前统一执行 goal。|
|
||||
|
||||
### 15. 示例执行时间线
|
||||
```
|
||||
T0 定时器触发 CerebrumTask → 运行 RunVlmModel 选序列 [A,B]
|
||||
T0+ CancelActiveExecuteBtGoal (无旧 goal) → UpdateBehaviorTree → epoch=12
|
||||
T0+ bt_pending_run_=true → 10ms 定时器开始 tick root
|
||||
T1 节点 A on_start → 发送 ExecuteBtAction(goal: "A,B", epoch=12)
|
||||
T1+ 等待 Cerebellum 反馈；A on_running 维持 RUNNING
|
||||
T2 收到 A END OK 反馈 → A 节点成功 → BT 继续到 B
|
||||
T3 B on_start → 已是后续节点，不再发送 goal（沿用同一次序列）
|
||||
T4 收到 B END FAIL → Sequence 失败 → root 返回 FAILURE → bt_pending_run_=false
|
||||
```
|
||||
|
||||
### 16. 设计取舍与理由
|
||||
| 需求 | 方案 | 原因 | 代价 |
|
||||
| ---- | ---- | ---- | ---- |
|
||||
| 低耦合技能列表变更 | CSV+epoch 单一 Action 传输 | 简化网络/接口数量 | 需解析/过滤反馈 |
|
||||
| 行为树可观测性 | per_node_exec_ 状态表 | 易日志与调试 | 需互斥锁维护 |
|
||||
| 反馈抗陈旧 | epoch + 技能集合校验 | 简洁快速 | 跨 epoch 丢弃旧技能晚到反馈 |
|
||||
| 最小侵入扩展 | 自动注册 BT 节点 | 新技能零修改主逻辑 | 节点命名规则固定 `_H` |
|
||||
| 进度单调 | 过滤回退 progress | 避免 UI 抖动 | 不显示真实回退场景 |
|
||||
|
||||
### 17. 后续增强
|
||||
1. 更细粒度的 per-node 状态（排队、等待 server、执行中、被取消）。
|
||||
2. 结合行为树黑板记录节点执行统计历史（平均时长、成功率）。
|
||||
3. 支持多序列并行（需要多 Action 或自定义 goal id 映射）。
|
||||
4. 将 per_node_exec_ 暴露为诊断 topic（如 `/brain/cerebrum_nodes`）。
|
||||
5. 插入 “预处理 / 过滤” 节点（如安全约束、资源仲裁）。
|
||||
|
||||
---
|
||||
|
||||
## CerebellumNode 机制详解
|
||||
|
||||
本节全面描述小脑节点（`CerebellumNode`）的内部结构、执行路径、并发模型与统计/反馈策略。它承担“具体技能执行器 + 序列反馈聚合 + 统计汇总”三大角色，是大脑行为树（Cerebrum）与底层原子技能 Action/Service 之间的桥梁。
|
||||
|
||||
### 1. 组件职责分层
|
||||
|
||||
| 层次 | 责任 | Cerebellum 侧落点 |
|
||||
| ---- | ---- | ----------------- |
|
||||
| 序列入口 | 接收统一 ExecuteBtAction 目标（包含技能 CSV + epoch） | `SetupExecuteBtServer` + `RunExecuteBtGoal` |
|
||||
| 技能解析 | 拆解 CSV，校验技能定义 | `ValidateAndParseGoal` / `ParseSkillSequence` |
|
||||
| 超时决策 | 计算单技能执行窗口 | `GetTimeoutForSkill` + skill_timeouts_ map |
|
||||
| 动作执行 | 通过注册的 ActionClient 发送并等待 | `ExecuteActionSkill` / `RunActionSkillWithProgress` |
|
||||
| 服务执行 | 同步调用支持的服务接口 | `ExecuteServiceSkill` |
|
||||
| 反馈生成 | START / WAIT / RUN / END 四阶段发布 | `PublishFeedbackStage` + 各执行函数内循环 |
|
||||
| 统计汇总 | 成功/失败计数 & 序列聚合 | `RecordSkillResult` / `LogStatsPeriodic` |
|
||||
| 终止策略 | 失败是否提前中断整序列 | `abort_on_failure_` 在 `RunExecuteBtGoal` 中判定 |
|
||||
| 结果合成 | 序列总结 message 组织 | `FinalizeExecuteBtResult` |
|
||||
|
||||
### 2. 生命周期概览
|
||||
|
||||
1. 节点启动：构造函数中初始化 ActionServerRegistry / ActionClientRegistry / SkillManager；声明参数并加载技能 YAML；创建统计定时器；注册 ExecuteBtAction 服务端；可选启动 SMACC2 状态机。
|
||||
2. 接收 Goal：`on_goal` 回调根据 epoch 递增规则决定拒绝/接受；触发状态机事件 `EvGoalReceived`。
|
||||
3. 执行序列：`RunExecuteBtGoal` 解析技能列表，循环按顺序逐个执行；每个技能内包含等待 server、执行、反馈插值、结束判定。
|
||||
4. 更新统计：每个技能结束后调用 `RecordSkillResult`；整体结果更新序列成败计数。
|
||||
5. 发布结果：`FinalizeExecuteBtResult` 汇总 message（含每技能 OK/FAIL 及 detail），调用 succeed/abort。
|
||||
6. 定期输出：`LogStatsPeriodic` 定时器发布/打印全局统计串。
|
||||
|
||||
### 3. ExecuteBtAction 反馈语义细化
|
||||
|
||||
| 阶段 | 触发点 | 进度计算 | detail 示例 | 失败路径 |
|
||||
| ---- | ------ | -------- | ----------- | -------- |
|
||||
| START | 技能进入执行逻辑前 | i / N | 空 | - |
|
||||
| WAIT | Server 每次不可用重试 | 固定基线 (i / N) | attempt=1..k | 若超出重试次数直接 END=FAIL |
|
||||
| RUN | 已发送 Action 并在超时窗口内 | i + 子进度 / N (上限 < (i+1)/N) | 空或插值内部不填 | 超时后线程结束，END=FAIL |
|
||||
| END | 技能成功/失败完成 | (i+1)/N | OK / OK:found pose / FAIL:reason | 已最终化 |
|
||||
|
||||
设计目标：即便底层 Action 本身不推送 feedback，也能通过 RUN 插值为上层（Cerebrum / UI）提供平滑进度感知。
|
||||
|
||||
### 4. 单技能动作执行时序
|
||||
|
||||
```
|
||||
ExecuteActionSkill
|
||||
├─ START 反馈
|
||||
├─ WaitForActionServer (循环 WAIT 反馈 / 最多 8 次)
|
||||
├─ 计算超时窗口 timeout_sec
|
||||
└─ RunActionSkillWithProgress
|
||||
├─ 启动工作线程 dispatch_skill_action (阻塞发送+等待结果)
|
||||
├─ 主线程 200ms 周期：计算 phase = elapsed/timeout，发布 RUN 反馈
|
||||
├─ 工作线程返回：设置 finished
|
||||
└─ 结束汇总 local_detail → 返回 ok
|
||||
```
|
||||
|
||||
`dispatch_skill_action` 利用模板递归在 `SkillActionTypes` 元组中匹配技能名并调用对应类型安全的 `send_and_wait`，抽象出不同 Action 的 goal / result 字段差异。
|
||||
|
||||
### 5. 服务型技能执行
|
||||
|
||||
目前示例仅支持 `VisionObjectRecognition`：
|
||||
1. 检索 typed service client；等待可用（2s）。
|
||||
2. 发送请求并同步等待（5s 限时）。
|
||||
3. 根据 `found` 字段判定成功与 detail（`found pose` / `Not found`）。
|
||||
4. 统一通过 START → END 两阶段反馈（无 RUN 插值）。
|
||||
|
||||
可扩展思路：抽象 `ServiceSkillTrait`，与 Action 分派类似以支持多服务类型。
|
||||
|
||||
### 6. 超时与重试策略
|
||||
|
||||
| 粒度 | 策略 | 相关函数 / 参数 |
|
||||
| ---- | ---- | --------------- |
|
||||
| Server 可用性 | 8 次重试，250ms 间隔，50ms is_server_available 等待 | `WaitForActionServer` |
|
||||
| 动作执行 | 线性插值 RUN，超过 timeout 判 FAIL | `RunActionSkillWithProgress` + `GetTimeoutForSkill` |
|
||||
| 服务执行 | 单次 wait_for_service(2s) + spin_until_future(5s) | `ExecuteServiceSkill` |
|
||||
| 动作超时表 | skill_timeouts_ > vision_grasp_object_timeout > default | `GetTimeoutForSkill` |
|
||||
|
||||
### 7. 并发与线程安全
|
||||
|
||||
| 数据 / 操作 | 并发措施 | 说明 |
|
||||
| ------------ | -------- | ---- |
|
||||
| 统计计数 (success/fail maps) | `std::mutex stats_mutex_` | 在 `RecordSkillResult` 与 `LogStatsPeriodic` 加锁 |
|
||||
| 进度反馈发布 | 单线程主执行循环调用 | worker 线程只设原子 flag，不直接 publish |
|
||||
| 工作线程完成信号 | `std::atomic<bool> finished` | 主循环轮询 200ms |
|
||||
| epoch 切换 | `current_epoch_` (atomic) | goal on_goal 时递增；反馈携带 epoch 供上层过滤 |
|
||||
| 行为树并发 | 不在 Cerebellum 内创建多 executor 线程 | 由外层 ROS 2 executor 控制 |
|
||||
|
||||
当前不对 `action_clients_` 做细粒度锁，假设注册阶段已完成且执行期只读；若未来动态增加/卸载客户端需引入 RW 锁或线程安全封装。
|
||||
|
||||
### 8. 失败分类与消息合成
|
||||
|
||||
序列结果消息格式示例：
|
||||
```
|
||||
Seq{ArmSpaceControl,VisionGraspObject}: ArmSpaceControl(OK)[pose reached], VisionGraspObject(FAIL)[Not found]
|
||||
```
|
||||
组装逻辑：循环累积 `skill(OK|FAIL)[detail]`，失败时在日志打印并依据 `abort_on_failure_` 决定是否跳出。
|
||||
|
||||
### 9. 与 SMACC2 的交互
|
||||
|
||||
若未定义 `BRAIN_DISABLE_SM`：
|
||||
- 接收新 goal → `EvGoalReceived`。
|
||||
- 单技能成功 → `EvExecutionFinished`。
|
||||
- 单技能失败 → `EvExecutionFailed`。
|
||||
|
||||
状态机可据此进行更高层的恢复、统计或模式切换（例如失败后进入恢复状态）。当前实现保持事件投递简单，不阻塞执行主线。
|
||||
|
||||
### 10. 可扩展方向
|
||||
|
||||
1. 真正的取消支持：在 RUN 循环检查 `goal_handle->is_canceling()`，触发底层 `cancel_goal_async`，并发布 END:FAIL:Canceled。
|
||||
2. 细粒度反馈：透传底层 Action 自身的 feedback 字段（若存在），与插值融合。
|
||||
3. 统计类型升级：发布结构化自定义消息（包含平均耗时、成功率、最近窗口数据）。
|
||||
4. 多并发序列：支持多个 ExecuteBtAction 并行（需拆分 epoch / 反馈通道或使用 goal UUID 区分）。
|
||||
5. 失败重试策略：对特定技能定义最大重试次数与指数退避。
|
||||
6. 服务技能抽象：与 Action 一致的 trait + 统一执行包装。
|
||||
7. 观测指标：Prometheus exporter 暴露计数与耗时直方图。
|
||||
|
||||
### 11. 关键函数对照表
|
||||
|
||||
| 函数 | 作用概述 |
|
||||
| ---- | -------- |
|
||||
| `SetupExecuteBtServer` | 注册 ExecuteBtAction 服务端及回调 |
|
||||
| `RunExecuteBtGoal` | 序列主调度循环：技能遍历、执行、统计、结果聚合 |
|
||||
| `ValidateAndParseGoal` | 校验 goal 与解析技能 CSV |
|
||||
| `ParseSkillSequence` | 分隔符拆分 + 去 `_H` 后缀 |
|
||||
| `ExecuteSingleSkill` | 统一入口，分派到 Action 或 Service 执行路径 |
|
||||
| `ExecuteActionSkill` | 包装启动、等待 server、构造超时、调用运行函数 |
|
||||
| `WaitForActionServer` | Server 可用性重试 + WAIT 反馈 |
|
||||
| `RunActionSkillWithProgress` | 启动行动作线程 + RUN 插值反馈循环 |
|
||||
| `ExecuteServiceSkill` | 同步服务调用 + START/END 反馈 |
|
||||
| `PublishFeedbackStage` | 构造并发送反馈消息对象 |
|
||||
| `GetTimeoutForSkill` | 计算最终超时（优先级链） |
|
||||
| `RecordSkillResult` | 原子更新 per-skill 成败计数 |
|
||||
| `LogStatsPeriodic` | 周期发布/打印统计字符串 |
|
||||
| `FinalizeExecuteBtResult` | 组织 result message & succeed/abort |
|
||||
|
||||
### 12. 典型执行示例（时间线）
|
||||
|
||||
```
|
||||
T0 : Goal 接收 (epoch=5) → 接受并进入 RunExecuteBtGoal
|
||||
T0+ : 解析技能: [ArmSpaceControl, VisionGraspObject]
|
||||
T1 : ArmSpaceControl START
|
||||
T1+ : ArmSpaceControl WAIT attempt=1..n (server 暂不可用)
|
||||
T2 : ArmSpaceControl RUN (周期进度上升 0.0→~0.49)
|
||||
T4 : ArmSpaceControl END OK (进度=0.5)
|
||||
T4+ : VisionGraspObject START
|
||||
T5 : VisionGraspObject RUN（若是 Action；当前示例为 Service 则直接调用）
|
||||
T6 : VisionGraspObject END FAIL:Not found (进度=1.0)
|
||||
T6+ : 序列结果汇总 & 发布 Result(success=false)
|
||||
T6+ : 统计串更新 & 定时器下一周期打印
|
||||
```
|
||||
|
||||
### 13. 设计取舍
|
||||
|
||||
| 目标 | 方案 | 取舍 |
|
||||
| ---- | ---- | ---- |
|
||||
| 统一入口 | ExecuteBtAction 聚合 | 简化上层调度；需解析 CSV/epoch |
|
||||
| 平滑进度 | RUN 插值 + 超时窗口 | 不依赖底层 feedback；进度为近似值 |
|
||||
| 易扩展 | trait 模板分派 | 新增类型需专门化 trait |
|
||||
| 统计简洁 | 文本串发布 | 解析不结构化；后续升级定制 msg |
|
||||
| 线程简单 | 单 worker + 轮询 | 200ms 粒度有限；高精度需事件驱动 |
|
||||
| 超时灵活 | 三层优先级 + map | map 维护需参数管理规范 |
|
||||
|
||||
---
|
||||
33
src/brain/config/robot_skills.yaml
Normal file
33
src/brain/config/robot_skills.yaml
Normal file
@@ -0,0 +1,33 @@
|
||||
- name: VisionObjectRecognition
|
||||
version: 1.0.0
|
||||
description: "视觉识别指定物体"
|
||||
interfaces:
|
||||
- VisionObjectRecognition.service
|
||||
|
||||
- name: ArmSpaceControl
|
||||
version: 1.0.0
|
||||
description: "手臂空间控制"
|
||||
interfaces:
|
||||
- ArmSpaceControl.action
|
||||
|
||||
- name: HandControl
|
||||
version: 1.0.0
|
||||
description: "手部控制"
|
||||
interfaces:
|
||||
- HandControl.action
|
||||
|
||||
- name: LegControl
|
||||
version: 1.0.0
|
||||
description: "腿部控制"
|
||||
interfaces:
|
||||
- LegControl.action
|
||||
|
||||
- name: VisionGraspObject
|
||||
version: 1.0.0
|
||||
description: "视觉识别并抓取指定物体"
|
||||
required_skills:
|
||||
- VisionObjectRecognition
|
||||
- ArmSpaceControl
|
||||
- HandControl
|
||||
interfaces:
|
||||
- VisionGraspObject.action
|
||||
386
src/brain/include/brain/action_registry.hpp
Normal file
386
src/brain/include/brain/action_registry.hpp
Normal file
@@ -0,0 +1,386 @@
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
#include <future>
|
||||
#include <optional>
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
/**
|
||||
* @file action_registry.hpp
|
||||
* @brief Utilities for registering multiple ROS 2 action servers and clients via simple name lookups.
|
||||
*
|
||||
* This header defines two registry classes:
|
||||
* - ActionServerRegistry: Register and own multiple action servers via a uniform handler bundle.
|
||||
* - ActionClientRegistry: Register and trigger multiple action clients by name with reduced boilerplate.
|
||||
*
|
||||
* Design goals:
|
||||
* - Reduce repetitive action server/client setup code inside nodes.
|
||||
* - Provide a consistent pattern (name -> handlers) for adding new action types.
|
||||
* - Ensure lifetime safety by storing shared pointers inside the registry objects.
|
||||
*
|
||||
* Notes:
|
||||
* - Both registries are independent; either can be used alone.
|
||||
* - The logical name should normally match the action topic (e.g. "move_to_position").
|
||||
* - For servers, if only an execute functor is provided (no custom on_accepted), a worker thread is spawned.
|
||||
*
|
||||
* Thread-safety:
|
||||
* - Registration methods are expected to be called during initialization (single-threaded).
|
||||
* - Sending goals / cancelling may be invoked concurrently; underlying rclcpp action APIs are thread-safe.
|
||||
*/
|
||||
|
||||
class ActionServerRegistry
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a server registry bound to a node.
|
||||
* @param node Owning rclcpp node (not owned).
|
||||
*/
|
||||
explicit ActionServerRegistry(rclcpp::Node * node)
|
||||
: node_(node) {}
|
||||
|
||||
/**
|
||||
* @brief Bundle of callbacks used to configure an action server.
|
||||
* @tparam ActionT Action type.
|
||||
*
|
||||
* Provide either (on_accepted) OR (execute). If on_accepted is empty and execute is set,
|
||||
* the registry spawns a detached thread invoking execute() so it does not block the executor.
|
||||
*/
|
||||
template<typename ActionT>
|
||||
struct Handlers
|
||||
{
|
||||
using Goal = typename ActionT::Goal;
|
||||
using GoalHandle = rclcpp_action::ServerGoalHandle<ActionT>;
|
||||
/** @brief New goal request handler; return ACCEPT_AND_EXECUTE, REJECT, etc. */
|
||||
std::function<rclcpp_action::GoalResponse(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const Goal>)> on_goal;
|
||||
/** @brief Cancellation request handler (optional). Return ACCEPT / REJECT. */
|
||||
std::function<rclcpp_action::CancelResponse(const std::shared_ptr<GoalHandle>)> on_cancel;
|
||||
/** @brief Handler invoked immediately after a goal is accepted (optional). */
|
||||
std::function<void(const std::shared_ptr<GoalHandle>)> on_accepted; ///< Optional
|
||||
/** @brief Execute callback (optional, used if on_accepted is omitted). */
|
||||
std::function<void(const std::shared_ptr<GoalHandle>)> execute; ///< Optional
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Register (create and retain) an action server instance.
|
||||
* @tparam ActionT Action type.
|
||||
* @param name Action topic name.
|
||||
* @param handlers Callback bundle.
|
||||
* @thread_safety Call during initialization; not thread-safe with concurrent registration.
|
||||
*/
|
||||
template<typename ActionT>
|
||||
void register_action_server(const std::string & name, const Handlers<ActionT> & handlers)
|
||||
{
|
||||
holders_.push_back(std::make_unique<Holder<ActionT>>(node_, name, handlers));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_;
|
||||
struct IHolder { virtual ~IHolder() = default; };
|
||||
// Keep concrete holders alive via base pointer
|
||||
std::vector<std::unique_ptr<IHolder>> holders_;
|
||||
|
||||
template<typename AT>
|
||||
struct Holder : IHolder
|
||||
{
|
||||
using GoalHandle = rclcpp_action::ServerGoalHandle<AT>;
|
||||
typename rclcpp_action::Server<AT>::SharedPtr server_;
|
||||
Holder(rclcpp::Node * node, const std::string & name, const Handlers<AT> & h)
|
||||
{
|
||||
auto goal_cb = [h](const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const typename AT::Goal> goal) {
|
||||
return h.on_goal ? h.on_goal(
|
||||
uuid,
|
||||
goal) : rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
auto cancel_cb = [h](const std::shared_ptr<GoalHandle> gh) {
|
||||
return h.on_cancel ? h.on_cancel(gh) : rclcpp_action::CancelResponse::ACCEPT;
|
||||
};
|
||||
auto accepted_cb = [h](const std::shared_ptr<GoalHandle> gh) {
|
||||
if (h.on_accepted) {
|
||||
h.on_accepted(gh);
|
||||
} else if (h.execute) {
|
||||
std::thread{[h, gh]() {h.execute(gh);}}.detach();
|
||||
}
|
||||
};
|
||||
server_ = rclcpp_action::create_server<AT>(node, name, goal_cb, cancel_cb, accepted_cb);
|
||||
}
|
||||
};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Registry to manage and trigger multiple action clients by name.
|
||||
*
|
||||
* Provides simplified registration with goal construction and callback wiring, as well as
|
||||
* convenience methods to send, cancel, wait for server availability, and synchronously
|
||||
* wait for a result (with timeout and optional cancellation).
|
||||
*/
|
||||
class ActionClientRegistry
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct a client registry bound to a node.
|
||||
* @param node Owning rclcpp node (not owned).
|
||||
*/
|
||||
explicit ActionClientRegistry(rclcpp::Node * node)
|
||||
: node_(node) {}
|
||||
|
||||
template<typename ActionT>
|
||||
using GoalHandle = rclcpp_action::ClientGoalHandle<ActionT>;
|
||||
|
||||
/**
|
||||
* @brief Abstract base for stored client entries (type-erased).
|
||||
*/
|
||||
struct EntryBase
|
||||
{
|
||||
virtual ~EntryBase() = default;
|
||||
/** @brief Send a goal using the entry's callbacks. */
|
||||
virtual void send(rclcpp::Logger logger) = 0;
|
||||
/** @brief Check if server is available within timeout. */
|
||||
virtual bool available(std::chrono::nanoseconds timeout) const = 0;
|
||||
/** @brief Attempt to cancel last goal if present. */
|
||||
virtual void cancel(rclcpp::Logger logger) = 0;
|
||||
};
|
||||
|
||||
template<typename ActionT>
|
||||
struct Entry : EntryBase
|
||||
{
|
||||
using Goal = typename ActionT::Goal;
|
||||
typename rclcpp_action::Client<ActionT>::SharedPtr client; ///< Underlying client instance
|
||||
std::weak_ptr<GoalHandle<ActionT>> last_goal_handle; ///< Last goal handle (weak)
|
||||
std::function<Goal()> make_goal; ///< Goal builder
|
||||
std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> on_goal_response; ///< Optional goal response
|
||||
std::function<void(typename GoalHandle<ActionT>::SharedPtr,
|
||||
const std::shared_ptr<const typename ActionT::Feedback>)> on_feedback; ///< Optional feedback
|
||||
std::function<void(const typename GoalHandle<ActionT>::WrappedResult &)> on_result; ///< Optional result
|
||||
std::string name; ///< Action name/topic
|
||||
|
||||
Entry(
|
||||
rclcpp::Node * node,
|
||||
const std::string & name,
|
||||
std::function<Goal()> mk,
|
||||
std::function<void(typename GoalHandle<ActionT>::SharedPtr,
|
||||
const std::shared_ptr<const typename ActionT::Feedback>)> fb,
|
||||
std::function<void(const typename GoalHandle<ActionT>::WrappedResult &)> rc,
|
||||
std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> gr)
|
||||
: make_goal(std::move(mk)), on_goal_response(std::move(gr)), on_feedback(std::move(fb)),
|
||||
on_result(std::move(rc)), name(name)
|
||||
{
|
||||
client = rclcpp_action::create_client<ActionT>(node, name);
|
||||
}
|
||||
|
||||
void send(rclcpp::Logger logger) override
|
||||
{
|
||||
if (!client) {return;}
|
||||
RCLCPP_DEBUG(logger, "[ActionClientRegistry] waiting for server '%s'", name.c_str());
|
||||
if (!client->wait_for_action_server(std::chrono::seconds(10))) {
|
||||
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
|
||||
return;
|
||||
}
|
||||
auto goal_msg = make_goal ? make_goal() : Goal{};
|
||||
RCLCPP_INFO(logger, "[ActionClientRegistry] sending goal on '%s'", name.c_str());
|
||||
|
||||
auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
|
||||
if (on_goal_response) {opts.goal_response_callback = on_goal_response;}
|
||||
if (on_feedback) {opts.feedback_callback = on_feedback;}
|
||||
if (on_result) {opts.result_callback = on_result;}
|
||||
auto future_handle = client->async_send_goal(goal_msg, opts);
|
||||
std::thread(
|
||||
[future_handle, this, logger]() mutable {
|
||||
if (future_handle.wait_for(std::chrono::seconds(0)) != std::future_status::ready) {
|
||||
future_handle.wait();
|
||||
}
|
||||
try {
|
||||
auto gh = future_handle.get();
|
||||
if (gh) {
|
||||
last_goal_handle = gh;
|
||||
RCLCPP_DEBUG(logger, "Stored goal handle for '%s'", name.c_str());
|
||||
}
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_WARN(
|
||||
logger, "Failed to obtain goal handle for '%s': %s", name.c_str(),
|
||||
e.what());
|
||||
}
|
||||
}).detach();
|
||||
}
|
||||
|
||||
bool available(std::chrono::nanoseconds timeout) const override
|
||||
{
|
||||
if (!client) {return false;}
|
||||
return client->wait_for_action_server(timeout);
|
||||
}
|
||||
|
||||
void cancel(rclcpp::Logger logger) override
|
||||
{
|
||||
auto gh = last_goal_handle.lock();
|
||||
if (!gh) {
|
||||
RCLCPP_WARN(logger, "No active goal handle to cancel for '%s'", name.c_str());
|
||||
return;
|
||||
}
|
||||
RCLCPP_INFO(logger, "Cancelling goal on '%s'", name.c_str());
|
||||
client->async_cancel_goal(gh);
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Register an action client and associated callbacks.
|
||||
* @tparam ActionT Action type.
|
||||
* @param name Action topic name.
|
||||
* @param make_goal Goal construction functor.
|
||||
* @param on_feedback Optional feedback callback.
|
||||
* @param on_result Optional result callback.
|
||||
* @param on_goal_response Optional goal response callback.
|
||||
* @thread_safety Call during initialization or in a controlled context; not thread-safe with concurrent registration.
|
||||
*/
|
||||
template<typename ActionT>
|
||||
void register_client(
|
||||
const std::string & name,
|
||||
std::function<typename ActionT::Goal()> make_goal,
|
||||
std::function<void(typename GoalHandle<ActionT>::SharedPtr, const std::shared_ptr<const typename ActionT::Feedback>)> on_feedback,
|
||||
std::function<void(const typename GoalHandle<ActionT>::WrappedResult &)> on_result,
|
||||
std::function<void(const std::shared_ptr<GoalHandle<ActionT>> &)> on_goal_response = nullptr)
|
||||
{
|
||||
entries_[name] =
|
||||
std::make_unique<Entry<ActionT>>(
|
||||
node_, name, std::move(make_goal), std::move(
|
||||
on_feedback), std::move(on_result), std::move(on_goal_response));
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send a goal on a registered client by name.
|
||||
* @param name Action client key.
|
||||
* @param logger Logger instance.
|
||||
*/
|
||||
void send(const std::string & name, rclcpp::Logger logger)
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {
|
||||
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
|
||||
return;
|
||||
}
|
||||
RCLCPP_DEBUG(logger, "Sending action '%s'", name.c_str());
|
||||
it->second->send(logger);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cancel the last goal sent on a registered client (if any).
|
||||
* @param name Action client key.
|
||||
* @param logger Logger instance.
|
||||
*/
|
||||
void cancel(const std::string & name, rclcpp::Logger logger)
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {
|
||||
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
|
||||
return;
|
||||
}
|
||||
it->second->cancel(logger);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Wait for a server to become available on a registered client.
|
||||
* @param name Action client key.
|
||||
* @param timeout Maximum wait duration.
|
||||
* @return true if available within timeout.
|
||||
*/
|
||||
bool wait_for_server(const std::string & name, std::chrono::nanoseconds timeout)
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {return false;}
|
||||
return it->second->available(timeout);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check whether a client has been registered.
|
||||
* @param name Action client key.
|
||||
* @return true if present.
|
||||
*/
|
||||
bool has_client(const std::string & name) const {return entries_.find(name) != entries_.end();}
|
||||
|
||||
/**
|
||||
* @brief Lightweight availability check for a named client.
|
||||
* @param name Action client key.
|
||||
* @param timeout Wait duration.
|
||||
* @return true if server responds within timeout.
|
||||
*/
|
||||
bool is_server_available(const std::string & name, std::chrono::nanoseconds timeout) const
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {return false;}
|
||||
return it->second->available(timeout);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convenience: send a goal and block until result or timeout.
|
||||
* @tparam ActionT Action type.
|
||||
* @param name Action client key.
|
||||
* @param logger Logger instance.
|
||||
* @param timeout Overall result wait timeout.
|
||||
* @return WrappedResult if completed in time; std::nullopt on timeout or error.
|
||||
*/
|
||||
template<typename ActionT>
|
||||
std::optional<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult> send_and_wait(
|
||||
const std::string & name,
|
||||
rclcpp::Logger logger,
|
||||
std::chrono::nanoseconds timeout = std::chrono::seconds(30))
|
||||
{
|
||||
auto it = entries_.find(name);
|
||||
if (it == entries_.end()) {
|
||||
RCLCPP_ERROR(logger, "No action client registered for '%s'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
auto * e = dynamic_cast<Entry<ActionT> *>(it->second.get());
|
||||
if (!e) {
|
||||
RCLCPP_ERROR(logger, "Action client type mismatch for '%s'", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
if (!e->client) {return std::nullopt;}
|
||||
if (!e->client->wait_for_action_server(std::chrono::seconds(10))) {
|
||||
RCLCPP_ERROR(logger, "Action server '%s' not available", name.c_str());
|
||||
return std::nullopt;
|
||||
}
|
||||
auto goal = e->make_goal ? e->make_goal() : typename ActionT::Goal{};
|
||||
auto prom =
|
||||
std::make_shared<std::promise<typename rclcpp_action::ClientGoalHandle<ActionT>::WrappedResult>>();
|
||||
auto fut = prom->get_future();
|
||||
auto opts = typename rclcpp_action::Client<ActionT>::SendGoalOptions{};
|
||||
if (e->on_goal_response) {opts.goal_response_callback = e->on_goal_response;}
|
||||
if (e->on_feedback) {opts.feedback_callback = e->on_feedback;}
|
||||
opts.result_callback = [p = prom, cb = e->on_result](const auto & res) mutable {
|
||||
if (cb) {cb(res);}
|
||||
p->set_value(res);
|
||||
};
|
||||
auto future_handle = e->client->async_send_goal(goal, opts);
|
||||
std::shared_ptr<GoalHandle<ActionT>> goal_handle_ptr;
|
||||
try {
|
||||
if (future_handle.wait_for(std::chrono::seconds(5)) == std::future_status::ready) {
|
||||
goal_handle_ptr = future_handle.get();
|
||||
if (goal_handle_ptr) {e->last_goal_handle = goal_handle_ptr;}
|
||||
}
|
||||
} catch (const std::exception & ex) {
|
||||
RCLCPP_WARN(logger, "Exception getting goal handle for '%s': %s", name.c_str(), ex.what());
|
||||
}
|
||||
if (fut.wait_for(timeout) == std::future_status::ready) {
|
||||
return fut.get();
|
||||
}
|
||||
RCLCPP_ERROR(logger, "Timed out waiting for result on '%s', sending cancel", name.c_str());
|
||||
if (goal_handle_ptr) {
|
||||
try {e->client->async_cancel_goal(goal_handle_ptr);} catch (...) {}
|
||||
} else {
|
||||
e->cancel(logger);
|
||||
}
|
||||
return std::nullopt;
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_;
|
||||
std::unordered_map<std::string, std::unique_ptr<EntryBase>> entries_;
|
||||
};
|
||||
240
src/brain/include/brain/bt_registry.hpp
Normal file
240
src/brain/include/brain/bt_registry.hpp
Normal file
@@ -0,0 +1,240 @@
|
||||
#ifndef BRAIN_BT_REGISTRY_HPP_
|
||||
#define BRAIN_BT_REGISTRY_HPP_
|
||||
|
||||
/**
|
||||
* @file bt_registry.hpp
|
||||
* @brief Behavior Tree registration utilities wrapping BehaviorTree.CPP factory with higher level helpers.
|
||||
*
|
||||
* Provides:
|
||||
* - BTActionSpec / BTSequenceSpec: lightweight declarative descriptions of action nodes and sequences.
|
||||
* - BTActionHandlers / RegisteredActionNode: injection of user supplied callbacks for start / running / halted phases.
|
||||
* - BTRegistry: central registry to declare custom action node builders and sequence archetypes, and to build trees.
|
||||
*
|
||||
* Design goals:
|
||||
* - Minimize boilerplate when adding custom stateful BT action nodes whose behavior is defined by lambdas.
|
||||
* - Allow dynamic composition of sequences (converted to XML text consumed by BehaviorTreeFactory).
|
||||
* - Keep user code decoupled from raw XML authoring while still producing valid BTCPP v4 format.
|
||||
*
|
||||
* Thread-safety:
|
||||
* - Registration APIs (register_bt_node / register_bt_action / register_bt_sequence) are intended for single-threaded
|
||||
* initialization before any trees are executed. They are NOT internally synchronized.
|
||||
* - build_tree_from_sequence may be called concurrently if the underlying BehaviorTreeFactory is used in that way;
|
||||
* BehaviorTree.CPP factory itself is generally not thread-safe for concurrent mutation, so calls should serialize.
|
||||
*/
|
||||
|
||||
#include <behaviortree_cpp/bt_factory.h>
|
||||
#include <behaviortree_cpp/action_node.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <functional>
|
||||
#include <map>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <sstream>
|
||||
#include <unordered_map>
|
||||
|
||||
/**
|
||||
* @brief Specification for a single Behavior Tree action node used when programmatically composing XML.
|
||||
*
|
||||
* Each node is described by its type (registered with the factory), an instance name, and an optional
|
||||
* map of port name -> value pairs which become XML attributes in the emitted tree definition.
|
||||
*/
|
||||
struct BTActionSpec
|
||||
{
|
||||
std::string type; ///< Registered node type ID (XML tag)
|
||||
std::string name; ///< Instance name attribute
|
||||
std::map<std::string, std::string> ports; ///< Static input/output port bindings as string literals
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief A linear sequence of BTActionSpec items rendered as a <Sequence> subtree.
|
||||
*/
|
||||
using BTSequenceSpec = std::vector<BTActionSpec>;
|
||||
|
||||
/**
|
||||
* @brief Callback bundle implementing the lifecycle hooks of a BehaviorTree.CPP StatefulActionNode.
|
||||
*
|
||||
* All callbacks are optional. Missing callbacks default to immediate SUCCESS (for start/running)
|
||||
* or no-op (for halted).
|
||||
*/
|
||||
struct BTActionHandlers
|
||||
{
|
||||
std::function<BT::NodeStatus(rclcpp::Node *, BT::TreeNode &)> on_start; ///< Invoked once when the node transitions from IDLE to RUNNING
|
||||
std::function<BT::NodeStatus(rclcpp::Node *, BT::TreeNode &)> on_running; ///< Invoked each tick while RUNNING
|
||||
std::function<void(rclcpp::Node *, BT::TreeNode &)> on_halted; ///< Invoked if the node is halted by the tree
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Concrete BehaviorTree.CPP stateful action node whose behavior is delegated to BTActionHandlers.
|
||||
*
|
||||
* This class is registered dynamically by BTRegistry when register_bt_action is invoked. Users do not
|
||||
* normally instantiate it directly; instead they provide a handlers bundle associated with a type name.
|
||||
*/
|
||||
class RegisteredActionNode : public BT::StatefulActionNode
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct with name, handler bundle, and ROS node context.
|
||||
* @param name Instance name inside the tree.
|
||||
* @param handlers Lifecycle callback handlers.
|
||||
* @param node ROS 2 node pointer passed back to callbacks.
|
||||
*/
|
||||
RegisteredActionNode(
|
||||
const std::string & name, const BTActionHandlers & handlers,
|
||||
rclcpp::Node * node)
|
||||
: BT::StatefulActionNode(name, BT::NodeConfiguration()), name_(name), handlers_(handlers), node_(
|
||||
node) {}
|
||||
|
||||
/**
|
||||
* @brief Declare static ports. Currently none (empty list) simplifying dynamic XML generation.
|
||||
*/
|
||||
static BT::PortsList providedPorts() {return {};}
|
||||
|
||||
/**
|
||||
* @brief First tick transition handler.
|
||||
* @return Status from user callback or SUCCESS if absent.
|
||||
*/
|
||||
BT::NodeStatus onStart() override
|
||||
{
|
||||
if (handlers_.on_start) {return handlers_.on_start(node_, *this);}
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
/**
|
||||
* @brief Repeated tick while status remains RUNNING.
|
||||
* @return Status from user callback or SUCCESS if absent.
|
||||
*/
|
||||
BT::NodeStatus onRunning() override
|
||||
{
|
||||
if (handlers_.on_running) {return handlers_.on_running(node_, *this);}
|
||||
return BT::NodeStatus::SUCCESS;
|
||||
}
|
||||
/**
|
||||
* @brief Invoked if the node is halted externally.
|
||||
*/
|
||||
void onHalted() override
|
||||
{
|
||||
if (handlers_.on_halted) {handlers_.on_halted(node_, *this);}
|
||||
}
|
||||
|
||||
private:
|
||||
std::string name_; ///< Instance name
|
||||
BTActionHandlers handlers_; ///< Lifecycle callbacks
|
||||
rclcpp::Node * node_ {nullptr}; ///< Non-owning node pointer
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Registry encapsulating BehaviorTreeFactory operations and declarative sequence handling.
|
||||
*
|
||||
* Responsibilities:
|
||||
* - Register arbitrary custom node types (templated) mapping a type_name to a builder.
|
||||
* - Register action node types backed by BTActionHandlers (auto-wrapped with RegisteredActionNode).
|
||||
* - Store named sequences (lists of BTActionSpec) and build executable trees from them.
|
||||
*/
|
||||
class BTRegistry
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct the registry with an external BehaviorTreeFactory and ROS node context.
|
||||
* @param factory Reference to an existing factory (not owned).
|
||||
* @param node ROS 2 node pointer for callbacks (not owned).
|
||||
*/
|
||||
BTRegistry(BT::BehaviorTreeFactory & factory, rclcpp::Node * node)
|
||||
: factory_(factory), node_(node) {}
|
||||
|
||||
/**
|
||||
* @brief Register a custom behavior tree node type using BehaviorTree.CPP's registerBuilder.
|
||||
* @tparam T Node class; must provide constructor (const std::string&, rclcpp::Node*).
|
||||
* @param type_name String identifier used in XML.
|
||||
* @thread_safety Not thread-safe with other registration calls.
|
||||
*/
|
||||
template<typename T>
|
||||
void register_bt_node(const std::string & type_name)
|
||||
{
|
||||
factory_.registerBuilder<T>(
|
||||
type_name, [this](const std::string & name, const BT::NodeConfiguration & config) {
|
||||
(void)config;
|
||||
return std::make_unique<T>(name, node_);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Register a stateful action node defined by handler callbacks.
|
||||
* @param type_name Type identifier used in XML.
|
||||
* @param handlers Lifecycle callbacks; may omit any for default behavior.
|
||||
* @throws std::runtime_error if a duplicate registration is attempted via internal factory behavior.
|
||||
* @thread_safety Not thread-safe with other registration calls.
|
||||
*/
|
||||
void register_bt_action(const std::string & type_name, const BTActionHandlers & handlers)
|
||||
{
|
||||
bt_action_handlers_[type_name] = handlers;
|
||||
factory_.registerBuilder<RegisteredActionNode>(
|
||||
type_name, [this, type_name](const std::string & name, const BT::NodeConfiguration & config) {
|
||||
(void)config;
|
||||
auto it = bt_action_handlers_.find(type_name);
|
||||
if (it == bt_action_handlers_.end()) {
|
||||
throw std::runtime_error("Handlers not found for BT type: " + type_name);
|
||||
}
|
||||
return std::make_unique<RegisteredActionNode>(name, it->second, node_);
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Register (or overwrite) a named linear sequence specification.
|
||||
* @param seq_name Logical sequence identifier.
|
||||
* @param seq Ordered list of action specs.
|
||||
*/
|
||||
void register_bt_sequence(const std::string & seq_name, const BTSequenceSpec & seq)
|
||||
{
|
||||
bt_sequences_[seq_name] = seq;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Build a BehaviorTree.CPP Tree instance from a previously registered sequence.
|
||||
* @param seq_name Sequence identifier.
|
||||
* @param out_tree Output tree (assigned on success).
|
||||
* @return true if the sequence exists and tree creation succeeded; false if sequence not found.
|
||||
* @note Fails early with log output if the sequence name is missing.
|
||||
*/
|
||||
bool build_tree_from_sequence(const std::string & seq_name, BT::Tree & out_tree)
|
||||
{
|
||||
auto it = bt_sequences_.find(seq_name);
|
||||
if (it == bt_sequences_.end()) {
|
||||
if (node_) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "BT sequence '%s' not found", seq_name.c_str());
|
||||
}
|
||||
return false;
|
||||
}
|
||||
const std::string xml_text = build_xml_from_sequence(it->second);
|
||||
RCLCPP_INFO(node_->get_logger(), "Building BT sequence '%s'", xml_text.c_str());
|
||||
out_tree = factory_.createTreeFromText(xml_text);
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
/**
|
||||
* @brief Convert a linear sequence spec into BTCPP v4 XML text.
|
||||
* @param seq Sequence specification.
|
||||
* @return Complete XML document string.
|
||||
*/
|
||||
std::string build_xml_from_sequence(const BTSequenceSpec & seq) const
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss <<
|
||||
"\n <root BTCPP_format=\"4\" >\n\n <BehaviorTree ID=\"MainTree\">\n <Sequence name=\"root_sequence\">\n";
|
||||
for (const auto & act : seq) {
|
||||
oss << " <" << act.type << " name=\"" << act.name << "\"";
|
||||
for (const auto & kv : act.ports) {
|
||||
oss << " " << kv.first << "=\"" << kv.second << "\"";
|
||||
}
|
||||
oss << "/>\n";
|
||||
}
|
||||
oss << " </Sequence>\n </BehaviorTree>\n\n </root>\n";
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
BT::BehaviorTreeFactory & factory_; ///< External factory reference
|
||||
rclcpp::Node * node_; ///< Non-owning ROS node pointer
|
||||
std::unordered_map<std::string, BTSequenceSpec> bt_sequences_; ///< Registered sequence specs
|
||||
std::unordered_map<std::string, BTActionHandlers> bt_action_handlers_; ///< Action handlers by type
|
||||
};
|
||||
|
||||
#endif // BRAIN_BT_REGISTRY_HPP_
|
||||
273
src/brain/include/brain/cerebellum_node.hpp
Normal file
273
src/brain/include/brain/cerebellum_node.hpp
Normal file
@@ -0,0 +1,273 @@
|
||||
#ifndef CEREBELLUM_CEREBELLUM_NODE_HPP_
|
||||
#define CEREBELLUM_CEREBELLUM_NODE_HPP_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "brain/action_registry.hpp"
|
||||
#include "brain/skill_manager.hpp"
|
||||
#include <smacc2/smacc_signal_detector.hpp>
|
||||
#include "brain/sm_cerebellum.hpp" // for CerebellumData::ExecResult
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
class CerebellumNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct the CerebellumNode.
|
||||
*
|
||||
* Responsibilities:
|
||||
* - Initialize action server/client registries and skill manager.
|
||||
* - Declare & load parameters (timeouts, abort policy, stats interval).
|
||||
* - Load skills YAML (either user-specified or default share path).
|
||||
* - Create statistics publisher & periodic timer (if enabled).
|
||||
* - Register ExecuteBtAction server for external skill sequence execution.
|
||||
* - Optionally spin up the SMACC2 state machine execution context.
|
||||
* @param options (optional) ROS 2 node options.
|
||||
*/
|
||||
explicit CerebellumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
|
||||
private:
|
||||
// Generic action registry to support multiple actions via registration
|
||||
std::unique_ptr<ActionServerRegistry> action_registry_;
|
||||
std::unique_ptr<ActionClientRegistry> action_clients_;
|
||||
std::unique_ptr<SkillManager> skill_manager_;
|
||||
std::unique_ptr<BTRegistry> bt_registry_;
|
||||
|
||||
// SMACC2 execution context
|
||||
std::unique_ptr<smacc2::SmExecution> sm_exec_;
|
||||
|
||||
// Parameters: abort policy, default action timeout, specific grasp timeout
|
||||
bool abort_on_failure_ {true};
|
||||
double default_action_timeout_sec_ {60.0};
|
||||
double vision_grasp_object_timeout_sec_ {120.0};
|
||||
|
||||
// Optional per-skill override timeout (skill_name -> seconds)
|
||||
std::unordered_map<std::string, double> skill_timeouts_;
|
||||
|
||||
// Statistics: per-skill success/failure counters & sequence totals
|
||||
std::unordered_map<std::string, size_t> skill_success_counts_;
|
||||
std::unordered_map<std::string, size_t> skill_failure_counts_;
|
||||
size_t total_sequence_success_ {0};
|
||||
size_t total_sequence_failure_ {0};
|
||||
rclcpp::TimerBase::SharedPtr stats_timer_;
|
||||
double stats_log_interval_sec_ {60.0};
|
||||
std::mutex stats_mutex_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr stats_pub_;
|
||||
|
||||
// Current accepted sequence epoch (from client) used to filter/reject stale goals
|
||||
std::atomic<uint32_t> current_epoch_{0};
|
||||
|
||||
// ---- helpers ----
|
||||
/**
|
||||
* @brief Parse a raw comma / semicolon / whitespace separated list of skill names.
|
||||
*
|
||||
* Strips an optional trailing "_H" suffix from each token (behavior-tree leaf naming
|
||||
* convention) but otherwise preserves order and duplicates.
|
||||
* Thread-safety: const, no shared state usage.
|
||||
* @param raw Input string e.g. "ArmMove,HandOpen VisionGraspObject_H".
|
||||
* @return Vector of parsed skill names with suffix removed.
|
||||
*/
|
||||
std::vector<std::string> ParseSkillSequence(const std::string & raw) const;
|
||||
|
||||
/**
|
||||
* @brief Convert CamelCase to snake_case (ASCII letters only).
|
||||
* Thread-safety: const.
|
||||
* @param s Skill name, e.g. "VisionGraspObject".
|
||||
* @return snake_case representation, e.g. "vision_grasp_object".
|
||||
*/
|
||||
std::string ToSnake(const std::string & s) const;
|
||||
|
||||
/**
|
||||
* @brief Compute timeout for a skill.
|
||||
* Precedence: explicit per-skill override > special-case (VisionGraspObject) > default.
|
||||
* Thread-safety: const (reads immutable maps after construction phase).
|
||||
* @param skill Skill name.
|
||||
* @return Timeout in seconds.
|
||||
*/
|
||||
double GetTimeoutForSkill(const std::string & skill) const;
|
||||
|
||||
/**
|
||||
* @brief Publish a single feedback stage message (START, RUN, WAIT, END).
|
||||
* Thread-safety: safe (ROS2 publisher is thread-safe; uses atomic epoch).
|
||||
* @param gh Goal handle.
|
||||
* @param stage Stage label.
|
||||
* @param current_skill Current skill name.
|
||||
* @param progress Fractional sequence progress in [0,1].
|
||||
* @param detail Additional free-form detail (may be empty).
|
||||
*/
|
||||
void PublishFeedbackStage(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & gh,
|
||||
const std::string & stage,
|
||||
const std::string & current_skill,
|
||||
float progress,
|
||||
const std::string & detail) const;
|
||||
/**
|
||||
* @brief Execute one skill (action or service) updating detail and emitting feedback.
|
||||
* Dispatches to ExecuteActionSkill / ExecuteServiceSkill based on interface kind.
|
||||
* @param skill Skill name.
|
||||
* @param chosen_topic Topic / action client key used for actions.
|
||||
* @param spec Skill specification metadata.
|
||||
* @param detail [out] Filled with result or error detail.
|
||||
* @param index 0-based index of this skill in the sequence.
|
||||
* @param total_skills Total number of skills in the sequence.
|
||||
* @param goal_handle ExecuteBtAction goal handle.
|
||||
* @return true if successful, false otherwise.
|
||||
*/
|
||||
bool ExecuteSingleSkill(
|
||||
const std::string & skill, const std::string & chosen_topic,
|
||||
const SkillSpec & spec, std::string & detail, int index, int total_skills,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
|
||||
/**
|
||||
* @brief Determine interface kind from first interface entry.
|
||||
* Example: some_pkg/DoThing.action => "action".
|
||||
* @param spec Skill specification.
|
||||
* @return Interface kind token ("action", "service" or empty).
|
||||
*/
|
||||
std::string GetInterfaceKind(const SkillSpec & spec) const;
|
||||
|
||||
/**
|
||||
* @brief Wait (with retries) for action server readiness emitting WAIT feedback.
|
||||
* @param topic Action client topic key.
|
||||
* @param skill Skill name for feedback context.
|
||||
* @param base_progress Progress before this skill's execution.
|
||||
* @param goal_handle Goal handle.
|
||||
* @param detail [out] Error detail if unavailable.
|
||||
* @return true if server becomes available before retry exhaustion.
|
||||
*/
|
||||
bool WaitForActionServer(
|
||||
const std::string & topic,
|
||||
const std::string & skill,
|
||||
float base_progress,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
std::string & detail) const;
|
||||
/**
|
||||
* @brief Execute an action-type skill.
|
||||
* Orchestrates START feedback, action server wait, worker thread dispatch, and progress loop.
|
||||
* @param skill Skill name.
|
||||
* @param topic Resolved action client topic.
|
||||
* @param spec Skill specification (currently unused for actions beyond interface detection).
|
||||
* @param detail [out] Detail / diagnostics.
|
||||
* @param index Skill index in sequence.
|
||||
* @param total_skills Total skills count.
|
||||
* @param goal_handle Goal handle for feedback.
|
||||
* @return true if action succeeds.
|
||||
*/
|
||||
bool ExecuteActionSkill(
|
||||
const std::string & skill,
|
||||
const std::string & topic,
|
||||
const SkillSpec & spec,
|
||||
std::string & detail,
|
||||
int index,
|
||||
int total_skills,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
|
||||
/**
|
||||
* @brief Internal helper: run the action worker thread and emit RUN feedback until completion.
|
||||
* Extracted to reduce cyclomatic complexity of ExecuteActionSkill.
|
||||
* @param skill Skill name.
|
||||
* @param topic Action topic.
|
||||
* @param timeout_ns Timeout duration (nanoseconds).
|
||||
* @param index Skill index.
|
||||
* @param total_skills Total skills count.
|
||||
* @param goal_handle Goal handle for feedback publishing.
|
||||
* @param detail [out] Detailed status text.
|
||||
* @return true if worker reports success.
|
||||
*/
|
||||
bool RunActionSkillWithProgress(
|
||||
const std::string & skill,
|
||||
const std::string & topic,
|
||||
std::chrono::nanoseconds timeout_ns,
|
||||
int index,
|
||||
int total_skills,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
std::string & detail);
|
||||
|
||||
/**
|
||||
* @brief Execute a service-based skill (currently VisionObjectRecognition only).
|
||||
* @param skill Skill name.
|
||||
* @param detail [out] Detail / diagnostics.
|
||||
* @param goal_handle Goal handle.
|
||||
* @return true if successful.
|
||||
*/
|
||||
bool ExecuteServiceSkill(
|
||||
const std::string & skill,
|
||||
std::string & detail,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
|
||||
/**
|
||||
* @brief Record per-skill success/failure counts (thread-safe via mutex).
|
||||
* @param skill Skill name.
|
||||
* @param success True if skill succeeded.
|
||||
*/
|
||||
void RecordSkillResult(const std::string & skill, bool success);
|
||||
/** @brief Periodically log and publish aggregate statistics snapshot. */
|
||||
void LogStatsPeriodic();
|
||||
/**
|
||||
* @brief Process an accepted ExecuteBtAction goal end-to-end.
|
||||
* Validates goal, executes skills in order (respecting abort policy), publishes END feedback.
|
||||
* This function is now wrapped into the state machine via execute_fn; kept private for internal call.
|
||||
* @param goal_handle Goal handle.
|
||||
*/
|
||||
void RunExecuteBtGoal(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle);
|
||||
|
||||
/**
|
||||
* @brief Execute the full skill sequence building a CerebellumData::ExecResult summary.
|
||||
* (Refactored facade used by execute_fn so state machine can own lifecycle.)
|
||||
* @param goal_handle ExecuteBtAction goal handle.
|
||||
* @param skills Parsed ordered skill list.
|
||||
* @return CerebellumData::ExecResult aggregated result.
|
||||
*/
|
||||
brain::CerebellumData::ExecResult ExecuteSequence(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
const std::vector<std::string> & skills);
|
||||
// --- Decomposed helpers for RunExecuteBtGoal ---
|
||||
/**
|
||||
* @brief Validate goal and parse action_name into skill list.
|
||||
* @param goal_handle Goal handle.
|
||||
* @param skills_out [out] Parsed skills.
|
||||
* @return true if goal is valid; false otherwise (goal aborted internally).
|
||||
*/
|
||||
bool ValidateAndParseGoal(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
std::vector<std::string> & skills_out) const;
|
||||
/**
|
||||
* @brief Resolve action topic name for a skill.
|
||||
* Prefers snake_case, falls back to original name if client registered directly.
|
||||
* @param skill Skill name.
|
||||
* @return Topic / action client key.
|
||||
*/
|
||||
std::string ResolveTopicForSkill(const std::string & skill) const;
|
||||
/**
|
||||
* @brief Finalize goal with summary result (success or abort) and log message.
|
||||
* @param goal_handle Goal handle.
|
||||
* @param overall_ok True if whole sequence succeeded.
|
||||
* @param succeeded_skills Number of succeeded skills.
|
||||
* @param total_skills Total skills count.
|
||||
* @param summary_stream Accumulated summary text.
|
||||
*/
|
||||
[[deprecated("Result finalization now handled inside state machine complete_cb; kept for backward compatibility")]]
|
||||
void FinalizeExecuteBtResult(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
bool overall_ok,
|
||||
int32_t succeeded_skills,
|
||||
int32_t total_skills,
|
||||
const std::ostringstream & summary_stream);
|
||||
/** @brief Declare parameters and load initial values. */
|
||||
void DeclareAndLoadParameters();
|
||||
/** @brief Load skill definitions from YAML file (path via parameter or default). */
|
||||
void LoadSkillsFile();
|
||||
/** @brief Setup periodic stats timer and publisher if interval > 0. */
|
||||
void SetupStatsTimerAndPublisher();
|
||||
/** @brief Register ExecuteBtAction server with goal/execute handlers. */
|
||||
void SetupExecuteBtServer();
|
||||
|
||||
// NOTE: Additional future helpers (cancellation handling, concurrency) may be added here.
|
||||
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
|
||||
#endif // CEREBELLUM_CEREBELLUM_NODE_HPP_
|
||||
255
src/brain/include/brain/cerebrum_node.hpp
Normal file
255
src/brain/include/brain/cerebrum_node.hpp
Normal file
@@ -0,0 +1,255 @@
|
||||
#ifndef BRAIN_CEREBRUM_NODE_HPP_
|
||||
#define BRAIN_CEREBRUM_NODE_HPP_
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/service.hpp>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
#include <string>
|
||||
#include <atomic>
|
||||
#include <unordered_set>
|
||||
#include <optional>
|
||||
#include <mutex>
|
||||
#include "brain/action_registry.hpp"
|
||||
#include <behaviortree_cpp/bt_factory.h>
|
||||
#include "brain/bt_registry.hpp"
|
||||
#include "brain/skill_manager.hpp"
|
||||
#include "brain/constants.hpp"
|
||||
#include "brain/epoch_filter.hpp"
|
||||
#include <std_srvs/srv/trigger.hpp>
|
||||
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
class CerebrumNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct the Cerebrum node.
|
||||
*
|
||||
* Responsibilities:
|
||||
* - Load skill definitions (YAML) and register corresponding BT leaf actions.
|
||||
* - Provide timers to (a) periodically choose / rebuild a skill sequence BT and (b) tick it.
|
||||
* - Funnel sequence execution through a single ExecuteBtAction toward the cerebellum (executor).
|
||||
* - Expose a trigger service to force an immediate rebuild.
|
||||
*
|
||||
* @param options Optional ROS 2 node options.
|
||||
* @thread_safety Construction is single-threaded; no concurrent access occurs during ctor.
|
||||
*/
|
||||
explicit CerebrumNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
|
||||
|
||||
/**
|
||||
* @brief Send a single action goal by its registered name using the ActionClientRegistry.
|
||||
* @param name Action client key (must have been previously registered).
|
||||
* @thread_safety Thread-safe if underlying ActionClientRegistry::send is thread-safe (read-only state).
|
||||
*/
|
||||
void SendAction(const std::string & name);
|
||||
|
||||
/**
|
||||
* @brief Register a custom BehaviorTree node type (template wrapper that forwards to BTRegistry).
|
||||
* @tparam T Concrete BT node class.
|
||||
* @param type_name Name under which the node type will be registered.
|
||||
*/
|
||||
template<typename T>
|
||||
void RegisterBtNode(const std::string & type_name) {bt_registry_->register_bt_node<T>(type_name);}
|
||||
/**
|
||||
* @brief Register a leaf BT action with its handler callbacks.
|
||||
* @param type_name Node type string (convention: <SkillName>_H).
|
||||
* @param handlers Callback bundle (on_start/on_running/on_halted).
|
||||
* @thread_safety Should be invoked during initialization before concurrent ticking.
|
||||
*/
|
||||
void RegisterBtAction(const std::string & type_name, const BTActionHandlers & handlers);
|
||||
/**
|
||||
* @brief Register a sequence specification with a symbolic name.
|
||||
* @param seq_name Logical sequence key.
|
||||
* @param seq Sequence specification (ordered vector of BTActionSpec).
|
||||
* @thread_safety Typically called from model logic prior to UpdateBehaviorTree; not thread-safe if BT rebuild is concurrent.
|
||||
*/
|
||||
void RegisterBtSequence(const std::string & seq_name, const BTSequenceSpec & seq)
|
||||
{
|
||||
bt_registry_->register_bt_sequence(seq_name, seq);
|
||||
}
|
||||
/**
|
||||
* @brief Build a new behavior tree instance from a registered sequence.
|
||||
* @param seq_name Sequence key.
|
||||
* @return true if build succeeds; false otherwise.
|
||||
* @thread_safety Not thread-safe with concurrent ticking; intended for controlled rebuild phases.
|
||||
*/
|
||||
bool BuildTreeFromSequence(const std::string & seq_name)
|
||||
{
|
||||
return bt_registry_->build_tree_from_sequence(seq_name, tree_);
|
||||
}
|
||||
/**
|
||||
* @brief Mark a sequence as active (does not perform a rebuild).
|
||||
* @param seq_name Sequence key.
|
||||
* @thread_safety Requires external synchronization with UpdateBehaviorTree / ticking.
|
||||
*/
|
||||
void SetActiveSequence(const std::string & seq_name) {active_sequence_ = seq_name;}
|
||||
|
||||
/**
|
||||
* @brief Send ExecuteBtAction through its dedicated action client.
|
||||
* @param bt_action_name Typically brain::kExecuteBtActionName.
|
||||
* @thread_safety Thread-safe if ActionClientRegistry::send is; goal building uses immutable snapshot of sequence.
|
||||
*/
|
||||
void SendExecuteBt(const std::string & bt_action_name);
|
||||
|
||||
// ---- Public utility types & helpers (exposed for unit tests) ----
|
||||
enum class ExecResultCode { UNKNOWN, SUCCESS, FAILURE, CANCELLED };
|
||||
|
||||
/**
|
||||
* @brief Pick up to count random action-capable skills.
|
||||
* @param count Maximum number of distinct skills to return.
|
||||
* @return Vector of selected skill names (size <= count).
|
||||
* @thread_safety Reads skill metadata; safe after initialization if SkillManager is immutable.
|
||||
*/
|
||||
std::vector<std::string> PickRandomActionSkills(std::size_t count);
|
||||
|
||||
/**
|
||||
* @brief Parse a delimited list into unique tokens (order preserved, duplicates removed).
|
||||
* @param raw Raw input string.
|
||||
* @return Vector of unique tokens in first-occurrence order.
|
||||
* @thread_safety Pure function.
|
||||
*/
|
||||
static std::vector<std::string> ParseListString(const std::string & raw);
|
||||
|
||||
/**
|
||||
* @brief Classify textual result detail (prefix or code= token) into ExecResultCode.
|
||||
* @param detail Input detail string.
|
||||
* @return Parsed ExecResultCode enumeration.
|
||||
* @thread_safety Pure function.
|
||||
*/
|
||||
static ExecResultCode ParseResultDetail(const std::string & detail);
|
||||
|
||||
private:
|
||||
BT::Tree tree_;
|
||||
BT::BehaviorTreeFactory factory_;
|
||||
std::unique_ptr<ActionClientRegistry> action_registry_;
|
||||
std::unique_ptr<BTRegistry> bt_registry_;
|
||||
std::unique_ptr<SkillManager> skill_manager_;
|
||||
std::string active_sequence_;
|
||||
rclcpp::TimerBase::SharedPtr task_timer_;
|
||||
rclcpp::TimerBase::SharedPtr bt_timer_;
|
||||
// Service to trigger immediate rebuild of sequence + BT
|
||||
rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr rebuild_service_;
|
||||
std::atomic<bool> bt_updating_{false};
|
||||
std::atomic<bool> bt_pending_run_{false};
|
||||
// Encapsulated epoch+skill filter
|
||||
EpochSkillFilter epoch_filter_;
|
||||
// Current sequence skills (kept in original order for logging / statistics).
|
||||
std::vector<std::string> current_sequence_skills_;
|
||||
// Mutex that protects both per_node_exec_ and epoch_skills_ updates.
|
||||
std::mutex exec_mutex_;
|
||||
// Removed legacy global pending_bt_action_name_: each node now stores its own skill_name.
|
||||
|
||||
struct PerNodeExecInfo
|
||||
{
|
||||
bool in_flight {false};
|
||||
bool result {false}; // legacy bool success flag for backward compatibility
|
||||
rclcpp::Time start_time;
|
||||
std::string skill_name; ///< Associated skill name
|
||||
double last_progress {0.0};
|
||||
enum class CancelState { NONE, REQUESTED, CONFIRMED } cancel_state {CancelState::NONE};
|
||||
ExecResultCode result_code {ExecResultCode::UNKNOWN};
|
||||
bool awaiting_result {false}; ///< 已收到 END(SUCCESS) 反馈，等待最终 action result
|
||||
rclcpp::Time end_feedback_time; ///< 记录 END(SUCCESS) 到达时间
|
||||
};
|
||||
std::unordered_map<std::string, PerNodeExecInfo> per_node_exec_;
|
||||
// Set of already registered action client names (prevents duplicate registration & duplicate feedback).
|
||||
std::unordered_set<std::string> registered_actions_;
|
||||
// Per-BT-node execution timeout (seconds).
|
||||
double node_timeout_sec_ {5.0};
|
||||
// Warning threshold: END(SUCCESS) 与最终 result 回调之间超过该秒数则输出警告日志。
|
||||
double result_wait_warn_sec_ {2.0};
|
||||
// Allow rebuilding the BT while it is still running (default false to avoid preemption).
|
||||
bool allow_bt_rebuild_during_execution_ {false};
|
||||
|
||||
// Parameter: number of random skills to pick (exposed as 'random_skill_pick_count').
|
||||
std::size_t random_skill_pick_count_{2};
|
||||
|
||||
// Optional whitelist of skill names allowed to be randomly selected; empty => unrestricted.
|
||||
std::vector<std::string> allowed_action_skills_;
|
||||
// Fixed skill sequence (if set, random selection is skipped entirely).
|
||||
std::optional<std::vector<std::string>> fixed_skill_sequence_;
|
||||
/**
|
||||
* @brief Cancel the currently active ExecuteBtAction goal (if any).
|
||||
* @thread_safety Calls into ActionClientRegistry; assumed thread-safe for cancellation.
|
||||
*/
|
||||
void CancelActiveExecuteBtGoal();
|
||||
/**
|
||||
* @brief (Legacy) Update per-epoch allowed skills. Currently a no-op (handled by EpochSkillFilter).
|
||||
* @param skills Ignored.
|
||||
* @thread_safety Safe (no state mutation).
|
||||
*/
|
||||
void SetEpochSkills(const std::vector<std::string> & skills); // transitional; will delegate to epoch_filter_
|
||||
|
||||
/**
|
||||
* @brief Rebuild the behavior tree from the active sequence.
|
||||
* @post On success: epoch incremented, per-node state reset, bt_pending_run_ = true.
|
||||
* @thread_safety Guarded by atomic flag bt_updating_; not re-entrant.
|
||||
*/
|
||||
void UpdateBehaviorTree();
|
||||
/**
|
||||
* @brief Tick the behavior tree root if a run is pending.
|
||||
* @thread_safety Called from a timer callback; not thread-safe against concurrent UpdateBehaviorTree (atomic gating only).
|
||||
*/
|
||||
void ExecuteBehaviorTree();
|
||||
/**
|
||||
* @brief Ensure ExecuteBtAction client is registered (idempotent).
|
||||
* @thread_safety Initialization routine; avoid concurrent calls.
|
||||
*/
|
||||
void RegisterActionClient();
|
||||
/**
|
||||
* @brief Periodic task to select a new sequence (model-driven) then trigger rebuild.
|
||||
* @thread_safety Timer callback; coordinates with UpdateBehaviorTree via atomics and mutex for sequence list.
|
||||
*/
|
||||
void CerebrumTask();
|
||||
|
||||
/**
|
||||
* @brief Pick up to count random skills that expose an action interface (filtered by whitelist if provided).
|
||||
* @param count Maximum number of skills to return.
|
||||
* @return Vector of distinct skill names (size <= count).
|
||||
*/
|
||||
/**
|
||||
* @brief Convert ordered skill names into a BT sequence of per-skill nodes.
|
||||
*
|
||||
* Each skill becomes a <Skill>_H leaf. Unlike the earlier unified-node
|
||||
* experiment, we now send one ExecuteBtAction goal per skill (goal contains
|
||||
* only that skill) so BT node status aligns with the underlying action
|
||||
* result. This preserves fine-grained visibility while avoiding duplicated
|
||||
* sequencing inside a single action invocation.
|
||||
* @param names Ordered skill list.
|
||||
* @return BTSequenceSpec object preserving order.
|
||||
* @thread_safety Pure function.
|
||||
*/
|
||||
BTSequenceSpec MakeSequenceFromSkills(const std::vector<std::string> & names);
|
||||
/**
|
||||
* @brief Build a minimal XML representation of a sequence for diagnostics.
|
||||
* @param seq Sequence specification.
|
||||
* @return XML string containing a root <Sequence> with leaf nodes.
|
||||
* @thread_safety Pure function.
|
||||
*/
|
||||
std::string BuildXmlFromSequence(const BTSequenceSpec & seq) const;
|
||||
/**
|
||||
* @brief Choose and activate a skill sequence (fixed preferred over random selection).
|
||||
* @thread_safety Uses mutex to update current_sequence_skills_.
|
||||
*/
|
||||
void RunVlmModel();
|
||||
/**
|
||||
* @brief Register per-skill BT action handlers (idempotent per skill).
|
||||
*
|
||||
* Each handler now directly triggers an ExecuteBtAction goal containing only
|
||||
* its skill (single-skill override) instead of only the first node sending a
|
||||
* multi-skill goal.
|
||||
*/
|
||||
void RegisterSkillBtActions();
|
||||
|
||||
// Temporary override used so the generic ExecuteBtAction goal builder can
|
||||
// send a single-skill goal when a per-skill BT node starts.
|
||||
std::optional<std::string> single_skill_goal_override_;
|
||||
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
|
||||
#endif // BRAIN_CEREBRUM_NODE_HPP_
|
||||
57
src/brain/include/brain/constants.hpp
Normal file
57
src/brain/include/brain/constants.hpp
Normal file
@@ -0,0 +1,57 @@
|
||||
/**
|
||||
* @file constants.hpp
|
||||
* @brief Internal constants and lightweight helper utilities for the brain module.
|
||||
*
|
||||
* Centralizes magic strings and naming helpers to avoid scattering literals. If broader reuse is
|
||||
* required later these can be promoted to a shared package/API surface.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
/// Canonical action name used for ExecuteBtAction (historical misspelling fixed).
|
||||
inline constexpr const char * kExecuteBtActionName = "ExecuteBtAction";
|
||||
|
||||
/// Suffix appended to skill names to form BT node type identifiers.
|
||||
inline constexpr const char * kBtNodeSuffix = "_H";
|
||||
|
||||
/**
|
||||
* @brief Compose a Behavior Tree node type name from a skill base name.
|
||||
* @param skill_name Base skill name.
|
||||
* @return Concatenated skill name with the node suffix.
|
||||
*/
|
||||
inline std::string make_bt_node_type(const std::string & skill_name)
|
||||
{
|
||||
return skill_name + std::string{kBtNodeSuffix};
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Check if a string ends with a specified suffix.
|
||||
* @param s Input string.
|
||||
* @param suffix Suffix to test.
|
||||
* @return true if s ends with suffix.
|
||||
*/
|
||||
inline bool ends_with(const std::string & s, const std::string & suffix)
|
||||
{
|
||||
return s.size() >= suffix.size() &&
|
||||
s.compare(s.size() - suffix.size(), suffix.size(), suffix) == 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Remove the BT node suffix from a type name if present.
|
||||
* @param node_type Candidate node type string.
|
||||
* @return Stripped base name or the original if the suffix is absent.
|
||||
*/
|
||||
inline std::string strip_bt_node_suffix(const std::string & node_type)
|
||||
{
|
||||
if (ends_with(node_type, kBtNodeSuffix)) {
|
||||
return node_type.substr(0, node_type.size() - std::string(kBtNodeSuffix).size());
|
||||
}
|
||||
return node_type;
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
68
src/brain/include/brain/epoch_filter.hpp
Normal file
68
src/brain/include/brain/epoch_filter.hpp
Normal file
@@ -0,0 +1,68 @@
|
||||
#ifndef BRAIN_EPOCH_FILTER_HPP_
|
||||
#define BRAIN_EPOCH_FILTER_HPP_
|
||||
|
||||
#include <unordered_set>
|
||||
#include <string>
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Maintains an epoch counter and allowed skill set to filter incoming BT feedback.
|
||||
*
|
||||
* Used to ignore stale or unrelated feedback messages. Only feedback matching the current epoch and
|
||||
* belonging to the declared skill subset (if non-empty) is accepted.
|
||||
*
|
||||
* Thread-safety:
|
||||
* - reset_epoch acquires a mutex to update the skill set and epoch atomically.
|
||||
* - accept acquires the mutex only if a non-empty skill name needs membership validation.
|
||||
*/
|
||||
class EpochSkillFilter
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Set a new epoch and replace the allowed skill filter set.
|
||||
* @param new_epoch Epoch identifier.
|
||||
* @param skills List of allowed skill names (cleared if empty).
|
||||
*/
|
||||
void reset_epoch(uint64_t new_epoch, const std::vector<std::string> & skills)
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(mtx_);
|
||||
epoch_.store(new_epoch, std::memory_order_relaxed);
|
||||
skills_.clear();
|
||||
for (auto & s : skills) {skills_.insert(s);}
|
||||
}
|
||||
/**
|
||||
* @brief Get the current epoch.
|
||||
*/
|
||||
uint64_t epoch() const {return epoch_.load(std::memory_order_relaxed);}
|
||||
/**
|
||||
* @brief Determine if feedback should be considered.
|
||||
* @param feedback_epoch Epoch in the feedback message.
|
||||
* @param skill Skill name associated with feedback (may be empty to bypass name filtering).
|
||||
* @param bt_pending Whether a BT execution is still pending.
|
||||
* @return true if epoch matches, bt_pending is true, and skill (if provided) is allowed.
|
||||
*/
|
||||
bool accept(uint64_t feedback_epoch, const std::string & skill, bool bt_pending) const
|
||||
{
|
||||
if (!bt_pending) {return false;}
|
||||
if (feedback_epoch != epoch_.load(std::memory_order_relaxed)) {return false;}
|
||||
if (!skill.empty()) {
|
||||
std::lock_guard<std::mutex> lk(mtx_);
|
||||
if (skills_.find(skill) == skills_.end()) {return false;}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
private:
|
||||
mutable std::mutex mtx_; ///< Guards skill set
|
||||
std::atomic<uint64_t> epoch_{0}; ///< Current epoch
|
||||
std::unordered_set<std::string> skills_; ///< Allowed skills for current epoch
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
|
||||
#endif // BRAIN_EPOCH_FILTER_HPP_
|
||||
196
src/brain/include/brain/skill_manager.hpp
Normal file
196
src/brain/include/brain/skill_manager.hpp
Normal file
@@ -0,0 +1,196 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include "brain/action_registry.hpp"
|
||||
#include "brain/bt_registry.hpp"
|
||||
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
#include "interfaces/action/leg_control.hpp"
|
||||
#include "interfaces/action/vision_grasp_object.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp" // ExecuteBtAction bridge
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Specification of a skill as parsed from YAML.
|
||||
*
|
||||
* Fields capture metadata, dependency declarations and interface exposure. Interface strings use
|
||||
* the convention "BaseName.action" or "BaseName.service" to distinguish interface kinds.
|
||||
*/
|
||||
struct SkillSpec
|
||||
{
|
||||
std::string name;
|
||||
std::string version;
|
||||
std::string description;
|
||||
std::vector<std::string> required_skills;
|
||||
std::vector<std::string> interfaces; // e.g. "BaseName.action" / "BaseName.service"
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Loads skill specifications and registers related action/service interfaces.
|
||||
*
|
||||
* Responsibilities:
|
||||
* 1. Parse a YAML description file (robot_skills.yaml or equivalent).
|
||||
* 2. Register action clients and service clients for declared interfaces.
|
||||
* 3. Provide enumeration utilities and type-safe access to service clients.
|
||||
*
|
||||
* BehaviorTree action node registration is handled elsewhere; this class only prepares interface bindings.
|
||||
*/
|
||||
class SkillManager
|
||||
{
|
||||
public:
|
||||
/**
|
||||
* @brief Construct with external registry references.
|
||||
* @param node ROS 2 node pointer (not owned).
|
||||
* @param action_clients Action client registry used to register skill actions.
|
||||
* @param bt Behavior tree registry reference (not used for node creation directly here).
|
||||
*/
|
||||
SkillManager(rclcpp::Node * node, ActionClientRegistry * action_clients, BTRegistry * bt)
|
||||
: node_(node), action_clients_(action_clients), bt_(bt) {}
|
||||
|
||||
/**
|
||||
* @brief Load skill specifications from a YAML file.
|
||||
* @param yaml_path File system path to YAML document.
|
||||
* @return true on success, false if parsing or validation fails.
|
||||
*/
|
||||
bool load_from_file(const std::string & yaml_path);
|
||||
|
||||
/**
|
||||
* @brief Access loaded skills mapping.
|
||||
* @return Map of skill name -> SkillSpec.
|
||||
*/
|
||||
const std::unordered_map<std::string, SkillSpec> & skills() const {return skills_;}
|
||||
|
||||
/**
|
||||
* @brief Enumerate action oriented skills.
|
||||
* @return Vector of (skill_name, action_base) pairs.
|
||||
*/
|
||||
std::vector<std::pair<std::string, std::string>> enumerate_action_skills() const;
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_ {nullptr};
|
||||
ActionClientRegistry * action_clients_ {nullptr};
|
||||
BTRegistry * bt_ {nullptr};
|
||||
std::unordered_map<std::string, SkillSpec> skills_;
|
||||
std::vector<rclcpp::ClientBase::SharedPtr> service_clients_; // 保存 service client 生命周期
|
||||
std::unordered_map<std::string, rclcpp::ClientBase::SharedPtr> service_client_map_; // skill name -> client
|
||||
|
||||
void register_interfaces_(const SkillSpec & s);
|
||||
|
||||
public:
|
||||
/**
|
||||
* @brief Retrieve a typed service client for a named skill if available.
|
||||
* @tparam T Service type.
|
||||
* @param skill_name Registered skill name.
|
||||
* @return Shared pointer to service client or nullptr if not present or type mismatch.
|
||||
*/
|
||||
template<typename T>
|
||||
std::shared_ptr<rclcpp::Client<T>> get_typed_service_client(const std::string & skill_name) const
|
||||
{
|
||||
auto it = service_client_map_.find(skill_name);
|
||||
if (it == service_client_map_.end()) {return nullptr;}
|
||||
return std::dynamic_pointer_cast<rclcpp::Client<T>>(it->second);
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
/**
|
||||
* @brief Primary template (unspecialized) for skill action trait extraction.
|
||||
* Specializations must define: skill_name (constexpr const char*), success(result), message(result).
|
||||
*/
|
||||
template<typename ActionT>
|
||||
struct SkillActionTrait; // intentionally undefined primary template
|
||||
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::ArmSpaceControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "ArmSpaceControl";
|
||||
static bool success(const interfaces::action::ArmSpaceControl::Result & r) {return r.success;}
|
||||
static std::string message(const interfaces::action::ArmSpaceControl::Result & r)
|
||||
{
|
||||
return r.message;
|
||||
}
|
||||
};
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::HandControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "HandControl";
|
||||
static bool success(const interfaces::action::HandControl::Result & r) {return r.success;}
|
||||
static std::string message(const interfaces::action::HandControl::Result & r) {return r.message;}
|
||||
};
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::LegControl>
|
||||
{
|
||||
static constexpr const char * skill_name = "LegControl";
|
||||
static bool success(const interfaces::action::LegControl::Result & r) {return r.success;}
|
||||
static std::string message(const interfaces::action::LegControl::Result & r) {return r.message;}
|
||||
};
|
||||
template<>
|
||||
struct SkillActionTrait<interfaces::action::VisionGraspObject>
|
||||
{
|
||||
static constexpr const char * skill_name = "VisionGraspObject";
|
||||
static bool success(const interfaces::action::VisionGraspObject::Result & r) {return r.success;}
|
||||
static std::string message(const interfaces::action::VisionGraspObject::Result & r)
|
||||
{
|
||||
return r.message;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Tuple listing of all supported skill action types for dispatcher iteration.
|
||||
*/
|
||||
using SkillActionTypes = std::tuple<
|
||||
interfaces::action::ArmSpaceControl,
|
||||
interfaces::action::HandControl,
|
||||
interfaces::action::LegControl,
|
||||
interfaces::action::VisionGraspObject
|
||||
>;
|
||||
|
||||
/**
|
||||
* @brief Recursive compile-time dispatcher that matches a skill name to an action type and invokes send_and_wait.
|
||||
* @tparam I Current tuple index (internal use, defaults to 0).
|
||||
* @tparam TupleT Tuple type containing action types (e.g., SkillActionTypes).
|
||||
* @tparam RegistryT Registry providing template send_and_wait<ActionT>(...).
|
||||
* @param skill Canonical skill name to match (SkillActionTrait<ActionT>::skill_name).
|
||||
* @param topic Action topic name used when calling the registry.
|
||||
* @param reg Pointer to registry instance.
|
||||
* @param logger Logger for diagnostics.
|
||||
* @param timeout Wait timeout for the action result.
|
||||
* @param detail Populated with success/failure detail message.
|
||||
* @return true if execution succeeded and result success flag is true; false otherwise.
|
||||
*/
|
||||
template<size_t I = 0, typename TupleT, typename RegistryT>
|
||||
bool dispatch_skill_action(
|
||||
const std::string & skill, const std::string & topic, RegistryT * reg,
|
||||
rclcpp::Logger logger, std::chrono::nanoseconds timeout, std::string & detail)
|
||||
{
|
||||
if constexpr (I == std::tuple_size<TupleT>::value) {
|
||||
return false;
|
||||
} else {
|
||||
using ActionT = std::tuple_element_t<I, TupleT>;
|
||||
if (skill == SkillActionTrait<ActionT>::skill_name) {
|
||||
auto opt = reg->template send_and_wait<ActionT>(topic, logger, timeout);
|
||||
if (opt && opt->code == rclcpp_action::ResultCode::SUCCEEDED && opt->result) {
|
||||
bool ok = SkillActionTrait<ActionT>::success(*opt->result);
|
||||
detail = ok ? SkillActionTrait<ActionT>::message(*opt->result) : std::string(
|
||||
"result flag false");
|
||||
return ok;
|
||||
}
|
||||
detail = "action failed";
|
||||
return false;
|
||||
}
|
||||
return dispatch_skill_action<I + 1, TupleT>(skill, topic, reg, logger, timeout, detail);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
} // namespace brain
|
||||
282
src/brain/include/brain/sm_cerebellum.hpp
Normal file
282
src/brain/include/brain/sm_cerebellum.hpp
Normal file
@@ -0,0 +1,282 @@
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/smacc.hpp>
|
||||
#include <boost/mpl/list.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <atomic>
|
||||
#include <string>
|
||||
#include <functional>
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
namespace sc = boost::statechart;
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Runtime shared data
|
||||
// ----------------------------------------------------------------------------
|
||||
/**
|
||||
* @brief Shared runtime container passed implicitly between the state machine
|
||||
* (SMACC states) and the action server glue code.
|
||||
*
|
||||
* Fields are intentionally simple POD / atomics so they can be written from
|
||||
* multiple threads (worker threads, cancel callbacks, etc.) without heavy
|
||||
* synchronization. Lifecycle is a process‑wide singleton via runtime().
|
||||
*/
|
||||
struct CerebellumData
|
||||
{
|
||||
/** @brief External preemption / cancel flag (set by on_cancel or timeout). */
|
||||
std::atomic<bool> preempt{false}; // external cancel / interruption flag
|
||||
/** @brief Human readable reason associated with last preempt / completion. */
|
||||
std::string preempt_reason; // reason / last message
|
||||
|
||||
/**
|
||||
* @brief Result object produced by the asynchronous execute_fn functor.
|
||||
*
|
||||
* It aggregates overall success plus a textual summary and per‑sequence
|
||||
* counters so the state machine terminal states can publish action results.
|
||||
*/
|
||||
struct ExecResult
|
||||
{
|
||||
bool success{true}; ///< True if sequence finished without failure / cancel.
|
||||
std::string message; ///< Final textual summary of the sequence.
|
||||
int32_t succeeded_skills{0}; ///< Number of skills that succeeded.
|
||||
int32_t total_skills{0}; ///< Total skills attempted / requested.
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Bound by the action execute callback: performs the whole skill
|
||||
* sequence synchronously when invoked inside StExecuting supervision thread.
|
||||
*/
|
||||
std::function<ExecResult(rclcpp::Logger)> execute_fn; // installed by action execute
|
||||
/**
|
||||
* @brief Completion callback invoked by terminal states (Completed/Failed/Emergency)
|
||||
* to produce the actual action result (succeed/abort) back to the client.
|
||||
*/
|
||||
std::function<void(bool, const std::string &)> complete_cb; // called in terminal states
|
||||
|
||||
/** @brief Latched last success flag (copied from ExecResult). */
|
||||
std::atomic<bool> last_success{true};
|
||||
/** @brief Latched total skills count from ExecResult. */
|
||||
std::atomic<int32_t> last_total_skills{0};
|
||||
/** @brief Latched succeeded skills count from ExecResult. */
|
||||
std::atomic<int32_t> last_succeeded_skills{0};
|
||||
};
|
||||
|
||||
/**
|
||||
* @brief Singleton accessor for runtime shared data.
|
||||
* @return Reference to globally stored CerebellumData instance.
|
||||
*/
|
||||
inline CerebellumData & runtime()
|
||||
{
|
||||
static CerebellumData inst; return inst;
|
||||
}
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Events
|
||||
// ----------------------------------------------------------------------------
|
||||
/** @brief Event: a new goal has been received and accepted. */
|
||||
struct EvGoalReceived : sc::event<EvGoalReceived> {};
|
||||
/** @brief Event: asynchronous execution finished successfully. */
|
||||
struct EvExecutionFinished : sc::event<EvExecutionFinished> {};
|
||||
/** @brief Event: asynchronous execution ended with a failure (non emergency). */
|
||||
struct EvExecutionFailed : sc::event<EvExecutionFailed> {};
|
||||
/** @brief Event: unexpected exception / emergency condition occurred. */
|
||||
struct EvEmergency : sc::event<EvEmergency> {};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Forward declarations
|
||||
// ----------------------------------------------------------------------------
|
||||
struct StIdle; struct StExecuting; struct StCompleted; struct StFailed; struct StEmergency;
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// State Machine
|
||||
// ----------------------------------------------------------------------------
|
||||
/**
|
||||
* @brief Root SMACC2 state machine for the cerebellum execution lifecycle.
|
||||
* Initial state is StIdle.
|
||||
*/
|
||||
struct SmCerebellum : smacc2::SmaccStateMachineBase<SmCerebellum, StIdle>
|
||||
{ using SmaccStateMachineBase::SmaccStateMachineBase; };
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Idle
|
||||
// ----------------------------------------------------------------------------
|
||||
/**
|
||||
* @brief Idle state: waits for EvGoalReceived to start executing a skill sequence.
|
||||
*/
|
||||
struct StIdle : smacc2::SmaccState<StIdle, SmCerebellum>
|
||||
{
|
||||
using SmaccState::SmaccState;
|
||||
typedef boost::mpl::list<sc::custom_reaction<EvGoalReceived>> reactions; ///< Supported events.
|
||||
/**
|
||||
* @brief Reaction: transition to StExecuting when a goal arrives.
|
||||
* @param unused Event payload (none).
|
||||
* @return Transition result to Executing.
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{
|
||||
RCLCPP_INFO(this->getLogger(), "[SM] Transition Idle -> Executing (goal received)");
|
||||
return transit<StExecuting>();
|
||||
}
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Executing
|
||||
// ----------------------------------------------------------------------------
|
||||
/**
|
||||
* @brief Executing state: waits until an execute_fn is installed, then runs it
|
||||
* in a detached supervision thread, posting outcome events.
|
||||
*
|
||||
* It periodically logs a warning if execute_fn has not yet been set (defensive
|
||||
* against race conditions). Exceptions are mapped to EvEmergency.
|
||||
*/
|
||||
struct StExecuting : smacc2::SmaccState<StExecuting, SmCerebellum>
|
||||
{
|
||||
using SmaccState::SmaccState;
|
||||
typedef boost::mpl::list<sc::custom_reaction<EvExecutionFinished>, sc::custom_reaction<EvExecutionFailed>, sc::custom_reaction<EvEmergency>> reactions; ///< Reaction table.
|
||||
|
||||
/** @brief State entry: spawns supervision thread and invokes execute_fn when ready. */
|
||||
void onEntry()
|
||||
{
|
||||
RCLCPP_INFO(this->getLogger(), "[SM] Enter Executing");
|
||||
// Supervising thread waits until execute_fn is set, then runs it.
|
||||
std::thread{[this]() {
|
||||
auto & d = runtime();
|
||||
using namespace std::chrono_literals;
|
||||
auto last_warn = std::chrono::steady_clock::now();
|
||||
while (!d.execute_fn) {
|
||||
std::this_thread::sleep_for(5ms);
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
if (now - last_warn > 5s) {
|
||||
RCLCPP_WARN(this->getLogger(), "[SM] Still waiting for execute_fn to be set...");
|
||||
last_warn = now;
|
||||
}
|
||||
}
|
||||
try {
|
||||
auto res = d.execute_fn(this->getLogger());
|
||||
d.preempt.store(!res.success, std::memory_order_release);
|
||||
d.preempt_reason = res.message;
|
||||
d.last_success.store(res.success, std::memory_order_release);
|
||||
d.last_total_skills.store(res.total_skills, std::memory_order_release);
|
||||
d.last_succeeded_skills.store(res.succeeded_skills, std::memory_order_release);
|
||||
if (res.success) this->template postEvent<EvExecutionFinished>();
|
||||
else this->template postEvent<EvExecutionFailed>();
|
||||
} catch (const std::exception & e) {
|
||||
d.preempt.store(true, std::memory_order_release);
|
||||
d.preempt_reason = std::string("Exception: ") + e.what();
|
||||
this->template postEvent<EvEmergency>();
|
||||
} catch (...) {
|
||||
d.preempt.store(true, std::memory_order_release);
|
||||
d.preempt_reason = "Unknown exception";
|
||||
this->template postEvent<EvEmergency>();
|
||||
}
|
||||
}}.detach();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Reaction: successful completion -> StCompleted.
|
||||
* @return Transition to Completed state.
|
||||
*/
|
||||
sc::result react(const EvExecutionFinished &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Executing -> Completed"); return transit<StCompleted>(); }
|
||||
/**
|
||||
* @brief Reaction: functional failure -> StFailed.
|
||||
* @return Transition to Failed state.
|
||||
*/
|
||||
sc::result react(const EvExecutionFailed &)
|
||||
{ RCLCPP_WARN(this->getLogger(), "[SM] Transition Executing -> Failed"); return transit<StFailed>(); }
|
||||
/**
|
||||
* @brief Reaction: exception / emergency -> StEmergency.
|
||||
* @return Transition to Emergency state.
|
||||
*/
|
||||
sc::result react(const EvEmergency &)
|
||||
{ RCLCPP_ERROR(this->getLogger(), "[SM] Transition Executing -> Emergency"); return transit<StEmergency>(); }
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Completed
|
||||
// ----------------------------------------------------------------------------
|
||||
/**
|
||||
* @brief Completed state: final success path; invokes completion callback and
|
||||
* resets runtime fields before optionally accepting a new goal.
|
||||
*/
|
||||
struct StCompleted : smacc2::SmaccState<StCompleted, SmCerebellum>
|
||||
{
|
||||
using SmaccState::SmaccState;
|
||||
typedef boost::mpl::list<sc::custom_reaction<EvGoalReceived>> reactions; ///< Accepts new goals.
|
||||
/** @brief State entry: call complete_cb with success=true and clear runtime. */
|
||||
void onEntry()
|
||||
{
|
||||
auto & d = runtime();
|
||||
if (d.complete_cb) {
|
||||
bool success = d.last_success.load(std::memory_order_acquire) && !d.preempt.load();
|
||||
std::string msg = d.preempt_reason.empty() ? std::string("Done") : d.preempt_reason;
|
||||
d.complete_cb(success, msg);
|
||||
}
|
||||
d.preempt.store(false); d.preempt_reason.clear(); d.execute_fn = nullptr; d.complete_cb = nullptr;
|
||||
d.last_total_skills.store(0); d.last_succeeded_skills.store(0);
|
||||
}
|
||||
/**
|
||||
* @brief Reaction: new goal transitions back to Executing.
|
||||
* @return Transition to StExecuting.
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Completed -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Failed
|
||||
// ----------------------------------------------------------------------------
|
||||
/**
|
||||
* @brief Failed state: sequence ended in a non‑emergency failure. Reports
|
||||
* failure via completion callback and resets runtime data.
|
||||
*/
|
||||
struct StFailed : smacc2::SmaccState<StFailed, SmCerebellum>
|
||||
{
|
||||
using SmaccState::SmaccState;
|
||||
typedef boost::mpl::list<sc::custom_reaction<EvGoalReceived>> reactions; ///< Can accept a new goal.
|
||||
/** @brief State entry: invoke completion callback with failure status. */
|
||||
void onEntry()
|
||||
{
|
||||
auto & d = runtime();
|
||||
if (d.complete_cb) d.complete_cb(false, d.preempt_reason.empty()?"Failed":d.preempt_reason);
|
||||
d.execute_fn = nullptr; d.complete_cb = nullptr; d.preempt.store(false);
|
||||
}
|
||||
/**
|
||||
* @brief Reaction: accept new goal and transition to Executing.
|
||||
* @return Transition to StExecuting.
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Failed -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
};
|
||||
|
||||
// ----------------------------------------------------------------------------
|
||||
// Emergency
|
||||
// ----------------------------------------------------------------------------
|
||||
/**
|
||||
* @brief Emergency state: entered due to an unhandled exception in execute_fn.
|
||||
* Calls completion callback with an emergency prefix and resets runtime.
|
||||
*/
|
||||
struct StEmergency : smacc2::SmaccState<StEmergency, SmCerebellum>
|
||||
{
|
||||
using SmaccState::SmaccState;
|
||||
typedef boost::mpl::list<sc::custom_reaction<EvGoalReceived>> reactions; ///< Can accept a new goal.
|
||||
/** @brief State entry: log emergency and invoke completion callback. */
|
||||
void onEntry()
|
||||
{
|
||||
auto & d = runtime();
|
||||
if (d.complete_cb) d.complete_cb(false, std::string("EMERGENCY: ")+ d.preempt_reason);
|
||||
RCLCPP_ERROR(this->getLogger(), "[SM] Emergency: %s", d.preempt_reason.c_str());
|
||||
d.execute_fn = nullptr; d.complete_cb = nullptr; d.preempt.store(false);
|
||||
}
|
||||
/**
|
||||
* @brief Reaction: accept new goal and transition to Executing.
|
||||
* @return Transition to StExecuting.
|
||||
*/
|
||||
sc::result react(const EvGoalReceived &)
|
||||
{ RCLCPP_INFO(this->getLogger(), "[SM] Transition Emergency -> Executing (new goal)"); return transit<StExecuting>(); }
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
29
src/brain/package.xml
Normal file
29
src/brain/package.xml
Normal file
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>brain</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>behaviortree_cpp</depend>
|
||||
<depend>interfaces</depend>
|
||||
<depend>rclcpp_lifecycle</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>smacc2</depend>
|
||||
<depend>smacc2_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
<test_depend>ament_cmake_gtest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
41
src/brain/src/brain_node.cpp
Normal file
41
src/brain/src/brain_node.cpp
Normal file
@@ -0,0 +1,41 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
|
||||
#include "../include/brain/cerebellum_node.hpp"
|
||||
#include "../include/brain/cerebrum_node.hpp"
|
||||
|
||||
/**
|
||||
* @file brain_node.cpp
|
||||
* @brief Entry point launching both Cerebrum and Cerebellum nodes in a single-threaded executor.
|
||||
*/
|
||||
|
||||
/**
|
||||
* @brief Program entry: initialize ROS, create nodes, spin executor.
|
||||
*
|
||||
* Creates a SingleThreadedExecutor hosting both high-level (Cerebrum) and execution (Cerebellum)
|
||||
* components. Shutdown occurs when SIGINT or rclcpp shutdown is triggered.
|
||||
* @param argc Argument count.
|
||||
* @param argv Argument vector.
|
||||
* @return Zero on normal termination.
|
||||
*/
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
|
||||
rclcpp::Logger logger = rclcpp::get_logger("brain_node");
|
||||
RCLCPP_INFO(logger, "Starting brain node...");
|
||||
|
||||
auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
|
||||
rclcpp::NodeOptions options;
|
||||
|
||||
auto cerebellum_node = std::make_shared<brain::CerebellumNode>(options);
|
||||
auto cerebrum_node = std::make_shared<brain::CerebrumNode>(options);
|
||||
|
||||
executor->add_node(cerebellum_node);
|
||||
executor->add_node(cerebrum_node);
|
||||
|
||||
executor->spin();
|
||||
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
858
src/brain/src/cerebellum_node.cpp
Normal file
858
src/brain/src/cerebellum_node.cpp
Normal file
@@ -0,0 +1,858 @@
|
||||
// Copyright (c) 2025
|
||||
// This file implements the CerebellumNode which hosts execution infrastructure
|
||||
// for skill/action/service orchestration.
|
||||
|
||||
// Own header first.
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
|
||||
// C / C++ standard library.
|
||||
#include <chrono>
|
||||
#include <cctype>
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
|
||||
// Third-party / external libraries (ROS2, ament).
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include <rclcpp/logger.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
// Project headers.
|
||||
#include "brain/action_registry.hpp"
|
||||
#include "brain/sm_cerebellum.hpp"
|
||||
#include "brain/constants.hpp"
|
||||
|
||||
// Skill interfaces (clients are registered by SkillManager).
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
#include "interfaces/action/leg_control.hpp"
|
||||
#include "interfaces/action/vision_grasp_object.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp" // ExecuteBtAction bridge
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Construct the CerebellumNode.
|
||||
*
|
||||
* Initializes internal registries (action server/client, skill manager), declares and
|
||||
* loads parameters, loads the skills YAML, sets up statistics publisher/timer, registers
|
||||
* the ExecuteBtAction action server, and optionally launches the SMACC2 state machine
|
||||
* unless disabled at compile time via BRAIN_DISABLE_SM.
|
||||
* @param options Optional ROS 2 node options.
|
||||
*/
|
||||
CerebellumNode::CerebellumNode(const rclcpp::NodeOptions & options)
|
||||
: Node("cerebellum_node", options)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Cerebellum node started");
|
||||
action_registry_ = std::make_unique<ActionServerRegistry>(this);
|
||||
action_clients_ = std::make_unique<ActionClientRegistry>(this);
|
||||
skill_manager_ = std::make_unique<SkillManager>(this, action_clients_.get(), nullptr);
|
||||
|
||||
DeclareAndLoadParameters();
|
||||
LoadSkillsFile();
|
||||
SetupStatsTimerAndPublisher();
|
||||
SetupExecuteBtServer();
|
||||
|
||||
#ifndef BRAIN_DISABLE_SM
|
||||
if (!sm_exec_) {
|
||||
sm_exec_.reset(smacc2::run_async<brain::SmCerebellum>());
|
||||
RCLCPP_INFO(this->get_logger(), "SMACC2 SmCerebellum started");
|
||||
}
|
||||
#else
|
||||
RCLCPP_DEBUG(
|
||||
this->get_logger(), "SMACC2 disabled via BRAIN_DISABLE_SM for lightweight test context");
|
||||
#endif
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Declare all configurable ROS parameters and load their initial values.
|
||||
*
|
||||
* Also parses the consolidated skill timeout override string parameter (skill_timeouts)
|
||||
* formatted as "SkillA=30,SkillB=10" into the internal map used by GetTimeoutForSkill.
|
||||
* Missing or malformed entries are skipped with a warning.
|
||||
*/
|
||||
void CerebellumNode::DeclareAndLoadParameters()
|
||||
{
|
||||
// Core behavior parameters
|
||||
this->declare_parameter<std::string>("skills_file", "");
|
||||
this->declare_parameter<bool>("abort_on_failure", abort_on_failure_);
|
||||
this->declare_parameter<double>("default_action_timeout_sec", default_action_timeout_sec_);
|
||||
this->declare_parameter<double>(
|
||||
"vision_grasp_object_timeout_sec",
|
||||
vision_grasp_object_timeout_sec_);
|
||||
this->declare_parameter<std::string>("skill_timeouts", ""); // format: SkillA=30,SkillB=10
|
||||
this->declare_parameter<double>("stats_log_interval_sec", stats_log_interval_sec_);
|
||||
// Read back
|
||||
this->get_parameter("abort_on_failure", abort_on_failure_);
|
||||
this->get_parameter("default_action_timeout_sec", default_action_timeout_sec_);
|
||||
this->get_parameter("vision_grasp_object_timeout_sec", vision_grasp_object_timeout_sec_);
|
||||
this->get_parameter("stats_log_interval_sec", stats_log_interval_sec_);
|
||||
|
||||
// Parse skill_timeouts param into map
|
||||
std::string raw;
|
||||
this->get_parameter("skill_timeouts", raw);
|
||||
if (!raw.empty()) {
|
||||
std::string token; token.reserve(raw.size());
|
||||
auto flush = [&]() {
|
||||
if (token.empty()) {return;}
|
||||
auto eq = token.find('=');
|
||||
if (eq != std::string::npos) {
|
||||
auto key = token.substr(0, eq);
|
||||
auto vstr = token.substr(eq + 1);
|
||||
try {
|
||||
skill_timeouts_[key] = std::stod(vstr);
|
||||
} catch (...) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "Bad timeout value for %s: %s", key.c_str(),
|
||||
vstr.c_str());
|
||||
}
|
||||
}
|
||||
token.clear();
|
||||
};
|
||||
for (char c : raw) {
|
||||
if (c == ',' || c == ';' || c == ' ' || c == '\t') {
|
||||
flush();
|
||||
} else {
|
||||
token.push_back(c);
|
||||
}
|
||||
}
|
||||
flush();
|
||||
if (!skill_timeouts_.empty()) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "[cerebellum_node] Loaded %zu skill-specific timeouts",
|
||||
skill_timeouts_.size());
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "[cerebellum_node] abort_on_failure=%s default_action_timeout=%.1f vision_grasp_object_timeout=%.1f",
|
||||
abort_on_failure_ ? "true" : "false", default_action_timeout_sec_,
|
||||
vision_grasp_object_timeout_sec_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Load the skills definition YAML file.
|
||||
*
|
||||
* If parameter 'skills_file' is empty, attempts to locate a default file within the
|
||||
* package share directory (config/robot_skills.yaml). Each successfully loaded skill
|
||||
* name is logged for visibility. Failure to load results in a warning but is non-fatal.
|
||||
*/
|
||||
void CerebellumNode::LoadSkillsFile()
|
||||
{
|
||||
std::string skills_file;
|
||||
this->get_parameter("skills_file", skills_file);
|
||||
if (skills_file.empty()) {
|
||||
try {
|
||||
const auto share = ament_index_cpp::get_package_share_directory("brain");
|
||||
skills_file = share + std::string("/config/robot_skills.yaml");
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "[cerebellum_node] Using default skills file: %s",
|
||||
skills_file.c_str());
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "[cerebellum_node] Could not locate default skills file: %s",
|
||||
e.what());
|
||||
}
|
||||
}
|
||||
if (skills_file.empty()) {return;}
|
||||
if (!skill_manager_->load_from_file(skills_file)) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "[cerebellum_node] Failed to load skills from %s", skills_file.c_str());
|
||||
return;
|
||||
}
|
||||
for (const auto & kv : skill_manager_->skills()) {
|
||||
RCLCPP_INFO(this->get_logger(), "[cerebellum_node] Loaded skill '%s'", kv.first.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Create the statistics publisher and periodic logging timer.
|
||||
*
|
||||
* The timer is only created when stats_log_interval_sec_ is strictly positive. Each tick
|
||||
* invokes LogStatsPeriodic to publish and log aggregate counts.
|
||||
*/
|
||||
void CerebellumNode::SetupStatsTimerAndPublisher()
|
||||
{
|
||||
stats_pub_ = this->create_publisher<std_msgs::msg::String>("/brain/skill_stats", 10);
|
||||
if (stats_log_interval_sec_ > 0) {
|
||||
stats_timer_ = this->create_wall_timer(
|
||||
std::chrono::duration<double>(stats_log_interval_sec_),
|
||||
[this]() {LogStatsPeriodic();});
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Register the ExecuteBtAction action server.
|
||||
*
|
||||
* Goal callback enforces monotonic epoch progression (rejecting stale requests) and posts
|
||||
* an SmACC2 EvGoalReceived event when available. Execution callback delegates to
|
||||
* RunExecuteBtGoal which performs the ordered skill execution logic.
|
||||
*/
|
||||
void CerebellumNode::SetupExecuteBtServer()
|
||||
{
|
||||
using EBA = interfaces::action::ExecuteBtAction;
|
||||
ActionServerRegistry::Handlers<EBA> h;
|
||||
// Track active goal handle for cancellation handling
|
||||
static std::weak_ptr<rclcpp_action::ServerGoalHandle<EBA>> active_goal_wptr;
|
||||
h.on_goal = [this](const rclcpp_action::GoalUUID &, std::shared_ptr<const EBA::Goal> goal) {
|
||||
if (!goal) {return rclcpp_action::GoalResponse::REJECT;}
|
||||
if (goal->epoch < current_epoch_) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "Reject stale ExecuteBtAction goal epoch=%u < current=%u", goal->epoch,
|
||||
current_epoch_.load());
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
if (goal->epoch > current_epoch_) {
|
||||
current_epoch_ = goal->epoch;
|
||||
RCLCPP_INFO(this->get_logger(), "Switch to new epoch %u", current_epoch_.load());
|
||||
}
|
||||
if (sm_exec_ && sm_exec_->signalDetector) {
|
||||
sm_exec_->signalDetector->postEvent(new brain::EvGoalReceived());
|
||||
}
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "ExecuteBtAction GoalReceived goal: %s", goal->action_name.c_str());
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
// Accept cancel requests: set runtime preempt flag; the execute loop will notice and stop
|
||||
h.on_cancel = [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<EBA>> gh) {
|
||||
auto & d = brain::runtime();
|
||||
d.preempt.store(true, std::memory_order_release);
|
||||
d.preempt_reason = "Cancelled";
|
||||
RCLCPP_WARN(this->get_logger(), "[ExecuteBtAction] cancellation requested (epoch=%u)", gh->get_goal()->epoch);
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
};
|
||||
h.execute = [this](const std::shared_ptr<rclcpp_action::ServerGoalHandle<EBA>> goal_handle) {
|
||||
// Instead of executing inline, install execute_fn & complete_cb for state machine.
|
||||
active_goal_wptr = goal_handle;
|
||||
RunExecuteBtGoal(goal_handle);
|
||||
};
|
||||
action_registry_->register_action_server<EBA>(brain::kExecuteBtActionName, h);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Parse a raw delimited skill list string into individual skill tokens.
|
||||
*
|
||||
* Accepted delimiters: comma, semicolon, space, tab. Removes a trailing '_H' suffix from
|
||||
* each token to normalize behavior-tree leaf decorated names back to canonical skill names.
|
||||
* @param raw Input raw skill list string.
|
||||
* @return Vector of parsed (possibly duplicate) canonical skill names.
|
||||
*/
|
||||
std::vector<std::string> CerebellumNode::ParseSkillSequence(
|
||||
const std::string & raw) const
|
||||
{
|
||||
std::vector<std::string> skills;
|
||||
skills.reserve(8);
|
||||
std::string token;
|
||||
token.reserve(raw.size());
|
||||
for (char c : raw) {
|
||||
if (c == ',' || c == ';' || c == ' ' || c == '\t') {
|
||||
if (!token.empty()) {
|
||||
skills.push_back(token);
|
||||
token.clear();
|
||||
}
|
||||
} else {
|
||||
token.push_back(c);
|
||||
}
|
||||
}
|
||||
if (!token.empty()) {skills.push_back(token);}
|
||||
// Trim optional suffix _H.
|
||||
for (auto & s : skills) {
|
||||
if (s.size() > 2 && s.rfind("_H") == s.size() - 2) {
|
||||
s.erase(s.size() - 2);
|
||||
}
|
||||
}
|
||||
return skills;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert a CamelCase identifier to snake_case.
|
||||
*
|
||||
* Inserts underscores before interior uppercase letters and lowercases all characters.
|
||||
* @param s CamelCase input string.
|
||||
* @return snake_case transformed string.
|
||||
*/
|
||||
std::string CerebellumNode::ToSnake(const std::string & s) const
|
||||
{
|
||||
std::string out;
|
||||
out.reserve(s.size() * 2);
|
||||
for (size_t i = 0; i < s.size(); ++i) {
|
||||
unsigned char uc = static_cast<unsigned char>(s[i]);
|
||||
if (::isupper(uc)) {
|
||||
if (i > 0) {out.push_back('_');}
|
||||
out.push_back(static_cast<char>(::tolower(uc)));
|
||||
} else {
|
||||
out.push_back(static_cast<char>(uc));
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Resolve timeout value for a specific skill.
|
||||
*
|
||||
* Precedence order: explicit per-skill override map > VisionGraspObject special timeout >
|
||||
* default configured timeout.
|
||||
* @param skill Skill name.
|
||||
* @return Timeout in seconds for the given skill.
|
||||
*/
|
||||
double CerebellumNode::GetTimeoutForSkill(const std::string & skill) const
|
||||
{
|
||||
auto it = skill_timeouts_.find(skill);
|
||||
if (it != skill_timeouts_.end()) {return it->second;}
|
||||
if (skill == "VisionGraspObject") {return vision_grasp_object_timeout_sec_;}
|
||||
return default_action_timeout_sec_;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Publish one stage feedback message for ExecuteBtAction.
|
||||
*
|
||||
* Wraps construction and publishing of the feedback object so callers only
|
||||
* provide the semantic fields. Epoch is filled from current_epoch_.
|
||||
* @param gh Goal handle to publish feedback on.
|
||||
* @param stage Stage label (START, RUN, WAIT, END).
|
||||
* @param current_skill Skill name currently processed.
|
||||
* @param progress Overall fractional progress in current sequence [0,1].
|
||||
* @param detail Optional detail string (may encode status or diagnostics).
|
||||
*/
|
||||
void CerebellumNode::PublishFeedbackStage(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & gh,
|
||||
const std::string & stage,
|
||||
const std::string & current_skill,
|
||||
float progress,
|
||||
const std::string & detail) const
|
||||
{
|
||||
auto fb = std::make_shared<interfaces::action::ExecuteBtAction::Feedback>();
|
||||
fb->epoch = current_epoch_;
|
||||
fb->stage = stage;
|
||||
fb->current_skill = current_skill;
|
||||
fb->progress = progress;
|
||||
fb->detail = detail;
|
||||
gh->publish_feedback(fb);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Execute a single skill (action or service) with feedback and timeout handling.
|
||||
*
|
||||
* Delegates to ExecuteActionSkill or ExecuteServiceSkill depending on interface kind.
|
||||
* Unknown kinds reported as failure. Emits mandatory END feedback on failure paths.
|
||||
* @param skill Skill name.
|
||||
* @param chosen_topic Resolved action client topic key.
|
||||
* @param spec Skill specification metadata (interface list, etc.).
|
||||
* @param detail (out) Additional status information; may remain empty if OK.
|
||||
* @param index Zero-based index of this skill in the current sequence.
|
||||
* @param total_skills Total number of skills in the sequence.
|
||||
* @param goal_handle ExecuteBtAction goal handle for feedback.
|
||||
* @return true if execution succeeded, else false.
|
||||
*/
|
||||
bool CerebellumNode::ExecuteSingleSkill(
|
||||
const std::string & skill, const std::string & chosen_topic, const SkillSpec & spec,
|
||||
std::string & detail, int index, int total_skills,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
|
||||
{
|
||||
if (spec.interfaces.empty()) {
|
||||
detail = "No interfaces";
|
||||
return false;
|
||||
}
|
||||
const std::string kind = GetInterfaceKind(spec);
|
||||
if (kind == "action") {
|
||||
return ExecuteActionSkill(
|
||||
skill, chosen_topic, spec, detail, index, total_skills, goal_handle);
|
||||
}
|
||||
if (kind == "service") {
|
||||
return ExecuteServiceSkill(skill, detail, goal_handle);
|
||||
}
|
||||
detail = "Unknown interface kind";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Extract lowercase interface kind string from first interface entry.
|
||||
*
|
||||
* Example interface tokens: package/Type.action -> kind = "action".
|
||||
* Returns empty string if specification has no interfaces.
|
||||
* @param spec Skill specification.
|
||||
* @return std::string Lowercase kind token (e.g., "action", "service").
|
||||
*/
|
||||
std::string CerebellumNode::GetInterfaceKind(const SkillSpec & spec) const
|
||||
{
|
||||
if (spec.interfaces.empty()) {return {};}
|
||||
std::string first = spec.interfaces.front();
|
||||
auto pos = first.rfind('.');
|
||||
std::string kind = (pos == std::string::npos) ? std::string{} : first.substr(pos + 1);
|
||||
for (auto & c : kind) {
|
||||
c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
|
||||
}
|
||||
return kind;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Retry-wait loop for action server availability with WAIT feedback.
|
||||
*
|
||||
* Attempts up to kMaxAttempts, each attempt waiting a short period. Publishes
|
||||
* WAIT feedback containing the attempt number. On success returns true.
|
||||
* @param topic Action client key/topic string.
|
||||
* @param skill Skill name (for feedback context).
|
||||
* @param base_progress Fractional progress before this skill runs.
|
||||
* @param goal_handle ExecuteBtAction goal handle for feedback.
|
||||
* @param detail (out) Set to error text on failure.
|
||||
* @return true if server became available; false otherwise.
|
||||
*/
|
||||
bool CerebellumNode::WaitForActionServer(
|
||||
const std::string & topic,
|
||||
const std::string & skill,
|
||||
float base_progress,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
std::string & detail) const
|
||||
{
|
||||
constexpr int kMaxAttempts = 8;
|
||||
for (int attempt = 1; attempt <= kMaxAttempts; ++attempt) {
|
||||
if (action_clients_->is_server_available(topic, std::chrono::milliseconds(50))) {
|
||||
return true;
|
||||
}
|
||||
PublishFeedbackStage(
|
||||
goal_handle, "WAIT", skill, base_progress,
|
||||
std::string("attempt=") + std::to_string(attempt));
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(250));
|
||||
}
|
||||
detail = "Server unavailable after retries";
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Execute an action-based skill with periodic RUN feedback.
|
||||
*
|
||||
* Spawns a worker thread to perform the blocking dispatch_skill_action call.
|
||||
* Main thread emits RUN feedback until completion or timeout. Progress scales
|
||||
* within the skill's time slice based on elapsed/timeout.
|
||||
* @param skill Skill name.
|
||||
* @param topic Resolved action client topic.
|
||||
* @param spec Skill spec (currently unused beyond interface detection).
|
||||
* @param detail (out) Populated with result detail or error.
|
||||
* @param index Index of skill in sequence.
|
||||
* @param total_skills Total skills count.
|
||||
* @param goal_handle Goal handle for feedback.
|
||||
* @return true on success, false otherwise.
|
||||
*/
|
||||
bool CerebellumNode::ExecuteActionSkill(
|
||||
const std::string & skill,
|
||||
const std::string & topic,
|
||||
const SkillSpec & spec,
|
||||
std::string & detail,
|
||||
int index,
|
||||
int total_skills,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
|
||||
{
|
||||
(void)spec; // Currently unused beyond interface kind selection.
|
||||
if (!action_clients_->has_client(topic)) {
|
||||
detail = "Client not registered";
|
||||
return false;
|
||||
}
|
||||
const float base_progress =
|
||||
(total_skills > 0) ? (static_cast<float>(index) / static_cast<float>(total_skills)) : 0.f;
|
||||
PublishFeedbackStage(goal_handle, "START", skill, base_progress, "");
|
||||
if (!WaitForActionServer(topic, skill, base_progress, goal_handle, detail)) {
|
||||
return false;
|
||||
}
|
||||
auto timeout = std::chrono::duration_cast<std::chrono::nanoseconds>(
|
||||
std::chrono::duration<double>(GetTimeoutForSkill(skill)));
|
||||
return RunActionSkillWithProgress(
|
||||
skill, topic, timeout, index, total_skills, goal_handle, detail);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Worker-thread based action execution with periodic RUN feedback.
|
||||
*
|
||||
* Launches a thread executing dispatch_skill_action while the main thread publishes
|
||||
* RUN stage feedback at fixed intervals to reflect intra-skill progress relative to the
|
||||
* skill's timeout budget.
|
||||
* @param skill Skill name.
|
||||
* @param topic Action topic/client key.
|
||||
* @param timeout_ns Absolute timeout for the action (nanoseconds).
|
||||
* @param index Skill's zero-based index within the sequence.
|
||||
* @param total_skills Total number of skills in the sequence.
|
||||
* @param goal_handle ExecuteBtAction goal handle for feedback publication.
|
||||
* @param detail [out] Detailed status or error information.
|
||||
* @return true if the action reports success; false otherwise.
|
||||
*/
|
||||
bool CerebellumNode::RunActionSkillWithProgress(
|
||||
const std::string & skill,
|
||||
const std::string & topic,
|
||||
std::chrono::nanoseconds timeout_ns,
|
||||
int index,
|
||||
int total_skills,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
std::string & detail)
|
||||
{
|
||||
std::atomic<bool> finished{false};
|
||||
bool ok = false;
|
||||
auto logger = this->get_logger();
|
||||
std::string local_detail;
|
||||
std::thread worker([&]() {
|
||||
ok = brain::dispatch_skill_action<0, brain::SkillActionTypes>(
|
||||
skill, topic, action_clients_.get(), logger, timeout_ns, local_detail);
|
||||
finished.store(true, std::memory_order_release);
|
||||
});
|
||||
const auto start_steady = std::chrono::steady_clock::now();
|
||||
const double timeout_sec = timeout_ns.count() / 1e9;
|
||||
while (!finished.load(std::memory_order_acquire)) {
|
||||
auto now = std::chrono::steady_clock::now();
|
||||
double elapsed = std::chrono::duration<double>(now - start_steady).count();
|
||||
double phase = timeout_sec > 0.0 ? std::min(elapsed / timeout_sec, 0.99) : 0.0;
|
||||
float seq_progress =
|
||||
(total_skills > 0) ? static_cast<float>((index + phase) / total_skills) : 0.f;
|
||||
PublishFeedbackStage(goal_handle, "RUN", skill, seq_progress, "");
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(200));
|
||||
}
|
||||
if (worker.joinable()) {worker.join();}
|
||||
detail = local_detail;
|
||||
if (!ok && detail.empty()) {detail = "Unsupported action skill";}
|
||||
return ok;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Execute a supported service-based skill (currently VisionObjectRecognition).
|
||||
*
|
||||
* Performs synchronous service call with simple START/END feedback. Unsupported
|
||||
* service names or failures emit END with FAIL classification.
|
||||
* @param skill Skill name.
|
||||
* @param detail (out) Status / diagnostic text.
|
||||
* @param goal_handle ExecuteBtAction goal handle.
|
||||
* @return true if service call succeeds with positive result; false otherwise.
|
||||
*/
|
||||
bool CerebellumNode::ExecuteServiceSkill(
|
||||
const std::string & skill,
|
||||
std::string & detail,
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
|
||||
{
|
||||
if (skill == "VisionObjectRecognition") {
|
||||
using Srv = interfaces::srv::VisionObjectRecognition;
|
||||
auto client = skill_manager_->get_typed_service_client<Srv>(skill);
|
||||
if (!client) {
|
||||
detail = "Service client missing";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
|
||||
return false;
|
||||
}
|
||||
if (!client->wait_for_service(std::chrono::seconds(2))) {
|
||||
detail = "Service unavailable";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
|
||||
return false;
|
||||
}
|
||||
PublishFeedbackStage(goal_handle, "START", skill, 0.f, "");
|
||||
auto req = std::make_shared<Srv::Request>();
|
||||
req->object_id = "demo_object";
|
||||
auto future = client->async_send_request(req);
|
||||
// NOTE:
|
||||
// Do NOT call rclcpp::spin_until_future_complete(this->get_node_base_interface(), ...)
|
||||
// Doing so would attempt to add this node to a temporary executor a second time
|
||||
// and trigger the runtime error: "Node has already been added".
|
||||
// The action execute callback already runs in its own thread; the main executor
|
||||
// is spinning elsewhere and will process the service response.
|
||||
if (future.wait_for(std::chrono::seconds(5)) != std::future_status::ready) {
|
||||
detail = "Service call timeout";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
|
||||
return false;
|
||||
}
|
||||
auto resp = future.get();
|
||||
if (!resp) {
|
||||
detail = "Null response";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
|
||||
return false;
|
||||
}
|
||||
if (!resp->found) {
|
||||
detail = "Not found";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
|
||||
return false;
|
||||
}
|
||||
detail = "found pose";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "OK");
|
||||
return true;
|
||||
}
|
||||
detail = "Unsupported service skill";
|
||||
PublishFeedbackStage(goal_handle, "END", skill, 0.f, "FAIL");
|
||||
return false;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Update success/failure counters for a single skill execution.
|
||||
*
|
||||
* Thread-safe via internal mutex protecting the statistics maps.
|
||||
* @param skill Skill name.
|
||||
* @param success True if the execution succeeded; false otherwise.
|
||||
*/
|
||||
void CerebellumNode::RecordSkillResult(const std::string & skill, bool success)
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(stats_mutex_);
|
||||
if (success) {
|
||||
++skill_success_counts_[skill];
|
||||
} else {
|
||||
++skill_failure_counts_[skill];
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Periodic statistics logger and publisher callback.
|
||||
*
|
||||
* Constructs a concise textual summary containing overall sequence counters and per-skill
|
||||
* success/failure tallies, publishes it on /brain/skill_stats, and logs the same.
|
||||
*/
|
||||
void CerebellumNode::LogStatsPeriodic()
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(stats_mutex_);
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "[stats] sequences success=%zu failure=%zu",
|
||||
total_sequence_success_, total_sequence_failure_);
|
||||
for (const auto & kv : skill_success_counts_) {
|
||||
size_t fails = skill_failure_counts_[kv.first];
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), " skill %s: OK=%zu FAIL=%zu", kv.first.c_str(), kv.second,
|
||||
fails);
|
||||
}
|
||||
if (stats_pub_) {
|
||||
std_msgs::msg::String msg; std::ostringstream oss;
|
||||
oss << "seq_ok=" << total_sequence_success_ << ",seq_fail=" << total_sequence_failure_;
|
||||
for (const auto & kv : skill_success_counts_) {
|
||||
size_t fails = skill_failure_counts_[kv.first];
|
||||
oss << ";" << kv.first << "=" << kv.second << "/" << fails;
|
||||
}
|
||||
msg.data = oss.str();
|
||||
stats_pub_->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Execute an accepted ExecuteBtAction goal end-to-end.
|
||||
*
|
||||
* Validates and parses the goal into an ordered skill list, then iteratively executes
|
||||
* each skill, publishing START/RUN/WAIT/END feedback, respecting the abort_on_failure
|
||||
* policy, updating per-skill and sequence statistics, and finally reporting the result.
|
||||
* @param goal_handle Goal handle provided by the action server layer.
|
||||
*/
|
||||
void CerebellumNode::RunExecuteBtGoal(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle)
|
||||
{
|
||||
std::vector<std::string> skills;
|
||||
if (!ValidateAndParseGoal(goal_handle, skills)) {
|
||||
return; // goal aborted
|
||||
}
|
||||
// Install execute_fn & complete_cb into runtime so state machine owns lifecycle.
|
||||
auto & d = brain::runtime();
|
||||
d.execute_fn = [this, goal_handle, skills](rclcpp::Logger logger) -> brain::CerebellumData::ExecResult {
|
||||
(void)logger; // we already have this->get_logger() inside helpers
|
||||
return ExecuteSequence(goal_handle, skills);
|
||||
};
|
||||
d.complete_cb = [this, goal_handle](bool /*success*/, const std::string & message) {
|
||||
// Build result message from runtime latched fields
|
||||
auto & d = brain::runtime();
|
||||
using EBA = interfaces::action::ExecuteBtAction;
|
||||
auto res = std::make_shared<EBA::Result>();
|
||||
res->success = d.last_success.load();
|
||||
res->total_skills = d.last_total_skills;
|
||||
res->succeeded_skills = d.last_succeeded_skills;
|
||||
res->message = message.empty() ? std::string("(no message)") : message;
|
||||
if (res->success) {
|
||||
goal_handle->succeed(res);
|
||||
RCLCPP_INFO(this->get_logger(), "%s", res->message.c_str());
|
||||
} else {
|
||||
goal_handle->abort(res);
|
||||
RCLCPP_ERROR(this->get_logger(), "%s", res->message.c_str());
|
||||
}
|
||||
};
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Execute the full ordered skill sequence inside the state-machine supervised context.
|
||||
*
|
||||
* This function is bound as execute_fn and invoked by StExecuting in a supervision thread.
|
||||
* It iterates over the provided skill list, honoring:
|
||||
* - Preemption / cancellation (runtime().preempt flag)
|
||||
* - Optional overall sequence timeout (parameter overall_sequence_timeout_sec)
|
||||
* - Per-skill abort_on_failure policy
|
||||
*
|
||||
* For each skill it resolves the action/service topic, performs execution via ExecuteSingleSkill,
|
||||
* publishes END feedback (including final per-skill status), aggregates statistics, and builds a
|
||||
* textual summary string of the form: Seq{ActionName}: SkillA(OK), SkillB(FAIL)[detail], ...
|
||||
*
|
||||
* Sequence level success is true only if all executed skills succeeded or failures were allowed
|
||||
* to continue (abort_on_failure_ == false) and no cancellation or timeout occurred.
|
||||
*
|
||||
* @param goal_handle ExecuteBtAction goal handle used for publishing feedback / final result.
|
||||
* @param skills Ordered list of skill names parsed from the goal.
|
||||
* @return brain::CerebellumData::ExecResult Aggregated outcome for the state machine.
|
||||
*/
|
||||
brain::CerebellumData::ExecResult CerebellumNode::ExecuteSequence(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
const std::vector<std::string> & skills)
|
||||
{
|
||||
brain::CerebellumData::ExecResult result;
|
||||
result.total_skills = static_cast<int32_t>(skills.size());
|
||||
if (skills.empty()) {
|
||||
result.success = false;
|
||||
result.message = "Empty skill list";
|
||||
return result;
|
||||
}
|
||||
// Optional overall sequence timeout parameter (seconds); default 0 = disabled
|
||||
double overall_timeout_sec = 0.0;
|
||||
(void)this->get_parameter("overall_sequence_timeout_sec", overall_timeout_sec); // ignore failure
|
||||
const bool timeout_enabled = overall_timeout_sec > 0.0;
|
||||
auto seq_start = std::chrono::steady_clock::now();
|
||||
|
||||
std::ostringstream summary;
|
||||
summary << "Seq{" << goal_handle->get_goal()->action_name << "}: ";
|
||||
bool overall_ok = true;
|
||||
size_t loop_index = 0;
|
||||
for (size_t seq_index = 0; seq_index < skills.size(); ++seq_index) {
|
||||
// Preempt / cancel check
|
||||
if (brain::runtime().preempt.load(std::memory_order_acquire)) {
|
||||
overall_ok = false;
|
||||
summary << "(Cancelled)";
|
||||
break;
|
||||
}
|
||||
// Global timeout check
|
||||
if (timeout_enabled) {
|
||||
double elapsed = std::chrono::duration<double>(std::chrono::steady_clock::now() - seq_start).count();
|
||||
if (elapsed > overall_timeout_sec) {
|
||||
overall_ok = false;
|
||||
brain::runtime().preempt.store(true, std::memory_order_release);
|
||||
brain::runtime().preempt_reason = "Sequence timeout";
|
||||
summary << "(Timeout)";
|
||||
break;
|
||||
}
|
||||
}
|
||||
const auto & skill = skills[seq_index];
|
||||
const std::string topic = ResolveTopicForSkill(skill);
|
||||
RCLCPP_INFO(this->get_logger(), "[ExecuteBtAction] Dispatch skill=%s topic=%s", skill.c_str(), topic.c_str());
|
||||
bool ok = false; std::string detail;
|
||||
auto it = skill_manager_->skills().find(skill);
|
||||
if (it == skill_manager_->skills().end()) {
|
||||
detail = "Skill not defined";
|
||||
PublishFeedbackStage(
|
||||
goal_handle, "END", skill,
|
||||
static_cast<float>(seq_index + 1) / static_cast<float>(skills.size()),
|
||||
"FAIL:undefined");
|
||||
} else {
|
||||
ok = ExecuteSingleSkill(
|
||||
skill, topic, it->second, detail, static_cast<int>(seq_index), static_cast<int>(skills.size()), goal_handle);
|
||||
PublishFeedbackStage(
|
||||
goal_handle, "END", skill,
|
||||
static_cast<float>(seq_index + 1) / static_cast<float>(skills.size()),
|
||||
ok ? (detail.empty() ? "OK" : std::string("OK:") + detail)
|
||||
: (detail.empty() ? "FAIL" : std::string("FAIL:") + detail));
|
||||
}
|
||||
RecordSkillResult(skill, ok);
|
||||
if (ok) { ++result.succeeded_skills; }
|
||||
overall_ok = overall_ok && ok;
|
||||
summary << skill << '(' << (ok ? "OK" : "FAIL") << ')';
|
||||
if (!detail.empty()) summary << '[' << detail << ']';
|
||||
if (loop_index + 1 < skills.size()) summary << ", ";
|
||||
++loop_index;
|
||||
if (!ok) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Skill %s failed%s", skill.c_str(),
|
||||
abort_on_failure_ ? ", abort sequence" : " (continue)");
|
||||
if (abort_on_failure_) break;
|
||||
}
|
||||
}
|
||||
// stats update
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(stats_mutex_);
|
||||
if (overall_ok) ++total_sequence_success_; else ++total_sequence_failure_;
|
||||
}
|
||||
result.success = overall_ok;
|
||||
result.message = summary.str();
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Validate the presence of a goal and parse its action_name into skill tokens.
|
||||
*
|
||||
* Aborts the goal with an appropriate message if the goal pointer is null or the parsed
|
||||
* skill list is empty.
|
||||
* @param goal_handle Goal handle from action server.
|
||||
* @param skills_out [out] Parsed list of skills (unchanged on failure).
|
||||
* @return true if validation succeeds and skills_out is populated; false otherwise.
|
||||
*/
|
||||
bool CerebellumNode::ValidateAndParseGoal(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
std::vector<std::string> & skills_out) const
|
||||
{
|
||||
using EBA = interfaces::action::ExecuteBtAction;
|
||||
auto logger = this->get_logger();
|
||||
const auto goal = goal_handle->get_goal();
|
||||
if (!goal) {
|
||||
auto res = std::make_shared<EBA::Result>();
|
||||
res->success = false;
|
||||
res->message = "Null goal";
|
||||
goal_handle->abort(res);
|
||||
return false;
|
||||
}
|
||||
const std::string & raw = goal->action_name;
|
||||
RCLCPP_INFO(logger, "[ExecuteBtAction] Received goal action_name=%s", raw.c_str());
|
||||
skills_out = ParseSkillSequence(raw);
|
||||
if (skills_out.empty()) {
|
||||
auto res = std::make_shared<EBA::Result>();
|
||||
res->success = false;
|
||||
res->message = "Empty skill sequence";
|
||||
goal_handle->abort(res);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Resolve an action topic/client key for a skill.
|
||||
*
|
||||
* Prefers the snake_case variant if a registered client exists; otherwise tries the
|
||||
* original CamelCase skill name; finally falls back to snake_case even if unregistered
|
||||
* (allowing later availability checks to fail cleanly).
|
||||
* @param skill Skill name.
|
||||
* @return Chosen topic string.
|
||||
*/
|
||||
std::string CerebellumNode::ResolveTopicForSkill(const std::string & skill) const
|
||||
{
|
||||
const std::string snake = ToSnake(skill);
|
||||
if (action_clients_->has_client(snake)) {return snake;}
|
||||
if (action_clients_->has_client(skill)) {return skill;}
|
||||
return snake; // fallback
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Finalize an ExecuteBtAction goal with the aggregated result summary.
|
||||
*
|
||||
* Emits either succeed() or abort() on the goal handle, logging the composed summary
|
||||
* which includes per-skill success/failure annotations.
|
||||
* @param goal_handle Goal handle.
|
||||
* @param overall_ok True if all (or allowed subset under abort policy) skills succeeded.
|
||||
* @param succeeded_skills Count of individually successful skills.
|
||||
* @param total_skills Total number of attempted skills.
|
||||
* @param summary_stream Stream containing the pre-built textual summary.
|
||||
*/
|
||||
void CerebellumNode::FinalizeExecuteBtResult(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<interfaces::action::ExecuteBtAction>> & goal_handle,
|
||||
bool overall_ok,
|
||||
int32_t succeeded_skills,
|
||||
int32_t total_skills,
|
||||
const std::ostringstream & summary_stream)
|
||||
{
|
||||
using EBA = interfaces::action::ExecuteBtAction;
|
||||
auto res = std::make_shared<EBA::Result>();
|
||||
res->success = overall_ok;
|
||||
res->message = summary_stream.str();
|
||||
res->total_skills = total_skills;
|
||||
res->succeeded_skills = succeeded_skills;
|
||||
auto logger = this->get_logger();
|
||||
if (overall_ok) {
|
||||
goal_handle->succeed(res);
|
||||
RCLCPP_INFO(logger, "%s", res->message.c_str());
|
||||
} else {
|
||||
goal_handle->abort(res);
|
||||
RCLCPP_ERROR(logger, "%s", res->message.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
684
src/brain/src/cerebrum_node.cpp
Normal file
684
src/brain/src/cerebrum_node.cpp
Normal file
@@ -0,0 +1,684 @@
|
||||
// Copyright 2025
|
||||
// CerebrumNode orchestrates behavior tree construction and ticking,
|
||||
// mediating between high-level skill selection (possibly via a VLM model)
|
||||
// and the unified ExecuteBtAction channel handled by the Cerebellum.
|
||||
|
||||
// Own header first.
|
||||
#include "brain/cerebrum_node.hpp"
|
||||
|
||||
// C++ standard library
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
#include <random>
|
||||
#include <sstream>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <utility>
|
||||
|
||||
#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include <behaviortree_cpp/action_node.h>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
// Project headers
|
||||
#include "brain/constants.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
|
||||
using namespace std::chrono_literals; // NOLINT
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
bool HasActionInterface(const SkillSpec & spec)
|
||||
{
|
||||
for (const auto & iface : spec.interfaces) {
|
||||
auto pos = iface.rfind('.');
|
||||
if (pos != std::string::npos) {
|
||||
std::string kind = iface.substr(pos + 1);
|
||||
for (auto & c : kind) {
|
||||
c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
|
||||
}
|
||||
if (kind == "action" || kind == "service") {
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
struct UpdatingFlagGuard
|
||||
{
|
||||
explicit UpdatingFlagGuard(std::atomic<bool> & f)
|
||||
: flag(f) {}
|
||||
std::atomic<bool> & flag;
|
||||
~UpdatingFlagGuard() {flag = false;}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
/**
|
||||
* @brief Construct the CerebrumNode.
|
||||
*
|
||||
* Initializes registries (action clients, BT registry, skill manager), declares and loads
|
||||
* parameters, loads skill definitions, registers BT actions, sets up timers for periodic
|
||||
* task dispatch (sequence selection) and BT ticking, and creates a rebuild service.
|
||||
* @thread_safety Construction is single-threaded; no concurrent access occurs.
|
||||
*/
|
||||
CerebrumNode::CerebrumNode(const rclcpp::NodeOptions & options)
|
||||
: rclcpp::Node("cerebrum_node", options)
|
||||
{
|
||||
action_registry_ = std::make_unique<ActionClientRegistry>(this);
|
||||
bt_registry_ = std::make_unique<BTRegistry>(factory_, this);
|
||||
skill_manager_ = std::make_unique<SkillManager>(this, action_registry_.get(), bt_registry_.get());
|
||||
|
||||
// Parameters
|
||||
this->declare_parameter<std::string>("skills_file", "");
|
||||
this->declare_parameter<double>("node_timeout_sec", node_timeout_sec_);
|
||||
this->declare_parameter<bool>(
|
||||
"allow_bt_rebuild_during_execution",
|
||||
allow_bt_rebuild_during_execution_);
|
||||
this->declare_parameter<std::string>("allowed_action_skills", "");
|
||||
this->declare_parameter<std::string>("fixed_skill_sequence", "");
|
||||
this->declare_parameter<int>(
|
||||
"random_skill_pick_count",
|
||||
static_cast<int>(random_skill_pick_count_));
|
||||
|
||||
std::string skills_file;
|
||||
this->get_parameter("skills_file", skills_file);
|
||||
if (skills_file.empty()) {
|
||||
try {
|
||||
auto share = ament_index_cpp::get_package_share_directory("brain");
|
||||
skills_file = share + std::string("/config/robot_skills.yaml");
|
||||
RCLCPP_INFO(this->get_logger(), "Using default skills file: %s", skills_file.c_str());
|
||||
} catch (const std::exception & e) {
|
||||
RCLCPP_WARN(this->get_logger(), "Could not locate default skills file: %s", e.what());
|
||||
}
|
||||
}
|
||||
if (!skills_file.empty()) {
|
||||
if (!skill_manager_->load_from_file(skills_file)) {
|
||||
RCLCPP_WARN(this->get_logger(), "Failed to load skills from %s", skills_file.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
this->get_parameter("node_timeout_sec", node_timeout_sec_);
|
||||
this->get_parameter("allow_bt_rebuild_during_execution", allow_bt_rebuild_during_execution_);
|
||||
int random_pick_param = static_cast<int>(random_skill_pick_count_);
|
||||
this->get_parameter("random_skill_pick_count", random_pick_param);
|
||||
if (random_pick_param < 1) {
|
||||
random_pick_param = 1;
|
||||
}
|
||||
random_skill_pick_count_ = static_cast<std::size_t>(random_pick_param);
|
||||
|
||||
std::string allowed_raw;
|
||||
this->get_parameter("allowed_action_skills", allowed_raw);
|
||||
if (!allowed_raw.empty()) {
|
||||
// parse allowed action skills list
|
||||
allowed_action_skills_ = ParseListString(allowed_raw);
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Allowed action skills count=%zu",
|
||||
allowed_action_skills_.size());
|
||||
}
|
||||
|
||||
std::string fixed_seq_raw;
|
||||
this->get_parameter("fixed_skill_sequence", fixed_seq_raw);
|
||||
if (!fixed_seq_raw.empty()) {
|
||||
auto tmp = ParseListString(fixed_seq_raw);
|
||||
if (!tmp.empty()) {
|
||||
fixed_skill_sequence_ = tmp;
|
||||
}
|
||||
}
|
||||
if (fixed_skill_sequence_) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Using fixed skill sequence size=%zu",
|
||||
fixed_skill_sequence_->size());
|
||||
}
|
||||
|
||||
RegisterSkillBtActions();
|
||||
RegisterActionClient();
|
||||
|
||||
bt_timer_ = this->create_wall_timer(
|
||||
10ms,
|
||||
[this]() {
|
||||
ExecuteBehaviorTree();
|
||||
});
|
||||
task_timer_ = this->create_wall_timer(
|
||||
5000ms,
|
||||
[this]() {
|
||||
CerebrumTask();
|
||||
});
|
||||
|
||||
rebuild_service_ = this->create_service<std_srvs::srv::Trigger>(
|
||||
"cerebrum/rebuild_now",
|
||||
[this](
|
||||
const std::shared_ptr<std_srvs::srv::Trigger::Request>,
|
||||
std::shared_ptr<std_srvs::srv::Trigger::Response> resp) {
|
||||
if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
|
||||
resp->success = false;
|
||||
resp->message = "BT running and rebuild not allowed";
|
||||
return;
|
||||
}
|
||||
RunVlmModel();
|
||||
CancelActiveExecuteBtGoal();
|
||||
UpdateBehaviorTree();
|
||||
resp->success = true;
|
||||
resp->message = "Rebuild triggered";
|
||||
});
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "CerebrumNode initialized (timeout=%.2f random_pick=%zu)", node_timeout_sec_,
|
||||
random_skill_pick_count_);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Register a BT leaf action with lifecycle handlers.
|
||||
* @param type_name BT node type (<SkillName>_H).
|
||||
* @param handlers Callback bundle (start/running/halted).
|
||||
* @throws May propagate BT::BehaviorTreeException on duplicate registration.
|
||||
* @thread_safety Call during initialization prior to concurrent ticking.
|
||||
*/
|
||||
void CerebrumNode::RegisterBtAction(
|
||||
const std::string & type_name,
|
||||
const BTActionHandlers & handlers)
|
||||
{
|
||||
bt_registry_->register_bt_action(type_name, handlers);
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Explicitly send the unified ExecuteBtAction goal (manual/test trigger).
|
||||
* @param bt_action_name Typically brain::kExecuteBtActionName.
|
||||
* @thread_safety Relies on ActionClientRegistry thread-safety for send.
|
||||
*/
|
||||
void CerebrumNode::SendExecuteBt(const std::string & bt_action_name)
|
||||
{
|
||||
if (!action_registry_) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(), "Action registry not initialized; cannot send %s",
|
||||
bt_action_name.c_str());
|
||||
return;
|
||||
}
|
||||
action_registry_->send(bt_action_name, this->get_logger());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Ensure ExecuteBtAction client is registered (idempotent) and attach callbacks.
|
||||
*
|
||||
* Goal builder serializes current sequence into comma list; feedback handler updates
|
||||
* per-node execution state with progress and results; result handler finalizes outstanding
|
||||
* nodes. Epoch filtering prevents stale feedback application.
|
||||
* @thread_safety Called during initialization; callbacks internally use mutex/atomics.
|
||||
*/
|
||||
void CerebrumNode::RegisterActionClient()
|
||||
{
|
||||
using EBA = interfaces::action::ExecuteBtAction;
|
||||
if (registered_actions_.count(brain::kExecuteBtActionName)) {
|
||||
return;
|
||||
}
|
||||
registered_actions_.insert(brain::kExecuteBtActionName);
|
||||
action_registry_->register_client<EBA>(
|
||||
brain::kExecuteBtActionName,
|
||||
// Goal builder
|
||||
[this]() {
|
||||
EBA::Goal g;
|
||||
g.epoch = static_cast<uint32_t>(epoch_filter_.epoch());
|
||||
std::lock_guard<std::mutex> lk(exec_mutex_);
|
||||
std::ostringstream oss;
|
||||
if (single_skill_goal_override_) {
|
||||
oss << *single_skill_goal_override_;
|
||||
} else {
|
||||
for (std::size_t i = 0; i < current_sequence_skills_.size(); ++i) {
|
||||
if (i) {oss << ',';}
|
||||
oss << current_sequence_skills_[i];
|
||||
}
|
||||
}
|
||||
g.action_name = oss.str();
|
||||
return g;
|
||||
},
|
||||
// Feedback
|
||||
[this](
|
||||
typename ActionClientRegistry::GoalHandle<EBA>::SharedPtr,
|
||||
const std::shared_ptr<const EBA::Feedback> feedback) {
|
||||
if (!feedback) {
|
||||
return;
|
||||
}
|
||||
if (!epoch_filter_.accept(
|
||||
feedback->epoch, feedback->current_skill, bt_pending_run_))
|
||||
{
|
||||
return;
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(exec_mutex_);
|
||||
// Match per-skill BT node by skill_name.
|
||||
const std::string bt_type = feedback->current_skill + std::string("_H");
|
||||
auto it = per_node_exec_.find(bt_type);
|
||||
if (it != per_node_exec_.end()) {
|
||||
auto & info = it->second;
|
||||
if (!info.in_flight && !info.result) {
|
||||
// Late feedback for an already closed node; ignore.
|
||||
} else {
|
||||
if (feedback->stage == "RUN" && feedback->progress + 1e-4 < info.last_progress) {
|
||||
return; // non-monotonic
|
||||
}
|
||||
if (feedback->stage == "RUN") {
|
||||
info.last_progress = feedback->progress;
|
||||
}
|
||||
if (feedback->stage == "END") {
|
||||
info.result_code = ParseResultDetail(feedback->detail);
|
||||
info.result = (info.result_code == ExecResultCode::SUCCESS);
|
||||
if (info.result_code == ExecResultCode::SUCCESS) {
|
||||
info.awaiting_result = true;
|
||||
info.end_feedback_time = this->now();
|
||||
} else {
|
||||
info.in_flight = false;
|
||||
info.awaiting_result = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"[ExecuteBtAction][fb][epoch=%u] stage=%s skill=%s progress=%.2f detail=%s",
|
||||
feedback->epoch,
|
||||
feedback->stage.c_str(),
|
||||
feedback->current_skill.c_str(),
|
||||
feedback->progress,
|
||||
feedback->detail.c_str());
|
||||
},
|
||||
// Result
|
||||
[this](const ActionClientRegistry::GoalHandle<EBA>::WrappedResult & res) {
|
||||
bool ok = (
|
||||
res.code == rclcpp_action::ResultCode::SUCCEEDED &&
|
||||
res.result &&
|
||||
res.result->success);
|
||||
if (ok) {
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"[ExecuteBtAction][result] OK: %s",
|
||||
res.result ? res.result->message.c_str() : "<null>");
|
||||
} else {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(),
|
||||
"[ExecuteBtAction][result] FAIL code=%d msg=%s",
|
||||
static_cast<int>(res.code),
|
||||
res.result ? res.result->message.c_str() : "<null>");
|
||||
}
|
||||
for (auto & kv : per_node_exec_) {
|
||||
auto & info = kv.second;
|
||||
if (info.in_flight || info.awaiting_result) {
|
||||
info.in_flight = false;
|
||||
info.awaiting_result = false;
|
||||
info.result = ok; // authoritative final
|
||||
info.result_code = ok ? ExecResultCode::SUCCESS : ExecResultCode::FAILURE;
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Send a previously registered standalone action (not the unified BT action).
|
||||
* @param name Action client key.
|
||||
* @thread_safety Depends on underlying registry implementation; assumed safe for concurrent sends.
|
||||
*/
|
||||
void CerebrumNode::SendAction(const std::string & name)
|
||||
{
|
||||
if (!action_registry_) {
|
||||
return;
|
||||
}
|
||||
action_registry_->send(name, this->get_logger());
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Periodic high-level task (timer callback) selecting and applying a new sequence.
|
||||
*
|
||||
* Steps:
|
||||
* 1. Skip if BT run is active and rebuilds are disallowed.
|
||||
* 2. Run model hook to choose sequence.
|
||||
* 3. Cancel active ExecuteBtAction goal (if any) to avoid stale execution.
|
||||
* 4. Rebuild behavior tree (increments epoch, resets tracking) and schedule run.
|
||||
* @thread_safety Timer callback; coordinates with mutex and atomic flags.
|
||||
*/
|
||||
void CerebrumNode::CerebrumTask()
|
||||
{
|
||||
if (bt_pending_run_ && !allow_bt_rebuild_during_execution_) {
|
||||
return;
|
||||
}
|
||||
// Use correctly cased API methods
|
||||
RunVlmModel();
|
||||
CancelActiveExecuteBtGoal();
|
||||
UpdateBehaviorTree();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Tick the behavior tree root if a run is pending.
|
||||
*
|
||||
* Clears bt_pending_run_ when root terminates (SUCCESS/FAILURE). Skips ticking while a
|
||||
* rebuild is in progress or the tree root is null.
|
||||
* @thread_safety Timer callback; guarded by atomics; tree_ access not concurrently mutated except during rebuild.
|
||||
*/
|
||||
void CerebrumNode::ExecuteBehaviorTree()
|
||||
{
|
||||
if (bt_updating_ || !bt_pending_run_) {
|
||||
return;
|
||||
}
|
||||
if (!tree_.rootNode()) {
|
||||
bt_pending_run_ = false;
|
||||
return;
|
||||
}
|
||||
auto status = tree_.rootNode()->executeTick();
|
||||
if (status != BT::NodeStatus::RUNNING) {
|
||||
bt_pending_run_ = false;
|
||||
RCLCPP_INFO(this->get_logger(), "BT finished status=%s", BT::toStr(status, true).c_str());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Rebuild the behavior tree for the active sequence, advancing the epoch.
|
||||
*
|
||||
* Resets per-node execution tracking and marks bt_pending_run_ to start ticking. Uses an
|
||||
* atomic flag and RAII guard to prevent re-entrancy.
|
||||
* @thread_safety Not re-entrant (atomic gate); safe against concurrent timer callbacks.
|
||||
*/
|
||||
void CerebrumNode::UpdateBehaviorTree()
|
||||
{
|
||||
if (bt_updating_) {
|
||||
return;
|
||||
}
|
||||
bt_updating_ = true;
|
||||
UpdatingFlagGuard guard(bt_updating_);
|
||||
BT::Tree new_tree;
|
||||
if (!bt_registry_->build_tree_from_sequence(active_sequence_, new_tree)) {
|
||||
RCLCPP_ERROR(
|
||||
this->get_logger(), "Failed building BT for sequence %s",
|
||||
active_sequence_.c_str());
|
||||
return;
|
||||
}
|
||||
try {
|
||||
tree_.haltTree();
|
||||
} catch (...) {
|
||||
// Swallow halt exceptions.
|
||||
}
|
||||
tree_ = std::move(new_tree);
|
||||
uint64_t new_epoch = epoch_filter_.epoch() + 1;
|
||||
std::vector<std::string> seq_skills;
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(exec_mutex_);
|
||||
seq_skills = current_sequence_skills_;
|
||||
}
|
||||
epoch_filter_.reset_epoch(new_epoch, seq_skills);
|
||||
per_node_exec_.clear();
|
||||
bt_pending_run_ = true;
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"BT updated epoch=%llu sequence=%s",
|
||||
static_cast<unsigned long long>(new_epoch),
|
||||
active_sequence_.c_str());
|
||||
}
|
||||
|
||||
void CerebrumNode::RegisterSkillBtActions()
|
||||
{
|
||||
if (!skill_manager_) {return;}
|
||||
for (const auto & kv : skill_manager_->skills()) {
|
||||
const auto & skill = kv.second;
|
||||
const std::string bt_type = skill.name + std::string("_H");
|
||||
BTActionHandlers handlers;
|
||||
handlers.on_start = [this, bt_type, skill](rclcpp::Node *, BT::TreeNode &) {
|
||||
auto & info = per_node_exec_[bt_type];
|
||||
if (info.in_flight || info.result) {
|
||||
return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
|
||||
}
|
||||
info.in_flight = true;
|
||||
info.result = false;
|
||||
info.result_code = ExecResultCode::UNKNOWN;
|
||||
info.start_time = this->now();
|
||||
info.skill_name = skill.name;
|
||||
info.last_progress = 0.0;
|
||||
info.cancel_state = PerNodeExecInfo::CancelState::NONE;
|
||||
// Override goal builder to send only this skill.
|
||||
single_skill_goal_override_ = skill.name;
|
||||
if (!action_registry_->wait_for_server(brain::kExecuteBtActionName, 1500ms)) {
|
||||
RCLCPP_ERROR(this->get_logger(), "ExecuteBtAction server unavailable");
|
||||
info.in_flight = false;
|
||||
info.result = false;
|
||||
info.result_code = ExecResultCode::FAILURE;
|
||||
single_skill_goal_override_.reset();
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
action_registry_->send(brain::kExecuteBtActionName, this->get_logger());
|
||||
// Clear override after send to avoid affecting other triggers.
|
||||
single_skill_goal_override_.reset();
|
||||
return BT::NodeStatus::RUNNING;
|
||||
};
|
||||
handlers.on_running = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
|
||||
auto it = per_node_exec_.find(bt_type);
|
||||
if (it == per_node_exec_.end()) {return BT::NodeStatus::FAILURE;}
|
||||
auto & info = it->second;
|
||||
if (info.in_flight) {
|
||||
double elapsed = (this->now() - info.start_time).seconds();
|
||||
if (elapsed > node_timeout_sec_) {
|
||||
info.in_flight = false;
|
||||
info.result = false;
|
||||
info.result_code = ExecResultCode::FAILURE;
|
||||
return BT::NodeStatus::FAILURE;
|
||||
}
|
||||
return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
if (info.awaiting_result) {
|
||||
double wait_elapsed = (this->now() - info.end_feedback_time).seconds();
|
||||
if (wait_elapsed > result_wait_warn_sec_) {
|
||||
RCLCPP_WARN_THROTTLE(
|
||||
this->get_logger(), *this->get_clock(), 2000,
|
||||
"Skill %s awaiting final result %.2fs after END feedback",
|
||||
bt_type.c_str(), wait_elapsed);
|
||||
}
|
||||
return BT::NodeStatus::RUNNING;
|
||||
}
|
||||
return info.result ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
|
||||
};
|
||||
handlers.on_halted = [this, bt_type](rclcpp::Node *, BT::TreeNode &) {
|
||||
RCLCPP_INFO(this->get_logger(), "Node halted %s", bt_type.c_str());
|
||||
};
|
||||
try {RegisterBtAction(bt_type, handlers);} catch (const BT::BehaviorTreeException & e) {
|
||||
RCLCPP_DEBUG(this->get_logger(), "BT type exists %s: %s", bt_type.c_str(), e.what());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Randomly select up to count distinct action-capable skills (respecting whitelist).
|
||||
* @param count Maximum number of skills.
|
||||
* @return Vector of selected skill names (size <= count).
|
||||
* @thread_safety Read-only access to skill_manager_ (assumed immutable post-init).
|
||||
*/
|
||||
std::vector<std::string> CerebrumNode::PickRandomActionSkills(std::size_t count)
|
||||
{
|
||||
if (!skill_manager_) {
|
||||
return {};
|
||||
}
|
||||
std::vector<std::string> pool;
|
||||
pool.reserve(skill_manager_->skills().size());
|
||||
const bool filter = !allowed_action_skills_.empty();
|
||||
for (const auto & kv : skill_manager_->skills()) {
|
||||
const auto & spec = kv.second;
|
||||
if (!HasActionInterface(spec)) {
|
||||
continue;
|
||||
}
|
||||
if (filter &&
|
||||
std::find(
|
||||
allowed_action_skills_.begin(),
|
||||
allowed_action_skills_.end(),
|
||||
spec.name) == allowed_action_skills_.end())
|
||||
{
|
||||
continue;
|
||||
}
|
||||
pool.push_back(spec.name);
|
||||
}
|
||||
if (pool.empty()) {
|
||||
return {};
|
||||
}
|
||||
std::shuffle(
|
||||
pool.begin(),
|
||||
pool.end(),
|
||||
std::mt19937{std::random_device{}()});
|
||||
if (pool.size() > count) {
|
||||
pool.resize(count);
|
||||
}
|
||||
return pool;
|
||||
}
|
||||
|
||||
BTSequenceSpec CerebrumNode::MakeSequenceFromSkills(const std::vector<std::string> & names)
|
||||
{
|
||||
BTSequenceSpec seq; seq.reserve(names.size());
|
||||
for (const auto & n : names) {
|
||||
seq.emplace_back(BTActionSpec{n + std::string("_H"), n + std::string("_H"), {}});
|
||||
}
|
||||
return seq;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Build minimal XML representation of a sequence for diagnostics.
|
||||
* @param seq Sequence specification.
|
||||
* @return XML string containing a single root <Sequence> element.
|
||||
* @thread_safety Pure function.
|
||||
*/
|
||||
std::string CerebrumNode::BuildXmlFromSequence(const BTSequenceSpec & seq) const
|
||||
{
|
||||
std::ostringstream oss;
|
||||
oss << "<root BTCPP_format=\"4\"><BehaviorTree ID=\"MainTree\"><Sequence name=\"root_sequence\">";
|
||||
for (const auto & act : seq) {
|
||||
oss << "<" << act.type << " name=\"" << act.name << "\"/>";
|
||||
}
|
||||
oss << "</Sequence></BehaviorTree></root>";
|
||||
return oss.str();
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Choose and activate a sequence (fixed preferred over random selection pool).
|
||||
*
|
||||
* Updates current_sequence_skills_ under mutex, registers sequence, and sets it active.
|
||||
* Does not rebuild immediately; the caller triggers UpdateBehaviorTree().
|
||||
* @thread_safety Uses mutex for sequence list assignment.
|
||||
*/
|
||||
void CerebrumNode::RunVlmModel()
|
||||
{
|
||||
auto register_and_activate = [this](
|
||||
const std::string & name,
|
||||
const std::vector<std::string> & skills) {
|
||||
if (skills.empty()) {
|
||||
return false;
|
||||
}
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(exec_mutex_);
|
||||
current_sequence_skills_ = skills;
|
||||
}
|
||||
auto seq = MakeSequenceFromSkills(skills);
|
||||
RegisterBtSequence(name, seq);
|
||||
SetActiveSequence(name);
|
||||
return true;
|
||||
};
|
||||
if (fixed_skill_sequence_) {
|
||||
if (!register_and_activate("FixedSequence", *fixed_skill_sequence_)) {
|
||||
return;
|
||||
}
|
||||
} else {
|
||||
auto picked = PickRandomActionSkills(random_skill_pick_count_);
|
||||
if (!register_and_activate("VLMSequence", picked)) {
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(),
|
||||
"No action-capable skills available");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Parse a delimited string into unique tokens (first occurrence preserved).
|
||||
* @param raw Input delimited string.
|
||||
* @return Vector of unique tokens.
|
||||
* @thread_safety Pure static utility.
|
||||
*/
|
||||
std::vector<std::string> CerebrumNode::ParseListString(const std::string & raw)
|
||||
{
|
||||
std::vector<std::string> result;
|
||||
result.reserve(8);
|
||||
std::unordered_set<std::string> seen;
|
||||
std::string token;
|
||||
token.reserve(raw.size());
|
||||
auto flush = [&]() {
|
||||
if (!token.empty()) {
|
||||
if (seen.insert(token).second) {
|
||||
result.push_back(token);
|
||||
}
|
||||
token.clear();
|
||||
}
|
||||
};
|
||||
for (char c : raw) {
|
||||
if (c == ',' || c == ';' || c == ' ' || c == '\t' || c == '\n') {
|
||||
flush();
|
||||
} else {
|
||||
token.push_back(c);
|
||||
}
|
||||
}
|
||||
flush();
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Cancel the active unified ExecuteBtAction goal (if any).
|
||||
* @thread_safety Pass-through to registry cancellation (assumed thread-safe).
|
||||
*/
|
||||
void CerebrumNode::CancelActiveExecuteBtGoal()
|
||||
{
|
||||
if (action_registry_) {
|
||||
action_registry_->cancel(
|
||||
brain::kExecuteBtActionName,
|
||||
this->get_logger());
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Legacy epoch skill setter (now a no-op; filtering handled elsewhere).
|
||||
* @param skills Ignored.
|
||||
* @thread_safety Pure no-op.
|
||||
*/
|
||||
void CerebrumNode::SetEpochSkills(const std::vector<std::string> & skills)
|
||||
{
|
||||
(void)skills; // Now handled by epoch_filter_
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Heuristically classify textual detail into an ExecResultCode.
|
||||
* @param detail Input detail string (prefix or code= pattern interpreted).
|
||||
* @return ExecResultCode classification (UNKNOWN if unrecognized).
|
||||
* @thread_safety Pure function.
|
||||
*/
|
||||
CerebrumNode::ExecResultCode CerebrumNode::ParseResultDetail(const std::string & detail)
|
||||
{
|
||||
if (detail.rfind("OK", 0) == 0 || detail.rfind("SUCCESS", 0) == 0) {
|
||||
return ExecResultCode::SUCCESS;
|
||||
}
|
||||
if (detail.rfind("FAIL", 0) == 0 || detail.rfind("ERROR", 0) == 0) {
|
||||
return ExecResultCode::FAILURE;
|
||||
}
|
||||
if (detail.rfind("CANCEL", 0) == 0 || detail.rfind("CANCELLED", 0) == 0) {
|
||||
return ExecResultCode::CANCELLED;
|
||||
}
|
||||
auto pos = detail.find("code=");
|
||||
if (pos != std::string::npos) {
|
||||
auto code = detail.substr(pos + 5, 16);
|
||||
if (code.rfind("0", 0) == 0) {
|
||||
return ExecResultCode::SUCCESS;
|
||||
}
|
||||
if (code.rfind("cancel", 0) == 0) {
|
||||
return ExecResultCode::CANCELLED;
|
||||
}
|
||||
return ExecResultCode::FAILURE;
|
||||
}
|
||||
return ExecResultCode::UNKNOWN;
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
283
src/brain/src/skill_manager.cpp
Normal file
283
src/brain/src/skill_manager.cpp
Normal file
@@ -0,0 +1,283 @@
|
||||
/**
|
||||
* @file skill_manager.cpp
|
||||
* @brief Implements SkillManager YAML loading and interface client registration logic.
|
||||
*
|
||||
* Originally generated by an internal script (gen_skill_manager.py). Manual edits now focus on
|
||||
* documentation and style consistency. If regeneration is reintroduced, ensure the generator
|
||||
* preserves Doxygen blocks or performs a merge.
|
||||
*/
|
||||
//#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include "brain/skill_manager.hpp"
|
||||
|
||||
#include <optional>
|
||||
#include <cctype>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
#include "interfaces/action/leg_control.hpp"
|
||||
#include "interfaces/action/vision_grasp_object.hpp"
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
|
||||
using interfaces::action::ArmSpaceControl;
|
||||
using interfaces::action::HandControl;
|
||||
using interfaces::action::LegControl;
|
||||
using interfaces::action::VisionGraspObject;
|
||||
using interfaces::srv::VisionObjectRecognition;
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Convert a string to lowercase (ASCII only).
|
||||
* @param s Input string (copied).
|
||||
* @return Lowercase transformed string.
|
||||
*/
|
||||
static inline std::string to_lower(std::string s)
|
||||
{
|
||||
for (char & c : s) {
|
||||
c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Convert a CamelCase / TitleCase identifier to snake_case.
|
||||
* @param s Input string.
|
||||
* @return snake_case string.
|
||||
*/
|
||||
static inline std::string to_snake_case(const std::string & s)
|
||||
{
|
||||
std::string out;
|
||||
out.reserve(s.size() * 2);
|
||||
for (size_t i = 0; i < s.size(); ++i) {
|
||||
unsigned char uc = static_cast<unsigned char>(s[i]);
|
||||
if (::isupper(uc)) {
|
||||
if (i > 0) {out.push_back('_');}
|
||||
out.push_back(static_cast<char>(::tolower(uc)));
|
||||
} else {
|
||||
out.push_back(static_cast<char>(uc));
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Parsed representation of an interface token BaseName.kind.
|
||||
*/
|
||||
struct ParsedInterface { std::string base; std::string kind; };
|
||||
|
||||
/**
|
||||
* @brief Parse an interface entry of the form BaseName.kind.
|
||||
* @param entry Raw interface token.
|
||||
* @return ParsedInterface on success; std::nullopt if malformed.
|
||||
*/
|
||||
static std::optional<ParsedInterface> parse_interface_entry(const std::string & entry)
|
||||
{
|
||||
const auto pos = entry.rfind('.');
|
||||
if (pos == std::string::npos || pos == 0 || pos + 1 >= entry.size()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
ParsedInterface p;
|
||||
p.base = entry.substr(0, pos);
|
||||
p.kind = to_lower(entry.substr(pos + 1));
|
||||
return p;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Load skill specifications from a YAML file.
|
||||
*
|
||||
* Expected top-level YAML node is a sequence; each element maps onto SkillSpec fields. Interfaces
|
||||
* are registered immediately upon successful parsing. Duplicate skill names overwrite earlier
|
||||
* entries (last one wins).
|
||||
* @param yaml_path Path to YAML document.
|
||||
* @return true if parsing succeeded and at least zero skills were processed (false only on format error).
|
||||
*/
|
||||
bool SkillManager::load_from_file(const std::string & yaml_path)
|
||||
{
|
||||
YAML::Node root = YAML::LoadFile(yaml_path);
|
||||
if (!root || !root.IsSequence()) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Invalid skills YAML: expected sequence at root");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (const auto & item : root) {
|
||||
if (!item.IsMap()) {continue;}
|
||||
SkillSpec s;
|
||||
s.name = item["name"].as<std::string>("");
|
||||
s.version = item["version"].as<std::string>("");
|
||||
s.description = item["description"].as<std::string>("");
|
||||
if (item["required_skills"]) {
|
||||
for (const auto & v : item["required_skills"]) {
|
||||
s.required_skills.push_back(v.as<std::string>());
|
||||
}
|
||||
}
|
||||
if (item["interfaces"]) {
|
||||
for (const auto & v : item["interfaces"]) {
|
||||
s.interfaces.push_back(v.as<std::string>());
|
||||
}
|
||||
}
|
||||
if (s.name.empty()) {continue;}
|
||||
skills_[s.name] = s;
|
||||
register_interfaces_(s);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Loaded %zu skills from %s", skills_.size(), yaml_path.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Enumerate skills that declare at least one action interface.
|
||||
* @return Vector of (skill_name, interface_base) pairs (one per qualifying skill).
|
||||
*/
|
||||
std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_skills() const
|
||||
{
|
||||
std::vector<std::pair<std::string, std::string>> out;
|
||||
for (const auto & kv : skills_) {
|
||||
const auto & s = kv.second;
|
||||
for (const auto & iface : s.interfaces) {
|
||||
auto p = parse_interface_entry(iface);
|
||||
if (p && p->kind == "action") {out.emplace_back(s.name, p->base); break;}
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Register action and service interfaces declared by a SkillSpec.
|
||||
*
|
||||
* Action registrars create action clients with result callbacks logging success/failure. Service
|
||||
* registrars create and retain service clients (stored for lifetime management). Topic naming:
|
||||
* currently uses the skill's canonical name (CamelCase) directly.
|
||||
* @param s Skill specification.
|
||||
*/
|
||||
void SkillManager::register_interfaces_(const SkillSpec & s)
|
||||
{
|
||||
// 由代码生成器根据 YAML 填充的注册器表（接口基名 -> 注册逻辑）
|
||||
static const std::unordered_map<std::string, std::function<void(const std::string &,
|
||||
const std::string &)>> action_registrars = {
|
||||
{"ArmSpaceControl", [this](const std::string & topic, const std::string & internal_skill) {
|
||||
if (action_clients_) {
|
||||
action_clients_->register_client<ArmSpaceControl>(
|
||||
topic,
|
||||
[]() {ArmSpaceControl::Goal g; return g;},
|
||||
nullptr,
|
||||
[this,
|
||||
internal_skill](const ActionClientRegistry::GoalHandle<ArmSpaceControl>::WrappedResult
|
||||
& res) {
|
||||
if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
|
||||
res.result->success)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
|
||||
}
|
||||
}
|
||||
);
|
||||
}
|
||||
}},
|
||||
{"HandControl", [this](const std::string & topic, const std::string & internal_skill) {
|
||||
if (action_clients_) {
|
||||
action_clients_->register_client<HandControl>(
|
||||
topic,
|
||||
[]() {HandControl::Goal g; return g;},
|
||||
nullptr,
|
||||
[this,
|
||||
internal_skill](const ActionClientRegistry::GoalHandle<HandControl>::WrappedResult &
|
||||
res) {
|
||||
if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
|
||||
res.result->success)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
|
||||
}
|
||||
}
|
||||
);
|
||||
}
|
||||
}},
|
||||
{"LegControl", [this](const std::string & topic, const std::string & internal_skill) {
|
||||
if (action_clients_) {
|
||||
action_clients_->register_client<LegControl>(
|
||||
topic,
|
||||
[]() {LegControl::Goal g; return g;},
|
||||
nullptr,
|
||||
[this,
|
||||
internal_skill](const ActionClientRegistry::GoalHandle<LegControl>::WrappedResult &
|
||||
res) {
|
||||
if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
|
||||
res.result->success)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
|
||||
}
|
||||
}
|
||||
);
|
||||
}
|
||||
}},
|
||||
{"VisionGraspObject", [this](const std::string & topic, const std::string & internal_skill) {
|
||||
if (action_clients_) {
|
||||
action_clients_->register_client<VisionGraspObject>(
|
||||
topic,
|
||||
[]() {VisionGraspObject::Goal g; return g;},
|
||||
nullptr,
|
||||
[this,
|
||||
internal_skill](const ActionClientRegistry::GoalHandle<VisionGraspObject>::WrappedResult
|
||||
& res) {
|
||||
if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result &&
|
||||
res.result->success)
|
||||
{
|
||||
RCLCPP_INFO(node_->get_logger(), "action succeeded: %s", internal_skill.c_str());
|
||||
} else {
|
||||
RCLCPP_ERROR(node_->get_logger(), "action failed: %s", internal_skill.c_str());
|
||||
}
|
||||
}
|
||||
);
|
||||
}
|
||||
}},
|
||||
|
||||
};
|
||||
static const std::unordered_map<std::string, std::function<void(const std::string &,
|
||||
const std::string &)>> service_registrars = {
|
||||
{"VisionObjectRecognition",
|
||||
[this](const std::string & topic, const std::string & internal_skill) {
|
||||
(void)internal_skill;
|
||||
auto client = node_->create_client<VisionObjectRecognition>(topic);
|
||||
service_clients_.push_back(client);
|
||||
service_client_map_[topic] = client;
|
||||
}},
|
||||
|
||||
};
|
||||
|
||||
for (const auto & iface : s.interfaces) {
|
||||
auto parsed = parse_interface_entry(iface);
|
||||
if (!parsed) {
|
||||
RCLCPP_WARN(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
|
||||
continue;
|
||||
}
|
||||
// Strategy: use the skill's CamelCase name directly as the action/service topic.
|
||||
const std::string topic_name = s.name;
|
||||
if (parsed->kind == "action") {
|
||||
auto it = action_registrars.find(parsed->base);
|
||||
if (it != action_registrars.end()) {
|
||||
it->second(topic_name, s.name);
|
||||
} else {
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(), "No action registrar for interface base '%s'",
|
||||
parsed->base.c_str());
|
||||
}
|
||||
} else if (parsed->kind == "service") {
|
||||
auto it = service_registrars.find(parsed->base);
|
||||
if (it != service_registrars.end()) {
|
||||
it->second(topic_name, s.name);
|
||||
} else {
|
||||
RCLCPP_WARN(
|
||||
node_->get_logger(), "No service registrar for interface base '%s'",
|
||||
parsed->base.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
274
src/brain/test/UNITTEST.md
Normal file
274
src/brain/test/UNITTEST.md
Normal file
@@ -0,0 +1,274 @@
|
||||
# Brain 包单元测试说明
|
||||
|
||||
本文档介绍如何为 `brain` 包搭建 Google Test 测试环境、构建与运行测试，以及测试覆盖范围与方法说明。所有测试文件均放置在独立目录 `src/brain/test/`，与运行时可执行程序解耦。
|
||||
|
||||
## 测试环境
|
||||
|
||||
- 操作系统：Linux
|
||||
- 构建系统：colcon + ament_cmake（ROS 2）
|
||||
- 主要依赖：
|
||||
- ament_cmake_gtest
|
||||
- rclcpp、rclcpp_action、rclcpp_lifecycle
|
||||
- behaviortree_cpp
|
||||
- smacc2、smacc2_msgs
|
||||
- 本工程内的 `interfaces`（消息/动作定义）
|
||||
|
||||
若 ROS 2 安装不完整，请根据发行版安装缺失组件或通过工作区覆盖安装。
|
||||
|
||||
## 环境准备步骤
|
||||
|
||||
1. 载入 ROS 2 与工作区环境：
|
||||
|
||||
```bash
|
||||
source /opt/ros/$ROS_DISTRO/setup.bash
|
||||
# 在工作区根目录
|
||||
source install/setup.bash || true
|
||||
```
|
||||
|
||||
2. 确认测试依赖可用：
|
||||
|
||||
`ament_cmake_gtest` 必需。若缺失（Debian/Ubuntu 例）：
|
||||
|
||||
```bash
|
||||
sudo apt-get update
|
||||
sudo apt-get install ros-$ROS_DISTRO-ament-cmake-gtest
|
||||
```
|
||||
|
||||
3. 启用测试并仅构建 brain 包：
|
||||
|
||||
```bash
|
||||
colcon build --cmake-args -DBUILD_TESTING=ON --packages-select brain
|
||||
```
|
||||
|
||||
4. 运行测试：
|
||||
|
||||
```bash
|
||||
colcon test --packages-select brain
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
## 可选：本地跳过 Linter（仅为便捷）
|
||||
|
||||
若本地仅关注单元测试而不希望被 uncrustify/cmake-lint 等阻塞，可在当前终端临时跳过 ament linter：
|
||||
|
||||
```bash
|
||||
export SKIP_LINT=1
|
||||
colcon build --cmake-args -DBUILD_TESTING=ON --packages-select brain
|
||||
colcon test --packages-select brain
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
说明：此环境变量仅影响本地构建过程中的 linter 触发，不影响代码逻辑与测试本身。
|
||||
|
||||
## 测试目录结构与说明
|
||||
|
||||
- `test/test_action_registry.cpp`：覆盖 `ActionServerRegistry` 与 `ActionClientRegistry` 的基础行为。
|
||||
- `test/test_bt_registry.cpp`：覆盖 `BTRegistry` 的动作注册、序列注册与树构建/执行。
|
||||
- `test/test_cerebellum_node.cpp`：验证 `CerebellumNode` 启动与示例动作服务端可用性（历史回归）。
|
||||
- `test/test_cerebrum_node.cpp`：验证 `CerebrumNode` 初始化（参数、BT 注册/构建、定时器）。
|
||||
- `test/test_sm_cerebellum.cpp`：验证 `sm_cerebellum.hpp` 中 `runtime()` 默认值（占位待扩展）。
|
||||
- `test/test_execute_bt_action.cpp`：验证统一 ExecuteBtAction 单技能执行（ArmSpaceControl），检查反馈阶段（至少 START / END），最终 result(total_skills=1 succeeded_skills=1) 与 progress≈1.0。
|
||||
|
||||
所有测试均与运行时可执行程序解耦，不需要启动 `brain_node`。`test_execute_bt_action.cpp` 内部注入一个最小假 ArmSpaceControl 动作服务器驱动小脑执行逻辑。
|
||||
|
||||
## 接口覆盖与测试方法
|
||||
|
||||
- `brain/action_registry.hpp`
|
||||
- `ActionServerRegistry::register_action_server`：
|
||||
- 方法：在测试中注册伪回调（goal/cancel/execute），构造 `rclcpp::Node` 并使用 `SingleThreadedExecutor` 执行 `spin_some()`，验证服务端注册与生命周期管理无异常。
|
||||
- `ActionClientRegistry::register_client` / `send`：
|
||||
- 方法：通过 `CerebrumNode` 的 BT handler（`MoveJ_H::on_start`）触发 `send_action("move_to_position")`，覆盖客户端注册与发送路径。在未启动真实服务端时仅记录日志，不崩溃。
|
||||
|
||||
- `brain/bt_registry.hpp`
|
||||
- `register_bt_action` / `register_bt_sequence` / `build_tree_from_sequence`：
|
||||
- 方法：注册测试动作与序列，构建树后调用 `tickWhileRunning()`，断言 `on_start` 被触发（标志位 `start_called` 置为 true）。
|
||||
|
||||
- `brain/cerebellum_node.hpp`
|
||||
- ExecuteBtAction 与技能调度：`test_execute_bt_action.cpp` 发送 ExecuteBtAction goal，断言：success 与反馈阶段集合；验证扩展 Result 字段(total_skills / succeeded_skills)。
|
||||
- 回归动作服务可用性：`test_cerebellum_node.cpp` 保留历史最小可用性检查。
|
||||
|
||||
- `brain/cerebrum_node.hpp`
|
||||
- 节点初始化与行为树构建：
|
||||
- 方法：构造节点、读取参数、注册 BT handler/序列并构建，`spin_some()` 验证初始化流程与定时器创建。`MoveJ_H::on_start` 会触发 `send_action("move_to_position")`，覆盖客户端发送路径（不要求服务端存在）。
|
||||
|
||||
- `brain/sm_cerebellum.hpp`
|
||||
- `runtime()` 默认值：
|
||||
- 方法：直接访问静态运行时数据，断言 `current_x/current_y` 初始为 `0.0`。
|
||||
|
||||
## 设计与稳定性备注
|
||||
|
||||
- 需要使用 ROS executor 的测试均采用 `SingleThreadedExecutor`，并尽量缩短执行时间（仅 `spin_some()` 或短周期）。
|
||||
- 避免在单元测试中引入完整的端到端系统依赖，通过 handler 与宏开关（`BRAIN_DISABLE_SM`）保证测试可重复、稳定。
|
||||
- ExecuteBtAction 单技能测试未覆盖 RUN 插值（因为伪服务器快速完成）。如需测试 RUN 进度，可扩展一个延时执行的假服务器并断言收到多个 RUN 阶段反馈(progress 递增 <1.0)。
|
||||
- 如需更深入的端到端校验，可在测试中同时起一个最小 action server/client 闭环，但建议引入超时与健壮的清理逻辑。
|
||||
|
||||
## 故障排查（重要）
|
||||
|
||||
- 现象：按本文命令运行后，单测实际已通过，但最终 `colcon test-result` 显示 `uncrustify` 失败（大量“Code style divergence”）。
|
||||
- 原因：CMake 第一次配置时已把 `ament_lint_auto` 的测试目标写入缓存；仅设置 `SKIP_LINT=1` 不会自动清理之前缓存中登记的 linter 测试目标。
|
||||
- 解决：在切换 `SKIP_LINT` 或希望临时跳过 Linter 时，需强制重新配置并清理旧测试记录：
|
||||
|
||||
```bash
|
||||
source /opt/ros/$ROS_DISTRO/setup.bash
|
||||
source install/setup.bash || true
|
||||
export SKIP_LINT=1
|
||||
colcon build --cmake-clean-cache --cmake-args -DBUILD_TESTING=ON --packages-select brain
|
||||
colcon test --packages-select brain --event-handlers console_direct+
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
- 说明：以上流程会移除缓存中的 linter 测试目标，仅保留 gtest；若你需要在 CI 中启用 Linter，请移除 `SKIP_LINT` 并修复格式化差异后再运行。
|
||||
|
||||
## CI 场景：启用 Linter 并自动修复
|
||||
|
||||
- 目的：在 CI 中开启 ament linters 并确保全部通过。
|
||||
- 步骤：
|
||||
|
||||
```bash
|
||||
source /opt/ros/$ROS_DISTRO/setup.bash
|
||||
source install/setup.bash || true
|
||||
unset SKIP_LINT
|
||||
colcon build --cmake-clean-cache --cmake-args -DBUILD_TESTING=ON --packages-select brain
|
||||
ament_uncrustify --reformat src/brain
|
||||
colcon build --packages-select brain
|
||||
colcon test --packages-select brain --event-handlers console_direct+
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
- 说明：首次启用 linters 建议使用 `--cmake-clean-cache` 触发重新配置；若仍看到旧的失败统计，可清理陈旧测试结果后重跑：
|
||||
|
||||
```bash
|
||||
rm -rf build/brain/test_results/brain
|
||||
colcon test --packages-select brain --event-handlers console_direct+
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
## 如何新增 gtest 单元测试（步骤详解）
|
||||
|
||||
- 概览：在 `src/brain/test/` 下新增 `test_xxx.cpp`，在 `src/brain/CMakeLists.txt` 添加 `ament_add_gtest(...)` 并声明依赖，然后 `colcon build && colcon test` 即可。
|
||||
|
||||
1. 新建测试文件
|
||||
|
||||
在 `src/brain/test/` 下创建文件，例如 `test_my_feature.cpp`：
|
||||
|
||||
```cpp
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
// 如需使用包内头文件
|
||||
#include "brain/cerebrum_node.hpp"
|
||||
|
||||
TEST(MyFeature, BasicBehavior) {
|
||||
auto node = std::make_shared<brain::CerebrumNode>();
|
||||
// 进行最小操作
|
||||
rclcpp::executors::SingleThreadedExecutor ex;
|
||||
ex.add_node(node);
|
||||
ex.spin_some();
|
||||
// 基本断言
|
||||
SUCCEED();
|
||||
}
|
||||
```
|
||||
|
||||
提示：
|
||||
- 如需构造 ROS 节点/动作客户端，优先使用 `SingleThreadedExecutor` 与短周期 `spin_some()`，避免长时间阻塞。
|
||||
- 若单测包含 `CerebellumNode` 且不希望启动 SMACC2，可参考现有 `test_cerebellum_node.cpp` 在测试目标上加 `BRAIN_DISABLE_SM` 宏（见下文第 2 步）。
|
||||
|
||||
2. 在 CMake 中注册测试目标
|
||||
|
||||
编辑 `src/brain/CMakeLists.txt`，在 `if(BUILD_TESTING)` 块中添加：
|
||||
|
||||
```cmake
|
||||
ament_add_gtest(test_my_feature
|
||||
test/test_my_feature.cpp
|
||||
# 如需直接复用包内源文件，可在此追加，例如：
|
||||
# src/cerebrum_node.cpp
|
||||
)
|
||||
if(TARGET test_my_feature)
|
||||
ament_target_dependencies(test_my_feature
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
behaviortree_cpp
|
||||
interfaces
|
||||
smacc2
|
||||
smacc2_msgs
|
||||
)
|
||||
target_include_directories(test_my_feature PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include)
|
||||
# 如需禁用 SMACC2：
|
||||
# target_compile_definitions(test_my_feature PRIVATE BRAIN_DISABLE_SM=1)
|
||||
# 如需 Boost 线程：
|
||||
# target_link_libraries(test_my_feature Boost::thread)
|
||||
endif()
|
||||
```
|
||||
|
||||
要点：
|
||||
- 依赖与包含目录参照已有测试目标，按需裁剪。
|
||||
- 需要用到包内类实现时，可把对应 `src/*.cpp` 添加进该测试的源文件列表（仅限测试用途，不改变生产可执行文件）。
|
||||
|
||||
3. 构建与运行新测试
|
||||
|
||||
仅选择 `brain` 包进行构建与测试：
|
||||
|
||||
```bash
|
||||
source /opt/ros/$ROS_DISTRO/setup.bash
|
||||
source install/setup.bash || true
|
||||
colcon build --cmake-args -DBUILD_TESTING=ON --packages-select brain
|
||||
colcon test --packages-select brain --event-handlers console_direct+
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
4. 常见技巧与约束
|
||||
|
||||
- 执行器：优先 `SingleThreadedExecutor` + `spin_some()` 或极短周期计时器，减少时间依赖。
|
||||
- 动作客户端/服务端：单元测试阶段可仅验证“注册/可用性/发送路径”，端到端闭环建议放到集成测试。
|
||||
- 行为树：构造 `BTRegistry`，注册最小 handler/序列，使用 `tree.tickWhileRunning()` 触发执行（参考 `test_bt_registry.cpp`）。
|
||||
- 宏开关：涉及 `CerebellumNode` 时可通过 `BRAIN_DISABLE_SM` 禁用 SMACC2，避免测试时创建服务造成 rcl 上下文异常。
|
||||
- Linter：若 CI 启用 linter，请确保通过“CI 场景”小节中的 `ament_uncrustify --reformat` 后再提交。
|
||||
|
||||
## 最新一次测试结果快照（2025-09-26 更新：拆分 ExecuteBtAction 扩展用例 & 全部场景稳定通过）
|
||||
|
||||
来源命令：
|
||||
|
||||
```bash
|
||||
colcon test --packages-select brain --event-handlers console_direct+
|
||||
colcon test-result --verbose
|
||||
```
|
||||
|
||||
BuildStamp: `20250926-SPLIT-ACTION-SCENARIOS`
|
||||
|
||||
总结：共 15 个 CTest 目标（11 gtest + 4 linter），全部通过。原先集中在单一 `test_execute_bt_action_extended` 文件的 6 个高级场景被拆分为 6 个独立 gtest 可执行文件，避免共享进程内 rclcpp 全局状态导致的 `std::bad_alloc` / 崩溃问题，实现稳定执行与更清晰的失败定位。
|
||||
|
||||
| 测试目标 | 类型 | 状态 | 说明（耗时为近似，省略） |
|
||||
|----------|------|------|---------------------------|
|
||||
| test_action_registry | gtest | PASS | Action 注册/生命周期 |
|
||||
| test_bt_registry | gtest | PASS | 行为树序列构建/执行 |
|
||||
| test_cerebellum_node | gtest | PASS | Cerebellum 启动 + 技能加载 (SM 禁用) |
|
||||
| test_cerebrum_node | gtest | PASS | Cerebrum 初始化/BT handler 注册 |
|
||||
| test_sm_cerebellum | gtest | PASS | 运行时默认值 |
|
||||
| test_execute_bt_action | gtest | PASS | 单技能 ExecuteBtAction 基础成功路径 |
|
||||
| test_execute_bt_action_extended | gtest | PASS | 仅保留多技能进度分段 (其它场景已拆分) |
|
||||
| test_execute_bt_action_unsupported | gtest | PASS | 未定义技能立即失败 (Abort) |
|
||||
| test_execute_bt_action_run_interp | gtest | PASS | 延时服务器产生多个 RUN 反馈 (>=3) |
|
||||
| test_execute_bt_action_timeout_continue | gtest | PASS | 第一技能超时继续第二技能 (abort_on_failure=false) |
|
||||
| test_execute_bt_action_stats | gtest | PASS | 统计话题（或为空，定时器禁用时）稳定无崩溃 |
|
||||
| test_execute_bt_action_cancel | gtest | PASS | 中途 cancel；当前实现返回 SUCCEEDED (兼容未来 CANCELED) |
|
||||
| cppcheck | linter | PASS | 静态分析 |
|
||||
| lint_cmake | linter | PASS | CMake 风格 |
|
||||
| uncrustify | linter | PASS | 代码格式统一 |
|
||||
| xmllint | linter | PASS | package.xml 合法 |
|
||||
|
||||
扩展 ExecuteBtAction 场景拆分后说明：
|
||||
|
||||
- test_execute_bt_action_extended (MultiSkillProgressSegmentation)：三技能序列分段进度验证。收集各技能 END 阶段反馈的 progress，断言≈ 1/3, 2/3, 1.0（容差给 0.25 / 0.05）。
|
||||
- test_execute_bt_action_unsupported：首技能不存在 -> 立即失败，Result code = ABORTED；result.total_skills=2，succeeded_skills=0。
|
||||
- test_execute_bt_action_run_interp：延时 1.2s 完成的伪服务器触发 CerebellumNode 在 RUN 阶段多次插值反馈（run_count >= 3）。
|
||||
- test_execute_bt_action_timeout_continue：第一技能挂起触发超时（0.5s），在 abort_on_failure=false 配置下继续第二技能；最终 Result code = ABORTED，total=2 succeeded=1。
|
||||
- test_execute_bt_action_stats：验证执行期间统计逻辑不崩溃；当 stats 定时器未禁用或日志刷新触发时，可观察到日志/话题输出；允许空发布以适配 timer 关闭情形。
|
||||
- test_execute_bt_action_cancel：长耗时技能中途调用 cancel；当前内部未传播取消到子动作（仍 SUCCEEDED），断言接受 SUCCEEDED 或未来增强后的 CANCELED（前向兼容）。
|
||||
|
||||
拆分动机：单一进程内连续多场景（含延迟、超时、挂起、取消）测试曾出现内存分配异常及进程崩溃。经拆分后，每个场景拥有独立的 rclcpp 初始化与资源生命周期，稳定性显著提升，也使故障定位更直接。
|
||||
|
||||
后续可选改进：
|
||||
|
||||
1. 为 cancel 测试添加显式 on_cancel 处理（CerebellumNode 增加取消传播路径）。
|
||||
2. stats 测试中设置非零 stats_log_interval_sec（例如 0.2）并等待一次定时器发布，改为强断言包含 seq_ok。当前由于需求宽松保持兼容。
|
||||
3. 引入模拟时间 (ROS clock) 以缩短等待时间并精确控制 RUN 插值节奏。
|
||||
|
||||
36
src/brain/test/test_action_registry.cpp
Normal file
36
src/brain/test/test_action_registry.cpp
Normal file
@@ -0,0 +1,36 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "brain/action_registry.hpp"
|
||||
#include "interfaces/action/move_to_position.hpp"
|
||||
|
||||
using MoveToPosition = interfaces::action::MoveToPosition;
|
||||
|
||||
TEST(ActionRegistry, ServerRegistersAndAccepts)
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
auto node = std::make_shared<rclcpp::Node>("test_action_registry_node");
|
||||
ActionServerRegistry registry(node.get());
|
||||
|
||||
bool goal_received = false;
|
||||
ActionServerRegistry::Handlers<MoveToPosition> h;
|
||||
h.on_goal =
|
||||
[&](const rclcpp_action::GoalUUID &, std::shared_ptr<const MoveToPosition::Goal> goal) {
|
||||
(void)goal;
|
||||
goal_received = true;
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
};
|
||||
h.on_cancel = [&](const std::shared_ptr<rclcpp_action::ServerGoalHandle<MoveToPosition>>) {
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
};
|
||||
registry.register_action_server<MoveToPosition>("move_to_position", h);
|
||||
|
||||
// Creation should not crash; we can't easily send a goal without spinning a client here.
|
||||
// Just spin some time to ensure server construction path runs
|
||||
rclcpp::executors::SingleThreadedExecutor exec;
|
||||
exec.add_node(node);
|
||||
exec.spin_some();
|
||||
|
||||
SUCCEED();
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
33
src/brain/test/test_bt_registry.cpp
Normal file
33
src/brain/test/test_bt_registry.cpp
Normal file
@@ -0,0 +1,33 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <behaviortree_cpp/bt_factory.h>
|
||||
#include "brain/bt_registry.hpp"
|
||||
|
||||
TEST(BTRegistry, BuildTreeFromSequence)
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
auto node = std::make_shared<rclcpp::Node>("test_bt_registry_node");
|
||||
BT::BehaviorTreeFactory factory;
|
||||
BTRegistry reg(factory, node.get());
|
||||
|
||||
BTActionHandlers handlers;
|
||||
bool start_called = false;
|
||||
handlers.on_start = [&](rclcpp::Node *, BT::TreeNode &) {
|
||||
start_called = true; return BT::NodeStatus::SUCCESS;
|
||||
};
|
||||
handlers.on_running = [&](rclcpp::Node *, BT::TreeNode &) {return BT::NodeStatus::SUCCESS;};
|
||||
handlers.on_halted = [&](rclcpp::Node *, BT::TreeNode &) {};
|
||||
reg.register_bt_action("TestAction", handlers);
|
||||
|
||||
BTSequenceSpec seq = {{"TestAction", "a1", {}}};
|
||||
reg.register_bt_sequence("Seq1", seq);
|
||||
|
||||
BT::Tree tree;
|
||||
ASSERT_TRUE(reg.build_tree_from_sequence("Seq1", tree));
|
||||
|
||||
// Tick once; it should call on_start.
|
||||
tree.tickWhileRunning();
|
||||
EXPECT_TRUE(start_called);
|
||||
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
38
src/brain/test/test_cerebellum_node.cpp
Normal file
38
src/brain/test/test_cerebellum_node.cpp
Normal file
@@ -0,0 +1,38 @@
|
||||
// Disable SMACC2 state machine for this unit test to avoid service initialization issues
|
||||
#ifndef BRAIN_DISABLE_SM
|
||||
#define BRAIN_DISABLE_SM 1
|
||||
#endif
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
// Legacy MoveToPosition action was removed from automatic server startup.
|
||||
// Adapt test to validate that CerebellumNode brings up the unified ExecuteBtAction server.
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
|
||||
TEST(CerebellumNode, StartsAndProvidesActionServer)
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
auto options = rclcpp::NodeOptions();
|
||||
auto cerebellum = std::make_shared<brain::CerebellumNode>(options);
|
||||
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(cerebellum, "ExecuteBtAction");
|
||||
rclcpp::executors::SingleThreadedExecutor exec;
|
||||
exec.add_node(cerebellum);
|
||||
|
||||
// Wait a bit for server availability
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
bool available = false;
|
||||
while (std::chrono::steady_clock::now() - start < std::chrono::seconds(5)) {
|
||||
exec.spin_some();
|
||||
available = client->action_server_is_ready();
|
||||
if (available) {break;}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
}
|
||||
EXPECT_TRUE(available) << "ExecuteBtAction server not ready within timeout";
|
||||
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
14
src/brain/test/test_cerebrum_node.cpp
Normal file
14
src/brain/test/test_cerebrum_node.cpp
Normal file
@@ -0,0 +1,14 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "brain/cerebrum_node.hpp"
|
||||
|
||||
TEST(CerebrumNode, InitializesBTAndTimers)
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
auto node = std::make_shared<brain::CerebrumNode>(rclcpp::NodeOptions());
|
||||
rclcpp::executors::SingleThreadedExecutor exec;
|
||||
exec.add_node(node);
|
||||
exec.spin_some();
|
||||
SUCCEED();
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
64
src/brain/test/test_cerebrum_utils.cpp
Normal file
64
src/brain/test/test_cerebrum_utils.cpp
Normal file
@@ -0,0 +1,64 @@
|
||||
// Copyright 2025
|
||||
// Unit tests for CerebrumNode utility helpers: ParseListString, PickRandomActionSkills, ParseResultDetail
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "brain/cerebrum_node.hpp"
|
||||
|
||||
using brain::CerebrumNode;
|
||||
|
||||
class CerebrumNodeFixture : public ::testing::Test
|
||||
{
|
||||
protected:
|
||||
void SetUp() override
|
||||
{
|
||||
rclcpp::init(0, nullptr);
|
||||
node_ = std::make_shared<CerebrumNode>();
|
||||
}
|
||||
void TearDown() override
|
||||
{
|
||||
if (rclcpp::ok()) {rclcpp::shutdown();}
|
||||
}
|
||||
std::shared_ptr<CerebrumNode> node_;
|
||||
};
|
||||
|
||||
TEST(CerebrumNodeStandalone, ParseListStringBasic) {
|
||||
auto v = CerebrumNode::ParseListString("a,b;c d\t e,a");
|
||||
ASSERT_EQ(v.size(), 5u);
|
||||
EXPECT_EQ(v[0], "a");
|
||||
EXPECT_EQ(v[1], "b");
|
||||
EXPECT_EQ(v[2], "c");
|
||||
EXPECT_EQ(v[3], "d");
|
||||
EXPECT_EQ(v[4], "e");
|
||||
}
|
||||
|
||||
TEST(CerebrumNodeStandalone, ParseListStringEmpty) {
|
||||
auto v = CerebrumNode::ParseListString(" ,, ; ");
|
||||
EXPECT_TRUE(v.empty());
|
||||
}
|
||||
|
||||
TEST(CerebrumNodeStandalone, ParseResultDetail) {
|
||||
EXPECT_EQ(
|
||||
(int)CerebrumNode::ParseResultDetail(
|
||||
"OK: done"), (int)CerebrumNode::ExecResultCode::SUCCESS);
|
||||
EXPECT_EQ(
|
||||
(int)CerebrumNode::ParseResultDetail(
|
||||
"FAIL something"), (int)CerebrumNode::ExecResultCode::FAILURE);
|
||||
EXPECT_EQ(
|
||||
(int)CerebrumNode::ParseResultDetail(
|
||||
"CANCEL user"), (int)CerebrumNode::ExecResultCode::CANCELLED);
|
||||
EXPECT_EQ(
|
||||
(int)CerebrumNode::ParseResultDetail(
|
||||
"code=0 all good"), (int)CerebrumNode::ExecResultCode::SUCCESS);
|
||||
EXPECT_EQ(
|
||||
(int)CerebrumNode::ParseResultDetail(
|
||||
"code=cancel requested"), (int)CerebrumNode::ExecResultCode::CANCELLED);
|
||||
EXPECT_EQ(
|
||||
(int)CerebrumNode::ParseResultDetail(
|
||||
"mystery"), (int)CerebrumNode::ExecResultCode::UNKNOWN);
|
||||
}
|
||||
|
||||
// NOTE: Removed PickRandomNoSkills test. The CerebrumNode constructor loads skills, so
|
||||
// asserting an empty random selection is invalid and caused intermittent failures.
|
||||
// If needed in the future, inject a mock SkillManager with zero action skills and
|
||||
// reintroduce a deterministic test around PickRandomActionSkills.
|
||||
141
src/brain/test/test_execute_bt_action.cpp
Normal file
141
src/brain/test/test_execute_bt_action.cpp
Normal file
@@ -0,0 +1,141 @@
|
||||
// Test ExecuteBtAction end-to-end for a single action skill (ArmSpaceControl)
|
||||
// Verifies: server availability, feedback stages sequencing, final result fields.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
// Minimal dummy ArmSpaceControl action server that succeeds almost immediately.
|
||||
class DummyArmServer
|
||||
{
|
||||
public:
|
||||
explicit DummyArmServer(const rclcpp::Node::SharedPtr & node)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
server_ = rclcpp_action::create_server<ArmSpaceControl>(
|
||||
node,
|
||||
"ArmSpaceControl",
|
||||
std::bind(&DummyArmServer::on_goal, this, _1, _2),
|
||||
std::bind(&DummyArmServer::on_cancel, this, _1),
|
||||
std::bind(&DummyArmServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;
|
||||
|
||||
rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const ArmSpaceControl::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> &)
|
||||
{
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> handle)
|
||||
{
|
||||
std::thread(
|
||||
[handle]() {
|
||||
auto feedback = std::make_shared<ArmSpaceControl::Feedback>();
|
||||
// (Feedback fields intentionally left default if none defined.)
|
||||
handle->publish_feedback(feedback);
|
||||
auto result = std::make_shared<ArmSpaceControl::Result>();
|
||||
result->success = true;
|
||||
result->message = "ok";
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(50));
|
||||
handle->succeed(result);
|
||||
}).detach();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(ExecuteBtAction, SingleSkillArmSpaceControlSuccess) {
|
||||
rclcpp::init(0, nullptr);
|
||||
auto options = rclcpp::NodeOptions();
|
||||
// Keep stats interval small to avoid side effects; not strictly needed
|
||||
options.append_parameter_override("stats_log_interval_sec", 0.0);
|
||||
auto cerebellum = std::make_shared<brain::CerebellumNode>(options);
|
||||
auto dummy_server_holder = std::make_shared<DummyArmServer>(cerebellum); // ensure server lifetime
|
||||
|
||||
// Create ExecuteBtAction client directly (bypassing Cerebrum for unit scope)
|
||||
// Use explicit action name (constant not exported in public headers)
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(cerebellum, "ExecuteBtAction");
|
||||
rclcpp::executors::SingleThreadedExecutor exec;
|
||||
exec.add_node(cerebellum);
|
||||
|
||||
// Wait for both servers: ExecuteBtAction (hosted by cerebellum) and ArmSpaceControl (dummy)
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start < std::chrono::seconds(5)) {
|
||||
exec.spin_some();
|
||||
if (client->action_server_is_ready()) {break;}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(20));
|
||||
}
|
||||
ASSERT_TRUE(client->action_server_is_ready()) << "ExecuteBtAction server not ready";
|
||||
|
||||
// Track feedback
|
||||
std::mutex fb_mutex;
|
||||
std::vector<std::string> stages;
|
||||
float last_progress = 0.f;
|
||||
auto send_goal_options = rclcpp_action::Client<ExecuteBtAction>::SendGoalOptions();
|
||||
send_goal_options.feedback_callback = [&](auto,
|
||||
const std::shared_ptr<const ExecuteBtAction::Feedback> fb) {
|
||||
if (!fb) {return;}
|
||||
std::lock_guard<std::mutex> lk(fb_mutex);
|
||||
stages.push_back(fb->stage + std::string("|") + fb->current_skill);
|
||||
last_progress = fb->progress;
|
||||
};
|
||||
|
||||
ExecuteBtAction::Goal goal; goal.action_name = "ArmSpaceControl";
|
||||
auto future_handle = client->async_send_goal(goal, send_goal_options);
|
||||
|
||||
// Wait for accept
|
||||
while (exec.spin_until_future_complete(
|
||||
future_handle,
|
||||
std::chrono::milliseconds(50)) == rclcpp::FutureReturnCode::TIMEOUT)
|
||||
{
|
||||
// loop
|
||||
}
|
||||
auto handle = future_handle.get();
|
||||
ASSERT_TRUE(handle);
|
||||
|
||||
auto result_future = client->async_get_result(handle);
|
||||
while (exec.spin_until_future_complete(
|
||||
result_future,
|
||||
std::chrono::milliseconds(50)) == rclcpp::FutureReturnCode::TIMEOUT)
|
||||
{
|
||||
// keep spinning to pump feedback
|
||||
}
|
||||
auto wrapped = result_future.get();
|
||||
ASSERT_EQ(wrapped.code, rclcpp_action::ResultCode::SUCCEEDED);
|
||||
ASSERT_TRUE(wrapped.result);
|
||||
EXPECT_TRUE(wrapped.result->success);
|
||||
EXPECT_EQ(wrapped.result->total_skills, 1);
|
||||
EXPECT_EQ(wrapped.result->succeeded_skills, 1);
|
||||
{
|
||||
std::lock_guard<std::mutex> lk(fb_mutex);
|
||||
// Expect at least START and END for ArmSpaceControl; optional WAIT/RUN may appear depending on timing
|
||||
bool saw_start = false, saw_end = false;
|
||||
for (auto & s : stages) {
|
||||
if (s.rfind("START|ArmSpaceControl", 0) == 0) {saw_start = true;}
|
||||
if (s.rfind("END|ArmSpaceControl", 0) == 0) {saw_end = true;}
|
||||
}
|
||||
EXPECT_TRUE(saw_start);
|
||||
EXPECT_TRUE(saw_end);
|
||||
EXPECT_NEAR(last_progress, 1.0f, 1e-3);
|
||||
}
|
||||
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
99
src/brain/test/test_execute_bt_action_cancel.cpp
Normal file
99
src/brain/test/test_execute_bt_action_cancel.cpp
Normal file
@@ -0,0 +1,99 @@
|
||||
// Cancel mid execution test
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
|
||||
namespace
|
||||
{
|
||||
class SlowArmServer
|
||||
{
|
||||
public:
|
||||
SlowArmServer(const rclcpp::Node::SharedPtr & node, int ms)
|
||||
{
|
||||
using namespace std::placeholders; delay_ = ms;
|
||||
server_ =
|
||||
rclcpp_action::create_server<ArmSpaceControl>(
|
||||
node, "ArmSpaceControl",
|
||||
std::bind(&SlowArmServer::on_goal, this, _1, _2),
|
||||
std::bind(&SlowArmServer::on_cancel, this, _1),
|
||||
std::bind(&SlowArmServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
int delay_; rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;
|
||||
rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const ArmSpaceControl::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> & gh)
|
||||
{
|
||||
auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = false; r->message = "cancel";
|
||||
gh->canceled(r); return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> gh)
|
||||
{
|
||||
std::thread(
|
||||
[gh, d = delay_]() {
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(
|
||||
d)); auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = true; r->message = "ok"; gh->succeed(
|
||||
r);
|
||||
}).detach();
|
||||
}
|
||||
};
|
||||
static bool spin_until(
|
||||
rclcpp::executors::SingleThreadedExecutor & ex,
|
||||
std::function<bool()> pred, std::chrono::milliseconds to)
|
||||
{
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start < to) {
|
||||
ex.spin_some(); if (pred()) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
return pred();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST(ExecuteBtActionCancel, CancelMidExecution) {
|
||||
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
|
||||
auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("stats_log_interval_sec", 0.0);
|
||||
opts.append_parameter_override("default_action_timeout_sec", 10.0);
|
||||
auto node = std::make_shared<brain::CerebellumNode>(opts);
|
||||
auto srv = std::make_shared<SlowArmServer>(node, 2000);
|
||||
rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node);
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(node, "ExecuteBtAction");
|
||||
ASSERT_TRUE(
|
||||
spin_until(
|
||||
exec,
|
||||
[&] {return client->action_server_is_ready();}, std::chrono::seconds(3)));
|
||||
ExecuteBtAction::Goal g; g.action_name = "ArmSpaceControl";
|
||||
auto gh_f = client->async_send_goal(g);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
gh_f, std::chrono::seconds(
|
||||
2)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto gh = gh_f.get(); ASSERT_TRUE(gh);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(300));
|
||||
client->async_cancel_goal(gh);
|
||||
auto r_f = client->async_get_result(gh);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
r_f, std::chrono::seconds(
|
||||
6)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto wrapped = r_f.get();
|
||||
EXPECT_TRUE(
|
||||
wrapped.code == rclcpp_action::ResultCode::SUCCEEDED ||
|
||||
wrapped.code == rclcpp_action::ResultCode::CANCELED);
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
126
src/brain/test/test_execute_bt_action_extended.cpp
Normal file
126
src/brain/test/test_execute_bt_action_extended.cpp
Normal file
@@ -0,0 +1,126 @@
|
||||
// Extended scenario tests for ExecuteBtAction (trimmed to only multi-skill progress segmentation)
|
||||
// Other scenarios have been split into their own dedicated test translation units to avoid
|
||||
// cross-test interference with rclcpp global state.
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
// no stats topic usage here
|
||||
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
#include "interfaces/action/leg_control.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
using HandControl = interfaces::action::HandControl;
|
||||
using LegControl = interfaces::action::LegControl;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
// Simple delayed success server for each action type
|
||||
template<typename ActionT>
|
||||
class SimpleDelayedServer
|
||||
{
|
||||
public:
|
||||
SimpleDelayedServer(const rclcpp::Node::SharedPtr & node, const std::string & name, int delay_ms)
|
||||
: delay_ms_(delay_ms)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
server_ = rclcpp_action::create_server<ActionT>(
|
||||
node, name,
|
||||
std::bind(&SimpleDelayedServer::on_goal, this, _1, _2),
|
||||
std::bind(&SimpleDelayedServer::on_cancel, this, _1),
|
||||
std::bind(&SimpleDelayedServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
int delay_ms_;
|
||||
typename rclcpp_action::Server<ActionT>::SharedPtr server_;
|
||||
rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const typename ActionT::Goal>)
|
||||
{return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> & gh)
|
||||
{
|
||||
auto res = std::make_shared<typename ActionT::Result>(); res->success = false;
|
||||
res->message = "cancel"; gh->canceled(res); return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ActionT>> gh)
|
||||
{
|
||||
std::thread(
|
||||
[gh, d = delay_ms_]() {
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(
|
||||
d)); auto res = std::make_shared<typename ActionT::Result>(); res->success = true; res->message = "ok"; gh->succeed(
|
||||
res);
|
||||
}).detach();
|
||||
}
|
||||
};
|
||||
|
||||
static bool spin_until(
|
||||
rclcpp::executors::SingleThreadedExecutor & exec,
|
||||
std::function<bool()> pred,
|
||||
std::chrono::milliseconds timeout)
|
||||
{
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start < timeout) {
|
||||
exec.spin_some();
|
||||
if (pred()) {return true;}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
return pred();
|
||||
}
|
||||
|
||||
TEST(ExecuteBtActionExtended, MultiSkillProgressSegmentation)
|
||||
{
|
||||
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);} // safe re-init if already init
|
||||
auto options = rclcpp::NodeOptions();
|
||||
options.append_parameter_override("stats_log_interval_sec", 0.0);
|
||||
options.append_parameter_override("default_action_timeout_sec", 5.0);
|
||||
auto node = std::make_shared<brain::CerebellumNode>(options);
|
||||
// Three skills with distinct durations to create progress partitions
|
||||
auto arm = std::make_shared<SimpleDelayedServer<ArmSpaceControl>>(node, "ArmSpaceControl", 150);
|
||||
auto hand = std::make_shared<SimpleDelayedServer<HandControl>>(node, "HandControl", 120);
|
||||
auto leg = std::make_shared<SimpleDelayedServer<LegControl>>(node, "LegControl", 180);
|
||||
|
||||
rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node);
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(node, "ExecuteBtAction");
|
||||
ASSERT_TRUE(
|
||||
spin_until(
|
||||
exec,
|
||||
[&] {return client->action_server_is_ready();}, std::chrono::seconds(3)));
|
||||
|
||||
ExecuteBtAction::Goal goal; goal.action_name = "ArmSpaceControl,HandControl,LegControl";
|
||||
std::vector<double> end_progresses;
|
||||
auto send_opts = rclcpp_action::Client<ExecuteBtAction>::SendGoalOptions();
|
||||
send_opts.feedback_callback =
|
||||
[&](auto, const std::shared_ptr<const ExecuteBtAction::Feedback> fb) {
|
||||
if (!fb) {return;} if (fb->stage == "END") {end_progresses.push_back(fb->progress);}};
|
||||
auto gh_future = client->async_send_goal(goal, send_opts);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
gh_future, std::chrono::seconds(
|
||||
2)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto gh = gh_future.get(); ASSERT_TRUE(gh);
|
||||
auto res_future = client->async_get_result(gh);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
res_future, std::chrono::seconds(
|
||||
10)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto wrapped = res_future.get(); ASSERT_EQ(wrapped.code, rclcpp_action::ResultCode::SUCCEEDED);
|
||||
ASSERT_TRUE(wrapped.result); EXPECT_EQ(wrapped.result->total_skills, 3); EXPECT_EQ(
|
||||
wrapped.result->succeeded_skills, 3);
|
||||
// We expect three phase end progress marks roughly near 1/3, 2/3, and 1.0 (allowing tolerance)
|
||||
ASSERT_EQ(end_progresses.size(), 3u);
|
||||
EXPECT_NEAR(end_progresses[0], 1.0 / 3.0, 0.25);
|
||||
EXPECT_NEAR(end_progresses[1], 2.0 / 3.0, 0.25);
|
||||
EXPECT_NEAR(end_progresses[2], 1.0, 0.05);
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
104
src/brain/test/test_execute_bt_action_run_interp.cpp
Normal file
104
src/brain/test/test_execute_bt_action_run_interp.cpp
Normal file
@@ -0,0 +1,104 @@
|
||||
// RUN interpolation multiple RUN feedback test isolated
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
|
||||
namespace
|
||||
{
|
||||
class DelayedArmServer
|
||||
{
|
||||
public:
|
||||
DelayedArmServer(const rclcpp::Node::SharedPtr & node, int delay_ms)
|
||||
: delay_(delay_ms)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
server_ = rclcpp_action::create_server<ArmSpaceControl>(
|
||||
node, "ArmSpaceControl",
|
||||
std::bind(&DelayedArmServer::on_goal, this, _1, _2),
|
||||
std::bind(&DelayedArmServer::on_cancel, this, _1),
|
||||
std::bind(&DelayedArmServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
int delay_;
|
||||
rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;
|
||||
rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const ArmSpaceControl::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> & gh)
|
||||
{
|
||||
auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = false; r->message = "cancel";
|
||||
gh->canceled(r); return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> gh)
|
||||
{
|
||||
std::thread(
|
||||
[gh, d = delay_]() {
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds(
|
||||
d)); auto res = std::make_shared<ArmSpaceControl::Result>(); res->success = true; res->message = "ok"; gh->succeed(
|
||||
res);
|
||||
}).detach();
|
||||
}
|
||||
};
|
||||
static bool spin_until(
|
||||
rclcpp::executors::SingleThreadedExecutor & ex,
|
||||
std::function<bool()> pred, std::chrono::milliseconds to)
|
||||
{
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start < to) {
|
||||
ex.spin_some(); if (pred()) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
return pred();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST(ExecuteBtActionRunInterp, MultipleRunFeedbacks) {
|
||||
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
|
||||
auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("stats_log_interval_sec", 0.0);
|
||||
opts.append_parameter_override("default_action_timeout_sec", 3.0);
|
||||
auto node = std::make_shared<brain::CerebellumNode>(opts);
|
||||
auto srv = std::make_shared<DelayedArmServer>(node, 1200);
|
||||
rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node);
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(node, "ExecuteBtAction");
|
||||
ASSERT_TRUE(
|
||||
spin_until(
|
||||
exec,
|
||||
[&] {return client->action_server_is_ready();}, std::chrono::seconds(3)));
|
||||
std::atomic<int> run_count{0};
|
||||
ExecuteBtAction::Goal goal; goal.action_name = "ArmSpaceControl";
|
||||
auto optsSend = rclcpp_action::Client<ExecuteBtAction>::SendGoalOptions();
|
||||
optsSend.feedback_callback =
|
||||
[&](auto, const std::shared_ptr<const ExecuteBtAction::Feedback> fb) {
|
||||
if (fb && fb->stage == "RUN") {
|
||||
run_count.fetch_add(1);
|
||||
}
|
||||
};
|
||||
auto gh_future = client->async_send_goal(goal, optsSend);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
gh_future, std::chrono::seconds(
|
||||
2)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto gh = gh_future.get(); ASSERT_TRUE(gh);
|
||||
auto res_future = client->async_get_result(gh);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
res_future, std::chrono::seconds(
|
||||
10)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto wrapped = res_future.get(); ASSERT_EQ(wrapped.code, rclcpp_action::ResultCode::SUCCEEDED);
|
||||
EXPECT_GE(run_count.load(), 3);
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
105
src/brain/test/test_execute_bt_action_stats.cpp
Normal file
105
src/brain/test/test_execute_bt_action_stats.cpp
Normal file
@@ -0,0 +1,105 @@
|
||||
// Stats topic content test
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
|
||||
namespace
|
||||
{
|
||||
class ArmFastServer
|
||||
{
|
||||
public:
|
||||
explicit ArmFastServer(const rclcpp::Node::SharedPtr & node)
|
||||
{
|
||||
using namespace std::placeholders; server_ = rclcpp_action::create_server<ArmSpaceControl>(
|
||||
node,
|
||||
"ArmSpaceControl", std::bind(
|
||||
&ArmFastServer::on_goal, this, _1, _2), std::bind(
|
||||
&ArmFastServer::on_cancel, this,
|
||||
_1),
|
||||
std::bind(&ArmFastServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_; rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &, std::shared_ptr<const ArmSpaceControl::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> & gh)
|
||||
{
|
||||
auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = false; r->message = "cancel";
|
||||
gh->canceled(r); return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> gh)
|
||||
{
|
||||
std::thread(
|
||||
[gh]() {
|
||||
auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = true; r->message = "ok"; gh->succeed(
|
||||
r);
|
||||
}).detach();
|
||||
}
|
||||
};
|
||||
static bool spin_until(
|
||||
rclcpp::executors::SingleThreadedExecutor & ex,
|
||||
std::function<bool()> pred, std::chrono::milliseconds to)
|
||||
{
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start < to) {
|
||||
ex.spin_some(); if (pred()) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
return pred();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST(ExecuteBtActionStats, SequenceStatsPublishOrEmptyWhenTimerDisabled) {
|
||||
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
|
||||
auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("stats_log_interval_sec", 0.2); // enable fast timer
|
||||
auto node = std::make_shared<brain::CerebellumNode>(opts);
|
||||
auto server = std::make_shared<ArmFastServer>(node);
|
||||
rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node);
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(node, "ExecuteBtAction");
|
||||
ASSERT_TRUE(
|
||||
spin_until(
|
||||
exec,
|
||||
[&] {return client->action_server_is_ready();}, std::chrono::seconds(3)));
|
||||
std::mutex m; std::string last;
|
||||
auto sub =
|
||||
node->create_subscription<std_msgs::msg::String>(
|
||||
"/brain/skill_stats", 10,
|
||||
[&](std_msgs::msg::String::ConstSharedPtr msg) {
|
||||
std::lock_guard<std::mutex> lk(m); last = msg->data;
|
||||
});
|
||||
ExecuteBtAction::Goal g; g.action_name = "ArmSpaceControl";
|
||||
auto gh_f = client->async_send_goal(g);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
gh_f, std::chrono::seconds(
|
||||
2)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto gh = gh_f.get(); ASSERT_TRUE(gh);
|
||||
auto r_f = client->async_get_result(gh);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
r_f, std::chrono::seconds(
|
||||
5)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
// wait for at least one stats timer publish
|
||||
spin_until(
|
||||
exec, [&] {
|
||||
std::lock_guard<std::mutex> lk(
|
||||
m); return !last.empty();
|
||||
}, std::chrono::milliseconds(600));
|
||||
std::lock_guard<std::mutex> lk(m);
|
||||
ASSERT_FALSE(last.empty());
|
||||
EXPECT_NE(last.find("seq_ok="), std::string::npos);
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
130
src/brain/test/test_execute_bt_action_timeout_continue.cpp
Normal file
130
src/brain/test/test_execute_bt_action_timeout_continue.cpp
Normal file
@@ -0,0 +1,130 @@
|
||||
// Timeout continuation test
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
using HandControl = interfaces::action::HandControl;
|
||||
|
||||
namespace
|
||||
{
|
||||
class HangingArmServer
|
||||
{
|
||||
public:
|
||||
explicit HangingArmServer(const rclcpp::Node::SharedPtr & node)
|
||||
{
|
||||
using namespace std::placeholders; server_ = rclcpp_action::create_server<ArmSpaceControl>(
|
||||
node,
|
||||
"ArmSpaceControl", std::bind(
|
||||
&HangingArmServer::on_goal, this, _1, _2), std::bind(
|
||||
&HangingArmServer::on_cancel, this,
|
||||
_1),
|
||||
std::bind(&HangingArmServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;
|
||||
rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const ArmSpaceControl::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> & gh)
|
||||
{
|
||||
auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = false; r->message = "cancel";
|
||||
gh->canceled(r); return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>>) /* never finish */
|
||||
{
|
||||
}
|
||||
};
|
||||
class HandQuickServer
|
||||
{
|
||||
public:
|
||||
explicit HandQuickServer(const rclcpp::Node::SharedPtr & node)
|
||||
{
|
||||
using namespace std::placeholders; server_ = rclcpp_action::create_server<HandControl>(
|
||||
node,
|
||||
"HandControl", std::bind(
|
||||
&HandQuickServer::on_goal, this, _1, _2), std::bind(
|
||||
&HandQuickServer::on_cancel, this,
|
||||
_1),
|
||||
std::bind(&HandQuickServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::Server<HandControl>::SharedPtr server_; rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &, std::shared_ptr<const HandControl::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> & gh)
|
||||
{
|
||||
auto r = std::make_shared<HandControl::Result>(); r->success = false; r->message = "cancel";
|
||||
gh->canceled(r); return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> gh)
|
||||
{
|
||||
std::thread(
|
||||
[gh]() {
|
||||
auto r = std::make_shared<HandControl::Result>(); r->success = true; r->message = "ok"; gh->succeed(
|
||||
r);
|
||||
}).detach();
|
||||
}
|
||||
};
|
||||
static bool spin_until(
|
||||
rclcpp::executors::SingleThreadedExecutor & ex,
|
||||
std::function<bool()> pred, std::chrono::milliseconds to)
|
||||
{
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start < to) {
|
||||
ex.spin_some(); if (pred()) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
return pred();
|
||||
}
|
||||
} // namespace
|
||||
|
||||
TEST(ExecuteBtActionTimeoutContinue, FirstTimeoutSecondSucceeds) {
|
||||
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
|
||||
auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("abort_on_failure", false);
|
||||
opts.append_parameter_override("default_action_timeout_sec", 0.5);
|
||||
opts.append_parameter_override("stats_log_interval_sec", 0.0);
|
||||
auto node = std::make_shared<brain::CerebellumNode>(opts);
|
||||
auto hang = std::make_shared<HangingArmServer>(node);
|
||||
auto hand = std::make_shared<HandQuickServer>(node);
|
||||
rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node);
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(node, "ExecuteBtAction");
|
||||
ASSERT_TRUE(
|
||||
spin_until(
|
||||
exec,
|
||||
[&] {return client->action_server_is_ready();}, std::chrono::seconds(3)));
|
||||
ExecuteBtAction::Goal goal; goal.action_name = "ArmSpaceControl,HandControl";
|
||||
auto gh_future = client->async_send_goal(goal);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
gh_future, std::chrono::seconds(
|
||||
2)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto gh = gh_future.get(); ASSERT_TRUE(gh);
|
||||
auto res_future = client->async_get_result(gh);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
res_future, std::chrono::seconds(
|
||||
10)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto wrapped = res_future.get();
|
||||
EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::ABORTED);
|
||||
ASSERT_TRUE(wrapped.result);
|
||||
EXPECT_EQ(wrapped.result->total_skills, 2);
|
||||
EXPECT_EQ(wrapped.result->succeeded_skills, 1);
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
97
src/brain/test/test_execute_bt_action_unsupported.cpp
Normal file
97
src/brain/test/test_execute_bt_action_unsupported.cpp
Normal file
@@ -0,0 +1,97 @@
|
||||
// Unsupported skill failure detail test split out for isolation
|
||||
#include <gtest/gtest.h>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "brain/cerebellum_node.hpp"
|
||||
#include "interfaces/action/execute_bt_action.hpp"
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
|
||||
using ExecuteBtAction = interfaces::action::ExecuteBtAction;
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
|
||||
namespace
|
||||
{
|
||||
|
||||
class ArmQuickServer
|
||||
{
|
||||
public:
|
||||
explicit ArmQuickServer(const rclcpp::Node::SharedPtr & node)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
server_ = rclcpp_action::create_server<ArmSpaceControl>(
|
||||
node, "ArmSpaceControl",
|
||||
std::bind(&ArmQuickServer::on_goal, this, _1, _2),
|
||||
std::bind(&ArmQuickServer::on_cancel, this, _1),
|
||||
std::bind(&ArmQuickServer::on_accept, this, _1));
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;
|
||||
rclcpp_action::GoalResponse on_goal(
|
||||
const rclcpp_action::GoalUUID &,
|
||||
std::shared_ptr<const ArmSpaceControl::Goal>)
|
||||
{
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
rclcpp_action::CancelResponse on_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> & gh)
|
||||
{
|
||||
auto r = std::make_shared<ArmSpaceControl::Result>(); r->success = false; r->message = "cancel";
|
||||
gh->canceled(r); return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
void on_accept(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ArmSpaceControl>> gh)
|
||||
{
|
||||
std::thread(
|
||||
[gh]() {
|
||||
auto res = std::make_shared<ArmSpaceControl::Result>(); res->success = true; res->message = "ok"; gh->succeed(
|
||||
res);
|
||||
}).detach();
|
||||
}
|
||||
};
|
||||
|
||||
static bool spin_until(
|
||||
rclcpp::executors::SingleThreadedExecutor & ex,
|
||||
std::function<bool()> pred, std::chrono::milliseconds to)
|
||||
{
|
||||
auto start = std::chrono::steady_clock::now();
|
||||
while (std::chrono::steady_clock::now() - start < to) {
|
||||
ex.spin_some(); if (pred()) {
|
||||
return true;
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(10));
|
||||
}
|
||||
return pred();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
TEST(ExecuteBtActionUnsupported, UndefinedFirstSkillAborts) {
|
||||
if (!rclcpp::ok()) {rclcpp::init(0, nullptr);}
|
||||
auto opts = rclcpp::NodeOptions(); opts.append_parameter_override("stats_log_interval_sec", 0.0);
|
||||
auto node = std::make_shared<brain::CerebellumNode>(opts);
|
||||
auto arm = std::make_shared<ArmQuickServer>(node);
|
||||
rclcpp::executors::SingleThreadedExecutor exec; exec.add_node(node);
|
||||
auto client = rclcpp_action::create_client<ExecuteBtAction>(node, "ExecuteBtAction");
|
||||
ASSERT_TRUE(
|
||||
spin_until(
|
||||
exec,
|
||||
[&] {return client->action_server_is_ready();}, std::chrono::seconds(3)));
|
||||
ExecuteBtAction::Goal goal; goal.action_name = "NonExistingSkill,ArmSpaceControl";
|
||||
auto gh_future = client->async_send_goal(goal);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
gh_future, std::chrono::seconds(
|
||||
2)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto gh = gh_future.get(); ASSERT_TRUE(gh);
|
||||
auto res_future = client->async_get_result(gh);
|
||||
ASSERT_EQ(
|
||||
exec.spin_until_future_complete(
|
||||
res_future, std::chrono::seconds(
|
||||
5)), rclcpp::FutureReturnCode::SUCCESS);
|
||||
auto wrapped = res_future.get();
|
||||
EXPECT_EQ(wrapped.code, rclcpp_action::ResultCode::ABORTED);
|
||||
ASSERT_TRUE(wrapped.result);
|
||||
EXPECT_FALSE(wrapped.result->success);
|
||||
EXPECT_NE(wrapped.result->message.find("NonExistingSkill(FAIL)"), std::string::npos);
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
9
src/brain/test/test_sm_cerebellum.cpp
Normal file
9
src/brain/test/test_sm_cerebellum.cpp
Normal file
@@ -0,0 +1,9 @@
|
||||
#include <gtest/gtest.h>
|
||||
#include "brain/sm_cerebellum.hpp"
|
||||
|
||||
TEST(SmCerebellum, RuntimeDefaults)
|
||||
{
|
||||
// auto & d = brain::runtime();
|
||||
// After first call, defaults should be zeroed (single static instance)
|
||||
|
||||
}
|
||||
60
src/interfaces/CMakeLists.txt
Normal file
60
src/interfaces/CMakeLists.txt
Normal file
@@ -0,0 +1,60 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(interfaces)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
find_package(std_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(builtin_interfaces REQUIRED)
|
||||
|
||||
set(msg_files
|
||||
msg/Movej.msg
|
||||
)
|
||||
set(action_files
|
||||
action/MoveToPosition.action
|
||||
action/ArmSpaceControl.action
|
||||
action/HandControl.action
|
||||
action/LegControl.action
|
||||
action/VisionGraspObject.action
|
||||
action/ExecuteBtAction.action
|
||||
)
|
||||
set(srv_files
|
||||
srv/VisionObjectRecognition.srv
|
||||
)
|
||||
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
${msg_files}
|
||||
${action_files}
|
||||
${srv_files}
|
||||
|
||||
DEPENDENCIES
|
||||
std_msgs
|
||||
geometry_msgs
|
||||
builtin_interfaces
|
||||
)
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_export_dependencies(rosidl_default_runtime)
|
||||
ament_package()
|
||||
10
src/interfaces/action/ArmSpaceControl.action
Normal file
10
src/interfaces/action/ArmSpaceControl.action
Normal file
@@ -0,0 +1,10 @@
|
||||
# Goal: target pose in space
|
||||
# Example fields; adjust as needed for your robot
|
||||
geometry_msgs/PoseStamped target
|
||||
---
|
||||
# Result
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
# Feedback
|
||||
float32 progress
|
||||
46
src/interfaces/action/ExecuteBtAction.action
Normal file
46
src/interfaces/action/ExecuteBtAction.action
Normal file
@@ -0,0 +1,46 @@
|
||||
###############################################################
|
||||
# ExecuteBtAction
|
||||
#
|
||||
# Goal:
|
||||
# action_name: A skill sequence or single skill name. For a single
|
||||
# skill this is typically the canonical CamelCase name
|
||||
# (e.g. "ArmSpaceControl"). Multiple skills may be
|
||||
# separated by commas/semicolons/spaces.
|
||||
#
|
||||
# Result:
|
||||
# success: Overall success flag for the entire (possibly multi)
|
||||
# skill sequence.
|
||||
# message: Human readable summary (e.g. "Seq{A,B}: A(OK), B(FAIL)[detail]").
|
||||
# total_skills:How many skills were requested (after parsing).
|
||||
# succeeded_skills: Number of skills that finished with success.
|
||||
#
|
||||
# Feedback (streamed):
|
||||
# stage: High‑level stage label: START | WAIT | RUN | END.
|
||||
# current_skill:Name of the skill currently being processed.
|
||||
# progress: Float in [0,1]. START of first skill => 0.0, END of last
|
||||
# skill => 1.0. Intermediate values are proportional to
|
||||
# (completed_skills + stage_offset)/total_skills where
|
||||
# stage_offset is 0 for START/WAIT/RUN and 1 for END of a skill.
|
||||
# detail: Optional detail (e.g. retry attempt, OK/FAIL, waiting cause).
|
||||
###############################################################
|
||||
|
||||
# Goal
|
||||
# epoch: Sequence epoch (monotonically increasing id assigned by Cerebrum). Used so the
|
||||
# server can reject stale goals and stop publishing feedback from older epochs.
|
||||
uint32 epoch
|
||||
string action_name
|
||||
|
||||
---
|
||||
# Result
|
||||
bool success
|
||||
string message
|
||||
int32 total_skills
|
||||
int32 succeeded_skills
|
||||
|
||||
---
|
||||
# Feedback
|
||||
uint32 epoch
|
||||
string stage
|
||||
string current_skill
|
||||
float32 progress
|
||||
string detail
|
||||
8
src/interfaces/action/HandControl.action
Normal file
8
src/interfaces/action/HandControl.action
Normal file
@@ -0,0 +1,8 @@
|
||||
# Goal: hand control parameters
|
||||
int32 mode # 0=open, 1=close
|
||||
float32 effort
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
float32 progress
|
||||
7
src/interfaces/action/LegControl.action
Normal file
7
src/interfaces/action/LegControl.action
Normal file
@@ -0,0 +1,7 @@
|
||||
# Goal: leg control parameters
|
||||
geometry_msgs/TwistStamped target
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
float32 progress
|
||||
12
src/interfaces/action/MoveToPosition.action
Normal file
12
src/interfaces/action/MoveToPosition.action
Normal file
@@ -0,0 +1,12 @@
|
||||
# Goal definition
|
||||
float32 target_x
|
||||
float32 target_y
|
||||
---
|
||||
# Result definition
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
# Feedback definition
|
||||
float32 current_x
|
||||
float32 current_y
|
||||
float32 progress
|
||||
7
src/interfaces/action/VisionGraspObject.action
Normal file
7
src/interfaces/action/VisionGraspObject.action
Normal file
@@ -0,0 +1,7 @@
|
||||
# Goal: specify object id/name
|
||||
string object_id
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
float32 grasp_progress
|
||||
5
src/interfaces/msg/Movej.msg
Executable file
5
src/interfaces/msg/Movej.msg
Executable file
@@ -0,0 +1,5 @@
|
||||
float32[] joint
|
||||
uint8 speed
|
||||
bool block
|
||||
uint8 trajectory_connect #0 代表立即规划，1 代表和下一条轨迹一起规划，当为 1 时，轨迹不会立即执行
|
||||
uint8 dof
|
||||
22
src/interfaces/package.xml
Normal file
22
src/interfaces/package.xml
Normal file
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>interfaces</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<build_depend>rosidl_default_generators</build_depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<member_of_group>rosidl_interface_packages</member_of_group>
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
6
src/interfaces/srv/VisionObjectRecognition.srv
Normal file
6
src/interfaces/srv/VisionObjectRecognition.srv
Normal file
@@ -0,0 +1,6 @@
|
||||
# Request: specify object name or class
|
||||
string object_id
|
||||
---
|
||||
# Response: detection result
|
||||
bool found
|
||||
geometry_msgs/PoseStamped pose
|
||||
40
src/modules/motion/arm_control/CMakeLists.txt
Normal file
40
src/modules/motion/arm_control/CMakeLists.txt
Normal file
@@ -0,0 +1,40 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(arm_control)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
add_executable(arm_control_node src/arm_control_node.cpp)
|
||||
target_include_directories(arm_control_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(arm_control_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
ament_target_dependencies(arm_control_node rclcpp rclcpp_action interfaces geometry_msgs)
|
||||
|
||||
install(TARGETS arm_control_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
127
src/modules/motion/arm_control/README.md
Normal file
127
src/modules/motion/arm_control/README.md
Normal file
@@ -0,0 +1,127 @@
|
||||
# arm_control 包（运动模块）
|
||||
|
||||
arm_control 是一个简易的 ROS 2 运动模块，提供 ArmSpaceControl 动作（Action）服务端，用于控制机械臂末端在空间中的目标位姿（geometry_msgs/PoseStamped）。该模块当前以模拟方式汇报进度、支持取消，并在完成后返回结果，方便上层流程联调与验证。
|
||||
|
||||
## 功能概览
|
||||
- 动作接口：`interfaces/action/ArmSpaceControl`
|
||||
- 默认动作名称：`/ArmSpaceControl`（可通过参数 `action_name` 修改，参数默认值为 `ArmSpaceControl`，统一使用首字母大写形式）
|
||||
- 反馈（Feedback）：`progress`（0.0 ~ 100.0）
|
||||
- 支持取消（Cancel）
|
||||
- 结果（Result）：`success`、`message`
|
||||
|
||||
## 依赖
|
||||
- ROS 2 Humble
|
||||
- rclcpp、rclcpp_action
|
||||
- interfaces（自定义接口包）
|
||||
- geometry_msgs
|
||||
|
||||
## 构建
|
||||
从工作空间根目录执行：
|
||||
```bash
|
||||
colcon build --packages-select arm_control
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
## 运行服务端
|
||||
两种方式择一：
|
||||
```bash
|
||||
# 方式一：使用多节点运行脚本，仅启动 arm_control 节点
|
||||
./src/scripts/run.sh --motion-only
|
||||
|
||||
# 方式二：直接运行可执行文件
|
||||
ros2 run arm_control arm_control_node
|
||||
```
|
||||
启动成功后日志类似：
|
||||
|
||||
```text
|
||||
[INFO] [arm_control_node]: ArmSpaceControl action server started on 'ArmSpaceControl'
|
||||
```
|
||||
|
||||
|
||||
## 使用 send_arm_goal.sh 发送目标（推荐）
|
||||
为避免命令行转义和 YAML 拼写问题，提供了辅助脚本：`src/scripts/send_arm_goal.sh`。
|
||||
|
||||
查看帮助：
|
||||
```bash
|
||||
./src/scripts/send_arm_goal.sh --help
|
||||
```
|
||||
常用示例：
|
||||
```bash
|
||||
# 1) 使用默认参数（/ArmSpaceControl，base_link，x=0.5 y=0.0 z=0.3，yaw=0deg）
|
||||
./src/scripts/send_arm_goal.sh
|
||||
|
||||
# 2) 指定位置和偏航角（角度）
|
||||
./src/scripts/send_arm_goal.sh --x 0.5 --y 0.0 --z 0.3 --yaw 90 --deg
|
||||
|
||||
# 3) 使用弧度制的偏航角
|
||||
./src/scripts/send_arm_goal.sh --yaw 1.57 --rad
|
||||
|
||||
# 4) 更改目标坐标系和动作名称
|
||||
./src/scripts/send_arm_goal.sh --frame map --action /ArmSpaceControl
|
||||
|
||||
# 5) 仅查看将要发送的 YAML（不真正发送）
|
||||
./src/scripts/send_arm_goal.sh --dry-run
|
||||
```
|
||||
脚本会：
|
||||
- 将 yaw（仅绕 Z 轴）转换为四元数（roll=pitch=0）
|
||||
- 组装 `PoseStamped` 的 YAML 负载
|
||||
- 调用：
|
||||
```
|
||||
ros2 action send_goal --feedback /ArmSpaceControl interfaces/action/ArmSpaceControl <yaml>
|
||||
```
|
||||
|
||||
若脚本没有执行权限，可赋权：
|
||||
```bash
|
||||
chmod +x src/scripts/send_arm_goal.sh
|
||||
```
|
||||
|
||||
## 直接使用 ros2 CLI 发送（手工方式）
|
||||
若更偏好手工命令，确保已 `source install/setup.bash`，并将 YAML 保持单行以避免转义问题：
|
||||
```bash
|
||||
ros2 action send_goal --feedback /ArmSpaceControl interfaces/action/ArmSpaceControl "{target: {header: {frame_id: base_link}, pose: {position: {x: 0.5, y: 0.0, z: 0.3}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}}}"
|
||||
```
|
||||
|
||||
## 取消目标
|
||||
从发送端输出中获取 `goal_id` 后执行：
|
||||
```bash
|
||||
ros2 action cancel /ArmSpaceControl <goal-id>
|
||||
```
|
||||
|
||||
## 查看与排查
|
||||
- 查看 action 列表与类型：
|
||||
```bash
|
||||
ros2 action list
|
||||
ros2 action info /ArmSpaceControl -t
|
||||
```
|
||||
- 查看接口定义：
|
||||
```bash
|
||||
ros2 interface show interfaces/action/ArmSpaceControl
|
||||
ros2 interface show geometry_msgs/msg/PoseStamped
|
||||
```
|
||||
- 常见问题：
|
||||
- 报错 “The passed action type is invalid”
|
||||
- 确认已在当前终端执行 `source install/setup.bash`
|
||||
- 类型字符串必须精确：`interfaces/action/ArmSpaceControl`
|
||||
- 避免使用反斜杠换行，改用单行 YAML 或使用脚本
|
||||
- 一直提示 “Waiting for an action server to become available…”
|
||||
- 确认服务端是否在运行：`ros2 action list` 应显示 `/ArmSpaceControl`
|
||||
- 检查命名空间或参数 `action_name` 是否与预期一致
|
||||
- 四元数异常
|
||||
- 脚本仅使用 yaw 生成姿态，确保 yaw 合理且单位正确（`--deg` 或 `--rad`）
|
||||
|
||||
## 与多节点运行脚本配合
|
||||
如果需要同时启动多个节点，并输出到文件：
|
||||
```bash
|
||||
./src/scripts/run.sh --motion-only --log-to-files
|
||||
# 查看日志
|
||||
ls -1 log/run/*/arm_control.log | tail -n1 | xargs tail -f
|
||||
```
|
||||
|
||||
## 实现说明
|
||||
服务端实现位于：`src/modules/motion/arm_control/src/arm_control_node.cpp`
|
||||
- 参数 `action_name` 控制动作名称（默认 `ArmSpaceControl`）
|
||||
- 回调 `handle_goal / handle_cancel / handle_accepted / execute` 分别处理目标、取消与执行流程
|
||||
- 当前为模拟执行逻辑，可在 `execute()` 中接入真实控制器
|
||||
|
||||
---
|
||||
如需将本模块接入上层状态机或其他组件，可继续在此 README 补充接口约定和联调步骤。
|
||||
22
src/modules/motion/arm_control/package.xml
Normal file
22
src/modules/motion/arm_control/package.xml
Normal file
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>arm_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>interfaces</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
140
src/modules/motion/arm_control/src/arm_control_node.cpp
Normal file
140
src/modules/motion/arm_control/src/arm_control_node.cpp
Normal file
@@ -0,0 +1,140 @@
|
||||
// Copyright (c) 2025
|
||||
// 简单的运动模块：提供 ArmSpaceControl 动作(Action)服务端。
|
||||
// Simple motion module that exposes an ArmSpaceControl action server.
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
|
||||
#include "interfaces/action/arm_space_control.hpp"
|
||||
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// MotionNode 节点：
|
||||
// - 启动一个 ArmSpaceControl 动作服务端（默认话题名称：/arm_space_control，可通过参数 action_name 修改）
|
||||
// - 接受目标后模拟进度反馈，支持取消，并在完成后返回结果
|
||||
class MotionNode : public rclcpp::Node
|
||||
{
|
||||
public:
|
||||
using ArmSpaceControl = interfaces::action::ArmSpaceControl;
|
||||
using GoalHandleArmSpaceControl = rclcpp_action::ServerGoalHandle<ArmSpaceControl>;
|
||||
|
||||
// 构造函数：
|
||||
// - 声明参数 action_name（默认 "ArmSpaceControl"，统一使用首字母大写形式避免重复）
|
||||
// - 创建 ArmSpaceControl 动作服务端，并绑定目标处理、取消处理和接收回调
|
||||
MotionNode()
|
||||
: Node("motion_node")
|
||||
{
|
||||
// Declare a parameter for action name (default: "ArmSpaceControl")
|
||||
action_name_ = this->declare_parameter<std::string>("action_name", "ArmSpaceControl");
|
||||
|
||||
using std::placeholders::_1;
|
||||
using std::placeholders::_2;
|
||||
|
||||
server_ = rclcpp_action::create_server<ArmSpaceControl>(
|
||||
this->get_node_base_interface(),
|
||||
this->get_node_clock_interface(),
|
||||
this->get_node_logging_interface(),
|
||||
this->get_node_waitables_interface(),
|
||||
action_name_,
|
||||
std::bind(&MotionNode::handle_goal, this, _1, _2),
|
||||
std::bind(&MotionNode::handle_cancel, this, _1),
|
||||
std::bind(&MotionNode::handle_accepted, this, _1));
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "ArmSpaceControl action server started on '%s'",
|
||||
action_name_.c_str());
|
||||
}
|
||||
|
||||
private:
|
||||
// 目标处理回调：
|
||||
// - 参数 uuid：目标的唯一标识
|
||||
// - 参数 goal：ArmSpaceControl 的目标数据（target pose）
|
||||
// - 返回值：是否接受目标，这里直接接受并执行
|
||||
rclcpp_action::GoalResponse handle_goal(
|
||||
const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const ArmSpaceControl::Goal> goal)
|
||||
{
|
||||
(void)uuid;
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(), "Received ArmSpaceControl goal: frame=%s (x=%.3f,y=%.3f,z=%.3f)",
|
||||
goal->target.header.frame_id.c_str(),
|
||||
goal->target.pose.position.x, goal->target.pose.position.y, goal->target.pose.position.z);
|
||||
// Accept all goals for now. You can validate mode/effort here.
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
// 取消处理回调：
|
||||
// - 当客户端请求取消时被调用，这里统一接受取消请求
|
||||
rclcpp_action::CancelResponse handle_cancel(
|
||||
const std::shared_ptr<GoalHandleArmSpaceControl>/*goal_handle*/)
|
||||
{
|
||||
RCLCPP_INFO(this->get_logger(), "Received request to cancel ArmSpaceControl goal");
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
// 接受目标回调：
|
||||
// - 收到新的目标后，开启独立线程执行，避免阻塞执行器线程
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleArmSpaceControl> goal_handle)
|
||||
{
|
||||
// Execute goal in a new thread to avoid blocking executor thread
|
||||
std::thread{std::bind(&MotionNode::execute, this, std::placeholders::_1), goal_handle}.detach();
|
||||
}
|
||||
|
||||
// 执行函数：
|
||||
// - 模拟执行流程，按 10% 递增发布反馈(progress)
|
||||
// - 如果检测到取消请求，则返回取消结果
|
||||
// - 正常完成后返回 success=true，并附带简单描述信息
|
||||
void execute(const std::shared_ptr<GoalHandleArmSpaceControl> goal_handle)
|
||||
{
|
||||
const auto goal = goal_handle->get_goal();
|
||||
|
||||
auto feedback = std::make_shared<ArmSpaceControl::Feedback>();
|
||||
auto result = std::make_shared<ArmSpaceControl::Result>();
|
||||
|
||||
// Simulate progress from 0 to 100 with small delay; react to cancel requests
|
||||
for (int i = 0; i <= 100; i += 10) {
|
||||
if (goal_handle->is_canceling()) {
|
||||
result->success = false;
|
||||
result->message = "ArmSpaceControl canceled";
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_INFO(this->get_logger(), "ArmSpaceControl goal canceled");
|
||||
return;
|
||||
}
|
||||
feedback->progress = static_cast<float>(i);
|
||||
goal_handle->publish_feedback(feedback);
|
||||
RCLCPP_DEBUG(this->get_logger(), "ArmSpaceControl progress: %.1f%%", feedback->progress);
|
||||
std::this_thread::sleep_for(100ms);
|
||||
}
|
||||
|
||||
// Complete successfully
|
||||
result->success = true;
|
||||
result->message = std::string("Arm moved to target in frame '") +
|
||||
goal->target.header.frame_id + "'";
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(this->get_logger(), "ArmSpaceControl goal succeeded: %s", result->message.c_str());
|
||||
}
|
||||
|
||||
private:
|
||||
std::string action_name_;
|
||||
rclcpp_action::Server<ArmSpaceControl>::SharedPtr server_;
|
||||
};
|
||||
|
||||
// 主函数：
|
||||
// - 初始化 ROS，启动 MotionNode，进入自旋，退出时关闭
|
||||
// 测试：
|
||||
// ros2 action send_goal --feedback /ArmSpaceControl interfaces/action/ArmSpaceControl "{target: {header: {frame_id: base_link}, pose: {position: {x: 0.5, y: 0.0, z: 0.3}, orientation: {w: 1.0, x: 0.0, y: 0.0, z: 0.0}}}}"
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<MotionNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
40
src/modules/motion/hand_control/CMakeLists.txt
Normal file
40
src/modules/motion/hand_control/CMakeLists.txt
Normal file
@@ -0,0 +1,40 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(hand_control)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
|
||||
add_executable(hand_control_node src/hand_control_node.cpp)
|
||||
target_include_directories(hand_control_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(hand_control_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
ament_target_dependencies(hand_control_node
|
||||
rclcpp
|
||||
rclcpp_action
|
||||
interfaces
|
||||
)
|
||||
|
||||
install(TARGETS hand_control_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
29
src/modules/motion/hand_control/package.xml
Normal file
29
src/modules/motion/hand_control/package.xml
Normal file
@@ -0,0 +1,29 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>hand_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<!-- Runtime and build dependencies -->
|
||||
<build_depend>rclcpp</build_depend>
|
||||
<exec_depend>rclcpp</exec_depend>
|
||||
|
||||
<build_depend>rclcpp_action</build_depend>
|
||||
<exec_depend>rclcpp_action</exec_depend>
|
||||
|
||||
<!-- Interface package that provides HandControl.action -->
|
||||
<build_depend>interfaces</build_depend>
|
||||
<exec_depend>interfaces</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
141
src/modules/motion/hand_control/src/hand_control_node.cpp
Normal file
141
src/modules/motion/hand_control/src/hand_control_node.cpp
Normal file
@@ -0,0 +1,141 @@
|
||||
// HandControl action server implementation
|
||||
|
||||
#include <memory>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <chrono>
|
||||
#include <atomic>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
|
||||
#include "interfaces/action/hand_control.hpp"
|
||||
|
||||
using HandControl = interfaces::action::HandControl;
|
||||
|
||||
class HandControlNode : public rclcpp::Node {
|
||||
public:
|
||||
HandControlNode()
|
||||
: Node("hand_control_node"), active_goal_count_(0)
|
||||
{
|
||||
using namespace std::placeholders;
|
||||
server_ = rclcpp_action::create_server<HandControl>(
|
||||
this,
|
||||
"HandControl",
|
||||
std::bind(&HandControlNode::handle_goal, this, _1, _2),
|
||||
std::bind(&HandControlNode::handle_cancel, this, _1),
|
||||
std::bind(&HandControlNode::handle_accepted, this, _1));
|
||||
|
||||
declare_parameter<double>("default_duration_sec", 2.0);
|
||||
declare_parameter<int>("open_mode_value", 0);
|
||||
declare_parameter<int>("close_mode_value", 1);
|
||||
declare_parameter<bool>("allow_concurrent_goals", false);
|
||||
RCLCPP_INFO(get_logger(), "HandControl action server started");
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::Server<HandControl>::SharedPtr server_;
|
||||
std::atomic<int> active_goal_count_;
|
||||
|
||||
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid, std::shared_ptr<const HandControl::Goal> goal)
|
||||
{
|
||||
(void)uuid;
|
||||
if (!get_parameter("allow_concurrent_goals").as_bool() && active_goal_count_.load() > 0) {
|
||||
RCLCPP_WARN(get_logger(), "Reject goal: another goal already active");
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
if (goal->mode != get_parameter("open_mode_value").as_int() && goal->mode != get_parameter("close_mode_value").as_int()) {
|
||||
RCLCPP_WARN(get_logger(), "Reject goal: unsupported mode %d", goal->mode);
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
RCLCPP_INFO(get_logger(), "Accept goal: mode=%d effort=%.2f", goal->mode, goal->effort);
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> goal_handle)
|
||||
{
|
||||
RCLCPP_INFO(get_logger(), "Cancel request received");
|
||||
(void)goal_handle;
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> goal_handle)
|
||||
{
|
||||
active_goal_count_.fetch_add(1);
|
||||
std::thread(&HandControlNode::execute, this, goal_handle).detach();
|
||||
}
|
||||
|
||||
void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<HandControl>> goal_handle)
|
||||
{
|
||||
const auto goal = goal_handle->get_goal();
|
||||
const double total_duration = get_parameter("default_duration_sec").as_double();
|
||||
const auto start = now();
|
||||
rclcpp::Rate r(20.0); // 50ms cycle
|
||||
float last_progress = 0.0f;
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
if (goal_handle->is_canceling()) {
|
||||
auto result = std::make_shared<HandControl::Result>();
|
||||
result->success = false;
|
||||
result->message = "canceled";
|
||||
goal_handle->canceled(result);
|
||||
RCLCPP_INFO(get_logger(), "Goal canceled");
|
||||
active_goal_count_.fetch_sub(1);
|
||||
return;
|
||||
}
|
||||
|
||||
auto elapsed = (now() - start).seconds();
|
||||
float progress = static_cast<float>(elapsed / total_duration);
|
||||
if (progress > 1.0f) progress = 1.0f;
|
||||
if (progress < last_progress) progress = last_progress; // monotonic
|
||||
last_progress = progress;
|
||||
|
||||
auto feedback = std::make_shared<HandControl::Feedback>();
|
||||
feedback->progress = progress;
|
||||
goal_handle->publish_feedback(feedback);
|
||||
|
||||
if (progress >= 1.0f) {
|
||||
break;
|
||||
}
|
||||
r.sleep();
|
||||
}
|
||||
|
||||
if (!rclcpp::ok()) {
|
||||
auto result = std::make_shared<HandControl::Result>();
|
||||
result->success = false;
|
||||
result->message = "shutdown";
|
||||
try { goal_handle->abort(result); } catch(...) {}
|
||||
active_goal_count_.fetch_sub(1);
|
||||
return;
|
||||
}
|
||||
|
||||
// Simulate success/failure based on mode and effort (simple heuristic)
|
||||
bool success = true;
|
||||
std::string msg = (goal->mode == get_parameter("open_mode_value").as_int()) ? "opened" : "closed";
|
||||
if (goal->effort < 0.0f) { success = false; msg = "negative effort"; }
|
||||
|
||||
auto result = std::make_shared<HandControl::Result>();
|
||||
result->success = success;
|
||||
result->message = msg;
|
||||
if (success) {
|
||||
goal_handle->succeed(result);
|
||||
RCLCPP_INFO(get_logger(), "Goal succeeded: %s", msg.c_str());
|
||||
} else {
|
||||
goal_handle->abort(result);
|
||||
RCLCPP_WARN(get_logger(), "Goal aborted: %s", msg.c_str());
|
||||
}
|
||||
active_goal_count_.fetch_sub(1);
|
||||
}
|
||||
};
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<HandControlNode>();
|
||||
rclcpp::executors::MultiThreadedExecutor exec;
|
||||
exec.add_node(node);
|
||||
exec.spin();
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
|
||||
39
src/modules/motion/leg_control/CMakeLists.txt
Normal file
39
src/modules/motion/leg_control/CMakeLists.txt
Normal file
@@ -0,0 +1,39 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(leg_control)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
# uncomment the following section in order to fill in
|
||||
# further dependencies manually.
|
||||
# find_package(<dependency> REQUIRED)
|
||||
|
||||
add_executable(leg_control_node src/leg_control_node.cpp)
|
||||
target_include_directories(leg_control_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(leg_control_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
ament_target_dependencies(leg_control_node rclcpp rclcpp_action interfaces)
|
||||
|
||||
install(TARGETS leg_control_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
133
src/modules/motion/leg_control/README.md
Normal file
133
src/modules/motion/leg_control/README.md
Normal file
@@ -0,0 +1,133 @@
|
||||
# leg_control
|
||||
|
||||
提供 `LegControl.action` 的 ROS2 Action Server 示例。
|
||||
|
||||
## Action 定义
|
||||
文件：`interfaces/action/LegControl.action`
|
||||
```
|
||||
# Goal: leg control parameters
|
||||
geometry_msgs/TwistStamped target
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
float32 progress
|
||||
```
|
||||
|
||||
- Goal: TwistStamped
|
||||
- Result: `success`, `message`
|
||||
- Feedback: `progress` (0.0~1.0)
|
||||
|
||||
## 节点
|
||||
可执行文件：`leg_control_node`
|
||||
- Action 名称：`/leg_control`
|
||||
- 行为：接受目标后按 10 个步骤模拟执行，发布进度反馈，最终返回成功。
|
||||
- 验证：当 `effort < 0` 会拒绝目标。
|
||||
|
||||
## 依赖
|
||||
`package.xml` 中已声明：
|
||||
- rclcpp
|
||||
- rclcpp_action
|
||||
- interfaces
|
||||
|
||||
## 编译
|
||||
在工作空间根目录：
|
||||
```
|
||||
# Goal: leg control parameters
|
||||
geometry_msgs/TwistStamped target
|
||||
---
|
||||
bool success
|
||||
string message
|
||||
---
|
||||
float32 progress
|
||||
```
|
||||
|
||||
- Goal: `target` (TwistStamped: header + twist.linear(x,y,z) + twist.angular(x,y,z))
|
||||
- Result: `success`, `message`
|
||||
- Feedback: `progress` (0.0~1.0)
|
||||
|
||||
## 发送测试目标 (命令行)
|
||||
使用 `ros2 action send_goal`：
|
||||
```bash
|
||||
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.3}}}}'
|
||||
```
|
||||
示例输出（简化）：
|
||||
```
|
||||
Accepted
|
||||
Feedback: progress: 0.1
|
||||
Feedback: progress: 0.2
|
||||
...
|
||||
Result: success: True message: "LegControl done"
|
||||
```
|
||||
|
||||
### 失败/拒绝示例
|
||||
```bash
|
||||
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.0, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.0}}}}'
|
||||
```
|
||||
输出：`Goal rejected`（零速度与零角速度被拒绝）。
|
||||
|
||||
### 取消示例
|
||||
另开终端发送：
|
||||
```bash
|
||||
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}}}' --feedback
|
||||
```
|
||||
在执行中（看到进度后）按 Ctrl+C 触发取消；或使用：
|
||||
```bash
|
||||
ros2 action send_goal /leg_control interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.2}}}}' --feedback --cancel-on=feedback:0.5
|
||||
```
|
||||
|
||||
## 在 C++ 客户端中调用示例
|
||||
```cpp
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "interfaces/action/leg_control.hpp"
|
||||
|
||||
using interfaces::action::LegControl;
|
||||
|
||||
class LegClient : public rclcpp::Node {
|
||||
public:
|
||||
LegClient(): Node("leg_client") {
|
||||
client_ = rclcpp_action::create_client<LegControl>(shared_from_this(), "leg_control");
|
||||
}
|
||||
void send() {
|
||||
if (!client_->wait_for_action_server(std::chrono::seconds(2))) {
|
||||
RCLCPP_ERROR(get_logger(), "Server not available");
|
||||
return;
|
||||
}
|
||||
auto goal_msg = LegControl::Goal();
|
||||
goal_msg.target.header.frame_id = "base_link";
|
||||
goal_msg.target.twist.linear.x = 0.2;
|
||||
goal_msg.target.twist.angular.z = 0.3;
|
||||
auto send_goal_options = rclcpp_action::Client<LegControl>::SendGoalOptions();
|
||||
send_goal_options.feedback_callback = [this](auto, auto feedback){
|
||||
RCLCPP_INFO(get_logger(), "Feedback progress=%.2f", feedback->progress);
|
||||
};
|
||||
send_goal_options.result_callback = [this](auto res){
|
||||
if (res.code == rclcpp_action::ResultCode::SUCCEEDED) {
|
||||
RCLCPP_INFO(get_logger(), "Result: %s", res.result->message.c_str());
|
||||
}
|
||||
};
|
||||
client_->async_send_goal(goal_msg, send_goal_options);
|
||||
}
|
||||
private:
|
||||
rclcpp_action::Client<LegControl>::SharedPtr client_;
|
||||
};
|
||||
```
|
||||
|
||||
## 与 SkillManager 集成（潜在）
|
||||
若在技能配置 `robot_skills.yaml` 中添加：
|
||||
```
|
||||
- name: LegControl
|
||||
interfaces:
|
||||
- LegControl.action
|
||||
```
|
||||
则 `SkillManager` 将可注册客户端并通过行为树调度。
|
||||
|
||||
## 扩展建议
|
||||
- 增加模式枚举常量（使用 `int32` + 常量说明）
|
||||
- 增加安全检查： effort 上限
|
||||
- 增加实时关节状态发布（自定义 msg）
|
||||
- 支持并行多目标（需队列管理）
|
||||
|
||||
---
|
||||
需要我继续补充测试脚本或 Python 客户端示例，请再告诉我。
|
||||
22
src/modules/motion/leg_control/package.xml
Normal file
22
src/modules/motion/leg_control/package.xml
Normal file
@@ -0,0 +1,22 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>leg_control</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>interfaces</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
102
src/modules/motion/leg_control/src/leg_control_node.cpp
Normal file
102
src/modules/motion/leg_control/src/leg_control_node.cpp
Normal file
@@ -0,0 +1,102 @@
|
||||
// leg_control_node.cpp
|
||||
// 提供 LegControl.action 的简单示例 Action Server
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
#include "interfaces/action/leg_control.hpp"
|
||||
#include <cmath>
|
||||
|
||||
using interfaces::action::LegControl;
|
||||
|
||||
class LegControlServer : public rclcpp::Node {
|
||||
public:
|
||||
using GoalHandleLeg = rclcpp_action::ServerGoalHandle<LegControl>;
|
||||
|
||||
LegControlServer(): Node("leg_control_node") {
|
||||
using namespace std::placeholders;
|
||||
action_server_ = rclcpp_action::create_server<LegControl>(
|
||||
this,
|
||||
"LegControl", // 话题/动作名称，与技能 snake_case 一致
|
||||
std::bind(&LegControlServer::handle_goal, this, _1, _2),
|
||||
std::bind(&LegControlServer::handle_cancel, this, _1),
|
||||
std::bind(&LegControlServer::handle_accepted, this, _1));
|
||||
RCLCPP_INFO(get_logger(), "LegControl action server ready on /LegControl");
|
||||
}
|
||||
|
||||
private:
|
||||
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID & uuid,
|
||||
std::shared_ptr<const LegControl::Goal> goal) {
|
||||
(void)uuid;
|
||||
const auto & lin = goal->target.twist.linear;
|
||||
const auto & ang = goal->target.twist.angular;
|
||||
double lin_norm = std::sqrt(lin.x * lin.x + lin.y * lin.y + lin.z * lin.z);
|
||||
double ang_norm = std::sqrt(ang.x * ang.x + ang.y * ang.y + ang.z * ang.z);
|
||||
RCLCPP_INFO(get_logger(), "Received LegControl goal: frame=%s lin_norm=%.3f ang_norm=%.3f",
|
||||
goal->target.header.frame_id.c_str(), lin_norm, ang_norm);
|
||||
// 简单校验: 线速度和角速度不能同时为 0
|
||||
// if (lin_norm < 1e-6 && ang_norm < 1e-6) {
|
||||
// RCLCPP_WARN(get_logger(), "Reject goal: zero twist");
|
||||
// return rclcpp_action::GoalResponse::REJECT;
|
||||
// }
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
|
||||
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<GoalHandleLeg> gh) {
|
||||
RCLCPP_INFO(get_logger(), "Cancel request received");
|
||||
(void)gh;
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
void handle_accepted(const std::shared_ptr<GoalHandleLeg> gh) {
|
||||
// 在新的线程里执行以免阻塞 executor
|
||||
std::thread{std::bind(&LegControlServer::execute, this, gh)}.detach();
|
||||
}
|
||||
|
||||
void execute(const std::shared_ptr<GoalHandleLeg> gh) {
|
||||
const auto goal = gh->get_goal();
|
||||
auto feedback = std::make_shared<LegControl::Feedback>();
|
||||
auto result = std::make_shared<LegControl::Result>();
|
||||
|
||||
const auto & lin = goal->target.twist.linear;
|
||||
const auto & ang = goal->target.twist.angular;
|
||||
RCLCPP_INFO(get_logger(), "Executing LegControl: lin(%.2f,%.2f,%.2f) ang(%.2f,%.2f,%.2f)",
|
||||
lin.x, lin.y, lin.z, ang.x, ang.y, ang.z);
|
||||
|
||||
const int steps = 10;
|
||||
for (int i = 1; rclcpp::ok() && i <= steps; ++i) {
|
||||
if (gh->is_canceling()) {
|
||||
result->success = false;
|
||||
result->message = "LegControl canceled";
|
||||
gh->canceled(result);
|
||||
RCLCPP_INFO(get_logger(), "LegControl goal canceled");
|
||||
return;
|
||||
}
|
||||
feedback->progress = static_cast<float>(i) / static_cast<float>(steps);
|
||||
gh->publish_feedback(feedback);
|
||||
RCLCPP_DEBUG(get_logger(), "Progress: %.2f", feedback->progress);
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(100));
|
||||
}
|
||||
|
||||
if (rclcpp::ok()) {
|
||||
result->success = true;
|
||||
result->message = "LegControl done";
|
||||
gh->succeed(result);
|
||||
RCLCPP_INFO(get_logger(), "LegControl goal succeeded");
|
||||
}
|
||||
}
|
||||
|
||||
rclcpp_action::Server<LegControl>::SharedPtr action_server_;
|
||||
};
|
||||
|
||||
|
||||
// 测试：
|
||||
// ros2 action send_goal /LegControl interfaces/action/LegControl '{target: {header: {frame_id: "base_link"}, twist: {linear: {x: 0.2, y: 0.0, z: 0.0}, angular: {x: 0.0, y: 0.0, z: 0.3}}}}'
|
||||
|
||||
|
||||
int main(int argc, char ** argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<LegControlServer>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
36
src/modules/vision/vision_object_recg/CMakeLists.txt
Normal file
36
src/modules/vision/vision_object_recg/CMakeLists.txt
Normal file
@@ -0,0 +1,36 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(vision_object_recg)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(interfaces REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
|
||||
add_executable(vision_object_recg_node src/vision_object_recg_node.cpp)
|
||||
target_include_directories(vision_object_recg_node PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>)
|
||||
target_compile_features(vision_object_recg_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17
|
||||
ament_target_dependencies(vision_object_recg_node rclcpp interfaces geometry_msgs)
|
||||
|
||||
install(TARGETS vision_object_recg_node
|
||||
DESTINATION lib/${PROJECT_NAME})
|
||||
|
||||
if(BUILD_TESTING)
|
||||
find_package(ament_lint_auto REQUIRED)
|
||||
# the following line skips the linter which checks for copyrights
|
||||
# comment the line when a copyright and license is added to all source files
|
||||
set(ament_cmake_copyright_FOUND TRUE)
|
||||
# the following line skips cpplint (only works in a git repo)
|
||||
# comment the line when this package is in a git repo and when
|
||||
# a copyright and license is added to all source files
|
||||
set(ament_cmake_cpplint_FOUND TRUE)
|
||||
ament_lint_auto_find_test_dependencies()
|
||||
endif()
|
||||
|
||||
ament_package()
|
||||
134
src/modules/vision/vision_object_recg/README.md
Normal file
134
src/modules/vision/vision_object_recg/README.md
Normal file
@@ -0,0 +1,134 @@
|
||||
# vision_object_recg
|
||||
|
||||
基于 ROS2 的视觉目标识别服务示例包，提供 `VisionObjectRecognition.srv` 服务端（service server）。
|
||||
|
||||
## 功能概述
|
||||
`vision_object_recg_node` 提供一个服务：`/vision_object_recognition`。
|
||||
|
||||
请求(`interfaces/srv/VisionObjectRecognition`):
|
||||
```
|
||||
string object_id
|
||||
---
|
||||
bool found
|
||||
geometry_msgs/PoseStamped pose
|
||||
```
|
||||
|
||||
当前示例逻辑：
|
||||
- 若 `object_id` 非空且不等于 `"unknown"`，认为识别成功：
|
||||
- `found = true`
|
||||
- 返回一个示例位姿（frame_id = `map`，位置固定 (1.0, 0.2, 0.8)）
|
||||
- 否则：
|
||||
- `found = false`
|
||||
- 位姿为零初始化（frame_id = `map`）
|
||||
|
||||
> 实际项目中可替换为：调用视觉推理 / 订阅检测结果 / 检索感知缓存 等。
|
||||
|
||||
## 依赖
|
||||
- rclcpp
|
||||
- interfaces (提供 `VisionObjectRecognition.srv`)
|
||||
- geometry_msgs
|
||||
|
||||
这些依赖已在本包的 `package.xml` 与 `CMakeLists.txt` 中声明。
|
||||
|
||||
## 编译
|
||||
在工作空间根目录（含有 `src/`）执行：
|
||||
```bash
|
||||
colcon build --packages-select vision_object_recg
|
||||
```
|
||||
编译完成后加载环境：
|
||||
```bash
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
## 运行服务节点
|
||||
```bash
|
||||
ros2 run vision_object_recg vision_object_recg_node
|
||||
```
|
||||
启动后会输出类似：
|
||||
```
|
||||
[INFO] [..] [vision_object_recg]: VisionObjectRecognition service server ready on /vision_object_recognition
|
||||
```
|
||||
|
||||
## 查看可用服务
|
||||
```bash
|
||||
ros2 service list | grep vision_object_recognition
|
||||
```
|
||||
应看到：
|
||||
```
|
||||
/vision_object_recognition
|
||||
```
|
||||
|
||||
## 测试调用 (命令行)
|
||||
### 成功示例
|
||||
```bash
|
||||
ros2 service call /vision_object_recognition interfaces/srv/VisionObjectRecognition '{object_id: "cup"}'
|
||||
```
|
||||
期望部分输出：
|
||||
```
|
||||
found: True
|
||||
pose: ... position: {x: 1.0, y: 0.2, z: 0.8} ...
|
||||
```
|
||||
|
||||
### 未找到示例
|
||||
```bash
|
||||
ros2 service call /vision_object_recognition interfaces/srv/VisionObjectRecognition '{object_id: "unknown"}'
|
||||
```
|
||||
期望部分输出：
|
||||
```
|
||||
found: False
|
||||
```
|
||||
|
||||
### 空字符串
|
||||
```bash
|
||||
ros2 service call /vision_object_recognition interfaces/srv/VisionObjectRecognition '{object_id: ""}'
|
||||
```
|
||||
期望：`found: False`。
|
||||
|
||||
## 在其它节点中作为客户端调用 (C++)
|
||||
简要示例：
|
||||
```cpp
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
|
||||
using interfaces::srv::VisionObjectRecognition;
|
||||
|
||||
auto node = rclcpp::Node::make_shared("test_client");
|
||||
auto client = node->create_client<VisionObjectRecognition>("vision_object_recognition");
|
||||
if (!client->wait_for_service(std::chrono::seconds(2))) {
|
||||
RCLCPP_ERROR(node->get_logger(), "Service not available");
|
||||
}
|
||||
auto req = std::make_shared<VisionObjectRecognition::Request>();
|
||||
req->object_id = "cup";
|
||||
auto future = client->async_send_request(req);
|
||||
if (future.wait_for(std::chrono::seconds(2)) == std::future_status::ready) {
|
||||
auto resp = future.get();
|
||||
RCLCPP_INFO(node->get_logger(), "found=%d", resp->found);
|
||||
}
|
||||
```
|
||||
|
||||
## 与 SkillManager 的关系
|
||||
`SkillManager` 中会为 `VisionObjectRecognition` 技能创建同名（snake_case）服务客户端，名称映射：
|
||||
```
|
||||
VisionObjectRecognition --> /vision_object_recognition
|
||||
```
|
||||
因此服务名必须保持为 `vision_object_recognition`。
|
||||
|
||||
## 典型扩展方向
|
||||
- 集成真实检测模型 (YOLO / RT-DETR / OpenVINO / TensorRT)
|
||||
- 支持多类别批量查询：将 `string object_id` 扩展为 `string[] object_ids`
|
||||
- 增加置信度字段：`float32 confidence`
|
||||
- 增加错误码：例如 `uint8 status` + 常量定义
|
||||
- 发布可视化 Marker (`visualization_msgs/MarkerArray`)
|
||||
|
||||
## 常见问题
|
||||
| 问题 | 可能原因 | 解决 |
|
||||
|------|----------|------|
|
||||
| 服务调用卡住 | 服务端未启动 | 确认已运行节点并 source 环境 |
|
||||
| include 找不到接口 | 未重新编译/未 source | 重新 `colcon build` 并 `source install/setup.bash` |
|
||||
| frame 不一致 | 下游期望的坐标系不同 | 修改返回位姿 header.frame_id |
|
||||
|
||||
## 许可证
|
||||
示例代码，可根据项目需要补充实际 License。
|
||||
|
||||
---
|
||||
若你需要我继续：接入真实识别逻辑 / 添加测试 / 修改服务定义，请再告诉我。
|
||||
23
src/modules/vision/vision_object_recg/package.xml
Normal file
23
src/modules/vision/vision_object_recg/package.xml
Normal file
@@ -0,0 +1,23 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>vision_object_recg</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="root@todo.todo">root</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<!-- Runtime & build dependencies -->
|
||||
<depend>rclcpp</depend>
|
||||
<depend>interfaces</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,59 @@
|
||||
// vision_object_recg_node.cpp
|
||||
// 提供 VisionObjectRecognition.srv 服务，实现简单的目标识别逻辑
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "interfaces/srv/vision_object_recognition.hpp"
|
||||
#include <geometry_msgs/msg/pose_stamped.hpp>
|
||||
|
||||
using interfaces::srv::VisionObjectRecognition;
|
||||
|
||||
class VisionObjectRecgNode : public rclcpp::Node {
|
||||
public:
|
||||
VisionObjectRecgNode(): Node("vision_object_recg") {
|
||||
// 服务名称与 SkillManager 中 to_snake_case("VisionObjectRecognition") 保持一致: VisionObjectRecognition
|
||||
service_ = create_service<VisionObjectRecognition>(
|
||||
"VisionObjectRecognition",
|
||||
std::bind(&VisionObjectRecgNode::handle_service, this, std::placeholders::_1, std::placeholders::_2));
|
||||
RCLCPP_INFO(get_logger(), "VisionObjectRecognition service server ready on /VisionObjectRecognition");
|
||||
}
|
||||
|
||||
private:
|
||||
void handle_service(const std::shared_ptr<VisionObjectRecognition::Request> req,
|
||||
std::shared_ptr<VisionObjectRecognition::Response> res) {
|
||||
RCLCPP_INFO(get_logger(), "Received recognition request for object_id='%s'", req->object_id.c_str());
|
||||
|
||||
// 这里放置实际的识别逻辑。当前示例: 当 object_id 非空且不是 'unknown' 时假设识别成功。
|
||||
if (!req->object_id.empty() && req->object_id != "unknown") {
|
||||
res->found = true;
|
||||
// 构造一个示例位姿 (可根据实际视觉结果替换)
|
||||
res->pose.header.stamp = now();
|
||||
res->pose.header.frame_id = "map"; // 或 camera_link/base_link 等
|
||||
res->pose.pose.position.x = 1.0;
|
||||
res->pose.pose.position.y = 0.2;
|
||||
res->pose.pose.position.z = 0.8;
|
||||
res->pose.pose.orientation.w = 1.0; // 单位四元数
|
||||
RCLCPP_INFO(get_logger(), "Object '%s' found at (%.2f, %.2f, %.2f)", req->object_id.c_str(),
|
||||
res->pose.pose.position.x,
|
||||
res->pose.pose.position.y,
|
||||
res->pose.pose.position.z);
|
||||
} else {
|
||||
res->found = false;
|
||||
res->pose = geometry_msgs::msg::PoseStamped();
|
||||
res->pose.header.stamp = now();
|
||||
res->pose.header.frame_id = "map";
|
||||
RCLCPP_WARN(get_logger(), "Object '%s' not found", req->object_id.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
rclcpp::Service<VisionObjectRecognition>::SharedPtr service_;
|
||||
};
|
||||
|
||||
//测试： ros2 service call /vision_object_recognition interfaces/srv/VisionObjectRecognition '{object_id: "cup"}'
|
||||
|
||||
int main(int argc, char ** argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
auto node = std::make_shared<VisionObjectRecgNode>();
|
||||
rclcpp::spin(node);
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
4
src/scripts/build.sh
Executable file
4
src/scripts/build.sh
Executable file
@@ -0,0 +1,4 @@
|
||||
#!/bin/bash
|
||||
|
||||
colcon build --packages-select interfaces smacc2_msgs smacc2 brain
|
||||
#colcon build --packages-select interfaces smacc2_msgs smacc2 brain arm_control leg_control vision_object_recg hand_control
|
||||
410
src/scripts/gen_skill_manager.py
Executable file
410
src/scripts/gen_skill_manager.py
Executable file
@@ -0,0 +1,410 @@
|
||||
#!/usr/bin/env python3
|
||||
"""
|
||||
Code generator: reads robot_skills.yaml and generates
|
||||
- src/brain/include/brain/skill_manager.hpp
|
||||
- src/brain/src/skill_manager.cpp
|
||||
|
||||
The generated C++ keeps the existing SkillManager API but
|
||||
instantiates concrete registrars based on YAML content, avoiding
|
||||
hardcoded if/else comparisons in C++.
|
||||
|
||||
Usage:
|
||||
python3 src/scripts/gen_skill_manager.py \
|
||||
--yaml src/brain/config/robot_skills.yaml \
|
||||
--include-dir src/brain/include/brain \
|
||||
--src-dir src/brain/src
|
||||
|
||||
Notes:
|
||||
- Topic naming (current): only CamelCase skill name is registered (no snake_case duplicate) to avoid redundant action/service topics.
|
||||
- BT node name (if used elsewhere): <SkillName>_H.
|
||||
- Interface entry format: BaseName.action | BaseName.service
|
||||
- C++ header includes Chinese comments.
|
||||
"""
|
||||
import argparse
|
||||
import sys
|
||||
from pathlib import Path
|
||||
from string import Template
|
||||
from typing import Dict
|
||||
|
||||
try:
|
||||
import yaml
|
||||
except Exception as e:
|
||||
print("Missing dependency: PyYAML. Install with: pip install pyyaml", file=sys.stderr)
|
||||
raise
|
||||
|
||||
HEADER_TEMPLATE = """#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <yaml-cpp/yaml.h>
|
||||
#include <string>
|
||||
#include <unordered_map>
|
||||
#include <vector>
|
||||
#include <memory>
|
||||
|
||||
#include \"brain/action_registry.hpp\"
|
||||
#include \"brain/bt_registry.hpp\"
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
// SkillSpec 用于描述一个技能在 YAML 中的结构体
|
||||
// - name: 技能名称（唯一标识）
|
||||
// - version: 版本号（可选，用于文档或兼容性控制）
|
||||
// - description: 技能说明文本
|
||||
// - required_skills: 该技能依赖的其它技能名列表（当前未自动生成流程，仅保留信息）
|
||||
// - interfaces: 该技能暴露/使用的接口列表，格式为 \"BaseName.action\" 或 \"BaseName.service\"
|
||||
struct SkillSpec
|
||||
{
|
||||
std::string name;
|
||||
std::string version;
|
||||
std::string description;
|
||||
std::vector<std::string> required_skills;
|
||||
std::vector<std::string> interfaces; // e.g. \"BaseName.action\" / \"BaseName.service\"
|
||||
};
|
||||
|
||||
// SkillManager 负责：
|
||||
// 1) 解析 robot_skills.yaml 描述文件
|
||||
// 2) 自动注册其 Action/Service 接口客户端
|
||||
// 3) 行为树节点注册在其它组件完成（本类不直接创建 BT action node）
|
||||
class SkillManager
|
||||
{
|
||||
public:
|
||||
SkillManager(rclcpp::Node * node, ActionClientRegistry * action_clients, BTRegistry * bt)
|
||||
: node_(node), action_clients_(action_clients), bt_(bt) {}
|
||||
|
||||
bool load_from_file(const std::string & yaml_path);
|
||||
|
||||
const std::unordered_map<std::string, SkillSpec> & skills() const {return skills_;}
|
||||
|
||||
// 枚举 action 技能 (skill_name, action_base)
|
||||
std::vector<std::pair<std::string, std::string>> enumerate_action_skills() const;
|
||||
|
||||
private:
|
||||
rclcpp::Node * node_ {nullptr};
|
||||
ActionClientRegistry * action_clients_ {nullptr};
|
||||
BTRegistry * bt_ {nullptr};
|
||||
std::unordered_map<std::string, SkillSpec> skills_;
|
||||
std::vector<rclcpp::ClientBase::SharedPtr> service_clients_; // 保存 service client 生命周期
|
||||
std::unordered_map<std::string, rclcpp::ClientBase::SharedPtr> service_client_map_; // skill name -> client
|
||||
|
||||
void register_interfaces_(const SkillSpec & s);
|
||||
|
||||
public:
|
||||
// 获取特定类型 service client（若存在）
|
||||
template<typename T>
|
||||
std::shared_ptr<rclcpp::Client<T>> get_typed_service_client(const std::string & skill_name) const
|
||||
{
|
||||
auto it = service_client_map_.find(skill_name);
|
||||
if (it == service_client_map_.end()) {return nullptr;}
|
||||
return std::dynamic_pointer_cast<rclcpp::Client<T>>(it->second);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace brain
|
||||
"""
|
||||
|
||||
CPP_PREAMBLE_TEMPLATE = Template("""// skill_manager.cpp (generated)
|
||||
// 本文件由 gen_skill_manager.py 自动生成，请勿手工修改。
|
||||
// - 根据 robot_skills.yaml 中的技能定义，生成接口类型引用与注册逻辑。
|
||||
//#include <ament_index_cpp/get_package_share_directory.hpp>
|
||||
#include \"brain/skill_manager.hpp\"
|
||||
|
||||
#include <optional>
|
||||
#include <cctype>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
${includes}
|
||||
|
||||
${usings}
|
||||
|
||||
namespace brain
|
||||
{
|
||||
|
||||
// 将字符串转为小写
|
||||
static inline std::string to_lower(std::string s)
|
||||
{
|
||||
for (char & c : s) {
|
||||
c = static_cast<char>(::tolower(static_cast<unsigned char>(c)));
|
||||
}
|
||||
return s;
|
||||
}
|
||||
|
||||
// 将 TitleCase/camelCase 转换为 snake_case
|
||||
static inline std::string to_snake_case(const std::string & s)
|
||||
{
|
||||
std::string out;
|
||||
out.reserve(s.size() * 2);
|
||||
for (size_t i = 0; i < s.size(); ++i) {
|
||||
unsigned char uc = static_cast<unsigned char>(s[i]);
|
||||
if (::isupper(uc)) {
|
||||
if (i > 0) {out.push_back('_');}
|
||||
out.push_back(static_cast<char>(::tolower(uc)));
|
||||
} else {
|
||||
out.push_back(static_cast<char>(uc));
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
struct ParsedInterface { std::string base; std::string kind; };
|
||||
|
||||
static std::optional<ParsedInterface> parse_interface_entry(const std::string & entry)
|
||||
{
|
||||
const auto pos = entry.rfind('.');
|
||||
if (pos == std::string::npos || pos == 0 || pos + 1 >= entry.size()) {
|
||||
return std::nullopt;
|
||||
}
|
||||
ParsedInterface p;
|
||||
p.base = entry.substr(0, pos);
|
||||
p.kind = to_lower(entry.substr(pos + 1));
|
||||
return p;
|
||||
}
|
||||
|
||||
bool SkillManager::load_from_file(const std::string & yaml_path)
|
||||
{
|
||||
YAML::Node root = YAML::LoadFile(yaml_path);
|
||||
if (!root || !root.IsSequence()) {
|
||||
RCLCPP_ERROR(node_->get_logger(), "Invalid skills YAML: expected sequence at root");
|
||||
return false;
|
||||
}
|
||||
|
||||
for (const auto & item : root) {
|
||||
if (!item.IsMap()) {continue;}
|
||||
SkillSpec s;
|
||||
s.name = item["name"].as<std::string>("");
|
||||
s.version = item["version"].as<std::string>("");
|
||||
s.description = item["description"].as<std::string>("");
|
||||
if (item["required_skills"]) {
|
||||
for (const auto & v : item["required_skills"]) {
|
||||
s.required_skills.push_back(v.as<std::string>());
|
||||
}
|
||||
}
|
||||
if (item["interfaces"]) {
|
||||
for (const auto & v : item["interfaces"]) {
|
||||
s.interfaces.push_back(v.as<std::string>());
|
||||
}
|
||||
}
|
||||
if (s.name.empty()) {continue;}
|
||||
skills_[s.name] = s;
|
||||
register_interfaces_(s);
|
||||
}
|
||||
|
||||
RCLCPP_INFO(node_->get_logger(), "Loaded %zu skills from %s", skills_.size(), yaml_path.c_str());
|
||||
return true;
|
||||
}
|
||||
|
||||
std::vector<std::pair<std::string, std::string>> SkillManager::enumerate_action_skills() const
|
||||
{
|
||||
std::vector<std::pair<std::string, std::string>> out;
|
||||
for (const auto & kv : skills_) {
|
||||
const auto & s = kv.second;
|
||||
for (const auto & iface : s.interfaces) {
|
||||
auto p = parse_interface_entry(iface);
|
||||
if (p && p->kind == "action") { out.emplace_back(s.name, p->base); break; }
|
||||
}
|
||||
}
|
||||
return out;
|
||||
}
|
||||
|
||||
void SkillManager::register_interfaces_(const SkillSpec & s)
|
||||
{
|
||||
// 由代码生成器根据 YAML 填充的注册器表（接口基名 -> 注册逻辑）
|
||||
static const std::unordered_map<std::string, std::function<void(const std::string &, const std::string &)>> action_registrars = {
|
||||
${action_registrars}
|
||||
};
|
||||
static const std::unordered_map<std::string, std::function<void(const std::string &, const std::string &)>> service_registrars = {
|
||||
${service_registrars}
|
||||
};
|
||||
|
||||
for (const auto & iface : s.interfaces) {
|
||||
auto parsed = parse_interface_entry(iface);
|
||||
if (!parsed) {
|
||||
RCLCPP_WARN(node_->get_logger(), "Skip invalid interface entry: %s", iface.c_str());
|
||||
continue;
|
||||
}
|
||||
// 统一策略：Action/Service 话题名直接使用技能名 (CamelCase)，不再额外注册 snake_case 变体
|
||||
const std::string topic_name = s.name;
|
||||
if (parsed->kind == "action") {
|
||||
auto it = action_registrars.find(parsed->base);
|
||||
if (it != action_registrars.end()) {
|
||||
it->second(topic_name, s.name);
|
||||
} else {
|
||||
RCLCPP_WARN(node_->get_logger(), "No action registrar for interface base '%s'", parsed->base.c_str());
|
||||
}
|
||||
} else if (parsed->kind == "service") {
|
||||
auto it = service_registrars.find(parsed->base);
|
||||
if (it != service_registrars.end()) {
|
||||
it->second(topic_name, s.name);
|
||||
} else {
|
||||
RCLCPP_WARN(node_->get_logger(), "No service registrar for interface base '%s'", parsed->base.c_str());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace brain
|
||||
""")
|
||||
|
||||
|
||||
def title_to_snake(name: str) -> str:
|
||||
s = []
|
||||
for i, ch in enumerate(name):
|
||||
if ch.isupper():
|
||||
if i > 0:
|
||||
s.append('_')
|
||||
s.append(ch.lower())
|
||||
else:
|
||||
s.append(ch)
|
||||
return ''.join(s)
|
||||
|
||||
|
||||
def load_goal_defaults(path: Path) -> Dict[str, str]:
|
||||
if not path or not path.exists():
|
||||
return {}
|
||||
try:
|
||||
data = yaml.safe_load(path.read_text(encoding='utf-8'))
|
||||
if isinstance(data, dict):
|
||||
# Expect mapping: SkillName: |\n <C++ lines using g; must end with return g;>
|
||||
out: Dict[str, str] = {}
|
||||
for k, v in data.items():
|
||||
if isinstance(v, str):
|
||||
out[k] = v.strip() if "return" in v else (v.strip() + "\nreturn g;")
|
||||
return out
|
||||
except Exception as e:
|
||||
print(f"[warn] Failed to load goal defaults: {e}", file=sys.stderr)
|
||||
return {}
|
||||
|
||||
|
||||
def generate(yaml_path: Path, include_dir: Path, src_dir: Path, goal_defaults_path: Path = None) -> None:
|
||||
# 1. Load goal defaults
|
||||
goal_defaults = load_goal_defaults(goal_defaults_path) if goal_defaults_path else {}
|
||||
|
||||
# 2. Parse YAML
|
||||
data = yaml.safe_load(yaml_path.read_text(encoding='utf-8'))
|
||||
if not isinstance(data, list):
|
||||
raise ValueError('YAML root must be a sequence (list) of skills')
|
||||
|
||||
action_bases = set()
|
||||
service_bases = set()
|
||||
# Keep skills for potential future use (currently not needed further here)
|
||||
for it in data:
|
||||
if not isinstance(it, dict):
|
||||
continue
|
||||
name = it.get('name') or ''
|
||||
if not name:
|
||||
continue
|
||||
interfaces = it.get('interfaces') or []
|
||||
for e in interfaces:
|
||||
if not isinstance(e, str):
|
||||
continue
|
||||
pos = e.rfind('.')
|
||||
if pos <= 0 or pos + 1 >= len(e):
|
||||
continue
|
||||
base = e[:pos]
|
||||
kind = e[pos+1:].lower()
|
||||
if kind == 'action':
|
||||
action_bases.add(base)
|
||||
elif kind == 'service':
|
||||
service_bases.add(base)
|
||||
|
||||
# 3. Build include + using lines
|
||||
include_lines: list[str] = []
|
||||
using_lines: list[str] = []
|
||||
for base in sorted(action_bases):
|
||||
include_lines.append(f"#include \"interfaces/action/{title_to_snake(base)}.hpp\"")
|
||||
using_lines.append(f"using interfaces::action::{base};")
|
||||
for base in sorted(service_bases):
|
||||
include_lines.append(f"#include \"interfaces/srv/{title_to_snake(base)}.hpp\"")
|
||||
using_lines.append(f"using interfaces::srv::{base};")
|
||||
|
||||
# 4. Helper for goal initialization (prefer skill name then base name)
|
||||
def goal_builder(skill: str, base: str) -> str:
|
||||
key = skill if skill in goal_defaults else (base if base in goal_defaults else None)
|
||||
if key:
|
||||
snippet = goal_defaults[key].replace('${BASE}', base)
|
||||
prefix = f'{base}::Goal g;'
|
||||
if '::Goal g' in snippet:
|
||||
return snippet
|
||||
return prefix + '\n' + snippet
|
||||
return f'{base}::Goal g; return g;'
|
||||
|
||||
# 5. Build registrar blocks keyed only by interface base
|
||||
action_entries: list[str] = []
|
||||
for base in sorted(action_bases):
|
||||
block = (
|
||||
' {"%s", [this](const std::string & topic, const std::string & internal_skill) {\n'
|
||||
' if (action_clients_) {\n'
|
||||
' action_clients_->register_client<%s>(\n'
|
||||
' topic,\n'
|
||||
' []() { %s },\n'
|
||||
' nullptr,\n'
|
||||
' [this, internal_skill](const ActionClientRegistry::GoalHandle<%s>::WrappedResult & res) {\n'
|
||||
' if (res.code == rclcpp_action::ResultCode::SUCCEEDED && res.result && res.result->success)\n'
|
||||
' {\n'
|
||||
' RCLCPP_INFO(node_->get_logger(), "action succeeded: %%s", internal_skill.c_str());\n'
|
||||
' } else {\n'
|
||||
' RCLCPP_ERROR(node_->get_logger(), "action failed: %%s", internal_skill.c_str());\n'
|
||||
' }\n'
|
||||
' }\n'
|
||||
' );\n'
|
||||
' }\n'
|
||||
' }},'
|
||||
) % (base, base, goal_builder(base, base), base)
|
||||
action_entries.append(block)
|
||||
|
||||
service_entries: list[str] = []
|
||||
for base in sorted(service_bases):
|
||||
block = (
|
||||
' {"%s", [this](const std::string & topic, const std::string & internal_skill) {\n'
|
||||
' (void)internal_skill;\n'
|
||||
' auto client = node_->create_client<%s>(topic);\n'
|
||||
' service_clients_.push_back(client);\n'
|
||||
' service_client_map_[topic] = client;\n'
|
||||
' }},'
|
||||
) % (base, base)
|
||||
service_entries.append(block)
|
||||
|
||||
# 6. Emit files
|
||||
hdr_path = include_dir / 'skill_manager.hpp'
|
||||
cpp_path = src_dir / 'skill_manager.cpp'
|
||||
|
||||
hdr_code = HEADER_TEMPLATE
|
||||
cpp_code = CPP_PREAMBLE_TEMPLATE.substitute(
|
||||
includes='\n'.join(include_lines),
|
||||
usings='\n'.join(using_lines),
|
||||
action_registrars=("\n".join(action_entries) + ("\n" if action_entries else "")),
|
||||
service_registrars=("\n".join(service_entries) + ("\n" if service_entries else "")),
|
||||
camelcase_duplicates='' # retained for template compatibility
|
||||
)
|
||||
|
||||
hdr_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
cpp_path.parent.mkdir(parents=True, exist_ok=True)
|
||||
hdr_path.write_text(hdr_code, encoding='utf-8')
|
||||
cpp_path.write_text(cpp_code, encoding='utf-8')
|
||||
print(f"Generated: {hdr_path}")
|
||||
print(f"Generated: {cpp_path}")
|
||||
|
||||
|
||||
def main():
|
||||
ap = argparse.ArgumentParser()
|
||||
ap.add_argument('--yaml', dest='yaml_path', default='src/brain/config/robot_skills.yaml')
|
||||
ap.add_argument('--include-dir', dest='include_dir', default='src/brain/include/brain')
|
||||
ap.add_argument('--src-dir', dest='src_dir', default='src/brain/src')
|
||||
ap.add_argument('--goal-defaults', dest='goal_defaults', default=None,
|
||||
help='Optional YAML providing per-skill goal initialization snippets (no hard-coded names in generator).')
|
||||
args = ap.parse_args()
|
||||
|
||||
yaml_path = Path(args.yaml_path)
|
||||
include_dir = Path(args.include_dir)
|
||||
src_dir = Path(args.src_dir)
|
||||
goal_defaults = Path(args.goal_defaults) if args.goal_defaults else None
|
||||
|
||||
if not yaml_path.exists():
|
||||
print(f"YAML not found: {yaml_path}", file=sys.stderr)
|
||||
sys.exit(1)
|
||||
|
||||
generate(yaml_path, include_dir, src_dir, goal_defaults)
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
main()
|
||||
141
src/scripts/run.sh
Executable file
141
src/scripts/run.sh
Executable file
@@ -0,0 +1,141 @@
|
||||
#!/usr/bin/env bash
|
||||
|
||||
set -euo pipefail
|
||||
|
||||
# Resolve workspace root (script may be invoked from repo root)
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
WS_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
|
||||
|
||||
# Source ROS 2 workspace (temporarily disable nounset to avoid unbound vars inside setup scripts)
|
||||
if [[ -f "${WS_ROOT}/install/setup.bash" ]]; then
|
||||
# shellcheck source=/dev/null
|
||||
set +u
|
||||
source "${WS_ROOT}/install/setup.bash"
|
||||
set -u
|
||||
else
|
||||
echo "[run.sh] install/setup.bash not found at ${WS_ROOT}/install/setup.bash"
|
||||
echo "[run.sh] Please build the workspace first, e.g.,: colcon build && source install/setup.bash"
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Human-friendly logging format with local wall-clock time and colors
|
||||
export RCUTILS_COLORIZED_OUTPUT=${RCUTILS_COLORIZED_OUTPUT:-1}
|
||||
export RCUTILS_CONSOLE_OUTPUT_FORMAT='[{time}] [{severity}] [{name}]: {message}'
|
||||
|
||||
LOG_TO_FILES=0
|
||||
LOG_LEVEL="info" # debug|info|warn|error|fatal
|
||||
RUN_BRAIN=1
|
||||
RUN_MOTION=1
|
||||
|
||||
usage() {
|
||||
cat <<EOF
|
||||
Usage: ${0##*/} [options]
|
||||
|
||||
Options:
|
||||
-l, --log-to-files Write each node's stdout/stderr to log/run/<timestamp>/*.log
|
||||
-L, --log-level LEVEL Set ROS 2 log level (default: info). Ex: debug, warn, error
|
||||
--brain-only Run only brain node
|
||||
--arm_control-only Run only arm_control node
|
||||
-h, --help Show this help and exit
|
||||
|
||||
Environment:
|
||||
RCUTILS_COLORIZED_OUTPUT Colorize console logs (default: 1)
|
||||
RCUTILS_CONSOLE_OUTPUT_FORMAT Log format (already configured here)
|
||||
EOF
|
||||
}
|
||||
|
||||
# Parse args
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
-l|--log-to-files)
|
||||
LOG_TO_FILES=1; shift ;;
|
||||
-L|--log-level)
|
||||
LOG_LEVEL="${2:-info}"; shift 2 ;;
|
||||
--brain-only)
|
||||
RUN_BRAIN=1; RUN_MOTION=0; shift ;;
|
||||
--arm_control-only)
|
||||
RUN_BRAIN=0; RUN_MOTION=1; shift ;;
|
||||
-h|--help)
|
||||
usage; exit 0 ;;
|
||||
*)
|
||||
echo "Unknown option: $1" >&2
|
||||
usage; exit 2 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
timestamp() { date +"%Y%m%d_%H%M%S"; }
|
||||
|
||||
LOG_DIR="${WS_ROOT}/log/run/$(timestamp)"
|
||||
mkdir -p "${LOG_DIR}"
|
||||
|
||||
pids=()
|
||||
names=()
|
||||
|
||||
start_node() {
|
||||
local name="$1"; shift
|
||||
local pkg="$1"; shift
|
||||
local exe="$1"; shift
|
||||
local log_file="${LOG_DIR}/${name}.log"
|
||||
|
||||
if (( LOG_TO_FILES )); then
|
||||
echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} (logs: ${log_file})"
|
||||
(
|
||||
# Disable ANSI colors in log files for readability
|
||||
RCUTILS_COLORIZED_OUTPUT=0 exec ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}"
|
||||
) >>"${log_file}" 2>&1 &
|
||||
else
|
||||
echo "[run.sh] Starting ${name} -> ros2 run ${pkg} ${exe} --ros-args --log-level ${LOG_LEVEL}"
|
||||
ros2 run "${pkg}" "${exe}" --ros-args --log-level "${LOG_LEVEL}" &
|
||||
fi
|
||||
local pid=$!
|
||||
pids+=("${pid}")
|
||||
names+=("${name}")
|
||||
}
|
||||
|
||||
cleanup_done=0
|
||||
cleanup() {
|
||||
(( cleanup_done )) && return 0
|
||||
cleanup_done=1
|
||||
echo ""
|
||||
echo "[run.sh] Shutting down (${#pids[@]} processes)..."
|
||||
# Send SIGINT first for graceful shutdown
|
||||
for pid in "${pids[@]:-}"; do
|
||||
if kill -0 "$pid" 2>/dev/null; then
|
||||
kill -INT "$pid" 2>/dev/null || true
|
||||
fi
|
||||
done
|
||||
# Give them a moment to exit gracefully
|
||||
sleep 1
|
||||
# Force kill any stubborn processes
|
||||
for pid in "${pids[@]:-}"; do
|
||||
if kill -0 "$pid" 2>/dev/null; then
|
||||
kill -TERM "$pid" 2>/dev/null || true
|
||||
fi
|
||||
done
|
||||
wait || true
|
||||
echo "[run.sh] All processes stopped."
|
||||
}
|
||||
|
||||
trap cleanup INT TERM
|
||||
|
||||
# Start requested nodes
|
||||
if (( RUN_MOTION )); then
|
||||
start_node "arm_control" "arm_control" "motion_node"
|
||||
fi
|
||||
if (( RUN_BRAIN )); then
|
||||
start_node "brain" "brain" "brain_node"
|
||||
fi
|
||||
|
||||
if (( ${#pids[@]} == 0 )); then
|
||||
echo "[run.sh] Nothing to run. Use --brain-only or --arm_control-only to pick a node."
|
||||
exit 0
|
||||
fi
|
||||
|
||||
echo "[run.sh] Started: ${names[*]}"
|
||||
echo "[run.sh] Press Ctrl+C to stop."
|
||||
|
||||
# Wait for all background processes
|
||||
wait
|
||||
|
||||
# Ensure cleanup executes on normal exit too
|
||||
cleanup
|
||||
94
src/scripts/send_arm_goal.sh
Executable file
94
src/scripts/send_arm_goal.sh
Executable file
@@ -0,0 +1,94 @@
|
||||
#!/usr/bin/env bash
|
||||
set -euo pipefail
|
||||
|
||||
# Defaults
|
||||
ACTION_NAME="/ArmSpaceControl"
|
||||
FRAME_ID="base_link"
|
||||
X=0.5
|
||||
Y=0.0
|
||||
Z=0.3
|
||||
YAW_DEG=0.0
|
||||
ANGLE_UNIT="deg" # deg|rad
|
||||
DRY_RUN=0
|
||||
LOG_LEVEL="info"
|
||||
|
||||
usage(){
|
||||
cat <<EOF
|
||||
Usage: ${0##*/} [options]
|
||||
|
||||
Options:
|
||||
-a, --action NAME Action name (default: /arm_space_control)
|
||||
-f, --frame ID Target frame_id (default: base_link)
|
||||
--x VALUE Position x (m) (default: 0.5)
|
||||
--y VALUE Position y (m) (default: 0.0)
|
||||
--z VALUE Position z (m) (default: 0.3)
|
||||
--yaw VALUE Yaw angle (default: 0.0)
|
||||
--rad Interpret yaw as radians (default is degrees)
|
||||
--deg Interpret yaw as degrees (default)
|
||||
--log-level LEVEL ROS 2 log level (default: info)
|
||||
--dry-run Print goal YAML and exit
|
||||
-h, --help Show this help
|
||||
|
||||
Examples:
|
||||
${0##*/} --x 0.5 --y 0.0 --z 0.3 --yaw 90 --deg --frame base_link
|
||||
${0##*/} --action /ArmSpaceControl --yaw 1.57 --rad
|
||||
EOF
|
||||
}
|
||||
|
||||
# Parse args
|
||||
while [[ $# -gt 0 ]]; do
|
||||
case "$1" in
|
||||
-a|--action) ACTION_NAME="$2"; shift 2 ;;
|
||||
-f|--frame) FRAME_ID="$2"; shift 2 ;;
|
||||
--x) X="$2"; shift 2 ;;
|
||||
--y) Y="$2"; shift 2 ;;
|
||||
--z) Z="$2"; shift 2 ;;
|
||||
--yaw) YAW_DEG="$2"; shift 2 ;;
|
||||
--rad) ANGLE_UNIT="rad"; shift ;;
|
||||
--deg) ANGLE_UNIT="deg"; shift ;;
|
||||
--log-level) LOG_LEVEL="$2"; shift 2 ;;
|
||||
--dry-run) DRY_RUN=1; shift ;;
|
||||
-h|--help) usage; exit 0 ;;
|
||||
*) echo "Unknown option: $1" >&2; usage; exit 2 ;;
|
||||
esac
|
||||
done
|
||||
|
||||
# Compute quaternion from yaw only (roll=pitch=0)
|
||||
# qx=qy=0, qz=sin(yaw/2), qw=cos(yaw/2)
|
||||
python_quat='import math,sys; yaw=float(sys.argv[1]); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")'
|
||||
|
||||
if [[ "$ANGLE_UNIT" == "deg" ]]; then
|
||||
# Convert degrees to radians in Python for accuracy
|
||||
read -r QZ QW < <(python3 -c 'import math,sys; yaw_deg=float(sys.argv[1]); yaw=math.radians(yaw_deg); print(f"{math.sin(yaw/2.0)} {math.cos(yaw/2.0)}")' "$YAW_DEG")
|
||||
else
|
||||
read -r QZ QW < <(python3 -c "$python_quat" "$YAW_DEG")
|
||||
fi
|
||||
|
||||
# Build YAML (avoid shell-escaping issues; no quotes around frame_id)
|
||||
GOAL_YAML=$(cat <<YAML
|
||||
{target: {header: {frame_id: ${FRAME_ID}}, pose: {position: {x: ${X}, y: ${Y}, z: ${Z}}, orientation: {x: 0.0, y: 0.0, z: ${QZ}, w: ${QW}}}}}
|
||||
YAML
|
||||
)
|
||||
|
||||
if (( DRY_RUN )); then
|
||||
echo "Will send to action: ${ACTION_NAME}"
|
||||
echo "Goal YAML:"
|
||||
echo "$GOAL_YAML"
|
||||
exit 0
|
||||
fi
|
||||
|
||||
# Source workspace
|
||||
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||||
WS_ROOT="$(cd "${SCRIPT_DIR}/../.." && pwd)"
|
||||
if [[ -f "${WS_ROOT}/install/setup.bash" ]]; then
|
||||
# shellcheck source=/dev/null
|
||||
source "${WS_ROOT}/install/setup.bash"
|
||||
else
|
||||
echo "install/setup.bash not found at ${WS_ROOT}/install/setup.bash" >&2
|
||||
exit 1
|
||||
fi
|
||||
|
||||
# Send goal
|
||||
set -x
|
||||
ros2 action send_goal --feedback "${ACTION_NAME}" interfaces/action/ArmSpaceControl "$GOAL_YAML" --ros-args --log-level "${LOG_LEVEL}"
|
||||
set +x
|
||||
15
src/thirdparty/SMACC2-humble/.clang-format
vendored
Executable file
15
src/thirdparty/SMACC2-humble/.clang-format
vendored
Executable file
@@ -0,0 +1,15 @@
|
||||
---
|
||||
Language: Cpp
|
||||
BasedOnStyle: Google
|
||||
|
||||
ColumnLimit: 100
|
||||
AccessModifierOffset: -2
|
||||
AlignAfterOpenBracket: AlwaysBreak
|
||||
BreakBeforeBraces: Allman
|
||||
ConstructorInitializerIndentWidth: 0
|
||||
ContinuationIndentWidth: 2
|
||||
DerivePointerAlignment: false
|
||||
PointerAlignment: Middle
|
||||
ReflowComments: false
|
||||
IncludeBlocks: Preserve
|
||||
...
|
||||
7
src/thirdparty/SMACC2-humble/.gitignore
vendored
Normal file
7
src/thirdparty/SMACC2-humble/.gitignore
vendored
Normal file
@@ -0,0 +1,7 @@
|
||||
dependencies
|
||||
build
|
||||
install
|
||||
log
|
||||
bin
|
||||
.ccache
|
||||
.work
|
||||
129
src/thirdparty/SMACC2-humble/.pre-commit-config.yaml
vendored
Normal file
129
src/thirdparty/SMACC2-humble/.pre-commit-config.yaml
vendored
Normal file
@@ -0,0 +1,129 @@
|
||||
# To use:
|
||||
#
|
||||
# pre-commit run -a
|
||||
#
|
||||
# Or:
|
||||
#
|
||||
# pre-commit install # (runs every time you commit in git)
|
||||
#
|
||||
# To update this file:
|
||||
#
|
||||
# pre-commit autoupdate
|
||||
#
|
||||
# See https://github.com/pre-commit/pre-commit
|
||||
|
||||
repos:
|
||||
# Standard hooks
|
||||
- repo: https://github.com/pre-commit/pre-commit-hooks
|
||||
rev: v4.1.0
|
||||
hooks:
|
||||
- id: check-added-large-files
|
||||
- id: check-ast
|
||||
- id: check-case-conflict
|
||||
- id: check-docstring-first
|
||||
- id: check-merge-conflict
|
||||
- id: check-symlinks
|
||||
- id: check-xml
|
||||
#- id: check-yaml
|
||||
- id: debug-statements
|
||||
- id: end-of-file-fixer
|
||||
- id: mixed-line-ending
|
||||
#- id: trailing-whitespace
|
||||
- id: fix-byte-order-marker
|
||||
|
||||
# Python hooks
|
||||
- repo: https://github.com/asottile/pyupgrade
|
||||
rev: v2.31.0
|
||||
hooks:
|
||||
- id: pyupgrade
|
||||
args: [--py36-plus]
|
||||
|
||||
- repo: https://github.com/psf/black
|
||||
rev: 22.3.0
|
||||
hooks:
|
||||
- id: black
|
||||
args: ["--line-length=99"]
|
||||
|
||||
# PEP 257
|
||||
# - repo: https://github.com/FalconSocial/pre-commit-mirrors-pep257
|
||||
# rev: v0.3.3
|
||||
# hooks:
|
||||
# - id: pep257
|
||||
# args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
|
||||
|
||||
#- repo: https://github.com/pycqa/flake8
|
||||
# rev: 4.0.1
|
||||
# hooks:
|
||||
# - id: flake8
|
||||
# args: ["--ignore=E501,W503"]
|
||||
|
||||
# CPP hooks
|
||||
- repo: local
|
||||
hooks:
|
||||
- id: clang-format
|
||||
name: clang-format
|
||||
description: Format files with ClangFormat.
|
||||
entry: clang-format
|
||||
language: system
|
||||
files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
|
||||
args: ['-fallback-style=none', '-i']
|
||||
exclude: smacc2_sm_reference_library/
|
||||
|
||||
# - repo: local
|
||||
# hooks:
|
||||
# - id: ament_cppcheck
|
||||
# name: ament_cppcheck
|
||||
# description: Static code analysis of C/C++ files.
|
||||
# stages: [commit]
|
||||
# entry: ament_cppcheck
|
||||
# language: system
|
||||
# files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
|
||||
|
||||
# Maybe use https://github.com/cpplint/cpplint instead
|
||||
#- repo: local
|
||||
#hooks:
|
||||
#- id: ament_cpplint
|
||||
#name: ament_cpplint
|
||||
#description: Static code analysis of C/C++ files.
|
||||
#stages: [commit]
|
||||
#entry: ament_cpplint
|
||||
#language: system
|
||||
#files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
|
||||
#args: ["--linelength=100", "--filter=-whitespace/newline,-build/header_guard,-build/include_order,-build/namespaces"]
|
||||
|
||||
# Cmake hooks
|
||||
- repo: local
|
||||
hooks:
|
||||
- id: ament_lint_cmake
|
||||
name: ament_lint_cmake
|
||||
description: Check format of CMakeLists.txt files.
|
||||
stages: [commit]
|
||||
entry: ament_lint_cmake
|
||||
language: system
|
||||
files: CMakeLists\.txt$
|
||||
|
||||
# Copyright
|
||||
- repo: local
|
||||
hooks:
|
||||
- id: ament_copyright
|
||||
name: ament_copyright
|
||||
description: Check if copyright notice is available in all files.
|
||||
stages: [commit]
|
||||
entry: ament_copyright
|
||||
language: system
|
||||
|
||||
# Docs - RestructuredText hooks
|
||||
- repo: https://github.com/PyCQA/doc8
|
||||
rev: 0.10.1
|
||||
hooks:
|
||||
- id: doc8
|
||||
args: ['--max-line-length=100', '--ignore=D001']
|
||||
exclude: CHANGELOG\.rst$
|
||||
|
||||
- repo: https://github.com/pre-commit/pygrep-hooks
|
||||
rev: v1.9.0
|
||||
hooks:
|
||||
- id: rst-backticks
|
||||
exclude: CHANGELOG\.rst$
|
||||
- id: rst-directive-colons
|
||||
- id: rst-inline-touching-normal
|
||||
293
src/thirdparty/SMACC2-humble/CLAUDE.md
vendored
Normal file
293
src/thirdparty/SMACC2-humble/CLAUDE.md
vendored
Normal file
@@ -0,0 +1,293 @@
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
# SMACC2 Object Lifetimes
|
||||
SMACC2 runtime objects can be categorized according to object lifetimes.
|
||||
State machine-scoped objects persist throughout the entire state machine
|
||||
execution, while state-scoped objects are dynamically created and destroyed with
|
||||
state transitions.
|
||||
|
||||
### State Scoped Objects (Lifetime tied to individual states)
|
||||
- States (St)
|
||||
- Client Behaviors (Cb)
|
||||
- State Reactors (Sr)
|
||||
- Event Generators (Eg)
|
||||
|
||||
### State Machine Scoped Objects (Lifetime tied to the State Machine)
|
||||
- State Machines (Sm)
|
||||
- Orthogonals (Or)
|
||||
- Clients (Cl)
|
||||
- Components (Cp)
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
# SMACC Signals
|
||||
SmaccSignal is a communication mechanism (template wrapper around boost::signals2).
|
||||
|
||||
### Runtime architecture
|
||||
The actual runtime object is the SignalDetector (State Machine Scoped)
|
||||
- No prefix - singleton per state machine
|
||||
- Lifetime: State machine lifetime
|
||||
- Purpose: Polls and processes all signals in the system
|
||||
- Key responsibilities:
|
||||
- Manages signal processing loop
|
||||
- Finds and updates ISmaccUpdatable objects
|
||||
- Coordinates signal connections/disconnections
|
||||
- Handles execution model (single-threaded vs multi-threaded)
|
||||
|
||||
### Signal Connection Lifetime Management
|
||||
Signal connections are managed through the state machine's
|
||||
createSignalConnection() which:
|
||||
1. Creates boost::signals2 connections between signal sources and
|
||||
callbacks
|
||||
2. Tracks connections by object pointer
|
||||
3. Automatically disconnects when objects are destroyed via
|
||||
disconnectSmaccSignalObject()
|
||||
4. Ensures proper cleanup when states exit (disconnecting state-scoped
|
||||
object signals)
|
||||
|
||||
### Important Core Files
|
||||
- smacc2/include/smacc2/common.hpp
|
||||
- smacc2/include/smacc2/smacc_signal.hpp
|
||||
- smacc2/include/smacc2/smacc_state_machine.hpp
|
||||
- smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp
|
||||
- smacc2/include/smacc2/smacc_client.hpp
|
||||
- smacc2/include/smacc2/impl/smacc_client_impl.hpp
|
||||
- smacc2/include/smacc2/callback_counter_semaphore.hpp
|
||||
- smacc2/src/smacc2/orthogonal.cpp
|
||||
|
||||
### Important Client Library Related Files
|
||||
- smacc2/include/smacc2/smacc_asynchronous_client_behavior.hpp
|
||||
- smacc2/include/smacc2/client_core_components/cp_topic_subscriber.hpp
|
||||
- smacc2/include/smacc2/impl/smacc_asynchronous_client_behavior_impl.hpp
|
||||
- smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp
|
||||
- smacc2/include/smacc2/client_bases/smacc_service_server_client.hpp
|
||||
|
||||
### Important Classes
|
||||
- SmaccSignal
|
||||
|
||||
### Usage in SMACC Client Libraries
|
||||
- Clients have signals: onSucceeded_, onAborted_, onMessageReceived_
|
||||
- Components have signals: onMessageTimeout_, onResponseReceived_
|
||||
- ClientBehaviors have signals: onSuccess_, onFailure_, onFinished_
|
||||
|
||||
### Example usage in Clients:
|
||||
- smacc2_client_library/lifecyclenode_client/include/lifecyclenode_client/lifecyclenode_client.hpp
|
||||
- smacc2_client_library/ros_timer_client/include/ros_timer_client/cl_ros_timer.hpp
|
||||
- smacc2_client_library/moveit2z_client/include/moveit2z_client/cl_moveit2z.hpp
|
||||
- smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/nav2z_client.hpp
|
||||
- smacc2_client_library/http_client/include/http_client/cl_http_client.hpp
|
||||
- smacc2_client_library/keyboard_client/include/keyboard_client/cl_keyboard.hpp
|
||||
|
||||
### Example usage in Client Behaviors:
|
||||
- smacc2_client_library/ros_timer_client/include/ros_timer_client/client_behaviors/cb_timer_countdown_once.hpp
|
||||
|
||||
### Example usage in Components:
|
||||
- smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp
|
||||
|
||||
### Boost Signals2 Documentation
|
||||
https://www.boost.org/doc/libs/1_89_0/doc/html/signals2.html
|
||||
|
||||
### Boost Signals2 Source Code
|
||||
https://github.com/boostorg/signals2
|
||||
|
||||
## Problems related to incorrectly using boost signal raw instead of the SmaccSignal approach.
|
||||
After analyzing the SMACC signal architecture, using raw boost::signals2
|
||||
would cause catastrophic failures:
|
||||
|
||||
### Memory Leaks from Orphaned Connections
|
||||
Client behaviors create signal connections in onEntry() but without disconnectSmaccSignalObject() being called automatically at state exit, these connections would persist indefinitely.
|
||||
Each state transition would accumulate more dead connections, eventually exhausting memory.
|
||||
|
||||
### Segmentation Faults from Dangling Callbacks
|
||||
Most critical for client behaviors: When a CbNavigateForward subscribes to ClNav2Z::onSucceeded_ signal using raw boost::signals2, the connection would outlive the behavior.
|
||||
When navigation completes after the state has exited, the signal would invoke a callback on a destroyed behavior object → immediate segfault.
|
||||
|
||||
### State Machine Corruption
|
||||
Without EventLifeTime::CURRENT_STATE enforcement, client behaviors could post events after their state exits.
|
||||
Example: An async CbTimer using raw signals could fire EvTimer<TsFinished> into the wrong state, triggering invalid transitions and breaking the entire state flow.
|
||||
|
||||
### Race Conditions in Multi-threaded Execution
|
||||
The SignalDetector synchronizes signal processing through its polling loop and scheduler.
|
||||
Raw boost::signals2 connections would fire callbacks directly from arbitrary threads (ROS executor threads, timer threads), causing:
|
||||
- Concurrent state modifications
|
||||
- Non-deterministic event ordering
|
||||
- Data races in state machine internals
|
||||
|
||||
### Invisible Signal Flows
|
||||
createSignalConnection() registers connections with the state machine for introspection.
|
||||
Raw connections would be invisible to:
|
||||
- State machine visualization tools
|
||||
- Runtime debugging
|
||||
- Signal flow analysis
|
||||
- Connection leak detection
|
||||
|
||||
### Async Client Behavior Breakdown
|
||||
SmaccAsyncClientBehavior relies on signals (onSuccess_, onFailure_, onFinished_) managed by the framework.
|
||||
Raw signal usage would:
|
||||
- Break the async behavior protocol
|
||||
- Prevent proper requestForceFinish() handling
|
||||
- Cause behaviors to "hang" indefinitely
|
||||
- Make waitOnEntryThread_ synchronization fail
|
||||
|
||||
### Connection Lifecycle Chaos
|
||||
Without centralized management through signalConnectionsManager_, developers would need to manually track every connection and ensure disconnection at the exact right moment - an error-prone nightmare in complex state machines with dozens of concurrent behaviors.
|
||||
|
||||
The SmaccSignal approach isn't just convenience - it's essential for correctness. The automatic lifecycle management prevents an entire class of runtime failures that would be nearly impossible to debug in production.
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
# SMACC2 Threading & Asynchronous I/O
|
||||
SMACC2's threading architectureuses a sophisticated hybrid concurrency model that solves a fundamental robotics challenge: maintaining deterministic state machine behavior while handling inherently asynchronous real-world I/O.
|
||||
|
||||
SMACC2 uses what can be referred to as event-queue-mediated concurrency: async operations run in separate threads but communicate exclusively through a synchronized event queue, never directly touching state machine internals.
|
||||
|
||||
## Thread Hierarchy & Roles
|
||||
|
||||
### 1. State Machine Main Thread (Deterministic Core)
|
||||
- Executes all state logic, transitions, and onEntry()/onExit() calls
|
||||
- Only thread allowed to modify state machine state
|
||||
- Processes events sequentially from SmaccFifoScheduler queue
|
||||
- Guaranteed deterministic behavior regardless of event timing
|
||||
|
||||
### 2. SignalDetector Coordination Thread
|
||||
- Bridges async world to synchronous state machine
|
||||
- Runs at configurable frequency (default 20Hz) via pollingLoop()
|
||||
- Critical responsibilities:
|
||||
- Calls executeUpdate() on all ISmaccUpdatable objects
|
||||
- Processes ROS2 callbacks via rclcpp::spin_some()
|
||||
- Manages updatable object lifecycle (finds new ones, cleans up
|
||||
destroyed ones)
|
||||
- Enforces state transition barriers (no updates during transitions)
|
||||
|
||||
### 3. Async Behavior Worker Threads
|
||||
- Spawned via std::async(std::launch::async) for each
|
||||
SmaccAsyncClientBehavior
|
||||
- Run completely independent operations (navigation, manipulation, long
|
||||
computations)
|
||||
- Controlled termination: requestForceFinish_ flag monitored by behavior
|
||||
implementations
|
||||
- Lifecycle synchronization: State exit blocks until all async behaviors
|
||||
complete via waitOnEntryThread()
|
||||
|
||||
### 4. ROS2 Executor Threads
|
||||
- Handle ROS2 subscriptions, timers, service calls
|
||||
- Never directly modify state machine - only post events through signal
|
||||
connections
|
||||
- Two modes: SINGLE_THREAD_SPINNER or MULTI_THREAD_SPINNER for callback
|
||||
concurrency
|
||||
|
||||
## Async I/O Patterns & Use Cases
|
||||
|
||||
### Pattern 1: Long-Running Operations (SmaccAsyncClientBehavior)
|
||||
```cpp
|
||||
// Navigation behavior runs in separate thread
|
||||
class CbNavigateForward : public SmaccAsyncClientBehavior {
|
||||
void onEntry() override {
|
||||
// This runs in dedicated thread, doesn't block state machine
|
||||
navigationClient_->sendGoal(target);
|
||||
// Behavior posts success/failure events when complete
|
||||
}
|
||||
};
|
||||
```
|
||||
- Use case: Navigation, manipulation, file I/O, network requests
|
||||
- Lifecycle: Thread spawned on state entry, must complete before state
|
||||
exit
|
||||
- Communication: Posts EvCbSuccess/EvCbFailure events to queue
|
||||
|
||||
### Pattern 2: Periodic Monitoring (ISmaccUpdatable)
|
||||
```cpp
|
||||
// Component that monitors sensor data
|
||||
class CpLidarMonitor : public ISmaccComponent, public ISmaccUpdatable {
|
||||
void update() override {
|
||||
// Called every 50ms by SignalDetector
|
||||
if (checkObstacleDetected()) {
|
||||
postEvent<EvObstacleDetected>();
|
||||
}
|
||||
}
|
||||
};
|
||||
```
|
||||
- Use case: Sensor monitoring, watchdogs, periodic calculations
|
||||
- Lifecycle: Updated while component/behavior is active
|
||||
- Frequency: Controlled by SignalDetector loop rate
|
||||
|
||||
### Pattern 3: Event-Driven I/O (ROS2 Callbacks)
|
||||
```cpp
|
||||
// Timer fires in ROS executor thread
|
||||
timer_->registerCallback([this]() {
|
||||
// This runs in ROS thread, posts event to state machine
|
||||
onTimerTick_.emit(); // Signal connection handles thread-safe event
|
||||
posting
|
||||
});
|
||||
```
|
||||
- Use case: ROS2 subscriptions, timers, service responses
|
||||
- Lifecycle: Managed by ROS2 executor
|
||||
- Thread safety: Signals automatically queue events to main thread
|
||||
|
||||
## Synchronization Mechanisms
|
||||
|
||||
### 1. Event Queue as Synchronization Primitive
|
||||
- All async→sync communication flows through SmaccFifoScheduler event
|
||||
queue
|
||||
- Thread-safe event posting from any thread
|
||||
- Sequential processing in main thread ensures determinism
|
||||
|
||||
### 2. State Transition Barriers
|
||||
- Updates suspended during STATE_CONFIGURING, STATE_ENTERING,
|
||||
STATE_EXITING
|
||||
- Prevents race conditions during state machine topology changes
|
||||
- Async behaviors must complete before state exit proceeds
|
||||
|
||||
### 3. Automatic Resource Management
|
||||
- Signal connections track object lifetimes
|
||||
- disconnectSmaccSignalObject() prevents callbacks to destroyed objects
|
||||
- State-scoped objects automatically cleaned up on state exit
|
||||
|
||||
## Design Strengths
|
||||
|
||||
- Determinism Without Blocking: State machine logic remains predictable
|
||||
while I/O operations run concurrently.
|
||||
|
||||
- Deadlock Prevention: Multiple escape mechanisms (requestForceFinish_,
|
||||
timeouts, force disconnections) prevent system hangs.
|
||||
|
||||
- Memory Safety: Automatic lifecycle management prevents dangling pointers
|
||||
and callback-after-destruction bugs.
|
||||
|
||||
- Flexible Concurrency: Three distinct patterns handle different async I/O
|
||||
requirements without forcing a one-size-fits-all solution.
|
||||
|
||||
- Debugging Simplicity: Single-threaded state logic is much easier to debug
|
||||
than fully concurrent state machines.
|
||||
|
||||
The architecture achieves controlled chaos - allowing the inherently
|
||||
chaotic async world to coexist with the deterministic requirements of
|
||||
state machine logic through careful interface boundaries and
|
||||
synchronization primitives.
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
# Other SMACC2 CLAUDE.md File Locations
|
||||
|
||||
### Client Library
|
||||
SMACC2/smacc2_client_library/CLAUDE.md
|
||||
|
||||
### Reference Library
|
||||
SMACC2/smacc2_sm_reference_library/CLAUDE.md
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
# Other References
|
||||
|
||||
### Boost Statechart Documentation
|
||||
https://www.boost.org/doc/libs/1_80_0/libs/statechart/doc/index.html
|
||||
|
||||
### Boost Statechart Source Code
|
||||
https://github.com/boostorg/statechart
|
||||
|
||||
### Boost ASIO Documentation
|
||||
https://www.boost.org/doc/libs/1_89_0/doc/html/boost_asio.html
|
||||
|
||||
### Boost ASIO Source Code
|
||||
https://github.com/boostorg/asio
|
||||
70
src/thirdparty/SMACC2-humble/CONTRIBUTING.md
vendored
Normal file
70
src/thirdparty/SMACC2-humble/CONTRIBUTING.md
vendored
Normal file
@@ -0,0 +1,70 @@
|
||||
# Contributing Guidelines
|
||||
Thank you for your interest in contributing to `smacc2`.
|
||||
Whether it's a bug report, new feature, correction, or additional
|
||||
documentation, we greatly value feedback and contributions from our community.
|
||||
|
||||
Please read through this document before submitting any issues or pull requests to ensure we have all the necessary
|
||||
information to effectively respond to your bug report or contribution.
|
||||
|
||||
|
||||
## Reporting Bugs/Feature Requests
|
||||
We welcome you to use the GitHub issue tracker to report bugs or suggest features.
|
||||
|
||||
When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure
|
||||
somebody else hasn't already reported the issue.
|
||||
Please try to include as much information as you can. Details like these are incredibly useful:
|
||||
|
||||
* A reproducible test case or series of steps
|
||||
* The version of our code being used
|
||||
* Any modifications you've made relevant to the bug
|
||||
* Anything unusual about your environment or deployment
|
||||
|
||||
|
||||
## Contributing via Pull Requests
|
||||
Contributions via pull requests are much appreciated.
|
||||
Before sending us a pull request, please ensure that:
|
||||
|
||||
1. Limited scope. Your PR should do one thing or one set of things. Avoid adding “random fixes” to PRs. Put those on separate PRs.
|
||||
2. Give your PR a descriptive title. Add a short summary, if required.
|
||||
3. Make sure the pipeline is green.
|
||||
4. Don’t be afraid to request reviews from maintainers.
|
||||
5. New code = new tests. If you are adding new functionality, always make sure to add some tests exercising the code and serving as live documentation of your original intention.
|
||||
|
||||
To send us a pull request, please:
|
||||
|
||||
1. Fork the repository.
|
||||
2. Modify the source; please focus on the specific change you are contributing.
|
||||
If you also reformat all the code, it will be hard for us to focus on your change.
|
||||
3. Ensure local tests pass. (`colcon test` and `pre-commit run -a` (requires you to install pre-commit by `pip3 install pre-commit`)
|
||||
4. Commit to your fork using clear commit messages.
|
||||
5. Send a pull request, answering any default questions in the pull request interface.
|
||||
6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.
|
||||
|
||||
GitHub provides additional documentation on [forking a repository](https://help.github.com/articles/fork-a-repo/) and
|
||||
[creating a pull request](https://help.github.com/articles/creating-a-pull-request/).
|
||||
|
||||
|
||||
## Finding contributions to work on
|
||||
Looking at the existing issues is a great way to find something to contribute on.
|
||||
As this project, by default, uses the default GitHub issue labels
|
||||
(enhancement/bug/duplicate/help wanted/invalid/question/wontfix), looking at any ['help wanted'][help-wanted] issues
|
||||
is a great place to start.
|
||||
|
||||
|
||||
## Licensing
|
||||
Any contribution that you make to this repository will be under the Apache 2 License, as dictated by that [license](http://www.apache.org/licenses/LICENSE-2.0.html):
|
||||
|
||||
~~~
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
~~~
|
||||
|
||||
[issues]: https://github.com/robosoft-ai/SMACC2/issues
|
||||
[closed-issues]: https://github.com/robosoft-ai/SMACC2/issues?utf8=%E2%9C%93&q=is%3Aissue%20is%3Aclosed%20
|
||||
[help-wanted]: https://github.com/robosoft-ai/SMACC2/issues?q=is%3Aopen+is%3Aissue+label%3A%22help+wanted%22
|
||||
[license]: http://www.apache.org/licenses/LICENSE-2.0.html
|
||||
202
src/thirdparty/SMACC2-humble/LICENSE
vendored
Normal file
202
src/thirdparty/SMACC2-humble/LICENSE
vendored
Normal file
@@ -0,0 +1,202 @@
|
||||
|
||||
Apache License
|
||||
Version 2.0, January 2004
|
||||
http://www.apache.org/licenses/
|
||||
|
||||
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
|
||||
|
||||
1. Definitions.
|
||||
|
||||
"License" shall mean the terms and conditions for use, reproduction,
|
||||
and distribution as defined by Sections 1 through 9 of this document.
|
||||
|
||||
"Licensor" shall mean the copyright owner or entity authorized by
|
||||
the copyright owner that is granting the License.
|
||||
|
||||
"Legal Entity" shall mean the union of the acting entity and all
|
||||
other entities that control, are controlled by, or are under common
|
||||
control with that entity. For the purposes of this definition,
|
||||
"control" means (i) the power, direct or indirect, to cause the
|
||||
direction or management of such entity, whether by contract or
|
||||
otherwise, or (ii) ownership of fifty percent (50%) or more of the
|
||||
outstanding shares, or (iii) beneficial ownership of such entity.
|
||||
|
||||
"You" (or "Your") shall mean an individual or Legal Entity
|
||||
exercising permissions granted by this License.
|
||||
|
||||
"Source" form shall mean the preferred form for making modifications,
|
||||
including but not limited to software source code, documentation
|
||||
source, and configuration files.
|
||||
|
||||
"Object" form shall mean any form resulting from mechanical
|
||||
transformation or translation of a Source form, including but
|
||||
not limited to compiled object code, generated documentation,
|
||||
and conversions to other media types.
|
||||
|
||||
"Work" shall mean the work of authorship, whether in Source or
|
||||
Object form, made available under the License, as indicated by a
|
||||
copyright notice that is included in or attached to the work
|
||||
(an example is provided in the Appendix below).
|
||||
|
||||
"Derivative Works" shall mean any work, whether in Source or Object
|
||||
form, that is based on (or derived from) the Work and for which the
|
||||
editorial revisions, annotations, elaborations, or other modifications
|
||||
represent, as a whole, an original work of authorship. For the purposes
|
||||
of this License, Derivative Works shall not include works that remain
|
||||
separable from, or merely link (or bind by name) to the interfaces of,
|
||||
the Work and Derivative Works thereof.
|
||||
|
||||
"Contribution" shall mean any work of authorship, including
|
||||
the original version of the Work and any modifications or additions
|
||||
to that Work or Derivative Works thereof, that is intentionally
|
||||
submitted to Licensor for inclusion in the Work by the copyright owner
|
||||
or by an individual or Legal Entity authorized to submit on behalf of
|
||||
the copyright owner. For the purposes of this definition, "submitted"
|
||||
means any form of electronic, verbal, or written communication sent
|
||||
to the Licensor or its representatives, including but not limited to
|
||||
communication on electronic mailing lists, source code control systems,
|
||||
and issue tracking systems that are managed by, or on behalf of, the
|
||||
Licensor for the purpose of discussing and improving the Work, but
|
||||
excluding communication that is conspicuously marked or otherwise
|
||||
designated in writing by the copyright owner as "Not a Contribution."
|
||||
|
||||
"Contributor" shall mean Licensor and any individual or Legal Entity
|
||||
on behalf of whom a Contribution has been received by Licensor and
|
||||
subsequently incorporated within the Work.
|
||||
|
||||
2. Grant of Copyright License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
copyright license to reproduce, prepare Derivative Works of,
|
||||
publicly display, publicly perform, sublicense, and distribute the
|
||||
Work and such Derivative Works in Source or Object form.
|
||||
|
||||
3. Grant of Patent License. Subject to the terms and conditions of
|
||||
this License, each Contributor hereby grants to You a perpetual,
|
||||
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
|
||||
(except as stated in this section) patent license to make, have made,
|
||||
use, offer to sell, sell, import, and otherwise transfer the Work,
|
||||
where such license applies only to those patent claims licensable
|
||||
by such Contributor that are necessarily infringed by their
|
||||
Contribution(s) alone or by combination of their Contribution(s)
|
||||
with the Work to which such Contribution(s) was submitted. If You
|
||||
institute patent litigation against any entity (including a
|
||||
cross-claim or counterclaim in a lawsuit) alleging that the Work
|
||||
or a Contribution incorporated within the Work constitutes direct
|
||||
or contributory patent infringement, then any patent licenses
|
||||
granted to You under this License for that Work shall terminate
|
||||
as of the date such litigation is filed.
|
||||
|
||||
4. Redistribution. You may reproduce and distribute copies of the
|
||||
Work or Derivative Works thereof in any medium, with or without
|
||||
modifications, and in Source or Object form, provided that You
|
||||
meet the following conditions:
|
||||
|
||||
(a) You must give any other recipients of the Work or
|
||||
Derivative Works a copy of this License; and
|
||||
|
||||
(b) You must cause any modified files to carry prominent notices
|
||||
stating that You changed the files; and
|
||||
|
||||
(c) You must retain, in the Source form of any Derivative Works
|
||||
that You distribute, all copyright, patent, trademark, and
|
||||
attribution notices from the Source form of the Work,
|
||||
excluding those notices that do not pertain to any part of
|
||||
the Derivative Works; and
|
||||
|
||||
(d) If the Work includes a "NOTICE" text file as part of its
|
||||
distribution, then any Derivative Works that You distribute must
|
||||
include a readable copy of the attribution notices contained
|
||||
within such NOTICE file, excluding those notices that do not
|
||||
pertain to any part of the Derivative Works, in at least one
|
||||
of the following places: within a NOTICE text file distributed
|
||||
as part of the Derivative Works; within the Source form or
|
||||
documentation, if provided along with the Derivative Works; or,
|
||||
within a display generated by the Derivative Works, if and
|
||||
wherever such third-party notices normally appear. The contents
|
||||
of the NOTICE file are for informational purposes only and
|
||||
do not modify the License. You may add Your own attribution
|
||||
notices within Derivative Works that You distribute, alongside
|
||||
or as an addendum to the NOTICE text from the Work, provided
|
||||
that such additional attribution notices cannot be construed
|
||||
as modifying the License.
|
||||
|
||||
You may add Your own copyright statement to Your modifications and
|
||||
may provide additional or different license terms and conditions
|
||||
for use, reproduction, or distribution of Your modifications, or
|
||||
for any such Derivative Works as a whole, provided Your use,
|
||||
reproduction, and distribution of the Work otherwise complies with
|
||||
the conditions stated in this License.
|
||||
|
||||
5. Submission of Contributions. Unless You explicitly state otherwise,
|
||||
any Contribution intentionally submitted for inclusion in the Work
|
||||
by You to the Licensor shall be under the terms and conditions of
|
||||
this License, without any additional terms or conditions.
|
||||
Notwithstanding the above, nothing herein shall supersede or modify
|
||||
the terms of any separate license agreement you may have executed
|
||||
with Licensor regarding such Contributions.
|
||||
|
||||
6. Trademarks. This License does not grant permission to use the trade
|
||||
names, trademarks, service marks, or product names of the Licensor,
|
||||
except as required for reasonable and customary use in describing the
|
||||
origin of the Work and reproducing the content of the NOTICE file.
|
||||
|
||||
7. Disclaimer of Warranty. Unless required by applicable law or
|
||||
agreed to in writing, Licensor provides the Work (and each
|
||||
Contributor provides its Contributions) on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
|
||||
implied, including, without limitation, any warranties or conditions
|
||||
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
|
||||
PARTICULAR PURPOSE. You are solely responsible for determining the
|
||||
appropriateness of using or redistributing the Work and assume any
|
||||
risks associated with Your exercise of permissions under this License.
|
||||
|
||||
8. Limitation of Liability. In no event and under no legal theory,
|
||||
whether in tort (including negligence), contract, or otherwise,
|
||||
unless required by applicable law (such as deliberate and grossly
|
||||
negligent acts) or agreed to in writing, shall any Contributor be
|
||||
liable to You for damages, including any direct, indirect, special,
|
||||
incidental, or consequential damages of any character arising as a
|
||||
result of this License or out of the use or inability to use the
|
||||
Work (including but not limited to damages for loss of goodwill,
|
||||
work stoppage, computer failure or malfunction, or any and all
|
||||
other commercial damages or losses), even if such Contributor
|
||||
has been advised of the possibility of such damages.
|
||||
|
||||
9. Accepting Warranty or Additional Liability. While redistributing
|
||||
the Work or Derivative Works thereof, You may choose to offer,
|
||||
and charge a fee for, acceptance of support, warranty, indemnity,
|
||||
or other liability obligations and/or rights consistent with this
|
||||
License. However, in accepting such obligations, You may act only
|
||||
on Your own behalf and on Your sole responsibility, not on behalf
|
||||
of any other Contributor, and only if You agree to indemnify,
|
||||
defend, and hold each Contributor harmless for any liability
|
||||
incurred by, or claims asserted against, such Contributor by reason
|
||||
of your accepting any such warranty or additional liability.
|
||||
|
||||
END OF TERMS AND CONDITIONS
|
||||
|
||||
APPENDIX: How to apply the Apache License to your work.
|
||||
|
||||
To apply the Apache License to your work, attach the following
|
||||
boilerplate notice, with the fields enclosed by brackets "[]"
|
||||
replaced with your own identifying information. (Don't include
|
||||
the brackets!) The text should be enclosed in the appropriate
|
||||
comment syntax for the file format. We also recommend that a
|
||||
file or class name and description of purpose be included on the
|
||||
same "printed page" as the copyright notice for easier
|
||||
identification within third-party archives.
|
||||
|
||||
Copyright [yyyy] [name of copyright owner]
|
||||
|
||||
Licensed under the Apache License, Version 2.0 (the "License");
|
||||
you may not use this file except in compliance with the License.
|
||||
You may obtain a copy of the License at
|
||||
|
||||
http://www.apache.org/licenses/LICENSE-2.0
|
||||
|
||||
Unless required by applicable law or agreed to in writing, software
|
||||
distributed under the License is distributed on an "AS IS" BASIS,
|
||||
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
See the License for the specific language governing permissions and
|
||||
limitations under the License.
|
||||
181
src/thirdparty/SMACC2-humble/README.md
vendored
Normal file
181
src/thirdparty/SMACC2-humble/README.md
vendored
Normal file
@@ -0,0 +1,181 @@
|
||||
# SMACC2
|
||||
|
||||
SMACC2 is an event-driven, asynchronous, behavioral state machine library for real-time ROS 2 (Robotic Operating System) applications written in C++, designed to allow programmers to build robot control applications for multicomponent robots, in an intuitive and systematic manner.
|
||||
|
||||
SMACC was inspired by Harel's statecharts and the [SMACH ROS package](http://wiki.ros.org/smach). SMACC is built on top of the [Boost StateChart library](https://www.boost.org/doc/libs/1_53_0/libs/statechart/doc/index.html).
|
||||
|
||||
## Repository Status, Packages and Documentation
|
||||
|
||||
ROS 2 Distro | Branch | Build status | Documentation | Released packages
|
||||
:---------: | :----: | :----------: | :-----------: | :---------------:
|
||||
**Foxy** | [`foxy`](https://github.com/robosoft-ai/SMACC2/tree/foxy) | [](https://github.com/robosoft-ai/SMACC2/actions/workflows/foxy-binary-build.yml?branch=foxy) <br /> [](https://github.com/robosoft-ai/SMACC2/actions/workflows/foxy-semi-binary-build.yml?branch=foxy) | [](https://github.com/robosoft-ai/SMACC2/actions/workflows/doxygen-deploy.yml) <br /> [Generated Doc](https://robosoft-ai.github.io/smacc2_doxygen/foxy/html/namespaces.html) | [](http://docs.ros.org/en/humble/Releases/End-of-Life.html) <br/>[SMACC2](https://index.ros.org/p/smacc2/github-robosoft-ai-SMACC2/#foxy)
|
||||
**Humble** | [`humble`](https://github.com/robosoft-ai/SMACC2/tree/humble) | [](https://github.com/robosoft-ai/SMACC2/actions/workflows/humble-binary-build.yml?branch=humble)<br/> [](https://github.com/robosoft-ai/SMACC2/actions/workflows/humble-semi-binary-build.yml?branch=humble) | [](https://github.com/robosoft-ai/SMACC2/actions/workflows/doxygen-deploy.yml) <br /> [Generated Doc](https://robosoft-ai.github.io/smacc2_doxygen/humble/html/namespaces.html)| [](https://build.ros2.org/job/Hsrc_uJ__smacc2__ubuntu_jammy__source/)<br/> [SMACC2](https://index.ros.org/p/smacc2/github-robosoft-ai-SMACC2/#humble)
|
||||
|
||||
**NOTE**: There are three build stages checking current and future compatibility of the package.
|
||||
|
||||
1. Binary builds - against released packages (main and testing) in ROS distributions. Shows that direct local build is possible.
|
||||
|
||||
Uses repos file: `src/SMACC2/.github/SMACC2-not-released.<ros-distro>.repos`
|
||||
|
||||
1. Semi-binary builds - against released core ROS packages (main and testing), but the immediate dependencies are pulled from source.
|
||||
Shows that local build with dependencies is possible and if fails there we can expect that after the next package sync we will not be able to build.
|
||||
|
||||
Uses repos file: `src/SMACC2/.github/SMACC2.repos`
|
||||
|
||||
1. Source build - also core ROS packages are build from source. It shows potential issues in the mid future.
|
||||
|
||||
## Getting started - ROS Humble
|
||||
|
||||
1. [Install ROS 2 Humble](https://docs.ros.org/en/humble/Installation/Ubuntu-Install-Debians.html).
|
||||
|
||||
2. Make sure that `colcon`, its extensions and `vcs` are installed:
|
||||
```
|
||||
sudo apt install python3-colcon-common-extensions python3-vcstool
|
||||
```
|
||||
3. Create a new ROS 2 workspace if necessary:
|
||||
```
|
||||
export COLCON_WS=~/workspace/humble_ws
|
||||
mkdir -p $COLCON_WS/src
|
||||
```
|
||||
4. Or just navigate to your workspace source folder:
|
||||
```
|
||||
cd ~/workspace/humble_ws/src
|
||||
```
|
||||
5. Clone the repo:
|
||||
```
|
||||
git clone https://github.com/robosoft-ai/SMACC2.git
|
||||
```
|
||||
6. Checkout the Humble branch:
|
||||
```
|
||||
cd ~/workspace/humble_ws/src/SMACC2
|
||||
git checkout humble
|
||||
```
|
||||
7. Navigate to the workspace:
|
||||
```
|
||||
cd ~/workspace/humble_ws
|
||||
```
|
||||
8. Update System:
|
||||
```
|
||||
sudo apt update
|
||||
sudo apt upgrade
|
||||
```
|
||||
9. Source the workspace:
|
||||
```
|
||||
source /opt/ros/humble/setup.bash
|
||||
```
|
||||
10. Update dependencies:
|
||||
```
|
||||
rosdep update
|
||||
```
|
||||
11. Pull relevant packages and install dependencies:
|
||||
```
|
||||
vcs import src --skip-existing --input src/SMACC2/.github/SMACC2.humble.repos
|
||||
rosdep install --ignore-src --from-paths src -y -r
|
||||
```
|
||||
12. Compile:
|
||||
```
|
||||
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
```
|
||||
|
||||
## Getting started - ROS Foxy
|
||||
|
||||
1. [Install ROS 2 Foxy](https://docs.ros.org/en/foxy/Installation/Ubuntu-Install-Debians.html).
|
||||
|
||||
2. Make sure that `colcon`, its extensions and `vcs` are installed:
|
||||
```
|
||||
sudo apt install python3-colcon-common-extensions python3-vcstool
|
||||
```
|
||||
3. Create a new ROS 2 workspace if necessary:
|
||||
```
|
||||
export COLCON_WS=~/workspace/foxy_ws
|
||||
mkdir -p $COLCON_WS/src
|
||||
```
|
||||
4. Or just navigate to your workspace source folder:
|
||||
```
|
||||
cd ~/workspace/foxy_ws/src
|
||||
```
|
||||
5. Clone the repo:
|
||||
```
|
||||
git clone https://github.com/robosoft-ai/SMACC2.git
|
||||
```
|
||||
6. Checkout the Foxy branch:
|
||||
```
|
||||
cd ~/workspace/foxy_ws/src/SMACC2
|
||||
git checkout foxy
|
||||
```
|
||||
7. Navigate to the workspace:
|
||||
```
|
||||
cd ~/workspace/foxy_ws
|
||||
```
|
||||
8. Update System:
|
||||
```
|
||||
sudo apt update
|
||||
sudo apt upgrade
|
||||
```
|
||||
9. Source the workspace:
|
||||
```
|
||||
source /opt/ros/foxy/setup.bash
|
||||
```
|
||||
10. Update dependencies:
|
||||
```
|
||||
rosdep update
|
||||
```
|
||||
11. Pull relevant packages and install dependencies:
|
||||
```
|
||||
vcs import src --skip-existing --input src/SMACC2/.github/SMACC2.foxy.repos
|
||||
rosdep install --ignore-src --from-paths src -y -r
|
||||
```
|
||||
12. Compile:
|
||||
```
|
||||
colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release
|
||||
```
|
||||
|
||||
## Features
|
||||
* ***Powered by ROS 2:*** SMACC2 has been developed specifically to work with ROS 2. It supports ROS 2 topics, services and actions, right out of the box.
|
||||
* ***Written in C++:*** Until now, ROS 2 has lacked a library to develop task-level behavioral state machines in C++. Although libraries have been developed in scripting languages such as python, these are unsuitable for real-world industrial environments where real-time requirements are demanded.
|
||||
* ***Orthogonals:*** Originally conceived by David Harel in 1987, orthogonality is absolutely crucial to developing state machines for complex robotic systems. This is because complex robots are always a collection of hardware devices which require communication protocols, start-up determinism, etc. With orthogonals, it is an intuitive and relatively straight forward exercise (at least conceptually;) to code a state machine for a robot comprising a mobile base, a robotic arm, a gripper, two lidar sensors, a gps transceiver and an imu, for instance.
|
||||
* ***Static State Machine Checking:*** One of the features that SMACC2 inherits from Boost Statechart is that you get compile time validation checking. This benefits developers in that the amount of runtime testing necessary to ship quality software that is both stable and safe is dramatically reduced. Our philosophy is "Wherever possible, let the compiler do it".
|
||||
* ***State Machine Reference Library:*** With a constantly growing library of out-of-the-box reference state machines, (found in the folder [sm_reference_library](smacc2_sm_reference_library)) guaranteed to compile and run, you can jumpstart your development efforts by choosing a reference machine that is closest to your needs, and then customize and extend to meet the specific requirements of your robotic application. All the while knowing that the library supports advanced functionalities that are practically universal among actual working robots.
|
||||
* ***SMACC2 Client Library:*** SMACC2 also features a constantly growing library of [clients](smacc2_client_library) that support ROS 2 Action Servers, Service Servers and other nodes right out-of-the box. The clients within the SMACC2 Client library have been built utilizing a component based architecture that allows for developer to build powerful clients of their own. Current clients of note include MoveBaseZ, a full featured Action Client built to integrate with Nav2, the ros_timer_client, the multi_role_sensor_client, and a keyboard_client used extensively for state machine drafting & debugging.
|
||||
* ***Extensive Documentation:*** Although many ROS users are familiar with doxygen, our development team has spent a lot of time researching the more advanced features of doxygen such as uml style class diagrams and call graphs, and we've used them to document the SMACC2 library. Have a look to [our doxygen sites](https://robosoft-ai.github.io/smacc2_doxygen/master/html/namespaces.html) and we think you'll be blown away at what Doxygen looks like when [it's done right](https://robosoft-ai.github.io/smacc2_doxygen/master/html/classsmacc2_1_1ISmaccStateMachine.html) and it becomes a powerful tool to research a codebase.
|
||||
* ***SMACC2 Runtime Analyzer:*** The SMACC2 library works out of the box with the SMACC2 RTA. This allows developers to visualize and runtime debug the state machines they are working on. The SMACC2 RTA is closed source, but is free for individual and academic use. It can be found [here](https://robosoft.ai/product-category/smacc2-runtime-analyzer/).
|
||||
|
||||
## Repository Structure
|
||||
- `smacc2` - core library of SMACC2.
|
||||
- `smacc2_client_library` - client libraries for SMACC2, e.g., Navigation2 (`nav2z_client`), MoveIt2 (`moveit2z_client`).
|
||||
- `smacc2_event_generators` - ...
|
||||
- `smacc2_msgs` - ROS 2 messages for SMACC2 framework.
|
||||
- `smacc2_sm_reference_library` - libraries with reference implementations of state-machines used for demonstration and testing of functionalities.
|
||||
- `↓smacc2_state_reactor_library` - ...
|
||||
- `smacc2_performance_tools` - ...
|
||||
|
||||
## SMACC2 applications
|
||||
From it's inception, SMACC2 was written to support the programming of multi-component, complex robots. If your project involves small, solar-powered insect robots, that simply navigate towards a light source, then SMACC2 might not be the right choice for you. But if you are trying to program a robot with a mobile base, a robotic arm, a gripper, two lidar sensors, a gps transceiver and an imu, then you've come to the right place.
|
||||
|
||||
## Run a State Machine
|
||||
The easiest way to get started is by selecting one of the state machines in our [reference library](smacc2_sm_reference_library), and then hacking it to meet your needs.
|
||||
|
||||
Each state machine in the reference library comes with it's own README.md file, which contains the appropriate operating instructions, so that all you have to do is simply copy & paste some commands into your terminal.
|
||||
|
||||
|
||||
* If you are looking for a minimal example, we recommend [sm_atomic](smacc2_sm_reference_library/sm_atomic).
|
||||
|
||||
* If you are looking for a minimal example but with a looping superstate, try [sm_three_some](smacc2_sm_reference_library/sm_three_some).
|
||||
|
||||
* If you want to get started with the ROS Navigation stack right away, try [sm_nav2_test_7](https://github.com/robosoft-ai/nova_carter_sm_library/tree/main/sm_nav2_test_7).
|
||||
|
||||
Operating instructions can be found in each reference state machines readme file.
|
||||
|
||||
## Writing your State Machines
|
||||
There is a [state machine generator in the reference library](smacc2_sm_reference_library/create-sm-package.bash).
|
||||
To use it go to the `src` folder of your ROS 2 workspace and execute:
|
||||
```
|
||||
smacc2/smacc2_sm_reference_library/create-sm-package.bash <name_of_state_machine>
|
||||
```
|
||||
After than compile your workspace and source it to set paths of the new package.
|
||||
Check `README.md` in new package about instructions to start newly created state machine.
|
||||
|
||||
Happy Coding!
|
||||
|
||||
## Support
|
||||
If you are interested in getting involved or need a little support, feel free to contact us by emailing techsupport@robosoft.ai
|
||||
57
src/thirdparty/SMACC2-humble/smacc2/CHANGELOG.rst
vendored
Normal file
57
src/thirdparty/SMACC2-humble/smacc2/CHANGELOG.rst
vendored
Normal file
@@ -0,0 +1,57 @@
|
||||
Changelog for package smacc2
|
||||
================================
|
||||
|
||||
0.4.0 (2022-04-04)
|
||||
------------------
|
||||
### Added
|
||||
- Feature/fixing type string walker (`#263 <https://github.com/StoglRobotics-forks/SMACC2/issues/263>`_)
|
||||
- Feature/fixing husky build rolling (`#258 <https://github.com/StoglRobotics-forks/SMACC2/issues/258>`_)
|
||||
- Merging code from backport foxy and updates about autoware (`#208 <https://github.com/StoglRobotics-forks/SMACC2/issues/208>`_)
|
||||
- wharehouse2 progress (`#179 <https://github.com/StoglRobotics-forks/SMACC2/issues/179>`_)
|
||||
- Feature/aws navigation sm dance bot (`#174 <https://github.com/StoglRobotics-forks/SMACC2/issues/174>`_)
|
||||
- Feature/sm dance bot strikes back refactoring (`#152 <https://github.com/StoglRobotics-forks/SMACC2/issues/152>`_)
|
||||
- Feature/cb pause slam (`#98 <https://github.com/StoglRobotics-forks/SMACC2/issues/98>`_)
|
||||
- Merge branch 'renameTracingEvents' of https://github.com/DecDury/SMACC2 into DecDury-renameTracingEvents
|
||||
|
||||
### Contributors
|
||||
- David Revay, DecDury, Denis Štogl, Pablo Iñigo Blasco, pabloinigoblasco, reelrbtx
|
||||
|
||||
0.1.0 (2021-08-31)
|
||||
------------------
|
||||
### Added
|
||||
- Initial release of SMACC2 core
|
||||
|
||||
### Contributors
|
||||
- Brett Aldrich, Pablo Inigo Blasco, Denis Štogl
|
||||
|
||||
2.3.16 (2023-07-16)
|
||||
-------------------
|
||||
### Added
|
||||
- Merge branch 'humble' of https://github.com/robosoft-ai/SMACC2 into humble
|
||||
|
||||
### Fixed
|
||||
- Brettpac branch (`#518 <https://github.com/robosoft-ai/SMACC2/issues/518>`_)
|
||||
- Attempt to fix weird issue with ros buildfarm
|
||||
- More on this buildfarm issue
|
||||
|
||||
### Contributors
|
||||
- brettpac, pabloinigoblasco
|
||||
|
||||
2.3.6 (2023-03-12)
|
||||
------------------
|
||||
|
||||
1.22.1 (2022-11-09)
|
||||
-------------------
|
||||
### Added
|
||||
- pre-release
|
||||
|
||||
### Contributors
|
||||
- pabloinigoblasco
|
||||
|
||||
0.0.0 (2021-08-30)
|
||||
------------------
|
||||
### Added
|
||||
- Initial release of SMACC2 core
|
||||
|
||||
### Contributors
|
||||
- Pablo Inigo Blasco
|
||||
66
src/thirdparty/SMACC2-humble/smacc2/CMakeLists.txt
vendored
Normal file
66
src/thirdparty/SMACC2-humble/smacc2/CMakeLists.txt
vendored
Normal file
@@ -0,0 +1,66 @@
|
||||
cmake_minimum_required(VERSION 3.5)
|
||||
project(smacc2)
|
||||
|
||||
# Default to C++17
|
||||
if(NOT CMAKE_CXX_STANDARD)
|
||||
set(CMAKE_CXX_STANDARD 17)
|
||||
endif()
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_action REQUIRED)
|
||||
find_package(smacc2_msgs REQUIRED)
|
||||
find_package(Boost COMPONENTS thread REQUIRED)
|
||||
find_package(tracetools)
|
||||
find_package(LTTngUST REQUIRED)
|
||||
|
||||
set(dependencies
|
||||
rclcpp
|
||||
smacc2_msgs
|
||||
rclcpp_action
|
||||
tracetools
|
||||
)
|
||||
|
||||
include_directories(include ${LTTNGUST_INCLUDE_DIRS})
|
||||
|
||||
file(GLOB_RECURSE SRC_FILES src *.cpp)
|
||||
add_library(smacc2 SHARED ${SRC_FILES})
|
||||
|
||||
# removes unused parameters warning related with lttng, compiler warning
|
||||
set_source_files_properties(
|
||||
src/smacc2/smacc_tracing.cpp
|
||||
PROPERTIES
|
||||
COMPILE_FLAGS "-Wno-unused-parameter"
|
||||
)
|
||||
|
||||
ament_target_dependencies(smacc2 ${dependencies})
|
||||
target_link_libraries(smacc2
|
||||
${Boost_LIBRARIES}
|
||||
${LTTNGUST_LIBRARIES}
|
||||
)
|
||||
|
||||
ament_export_include_directories(include)
|
||||
ament_export_libraries(smacc2)
|
||||
ament_export_dependencies(${dependencies})
|
||||
|
||||
install(
|
||||
DIRECTORY include/
|
||||
DESTINATION include
|
||||
)
|
||||
|
||||
install(TARGETS
|
||||
smacc2
|
||||
DESTINATION lib/)
|
||||
|
||||
install(FILES
|
||||
scripts/trace.sh
|
||||
PERMISSIONS OWNER_READ OWNER_WRITE OWNER_EXECUTE GROUP_READ GROUP_EXECUTE WORLD_READ WORLD_EXECUTE
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
51
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/callback_counter_semaphore.hpp
vendored
Normal file
51
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/callback_counter_semaphore.hpp
vendored
Normal file
@@ -0,0 +1,51 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
#include <boost/signals2.hpp>
|
||||
#include <condition_variable>
|
||||
#include <iostream>
|
||||
#include <mutex>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <thread>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
class CallbackCounterSemaphore
|
||||
{
|
||||
public:
|
||||
CallbackCounterSemaphore(std::string name, int count = 0);
|
||||
bool acquire();
|
||||
|
||||
void release();
|
||||
|
||||
void finalize();
|
||||
|
||||
void addConnection(boost::signals2::connection conn);
|
||||
|
||||
private:
|
||||
int count_;
|
||||
std::mutex mutex_;
|
||||
std::condition_variable cv_;
|
||||
std::vector<boost::signals2::connection> connections_;
|
||||
bool finalized = false;
|
||||
std::string name_;
|
||||
};
|
||||
} // namespace smacc2
|
||||
57
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_action_client.hpp
vendored
Normal file
57
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_action_client.hpp
vendored
Normal file
@@ -0,0 +1,57 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
#include <rclcpp_action/client.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
// This class interface shows the basic set of methods that
|
||||
// a SMACC "resource" or "plugin" Action Client has
|
||||
class ISmaccActionClient : public ISmaccClient
|
||||
{
|
||||
public:
|
||||
ISmaccActionClient();
|
||||
|
||||
// The destructor. This is called when the object is not
|
||||
// referenced anymore by its owner
|
||||
virtual ~ISmaccActionClient();
|
||||
|
||||
// Gets the ros path of the action...
|
||||
inline std::string getNamespace() const { return name_; }
|
||||
|
||||
virtual bool cancelGoal() = 0;
|
||||
|
||||
virtual std::shared_ptr<rclcpp_action::ClientBase> getClientBase() = 0;
|
||||
|
||||
virtual std::string getName() const { return name_; }
|
||||
|
||||
protected:
|
||||
// The ros path where the action server is located
|
||||
std::string name_;
|
||||
};
|
||||
} // namespace client_bases
|
||||
} // namespace smacc2
|
||||
444
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp
vendored
Normal file
444
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp
vendored
Normal file
@@ -0,0 +1,444 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/client_bases/smacc_action_client.hpp>
|
||||
#include <smacc2/smacc_default_events.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
|
||||
#include <optional>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
using namespace smacc2::default_events;
|
||||
|
||||
template <typename ActionType>
|
||||
class SmaccActionClientBase : public ISmaccActionClient
|
||||
{
|
||||
public:
|
||||
// Inside this macro you can find the typedefs for Goal and other types
|
||||
// ACTION_DEFINITION(ActionType);
|
||||
typedef rclcpp_action::Client<ActionType> ActionClient;
|
||||
// typedef actionlib::SimpleActionClient<ActionType> GoalHandle;
|
||||
|
||||
// typedef typename ActionClient::SimpleDoneCallback SimpleDoneCallback;
|
||||
// typedef typename ActionClient::SimpleActiveCallback SimpleActiveCallback;
|
||||
// typedef typename ActionClient::SimpleFeedbackCallback SimpleFeedbackCallback;
|
||||
|
||||
using Goal = typename ActionClient::Goal;
|
||||
using Feedback = typename ActionClient::Feedback;
|
||||
using GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
|
||||
typedef typename GoalHandle::WrappedResult WrappedResult;
|
||||
|
||||
using SendGoalOptions = typename ActionClient::SendGoalOptions;
|
||||
using GoalResponseCallback =
|
||||
std::function<void(std::shared_future<typename GoalHandle::SharedPtr>)>;
|
||||
using FeedbackCallback = typename GoalHandle::FeedbackCallback;
|
||||
using ResultCallback = typename GoalHandle::ResultCallback;
|
||||
using CancelRequest = typename ActionType::Impl::CancelGoalService::Request;
|
||||
using CancelResponse = typename ActionType::Impl::CancelGoalService::Response;
|
||||
using CancelCallback = std::function<void(typename CancelResponse::SharedPtr)>;
|
||||
|
||||
typedef smacc2::SmaccSignal<void(const WrappedResult &)> SmaccActionResultSignal;
|
||||
|
||||
std::string action_endpoint_;
|
||||
SmaccActionClientBase(std::string actionServerName) : ISmaccActionClient()
|
||||
{
|
||||
action_endpoint_ = actionServerName;
|
||||
}
|
||||
|
||||
SmaccActionClientBase() : ISmaccActionClient() { name_ = ""; }
|
||||
|
||||
virtual ~SmaccActionClientBase() {}
|
||||
|
||||
virtual std::shared_ptr<rclcpp_action::ClientBase> getClientBase() override { return client_; }
|
||||
|
||||
void onInitialize() override
|
||||
{
|
||||
if (name_ == "") name_ = smacc2::demangleSymbol(typeid(*this).name());
|
||||
this->client_ = rclcpp_action::create_client<ActionType>(getNode(), action_endpoint_);
|
||||
// RCLCPP_INFO_STREAM(
|
||||
// this->getLogger(),
|
||||
// "Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
|
||||
//client_->wait_for_action_server();
|
||||
}
|
||||
|
||||
void waitForServer()
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
this->getLogger(),
|
||||
"Waiting for action server '" << name_ << "' of type: " << demangledTypeName<ActionType>());
|
||||
client_->wait_for_action_server();
|
||||
}
|
||||
|
||||
static std::string getEventLabel()
|
||||
{
|
||||
auto type = TypeInfo::getTypeInfoFromType<ActionType>();
|
||||
return type->getNonTemplatedTypeName();
|
||||
}
|
||||
|
||||
std::optional<std::shared_future<typename GoalHandle::SharedPtr>> lastRequest_;
|
||||
// typename GoalHandle::SharedPtr goalHandle_;
|
||||
std::optional<std::shared_future<typename CancelResponse::SharedPtr>> lastCancelResponse_;
|
||||
|
||||
SmaccActionResultSignal onSucceeded_;
|
||||
SmaccActionResultSignal onAborted_;
|
||||
// SmaccActionResultSignal onPreempted_;
|
||||
// SmaccActionResultSignal onRejected_;
|
||||
SmaccActionResultSignal onCancelled_;
|
||||
|
||||
// event creation/posting factory functions
|
||||
std::function<void(WrappedResult)> postSuccessEvent;
|
||||
std::function<void(WrappedResult)> postAbortedEvent;
|
||||
std::function<void(WrappedResult)> postCancelledEvent;
|
||||
// std::function<void(WrappedResult)> postPreemptedEvent;
|
||||
// std::function<void(WrappedResult)> postRejectedEvent;
|
||||
|
||||
std::function<void(const Feedback &)> postFeedbackEvent;
|
||||
|
||||
FeedbackCallback feedback_cb;
|
||||
|
||||
template <typename EvType>
|
||||
void postResultEvent(WrappedResult & /*result*/)
|
||||
{
|
||||
auto * ev = new EvType();
|
||||
// ev->client = this;
|
||||
// ev->resultMessage = *result;
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "Action client Posting EVENT %s", demangleSymbol(typeid(ev).name()).c_str());
|
||||
this->postEvent(ev);
|
||||
}
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onOrthogonalAllocation()
|
||||
{
|
||||
// we create here all the event factory functions capturing the TOrthogonal
|
||||
postSuccessEvent = [this](auto msg)
|
||||
{ this->postResultEvent<EvActionSucceeded<TSourceObject, TOrthogonal>>(msg); };
|
||||
postAbortedEvent = [this](auto msg)
|
||||
{ this->postResultEvent<EvActionAborted<TSourceObject, TOrthogonal>>(msg); };
|
||||
|
||||
postCancelledEvent = [this](auto msg)
|
||||
{ this->postResultEvent<EvActionCancelled<TSourceObject, TOrthogonal>>(msg); };
|
||||
|
||||
postFeedbackEvent = [this](auto msg)
|
||||
{
|
||||
auto actionFeedbackEvent = new EvActionFeedback<Feedback, TOrthogonal>();
|
||||
actionFeedbackEvent->client = this;
|
||||
actionFeedbackEvent->feedbackMessage = msg;
|
||||
this->postEvent(actionFeedbackEvent);
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", demangleType(typeid(*this)).c_str());
|
||||
};
|
||||
|
||||
feedback_cb = [this](auto client, auto feedback) { this->onFeedback(client, feedback); };
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onSucceeded(void (T::*callback)(WrappedResult &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onSucceeded_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onSucceeded(std::function<void(WrappedResult &)> callback)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onSucceeded_, callback);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onAborted(void (T::*callback)(WrappedResult &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onAborted_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onAborted(std::function<void(WrappedResult &)> callback)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onAborted_, callback);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onCancelled(void (T::*callback)(WrappedResult &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onCancelled_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onCancelled(std::function<void(WrappedResult &)> callback)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onCancelled_, callback);
|
||||
}
|
||||
|
||||
/*
|
||||
template <typename T>
|
||||
boost::signals2::connection onPreempted(void (T::*callback)(WrappedResult &), T *object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onPreempted_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onPreempted(std::function<void(WrappedResult &)> callback)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onPreempted_, callback);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onRejected(void (T::*callback)(WrappedResult &), T *object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onRejected_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onRejected(std::function<void(WrappedResult &)> callback)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onRejected_, callback);
|
||||
}
|
||||
*/
|
||||
|
||||
virtual bool cancelGoal() override
|
||||
{
|
||||
lastCancelResponse_ = this->client_->async_cancel_all_goals();
|
||||
|
||||
// if (lastRequest_ && lastRequest_->valid())
|
||||
// {
|
||||
|
||||
// // rclcpp::spin_until_future_complete(getNode(), *lastRequest_);
|
||||
// auto req = lastRequest_->get();
|
||||
// RCLCPP_INFO_STREAM(
|
||||
// getLogger(), "[" << getName() << "] Cancelling goal. req id: "
|
||||
// << rclcpp_action::to_string(req->get_goal_id()));
|
||||
// auto cancelresult = client_->async_cancel_goal(req);
|
||||
|
||||
// // wait actively
|
||||
// // rclcpp::spin_until_future_complete(getNode(), cancelresult);
|
||||
// //lastRequest_.reset();
|
||||
// return true;
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// RCLCPP_ERROR(
|
||||
// getLogger(), "%s [at %s]: not connected with actionserver, skipping cancel goal ...",
|
||||
// getName().c_str(), getNamespace().c_str());
|
||||
// return false;
|
||||
// }
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
std::shared_future<typename GoalHandle::SharedPtr> sendGoal(
|
||||
Goal & goal, typename SmaccActionResultSignal::WeakPtr resultCallback =
|
||||
typename SmaccActionResultSignal::WeakPtr())
|
||||
//ResultCallback resultCallback = nullptr) // bug related with the cancel action and the issue
|
||||
{
|
||||
// client_->sendGoal(goal, result_cb, active_cb, feedback_cb);
|
||||
// std::shared_future<typename GoalHandle::SharedPtr>
|
||||
|
||||
SendGoalOptions options;
|
||||
|
||||
// GoalResponseCallback
|
||||
// options.goal_response_callback;
|
||||
|
||||
/// Function called whenever feedback is received for the goal.
|
||||
// FeedbackCallback
|
||||
options.feedback_callback = feedback_cb;
|
||||
|
||||
/// Function called when the result for the goal is received.
|
||||
// ResultCallback result_callback;
|
||||
// options.result_callback = result_cb;
|
||||
|
||||
options.result_callback =
|
||||
[this, resultCallback](
|
||||
const typename rclcpp_action::ClientGoalHandle<ActionType>::WrappedResult & result)
|
||||
{
|
||||
// TODO(#1652): a work around until rcl_action interface is updated
|
||||
// if goal ids are not matched, the older goa call this callback so ignore the result
|
||||
// if matched, it must be processed (including aborted)
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Action result callback, getting shared future");
|
||||
// auto goalHandle = result->get();
|
||||
// goalHandle_ = lastRequest_->get();
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Action client Result goal id: "
|
||||
<< rclcpp_action::to_string(result.goal_id));
|
||||
|
||||
// if (goalHandle_->get_goal_id() == result.goal_id)
|
||||
// {
|
||||
// // goal_result_available_ = true;
|
||||
// // result_ = result;
|
||||
// RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Result CB Goal id matches with last request");
|
||||
|
||||
auto resultCallbackPtr = resultCallback.lock();
|
||||
|
||||
if (resultCallbackPtr != nullptr)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Result CB calling user callback:"
|
||||
<< demangleSymbol(typeid(*resultCallbackPtr).name()));
|
||||
(*resultCallbackPtr)(result);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Result CB calling default callback");
|
||||
this->onResult(result);
|
||||
}
|
||||
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// RCLCPP_ERROR_STREAM(getLogger(), "[" << getName() << "] Result CB Goal id DOES NOT match with last request. Skipping, incorrect behavior.");
|
||||
// }
|
||||
};
|
||||
|
||||
// if (lastRequest_ && lastRequest_->valid())
|
||||
// {
|
||||
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": checking previous request is really finished.");
|
||||
// auto res = this->lastRequest_->get();
|
||||
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": okay");
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": no previous request.");
|
||||
// }
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] client ready clients: "
|
||||
<< this->client_->get_number_of_ready_clients());
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << getName() << "] Waiting it is ready? " << client_->action_server_is_ready());
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), getName() << ": async send goal.");
|
||||
auto lastRequest = this->client_->async_send_goal(goal, options);
|
||||
this->lastRequest_ = lastRequest;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << getName() << "] Action request "
|
||||
// << rclcpp_action::to_string(this->goalHandle_->get_goal_id()) <<". Goal sent to " << this->action_endpoint_
|
||||
// << "\": " << std::endl
|
||||
// << goal
|
||||
);
|
||||
|
||||
// if (client_->isServerConnected())
|
||||
// {
|
||||
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": Goal sent:" << goal);
|
||||
|
||||
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": Goal Id: " <<
|
||||
// rclcpp_action::to_string(lastRequest_->get()->get_goal_id()));
|
||||
|
||||
// for (auto& gh: this->goal_handles_)
|
||||
// {
|
||||
|
||||
// }
|
||||
|
||||
// RCLCPP_INFO_STREAM(getLogger(), getName() << ": spinning until completed");
|
||||
// if (rclcpp::spin_until_future_complete(this->getNode(), lastRequest_, std::chrono::seconds(2))
|
||||
// !=rclcpp::executor::FutureReturnCode::SUCCESS)
|
||||
// {
|
||||
// throw std::runtime_error("send_goal failed");
|
||||
// }
|
||||
|
||||
// goalHandle_ = lastRequest_->get();
|
||||
// if (!goalHandle_) {
|
||||
// throw std::runtime_error("Goal was rejected by the action server");
|
||||
// }
|
||||
|
||||
// }
|
||||
// else
|
||||
// {
|
||||
// RCLCPP_ERROR(getLogger(),"%s [at %s]: not connected with actionserver, skipping goal request
|
||||
// ...", getName().c_str(), getNamespace().c_str());
|
||||
// //client_->waitForServer();
|
||||
// }
|
||||
|
||||
return lastRequest;
|
||||
}
|
||||
|
||||
protected:
|
||||
typename ActionClient::SharedPtr client_;
|
||||
|
||||
void onFeedback(
|
||||
typename GoalHandle::SharedPtr /*goalhandle*/,
|
||||
const std::shared_ptr<const Feedback> feedback_msg)
|
||||
{
|
||||
postFeedbackEvent(*feedback_msg);
|
||||
}
|
||||
|
||||
void onResult(const WrappedResult & result_msg)
|
||||
{
|
||||
// auto *actionResultEvent = new EvActionResult<TDerived>();
|
||||
// actionResultEvent->client = this;
|
||||
// actionResultEvent->resultMessage = *result_msg;
|
||||
|
||||
// const auto &resultType = this->getState();
|
||||
const auto & resultType = result_msg.code;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] response result ["
|
||||
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
|
||||
|
||||
if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] request result: Success", this->getName().c_str());
|
||||
onSucceeded_(result_msg);
|
||||
postSuccessEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::ABORTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] request result: Aborted", this->getName().c_str());
|
||||
onAborted_(result_msg);
|
||||
postAbortedEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::CANCELED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] request result: Cancelled", this->getName().c_str());
|
||||
onCancelled_(result_msg);
|
||||
postCancelledEvent(result_msg);
|
||||
}
|
||||
/*
|
||||
else if (resultType == actionlib::SimpleClientGoalState::REJECTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(),"[%s] request result: Rejected", this->getName().c_str());
|
||||
onRejected_(result_msg);
|
||||
postRejectedEvent(result_msg);
|
||||
}
|
||||
else if (resultType == actionlib::SimpleClientGoalState::PREEMPTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(),"[%s] request result: Preempted", this->getName().c_str());
|
||||
onPreempted_(result_msg);
|
||||
postPreemptedEvent(result_msg);
|
||||
}*/
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] request result: NOT HANDLED TYPE: %d", this->getName().c_str(),
|
||||
(int)resultType);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace client_bases
|
||||
|
||||
} // namespace smacc2
|
||||
82
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_publisher_client.hpp
vendored
Normal file
82
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_publisher_client.hpp
vendored
Normal file
@@ -0,0 +1,82 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
class SmaccPublisherClient : public smacc2::ISmaccClient
|
||||
{
|
||||
public:
|
||||
std::optional<std::string> topicName;
|
||||
std::optional<int> queueSize;
|
||||
std::optional<rmw_qos_durability_policy_t> durability;
|
||||
std::optional<rmw_qos_reliability_policy_t> reliability;
|
||||
|
||||
SmaccPublisherClient();
|
||||
virtual ~SmaccPublisherClient();
|
||||
|
||||
template <typename MessageType>
|
||||
void configure(std::string topicName)
|
||||
{
|
||||
this->topicName = topicName;
|
||||
if (!initialized_)
|
||||
{
|
||||
if (!this->topicName)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "topic publisher with no topic name set. Skipping advertising.");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!queueSize) queueSize = 1;
|
||||
if (!durability) durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT;
|
||||
if (!reliability) reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
rclcpp::SensorDataQoS qos;
|
||||
qos.keep_last(*queueSize);
|
||||
qos.durability(*durability);
|
||||
qos.reliability(*reliability);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Client Publisher to topic: " << topicName);
|
||||
pub_ = getNode()->create_publisher<MessageType>(*(this->topicName), qos);
|
||||
|
||||
this->initialized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename MessageType>
|
||||
void publish(const MessageType & msg)
|
||||
{
|
||||
//pub_->publish(msg);
|
||||
std::dynamic_pointer_cast<rclcpp::Publisher<MessageType>>(pub_)->publish(msg);
|
||||
}
|
||||
|
||||
rclcpp::PublisherBase::SharedPtr pub_;
|
||||
|
||||
private:
|
||||
bool initialized_;
|
||||
};
|
||||
} // namespace client_bases
|
||||
} // namespace smacc2
|
||||
61
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_ros_launch_client.hpp
vendored
Normal file
61
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_ros_launch_client.hpp
vendored
Normal file
@@ -0,0 +1,61 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
#include <rclcpp_action/client.hpp>
|
||||
#include <thread>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
class ClRosLaunch : public ISmaccClient
|
||||
{
|
||||
public:
|
||||
ClRosLaunch(std::string packageName, std::string launchFilename);
|
||||
|
||||
virtual ~ClRosLaunch();
|
||||
|
||||
void launch();
|
||||
|
||||
void stop();
|
||||
|
||||
static std::future<std::string> executeRosLaunch(
|
||||
std::string packageName, std::string launchFilename, std::function<bool()> cancelCondition);
|
||||
|
||||
std::string packageName_;
|
||||
|
||||
std::string launchFileName_;
|
||||
|
||||
protected:
|
||||
std::future<std::string> result_;
|
||||
|
||||
typedef std::function<void> cancelCallback;
|
||||
|
||||
static std::map<std::future<void>, cancelCallback> detached_futures_;
|
||||
|
||||
std::atomic<bool> cancellationToken_ = ATOMIC_VAR_INIT(false);
|
||||
};
|
||||
} // namespace client_bases
|
||||
} // namespace smacc2
|
||||
80
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_ros_launch_client_2.hpp
vendored
Normal file
80
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_ros_launch_client_2.hpp
vendored
Normal file
@@ -0,0 +1,80 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
#include <rclcpp_action/client.hpp>
|
||||
#include <thread>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
struct ProcessInfo
|
||||
{
|
||||
pid_t pid; // PID del proceso hijo
|
||||
FILE * pipe; // Pipe para la salida del proceso hijo
|
||||
};
|
||||
|
||||
ProcessInfo runProcess(const char * command);
|
||||
void killGrandchildren(pid_t originalPid);
|
||||
void killProcessesRecursive(pid_t pid);
|
||||
|
||||
class ClRosLaunch2 : public ISmaccClient
|
||||
{
|
||||
public:
|
||||
ClRosLaunch2();
|
||||
|
||||
ClRosLaunch2(std::string packageName, std::string launchFilename);
|
||||
|
||||
virtual ~ClRosLaunch2();
|
||||
|
||||
void launch();
|
||||
|
||||
void stop();
|
||||
|
||||
static std::future<std::string> executeRosLaunch(
|
||||
std::string packageName, std::string launchFilename, std::function<bool()> cancelCondition,
|
||||
ClRosLaunch2 * client = nullptr);
|
||||
|
||||
// static std::string executeRosLaunch(
|
||||
// std::string packageName, std::string launchFilename, std::function<bool()> cancelCondition);
|
||||
|
||||
std::string packageName_;
|
||||
|
||||
std::string launchFileName_;
|
||||
|
||||
pid_t launchPid_;
|
||||
|
||||
protected:
|
||||
// std::future<std::string> result_;
|
||||
std::future<std::string> result_;
|
||||
|
||||
typedef std::function<void> cancelCallback;
|
||||
|
||||
static std::map<std::future<void>, cancelCallback> detached_futures_;
|
||||
|
||||
std::atomic<bool> cancellationToken_ = ATOMIC_VAR_INIT(false);
|
||||
};
|
||||
} // namespace client_bases
|
||||
} // namespace smacc2
|
||||
72
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_client.hpp
vendored
Normal file
72
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_client.hpp
vendored
Normal file
@@ -0,0 +1,72 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
template <typename ServiceType>
|
||||
class SmaccServiceClient : public smacc2::ISmaccClient
|
||||
{
|
||||
public:
|
||||
std::optional<std::string> serviceName_;
|
||||
|
||||
SmaccServiceClient(std::string serviceName) : serviceName_(serviceName) { initialized_ = false; }
|
||||
|
||||
SmaccServiceClient() { initialized_ = false; }
|
||||
|
||||
void onInitialize() override
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
if (!serviceName_)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "service client with no service name set. Skipping.");
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
|
||||
this->initialized_ = true;
|
||||
client_ = getNode()->create_client<ServiceType>(*serviceName_);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_ptr<typename ServiceType::Response> call(
|
||||
std::shared_ptr<typename ServiceType::Request> & request)
|
||||
{
|
||||
auto result = client_->async_send_request(request);
|
||||
//rclcpp::spin_until_future_complete(getNode(), result);
|
||||
return result.get();
|
||||
}
|
||||
|
||||
protected:
|
||||
//rclcpp::NodeHandle nh_;
|
||||
std::shared_ptr<rclcpp::Client<ServiceType>> client_;
|
||||
bool initialized_;
|
||||
};
|
||||
} // namespace client_bases
|
||||
} // namespace smacc2
|
||||
100
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_server_client.hpp
vendored
Normal file
100
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_service_server_client.hpp
vendored
Normal file
@@ -0,0 +1,100 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
template <typename TService>
|
||||
class SmaccServiceServerClient : public smacc2::ISmaccClient
|
||||
{
|
||||
using TServiceRequest = typename TService::Request;
|
||||
using TServiceResponse = typename TService::Response;
|
||||
|
||||
public:
|
||||
std::optional<std::string> serviceName_;
|
||||
SmaccServiceServerClient() { initialized_ = false; }
|
||||
SmaccServiceServerClient(std::string service_name)
|
||||
{
|
||||
serviceName_ = service_name;
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
virtual ~SmaccServiceServerClient() {}
|
||||
|
||||
smacc2::SmaccSignal<void(
|
||||
const std::shared_ptr<typename TService::Request>,
|
||||
std::shared_ptr<typename TService::Response>)>
|
||||
onServiceRequestReceived_;
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onServiceRequestReceived(
|
||||
void (T::*callback)(
|
||||
const std::shared_ptr<typename TService::Request>,
|
||||
std::shared_ptr<typename TService::Response>),
|
||||
T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(
|
||||
onServiceRequestReceived_, callback, object);
|
||||
}
|
||||
|
||||
void onInitialize() override
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
if (!serviceName_)
|
||||
{
|
||||
RCLCPP_ERROR_STREAM(
|
||||
getLogger(),
|
||||
"[" << this->getName() << "] service server with no service name set. Skipping.");
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Client Service: " << *serviceName_);
|
||||
|
||||
server_ = getNode()->create_service<TService>(
|
||||
*serviceName_, std::bind(
|
||||
&SmaccServiceServerClient<TService>::serviceCallback, this,
|
||||
std::placeholders::_1, std::placeholders::_2));
|
||||
|
||||
this->initialized_ = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
void serviceCallback(
|
||||
const std::shared_ptr<typename TService::Request> req,
|
||||
std::shared_ptr<typename TService::Response> res)
|
||||
{
|
||||
onServiceRequestReceived_(req, res);
|
||||
}
|
||||
typename rclcpp::Service<TService>::SharedPtr server_;
|
||||
bool initialized_;
|
||||
};
|
||||
} // namespace client_bases
|
||||
} // namespace smacc2
|
||||
141
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_subscriber_client.hpp
vendored
Normal file
141
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_bases/smacc_subscriber_client.hpp
vendored
Normal file
@@ -0,0 +1,141 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <optional>
|
||||
#include <smacc2/smacc_client.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_bases
|
||||
{
|
||||
using namespace smacc2::default_events;
|
||||
|
||||
// This client class warps a ros subscriber and publishes two kind of
|
||||
// smacc events: EvTopicMessage (always a ros topic message is received)
|
||||
// and EvTopicInitialMessage (only once)
|
||||
template <typename MessageType>
|
||||
class SmaccSubscriberClient : public smacc2::ISmaccClient
|
||||
{
|
||||
public:
|
||||
std::optional<std::string> topicName;
|
||||
std::optional<int> queueSize;
|
||||
|
||||
typedef MessageType TMessageType;
|
||||
|
||||
SmaccSubscriberClient() { initialized_ = false; }
|
||||
|
||||
SmaccSubscriberClient(std::string topicname) { topicName = topicname; }
|
||||
|
||||
virtual ~SmaccSubscriberClient() {}
|
||||
|
||||
smacc2::SmaccSignal<void(const MessageType &)> onFirstMessageReceived_;
|
||||
smacc2::SmaccSignal<void(const MessageType &)> onMessageReceived_;
|
||||
|
||||
std::function<void(const MessageType &)> postMessageEvent;
|
||||
std::function<void(const MessageType &)> postInitialMessageEvent;
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onMessageReceived(
|
||||
void (T::*callback)(const MessageType &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onMessageReceived_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onFirstMessageReceived(
|
||||
void (T::*callback)(const MessageType &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(
|
||||
onFirstMessageReceived_, callback, object);
|
||||
}
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onOrthogonalAllocation()
|
||||
{
|
||||
// ros topic message received smacc event callback
|
||||
this->postMessageEvent = [this](auto msg)
|
||||
{
|
||||
auto event = new EvTopicMessage<TSourceObject, TOrthogonal>();
|
||||
event->msgData = msg;
|
||||
this->postEvent(event);
|
||||
};
|
||||
|
||||
// initial ros topic message received smacc event callback
|
||||
this->postInitialMessageEvent = [this](auto msg)
|
||||
{
|
||||
auto event = new EvTopicInitialMessage<TSourceObject, TOrthogonal>();
|
||||
event->msgData = msg;
|
||||
this->postEvent(event);
|
||||
};
|
||||
}
|
||||
|
||||
protected:
|
||||
void onInitialize() override
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
firstMessage_ = true;
|
||||
|
||||
if (!queueSize) queueSize = 1;
|
||||
|
||||
if (!topicName)
|
||||
{
|
||||
RCLCPP_ERROR(getLogger(), "topic client with no topic name set. Skipping subscribing");
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Subscribing to topic: " << *topicName);
|
||||
|
||||
rclcpp::SensorDataQoS qos;
|
||||
if (queueSize) qos.keep_last(*queueSize);
|
||||
|
||||
std::function<void(typename MessageType::SharedPtr)> fn = [this](auto msg)
|
||||
{ this->messageCallback(*msg); };
|
||||
sub_ = getNode()->create_subscription<MessageType>(*topicName, qos, fn);
|
||||
this->initialized_ = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
typename rclcpp::Subscription<MessageType>::SharedPtr sub_;
|
||||
bool firstMessage_;
|
||||
bool initialized_;
|
||||
|
||||
void messageCallback(const MessageType & msg)
|
||||
{
|
||||
if (firstMessage_)
|
||||
{
|
||||
postInitialMessageEvent(msg);
|
||||
onFirstMessageReceived_(msg);
|
||||
firstMessage_ = false;
|
||||
}
|
||||
|
||||
postMessageEvent(msg);
|
||||
onMessageReceived_(msg);
|
||||
}
|
||||
};
|
||||
} // namespace client_bases
|
||||
} // namespace smacc2
|
||||
105
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_call_service.hpp
vendored
Normal file
105
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_call_service.hpp
vendored
Normal file
@@ -0,0 +1,105 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
#include <smacc2/impl/smacc_asynchronous_client_behavior_impl.hpp>
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
using namespace std::chrono_literals;
|
||||
template <typename ServiceType>
|
||||
class CbServiceCall : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
public:
|
||||
CbServiceCall(const char * serviceName) : serviceName_(serviceName)
|
||||
{
|
||||
request_ = std::make_shared<typename ServiceType::Request>();
|
||||
pollRate_ = 100ms;
|
||||
}
|
||||
|
||||
CbServiceCall(
|
||||
const char * serviceName, std::shared_ptr<typename ServiceType::Request> request,
|
||||
std::chrono::milliseconds pollRate = 100ms)
|
||||
: serviceName_(serviceName), request_(request), result_(nullptr), pollRate_(pollRate)
|
||||
{
|
||||
}
|
||||
|
||||
void onEntry() override
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] creating ros service client: " << serviceName_);
|
||||
|
||||
client_ = getNode()->create_client<ServiceType>(serviceName_);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] making service request to " << serviceName_);
|
||||
|
||||
resultFuture_ = client_->async_send_request(request_).future.share();
|
||||
|
||||
std::future_status status = resultFuture_.wait_for(0s);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "thread state: " << (int)status << " ok " << rclcpp::ok() << " shutdown "
|
||||
<< this->isShutdownRequested() << "");
|
||||
|
||||
while (status != std::future_status::ready && rclcpp::ok() && !this->isShutdownRequested())
|
||||
{
|
||||
RCLCPP_INFO_STREAM_THROTTLE(
|
||||
getLogger(), *getNode()->get_clock(), 1000,
|
||||
"[" << this->getName() << "] waiting response ");
|
||||
rclcpp::sleep_for(pollRate_);
|
||||
status = resultFuture_.wait_for(0s);
|
||||
}
|
||||
|
||||
if (status == std::future_status::ready)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
|
||||
result_ = resultFuture_.get();
|
||||
onServiceResponse(result_);
|
||||
this->postSuccessEvent();
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response not received ");
|
||||
this->postFailureEvent();
|
||||
}
|
||||
}
|
||||
|
||||
std::shared_future<std::shared_ptr<typename ServiceType::Response>> resultFuture_;
|
||||
|
||||
typename std::shared_ptr<typename ServiceType::Response> result_;
|
||||
std::chrono::milliseconds pollRate_;
|
||||
|
||||
protected:
|
||||
//rclcpp::NodeHandle nh_;
|
||||
std::shared_ptr<rclcpp::Client<ServiceType>> client_;
|
||||
std::string serviceName_;
|
||||
std::shared_ptr<typename ServiceType::Request> request_;
|
||||
|
||||
virtual void onServiceResponse(std::shared_ptr<typename ServiceType::Response> /*result*/)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] response received ");
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
70
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_ros_launch.hpp
vendored
Normal file
70
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_ros_launch.hpp
vendored
Normal file
@@ -0,0 +1,70 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/client_bases/smacc_ros_launch_client.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
enum class RosLaunchMode
|
||||
{
|
||||
LAUNCH_DETTACHED,
|
||||
LAUNCH_CLIENT_BEHAVIOR_LIFETIME
|
||||
};
|
||||
|
||||
class CbRosLaunch : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
private:
|
||||
static std::vector<std::future<std::string>> detached_futures_;
|
||||
|
||||
public:
|
||||
CbRosLaunch();
|
||||
|
||||
CbRosLaunch(std::string package, std::string launchfile, RosLaunchMode);
|
||||
|
||||
// CbRosLaunch(std::string packageName, std::string launchFileName);
|
||||
|
||||
virtual ~CbRosLaunch();
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onOrthogonalAllocation()
|
||||
{
|
||||
smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
|
||||
}
|
||||
|
||||
void onEntry() override;
|
||||
|
||||
std::optional<std::string> packageName_;
|
||||
std::optional<std::string> launchFileName_;
|
||||
|
||||
RosLaunchMode launchMode_;
|
||||
|
||||
protected:
|
||||
std::string result_;
|
||||
|
||||
smacc2::client_bases::ClRosLaunch * client_;
|
||||
|
||||
std::future<std::string> future_;
|
||||
};
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
70
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_ros_launch_2.hpp
vendored
Normal file
70
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_ros_launch_2.hpp
vendored
Normal file
@@ -0,0 +1,70 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/client_bases/smacc_ros_launch_client_2.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
enum class RosLaunchMode
|
||||
{
|
||||
LAUNCH_DETTACHED,
|
||||
LAUNCH_CLIENT_BEHAVIOR_LIFETIME
|
||||
};
|
||||
|
||||
class CbRosLaunch2 : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
private:
|
||||
static std::vector<std::future<std::string>> detached_futures_;
|
||||
|
||||
public:
|
||||
CbRosLaunch2();
|
||||
|
||||
CbRosLaunch2(std::string package, std::string launchfile, RosLaunchMode);
|
||||
|
||||
// CbRosLaunch2(std::string packageName, std::string launchFileName);
|
||||
|
||||
virtual ~CbRosLaunch2();
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onOrthogonalAllocation()
|
||||
{
|
||||
smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
|
||||
}
|
||||
|
||||
void onEntry() override;
|
||||
|
||||
std::optional<std::string> packageName_;
|
||||
std::optional<std::string> launchFileName_;
|
||||
|
||||
RosLaunchMode launchMode_;
|
||||
|
||||
protected:
|
||||
std::future<std::string> result_;
|
||||
|
||||
smacc2::client_bases::ClRosLaunch2 * client_;
|
||||
|
||||
std::future<std::string> future_;
|
||||
};
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
62
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_ros_stop_2.hpp
vendored
Normal file
62
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_ros_stop_2.hpp
vendored
Normal file
@@ -0,0 +1,62 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/client_bases/smacc_ros_launch_client_2.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
class CbRosStop2 : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
private:
|
||||
static std::vector<std::future<std::string>> detached_futures_;
|
||||
|
||||
public:
|
||||
CbRosStop2();
|
||||
|
||||
CbRosStop2(pid_t launchPid);
|
||||
|
||||
// CbRosStop2(std::string packageName, std::string launchFileName);
|
||||
|
||||
virtual ~CbRosStop2();
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onOrthogonalAllocation()
|
||||
{
|
||||
smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
|
||||
}
|
||||
|
||||
void onEntry() override;
|
||||
|
||||
std::optional<std::string> packageName_;
|
||||
std::optional<std::string> launchFileName_;
|
||||
|
||||
protected:
|
||||
std::future<std::string> result_;
|
||||
|
||||
smacc2::client_bases::ClRosLaunch2 * client_;
|
||||
|
||||
std::future<std::string> future_;
|
||||
};
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
92
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_sequence.hpp
vendored
Normal file
92
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_sequence.hpp
vendored
Normal file
@@ -0,0 +1,92 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
class CbSequence : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
public:
|
||||
CbSequence();
|
||||
|
||||
// template <typename TOrthogonal, typename TBehavior>
|
||||
// static void configure_orthogonal_runtime(
|
||||
// std::function<void(TBehavior & bh, MostDerived &)> initializationFunction)
|
||||
// {
|
||||
// configure_orthogonal_internal<TOrthogonal, TBehavior>([=](ISmaccState * state) {
|
||||
// // auto bh = std::make_shared<TBehavior>(args...);
|
||||
// // auto bh = state->configure<TOrthogonal, TBehavior>();
|
||||
// initializationFunction(*bh, *(static_cast<MostDerived *>(state)));
|
||||
// });
|
||||
// }
|
||||
|
||||
void onEntry() override;
|
||||
|
||||
// template <typename TOrthogonal, typename TSourceObject>
|
||||
// void onOrthogonalAllocation()
|
||||
// {
|
||||
// smacc2::SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
|
||||
// }
|
||||
|
||||
void onExit() override { sequenceNodes_.clear(); }
|
||||
|
||||
template <typename TOrthogonal, typename TBehavior, typename... Args>
|
||||
CbSequence * then(Args &&... args)
|
||||
{
|
||||
std::function<std::shared_ptr<smacc2::SmaccAsyncClientBehavior>()> delayedCBFactoryFn =
|
||||
[this, args...]()
|
||||
{
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[CbSequence] then creating new sub behavior %s ",
|
||||
demangleSymbol<TBehavior>().c_str());
|
||||
auto createdBh = std::shared_ptr<TBehavior>(new TBehavior(args...));
|
||||
|
||||
this->getCurrentState()->getOrthogonal<TOrthogonal>()->addClientBehavior(createdBh);
|
||||
createdBh->template onOrthogonalAllocation<TOrthogonal, TBehavior>();
|
||||
|
||||
return createdBh;
|
||||
};
|
||||
|
||||
sequenceNodes_.push_back(delayedCBFactoryFn);
|
||||
|
||||
return this;
|
||||
}
|
||||
|
||||
private:
|
||||
void onSubNodeSuccess();
|
||||
void onSubNodeAbort();
|
||||
void recursiveConsumeNext();
|
||||
|
||||
std::list<std::function<std::shared_ptr<smacc2::SmaccAsyncClientBehavior>()>> sequenceNodes_;
|
||||
boost::signals2::connection conn_;
|
||||
boost::signals2::connection conn2_;
|
||||
|
||||
std::shared_ptr<smacc2::SmaccAsyncClientBehavior> bh_;
|
||||
std::atomic<int> consume_{0};
|
||||
};
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
@@ -0,0 +1,38 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#pragma once
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
template <typename TService>
|
||||
class CbServiceServerCallbackBase : public smacc2::SmaccClientBehavior
|
||||
{
|
||||
public:
|
||||
virtual void onEntry() override
|
||||
{
|
||||
this->requiresClient(attachedClient_);
|
||||
attachedClient_->onServiceRequestReceived(
|
||||
&CbServiceServerCallbackBase::onServiceRequestReceived, this);
|
||||
}
|
||||
|
||||
virtual void onServiceRequestReceived(
|
||||
const std::shared_ptr<typename TService::Request> req,
|
||||
std::shared_ptr<typename TService::Response> res) = 0;
|
||||
|
||||
protected:
|
||||
smacc2::client_bases::SmaccServiceServerClient<TService> * attachedClient_ = nullptr;
|
||||
};
|
||||
} // namespace smacc2
|
||||
53
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_sleep_for.hpp
vendored
Normal file
53
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_sleep_for.hpp
vendored
Normal file
@@ -0,0 +1,53 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/duration.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2::client_behaviors
|
||||
{
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
class CbSleepFor : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
public:
|
||||
CbSleepFor(rclcpp::Duration sleeptime) : sleeptime_(sleeptime) {}
|
||||
|
||||
void onEntry() override
|
||||
{
|
||||
auto starttime = getNode()->now();
|
||||
while (!this->isShutdownRequested() && (getNode()->now() - starttime) < sleeptime_)
|
||||
{
|
||||
rclcpp::sleep_for(10ms);
|
||||
}
|
||||
|
||||
//rclcpp::sleep_for(std::chrono::nanoseconds(sleeptime_.nanoseconds()));
|
||||
this->postSuccessEvent();
|
||||
}
|
||||
|
||||
void onExit() override {}
|
||||
|
||||
private:
|
||||
rclcpp::Duration sleeptime_;
|
||||
};
|
||||
} // namespace smacc2::client_behaviors
|
||||
@@ -0,0 +1,43 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
#include <smacc2/smacc_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
template <typename TMsg>
|
||||
class CbSubscriptionCallbackBase : public smacc2::SmaccClientBehavior
|
||||
{
|
||||
public:
|
||||
void onEntry() override
|
||||
{
|
||||
this->requiresClient(attachedClient_);
|
||||
attachedClient_->onMessageReceived(&CbSubscriptionCallbackBase::onMessageReceived, this);
|
||||
}
|
||||
|
||||
virtual void onMessageReceived(const TMsg & msg) = 0;
|
||||
|
||||
protected:
|
||||
smacc2::client_bases::SmaccSubscriberClient<TMsg> * attachedClient_ = nullptr;
|
||||
};
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
53
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_action_server.hpp
vendored
Normal file
53
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_action_server.hpp
vendored
Normal file
@@ -0,0 +1,53 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/client_bases/smacc_action_client_base.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
using namespace smacc2::client_bases;
|
||||
|
||||
// waits the action server is available in the current orthogonal
|
||||
class CbWaitActionServer : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
public:
|
||||
CbWaitActionServer(std::chrono::milliseconds timeout);
|
||||
virtual ~CbWaitActionServer();
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onOrthogonalAllocation()
|
||||
{
|
||||
SmaccAsyncClientBehavior::onOrthogonalAllocation<TOrthogonal, TSourceObject>();
|
||||
this->requiresClient(client_);
|
||||
}
|
||||
|
||||
void onEntry() override;
|
||||
|
||||
private:
|
||||
ISmaccActionClient * client_;
|
||||
std::chrono::milliseconds timeout_;
|
||||
};
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
45
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_node.hpp
vendored
Normal file
45
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_node.hpp
vendored
Normal file
@@ -0,0 +1,45 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2::client_behaviors
|
||||
{
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// Asynchronous behavior that waits to a topic message to send EvCbSuccess event
|
||||
// a guard function can be set to use conditions on the contents
|
||||
class CbWaitNode : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
public:
|
||||
CbWaitNode(std::string nodeName);
|
||||
|
||||
void onEntry() override;
|
||||
|
||||
protected:
|
||||
std::string nodeName_;
|
||||
|
||||
rclcpp::Rate rate_;
|
||||
};
|
||||
} // namespace smacc2::client_behaviors
|
||||
47
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_topic.hpp
vendored
Normal file
47
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_topic.hpp
vendored
Normal file
@@ -0,0 +1,47 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2::client_behaviors
|
||||
{
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// Asynchronous behavior that waits to a topic message to send EvCbSuccess event
|
||||
// a guard function can be set to use conditions on the contents
|
||||
class CbWaitTopic : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
public:
|
||||
CbWaitTopic(std::string topicName);
|
||||
|
||||
virtual ~CbWaitTopic();
|
||||
|
||||
void onEntry() override;
|
||||
|
||||
protected:
|
||||
std::string topicName_;
|
||||
|
||||
rclcpp::Rate rate_;
|
||||
};
|
||||
} // namespace smacc2::client_behaviors
|
||||
86
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_topic_message.hpp
vendored
Normal file
86
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_behaviors/cb_wait_topic_message.hpp
vendored
Normal file
@@ -0,0 +1,86 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <functional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <smacc2/smacc_asynchronous_client_behavior.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_behaviors
|
||||
{
|
||||
using namespace std::chrono_literals;
|
||||
|
||||
// Asynchronous behavior that waits to a topic message to send EvCbSuccess event
|
||||
// a guard function can be set to use conditions on the contents
|
||||
template <typename TMessage>
|
||||
class CbWaitTopicMessage : public smacc2::SmaccAsyncClientBehavior
|
||||
{
|
||||
public:
|
||||
CbWaitTopicMessage(
|
||||
const char * topicname, std::function<bool(const TMessage &)> guardFunction = nullptr)
|
||||
{
|
||||
topicname_ = topicname;
|
||||
guardFn_ = guardFunction;
|
||||
}
|
||||
|
||||
virtual ~CbWaitTopicMessage() {}
|
||||
|
||||
void onEntry() override
|
||||
{
|
||||
rclcpp::SensorDataQoS qos;
|
||||
// qos.reliable();
|
||||
// rclcpp::SubscriptionOptions sub_option;
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[CbWaitTopicMessage] waiting message from topic: "
|
||||
<< topicname_ << "[" << demangledTypeName<TMessage>() << "]");
|
||||
|
||||
// sub_ = getNode()->create_subscription<TMessage>(
|
||||
// topicname_, qos,
|
||||
// std::bind(&CbWaitTopicMessage<TMessage>::onMessageReceived, this, std::placeholders::_1),
|
||||
// sub_option);
|
||||
|
||||
std::function<void(typename TMessage::SharedPtr)> fn = [this](auto msg)
|
||||
{ this->onMessageReceived(msg); };
|
||||
|
||||
auto nh = getNode();
|
||||
sub_ = nh->create_subscription<TMessage>(topicname_, qos, fn);
|
||||
}
|
||||
|
||||
void onMessageReceived(const typename TMessage::SharedPtr msg)
|
||||
{
|
||||
if (guardFn_ == nullptr || guardFn_(*msg))
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[CbWaitTopicMessage] message received.");
|
||||
success = true;
|
||||
this->postSuccessEvent();
|
||||
}
|
||||
}
|
||||
|
||||
protected:
|
||||
bool success = false;
|
||||
typename rclcpp::Subscription<TMessage>::SharedPtr sub_;
|
||||
std::function<bool(const TMessage &)> guardFn_;
|
||||
std::string topicname_;
|
||||
};
|
||||
} // namespace client_behaviors
|
||||
} // namespace smacc2
|
||||
286
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_action_client.hpp
vendored
Normal file
286
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_action_client.hpp
vendored
Normal file
@@ -0,0 +1,286 @@
|
||||
// Copyright 2024 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <smacc2/component.hpp>
|
||||
#include <smacc2/smacc_default_events.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
#include <smacc2/smacc_state_machine.hpp>
|
||||
|
||||
#include <chrono>
|
||||
#include <functional>
|
||||
#include <future>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <rclcpp_action/rclcpp_action.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_core_components
|
||||
{
|
||||
using namespace smacc2::default_events;
|
||||
|
||||
template <typename ActionType>
|
||||
class CpActionClient : public smacc2::ISmaccComponent
|
||||
{
|
||||
public:
|
||||
// Type aliases
|
||||
using ActionClient = rclcpp_action::Client<ActionType>;
|
||||
using Goal = typename ActionType::Goal;
|
||||
using Feedback = typename ActionType::Feedback;
|
||||
using Result = typename ActionType::Result;
|
||||
using GoalHandle = rclcpp_action::ClientGoalHandle<ActionType>;
|
||||
using WrappedResult = typename GoalHandle::WrappedResult;
|
||||
using SendGoalOptions = typename ActionClient::SendGoalOptions;
|
||||
using GoalResponseCallback =
|
||||
std::function<void(std::shared_future<typename GoalHandle::SharedPtr>)>;
|
||||
using FeedbackCallback = typename GoalHandle::FeedbackCallback;
|
||||
using ResultCallback = typename GoalHandle::ResultCallback;
|
||||
|
||||
// Configuration options
|
||||
std::optional<std::string> actionServerName;
|
||||
std::optional<std::chrono::milliseconds> serverTimeout;
|
||||
|
||||
// SMACC2 Signals for component communication
|
||||
smacc2::SmaccSignal<void(const WrappedResult &)> onActionSucceeded_;
|
||||
smacc2::SmaccSignal<void(const WrappedResult &)> onActionAborted_;
|
||||
smacc2::SmaccSignal<void(const WrappedResult &)> onActionCancelled_;
|
||||
smacc2::SmaccSignal<void(const Feedback &)> onActionFeedback_;
|
||||
smacc2::SmaccSignal<void()> onGoalAccepted_;
|
||||
smacc2::SmaccSignal<void()> onGoalRejected_;
|
||||
|
||||
// Event posting functions (set during orthogonal allocation)
|
||||
std::function<void(const WrappedResult &)> postSuccessEvent;
|
||||
std::function<void(const WrappedResult &)> postAbortedEvent;
|
||||
std::function<void(const WrappedResult &)> postCancelledEvent;
|
||||
std::function<void(const Feedback &)> postFeedbackEvent;
|
||||
|
||||
// Constructor
|
||||
CpActionClient() = default;
|
||||
|
||||
CpActionClient(const std::string & actionServerName) : actionServerName(actionServerName) {}
|
||||
|
||||
virtual ~CpActionClient() = default;
|
||||
|
||||
// Public API
|
||||
std::shared_future<typename GoalHandle::SharedPtr> sendGoal(
|
||||
Goal & goal, typename smacc2::SmaccSignal<void(const WrappedResult &)>::WeakPtr resultCallback =
|
||||
typename smacc2::SmaccSignal<void(const WrappedResult &)>::WeakPtr())
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(actionMutex_);
|
||||
|
||||
SendGoalOptions options;
|
||||
|
||||
// Set up feedback callback
|
||||
options.feedback_callback = feedbackCallback_;
|
||||
|
||||
// Set up result callback
|
||||
options.result_callback = [this, resultCallback](const WrappedResult & result)
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(actionMutex_);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Action result callback, goal id: "
|
||||
<< rclcpp_action::to_string(result.goal_id));
|
||||
|
||||
auto resultCallbackPtr = resultCallback.lock();
|
||||
if (resultCallbackPtr != nullptr)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Calling user result callback");
|
||||
(*resultCallbackPtr)(result);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Using default result handling");
|
||||
this->onResult(result);
|
||||
}
|
||||
};
|
||||
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Sending goal to action server");
|
||||
|
||||
auto goalFuture = client_->async_send_goal(goal, options);
|
||||
lastRequest_ = goalFuture;
|
||||
|
||||
return goalFuture;
|
||||
}
|
||||
|
||||
bool cancelGoal()
|
||||
{
|
||||
std::lock_guard<std::mutex> lock(actionMutex_);
|
||||
|
||||
if (lastRequest_ && lastRequest_->valid())
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling current goal");
|
||||
|
||||
auto cancelFuture = client_->async_cancel_all_goals();
|
||||
lastCancelResponse_ = cancelFuture;
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN_STREAM(getLogger(), "[" << this->getName() << "] No active goal to cancel");
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
bool isServerReady() const { return client_ && client_->action_server_is_ready(); }
|
||||
|
||||
void waitForServer()
|
||||
{
|
||||
if (client_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << this->getName() << "] Waiting for action server: " << *actionServerName);
|
||||
client_->wait_for_action_server();
|
||||
}
|
||||
}
|
||||
|
||||
// Component lifecycle
|
||||
void onInitialize() override
|
||||
{
|
||||
if (!actionServerName)
|
||||
{
|
||||
RCLCPP_ERROR_STREAM(getLogger(), "[" << this->getName() << "] Action server name not set!");
|
||||
return;
|
||||
}
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(),
|
||||
"[" << this->getName() << "] Initializing action client for: " << *actionServerName);
|
||||
|
||||
client_ = rclcpp_action::create_client<ActionType>(getNode(), *actionServerName);
|
||||
|
||||
// Set up feedback callback
|
||||
feedbackCallback_ = [this](auto goalHandle, auto feedback)
|
||||
{ this->onFeedback(goalHandle, feedback); };
|
||||
}
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onComponentInitialization()
|
||||
{
|
||||
// Set up event posting functions with proper template parameters
|
||||
postSuccessEvent = [this](const WrappedResult & result)
|
||||
{ this->postResultEvent<EvActionSucceeded<TSourceObject, TOrthogonal>>(result); };
|
||||
|
||||
postAbortedEvent = [this](const WrappedResult & result)
|
||||
{ this->postResultEvent<EvActionAborted<TSourceObject, TOrthogonal>>(result); };
|
||||
|
||||
postCancelledEvent = [this](const WrappedResult & result)
|
||||
{ this->postResultEvent<EvActionCancelled<TSourceObject, TOrthogonal>>(result); };
|
||||
|
||||
postFeedbackEvent = [this](const Feedback & feedback)
|
||||
{
|
||||
auto actionFeedbackEvent = new EvActionFeedback<Feedback, TOrthogonal>();
|
||||
actionFeedbackEvent->client = this;
|
||||
actionFeedbackEvent->feedbackMessage = feedback;
|
||||
this->postEvent(actionFeedbackEvent);
|
||||
RCLCPP_DEBUG(getLogger(), "[%s] FEEDBACK EVENT", this->getName().c_str());
|
||||
};
|
||||
}
|
||||
|
||||
// Signal connection methods
|
||||
template <typename T>
|
||||
boost::signals2::connection onSucceeded(void (T::*callback)(const WrappedResult &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onActionSucceeded_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onAborted(void (T::*callback)(const WrappedResult &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onActionAborted_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onCancelled(void (T::*callback)(const WrappedResult &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onActionCancelled_, callback, object);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onFeedback(void (T::*callback)(const Feedback &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onActionFeedback_, callback, object);
|
||||
}
|
||||
|
||||
// Access to underlying client for advanced usage
|
||||
std::shared_ptr<ActionClient> getActionClient() const { return client_; }
|
||||
|
||||
private:
|
||||
std::shared_ptr<ActionClient> client_;
|
||||
std::optional<std::shared_future<typename GoalHandle::SharedPtr>> lastRequest_;
|
||||
std::optional<
|
||||
std::shared_future<typename rclcpp_action::Client<ActionType>::CancelResponse::SharedPtr>>
|
||||
lastCancelResponse_;
|
||||
FeedbackCallback feedbackCallback_;
|
||||
std::mutex actionMutex_;
|
||||
|
||||
void onFeedback(
|
||||
typename GoalHandle::SharedPtr /* goalHandle */,
|
||||
const std::shared_ptr<const Feedback> feedback_msg)
|
||||
{
|
||||
onActionFeedback_(*feedback_msg);
|
||||
postFeedbackEvent(*feedback_msg);
|
||||
}
|
||||
|
||||
void onResult(const WrappedResult & result_msg)
|
||||
{
|
||||
const auto & resultType = result_msg.code;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Action result ["
|
||||
<< rclcpp_action::to_string(result_msg.goal_id) << "]: " << (int)resultType);
|
||||
|
||||
if (resultType == rclcpp_action::ResultCode::SUCCEEDED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] Action result: Success", this->getName().c_str());
|
||||
onActionSucceeded_(result_msg);
|
||||
postSuccessEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::ABORTED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] Action result: Aborted", this->getName().c_str());
|
||||
onActionAborted_(result_msg);
|
||||
postAbortedEvent(result_msg);
|
||||
}
|
||||
else if (resultType == rclcpp_action::ResultCode::CANCELED)
|
||||
{
|
||||
RCLCPP_INFO(getLogger(), "[%s] Action result: Cancelled", this->getName().c_str());
|
||||
onActionCancelled_(result_msg);
|
||||
postCancelledEvent(result_msg);
|
||||
}
|
||||
else
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
getLogger(), "[%s] Action result: Unhandled type: %d", this->getName().c_str(),
|
||||
(int)resultType);
|
||||
}
|
||||
}
|
||||
|
||||
template <typename EvType>
|
||||
void postResultEvent(const WrappedResult & /* result */)
|
||||
{
|
||||
auto * ev = new EvType();
|
||||
RCLCPP_INFO(
|
||||
getLogger(), "[%s] Posting event: %s", this->getName().c_str(),
|
||||
smacc2::demangleSymbol(typeid(ev).name()).c_str());
|
||||
this->postEvent(ev);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace client_core_components
|
||||
} // namespace smacc2
|
||||
121
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_ros2_timer.hpp
vendored
Normal file
121
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_ros2_timer.hpp
vendored
Normal file
@@ -0,0 +1,121 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <chrono>
|
||||
#include <optional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <smacc2/common.hpp>
|
||||
#include <smacc2/component.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_core_components
|
||||
{
|
||||
using namespace smacc2::default_events;
|
||||
|
||||
class CpRos2Timer : public smacc2::ISmaccComponent
|
||||
{
|
||||
public:
|
||||
CpRos2Timer(rclcpp::Duration duration, bool oneshot = false)
|
||||
: duration_(duration), oneshot_(oneshot), initialized_(false)
|
||||
{
|
||||
}
|
||||
|
||||
virtual ~CpRos2Timer()
|
||||
{
|
||||
if (timer_)
|
||||
{
|
||||
timer_->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
void onInitialize() override
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Initializing ROS2 Timer with duration: "
|
||||
<< duration_.seconds() << "s, oneshot: " << (oneshot_ ? "true" : "false"));
|
||||
|
||||
auto clock = this->getNode()->get_clock();
|
||||
timer_ = rclcpp::create_timer(
|
||||
this->getNode(), clock, duration_, std::bind(&CpRos2Timer::timerCallback, this));
|
||||
|
||||
this->initialized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
smacc2::SmaccSignal<void()> onTimerTick_;
|
||||
|
||||
void startTimer()
|
||||
{
|
||||
if (timer_ && timer_->is_canceled())
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Starting timer");
|
||||
timer_->reset();
|
||||
}
|
||||
}
|
||||
|
||||
void stopTimer()
|
||||
{
|
||||
if (timer_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Stopping timer");
|
||||
timer_->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
void cancelTimer()
|
||||
{
|
||||
if (timer_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(getLogger(), "[" << this->getName() << "] Cancelling timer");
|
||||
timer_->cancel();
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
boost::signals2::connection onTimerTick(void (T::*callback)(), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onTimerTick_, callback, object);
|
||||
}
|
||||
|
||||
private:
|
||||
void timerCallback()
|
||||
{
|
||||
RCLCPP_DEBUG_STREAM(getLogger(), "[" << this->getName() << "] Timer tick");
|
||||
|
||||
if (!onTimerTick_.empty())
|
||||
{
|
||||
onTimerTick_();
|
||||
}
|
||||
|
||||
if (oneshot_)
|
||||
{
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Oneshot timer completed, cancelling");
|
||||
timer_->cancel();
|
||||
}
|
||||
}
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::Duration duration_;
|
||||
bool oneshot_;
|
||||
bool initialized_;
|
||||
};
|
||||
|
||||
} // namespace client_core_components
|
||||
} // namespace smacc2
|
||||
87
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_publisher.hpp
vendored
Normal file
87
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_publisher.hpp
vendored
Normal file
@@ -0,0 +1,87 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
#include <optional>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <smacc2/common.hpp>
|
||||
#include <smacc2/component.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_core_components
|
||||
{
|
||||
using namespace smacc2::default_events;
|
||||
|
||||
template <typename MessageType>
|
||||
class CpTopicPublisher : public smacc2::ISmaccComponent
|
||||
{
|
||||
public:
|
||||
std::optional<int> queueSize;
|
||||
std::optional<rmw_qos_durability_policy_t> durability;
|
||||
std::optional<rmw_qos_reliability_policy_t> reliability;
|
||||
|
||||
typedef MessageType TMessageType;
|
||||
|
||||
CpTopicPublisher(std::string topicname)
|
||||
{
|
||||
this->topicName_ = topicname;
|
||||
initialized_ = false;
|
||||
}
|
||||
|
||||
virtual ~CpTopicPublisher() {}
|
||||
|
||||
void publish(const MessageType & msg) { pub_->publish(msg); }
|
||||
|
||||
void onInitialize() override;
|
||||
|
||||
private:
|
||||
typename rclcpp::Publisher<MessageType>::SharedPtr pub_;
|
||||
//rclcpp::PublisherBase::SharedPtr pub_;
|
||||
|
||||
bool initialized_;
|
||||
std::string topicName_;
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
void CpTopicPublisher<T>::onInitialize()
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
if (!queueSize) queueSize = 1;
|
||||
if (!durability) durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT;
|
||||
if (!reliability) reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
|
||||
rclcpp::SensorDataQoS qos;
|
||||
qos.keep_last(*queueSize);
|
||||
qos.durability(*durability);
|
||||
qos.reliability(*reliability);
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Publisher to topic: " << topicName_);
|
||||
|
||||
auto nh = this->getNode();
|
||||
pub_ = nh->template create_publisher<T>(this->topicName_, qos);
|
||||
|
||||
this->initialized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace client_core_components
|
||||
} // namespace smacc2
|
||||
130
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_subscriber.hpp
vendored
Normal file
130
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/client_core_components/cp_topic_subscriber.hpp
vendored
Normal file
@@ -0,0 +1,130 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
|
||||
#pragma once
|
||||
#include <optional>
|
||||
#include <smacc2/client_bases/smacc_subscriber_client.hpp>
|
||||
#include <smacc2/component.hpp>
|
||||
#include <smacc2/smacc_signal.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
namespace client_core_components
|
||||
{
|
||||
using namespace smacc2::default_events;
|
||||
|
||||
template <typename MessageType>
|
||||
class CpTopicSubscriber : public smacc2::ISmaccComponent
|
||||
{
|
||||
public:
|
||||
std::optional<int> queueSize;
|
||||
|
||||
typedef MessageType TMessageType;
|
||||
|
||||
CpTopicSubscriber() { initialized_ = false; }
|
||||
|
||||
CpTopicSubscriber(std::string topicname) { topicName_ = topicname; }
|
||||
|
||||
virtual ~CpTopicSubscriber() {}
|
||||
|
||||
std::function<void(const MessageType &)> postMessageEvent;
|
||||
std::function<void(const MessageType &)> postInitialMessageEvent;
|
||||
|
||||
smacc2::SmaccSignal<void(const MessageType &)> onFirstMessageReceived_;
|
||||
smacc2::SmaccSignal<void(const MessageType &)> onMessageReceived_;
|
||||
|
||||
// signal subscription method. This signal will be triggered when the first message is received
|
||||
template <typename T>
|
||||
boost::signals2::connection onMessageReceived(
|
||||
void (T::*callback)(const MessageType &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(onMessageReceived_, callback, object);
|
||||
}
|
||||
|
||||
// signal subscription method. This signal will be triggered when the first message is received
|
||||
template <typename T>
|
||||
boost::signals2::connection onFirstMessageReceived(
|
||||
void (T::*callback)(const MessageType &), T * object)
|
||||
{
|
||||
return this->getStateMachine()->createSignalConnection(
|
||||
onFirstMessageReceived_, callback, object);
|
||||
}
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onStateOrthogonalAllocation()
|
||||
{
|
||||
this->postMessageEvent = [=](auto msg)
|
||||
{
|
||||
auto event = new EvTopicMessage<TSourceObject, TOrthogonal, MessageType>();
|
||||
event->msgData = msg;
|
||||
this->postEvent(event);
|
||||
};
|
||||
|
||||
this->postInitialMessageEvent = [=](auto msg)
|
||||
{
|
||||
auto event = new EvTopicInitialMessage<TSourceObject, TOrthogonal, MessageType>();
|
||||
event->msgData = msg;
|
||||
this->postEvent(event);
|
||||
};
|
||||
}
|
||||
|
||||
void onInitialize() override
|
||||
{
|
||||
if (!initialized_)
|
||||
{
|
||||
firstMessage_ = true;
|
||||
|
||||
if (!queueSize) queueSize = 1;
|
||||
|
||||
RCLCPP_INFO_STREAM(
|
||||
getLogger(), "[" << this->getName() << "] Subscribing to topic: " << topicName_);
|
||||
|
||||
rclcpp::SensorDataQoS qos;
|
||||
if (queueSize) qos.keep_last(*queueSize);
|
||||
|
||||
std::function<void(typename MessageType::SharedPtr)> fn = [this](auto msg)
|
||||
{ this->messageCallback(*msg); };
|
||||
|
||||
sub_ = this->getNode()->template create_subscription<MessageType>(topicName_, qos, fn);
|
||||
this->initialized_ = true;
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
typename rclcpp::Subscription<MessageType>::SharedPtr sub_;
|
||||
bool firstMessage_;
|
||||
bool initialized_;
|
||||
std::string topicName_;
|
||||
|
||||
void messageCallback(const MessageType & msg)
|
||||
{
|
||||
if (firstMessage_)
|
||||
{
|
||||
postInitialMessageEvent(msg);
|
||||
onFirstMessageReceived_(msg);
|
||||
firstMessage_ = false;
|
||||
}
|
||||
|
||||
postMessageEvent(msg);
|
||||
onMessageReceived_(msg);
|
||||
}
|
||||
};
|
||||
} // namespace client_core_components
|
||||
} // namespace smacc2
|
||||
77
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/common.hpp
vendored
Normal file
77
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/common.hpp
vendored
Normal file
@@ -0,0 +1,77 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
|
||||
// boost statechart
|
||||
#define BOOST_BIND_NO_PLACEHOLDERS
|
||||
#include <boost/statechart/asynchronous_state_machine.hpp>
|
||||
#include <boost/statechart/custom_reaction.hpp>
|
||||
#include <boost/statechart/deep_history.hpp>
|
||||
#include <boost/statechart/event.hpp>
|
||||
#include <boost/statechart/simple_state.hpp>
|
||||
#include <boost/statechart/state.hpp>
|
||||
|
||||
// other boost includes
|
||||
#include <boost/algorithm/string.hpp>
|
||||
#include <boost/any.hpp>
|
||||
#include <boost/config.hpp>
|
||||
#include <boost/function.hpp>
|
||||
#include <boost/intrusive_ptr.hpp>
|
||||
#include <boost/mpl/list.hpp>
|
||||
#include <boost/signals2.hpp>
|
||||
|
||||
#include <mutex>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <smacc2/introspection/introspection.hpp>
|
||||
#include <smacc2/smacc_fifo_scheduler.hpp>
|
||||
#include <smacc2/smacc_types.hpp>
|
||||
|
||||
typedef boost::statechart::processor_container<
|
||||
boost::statechart::fifo_scheduler<>, boost::function0<void>,
|
||||
std::allocator<boost::statechart::none>>::processor_context my_context;
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
template <class T>
|
||||
using deep_history = sc::deep_history<T>;
|
||||
|
||||
namespace utils
|
||||
{
|
||||
// demangles the type name to be used as a ros node name
|
||||
std::string cleanShortTypeName(const std::type_info & tinfo);
|
||||
|
||||
template <typename T>
|
||||
std::string cleanShortTypeName()
|
||||
{
|
||||
return cleanShortTypeName(typeid(T));
|
||||
}
|
||||
} // namespace utils
|
||||
|
||||
enum class SMRunMode
|
||||
{
|
||||
DEBUG,
|
||||
RELEASE
|
||||
};
|
||||
|
||||
} // namespace smacc2
|
||||
|
||||
#include <smacc2/smacc_default_events.hpp>
|
||||
#include <smacc2/smacc_transition.hpp>
|
||||
109
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/component.hpp
vendored
Normal file
109
src/thirdparty/SMACC2-humble/smacc2/include/smacc2/component.hpp
vendored
Normal file
@@ -0,0 +1,109 @@
|
||||
// Copyright 2021 RobosoftAI Inc.
|
||||
//
|
||||
// Licensed under the Apache License, Version 2.0 (the "License");
|
||||
// you may not use this file except in compliance with the License.
|
||||
// You may obtain a copy of the License at
|
||||
//
|
||||
// http://www.apache.org/licenses/LICENSE-2.0
|
||||
//
|
||||
// Unless required by applicable law or agreed to in writing, software
|
||||
// distributed under the License is distributed on an "AS IS" BASIS,
|
||||
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
||||
// See the License for the specific language governing permissions and
|
||||
// limitations under the License.
|
||||
|
||||
/*****************************************************************************************************************
|
||||
*
|
||||
* Authors: Pablo Inigo Blasco, Brett Aldrich
|
||||
*
|
||||
******************************************************************************************************************/
|
||||
#pragma once
|
||||
|
||||
#include <string>
|
||||
|
||||
#include <boost/optional.hpp>
|
||||
#include <smacc2/common.hpp>
|
||||
|
||||
namespace smacc2
|
||||
{
|
||||
class ISmaccComponent
|
||||
{
|
||||
public:
|
||||
ISmaccComponent();
|
||||
|
||||
virtual ~ISmaccComponent();
|
||||
|
||||
// Returns a custom identifier defined by the specific plugin implementation
|
||||
virtual std::string getName() const;
|
||||
|
||||
protected:
|
||||
// this is the basic initialization method that each specific component should
|
||||
// implement. The owner and the node are already available when it is invoked by the client.
|
||||
virtual void onInitialize();
|
||||
|
||||
// component initialization with type information of the orthogonal and client that owns it
|
||||
template <typename TOrthogonal, typename TClient>
|
||||
void onComponentInitialization();
|
||||
|
||||
template <typename EventType>
|
||||
void postEvent(const EventType & ev);
|
||||
|
||||
template <typename EventType>
|
||||
void postEvent();
|
||||
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
[[deprecated(
|
||||
"Use onStateOrthogonalAllocation instead. onOrthogonalAllocation will be removed in future "
|
||||
"versions.")]] void
|
||||
onOrthogonalAllocation()
|
||||
{
|
||||
}
|
||||
|
||||
// New method: called when the component is allocated to a state (replaces onOrthogonalAllocation)
|
||||
template <typename TOrthogonal, typename TSourceObject>
|
||||
void onStateOrthogonalAllocation()
|
||||
{
|
||||
}
|
||||
|
||||
template <typename TComponent>
|
||||
void requiresComponent(
|
||||
TComponent *& requiredComponentStorage, bool throwExceptionIfNotExist = false);
|
||||
|
||||
template <typename TComponent>
|
||||
void requiresComponent(
|
||||
std::string name, TComponent *& requiredComponentStorage,
|
||||
bool throwExceptionIfNotExist = false);
|
||||
|
||||
template <typename TClient>
|
||||
void requiresClient(TClient *& requiredClientStorage);
|
||||
|
||||
template <typename SmaccComponentType, typename TOrthogonal, typename TClient, typename... TArgs>
|
||||
SmaccComponentType * createSiblingComponent(TArgs... targs);
|
||||
|
||||
template <typename SmaccComponentType, typename TOrthogonal, typename TClient, typename... TArgs>
|
||||
SmaccComponentType * createSiblingNamedComponent(std::string name, TArgs... targs);
|
||||
|
||||
rclcpp::Node::SharedPtr getNode();
|
||||
|
||||
rclcpp::Logger getLogger() const;
|
||||
|
||||
//inline
|
||||
ISmaccStateMachine * getStateMachine();
|
||||
|
||||
// A reference to the state machine object that owns this resource
|
||||
ISmaccStateMachine * stateMachine_;
|
||||
|
||||
ISmaccClient * owner_;
|
||||
|
||||
private:
|
||||
// friend method invoked by the client.
|
||||
void initialize(ISmaccClient * owner);
|
||||
|
||||
// friend method invoked by the client.
|
||||
// Assigns the owner of this resource to the given state machine parameter object
|
||||
void setStateMachine(ISmaccStateMachine * stateMachine);
|
||||
|
||||
friend class ISmaccOrthogonal;
|
||||
friend class ISmaccClient;
|
||||
};
|
||||
} // namespace smacc2
|
||||
Some files were not shown because too many files have changed in this diff Show More
Reference in New Issue
Block a user