From ac1c6745383f5eeb804385ecf903927005b4e0d4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20=C3=81ngel=20Gonz=C3=A1lez=20Santamarta?= Date: Thu, 31 Oct 2024 10:29:36 +0100 Subject: [PATCH] validating state machine before publishing --- README.md | 46 +++++--- yasmin_demos/src/action_client_demo.cpp | 12 +- yasmin_demos/src/monitor_demo.cpp | 8 +- yasmin_demos/src/service_client_demo.cpp | 16 ++- yasmin_demos/src/yasmin_demo.cpp | 10 +- .../yasmin_viewer/yasmin_viewer_pub.hpp | 3 +- .../src/yasmin_viewer/yasmin_viewer_pub.cpp | 27 +++-- .../yasmin_viewer/yasmin_viewer_pub.py | 37 ++++-- .../nodesource_setup.sh | 108 ------------------ 9 files changed, 111 insertions(+), 156 deletions(-) delete mode 100644 yasmin_viewer/yasmin_viewer_web_client/nodesource_setup.sh diff --git a/README.md b/README.md index bee58e2..a6e2747 100644 --- a/README.md +++ b/README.md @@ -749,8 +749,14 @@ int main(int argc, char *argv[]) { // add states sm->add_state("FOO", std::make_shared(), - {{"outcome1", "BAR"}, {"outcome2", "outcome4"}}); - sm->add_state("BAR", std::make_shared(), {{"outcome3", "FOO"}}); + { + {"outcome1", "BAR"}, + {"outcome2", "outcome4"}, + }); + sm->add_state("BAR", std::make_shared(), + { + {"outcome3", "FOO"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("yasmin_demo", sm); @@ -866,17 +872,23 @@ int main(int argc, char *argv[]) { std::initializer_list{ yasmin_ros::basic_outcomes::SUCCEED}, set_ints), - {{yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"}, + }); sm->add_state("ADD_TWO_INTS", std::make_shared(), - {{"outcome1", "PRINTING_SUM"}, - {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, - {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + { + {"outcome1", "PRINTING_SUM"}, + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}, + }); sm->add_state("PRINTING_SUM", std::make_shared( std::initializer_list{ yasmin_ros::basic_outcomes::SUCCEED}, print_sum), - {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); @@ -1007,15 +1019,19 @@ int main(int argc, char *argv[]) { // add states sm->add_state("CALLING_FIBONACCI", std::make_shared(), - {{yasmin_ros::basic_outcomes::SUCCEED, "PRINTING_RESULT"}, - {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}, - {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "PRINTING_RESULT"}, + {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}, + }); sm->add_state("PRINTING_RESULT", std::make_shared( std::initializer_list{ yasmin_ros::basic_outcomes::SUCCEED}, print_result), - {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); @@ -1120,9 +1136,11 @@ int main(int argc, char *argv[]) { // add states sm->add_state("PRINTING_ODOM", std::make_shared(5), - {{"outcome1", "PRINTING_ODOM"}, - {"outcome2", "outcome4"}, - {yasmin_ros::basic_outcomes::TIMEOUT, "outcome4"}}); + { + {"outcome1", "PRINTING_ODOM"}, + {"outcome2", "outcome4"}, + {yasmin_ros::basic_outcomes::TIMEOUT, "outcome4"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_MONITOR_DEMO", sm); diff --git a/yasmin_demos/src/action_client_demo.cpp b/yasmin_demos/src/action_client_demo.cpp index f2946e8..97d8a67 100644 --- a/yasmin_demos/src/action_client_demo.cpp +++ b/yasmin_demos/src/action_client_demo.cpp @@ -113,15 +113,19 @@ int main(int argc, char *argv[]) { // add states sm->add_state("CALLING_FIBONACCI", std::make_shared(), - {{yasmin_ros::basic_outcomes::SUCCEED, "PRINTING_RESULT"}, - {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}, - {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "PRINTING_RESULT"}, + {yasmin_ros::basic_outcomes::CANCEL, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}, + }); sm->add_state("PRINTING_RESULT", std::make_shared( std::initializer_list{ yasmin_ros::basic_outcomes::SUCCEED}, print_result), - {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); diff --git a/yasmin_demos/src/monitor_demo.cpp b/yasmin_demos/src/monitor_demo.cpp index 90eba0c..c3c068b 100644 --- a/yasmin_demos/src/monitor_demo.cpp +++ b/yasmin_demos/src/monitor_demo.cpp @@ -86,9 +86,11 @@ int main(int argc, char *argv[]) { // add states sm->add_state("PRINTING_ODOM", std::make_shared(5), - {{"outcome1", "PRINTING_ODOM"}, - {"outcome2", "outcome4"}, - {yasmin_ros::basic_outcomes::TIMEOUT, "outcome4"}}); + { + {"outcome1", "PRINTING_ODOM"}, + {"outcome2", "outcome4"}, + {yasmin_ros::basic_outcomes::TIMEOUT, "outcome4"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_MONITOR_DEMO", sm); diff --git a/yasmin_demos/src/service_client_demo.cpp b/yasmin_demos/src/service_client_demo.cpp index 111f14f..79e813f 100644 --- a/yasmin_demos/src/service_client_demo.cpp +++ b/yasmin_demos/src/service_client_demo.cpp @@ -98,17 +98,23 @@ int main(int argc, char *argv[]) { std::initializer_list{ yasmin_ros::basic_outcomes::SUCCEED}, set_ints), - {{yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "ADD_TWO_INTS"}, + }); sm->add_state("ADD_TWO_INTS", std::make_shared(), - {{"outcome1", "PRINTING_SUM"}, - {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, - {yasmin_ros::basic_outcomes::ABORT, "outcome4"}}); + { + {"outcome1", "PRINTING_SUM"}, + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + {yasmin_ros::basic_outcomes::ABORT, "outcome4"}, + }); sm->add_state("PRINTING_SUM", std::make_shared( std::initializer_list{ yasmin_ros::basic_outcomes::SUCCEED}, print_sum), - {{yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}}); + { + {yasmin_ros::basic_outcomes::SUCCEED, "outcome4"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("YASMIN_ACTION_CLIENT_DEMO", sm); diff --git a/yasmin_demos/src/yasmin_demo.cpp b/yasmin_demos/src/yasmin_demo.cpp index 3da1d65..d372c82 100644 --- a/yasmin_demos/src/yasmin_demo.cpp +++ b/yasmin_demos/src/yasmin_demo.cpp @@ -82,8 +82,14 @@ int main(int argc, char *argv[]) { // add states sm->add_state("FOO", std::make_shared(), - {{"outcome1", "BAR"}, {"outcome2", "outcome4"}}); - sm->add_state("BAR", std::make_shared(), {{"outcome3", "FOO"}}); + { + {"outcome1", "BAR"}, + {"outcome2", "outcome4"}, + }); + sm->add_state("BAR", std::make_shared(), + { + {"outcome3", "FOO"}, + }); // pub yasmin_viewer::YasminViewerPub yasmin_pub("yasmin_demo", sm); diff --git a/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp b/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp index 3918517..19c6acd 100644 --- a/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp +++ b/yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp @@ -49,8 +49,7 @@ class YasminViewerPub { std::vector &states_list, int parent); -protected: - void start_publisher(); + void publish_data(); private: rclcpp::Node::SharedPtr node_; diff --git a/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp b/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp index cc55342..a413dbb 100644 --- a/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp +++ b/yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp @@ -15,11 +15,13 @@ #include +#include "yasmin/logs.hpp" #include "yasmin_ros/yasmin_node.hpp" #include "yasmin_viewer/yasmin_viewer_pub.hpp" using namespace yasmin_viewer; using namespace std::chrono_literals; +using namespace yasmin; YasminViewerPub::YasminViewerPub(std::string fsm_name, std::shared_ptr fsm) @@ -41,7 +43,7 @@ YasminViewerPub::YasminViewerPub(const rclcpp::Node::SharedPtr &node, "/fsm_viewer", 10); this->timer = this->node_->create_wall_timer( - 250ms, std::bind(&YasminViewerPub::start_publisher, this)); + 250ms, std::bind(&YasminViewerPub::publish_data, this)); } std::vector YasminViewerPub::parse_transitions( @@ -101,13 +103,24 @@ void YasminViewerPub::parse_state( } } -void YasminViewerPub::start_publisher() { +void YasminViewerPub::publish_data() { - std::vector states_list; - this->parse_state(this->fsm_name, this->fsm, {}, states_list, -1); + try { + this->fsm->validate(); - auto state_machine_msg = yasmin_msgs::msg::StateMachine(); - state_machine_msg.states = states_list; + std::vector states_list; + this->parse_state(this->fsm_name, this->fsm, {}, states_list, -1); - this->publisher->publish(state_machine_msg); + auto state_machine_msg = yasmin_msgs::msg::StateMachine(); + state_machine_msg.states = states_list; + + YASMIN_LOG_DEBUG("Publishing data of state machine '%s'", + this->fsm_name.c_str()); + this->publisher->publish(state_machine_msg); + + } catch (const std::exception &e) { + YASMIN_LOG_ERROR("Not publishing state machine '%s' due to " + "validation failure: \"%s\"", + this->fsm_name.c_str(), e.what()); + } } diff --git a/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py b/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py index 1afe546..756edfc 100644 --- a/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py +++ b/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py @@ -16,13 +16,15 @@ from typing import Dict, List from rclpy.node import Node + +import yasmin +from yasmin import StateMachine, State +from yasmin_ros.yasmin_node import YasminNode from yasmin_msgs.msg import ( State as StateMsg, StateMachine as StateMachineMsg, Transition as TransitionMsg, ) -from yasmin_ros.yasmin_node import YasminNode -from yasmin import StateMachine, State class YasminViewerPub: @@ -44,7 +46,7 @@ def __init__( self._node = node self.pub = self._node.create_publisher(StateMachineMsg, "/fsm_viewer", 10) - self._timer = self._node.create_timer(1 / rate, self._start_publisher) + self._timer = self._node.create_timer(1 / rate, self._publish_data) def parse_transitions(self, transitions: Dict[str, str]) -> List[TransitionMsg]: @@ -104,14 +106,27 @@ def parse_state( state_msg.current_state = child_state.id break - def _start_publisher(self) -> None: + def _publish_data(self) -> None: + + try: + self._fsm.validate() + + states_list = [] + self.parse_state( + self._fsm_name, + {"state": self._fsm, "transitions": {}}, + states_list, + ) - states_list = [] - self.parse_state( - self._fsm_name, {"state": self._fsm, "transitions": {}}, states_list - ) + state_machine_msg = StateMachineMsg() + state_machine_msg.states = states_list - state_machine_msg = StateMachineMsg() - state_machine_msg.states = states_list + yasmin.YASMIN_LOG_DEBUG( + f"Publishing data of state machine '{self._fsm_name}'" + ) + self.pub.publish(state_machine_msg) - self.pub.publish(state_machine_msg) + except Exception as e: + yasmin.YASMIN_LOG_ERROR( + f"Not publishing state machine '{self._fsm_name}' due to validation failure: {e}" + ) diff --git a/yasmin_viewer/yasmin_viewer_web_client/nodesource_setup.sh b/yasmin_viewer/yasmin_viewer_web_client/nodesource_setup.sh deleted file mode 100644 index cb96ca3..0000000 --- a/yasmin_viewer/yasmin_viewer_web_client/nodesource_setup.sh +++ /dev/null @@ -1,108 +0,0 @@ -#!/bin/bash - -# Logger Function -log() { - local message="$1" - local type="$2" - local timestamp=$(date '+%Y-%m-%d %H:%M:%S') - local color - local endcolor="\033[0m" - - case "$type" in - "info") color="\033[38;5;79m" ;; - "success") color="\033[1;32m" ;; - "error") color="\033[1;31m" ;; - *) color="\033[1;34m" ;; - esac - - echo -e "${color}${timestamp} - ${message}${endcolor}" -} - -# Error handler function -handle_error() { - local exit_code=$1 - local error_message="$2" - log "Error: $error_message (Exit Code: $exit_code)" "error" - exit $exit_code -} - -# Function to check for command availability -command_exists() { - command -v "$1" &> /dev/null -} - -check_os() { - if ! [ -f "/etc/debian_version" ]; then - echo "Error: This script is only supported on Debian-based systems." - exit 1 - fi -} - -# Function to Install the script pre-requisites -install_pre_reqs() { - log "Installing pre-requisites" "info" - - # Run 'apt-get update' - if ! apt-get update -y; then - handle_error "$?" "Failed to run 'apt-get update'" - fi - - # Run 'apt-get install' - if ! apt-get install -y apt-transport-https ca-certificates curl gnupg; then - handle_error "$?" "Failed to install packages" - fi - - if ! mkdir -p /usr/share/keyrings; then - handle_error "$?" "Makes sure the path /usr/share/keyrings exist or run ' mkdir -p /usr/share/keyrings' with sudo" - fi - - rm -f /usr/share/keyrings/nodesource.gpg || true - rm -f /etc/apt/sources.list.d/nodesource.list || true - - # Run 'curl' and 'gpg' - if ! curl -fsSL https://deb.nodesource.com/gpgkey/nodesource-repo.gpg.key | gpg --dearmor -o /usr/share/keyrings/nodesource.gpg; then - handle_error "$?" "Failed to download and import the NodeSource signing key" - fi -} - -# Function to configure the Repo -configure_repo() { - local node_version=$1 - - arch=$(dpkg --print-architecture) - if [ "$arch" != "amd64" ] && [ "$arch" != "arm64" ] && [ "$arch" != "armhf" ]; then - handle_error "1" "Unsupported architecture: $arch. Only amd64, arm64, and armhf are supported." - fi - - echo "deb [arch=$arch signed-by=/usr/share/keyrings/nodesource.gpg] https://deb.nodesource.com/node_$node_version nodistro main" | tee /etc/apt/sources.list.d/nodesource.list > /dev/null - - # N|solid Config - echo "Package: nsolid" | tee /etc/apt/preferences.d/nsolid > /dev/null - echo "Pin: origin deb.nodesource.com" | tee -a /etc/apt/preferences.d/nsolid > /dev/null - echo "Pin-Priority: 600" | tee -a /etc/apt/preferences.d/nsolid > /dev/null - - # Nodejs Config - echo "Package: nodejs" | tee /etc/apt/preferences.d/nodejs > /dev/null - echo "Pin: origin deb.nodesource.com" | tee -a /etc/apt/preferences.d/nodejs > /dev/null - echo "Pin-Priority: 600" | tee -a /etc/apt/preferences.d/nodejs > /dev/null - - # Run 'apt-get update' - if ! apt-get update -y; then - handle_error "$?" "Failed to run 'apt-get update'" - else - log "Repository configured successfully." - log "To install Node.js, run: apt-get install nodejs -y" "info" - log "You can use N|solid Runtime as a node.js alternative" "info" - log "To install N|solid Runtime, run: apt-get install nsolid -y \n" "success" - fi -} - -# Define Node.js version -NODE_VERSION="18.x" - -# Check OS -check_os - -# Main execution -install_pre_reqs || handle_error $? "Failed installing pre-requisites" -configure_repo "$NODE_VERSION" || handle_error $? "Failed configuring repository"