Skip to content

Commit

Permalink
comments for Python attributes moved to __init__
Browse files Browse the repository at this point in the history
  • Loading branch information
mgonzs13 committed Nov 3, 2024
1 parent 216bc1a commit 04e7ec3
Show file tree
Hide file tree
Showing 8 changed files with 98 additions and 115 deletions.
8 changes: 3 additions & 5 deletions yasmin/src/yasmin/state_machine.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -281,11 +281,9 @@ StateMachine::execute(std::shared_ptr<blackboard::Blackboard> 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);
Expand Down
6 changes: 3 additions & 3 deletions yasmin/yasmin/state_machine.py
Original file line number Diff line number Diff line change
Expand Up @@ -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
)
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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

Expand Down
68 changes: 32 additions & 36 deletions yasmin_ros/yasmin_ros/action_state.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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")

Expand Down
47 changes: 21 additions & 26 deletions yasmin_ros/yasmin_ros/monitor_state.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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
)
Expand Down
44 changes: 20 additions & 24 deletions yasmin_ros/yasmin_ros/service_state.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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")
Expand Down
8 changes: 2 additions & 6 deletions yasmin_ros/yasmin_ros/yasmin_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -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":
Expand Down Expand Up @@ -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()
11 changes: 6 additions & 5 deletions yasmin_viewer/yasmin_viewer/yasmin_viewer_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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="",
Expand Down
21 changes: 11 additions & 10 deletions yasmin_viewer/yasmin_viewer/yasmin_viewer_pub.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand All @@ -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]:
Expand Down

0 comments on commit 04e7ec3

Please sign in to comment.