From d1a4f6ffd73cd51f21ccc15d3eaba8a82c20b8ff Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Thu, 27 Nov 2025 14:12:44 +0100 Subject: [PATCH 1/6] rise_motion: Implement state machine. * src/node_fsm.cpp: Add src/node_fsm.cpp. * include/rise_motion/node_fsm.hpp: Add include/rise_motion/node_fsm.hpp. --- .../include/rise_motion/node_fsm.hpp | 20 +++++++++ .../src/rise_motion/src/node_fsm.cpp | 45 +++++++++++++++++++ 2 files changed, 65 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp create mode 100644 rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp new file mode 100644 index 0000000..13dce6b --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp @@ -0,0 +1,20 @@ +#pragma once +#include +#include +#include +class StateMachine { +public: + enum class State { Idle, Initialized, Operational, Error }; + enum class Event { Idle, Initialized, Operational, Error }; + + StateMachine(); + + State handle_event(Event e); + void set_state(State s); + State get_state(); + static std::string state_to_string(State s); + +private: + State state; + static std::map, State> transition_table; +}; diff --git a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp new file mode 100644 index 0000000..bda2398 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp @@ -0,0 +1,45 @@ +#include +#include + +#include + +StateMachine::StateMachine() : state(State::Idle) {} + +StateMachine::State StateMachine::handle_event(StateMachine::Event e) { + std::pair keypair = {this->state, + e}; + auto it = StateMachine::transition_table.find(keypair); + if (it != StateMachine::transition_table.end()) { + this->state = it->second; + return it->second; + } else { + return this->state; + } +} + +void StateMachine::set_state(State s) { state = s; } + +StateMachine::State StateMachine::get_state() { return state; } + +std::string StateMachine::state_to_string(State s) { + switch (s) { + case State::Idle: + return "Idle"; + case State::Initialized: + return "Initialized"; + case State::Operational: + return "Operational"; + case State::Error: + return "Error"; + } + return "Unknown"; +} + +std::map, + StateMachine::State> + StateMachine::transition_table = { + {{State::Idle, Event::Initialized}, State::Initialized}, + {{State::Initialized, Event::Operational}, State::Operational}, + {{State::Initialized, Event::Error}, State::Error}, + {{State::Operational, Event::Error}, State::Error}, + {{State::Error, Event::Idle}, State::Idle}}; From f24425ebd249a2d7346c558ced7487e65e9c8d86 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Thu, 27 Nov 2025 14:15:31 +0100 Subject: [PATCH 2/6] rise_motion: Minimal example with state machine. * CMakeLists.txt: - Add dependencies: rclcpp, rise_motion_messages. - Add include dirs: include. - Add executable: rise_motion_main. - Link with rise_motion_messages. * package.xml: - Add dependencies: rclcpp, rise_motion_messages. * src/rise_motion_main.cpp: Add minimal example. - Moves through the states of the state machine and publishes state update via StateCurrentMsg on rise_motion_state_changes. --- .../src/rise_motion/CMakeLists.txt | 13 +++- .../src/rise_motion/package.xml | 3 + .../src/rise_motion/src/rise_motion_main.cpp | 63 +++++++++++++++++++ 3 files changed, 76 insertions(+), 3 deletions(-) create mode 100644 rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index aa620a5..0b80d27 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -7,9 +7,16 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rise_motion_messages REQUIRED) + +include_directories(include) + +add_executable(rise_motion_main src/rise_motion_main.cpp src/node_fsm.cpp) +target_link_libraries(rise_motion_main PUBLIC rclcpp::rclcpp ${rise_motion_messages_TARGETS}) +install(TARGETS + rise_motion_main + DESTINATION lib/${PROJECT_NAME}) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) diff --git a/rise_motion_dev_ws/src/rise_motion/package.xml b/rise_motion_dev_ws/src/rise_motion/package.xml index da1008f..9c5ac6c 100644 --- a/rise_motion_dev_ws/src/rise_motion/package.xml +++ b/rise_motion_dev_ws/src/rise_motion/package.xml @@ -9,6 +9,9 @@ ament_cmake + rclcpp + rise_motion_messages + ament_lint_auto ament_lint_common diff --git a/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp b/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp new file mode 100644 index 0000000..a376e35 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp @@ -0,0 +1,63 @@ +#include +#include +#include +#include +#include +#include +#include + +class RiseMotionMain : public rclcpp::Node { +public: + RiseMotionMain() : Node("rise_motion_main"), sm_() { + publisher_ = + this->create_publisher( + "rise_motion_state_changes", 10); + timer_ = this->create_wall_timer(std::chrono::milliseconds(500), + [this]() -> void { this->on_time_out(); }); + } + +private: + void on_time_out() { + auto message = rise_motion_messages::msg::StateCurrentMsg(); + StateMachine::State previous_state = sm_.get_state(); + StateMachine::State new_state; + + switch (previous_state) { + case StateMachine::State::Idle: + new_state = sm_.handle_event(StateMachine::Event::Initialized); + break; + case StateMachine::State::Initialized: + new_state = sm_.handle_event(StateMachine::Event::Operational); + break; + case StateMachine::State::Operational: + new_state = sm_.handle_event(StateMachine::Event::Error); + break; + case StateMachine::State::Error: + default: + new_state = sm_.handle_event(StateMachine::Event::Idle); + break; + } + + if (sm_.get_state() != previous_state) { + message.previous_state = static_cast(previous_state); + message.current_state = static_cast(new_state); + sm_.set_state(new_state); + + RCLCPP_INFO(this->get_logger(), "State changed to '%s'", + StateMachine::state_to_string(sm_.get_state()).c_str()); + this->publisher_->publish(message); + } + } + + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr + publisher_; + StateMachine sm_; +}; + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From 114e88bf6fa6748c3ce78e0f4a30d45442b488b6 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Wed, 3 Dec 2025 22:19:19 +0100 Subject: [PATCH 3/6] rise_motion: node_fsm: Rename Event members. --- .../src/rise_motion/include/rise_motion/node_fsm.hpp | 2 +- rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp | 11 ++++++----- .../src/rise_motion/src/rise_motion_main.cpp | 8 ++++---- 3 files changed, 11 insertions(+), 10 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp index 13dce6b..e38134e 100644 --- a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp @@ -5,7 +5,7 @@ class StateMachine { public: enum class State { Idle, Initialized, Operational, Error }; - enum class Event { Idle, Initialized, Operational, Error }; + enum class Event { ToIdle, ToInitialized, ToOperational, ToError }; StateMachine(); diff --git a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp index bda2398..d0609a1 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp @@ -38,8 +38,9 @@ std::string StateMachine::state_to_string(State s) { std::map, StateMachine::State> StateMachine::transition_table = { - {{State::Idle, Event::Initialized}, State::Initialized}, - {{State::Initialized, Event::Operational}, State::Operational}, - {{State::Initialized, Event::Error}, State::Error}, - {{State::Operational, Event::Error}, State::Error}, - {{State::Error, Event::Idle}, State::Idle}}; + {{State::Idle, Event::ToInitialized}, State::Initialized}, + {{State::Idle, Event::ToError}, State::Initialized}, + {{State::Initialized, Event::ToOperational}, State::Operational}, + {{State::Initialized, Event::ToError}, State::Error}, + {{State::Operational, Event::ToError}, State::Error}, + {{State::Error, Event::ToIdle}, State::Idle}}; diff --git a/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp b/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp index a376e35..428026e 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp @@ -24,17 +24,17 @@ class RiseMotionMain : public rclcpp::Node { switch (previous_state) { case StateMachine::State::Idle: - new_state = sm_.handle_event(StateMachine::Event::Initialized); + new_state = sm_.handle_event(StateMachine::Event::ToInitialized); break; case StateMachine::State::Initialized: - new_state = sm_.handle_event(StateMachine::Event::Operational); + new_state = sm_.handle_event(StateMachine::Event::ToOperational); break; case StateMachine::State::Operational: - new_state = sm_.handle_event(StateMachine::Event::Error); + new_state = sm_.handle_event(StateMachine::Event::ToError); break; case StateMachine::State::Error: default: - new_state = sm_.handle_event(StateMachine::Event::Idle); + new_state = sm_.handle_event(StateMachine::Event::ToIdle); break; } From fcc97de51acba386b2da21f02c1ac6c0f8dd75a8 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Wed, 3 Dec 2025 22:20:22 +0100 Subject: [PATCH 4/6] rise_motion: node_fsm: Add event_to_string method. --- .../rise_motion/include/rise_motion/node_fsm.hpp | 1 + .../src/rise_motion/src/node_fsm.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+) diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp index e38134e..7b78a94 100644 --- a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp @@ -13,6 +13,7 @@ class StateMachine { void set_state(State s); State get_state(); static std::string state_to_string(State s); + static std::string event_to_string(Event e); private: State state; diff --git a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp index d0609a1..c230bc5 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp @@ -35,6 +35,20 @@ std::string StateMachine::state_to_string(State s) { return "Unknown"; } +std::string StateMachine::event_to_string(Event e) { + switch (e) { + case Event::ToIdle: + return "ToIdle"; + case Event::ToInitialized: + return "ToInitialized"; + case Event::ToOperational: + return "ToOperational"; + case Event::ToError: + return "ToError"; + } + return "Unknown"; +} + std::map, StateMachine::State> StateMachine::transition_table = { From 82703e1a84d5db79de0a58a849cee00bc6a4c7f1 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Wed, 3 Dec 2025 22:29:09 +0100 Subject: [PATCH 5/6] rise_motion: node_fsm: Add logger class member. --- .../src/rise_motion/include/rise_motion/node_fsm.hpp | 3 +++ rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp | 2 ++ 2 files changed, 5 insertions(+) diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp index 7b78a94..31729dc 100644 --- a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/node_fsm.hpp @@ -1,7 +1,9 @@ #pragma once #include +#include #include #include + class StateMachine { public: enum class State { Idle, Initialized, Operational, Error }; @@ -18,4 +20,5 @@ class StateMachine { private: State state; static std::map, State> transition_table; + static rclcpp::Logger logger; }; diff --git a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp index c230bc5..e26f718 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp @@ -49,6 +49,8 @@ std::string StateMachine::event_to_string(Event e) { return "Unknown"; } +rclcpp::Logger StateMachine::logger = rclcpp::get_logger("StateMachine"); + std::map, StateMachine::State> StateMachine::transition_table = { From 9247f84760beaec14a699cf3fd34c029d3ab4619 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Wed, 3 Dec 2025 22:30:04 +0100 Subject: [PATCH 6/6] rise_motion: node_fsm+rise_motion_main: Internalize logging. Moves logging from the client code into the StateMachine class. --- rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp | 10 +++++++++- .../src/rise_motion/src/rise_motion_main.cpp | 3 --- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp index e26f718..c6f1e88 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/node_fsm.cpp @@ -1,18 +1,26 @@ #include #include +#include #include StateMachine::StateMachine() : state(State::Idle) {} -StateMachine::State StateMachine::handle_event(StateMachine::Event e) { +StateMachine::State StateMachine::handle_event(Event e) { std::pair keypair = {this->state, e}; auto it = StateMachine::transition_table.find(keypair); if (it != StateMachine::transition_table.end()) { + State old = state; this->state = it->second; + RCLCPP_INFO(logger, + "Transition %s --(%s)--> %s", state_to_string(old).c_str(), + event_to_string(e).c_str(), state_to_string(state).c_str()); return it->second; } else { + RCLCPP_WARN(logger, + "Invalid transition: (State: %s, Event: %s)", + state_to_string(state).c_str(), event_to_string(e).c_str()); return this->state; } } diff --git a/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp b/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp index 428026e..c596ca4 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/rise_motion_main.cpp @@ -42,9 +42,6 @@ class RiseMotionMain : public rclcpp::Node { message.previous_state = static_cast(previous_state); message.current_state = static_cast(new_state); sm_.set_state(new_state); - - RCLCPP_INFO(this->get_logger(), "State changed to '%s'", - StateMachine::state_to_string(sm_.get_state()).c_str()); this->publisher_->publish(message); } }