From 04e7ec31199d3d724ed48372001d9d9e9021139f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Miguel=20=C3=81ngel=20Gonz=C3=A1lez=20Santamarta?= Date: Sun, 3 Nov 2024 22:21:20 +0100 Subject: [PATCH] comments for Python attributes moved to __init__ --- yasmin/src/yasmin/state_machine.cpp | 8 +-- yasmin/yasmin/state_machine.py | 6 +- yasmin_ros/yasmin_ros/action_state.py | 68 +++++++++---------- yasmin_ros/yasmin_ros/monitor_state.py | 47 ++++++------- yasmin_ros/yasmin_ros/service_state.py | 44 ++++++------ yasmin_ros/yasmin_ros/yasmin_node.py | 8 +-- .../yasmin_viewer/yasmin_viewer_node.py | 11 +-- .../yasmin_viewer/yasmin_viewer_pub.py | 21 +++--- 8 files changed, 98 insertions(+), 115 deletions(-) diff --git a/yasmin/src/yasmin/state_machine.cpp b/yasmin/src/yasmin/state_machine.cpp index c67eba0..eeb5832 100644 --- a/yasmin/src/yasmin/state_machine.cpp +++ b/yasmin/src/yasmin/state_machine.cpp @@ -281,11 +281,9 @@ StateMachine::execute(std::shared_ptr blackboard) { while (!this->is_canceled()) { - this->current_state_mutex->lock(); - - auto state = this->states.at(this->current_state); - transitions = this->transitions.at(this->current_state); - this->current_state_mutex->unlock(); + std::string current_state = this->get_current_state(); + auto state = this->states.at(current_state); + transitions = this->transitions.at(current_state); outcome = (*state.get())(blackboard); old_outcome = std::string(outcome); diff --git a/yasmin/yasmin/state_machine.py b/yasmin/yasmin/state_machine.py index 0f58342..8351bca 100644 --- a/yasmin/yasmin/state_machine.py +++ b/yasmin/yasmin/state_machine.py @@ -104,6 +104,7 @@ def add_state( f"State '{name}' references unregistered outcomes '{key}', available outcomes are {list(state.get_outcomes())}" ) + # Debug state and its transitions transition_string = "\n\t" + "\n\t".join( f"{key} --> {transitions[key]}" for key in transitions ) @@ -278,6 +279,7 @@ def validate(self) -> None: if not self._start_state: raise RuntimeError("No initial state set") + # terminal outcomes terminal_outcomes = [] # Check all states @@ -346,9 +348,7 @@ def execute(self, blackboard: Blackboard) -> str: self.__current_state: str = start_state while not self.is_canceled(): - with self.__current_state_lock: - state = self._states[self.__current_state] - + state = self._states[self.get_current_state()] outcome = state["state"](blackboard) old_outcome = outcome diff --git a/yasmin_ros/yasmin_ros/action_state.py b/yasmin_ros/yasmin_ros/action_state.py index 71b202e..57a385d 100644 --- a/yasmin_ros/yasmin_ros/action_state.py +++ b/yasmin_ros/yasmin_ros/action_state.py @@ -51,31 +51,6 @@ class ActionState(State): _timeout (float): Timeout duration for waiting for the action server. """ - ## The ROS 2 node instance used to communicate with the action server. - _node: Node - ## The name of the action to be performed. - _action_name: str - ## The action client used to send goals. - _action_client: ActionClient - ## Event used to wait for action completion. - _action_done_event: Event = Event() - ## The result returned by the action server. - _action_result: Any - ## The status of the action execution. - _action_status: GoalStatus - ## Handle for the goal sent to the action server. - _goal_handle: ClientGoalHandle - ## Lock to manage access to the goal handle. - _goal_handle_lock: RLock = RLock() - ## Function that creates the goal to send. - _create_goal_handler: Callable[[Blackboard], Any] - ## Function to handle the result from the action server. - _result_handler: Callable[[Blackboard, Any], str] - ## Function to handle feedback from the action server. - _feedback_handler: Callable[[Blackboard, Any], None] - ## Timeout duration for waiting for the action server. - _timeout: float - def __init__( self, action_type: Type, @@ -105,32 +80,53 @@ def __init__( Raises: ValueError: If create_goal_handler is None. """ - self._action_name: str = action_name - _outcomes = [SUCCEED, ABORT, CANCEL] + ## Event used to wait for action completion. + self._action_done_event: Event = Event() + ## The result returned by the action server. + self._action_result: Any = None + ## The status of the action execution. + self._action_status: GoalStatus = None + ## Handle for the goal sent to the action server. + self._goal_handle: ClientGoalHandle = None + ## Lock to manage access to the goal handle. + self._goal_handle_lock: RLock = RLock() + + ## Function that creates the goal to send. + self._create_goal_handler: Callable[[Blackboard], Any] = create_goal_handler + ## Function to handle the result from the action server. + self._result_handler: Callable[[Blackboard, Any], str] = result_handler + ## Function to handle feedback from the action server. + self._feedback_handler: Callable[[Blackboard, Any], None] = feedback_handler + + ## Timeout duration for waiting for the action server. self._timeout: float = timeout + + _outcomes = [SUCCEED, ABORT, CANCEL] + if self._timeout: _outcomes.append(TIMEOUT) if outcomes: _outcomes = _outcomes + outcomes - if node is None: - node: Node = YasminNode.get_instance() - - # Create ROS 2 action + ## The ROS 2 node instance used to communicate with the action server. self._node: Node = node + + if self._node is None: + self._node: Node = YasminNode.get_instance() + + ## The name of the action to be performed. + self._action_name: str = action_name + + ## The action client used to send goals. self._action_client: ActionClient = ActionClient( - node, + self._node, action_type, action_name, callback_group=ReentrantCallbackGroup(), ) - self._create_goal_handler: Callable[[Blackboard], Any] = create_goal_handler - self._result_handler: Callable[[Blackboard, Any], str] = result_handler - self._feedback_handler: Callable[[Blackboard, Any], None] = feedback_handler - if not self._create_goal_handler: raise ValueError("create_goal_handler is needed") diff --git a/yasmin_ros/yasmin_ros/monitor_state.py b/yasmin_ros/yasmin_ros/monitor_state.py index 462b1d8..0b72f4a 100644 --- a/yasmin_ros/yasmin_ros/monitor_state.py +++ b/yasmin_ros/yasmin_ros/monitor_state.py @@ -45,25 +45,6 @@ class MonitorState(State): None raised directly; timeout is managed via outcome handling. """ - ## The ROS 2 node instance used for subscriptions. - _node: Node - ## Subscription object for the specified topic. - _sub: Subscription - ## Function to handle incoming messages. - _monitor_handler: Callable[[Blackboard, Any], None] - ## The name of the topic to monitor. - _topic_name: str - ## A set of messages received from the topic. - msg_list: List[Any] = [] - ## The maximum number of messages to retain. - msg_queue: int - ## Time to wait between checks for messages. - time_to_wait: float = 0.001 - ## Flag indicating if monitoring is active. - monitoring: bool = False - ## Timeout duration for monitoring. - _timeout: int - def __init__( self, msg_type: Type, @@ -92,19 +73,33 @@ def __init__( None """ - self._topic_name: str = topic_name + ## Function to handle incoming messages. + self._monitor_handler: Callable[[Blackboard, Any], None] = monitor_handler + ## A set of messages received from the topic. + self.msg_list: List[Any] = [] + ## The maximum number of messages to retain. + self.msg_queue: int = msg_queue + ## Time to wait between checks for messages. + self.time_to_wait: float = 0.001 + ## Flag indicating if monitoring is active. + self.monitoring: bool = False + + ## Timeout duration for monitoring. self._timeout: int = timeout + if timeout is not None: outcomes = [TIMEOUT] + outcomes - self._monitor_handler: Callable[[Blackboard, Any], None] = monitor_handler - self.msg_queue: int = msg_queue + ## The ROS 2 node instance used for subscriptions. + self._node: Node = node - # Create ROS 2 subscriber - if node is None: - node = YasminNode.get_instance() + if self._node is None: + self._node = YasminNode.get_instance() - self._node: Node = node + ## The name of the topic to monitor. + self._topic_name: str = topic_name + + ## Subscription object for the specified topic. self._sub: Subscription = self._node.create_subscription( msg_type, topic_name, self.__callback, qos ) diff --git a/yasmin_ros/yasmin_ros/service_state.py b/yasmin_ros/yasmin_ros/service_state.py index 87880b9..dd34f0a 100644 --- a/yasmin_ros/yasmin_ros/service_state.py +++ b/yasmin_ros/yasmin_ros/service_state.py @@ -41,19 +41,6 @@ class ServiceState(State): _timeout (float): Timeout duration for the service call. """ - ## The ROS node used to communicate with the service. - _node: Node - ## The name of the service to call. - _srv_name: str - ## The client used to call the service. - _service_client: Client - ## A function that creates the service request. - _create_request_handler: Callable[[Blackboard], Any] - ## A function that processes the service response. - _response_handler: Callable[[Blackboard, Any], str] - ## Timeout duration for the service call. - _timeout: float - def __init__( self, srv_type: Type, @@ -79,27 +66,36 @@ def __init__( Raises: ValueError: If the create_request_handler is not provided. """ - self._srv_name: str = srv_name - _outcomes = [SUCCEED, ABORT] + ## A function that creates the service request. + self._create_request_handler: Callable[[Blackboard], Any] = ( + create_request_handler + ) + ## A function that processes the service response. + self._response_handler: Callable[[Blackboard, Any], str] = response_handler + + ## Timeout duration for the service call. self._timeout: float = timeout + + _outcomes = [SUCCEED, ABORT] + if self._timeout: _outcomes.append(TIMEOUT) if outcomes: _outcomes = _outcomes + outcomes - # Create ROS 2 service - if node is None: - node: Node = YasminNode.get_instance() + ## The ROS node used to communicate with the service. + self._node = node - self._node: Node = node - self._service_client: Client = self._node.create_client(srv_type, srv_name) + if self._node is None: + self._node: Node = YasminNode.get_instance() - self._create_request_handler: Callable[[Blackboard], Any] = ( - create_request_handler - ) - self._response_handler: Callable[[Blackboard, Any], str] = response_handler + ## The name of the service to call. + self._srv_name: str = srv_name + + ## The client used to call the service. + self._service_client: Client = self._node.create_client(srv_type, srv_name) if not self._create_request_handler: raise ValueError("create_request_handler is needed") diff --git a/yasmin_ros/yasmin_ros/yasmin_node.py b/yasmin_ros/yasmin_ros/yasmin_node.py index 4c5fed3..81133be 100644 --- a/yasmin_ros/yasmin_ros/yasmin_node.py +++ b/yasmin_ros/yasmin_ros/yasmin_node.py @@ -41,10 +41,6 @@ class YasminNode(Node): _instance: "YasminNode" = None ## Lock to control access to the instance. _lock: RLock = RLock() - ## Executor for managing node operations. - _executor: MultiThreadedExecutor = None - ## Thread to execute the spinning of the node. - _spin_thread: Thread = None @staticmethod def get_instance() -> "YasminNode": @@ -80,10 +76,10 @@ def __init__(self) -> None: super().__init__(f"yasmin_{str(uuid.uuid4()).replace('-', '')[:16]}_node") - # Initialize the MultiThreadedExecutor + ## Executor for managing node operations. self._executor: MultiThreadedExecutor = MultiThreadedExecutor() self._executor.add_node(self) - # Start the executor in a separate thread + ## Thread to execute the spinning of the node. self._spin_thread: Thread = Thread(target=self._executor.spin) self._spin_thread.start() diff --git a/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py b/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py index 81095ac..e36873e 100644 --- a/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py +++ b/yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py @@ -54,11 +54,6 @@ class YasminFsmViewerNode(Node): fsm_viewer_cb(msg): Callback function for processing FSM updates. """ - ## ndicates whether the server has started. - __started: bool = False - ## A dictionary that stores FSM data with automatic expiration. - __fsm_dict: ExpiringDict = ExpiringDict(max_len=300, max_age_seconds=3) - def __init__(self) -> None: """ Initializes the YasminFsmViewerNode node. @@ -68,6 +63,12 @@ def __init__(self) -> None: """ super().__init__("yasmin_viewer") + ## indicates whether the server has started. + self.__started: bool = False + + ## A dictionary that stores FSM data with automatic expiration. + self.__fsm_dict: ExpiringDict = ExpiringDict(max_len=300, max_age_seconds=3) + # Declare parameters for the Flask server self.declare_parameters( namespace="", diff --git a/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py b/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py index be9c246..5d61468 100644 --- a/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py +++ b/yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py @@ -42,13 +42,6 @@ class YasminViewerPub: _publish_data(): Publishes the current state of the FSM. """ - ## The ROS 2 node instance used for publishing. - _node: Node - ## The finite state machine to be published. - _fsm: StateMachine - ## The name of the finite state machine. - _fsm_name: str - def __init__( self, fsm_name: str, @@ -68,18 +61,26 @@ def __init__( Raises: ValueError: If fsm_name is empty. """ + if not fsm_name: raise ValueError("FSM name cannot be empty.") + ## The finite state machine to be published. self._fsm: StateMachine = fsm + + ## The name of the finite state machine. self._fsm_name: str = fsm_name - if node is None: + ## The ROS 2 node instance used for publishing. + self._node = node + + if self._node is None: self._node: Node = YasminNode.get_instance() - else: - self._node: Node = node + ## The publisher for the state machine messages. self.pub = self._node.create_publisher(StateMachineMsg, "/fsm_viewer", 10) + + ## A timer to periodically publish the FSM state. self._timer = self._node.create_timer(1 / rate, self._publish_data) def parse_transitions(self, transitions: Dict[str, str]) -> List[TransitionMsg]: