patch..
This commit is contained in:
@@ -1,6 +1,7 @@
|
||||
#!/usr/bin/env python3
|
||||
import asyncio
|
||||
import array
|
||||
import base64
|
||||
import json
|
||||
import logging
|
||||
import math
|
||||
@@ -83,6 +84,44 @@ def compose_2d(a: dict, b: dict) -> dict:
|
||||
}
|
||||
|
||||
|
||||
def invert_2d(transform: dict) -> dict:
|
||||
"""Return the inverse of a planar TF transform."""
|
||||
yaw = -transform["yaw"]
|
||||
c = math.cos(yaw)
|
||||
s = math.sin(yaw)
|
||||
return {
|
||||
"x": c * -transform["x"] - s * -transform["y"],
|
||||
"y": s * -transform["x"] + c * -transform["y"],
|
||||
"z": -transform.get("z", 0.0),
|
||||
"yaw": yaw,
|
||||
}
|
||||
|
||||
|
||||
def resolve_2d_transform(transforms: dict, target_frame: str, source_frame: str) -> Optional[dict]:
|
||||
"""Resolve target <- source through the currently published TF graph."""
|
||||
if not target_frame or not source_frame:
|
||||
return None
|
||||
if target_frame == source_frame:
|
||||
return {"x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0}
|
||||
|
||||
pending = [(target_frame, {"x": 0.0, "y": 0.0, "z": 0.0, "yaw": 0.0})]
|
||||
visited = set()
|
||||
while pending:
|
||||
frame, accumulated = pending.pop(0)
|
||||
if frame in visited:
|
||||
continue
|
||||
visited.add(frame)
|
||||
if frame == source_frame:
|
||||
return accumulated
|
||||
|
||||
for (parent, child), transform in transforms.items():
|
||||
if parent == frame and child not in visited:
|
||||
pending.append((child, compose_2d(accumulated, transform)))
|
||||
elif child == frame and parent not in visited:
|
||||
pending.append((parent, compose_2d(accumulated, invert_2d(transform))))
|
||||
return None
|
||||
|
||||
|
||||
class VelocityPayload(BaseModel):
|
||||
linear: float = 0.0
|
||||
angular: float = 0.0
|
||||
@@ -238,6 +277,28 @@ class SlamNode(Node):
|
||||
value = data[row * width + column]
|
||||
pixels.append(0 if value >= 50 else 254 if value == 0 else 205)
|
||||
pgm_path.write_bytes(f"P5\n{width} {height}\n255\n".encode("ascii") + pixels)
|
||||
|
||||
# Keep a compact copy with the map record so the LMS can render an
|
||||
# actual saved-map preview without accessing the student's workspace.
|
||||
preview_limit = 192
|
||||
preview_scale = max(1, math.ceil(width / preview_limit), math.ceil(height / preview_limit))
|
||||
preview_width = math.ceil(width / preview_scale)
|
||||
preview_height = math.ceil(height / preview_scale)
|
||||
preview_pixels = bytearray()
|
||||
for preview_row in range(preview_height):
|
||||
source_top = preview_row * preview_scale
|
||||
source_bottom = min(height, source_top + preview_scale)
|
||||
for preview_column in range(preview_width):
|
||||
source_left = preview_column * preview_scale
|
||||
source_right = min(width, source_left + preview_scale)
|
||||
# Preserve occupied cells while reducing the grid: black is
|
||||
# lower than unknown/free in the generated PGM palette.
|
||||
preview_pixels.append(min(
|
||||
pixels[row * width + column]
|
||||
for row in range(source_top, source_bottom)
|
||||
for column in range(source_left, source_right)
|
||||
))
|
||||
preview_pgm = f"P5\n{preview_width} {preview_height}\n255\n".encode("ascii") + preview_pixels
|
||||
yaml_path.write_text(
|
||||
"\n".join([
|
||||
f"image: {pgm_path.name}",
|
||||
@@ -285,6 +346,7 @@ class SlamNode(Node):
|
||||
"resolution": resolution,
|
||||
"width": width,
|
||||
"height": height,
|
||||
"previewPgm": base64.b64encode(preview_pgm).decode("ascii"),
|
||||
}
|
||||
|
||||
def _on_scan(self, msg: LaserScan):
|
||||
@@ -553,25 +615,24 @@ class SlamNode(Node):
|
||||
"angular": vel.angular.z,
|
||||
"t": odom.header.stamp.sec + odom.header.stamp.nanosec * 1e-9,
|
||||
}
|
||||
base_to_scan = (
|
||||
tf_snapshot.get((odom.child_frame_id, scan.header.frame_id))
|
||||
odom_to_scan = (
|
||||
resolve_2d_transform(tf_snapshot, odom.header.frame_id, scan.header.frame_id)
|
||||
if scan is not None
|
||||
else None
|
||||
)
|
||||
if base_to_scan is not None:
|
||||
if odom_to_scan is not None:
|
||||
scan_pose = {
|
||||
**compose_2d(pose, base_to_scan),
|
||||
"z": pose.get("z", 0.0) + base_to_scan.get("z", 0.0),
|
||||
**odom_to_scan,
|
||||
"roll": 0.0,
|
||||
"pitch": 0.0,
|
||||
"frame_id": pose["frame_id"],
|
||||
"source_frame_id": scan.header.frame_id,
|
||||
"t": scan.header.stamp.sec + scan.header.stamp.nanosec * 1e-9,
|
||||
}
|
||||
map_to_odom = tf_snapshot.get(("map", odom.header.frame_id))
|
||||
if map_to_odom is not None:
|
||||
map_to_base = resolve_2d_transform(tf_snapshot, "map", odom.child_frame_id)
|
||||
if map_to_base is not None:
|
||||
pose_map = {
|
||||
**compose_2d(map_to_odom, pose),
|
||||
**map_to_base,
|
||||
"frame_id": "map",
|
||||
"child_frame_id": odom.child_frame_id,
|
||||
"source_frame_id": odom.header.frame_id,
|
||||
@@ -579,14 +640,12 @@ class SlamNode(Node):
|
||||
}
|
||||
|
||||
if scan is not None and pose_map is not None:
|
||||
# A scan is measured from its sensor frame, not base_link. Use the
|
||||
# published base_link -> scan-frame TF so its endpoints overlay the
|
||||
# occupancy grid in the same map coordinate system.
|
||||
base_to_scan = tf_snapshot.get((pose_map["child_frame_id"], scan.header.frame_id))
|
||||
if base_to_scan is not None:
|
||||
# Resolve map -> sensor directly through the published TF graph so
|
||||
# the scan uses the same localization correction as the map.
|
||||
map_to_scan = resolve_2d_transform(tf_snapshot, "map", scan.header.frame_id)
|
||||
if map_to_scan is not None:
|
||||
scan_pose_map = {
|
||||
**compose_2d(pose_map, base_to_scan),
|
||||
"z": pose_map.get("z", 0.0) + base_to_scan.get("z", 0.0),
|
||||
**map_to_scan,
|
||||
"roll": 0.0,
|
||||
"pitch": 0.0,
|
||||
"frame_id": "map",
|
||||
@@ -676,7 +735,7 @@ class SlamNode(Node):
|
||||
"point_cloud": point_cloud_payload,
|
||||
"map": map_payload,
|
||||
"tf": {
|
||||
"map_to_odom": tf_snapshot.get(("map", "odom")),
|
||||
"map_to_odom": resolve_2d_transform(tf_snapshot, "map", "odom"),
|
||||
},
|
||||
"navigation": navigation,
|
||||
"plan": {
|
||||
|
||||
Reference in New Issue
Block a user