diff --git a/config/T-29.yaml b/config/T-29.yaml index 92756c0..435739f 100644 --- a/config/T-29.yaml +++ b/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 diff --git a/unity_robot_controller/fire_extinguisher_emulator.py b/unity_robot_controller/fire_extinguisher_emulator.py index 8fe928e..bd504a6 100644 --- a/unity_robot_controller/fire_extinguisher_emulator.py +++ b/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) diff --git a/unity_robot_controller/joy_control_node.py b/unity_robot_controller/joy_control_node.py index ae76d74..02b444d 100644 --- a/unity_robot_controller/joy_control_node.py +++ b/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): diff --git a/unity_robot_controller/unity_robot_controller.py b/unity_robot_controller/unity_robot_controller.py index b23d39f..c2d0b90 100644 --- a/unity_robot_controller/unity_robot_controller.py +++ b/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)