You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
83 lines
2.6 KiB
Python
83 lines
2.6 KiB
Python
#!/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, Point
|
|
from builtin_interfaces.msg import Time
|
|
from std_srvs.srv import Trigger
|
|
|
|
|
|
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)
|
|
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)
|
|
|
|
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()
|
|
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 performing")
|
|
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)
|
|
|
|
point_msg = Point()
|
|
point_msg.x = float(horisontal_pose)
|
|
point_msg.y = float(vertical_pose)
|
|
|
|
self._target_point_pub.publish(point_msg)
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|