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.
108 lines
3.8 KiB
Python
108 lines
3.8 KiB
Python
#!/usr/bin/env python3
|
|
|
|
import rclpy
|
|
from rclpy.node import Node
|
|
from wave_rover_controller.robot_controller.robot_controller import RobotController
|
|
from serial import Serial
|
|
from serial import SerialException
|
|
import json
|
|
import time
|
|
from wave_rover_controller.fan_gpio import GPIOController
|
|
|
|
class WaveRoverController(RobotController):
|
|
|
|
def __init__(self, node):
|
|
super().__init__()
|
|
|
|
self._node = node
|
|
|
|
self._node.declare_parameter('serial_port', '/dev/ttyUSB0')
|
|
self._node.declare_parameter('baudrate', 115200)
|
|
|
|
self._node.declare_parameter('fan_pin', 13)
|
|
self._node.declare_parameter('fan_time', 5.)
|
|
|
|
self._serial_port = self._node.get_parameter('serial_port').value
|
|
self._baudrate = self._node.get_parameter('baudrate').value
|
|
fan_pin = self._node.get_parameter('fan_pin').value
|
|
self._gpio_controller = GPIOController(fan_pin)
|
|
self._fan_time = self._node.get_parameter('fan_time').value
|
|
self._fe_burst_timer = None
|
|
|
|
wheel_max = 0.5 # popugais
|
|
self._linear_max = wheel_max
|
|
self._base = 0.2 # m
|
|
self._angular_max = 2 * wheel_max / self._base #
|
|
|
|
|
|
# Serial подключение
|
|
self._serial_conn: Optional[Serial] = None
|
|
self._connect_serial()
|
|
|
|
def _connect_serial(self):
|
|
try:
|
|
self._serial_conn = Serial(
|
|
port=self._serial_port, baudrate=self._baudrate, timeout=1.0
|
|
)
|
|
time.sleep(0.5)
|
|
self._node.get_logger().info(f'Подключено к {self._serial_port}')
|
|
except SerialException as e:
|
|
self._node.get_logger().error(f'Ошибка serial: {e}')
|
|
self._serial_conn = None
|
|
|
|
|
|
def _send_json_command(self, command: dict):
|
|
if not self._serial_conn or not self._serial_conn.is_open:
|
|
self._connect_serial()
|
|
return
|
|
try:
|
|
json_str = json.dumps(command)
|
|
self._serial_conn.write(json_str.encode() + b'\n')
|
|
self._node.get_logger().debug(f'Отправлено: {json_str}')
|
|
except SerialException as e:
|
|
self._node.get_logger().error(f'Ошибка отправки: {e}')
|
|
self._serial_conn.close()
|
|
|
|
|
|
def send_speed_cmd(self, v, w):
|
|
v, w = super(WaveRoverController, self).send_speed_cmd(v, w)
|
|
|
|
vp = v * self._linear_max
|
|
wp = w * self._angular_max
|
|
|
|
left_speed, right_speed = self._twist_to_wheel_speeds(vp, wp)
|
|
|
|
command = {"T": 1, "L": left_speed, "R": right_speed}
|
|
self._send_json_command(command)
|
|
|
|
|
|
def send_fire_ext_burst_cmd(self):
|
|
result = super(WaveRoverController, self).send_fire_ext_burst_cmd()
|
|
if not result:
|
|
self._node.get_logger().error("Fire extinguisher is out of fuel!")
|
|
return False
|
|
|
|
if self._fe_burst_timer is None:
|
|
self._fe_burst_timer = self._node.create_timer(self._fan_time, self._fe_burst_timer_cb)
|
|
self._gpio_controller.enable()
|
|
self._node.get_logger().info("Fire extingusher burst is performing")
|
|
else:
|
|
self._node.get_logger().error("Fire extingusher burst is already performing")
|
|
|
|
def _fe_burst_timer_cb(self):
|
|
self._node.destroy_timer(self._fe_burst_timer)
|
|
self._fe_burst_timer = None
|
|
self._gpio_controller.disable()
|
|
|
|
def _twist_to_wheel_speeds(self, linear_x: float, angular_z: float):
|
|
"""Twist -> скорости колес для дифференциального привода"""
|
|
wheel_base = 0.2 # м
|
|
|
|
left_speed = (linear_x - (wheel_base * angular_z) / 2) #/ self.max_linear_speed
|
|
right_speed = (linear_x + (wheel_base * angular_z) / 2) #/ self.max_linear_speed
|
|
|
|
left_speed = max(-1.0, min(1.0, left_speed))
|
|
right_speed = max(-1.0, min(1.0, right_speed))
|
|
|
|
return left_speed, right_speed
|