Skip to content

Commit

Permalink
validating state machine before publishing
Browse files Browse the repository at this point in the history
  • Loading branch information
@mgonzs13
mgonzs13 committed Oct 31, 2024
1 parent 8440876 commit ac1c674
Show file tree
Hide file tree
Showing 9 changed files with 111 additions and 156 deletions.
46 changes: 32 additions & 14 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -749,8 +749,14 @@ int main(int argc, char *argv[]) {

// add states
sm->add_state("FOO", std::make_shared<FooState>(),
{{"outcome1", "BAR"}, {"outcome2", "outcome4"}});
sm->add_state("BAR", std::make_shared<BarState>(), {{"outcome3", "FOO"}});
{
{"outcome1", "BAR"},
{"outcome2", "outcome4"},
});
sm->add_state("BAR", std::make_shared<BarState>(),
{
{"outcome3", "FOO"},
});

// pub
yasmin_viewer::YasminViewerPub yasmin_pub("yasmin_demo", sm);
Expand Down Expand Up @@ -866,17 +872,23 @@ int main(int argc, char *argv[]) {
std::initializer_list<std::string>{
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<AddTwoIntsState>(),
{{"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<yasmin::CbState>(
std::initializer_list<std::string>{
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);
Expand Down Expand Up @@ -1007,15 +1019,19 @@ int main(int argc, char *argv[]) {

// add states
sm->add_state("CALLING_FIBONACCI", std::make_shared<FibonacciState>(),
{{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<yasmin::CbState>(
std::initializer_list<std::string>{
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);
Expand Down Expand Up @@ -1120,9 +1136,11 @@ int main(int argc, char *argv[]) {

// add states
sm->add_state("PRINTING_ODOM", std::make_shared<PrintOdometryState>(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);
Expand Down
12 changes: 8 additions & 4 deletions yasmin_demos/src/action_client_demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,15 +113,19 @@ int main(int argc, char *argv[]) {

// add states
sm->add_state("CALLING_FIBONACCI", std::make_shared<FibonacciState>(),
{{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<yasmin::CbState>(
std::initializer_list<std::string>{
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);
Expand Down
8 changes: 5 additions & 3 deletions yasmin_demos/src/monitor_demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,9 +86,11 @@ int main(int argc, char *argv[]) {

// add states
sm->add_state("PRINTING_ODOM", std::make_shared<PrintOdometryState>(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);
Expand Down
16 changes: 11 additions & 5 deletions yasmin_demos/src/service_client_demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,17 +98,23 @@ int main(int argc, char *argv[]) {
std::initializer_list<std::string>{
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<AddTwoIntsState>(),
{{"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<yasmin::CbState>(
std::initializer_list<std::string>{
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);
Expand Down
10 changes: 8 additions & 2 deletions yasmin_demos/src/yasmin_demo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -82,8 +82,14 @@ int main(int argc, char *argv[]) {

// add states
sm->add_state("FOO", std::make_shared<FooState>(),
{{"outcome1", "BAR"}, {"outcome2", "outcome4"}});
sm->add_state("BAR", std::make_shared<BarState>(), {{"outcome3", "FOO"}});
{
{"outcome1", "BAR"},
{"outcome2", "outcome4"},
});
sm->add_state("BAR", std::make_shared<BarState>(),
{
{"outcome3", "FOO"},
});

// pub
yasmin_viewer::YasminViewerPub yasmin_pub("yasmin_demo", sm);
Expand Down
3 changes: 1 addition & 2 deletions yasmin_viewer/include/yasmin_viewer/yasmin_viewer_pub.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,8 +49,7 @@ class YasminViewerPub {
std::vector<yasmin_msgs::msg::State> &states_list,
int parent);

protected:
void start_publisher();
void publish_data();

private:
rclcpp::Node::SharedPtr node_;
Expand Down
27 changes: 20 additions & 7 deletions yasmin_viewer/src/yasmin_viewer/yasmin_viewer_pub.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,11 +15,13 @@

#include <chrono>

#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<yasmin::StateMachine> fsm)
Expand All @@ -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<yasmin_msgs::msg::Transition> YasminViewerPub::parse_transitions(
Expand Down Expand Up @@ -101,13 +103,24 @@ void YasminViewerPub::parse_state(
}
}

void YasminViewerPub::start_publisher() {
void YasminViewerPub::publish_data() {

std::vector<yasmin_msgs::msg::State> 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<yasmin_msgs::msg::State> 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());
}
}
37 changes: 26 additions & 11 deletions yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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]:

Expand Down Expand Up @@ -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}"
)
Loading

0 comments on commit ac1c674

Please sign in to comment.