diff --git a/unity_robot_controller/joy_control_node.py b/unity_robot_controller/joy_control_node.py new file mode 100644 index 0000000..ae76d74 --- /dev/null +++ b/unity_robot_controller/joy_control_node.py @@ -0,0 +1,134 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from unity_robot_controller.unity_robot_controller import UnityRobotController +from sensor_msgs.msg import Joy + +class JoyControlNode(Node): + + def __init__(self): + + super().__init__('joy_control') + + self.URC = UnityRobotController(self) + + self.declare_parameter('speed_x_axis', 0) + self.declare_parameter('speed_y_axis', 1) + self.declare_parameter('speed_throttle_btn', 0) + + self.declare_parameter('crosshair_plus_x_btn', 1) + self.declare_parameter('crosshair_minus_x_btn', 2) + self.declare_parameter('crosshair_plus_y_btn', 3) + self.declare_parameter('crosshair_minus_y_btn', 4) + + self.declare_parameter('crosshair_step', 0.01) + + # alternative via axis + self.declare_parameter('crosshair_x_axis', -1) + self.declare_parameter('crosshair_y_axis', -1) + + self.speed_x_axis = self.get_parameter('speed_x_axis').value + self.speed_y_axis = self.get_parameter('speed_y_axis').value + self.speed_throttle_btn = self.get_parameter('speed_throttle_btn').value + + self.crosshair_plus_x_btn = self.get_parameter('crosshair_plus_x_btn').value + self.crosshair_minus_x_btn = self.get_parameter('crosshair_minus_x_btn').value + self.crosshair_plus_y_btn = self.get_parameter('crosshair_plus_y_btn').value + self.crosshair_minus_y_btn = self.get_parameter('crosshair_minus_y_btn').value + + self.crosshair_x_axis = self.get_parameter('crosshair_x_axis').value + self.crosshair_y_axis = self.get_parameter('crosshair_y_axis').value + + self.crosshair_current_x = 0.0 + self.crosshair_current_y = 0.0 + self.crosshair_step = self.get_parameter('crosshair_step').value + + self.prev_button_states = {} + self.prev_axes_states = {} + + self.create_subscription(Joy, 'joy', self.joy_cb, 10) + + def is_button_pressed(self, joy_msg, button): + if button == -1: + return False + if button >= len(joy_msg.buttons) : + self.get_logger().error(f"No button {button} in joy msg!") + return False + return bool(joy_msg.buttons[button]) + + def was_button_pressed(self, joy_msg, button): + if button == -1: + return False + if button >= len(joy_msg.buttons) : + self.get_logger().error(f"No button {button} in joy msg!") + if not button in self.prev_button_states: + self.prev_button_states[button] = joy_msg.buttons[button] + return False + if joy_msg.buttons[button] and joy_msg.buttons[button] != self.prev_button_states[button]: + self.prev_button_states[button] = joy_msg.buttons[button] + return True + self.prev_button_states[button] = joy_msg.buttons[button] + return False + + def joy_cb(self, msg): + # speed control + if self.is_button_pressed(msg, self.speed_throttle_btn): + + if self.speed_x_axis < len(msg.axes) and self.speed_y_axis < len(msg.axes): + self.URC.send_speed_cmd(msg.axes[self.speed_x_axis], msg.axes[self.speed_y_axis]) + else: + self.get_logger().error(f"Check axis for speed in joy control! Some of {self.speed_x_axis} an {self.speed_y_axis} values is wrong!") + + + # crosshair control + upd_pose = True + if self.was_button_pressed(msg, self.crosshair_plus_x_btn): + self.crosshair_current_x += self.crosshair_step + elif self.was_button_pressed(msg, self.crosshair_minus_x_btn): + self.crosshair_current_x -= self.crosshair_step + if self.was_button_pressed(msg, self.crosshair_plus_y_btn): + self.crosshair_current_y += self.crosshair_step + elif self.was_button_pressed(msg, self.crosshair_minus_y_btn): + self.crosshair_current_y -= self.crosshair_step + else: + upd_pose = False + if upd_pose: + self.URC.send_fire_ext_pose_cmd(self.crosshair_current_x, self.crosshair_current_y) + + # alternative + if self.crosshair_x_axis != -1 and self.crosshair_y_axis != -1: + if self.crosshair_x_axis < len(msg.axes) and self.crosshair_y_axis < len(msg.axes): + + if self.crosshair_x_axis in self.prev_axes_states and self.crosshair_y_axis in self.prev_axes_states: + + if msg.axes[self.crosshair_x_axis] != self.prev_axes_states[self.crosshair_x_axis] or msg.axes[self.crosshair_y_axis] != self.prev_axes_states[self.crosshair_y_axis]: + + self.crosshair_current_x += self.crosshair_step * -msg.axes[self.crosshair_x_axis] + self.crosshair_current_y += self.crosshair_step * msg.axes[self.crosshair_y_axis] + + self.crosshair_current_x = max(min(1, self.crosshair_current_x), -1) + self.crosshair_current_y = max(min(1, self.crosshair_current_y), -1) + + self.URC.send_fire_ext_pose_cmd(self.crosshair_current_x, self.crosshair_current_y) + + self.prev_axes_states[self.crosshair_x_axis] = msg.axes[self.crosshair_x_axis] + self.prev_axes_states[self.crosshair_y_axis] = msg.axes[self.crosshair_y_axis] + + else: + self.get_logger().error(f"Check axis for crosshair in joy control! Some of {self.speed_x_axis} an {self.speed_y_axis} values is wrong!") + + + +def main(args=None): + rclpy.init(args=args) + node = JoyControlNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() + + diff --git a/unity_robot_controller/robot_controller b/unity_robot_controller/robot_controller index eb6ec13..9f65622 160000 --- a/unity_robot_controller/robot_controller +++ b/unity_robot_controller/robot_controller @@ -1 +1 @@ -Subproject commit eb6ec135a5a3f4ed8361e75f1f6662b7a19b81ec +Subproject commit 9f65622b633900e07ee1f9dbe9e33a5add3167ae diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index 06c3757..dc9b667 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/unity_robot_controller/unity_robot_controller.py @@ -3,7 +3,7 @@ import rclpy from rclpy.node import Node from unity_robot_controller.robot_controller.robot_controller import RobotController -from geometry_msgs.msg import Twist +from geometry_msgs.msg import Twist, Point class UnityRobotController(RobotController): @@ -22,6 +22,7 @@ class UnityRobotController(RobotController): self._ang_max = self._node.get_parameter('max_angular_speed').value self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10) + self._target_point_pub = self._node.create_publisher(Point, '/fire_extinguisher_emulator/target_point', 10) def send_speed_cmd(self, v, w): @@ -36,6 +37,15 @@ class UnityRobotController(RobotController): def send_fire_ext_burst_cmd(self): result = super(UnityRobotController, self).send_fire_ext_burst_cmd() + def send_fire_ext_pose_cmd(self, horisontal_pose, vertical_pose): + horisontal_pose, vertical_pose = super(UnityRobotController, self).send_fire_ext_pose_cmd(horisontal_pose, vertical_pose) + + point_msg = Point() + point_msg.x = float(horisontal_pose) + point_msg.y = float(vertical_pose) + + self._target_point_pub.publish(point_msg) +