FE emulator

.gitmodules

@@ -0,0 +1,6 @@
+[submodule "robot_controller"]
+	path = robot_controller
+	url = https://git.robofob.ru/sirius/robot_controller
+[submodule "unity_robot_controller/robot_controller"]
+	path = unity_robot_controller/robot_controller
+	url = https://git.robofob.ru/sirius/robot_controller

unity_robot_controller/fire_extinguisher_emulator.py

@@ -0,0 +1,71 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Point, Vector3
+from rclpy.executors import MultiThreadedExecutor
+from sensors_msgs.msg import Image, CameraInfo
+
+
+class FireExtinguisherEmulatorNode(Node):
+
+    def __init__(self):
+        super().__init__('fire_extinguisher_emulator')
+
+        self.declare_parameter('crosshair_alpha', 0.5)
+        self.declare_parameter('crosshair_speed', 1.) # pixel/sec
+
+        self.crosshair_alpha = self.get_parameter('crosshair_alpha').value
+        self.crosshair_speed = self.get_parameter('crosshair_speed').value
+
+        self.fire_ext_image_pub = self.create_publisher(Image, '~/fire_extinguisher_image', 10)
+        self.fire_ext_image_pub = self.create_publisher(Vector3, '~/fire_extinguisher_vector', 10)
+
+        self.target_x = 0.0
+        self.target_y = 0.0
+
+        self.current_x = 0.0
+        self.current_y = 0.0
+
+        self.target_timer = self.create_timer(0.1, self.target_timer_cb)
+        self.create_subscription(Point, '~/target_point', self.target_cb, 10)
+        self.create_subscription(CameraInfo, 'camera_info', self.cam_info_cb, 10)
+        self.create_subscription(Image, 'raw_image', self.raw_image_cb, 10)
+
+
+    def target_cb(self, msg):
+        self.target_x = min( max(-1., msg.x), 1.)
+        self.target_y = min( max(-1., msg.y), 1.)
+
+    def cam_info_cb(self, msg):
+        # save info and shutdown that subscription
+
+
+    def target_timer_cb(self):
+        if self.current_x != self.target_x or self.current_y != self.target_x:
+            # do target smooth movement to target assumint crosshair_speed limits
+
+    def raw_image_cb(self, raw_msg):
+        # draw some kind of simple crosshair from current x and y
+
+        # using saved info calculate 3d unit vector from camera frame and publish it
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = FireExtinguisherEmulatorNode()
+    executor = MultiThreadedExecutor(num_threads=4)
+    executor.add_node(node)
+    try:
+        rclpy.spin(node, executor)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+
+

unity_robot_controller/robot_controller

@@ -0,0 +1 @@
+Subproject commit eb6ec135a5a3f4ed8361e75f1f6662b7a19b81ec

unity_robot_controller/unity_robot_controller.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from unity_robot_controller.robot_controller.robot_controller import RobotController
+from geometry_msgs.msg import Twist
+
+
+class UnityRobotController(RobotController):
+
+    def __init__(self, node):
+        super().__init__()
+
+        self._node = node
+
+        self._node.declare_parameter('max_linear_speed', 0.5)
+        self._node.declare_parameter('max_angular_speed', 1.0)
+
+        self._node.declare_parameter('fire_ext_pixel_speed', 1.0)
+
+        self._lin_max = self._node.get_parameter('max_linear_speed').value
+        self._ang_max = self._node.get_parameter('max_angular_speed').value
+
+        self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10)
+
+
+    def send_speed_cmd(self, v, w):
+        v, w = super(UnityRobotController, self).send_speed_cmd(v, w)
+
+        twist_msg = Twist()
+        twist_msg.linear.x = v * self._lin_max
+        twist_msg.angular.z = w * self._ang_max
+
+        self._twist_pub.publish(twist_msg)
+
+    def send_fire_ext_burst_cmd(self):
+        result = super(UnityRobotController, self).send_fire_ext_burst_cmd()
+
+
+
+
+
+
+