71 lines
2.4 KiB
Python
71 lines
2.4 KiB
Python
#!/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()
|
|
|