Skip to content

Commit fe293f2

Browse files
authored
Feature/testing (#8)
* Automatic Controller Chained Groups Detection * Minor Config Update * Removed Unnecesary Debug Output * Refactoring * Bugfixing * Refactoring and Bugfixing * Bugfixing * Removed Unnecessary Debugging Output * Started implementing tests * basic test setup * implemented more tests * Automatic Controller Chained Groups Detection * Minor Config Update * Removed Unnecesary Debug Output * Refactoring * Bugfixing * Refactoring and Bugfixing * Bugfixing * Removed Unnecessary Debugging Output * Inital Pre-commit run * Bugfix * Added missing test_dependencies * E-Stop Test Bugfix * Modified CI
1 parent 7019980 commit fe293f2

28 files changed

+3513
-131
lines changed

.clang-format

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,4 +11,4 @@ SpaceAfterTemplateKeyword: false
1111
SpaceInEmptyBlock: true
1212
SpacesInConditionalStatement: true
1313
SpacesInParentheses: true
14-
UseTab: Never
14+
UseTab: Never

.github/workflows/lint_build_test.yaml

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -42,8 +42,8 @@ jobs:
4242
setup:
4343
- rosdistro: jazzy
4444
os: ubuntu-24.04
45-
- rosdistro: rolling
46-
os: ubuntu-latest
45+
#- rosdistro: rolling
46+
# os: ubuntu-latest
4747
runs-on: ${{ matrix.setup.os }}
4848
container:
4949
image: ros:${{ matrix.setup.rosdistro }}-ros-base

.gitignore

Lines changed: 3 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,4 +2,6 @@ cmake-build-*
22
.idea
33
.vscode
44
CMakeLists.txt.user
5-
.vscode
5+
.vscode
6+
__pycache__/
7+
**/__pycache__/

.pre-commit-config.yaml

Lines changed: 54 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,37 @@
1+
# To use:
2+
#
3+
# pre-commit run -a
4+
#
5+
# Or:
6+
#
7+
# pre-commit install # (runs every time you commit in git)
8+
#
9+
# To update this file:
10+
#
11+
# pre-commit autoupdate
12+
#
13+
# See https://github.com/pre-commit/pre-commit
14+
115
repos:
16+
# Standard hooks
17+
- repo: https://github.com/pre-commit/pre-commit-hooks
18+
rev: v5.0.0
19+
hooks:
20+
- id: check-added-large-files
21+
- id: check-docstring-first
22+
- id: check-merge-conflict
23+
- id: check-symlinks
24+
- id: check-xml
25+
- id: check-yaml
26+
args: ["--allow-multiple-documents"]
27+
- id: end-of-file-fixer
28+
- id: mixed-line-ending
29+
- id: trailing-whitespace
30+
exclude_types: [rst]
31+
32+
# CPP Checks
233
- repo: https://github.com/pre-commit/mirrors-clang-format
3-
rev: v20.1.6
34+
rev: v20.1.4
435
hooks:
536
- id: clang-format
637
name: Clang Format
@@ -16,32 +47,36 @@ repos:
1647
files: \.(cpp|cc|hpp|h)$
1748
pass_filenames: false
1849

19-
- repo: https://github.com/psf/black
20-
rev: 25.1.0
21-
hooks:
22-
- id: black
23-
name: Black
24-
language_version: python3
25-
26-
- repo: https://github.com/pre-commit/pre-commit-hooks
27-
rev: v5.0.0
50+
# Python hooks
51+
- repo: https://github.com/astral-sh/ruff-pre-commit
52+
rev: v0.12.7
2853
hooks:
29-
- id: trailing-whitespace
30-
- id: check-yaml
31-
- id: check-xml
32-
- id: check-merge-conflict
33-
54+
- id: ruff-check
55+
args: [ --fix ]
56+
- id: ruff-format
3457

58+
# CMake Formatting
3559
- repo: https://github.com/cheshirekow/cmake-format-precommit
3660
rev: v0.6.13
3761
hooks:
38-
- id: cmake-format
39-
- id: cmake-lint
40-
62+
- id: cmake-format
63+
- id: cmake-lint
4164

65+
# Package XML
4266
- repo: https://github.com/Joschi3/package_xml_validation.git
43-
rev: v1.1.7
67+
rev: v1.2.0
4468
hooks:
4569
- id: format-package-xml
4670
name: Validate and Format package.xml
4771

72+
# Spellcheck
73+
- repo: https://github.com/crate-ci/typos
74+
rev: v1.34.0
75+
hooks:
76+
- id: typos
77+
78+
- repo: https://github.com/python-jsonschema/check-jsonschema
79+
rev: 0.33.2
80+
hooks:
81+
- id: check-github-workflows
82+
args: ["--verbose"]

README.md

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -8,4 +8,4 @@ Simple controllers for the [ros2_control](https://control.ros.org/jazzy/index.ht
88

99
# hector_controller_spawner
1010
A lightweight ROS2 node that boots an entire *ros2_control* setup in a single shot, managing hardware interfaces and controllers efficiently.
11-
See [README.md](hector_controller_spawner/README.md) for details.
11+
See [README.md](hector_controller_spawner/README.md) for details.

hector_controller_spawner/CMakeLists.txt

Lines changed: 14 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -25,10 +25,23 @@ target_include_directories(
2525
ament_target_dependencies(hector_controller_spawner rclcpp std_msgs
2626
controller_manager_msgs)
2727

28+
if(BUILD_TESTING)
29+
find_package(ament_lint_auto REQUIRED)
30+
ament_lint_auto_find_test_dependencies()
31+
32+
# for ROS 2 launch‐testing
33+
find_package(launch_testing_ament_cmake REQUIRED)
34+
# This will install and run your test/controller_spawner_launch.py
35+
add_launch_test(test/test_spawner_basic.test.py TIMEOUT 360)
36+
add_launch_test(test/test_spawner_twice.test.py TIMEOUT 360)
37+
add_launch_test(test/test_spawner_estop.test.py TIMEOUT 360)
38+
add_launch_test(test/test_spawner_chain_detection.test.py TIMEOUT 360)
39+
endif()
40+
2841
install(TARGETS hector_controller_spawner DESTINATION lib/${PROJECT_NAME})
2942

3043
install(
31-
DIRECTORY config launch
44+
DIRECTORY config launch test
3245
DESTINATION share/${PROJECT_NAME}
3346
OPTIONAL)
3447

Lines changed: 35 additions & 20 deletions
Original file line numberDiff line numberDiff line change
@@ -1,34 +1,49 @@
1-
# Hector Controller Spawner - Multispawner – ROS2 Hardware & Controller Launcher
1+
# Hector Controller Spawner **Multispawner**
22

3-
Multi-spawner is a lightweight ROS2 node that boots an entire *ros2\_control* setup in a single shot. It resolves the tedium of juggling multiple **spawner** processes by batching most step:
3+
**ROS2 Hardware & Controller Launcher for `ros2_control`**
44

5-
* **Wait‑for‑safety/Wait-for-Hardware:** Optionally blocks on an emergency‑stop (`std_msgs/Bool`) topic before doing anything. The motors may be impossible to activate while the e‑stop is engaged.
6-
* **Hardware first:** Ensures every listed hardware interface is *loaded* **and** *active* (with automatic retries).
7-
* **Smart loading:** Loads only the controllers that are missing (skips those already present).
8-
* **Reduced overhead:** No per‑controller spawner nodes required, just one multispawner node.
9-
* **Restarts after e-stop deactivation**: Restarts the hardware/controller setup after the e-stop is deactivated, if an e-stop topic is specified.
5+
**Multispawner** is a minimal ROS2 node that launches an entire `ros2_control` setup in a single coordinated pass.
6+
It robustly manages hardware interfaces and controllers, ensuring everything is loaded, activated (if required), and
7+
ready to go with minimal configuration.
108

119
---
1210

13-
## Key Parameters
11+
## 🚀 Features
12+
13+
* **Wait-for-safety (e-stop):** Optionally blocks on an `std_msgs/Bool` topic (e.g., emergency stop) before starting.
14+
Useful when motors can't be activated while safety is engaged.
15+
* **Re-activation:** Automatically reactivates hardware interfaces and controllers after releasing the e-stop (if they
16+
became inactive).
17+
* **Controller Manager Synchronization:** Waits for the controller manager to become available before proceeding.
18+
* **Hardware-first strategy:** Ensures all listed hardware interfaces are both *loaded* and *activated* (with automatic
19+
retries on failure).
20+
* **Intelligent controller loading:** Skips controllers already present - only loads and activates what’s missing.
21+
* **Automatic chaining:** Automatically detects and starts *chained controllers* together - no additional config
22+
required.
23+
* **Single-node simplicity:** No need to spawn one spawner per controller - Multispawner handles everything.
24+
* **Robust retry logic:** Retries failed hardware/controller activations with configurable delays.
1425

15-
| Name | Type | Default | Purpose |
16-
|------------------------------------| ---------- | ------- |---------------------------------------------------------------------|
17-
| `hardware_interfaces` | `string[]` || Ordered list of hardware interface names to activate. |
18-
| `controllers` | `string[]` || Ordered list of controller names under management. |
19-
| `<ctrl>.activate` | `bool` | `true` | Activate this controller after loading? |
20-
| `<ctrl>.activate_as_group` | `string[]` || Controller groups that are activated togehter. |
21-
| `retry_delay` | `double` | `5.0` | Seconds between retry attempts. |
22-
| `estop_topic` | `string` | "" | Topic to wait on (false ⇒ proceed). Empty string disables the gate. |
23-
| `restart_after_estop_deactivation` | `bool` | `true` | Restart hardware and controllers after e-stop deactivation |
26+
---
2427

25-
See **athena.yaml** for a full example.
28+
## 🔧 Key Parameters
29+
30+
| Name | Type | Default | Description |
31+
|-----------------------|------------|---------|---------------------------------------------------------------------------|
32+
| `hardware_interfaces` | `string[]` | - | Ordered list of hardware interface names to activate. |
33+
| `controllers` | `string[]` | - | Ordered list of controller names to load and manage. |
34+
| `<ctrl>.activate` | `bool` | `true` | Should the controller be activated after loading? |
35+
| `retry_delay` | `double` | `5.0` | Delay (in seconds) between retry attempts. |
36+
| `estop_topic` | `string` | `""` | Topic to wait on (false ⇒ proceed). Leave empty to disable e-stop gating. |
37+
| `restart_after_estop_deactivation` | `bool` | `true` | Restart hardware and controllers after e-stop deactivation |
38+
📄 See [`athena.yaml`](config/athena.yaml) for a complete configuration example.
2639

2740
---
2841

29-
## Typical Usage
42+
## 🧪 Example Usage
3043

3144
```bash
3245
ros2 launch hector_controller_spawner hector_controller_spawner_launch.yml
3346
```
34-
Add it to a launch file exactly once—no per‑controller spawner nodes required.
47+
48+
* Include **only once** in your launch setup.
49+
* No need for individual `spawner` calls per controller.

hector_controller_spawner/config/athena.yaml

Lines changed: 1 addition & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
- "athena_flipper_interface"
1313
- "athena_arm_interface"
1414

15-
# ---------- controllers, in deterministic order -------------
15+
# ---------- controllers --------------------------------------
1616
controllers:
1717
- joint_state_broadcaster
1818
- flipper_trajectory_controller
@@ -31,11 +31,9 @@
3131

3232
self_collision_avoidance_controller:
3333
activate: true
34-
activate_as_group: ["flipper_velocity_controller"]
3534

3635
flipper_velocity_controller:
3736
activate: true
38-
activate_as_group: ["self_collision_avoidance_controller"]
3937

4038
gripper_trajectory_controller:
4139
activate: true

hector_controller_spawner/include/hector_controller_spawner/hector_controller_spawner.hpp

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,13 +18,27 @@
1818
namespace hector_controller_spawner
1919
{
2020

21+
inline std::string vecToString( const std::vector<std::string> &vec )
22+
{
23+
std::string result;
24+
for ( const auto &s : vec ) {
25+
if ( !result.empty() )
26+
result += ", ";
27+
result += s;
28+
}
29+
result += " (size: " + std::to_string( vec.size() ) + ")";
30+
return result;
31+
}
32+
2133
/**
2234
* @brief Multispawner waits for an (optional) e‑stop, then loads & activates
2335
* hardware interfaces followed by controllers .
2436
*/
2537
class MultiSpawner final : public rclcpp::Node
2638
{
2739
public:
40+
using ControllerGroup = std::vector<std::string>; // group of controllers to activate together
41+
2842
explicit MultiSpawner();
2943
void initialize();
3044
void start_sequence( bool initial_init );
@@ -42,7 +56,7 @@ class MultiSpawner final : public rclcpp::Node
4256
// ----- helper structs -----
4357
struct ControllerCfg {
4458
bool activate{ true };
45-
std::vector<std::string> activate_as_group;
59+
bool specified{ false }; // true if the controller was specified in the parameters
4660
};
4761

4862
// ----- callbacks -----
@@ -54,11 +68,18 @@ class MultiSpawner final : public rclcpp::Node
5468
bool configureController( const std::string &name );
5569
bool replicateParamsToCM();
5670
void verifyFinalStates();
71+
void parseControllerInfo( const controller_manager_msgs::srv::ListControllers_Response &resp,
72+
std::unordered_map<std::string, std::string> &current_state );
73+
bool ensureControllerState( bool desired_state,
74+
const std::unordered_map<std::string, std::string> &current_state );
75+
bool switchControllersRequest( const std::vector<std::string> &to_activate,
76+
const std::vector<std::string> &to_deactivate );
5777

5878
// ----- parameters -----
5979
std::vector<std::string> hw_interfaces_;
6080
std::vector<std::string> controllers_;
6181
std::unordered_map<std::string, ControllerCfg> controller_cfg_;
82+
std::vector<ControllerGroup> controller_groups_;
6283
double retry_delay_{ 5.0 };
6384
std::string estop_topic_;
6485
bool restart_after_estop_deactivation_{ false };

hector_controller_spawner/package.xml

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -15,6 +15,15 @@
1515
<depend>rclcpp</depend>
1616
<depend>std_msgs</depend>
1717

18+
<test_depend>ament_lint_auto</test_depend>
19+
<test_depend>controller_manager</test_depend>
20+
<test_depend>hector_ros_controllers</test_depend>
21+
<test_depend>joint_state_broadcaster</test_depend>
22+
<test_depend>joint_trajectory_controller</test_depend>
23+
<test_depend>launch_testing</test_depend>
24+
<test_depend>launch_testing_ament_cmake</test_depend>
25+
<test_depend>robot_state_publisher</test_depend>
26+
1827
<export>
1928
<build_type>ament_cmake</build_type>
2029
</export>

0 commit comments

Comments
 (0)