diff --git a/.DS_Store b/.DS_Store deleted file mode 100644 index 3b9732ce..00000000 Binary files a/.DS_Store and /dev/null differ diff --git a/.gitignore b/.gitignore index b195a5d4..7a0cc001 100644 --- a/.gitignore +++ b/.gitignore @@ -5,4 +5,8 @@ __pycache__ .mypy_cache *.bag -rosdep/ \ No newline at end of file +rosdep/ +*.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 new file mode 100644 index 00000000..84900665 --- /dev/null +++ b/mrobosub_teleop/launch/teleop_launch.xml @@ -0,0 +1,6 @@ + + + + + + diff --git a/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py new file mode 100644 index 00000000..a7b802e8 --- /dev/null +++ b/mrobosub_teleop/mrobosub_teleop/joystick_teleop.py @@ -0,0 +1,212 @@ +#!/usr/bin/env python + +from enum import Enum +from typing import List +from sensor_msgs.msg import Joy +from std_msgs.msg import Float64 +from typing import Dict, Tuple +from mrobosub_lib import Node +import yaml +import rclpy + +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, + node: Node, + ) -> 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 = 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(data=self.setPoint)) + elif self.state == DOF.DOFState.TWIST: + self.twist_pub.publish(Float64(data=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") + # 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: + 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.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)): + 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"]))) + 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 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: + pub.publish(Float64(data=0.0)) + return + + def loop(self): + if not (self._hard_stop and 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/params/joystick_controls_b2a.yaml b/mrobosub_teleop/params/joystick_controls_b2a.yaml new file mode 100644 index 00000000..077ea3c9 --- /dev/null +++ b/mrobosub_teleop/params/joystick_controls_b2a.yaml @@ -0,0 +1,58 @@ + +# 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 01c977f6..f4c28b84 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:main"], + "console_scripts": [ + "all_dof_teleop = mrobosub_teleop.all_dof_teleop:run", + "joystick_teleop = mrobosub_teleop.joystick_teleop:main", + ], }, )