diff --git a/scripts/visualizer.py b/scripts/visualizer.py
index a849ec49c..2e16d6cba 100755
--- a/scripts/visualizer.py
+++ b/scripts/visualizer.py
@@ -11,14 +11,14 @@
import rospy
import sys
-from smach_msgs.msg import SmachContainerStatus, SmachContainerStructure
+from mrover.msg import StateMachineStructure, StateMachineStateUpdate
from threading import Lock
from dataclasses import dataclass
from typing import Optional, List, Dict
-STRUCTURE_TOPIC = "/smach/container_structure"
-STATUS_TOPIC = "/smach/container_status"
+STRUCTURE_TOPIC = "nav_structure"
+STATUS_TOPIC = "nav_state"
@dataclass
@@ -30,7 +30,7 @@ class State:
class StateMachine:
def __init__(self):
self.states: Dict[str, State] = {}
- self.structure: Optional[SmachContainerStructure] = None
+ self.structure: Optional[StateMachineStructure] = None
self.mutex: Lock = Lock()
self.cur_active: str = ""
self.previous_state: str = ""
@@ -50,17 +50,18 @@ def set_active_state(self, active_state):
f"Current time: {now} Previous state: {self.previous_state} Current State: { self.cur_active}"
)
- def _rebuild(self, structure: SmachContainerStructure):
+ def _rebuild(self, structure: StateMachineStructure):
"""
rebuilds the state dictionary with a new structure message
"""
- self.states = {child: State(child, []) for child in structure.children}
- for start, end in zip(structure.outcomes_from, structure.outcomes_to):
- if end != "None":
- self.states[start].children.append(self.states[end])
+ self.states = {child.origin: State(child.origin, []) for child in structure.transitions}
+ for transition in structure.transitions:
+ origin = transition.origin
+ for to in transition.destinations:
+ self.states[origin].children.append(self.states[to])
self.needs_redraw = True
- def check_rebuild(self, structure: SmachContainerStructure):
+ def check_rebuild(self, structure: StateMachineStructure):
"""
checks if the structure passed as input matches the structure already represented (thread safe)
"""
@@ -71,10 +72,10 @@ def check_rebuild(self, structure: SmachContainerStructure):
self._rebuild(structure)
self.structure = structure
- def container_status_callback(self, status: SmachContainerStatus):
- self.set_active_state(status.active_states[0])
+ def container_status_callback(self, status: StateMachineStateUpdate):
+ self.set_active_state(status.state)
- def container_structure_callback(self, structure: SmachContainerStructure):
+ def container_structure_callback(self, structure: StateMachineStructure):
self.check_rebuild(structure)
@@ -121,12 +122,12 @@ def update(self):
rospy.init_node("smach_visualizer", anonymous=False, disable_signals=True, log_level=rospy.INFO)
rospy.Subscriber(
STRUCTURE_TOPIC,
- SmachContainerStructure,
+ StateMachineStructure,
state_machine.container_structure_callback,
)
rospy.Subscriber(
STATUS_TOPIC,
- SmachContainerStatus,
+ StateMachineStateUpdate,
state_machine.container_status_callback,
)
app = QApplication([]) # type: ignore
diff --git a/src/navigation/navigation.py b/src/navigation/navigation.py
index 78088756b..c58d127da 100755
--- a/src/navigation/navigation.py
+++ b/src/navigation/navigation.py
@@ -45,7 +45,9 @@ def __init__(self, context: Context):
)
self.state_machine.add_transitions(
SearchState(),
- [ApproachTargetState(), LongRangeState(), WaypointState(), RecoveryState()],
+ [
+ ApproachTargetState(), LongRangeState(), WaypointState(), RecoveryState()
+ ],
)
self.state_machine.add_transitions(DoneState(), [WaypointState()])
self.state_machine.add_transitions(
diff --git a/urdf/world/world.urdf.xacro b/urdf/world/world.urdf.xacro
index 346deec0e..7bd248c56 100644
--- a/urdf/world/world.urdf.xacro
+++ b/urdf/world/world.urdf.xacro
@@ -7,12 +7,12 @@
-
+
-
+