feat: add basic ROS rover scripts

basic/watch_robot.py

@@ -0,0 +1,70 @@
+#!/usr/bin/env python3
+"""Показывает одометрию и минимальную дистанцию перед ровером."""
+
+import argparse
+import math
+import time
+
+import rclpy
+from nav_msgs.msg import Odometry
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import LaserScan
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument("--robot", choices=("RMC1", "RMC2"), required=True)
+    args = parser.parse_args()
+
+    rclpy.init()
+    node = Node("mvch_basic_watch")
+    state = {"pose": None, "front": None}
+
+    def on_odom(message):
+        position = message.pose.pose.position
+        orientation = message.pose.pose.orientation
+        siny = 2.0 * (orientation.w * orientation.z + orientation.x * orientation.y)
+        cosy = 1.0 - 2.0 * (orientation.y**2 + orientation.z**2)
+        state["pose"] = (position.x, position.y, math.atan2(siny, cosy))
+
+    def on_scan(message):
+        front = []
+        for index, distance in enumerate(message.ranges):
+            angle = message.angle_min + index * message.angle_increment
+            if abs(angle) <= math.radians(20) and math.isfinite(distance):
+                if message.range_min <= distance <= message.range_max:
+                    front.append(distance)
+        state["front"] = min(front, default=math.inf)
+
+    node.create_subscription(
+        Odometry, f"/{args.robot}/odometry", on_odom, qos_profile_sensor_data
+    )
+    node.create_subscription(
+        LaserScan, f"/{args.robot}/scan_front", on_scan, qos_profile_sensor_data
+    )
+
+    print(f"Слушаю {args.robot}. Ctrl+C — выход.")
+    last_print = 0.0
+    try:
+        while rclpy.ok():
+            rclpy.spin_once(node, timeout_sec=0.1)
+            if time.monotonic() - last_print >= 0.5:
+                pose = state["pose"]
+                front = state["front"]
+                pose_text = "нет odometry" if pose is None else (
+                    f"x={pose[0]:.2f}, y={pose[1]:.2f}, yaw={math.degrees(pose[2]):.1f}°"
+                )
+                front_text = "нет scan_front" if front is None else f"{front:.2f} м"
+                print(f"{pose_text}; спереди: {front_text}")
+                last_print = time.monotonic()
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()
+