Pixel to world

.gitignore

@@ -16,5 +16,6 @@ complex_yolov4_mse_loss.pth
 *.bt
 *.pyc
 trace*
+ignore/
 
 rosbag2*

launches/launch.vehicle.py

@@ -79,11 +79,14 @@ def generate_launch_description():
                         path_planner,
                         # *nav2_launch_entities,
                         # path_planner_nav2,
-                        pure_pursuit_controller,
+                        # Path-following autonomy: fights keyboard teleop on /vehicle/control.
+                        # Re-enable when driving from the planner instead of WASD.
+                        # pure_pursuit_controller,
                         # SAFETY
                         ##airbags,
                         ##guardian,
                         rviz,
+                        keyboard_controller
                     ],
                 )
             ),

launches/launch_node_definitions.py

@@ -256,6 +256,11 @@
     executable='road_user_detection'
 )
 
+keyboard_controller = Node(
+    package='keyboard_control',
+    executable='keyboard_control_node'
+)
+
 lane_change_controller = Node(
     package='navigator_lane_change',
     executable='lane_change_node',

src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py

@@ -0,0 +1,112 @@
+import sys
+import rclpy
+from rclpy.node import Node
+from frame_tf_serv.srv import FrameTF
+from builtin_interfaces.msg import Time
+import numpy as np
+
+class FrameTFClient(Node):
+    def __init__(self):
+        super().__init__("FrameTFClient")
+
+        # initialize client
+        self.client = self.create_client(FrameTF, 'frame_tf')
+        while not self.client.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('service not available, trying again...')
+        self.req = FrameTF.Request()
+
+    def send_request(
+        self,
+        cam_to_world,
+        cam_to_pixel,
+        world_to_pixel,
+        pixel_to_world,
+        camera_name,
+        stamp,
+        x,
+        y,
+        z,
+    ):
+        """ packages and sends request to FrameTF service
+
+        Args:
+        cam_to_world (int either 0 or 1)
+        cam_to_pixel (int either 0 or 1)
+        world_to_pixel (int either 0 or 1)
+        pixel_to_world (int either 0 or 1)
+        camera_name (string)
+        stamp (Time)
+        x (float)  — world X, or pixel u for pixel_to_world
+        y (float)  — world Y, or pixel v for pixel_to_world
+        z (float)  — world Z (unused for pixel_to_world)
+
+        Returns:
+            response.tf_success (bool)
+            deserialized (np.ndarray)
+
+        """
+        self.req.cam_to_world = cam_to_world
+        self.req.cam_to_pixel = cam_to_pixel
+        self.req.world_to_pixel = world_to_pixel
+        self.req.pixel_to_world = pixel_to_world
+        self.req.camera_name = camera_name
+        self.req.stamp = stamp
+        self.req.x = float(x)
+        self.req.y = float(y)
+        self.req.z = float(z)
+        self.future = self.client.call_async(self.req)
+        rclpy.spin_until_future_complete(self, self.future)
+        self.response = self.future.result()
+        if self.response.coords:
+            self.deserialized = np.frombuffer(self.response.coords, dtype=np.float64)
+        else:
+            self.deserialized = np.array([])
+        return self.response.tf_success, self.deserialized
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    if len(sys.argv) < 9:
+        print(
+            "Usage: ros2 run frame_tf_client client "
+            "<cam_to_world> <cam_to_pixel> <world_to_pixel> <pixel_to_world> "
+            "<camera_name> <x> <y> <z>"
+        )
+        print(
+            "pixel_to_world example: "
+            "0 0 0 1 rgb_front 512.0 256.0 0.0"
+        )
+        rclpy.shutdown()
+        return
+
+    minimal_client = FrameTFClient()
+    stamp = Time()  # 0/0 → service falls back to cloud stamp
+    response = minimal_client.send_request(
+        int(sys.argv[1]),
+        int(sys.argv[2]),
+        int(sys.argv[3]),
+        int(sys.argv[4]),
+        sys.argv[5],
+        stamp,
+        float(sys.argv[6]),
+        float(sys.argv[7]),
+        float(sys.argv[8]),
+    )
+    minimal_client.get_logger().info(
+        'Result of CAM_TO_WORLD: %d, CAM_TO_PIXEL: %d, WORLD_TO_PIXEL: %d, '
+        'PIXEL_TO_WORLD: %d, CAMERA: %s, x=%.2f y=%.2f z=%.2f'
+        % (
+            int(sys.argv[1]), int(sys.argv[2]), int(sys.argv[3]), int(sys.argv[4]),
+            sys.argv[5], float(sys.argv[6]), float(sys.argv[7]), float(sys.argv[8]),
+        )
+    )
+
+    print(response)
+
+    minimal_client.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/perception/frame_tf_client/frame_tf_client/pixel_overlay.py

@@ -0,0 +1,109 @@
+#!/usr/bin/env python3
+"""Draw a crosshair at (u, v) on a camera image for pixel_to_world checks.
+
+Usage:
+  ros2 run frame_tf_client overlay 400 350
+  ros2 run frame_tf_client overlay 400 350 rgb_left
+
+Then in RViz: Add → Image, topic /pixel_query_overlay
+Compare that marked spot to the red sphere from pixel_to_world (map view).
+
+Camera name → image topic (this stack's remaps):
+  rgb_front  → /cameras/camera0
+  rgb_right  → /cameras/camera1
+  rgb_back   → /cameras/camera2
+  rgb_left   → /cameras/camera3
+"""
+
+import sys
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge
+import cv2
+
+
+# Matches carla_interface remaps in launch.carla_interface.py
+CAMERA_IMAGE_TOPICS = {
+    "rgb_front": "/cameras/camera0",
+    "rgb_right": "/cameras/camera1",
+    "rgb_back": "/cameras/camera2",
+    "rgb_left": "/cameras/camera3",
+}
+
+
+class PixelOverlay(Node):
+    def __init__(self, u: float, v: float, camera_name: str):
+        super().__init__("pixel_query_overlay")
+        self.u = int(round(u))
+        self.v = int(round(v))
+        self.camera_name = camera_name
+        self.bridge = CvBridge()
+
+        topic = CAMERA_IMAGE_TOPICS.get(camera_name)
+        if topic is None:
+            raise ValueError(
+                f"Unknown camera '{camera_name}'. "
+                f"Choose one of: {', '.join(CAMERA_IMAGE_TOPICS)}"
+            )
+
+        self.sub = self.create_subscription(
+            Image, topic, self._cb, qos_profile_sensor_data)
+        self.pub = self.create_publisher(Image, "/pixel_query_overlay", 10)
+        self.get_logger().info(
+            f"Overlay ({self.u},{self.v}) on {camera_name} [{topic}] → /pixel_query_overlay"
+        )
+
+    def _cb(self, msg: Image):
+        img = self.bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
+        h, w = img.shape[:2]
+        u = min(max(self.u, 0), w - 1)
+        v = min(max(self.v, 0), h - 1)
+
+        # Crosshair only (label in a corner so it isn't mistaken for geometry)
+        color = (0, 0, 255)  # red BGR
+        cv2.drawMarker(
+            img, (u, v), color,
+            markerType=cv2.MARKER_CROSS, markerSize=28, thickness=2)
+        cv2.circle(img, (u, v), 14, color, 2)
+
+        label = f"{self.camera_name}  ({u},{v})"
+        cv2.rectangle(img, (8, 8), (8 + 12 * len(label), 36), (0, 0, 0), -1)
+        cv2.putText(
+            img, label, (14, 30),
+            cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
+
+        out = self.bridge.cv2_to_imgmsg(img, encoding="bgr8")
+        out.header = msg.header
+        self.pub.publish(out)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    if len(sys.argv) < 3:
+        print("Usage: ros2 run frame_tf_client overlay <u> <v> [camera_name]")
+        print("Example: ros2 run frame_tf_client overlay 400 350")
+        print("Example: ros2 run frame_tf_client overlay 400 350 rgb_left")
+        print(f"Cameras: {', '.join(CAMERA_IMAGE_TOPICS)}")
+        rclpy.shutdown()
+        return
+
+    camera = sys.argv[3] if len(sys.argv) > 3 else "rgb_front"
+    try:
+        node = PixelOverlay(float(sys.argv[1]), float(sys.argv[2]), camera)
+    except ValueError as ex:
+        print(ex)
+        rclpy.shutdown()
+        return
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/perception/frame_tf_client/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>frame_tf_client</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="root@todo.todo">root</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>rclpy</depend>
+  <depend>sensor_msgs</depend>
+  <depend>cv_bridge</depend>
+  <depend>frame_tf_serv</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/perception/frame_tf_client/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/frame_tf_client
+[install]
+install_scripts=$base/lib/frame_tf_client

src/perception/frame_tf_client/setup.py

@@ -0,0 +1,31 @@
+from setuptools import find_packages, setup
+
+package_name = 'frame_tf_client'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='root',
+    maintainer_email='root@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': [
+            'pytest',
+        ],
+    },
+    entry_points={
+        'console_scripts': [
+            'client = frame_tf_client.frame_tf_client_node:main',
+            'overlay = frame_tf_client.pixel_overlay:main',
+        ],
+    },
+)

src/perception/frame_tf_client/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/perception/frame_tf_client/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/perception/frame_tf_client/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'