added burst interface

config/T-29.yaml

@@ -4,6 +4,8 @@ arctic_bot/joy_control:
     speed_y_axis: 3
     speed_throttle_btn: 7
 
+    fe_burst_btn: 2
+
     crosshair_plus_x_btn: -1
     crosshair_minus_x_btn: -1
     crosshair_plus_y_btn: -1

unity_robot_controller/fire_extinguisher_emulator.py

@@ -8,7 +8,7 @@ from sensor_msgs.msg import Image, CameraInfo
 from cv_bridge import CvBridge
 import cv2
 import numpy as np
-
+from std_srvs.srv import Trigger
 
 class FireExtinguisherEmulatorNode(Node):
 
@@ -52,6 +52,19 @@ class FireExtinguisherEmulatorNode(Node):
         self.create_subscription(CameraInfo, 'camera_info', self.cam_info_cb, 10)
         self.create_subscription(Image, 'raw_image', self.raw_image_cb, 10)
 
+        self.create_service(Trigger, 'fe_burst', self.fe_burst_cb)
+
+    def fe_burst_cb(self, req, res):
+        '''
+        TODO successful burst logic
+        '''
+
+        res.success = False
+        res.message = "0" # add ID of source of fire if success
+
+        return res
+
+
     def target_cb(self, msg):
         """Received target point in normalized coordinates [-1, 1]"""
         self.target_x = min(max(-1.0, msg.x), 1.0)

unity_robot_controller/joy_control_node.py

@@ -22,6 +22,8 @@ class JoyControlNode(Node):
         self.declare_parameter('crosshair_plus_y_btn', 3)
         self.declare_parameter('crosshair_minus_y_btn', 4)
 
+        self.declare_parameter('fe_burst_btn', 5)
+
         self.declare_parameter('crosshair_step', 0.01)
 
         # alternative via axis
@@ -37,6 +39,8 @@ class JoyControlNode(Node):
         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.fe_burst_btn = self.get_parameter('fe_burst_btn').value
+
         self.crosshair_x_axis = self.get_parameter('crosshair_x_axis').value
         self.crosshair_y_axis = self.get_parameter('crosshair_y_axis').value
 
@@ -118,6 +122,10 @@ class JoyControlNode(Node):
             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!")
 
+        # fire extinguisher burst
+        if self.was_button_pressed(msg, self.fe_burst_btn):
+            self.URC.send_fire_ext_burst_cmd()
+
 
 
 def main(args=None):

unity_robot_controller/unity_robot_controller.py

@@ -4,6 +4,8 @@ import rclpy
 from rclpy.node import Node
 from unity_robot_controller.robot_controller.robot_controller import RobotController
 from geometry_msgs.msg import Twist, Point
+from builtin_interfaces.msg import Time
+from std_srvs.srv import Trigger
 
 
 class UnityRobotController(RobotController):
@@ -24,6 +26,18 @@ class UnityRobotController(RobotController):
         self._twist_pub = self._node.create_publisher(Twist, 'cmd_vel', 10)
         self._target_point_pub = self._node.create_publisher(Point, 'target_point', 10)
 
+        self.fe_burst_srv = self._node.create_client(Trigger, 'fe_burst')
+
+        self.start_time = self.get_time_seconds() # TODO save it when first cmd is given
+
+        # TODO create collision subscriber from Unity
+
+    def get_time_seconds(self):
+        current_time = self._node.get_clock().now()
+        return current_time.nanoseconds / 1e9
+
+    def get_relative_time(self):
+        return self.get_time_seconds() - self.start_time
 
     def send_speed_cmd(self, v, w):
         v, w = super(UnityRobotController, self).send_speed_cmd(v, w)
@@ -36,6 +50,20 @@ class UnityRobotController(RobotController):
 
     def send_fire_ext_burst_cmd(self):
         result = super(UnityRobotController, self).send_fire_ext_burst_cmd()
+        if not result:
+            self._node.get_logger().error("Fire extinguisher is out of fuel!")
+            return False
+
+        self._node.get_logger().error("Fire extingusher burst is sperforming")
+        future = self.fe_burst_srv.call_async(Trigger.Request())
+        future.add_done_callback(self._fe_burst_done_cb)
+        return True
+
+    def _fe_burst_done_cb(self, future):
+        result = future.result()
+        if result.success:
+            self._register_exted_sof(self.get_relative_time(), {'id': success.message})
+
 
     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)