383 lines
13 KiB
Python
383 lines
13 KiB
Python
#!/usr/bin/env python3
|
|
import argparse
|
|
import json
|
|
import math
|
|
import re
|
|
import time
|
|
from collections import deque
|
|
from datetime import datetime
|
|
from pathlib import Path
|
|
|
|
|
|
ROWS = 5
|
|
COLUMNS = 5
|
|
SPACING = 1.0
|
|
MAX_SPEED = 0.35
|
|
MAX_ANGULAR = 0.60
|
|
STOP_DISTANCE = 0.65
|
|
TOLERANCE = 0.07
|
|
SCAN_TOPIC = "/RMC2/scan_front"
|
|
BLOCKED = set()
|
|
|
|
|
|
def clamp(value, low, high):
|
|
return max(low, min(high, value))
|
|
|
|
|
|
def wrap(angle):
|
|
return math.atan2(math.sin(angle), math.cos(angle))
|
|
|
|
|
|
def yaw(quaternion):
|
|
return math.atan2(
|
|
2 * (quaternion.w * quaternion.z + quaternion.x * quaternion.y),
|
|
1 - 2 * (quaternion.y**2 + quaternion.z**2),
|
|
)
|
|
|
|
|
|
def compose(a, b):
|
|
cosine, sine = math.cos(a[2]), math.sin(a[2])
|
|
return (
|
|
a[0] + cosine * b[0] - sine * b[1],
|
|
a[1] + sine * b[0] + cosine * b[1],
|
|
wrap(a[2] + b[2]),
|
|
)
|
|
|
|
|
|
def inverse(pose):
|
|
cosine, sine = math.cos(pose[2]), math.sin(pose[2])
|
|
return (
|
|
-cosine * pose[0] - sine * pose[1],
|
|
sine * pose[0] - cosine * pose[1],
|
|
-pose[2],
|
|
)
|
|
|
|
|
|
def marker_pose(marker):
|
|
row, column = divmod(marker, COLUMNS)
|
|
return -row * SPACING, column * SPACING, 0.0
|
|
|
|
|
|
def shortest_path(start, goal, forbidden=()):
|
|
if start in BLOCKED or goal in BLOCKED:
|
|
raise RuntimeError("невозможная цель")
|
|
|
|
forbidden = {frozenset(edge) for edge in forbidden}
|
|
search = deque([start])
|
|
parent = {start: None}
|
|
|
|
while search:
|
|
current = search.popleft()
|
|
if current == goal:
|
|
break
|
|
row, column = divmod(current, COLUMNS)
|
|
for next_row, next_column in (
|
|
(row - 1, column),
|
|
(row + 1, column),
|
|
(row, column - 1),
|
|
(row, column + 1),
|
|
):
|
|
if not (0 <= next_row < ROWS and 0 <= next_column < COLUMNS):
|
|
continue
|
|
candidate = next_row * COLUMNS + next_column
|
|
edge = frozenset((current, candidate))
|
|
if candidate in BLOCKED or candidate in parent or edge in forbidden:
|
|
continue
|
|
parent[candidate] = current
|
|
search.append(candidate)
|
|
|
|
if goal not in parent:
|
|
raise RuntimeError(f"нет маршрута {start} к {goal}")
|
|
|
|
route = []
|
|
current = goal
|
|
while current is not None:
|
|
route.append(current)
|
|
current = parent[current]
|
|
return list(reversed(route))
|
|
|
|
|
|
def arguments():
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument("--target", type=int, required=True)
|
|
return parser.parse_args().target
|
|
|
|
|
|
def main():
|
|
target_id = arguments()
|
|
|
|
import rclpy
|
|
from geometry_msgs.msg import Twist
|
|
from nav_msgs.msg import Odometry
|
|
from rclpy.node import Node
|
|
from rclpy.qos import (
|
|
DurabilityPolicy,
|
|
QoSProfile,
|
|
ReliabilityPolicy,
|
|
qos_profile_sensor_data,
|
|
)
|
|
from rclpy.signals import SignalHandlerOptions
|
|
from rclpy.time import Time
|
|
from sensor_msgs.msg import LaserScan
|
|
from std_msgs.msg import String
|
|
from tf2_msgs.msg import TFMessage
|
|
from tf2_ros import Buffer, TransformException, TransformListener
|
|
|
|
class Rover(Node):
|
|
def __init__(self):
|
|
super().__init__("mvch_module_b")
|
|
self.cmd = self.create_publisher(Twist, "/RMC2/cmd_vel", 10)
|
|
self.create_subscription(
|
|
Odometry, "/RMC2/odometry", self.on_odom, qos_profile_sensor_data
|
|
)
|
|
self.create_subscription(
|
|
LaserScan, SCAN_TOPIC, self.on_scan, qos_profile_sensor_data
|
|
)
|
|
self.create_subscription(
|
|
String, "/RMC2/camera_bottom/aruco_id", self.on_marker, 10
|
|
)
|
|
self.create_subscription(String, "/RMC2/aruco_id", self.on_marker, 10)
|
|
|
|
self.tf_buffer = Buffer()
|
|
self.tf_listener = TransformListener(self.tf_buffer, self)
|
|
tf_qos = QoSProfile(
|
|
depth=100,
|
|
reliability=ReliabilityPolicy.BEST_EFFORT,
|
|
durability=DurabilityPolicy.VOLATILE,
|
|
)
|
|
static_qos = QoSProfile(
|
|
depth=100,
|
|
reliability=ReliabilityPolicy.RELIABLE,
|
|
durability=DurabilityPolicy.TRANSIENT_LOCAL,
|
|
)
|
|
self.create_subscription(TFMessage, "/RMC2/tf", self.on_tf, tf_qos)
|
|
self.create_subscription(
|
|
TFMessage, "/RMC2/tf_static", self.on_tf_static, static_qos
|
|
)
|
|
|
|
self.odom = None
|
|
self.odom_frame = "RMC2/odom"
|
|
self.map_from_odom = None
|
|
self.pose = None
|
|
self.marker = None
|
|
self.marker_seen = 0.0
|
|
self.marker_error = math.inf
|
|
self.front = math.inf
|
|
self.current = None
|
|
self.leg = "outbound"
|
|
|
|
reports = Path(__file__).parent / "reports"
|
|
reports.mkdir(exist_ok=True)
|
|
self.log_path = reports / f"module_b_{datetime.now():%Y%m%d_%H%M%S}.jsonl"
|
|
self.log_file = self.log_path.open("a", encoding="utf-8", buffering=1)
|
|
|
|
def log(self, event, **data):
|
|
item = {
|
|
"time": datetime.now().isoformat(timespec="milliseconds"),
|
|
"event": event,
|
|
"leg": self.leg,
|
|
**data,
|
|
}
|
|
line = json.dumps(item, ensure_ascii=False)
|
|
print(line, flush=True)
|
|
self.log_file.write(line + "\n")
|
|
|
|
def on_tf(self, message):
|
|
for transform in message.transforms:
|
|
self.tf_buffer.set_transform(transform, "RMC2 tf")
|
|
|
|
def on_tf_static(self, message):
|
|
for transform in message.transforms:
|
|
self.tf_buffer.set_transform_static(transform, "RMC2 tf_static")
|
|
|
|
def on_odom(self, message):
|
|
frame = message.header.frame_id.strip("/")
|
|
if frame:
|
|
self.odom_frame = frame
|
|
pose = message.pose.pose
|
|
self.odom = pose.position.x, pose.position.y, yaw(pose.orientation)
|
|
|
|
def on_marker(self, message):
|
|
match = re.search(r"\d+", message.data)
|
|
if not match:
|
|
return
|
|
marker = int(match.group())
|
|
if not 0 <= marker < ROWS * COLUMNS:
|
|
return
|
|
if marker != self.marker:
|
|
self.log("MARKER", marker=marker)
|
|
self.marker = marker
|
|
self.marker_seen = time.monotonic()
|
|
|
|
def on_scan(self, message):
|
|
front = []
|
|
for index, value in enumerate(message.ranges):
|
|
angle = message.angle_min + index * message.angle_increment
|
|
if abs(angle) <= math.radians(25):
|
|
if math.isfinite(value) and message.range_min <= value <= message.range_max:
|
|
front.append(value)
|
|
self.front = min(front, default=math.inf)
|
|
|
|
def localize(self):
|
|
if self.odom is None or self.marker is None:
|
|
return
|
|
if time.monotonic() - self.marker_seen > 0.8:
|
|
return
|
|
try:
|
|
transform = self.tf_buffer.lookup_transform(
|
|
self.odom_frame, f"aruco_{self.marker}", Time()
|
|
).transform
|
|
except TransformException:
|
|
return
|
|
|
|
odom_marker = (
|
|
transform.translation.x,
|
|
transform.translation.y,
|
|
yaw(transform.rotation),
|
|
)
|
|
self.map_from_odom = compose(
|
|
marker_pose(self.marker), inverse(odom_marker)
|
|
)
|
|
self.pose = compose(self.map_from_odom, self.odom)
|
|
self.marker_error = math.hypot(
|
|
odom_marker[0] - self.odom[0], odom_marker[1] - self.odom[1]
|
|
)
|
|
|
|
def update(self):
|
|
rclpy.spin_once(self, timeout_sec=0.05)
|
|
self.localize()
|
|
if self.map_from_odom is not None and self.odom is not None:
|
|
self.pose = compose(self.map_from_odom, self.odom)
|
|
|
|
def stop(self):
|
|
self.cmd.publish(Twist())
|
|
|
|
def send(self, linear, angular):
|
|
command = Twist()
|
|
command.linear.x = float(linear)
|
|
command.angular.z = float(angular)
|
|
self.cmd.publish(command)
|
|
|
|
def wait_start(self):
|
|
print("Жду ArUco под ровером. Старт определится автоматически.")
|
|
while rclpy.ok():
|
|
self.update()
|
|
self.stop()
|
|
if self.pose is not None and self.marker_error <= TOLERANCE:
|
|
self.current = self.marker
|
|
self.log("START_FOUND", marker=self.current)
|
|
return self.current
|
|
|
|
def reached(self, marker):
|
|
return (
|
|
self.marker == marker
|
|
and time.monotonic() - self.marker_seen < 0.8
|
|
and self.marker_error <= TOLERANCE
|
|
)
|
|
|
|
def drive_to(self, marker, check_obstacle=True):
|
|
target = marker_pose(marker)
|
|
while rclpy.ok():
|
|
self.update()
|
|
if self.pose is None:
|
|
self.stop()
|
|
continue
|
|
if self.reached(marker):
|
|
self.stop()
|
|
return True
|
|
|
|
dx = target[0] - self.pose[0]
|
|
dy = target[1] - self.pose[1]
|
|
distance = math.hypot(dx, dy)
|
|
error = wrap(math.atan2(dy, dx) - self.pose[2])
|
|
linear = 0.0 if abs(error) > 0.16 else min(MAX_SPEED, max(0.04, 0.7 * distance))
|
|
angular = clamp(1.4 * error, -MAX_ANGULAR, MAX_ANGULAR)
|
|
|
|
if check_obstacle and linear > 0 and self.front < STOP_DISTANCE:
|
|
self.stop()
|
|
return False
|
|
self.send(linear, angular)
|
|
|
|
def follow(self, goal, forbidden):
|
|
route = shortest_path(self.current, goal, forbidden)
|
|
index = 1
|
|
while index < len(route):
|
|
next_marker = route[index]
|
|
if self.drive_to(next_marker):
|
|
self.current = next_marker
|
|
self.log("WAYPOINT", marker=self.current)
|
|
index += 1
|
|
continue
|
|
|
|
edge = frozenset((self.current, next_marker))
|
|
forbidden.add(edge)
|
|
self.log(
|
|
"OBSTACLE",
|
|
edge=sorted(edge),
|
|
distance=round(self.front, 3),
|
|
)
|
|
print(f"Препятствие перед {next_marker}. Возвращаюсь к {self.current}.")
|
|
self.drive_to(self.current, check_obstacle=False)
|
|
route = shortest_path(self.current, goal, forbidden)
|
|
index = 1
|
|
show_route(self, "НОВЫЙ МАРШРУТ", route, "ROUTE_REPLANNED")
|
|
return route
|
|
|
|
def show_route(rover, title, route, event):
|
|
print(f"{title}: {' путь: '.join(map(str, route))}")
|
|
rover.log(event, markers=route)
|
|
|
|
rclpy.init(signal_handler_options=SignalHandlerOptions.NO)
|
|
rover = Rover()
|
|
exit_code = 0
|
|
try:
|
|
start = rover.wait_start()
|
|
route_there = shortest_path(start, target_id)
|
|
route_back = shortest_path(target_id, start)
|
|
|
|
print()
|
|
show_route(rover, "МАРШРУТ К ЦЕЛИ", route_there, "ROUTE_TO_TARGET")
|
|
show_route(rover, "МАРШРУТ НА СТАРТ", route_back, "ROUTE_TO_START")
|
|
input("\nenter - начать движение:")
|
|
|
|
started = time.monotonic()
|
|
rover.log("MOVEMENT_START", markers=route_there)
|
|
rover.follow(target_id, set())
|
|
first_time = time.monotonic() - started
|
|
rover.log("MOVEMENT_FINISHED", marker=target_id, seconds=round(first_time, 2))
|
|
rover.log("TARGET_REACHED", marker=target_id, seconds=round(first_time, 2))
|
|
|
|
input("\nцель достигнута. ожидание препятствие и enter: ")
|
|
rover.leg = "return"
|
|
show_route(rover, "МАРШРУТ НА СТАРТ", route_back, "ROUTE_TO_START")
|
|
input("Enter - начать возврат: ")
|
|
|
|
started = time.monotonic()
|
|
rover.log("MOVEMENT_START", markers=route_back)
|
|
rover.follow(start, set())
|
|
second_time = time.monotonic() - started
|
|
rover.stop()
|
|
rover.log("MOVEMENT_FINISHED", marker=start, seconds=round(second_time, 2))
|
|
rover.log("START_REACHED", marker=start, seconds=round(second_time, 2))
|
|
rover.log(
|
|
"MISSION_FINISHED",
|
|
marker=start,
|
|
movement_seconds=round(first_time + second_time, 2),
|
|
log=str(rover.log_path),
|
|
)
|
|
except (KeyboardInterrupt, RuntimeError) as error:
|
|
exit_code = 1
|
|
rover.log("FAILED", reason=str(error) or "Ctrl+C")
|
|
finally:
|
|
for _ in range(5):
|
|
rover.stop()
|
|
rclpy.spin_once(rover, timeout_sec=0.02)
|
|
rover.log_file.close()
|
|
rover.destroy_node()
|
|
rclpy.shutdown()
|
|
raise SystemExit(exit_code)
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|