joy stuff tested alone

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()
+
+

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)
+