From 85c27eb7d15e87e3ec544dba5b0344d67d0295c3 Mon Sep 17 00:00:00 2001 From: gsh Date: Sun, 14 Sep 2025 15:18:58 -0400 Subject: [PATCH 1/7] port the new teleop ros2 --- .DS_Store | Bin 6148 -> 0 bytes .gitignore | 1 + mrobosub_lib/setup.py | 2 + mrobosub_teleop/launch/teleop_launch.xml | 6 + .../mrobosub_teleop/joystick_teleop.py | 183 ++++++++++++++++++ .../resource/joystick_controls_b2a.yaml | 57 ++++++ mrobosub_teleop/setup.py | 5 +- 7 files changed, 253 insertions(+), 1 deletion(-) delete mode 100644 .DS_Store create mode 100644 mrobosub_teleop/launch/teleop_launch.xml create mode 100644 mrobosub_teleop/mrobosub_teleop/joystick_teleop.py create mode 100644 mrobosub_teleop/resource/joystick_controls_b2a.yaml diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index 3b9732ce161671acc9e908b5172dbfa18e9f9016..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 6148 zcmeHKOG*P#5UkcL0&e2Y<-3A67(zTj4iF7O5aL84aXl-~<#b@1@Oe5*4L@6p#Yn3i$V-(H*(qT1xSlw(jp;+9` z^IMd|xT<3P-{r`df!~8!cX(t7wz`s(!W~=pT$yci0I(a$owT=Er_nI%d r8`nW$h;~ejcFc{p?+S;+pfewIqW%oHE;1?b*9x2g5 + + + + + diff --git a/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py new file mode 100644 index 00000000..7e4429c3 --- /dev/null +++ b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py @@ -0,0 +1,183 @@ +#!/usr/bin/env python + +from enum import Enum +from typing import List +import rclpy +from sensor_msgs.msg import Joy +from std_msgs.msg import Float64 +from typing import Dict, Union, Tuple +from mrobosub_lib import Node + +class ButtonCommand(int, Enum): + INCREASE = 1 + DECREASE = -1 + TOGGLE = 0 + +class Button: + """Store commands tied to buttons""" + def __init__(self, idx : int, commandstr : str) -> None: + self.idx = idx + self.commandstr = commandstr + self._ispressed = False + self._isrising_edge = False + self.name, self._action = self.__split_command(commandstr) + + @staticmethod + def __split_command(comstr : str) -> Tuple[str, ButtonCommand]: + """ Split command string into (axis, command) """ + if comstr.endswith(".increase"): + return (comstr[:-9], ButtonCommand.INCREASE) + elif comstr.endswith(".decrease"): + return (comstr[:-9], ButtonCommand.DECREASE) + elif comstr.endswith(".toggle"): + return (comstr[:-7], ButtonCommand.TOGGLE) + # Special commands + elif comstr.endswith("estop") or comstr.endswith("switch"): + return (comstr[4:], ButtonCommand.TOGGLE) + else: + raise ValueError(f"Button command {comstr} not recognised") + + def update(self, ispress : bool) -> None: + self._isrising_edge = (not self.pressed) and ispress + self._ispressed = ispress + + @property + def rise_edge(self) -> bool: + return self._isrising_edge + + @property + def pressed(self) -> bool: + return self._ispressed + + @property + def command(self) -> Tuple[str, ButtonCommand]: + return self.name, self._action + +class DOF: + """Store state of a degree of freedom""" + class DOFState(int, Enum): + POSE = 0 + TWIST = 1 + + def __init__(self, id : int, name : str, scale_t : float, scale_p : float, istoggleable : bool) -> None: + self.idx = id + self.name = name + + ## state variables + self.pos = 0.0 # current position + self.state = DOF.DOFState.TWIST + self.scale = 0.0 # current scale + self.setPoint = 0.0 # target position + + ## configs + self.scale_t = scale_t + self.scale_p = scale_p + self.istogglable = istoggleable + + self.twist_pub = self.create_publisher(Float64, f'/target_twist/{name}', qos_profile=1) + self.pose_pub = self.create_publisher(Float64, f'/target_pose/{name}', qos_profile=1) + self.pose_sub = self.create_subscriber(Float64, f'/pose/{name}', self.pose_callback) + + def pose_callback(self, msg : Float64) -> None: + self.pos = msg.data + + def update(self, command : ButtonCommand, scale : float) -> None: + self.state ^= int(not bool(abs(command) and self.istogglable)) # toggle if command is TOGGLE && istoggleable + self.scale = (self.scale_t * self.state + self.scale_p * (1 - self.state)) * scale # set scale based on state + self.setPoint = self.pos + self.scale_p * scale + + def publish(self) -> None: + if self.state == DOF.DOFState.POSE: + self.pose_pub.publish(Float64(self.setPoint)) + elif self.state == DOF.DOFState.TWIST: + self.twist_pub.publish(Float64(self.scale)) + +class Joystick_teleop(Node): + """ + Publishers + - /target_pose/heave + - /target_twist/heave + - /output_wrench/heave + - /target_twist/surge + - /output_wrench/surge + - /target_twist/sway + - /output_wrench/sway + - /target_pose/yaw + - /target_twist/yaw + - /output_wrench/yaw + - /target_pose/roll + - /target_twist/roll + - /output_wrench/roll + - /target_pose/pitch + - /target_twist/pitch + - /output_wrench/pitch + + Subscribers + - /joy + - /pose/heave + - /pose/yaw + - /pose/roll + - /pose/pitch + """ + axes : Dict[str, Dict[str, Union[str,float]]] + buttons : Dict[str, Dict[str, str]] + + + def __init__(self) -> None: + super().__init__("joystick_teleopn") + self.input_subscriber = self.create_subscriber(Joy, "/joy", self.joystick_callback) + self.wrench_pubs = [self.create_publisher(Float64, f'/output_wrench/{axis}', qos_profile=1) + for axis in ['sway', 'surge', 'heave', 'yaw', 'roll', 'pitch']] + # Globals + self.buttonsInstance : List[Button] = [] + self.DOFInstance : Dict[str, DOF] = {} + self.stateMachineMode = False + self.timer = self.create_timer(1.0/50, self.loop) + for i in range(6): + if (DOFConfig := self.axes.get(str(i))): + name = str(DOFConfig['use']) + self.DOFInstance[name]= DOF(i, name, float(DOFConfig['scale_t']), float(DOFConfig['scale_p']), + name in [ "yaw", "heave", "roll", "pitch"]) # toggleable DOFs + else: + raise ValueError(f"Axis {i} not configured in params") + for i in range(10): + if (ButtonConfig := self.buttons.get(str(i))): + self.buttonsInstance.append(Button(i, str(ButtonConfig['use']))) + else: + raise ValueError(f"Button {i} not configured in params") + + def joystick_callback(self, msg: Joy): + for button in self.buttonsInstance: + button.update(msg.buttons[button.idx] == 1) + if button.rise_edge: + name, command = button.command + if name == "estop": + self.hard_stop() + return + elif name == "switch": + self.stateMachineMode ^= True + else: + if tar := self.DOFInstance.get(name): + tar.update(command, msg.axes[tar.idx]) + else: + raise ValueError(f"Button command {name} not recognised") + + def hard_stop(self): + for pub in self.wrench_pubs: + pub.publish(Float64(0.0)) + return + + def loop(self): + if not self.stateMachineMode: + for dof in self.DOFInstance.values(): + dof.publish() + else: + self.hard_stop() + +def main(): + rclpy.init() + node = Joystick_teleop() + rclpy.spin(node) + +if __name__ == "__main__": + main() diff --git a/mrobosub_teleop/resource/joystick_controls_b2a.yaml b/mrobosub_teleop/resource/joystick_controls_b2a.yaml new file mode 100644 index 00000000..611a3b5a --- /dev/null +++ b/mrobosub_teleop/resource/joystick_controls_b2a.yaml @@ -0,0 +1,57 @@ +# Tie axis numbers to commands +axes: + "0": + use: sway + scale_t: -1 + scale_p: 0 + "1": + use: surge + scale_t: 1 + scale_p: 0 + "2": + use: yaw + scale_t: -0.2 + scale_p: -1 + "3": + use: heave + scale_t: -0.2 + scale_p: -0.05 + "4": + use: roll + scale_t: -0.2 + scale_p: -0.1 + "5": + use: pitch + scale_t: 0.2 + scale_p: 0.1 + +# Tie button numbers to commands +buttons: + "0": + use: yaw.increase + scale: 30 + "1": + use: heave.decrease + scale: -0.2 + "2": + use: heave.increase + scale: 0.2 + "3": + use: yaw.decrease + scale: -30 + "4": + use: roll.toggle + "5": + use: pitch.toggle + "6": + use: yaw.toggle + "7": + use: heave.toggle + # Special commands + "8": + use: com.switch # between state machine mode + "9": + use: com.estop + +# Rate to send commands at (Hz) +rate: 50 diff --git a/mrobosub_teleop/setup.py b/mrobosub_teleop/setup.py index 0fbf3346..8ab91819 100644 --- a/mrobosub_teleop/setup.py +++ b/mrobosub_teleop/setup.py @@ -21,6 +21,9 @@ license="BSD-2.0", tests_require=["pytest"], entry_points={ - "console_scripts": ["all_dof_teleop = mrobosub_teleop.all_dof_teleop:run"], + "console_scripts": [ + "all_dof_teleop = mrobosub_teleop.all_dof_teleop:run", + "joystick_teleop" = "mrobosub_teleop.joystick_teleop:main", + ], }, ) From bb5670b697f1e6a3e7f944fb44cdc0aa7f742f3a Mon Sep 17 00:00:00 2001 From: Gsh <151632744+gaoshenghan1130@users.noreply.github.com> Date: Thu, 18 Sep 2025 09:19:46 -0400 Subject: [PATCH 2/7] Update mrobosub_teleop/launch/teleop_launch.xml, rosparam -> param in .launch file Co-authored-by: Henry LeCompte --- mrobosub_teleop/launch/teleop_launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mrobosub_teleop/launch/teleop_launch.xml b/mrobosub_teleop/launch/teleop_launch.xml index 20ec500f..d15035b3 100644 --- a/mrobosub_teleop/launch/teleop_launch.xml +++ b/mrobosub_teleop/launch/teleop_launch.xml @@ -1,6 +1,6 @@ - + From 696d2c8eac15472fec67d8af95bd4053ab1527df Mon Sep 17 00:00:00 2001 From: gsh Date: Thu, 18 Sep 2025 09:20:23 -0400 Subject: [PATCH 3/7] fix migration error according to pr check --- mrobosub_lib/setup.py | 2 -- mrobosub_teleop/launch/teleop_launch.xml | 4 ++-- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/mrobosub_lib/setup.py b/mrobosub_lib/setup.py index e718b637..650e8dd0 100644 --- a/mrobosub_lib/setup.py +++ b/mrobosub_lib/setup.py @@ -1,7 +1,5 @@ from setuptools import find_packages, setup -from mrobosub_teleop.mrobosub_teleop import joystick_teleop - package_name = "mrobosub_lib" setup( diff --git a/mrobosub_teleop/launch/teleop_launch.xml b/mrobosub_teleop/launch/teleop_launch.xml index 20ec500f..4ca8c433 100644 --- a/mrobosub_teleop/launch/teleop_launch.xml +++ b/mrobosub_teleop/launch/teleop_launch.xml @@ -1,6 +1,6 @@ - + - + From 048a398e6b755c969f2c08d47e5b2d4b50d54496 Mon Sep 17 00:00:00 2001 From: gsh Date: Thu, 18 Sep 2025 09:21:28 -0400 Subject: [PATCH 4/7] fix exec and delete required = true in teleop_launch.xml --- mrobosub_teleop/launch/teleop_launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mrobosub_teleop/launch/teleop_launch.xml b/mrobosub_teleop/launch/teleop_launch.xml index d15035b3..349793a3 100644 --- a/mrobosub_teleop/launch/teleop_launch.xml +++ b/mrobosub_teleop/launch/teleop_launch.xml @@ -1,5 +1,5 @@ - + From 0344b1fea88b15b6c2eaab9d8c83d9cacb1f8e01 Mon Sep 17 00:00:00 2001 From: gsh Date: Thu, 18 Sep 2025 09:23:14 -0400 Subject: [PATCH 5/7] type -> exec in teleop_launch --- mrobosub_teleop/launch/teleop_launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mrobosub_teleop/launch/teleop_launch.xml b/mrobosub_teleop/launch/teleop_launch.xml index 349793a3..5366cd85 100644 --- a/mrobosub_teleop/launch/teleop_launch.xml +++ b/mrobosub_teleop/launch/teleop_launch.xml @@ -2,5 +2,5 @@ - + From 7ba06429f680b70b440b84a2e9911232bde13c40 Mon Sep 17 00:00:00 2001 From: gsh Date: Sun, 28 Sep 2025 22:21:32 -0400 Subject: [PATCH 6/7] fix port error to ros2 --- .gitignore | 5 +- mrobosub_teleop/launch/teleop_launch.xml | 2 +- .../mrobosub_teleop/joystick_teleop.py | 167 ++++++++++++------ .../joystick_controls_b2a.yaml | 1 + mrobosub_teleop/setup.py | 2 +- 5 files changed, 119 insertions(+), 58 deletions(-) rename mrobosub_teleop/{resource => params}/joystick_controls_b2a.yaml (99%) diff --git a/.gitignore b/.gitignore index f710375c..7a0cc001 100644 --- a/.gitignore +++ b/.gitignore @@ -6,4 +6,7 @@ __pycache__ .mypy_cache *.bag rosdep/ -*.DS_Store \ No newline at end of file +*.DS_Store +build/ +install/ +log/ \ No newline at end of file diff --git a/mrobosub_teleop/launch/teleop_launch.xml b/mrobosub_teleop/launch/teleop_launch.xml index 5366cd85..84900665 100644 --- a/mrobosub_teleop/launch/teleop_launch.xml +++ b/mrobosub_teleop/launch/teleop_launch.xml @@ -1,6 +1,6 @@ - + diff --git a/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py index 7e4429c3..8938de55 100644 --- a/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py +++ b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py @@ -2,29 +2,36 @@ from enum import Enum from typing import List + +from matplotlib import axes import rclpy from sensor_msgs.msg import Joy from std_msgs.msg import Float64 from typing import Dict, Union, Tuple from mrobosub_lib import Node +import os +from ament_index_python.packages import get_package_share_directory + class ButtonCommand(int, Enum): INCREASE = 1 DECREASE = -1 TOGGLE = 0 + class Button: """Store commands tied to buttons""" - def __init__(self, idx : int, commandstr : str) -> None: + + def __init__(self, idx: int, commandstr: str) -> None: self.idx = idx self.commandstr = commandstr self._ispressed = False self._isrising_edge = False self.name, self._action = self.__split_command(commandstr) - + @staticmethod - def __split_command(comstr : str) -> Tuple[str, ButtonCommand]: - """ Split command string into (axis, command) """ + def __split_command(comstr: str) -> Tuple[str, ButtonCommand]: + """Split command string into (axis, command)""" if comstr.endswith(".increase"): return (comstr[:-9], ButtonCommand.INCREASE) elif comstr.endswith(".decrease"): @@ -36,61 +43,82 @@ def __split_command(comstr : str) -> Tuple[str, ButtonCommand]: return (comstr[4:], ButtonCommand.TOGGLE) else: raise ValueError(f"Button command {comstr} not recognised") - - def update(self, ispress : bool) -> None: + + def update(self, ispress: bool) -> None: self._isrising_edge = (not self.pressed) and ispress self._ispressed = ispress - + @property def rise_edge(self) -> bool: return self._isrising_edge - + @property def pressed(self) -> bool: return self._ispressed - + @property def command(self) -> Tuple[str, ButtonCommand]: return self.name, self._action - + + class DOF: """Store state of a degree of freedom""" + class DOFState(int, Enum): - POSE = 0 + POSE = 0 TWIST = 1 - - def __init__(self, id : int, name : str, scale_t : float, scale_p : float, istoggleable : bool) -> None: + + def __init__( + self, + id: int, + name: str, + scale_t: float, + scale_p: float, + istoggleable: bool, + node: Node, + ) -> None: self.idx = id self.name = name - + ## state variables - self.pos = 0.0 # current position + self.pos = 0.0 # current position self.state = DOF.DOFState.TWIST - self.scale = 0.0 # current scale - self.setPoint = 0.0 # target position - + self.scale = 0.0 # current scale + self.setPoint = 0.0 # target position + ## configs self.scale_t = scale_t self.scale_p = scale_p self.istogglable = istoggleable - self.twist_pub = self.create_publisher(Float64, f'/target_twist/{name}', qos_profile=1) - self.pose_pub = self.create_publisher(Float64, f'/target_pose/{name}', qos_profile=1) - self.pose_sub = self.create_subscriber(Float64, f'/pose/{name}', self.pose_callback) - - def pose_callback(self, msg : Float64) -> None: - self.pos = msg.data - - def update(self, command : ButtonCommand, scale : float) -> None: - self.state ^= int(not bool(abs(command) and self.istogglable)) # toggle if command is TOGGLE && istoggleable - self.scale = (self.scale_t * self.state + self.scale_p * (1 - self.state)) * scale # set scale based on state + self.twist_pub = node.create_publisher( + Float64, f"/target_twist/{name}", qos_profile=1 + ) + self.pose_pub = node.create_publisher( + Float64, f"/target_pose/{name}", qos_profile=1 + ) + self.pose_sub = node.create_subscription( + Float64, f"/pose/{name}", self.pose_callback, qos_profile=1 + ) + + def pose_callback(self, msg: Float64) -> None: + self.pos = msg.data + + def update(self, command: ButtonCommand, scale: float) -> None: + self.state ^= int( + not bool(abs(command)) and self.istogglable + ) # toggle if command is TOGGLE && istoggleable + self.scale = ( + self.scale_t * self.state + self.scale_p * (1 - self.state) + ) * scale # set scale based on state self.setPoint = self.pos + self.scale_p * scale - + def publish(self) -> None: if self.state == DOF.DOFState.POSE: - self.pose_pub.publish(Float64(self.setPoint)) + self.pose_pub.publish(Float64(data=self.setPoint)) elif self.state == DOF.DOFState.TWIST: - self.twist_pub.publish(Float64(self.scale)) + self.twist_pub.publish(Float64(data=self.scale)) + class Joystick_teleop(Node): """ @@ -119,33 +147,60 @@ class Joystick_teleop(Node): - /pose/roll - /pose/pitch """ - axes : Dict[str, Dict[str, Union[str,float]]] - buttons : Dict[str, Dict[str, str]] - - + + # axes : Dict[str, Dict[str, Union[str,float]]] + # buttons : Dict[str, Dict[str, str]] + def __init__(self) -> None: super().__init__("joystick_teleopn") - self.input_subscriber = self.create_subscriber(Joy, "/joy", self.joystick_callback) - self.wrench_pubs = [self.create_publisher(Float64, f'/output_wrench/{axis}', qos_profile=1) - for axis in ['sway', 'surge', 'heave', 'yaw', 'roll', 'pitch']] + # load params + + paramsfile = paramsfile = ( + "/home/ubuntu/ros2_ws/src/mrobosub_teleop/params/joystick_controls_b2a.yaml" + ) + # load directly from params folder, instead of ros2 param server (do not support recursive dict) + + with open(paramsfile, "r") as f: + import yaml + + params = yaml.safe_load(f) + self.axes = params["axes"] + self.buttons = params["buttons"] + + print(self.axes) + print(self.buttons) + + self.input_subscriber = self.create_subscription( + Joy, "/joy", self.joystick_callback, qos_profile=1 + ) + self.wrench_pubs = [ + self.create_publisher(Float64, f"/output_wrench/{axis}", qos_profile=1) + for axis in ["sway", "surge", "heave", "yaw", "roll", "pitch"] + ] # Globals - self.buttonsInstance : List[Button] = [] - self.DOFInstance : Dict[str, DOF] = {} + self.buttonsInstance: List[Button] = [] + self.DOFInstance: Dict[str, DOF] = {} self.stateMachineMode = False - self.timer = self.create_timer(1.0/50, self.loop) + self.timer = self.create_timer(1.0 / 50, self.loop) for i in range(6): - if (DOFConfig := self.axes.get(str(i))): - name = str(DOFConfig['use']) - self.DOFInstance[name]= DOF(i, name, float(DOFConfig['scale_t']), float(DOFConfig['scale_p']), - name in [ "yaw", "heave", "roll", "pitch"]) # toggleable DOFs + if DOFConfig := self.axes.get(str(i)): + name = str(DOFConfig["use"]) + self.DOFInstance[name] = DOF( + i, + name, + float(DOFConfig["scale_t"]), + float(DOFConfig["scale_p"]), + name in ["yaw", "heave", "roll", "pitch"], + self, + ) # toggleable DOFs else: raise ValueError(f"Axis {i} not configured in params") for i in range(10): - if (ButtonConfig := self.buttons.get(str(i))): - self.buttonsInstance.append(Button(i, str(ButtonConfig['use']))) + if ButtonConfig := self.buttons.get(str(i)): + self.buttonsInstance.append(Button(i, str(ButtonConfig["use"]))) else: raise ValueError(f"Button {i} not configured in params") - + def joystick_callback(self, msg: Joy): for button in self.buttonsInstance: button.update(msg.buttons[button.idx] == 1) @@ -156,28 +211,30 @@ def joystick_callback(self, msg: Joy): return elif name == "switch": self.stateMachineMode ^= True - else: + else: if tar := self.DOFInstance.get(name): tar.update(command, msg.axes[tar.idx]) - else: + else: raise ValueError(f"Button command {name} not recognised") - + def hard_stop(self): for pub in self.wrench_pubs: - pub.publish(Float64(0.0)) + pub.publish(Float64(data=0.0)) return - + def loop(self): if not self.stateMachineMode: for dof in self.DOFInstance.values(): - dof.publish() + dof.publish() else: self.hard_stop() - + + def main(): rclpy.init() node = Joystick_teleop() rclpy.spin(node) + if __name__ == "__main__": main() diff --git a/mrobosub_teleop/resource/joystick_controls_b2a.yaml b/mrobosub_teleop/params/joystick_controls_b2a.yaml similarity index 99% rename from mrobosub_teleop/resource/joystick_controls_b2a.yaml rename to mrobosub_teleop/params/joystick_controls_b2a.yaml index 611a3b5a..077ea3c9 100644 --- a/mrobosub_teleop/resource/joystick_controls_b2a.yaml +++ b/mrobosub_teleop/params/joystick_controls_b2a.yaml @@ -1,3 +1,4 @@ + # Tie axis numbers to commands axes: "0": diff --git a/mrobosub_teleop/setup.py b/mrobosub_teleop/setup.py index 8ab91819..f4c28b84 100644 --- a/mrobosub_teleop/setup.py +++ b/mrobosub_teleop/setup.py @@ -23,7 +23,7 @@ entry_points={ "console_scripts": [ "all_dof_teleop = mrobosub_teleop.all_dof_teleop:run", - "joystick_teleop" = "mrobosub_teleop.joystick_teleop:main", + "joystick_teleop = mrobosub_teleop.joystick_teleop:main", ], }, ) From c8fe3e51ed1b2bf2ce1ddcda8a4d087c815b72f0 Mon Sep 17 00:00:00 2001 From: gsh Date: Thu, 2 Oct 2025 23:31:50 -0400 Subject: [PATCH 7/7] estop logic change into: once set, never change --- .../mrobosub_teleop/joystick_teleop.py | 68 ++++++------------- 1 file changed, 20 insertions(+), 48 deletions(-) diff --git a/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py index 8938de55..a7b802e8 100644 --- a/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py +++ b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py @@ -2,16 +2,12 @@ from enum import Enum from typing import List - -from matplotlib import axes -import rclpy from sensor_msgs.msg import Joy from std_msgs.msg import Float64 -from typing import Dict, Union, Tuple +from typing import Dict, Tuple from mrobosub_lib import Node -import os -from ament_index_python.packages import get_package_share_directory - +import yaml +import rclpy class ButtonCommand(int, Enum): INCREASE = 1 @@ -91,26 +87,16 @@ def __init__( self.scale_p = scale_p self.istogglable = istoggleable - self.twist_pub = node.create_publisher( - Float64, f"/target_twist/{name}", qos_profile=1 - ) - self.pose_pub = node.create_publisher( - Float64, f"/target_pose/{name}", qos_profile=1 - ) - self.pose_sub = node.create_subscription( - Float64, f"/pose/{name}", self.pose_callback, qos_profile=1 - ) + self.twist_pub = node.create_publisher(Float64, f"/target_twist/{name}", qos_profile=1) + self.pose_pub = node.create_publisher(Float64, f"/target_pose/{name}", qos_profile=1) + self.pose_sub = node.create_subscription( Float64, f"/pose/{name}", self.pose_callback, qos_profile=1) def pose_callback(self, msg: Float64) -> None: self.pos = msg.data def update(self, command: ButtonCommand, scale: float) -> None: - self.state ^= int( - not bool(abs(command)) and self.istogglable - ) # toggle if command is TOGGLE && istoggleable - self.scale = ( - self.scale_t * self.state + self.scale_p * (1 - self.state) - ) * scale # set scale based on state + self.state ^= int(not bool(abs(command)) and self.istogglable) # toggle if command is TOGGLE && istoggleable + self.scale = ( self.scale_t * self.state + self.scale_p * (1 - self.state)) * scale # set scale based on state self.setPoint = self.pos + self.scale_p * scale def publish(self) -> None: @@ -155,14 +141,10 @@ def __init__(self) -> None: super().__init__("joystick_teleopn") # load params - paramsfile = paramsfile = ( - "/home/ubuntu/ros2_ws/src/mrobosub_teleop/params/joystick_controls_b2a.yaml" - ) + paramsfile = paramsfile = ("/home/ubuntu/ros2_ws/src/mrobosub_teleop/params/joystick_controls_b2a.yaml") # load directly from params folder, instead of ros2 param server (do not support recursive dict) with open(paramsfile, "r") as f: - import yaml - params = yaml.safe_load(f) self.axes = params["axes"] self.buttons = params["buttons"] @@ -170,17 +152,13 @@ def __init__(self) -> None: print(self.axes) print(self.buttons) - self.input_subscriber = self.create_subscription( - Joy, "/joy", self.joystick_callback, qos_profile=1 - ) - self.wrench_pubs = [ - self.create_publisher(Float64, f"/output_wrench/{axis}", qos_profile=1) - for axis in ["sway", "surge", "heave", "yaw", "roll", "pitch"] - ] + self.input_subscriber = self.create_subscription(Joy, "/joy", self.joystick_callback, qos_profile=1) + self.wrench_pubs = [self.create_publisher(Float64, f"/output_wrench/{axis}", qos_profile=1) for axis in ["sway", "surge", "heave", "yaw", "roll", "pitch"]] # Globals self.buttonsInstance: List[Button] = [] self.DOFInstance: Dict[str, DOF] = {} self.stateMachineMode = False + self._hard_stop = False self.timer = self.create_timer(1.0 / 50, self.loop) for i in range(6): if DOFConfig := self.axes.get(str(i)): @@ -204,18 +182,13 @@ def __init__(self) -> None: def joystick_callback(self, msg: Joy): for button in self.buttonsInstance: button.update(msg.buttons[button.idx] == 1) - if button.rise_edge: - name, command = button.command - if name == "estop": - self.hard_stop() - return - elif name == "switch": - self.stateMachineMode ^= True - else: - if tar := self.DOFInstance.get(name): - tar.update(command, msg.axes[tar.idx]) - else: - raise ValueError(f"Button command {name} not recognised") + if not button.rise_edge: + continue + name, command = button.command + self._hard_stop |= (name == "estop") + self.stateMachineMode ^= (name == "switch") + if name not in ("estop", "switch"): + (self.DOFInstance.get(name) or (_ for _ in ()).throw(ValueError(f"Button command {name} not recognised"))).update(command, msg.axes[self.DOFInstance[name].idx]) def hard_stop(self): for pub in self.wrench_pubs: @@ -223,7 +196,7 @@ def hard_stop(self): return def loop(self): - if not self.stateMachineMode: + if not (self._hard_stop and self.stateMachineMode): for dof in self.DOFInstance.values(): dof.publish() else: @@ -235,6 +208,5 @@ def main(): node = Joystick_teleop() rclpy.spin(node) - if __name__ == "__main__": main()