From 529500c374db44023e0e1768a4dc0bcaae9a0b6c Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Mon, 13 Jul 2026 18:47:59 -0500 Subject: [PATCH 01/11] copied over starting code from origin/feature/frame_tf_service --- .../frame_tf_client/__init__.py | 0 .../frame_tf_client/frame_tf_client_node.py | 73 ++++ src/perception/frame_tf_client/package.xml | 18 + .../frame_tf_client/resource/frame_tf_client | 0 src/perception/frame_tf_client/setup.cfg | 4 + src/perception/frame_tf_client/setup.py | 30 ++ .../frame_tf_client/test/test_copyright.py | 25 ++ .../frame_tf_client/test/test_flake8.py | 25 ++ .../frame_tf_client/test/test_pep257.py | 23 ++ src/perception/frame_tf_serv/CMakeLists.txt | 33 ++ src/perception/frame_tf_serv/package.xml | 21 ++ src/perception/frame_tf_serv/srv/FrameTF.srv | 15 + .../frame_tf_service/__init__.py | 0 .../frame_tf_service/frame_tf_service_node.py | 322 ++++++++++++++++++ src/perception/frame_tf_service/package.xml | 18 + .../resource/frame_tf_service | 0 src/perception/frame_tf_service/setup.cfg | 4 + src/perception/frame_tf_service/setup.py | 30 ++ .../frame_tf_service/srv/FrameTF.srv | 17 + .../frame_tf_service/test/test_copyright.py | 25 ++ .../frame_tf_service/test/test_flake8.py | 25 ++ .../frame_tf_service/test/test_pep257.py | 23 ++ 22 files changed, 731 insertions(+) create mode 100644 src/perception/frame_tf_client/frame_tf_client/__init__.py create mode 100644 src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py create mode 100644 src/perception/frame_tf_client/package.xml create mode 100644 src/perception/frame_tf_client/resource/frame_tf_client create mode 100644 src/perception/frame_tf_client/setup.cfg create mode 100644 src/perception/frame_tf_client/setup.py create mode 100644 src/perception/frame_tf_client/test/test_copyright.py create mode 100644 src/perception/frame_tf_client/test/test_flake8.py create mode 100644 src/perception/frame_tf_client/test/test_pep257.py create mode 100644 src/perception/frame_tf_serv/CMakeLists.txt create mode 100644 src/perception/frame_tf_serv/package.xml create mode 100644 src/perception/frame_tf_serv/srv/FrameTF.srv create mode 100644 src/perception/frame_tf_service/frame_tf_service/__init__.py create mode 100644 src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py create mode 100644 src/perception/frame_tf_service/package.xml create mode 100644 src/perception/frame_tf_service/resource/frame_tf_service create mode 100644 src/perception/frame_tf_service/setup.cfg create mode 100644 src/perception/frame_tf_service/setup.py create mode 100644 src/perception/frame_tf_service/srv/FrameTF.srv create mode 100644 src/perception/frame_tf_service/test/test_copyright.py create mode 100644 src/perception/frame_tf_service/test/test_flake8.py create mode 100644 src/perception/frame_tf_service/test/test_pep257.py diff --git a/src/perception/frame_tf_client/frame_tf_client/__init__.py b/src/perception/frame_tf_client/frame_tf_client/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py b/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py new file mode 100644 index 000000000..6e32d10a4 --- /dev/null +++ b/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py @@ -0,0 +1,73 @@ +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,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) + camera_name (string) + stamp (Time) + x (float32) + y (float32) + z (float32) + world_to_pixel + + Returns: + response.tf_success (bool) + desserialized () + + """ + 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.camera_name = camera_name + self.stamp = stamp + self.req.x = x + self.req.y = y + self.req.z = z + self.future = self.client.call_async(self.req) + rclpy.spin_until_future_complete(self, self.future) + self.response = self.future.result() + self.deserialized = np.frombuffer(self.response.coords,dtype=float) + return self.response.tf_success, self.deserialized + + + +def main(args=None): + rclpy.init(args=args) + + minimal_client = FrameTFClient() + stamp = Time() + ## Example Time input (change it up as you see fit) + example_time = rclpy.time.Time() + example_time.seconds = 1781736843 + example_time.nanosec = 163943087 + response = minimal_client.send_request(int(sys.argv[1]), int(sys.argv[2]), int(sys.argv[3]), sys.argv[4],example_time, int(sys.argv[5]),int(sys.argv[6]),int(sys.argv[7])) + minimal_client.get_logger().info( + 'Result of CAM_TO_WORLD: %d, CAM_TO_PIXEL: %d, WORLD_TO_PIXEL: %d, CAMERA: %s, Provided Time = %d seconds, %d nanoseconds' % + (int(sys.argv[1]), int(sys.argv[2]), int(sys.argv[3]), sys.argv[4],example_time.seconds, example_time.nanosec)) + + print(response) + + minimal_client.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/perception/frame_tf_client/package.xml b/src/perception/frame_tf_client/package.xml new file mode 100644 index 000000000..7e6a8b665 --- /dev/null +++ b/src/perception/frame_tf_client/package.xml @@ -0,0 +1,18 @@ + + + + frame_tf_client + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception/frame_tf_client/resource/frame_tf_client b/src/perception/frame_tf_client/resource/frame_tf_client new file mode 100644 index 000000000..e69de29bb diff --git a/src/perception/frame_tf_client/setup.cfg b/src/perception/frame_tf_client/setup.cfg new file mode 100644 index 000000000..156130303 --- /dev/null +++ b/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 diff --git a/src/perception/frame_tf_client/setup.py b/src/perception/frame_tf_client/setup.py new file mode 100644 index 000000000..e102f2242 --- /dev/null +++ b/src/perception/frame_tf_client/setup.py @@ -0,0 +1,30 @@ +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' + ], + }, +) diff --git a/src/perception/frame_tf_client/test/test_copyright.py b/src/perception/frame_tf_client/test/test_copyright.py new file mode 100644 index 000000000..97a39196e --- /dev/null +++ b/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' diff --git a/src/perception/frame_tf_client/test/test_flake8.py b/src/perception/frame_tf_client/test/test_flake8.py new file mode 100644 index 000000000..27ee1078f --- /dev/null +++ b/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) diff --git a/src/perception/frame_tf_client/test/test_pep257.py b/src/perception/frame_tf_client/test/test_pep257.py new file mode 100644 index 000000000..b234a3840 --- /dev/null +++ b/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' diff --git a/src/perception/frame_tf_serv/CMakeLists.txt b/src/perception/frame_tf_serv/CMakeLists.txt new file mode 100644 index 000000000..2076ae613 --- /dev/null +++ b/src/perception/frame_tf_serv/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.8) +project(frame_tf_serv) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rosidl_default_generators REQUIRED) + +find_package(builtin_interfaces REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + "srv/FrameTF.srv" + DEPENDENCIES builtin_interfaces +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/perception/frame_tf_serv/package.xml b/src/perception/frame_tf_serv/package.xml new file mode 100644 index 000000000..f4fdb7fcf --- /dev/null +++ b/src/perception/frame_tf_serv/package.xml @@ -0,0 +1,21 @@ + + + + frame_tf_serv + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_cmake + + ament_lint_auto + ament_lint_common + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + builtin_interfaces + + ament_cmake + + diff --git a/src/perception/frame_tf_serv/srv/FrameTF.srv b/src/perception/frame_tf_serv/srv/FrameTF.srv new file mode 100644 index 000000000..095e4d049 --- /dev/null +++ b/src/perception/frame_tf_serv/srv/FrameTF.srv @@ -0,0 +1,15 @@ +# Select Frame TF Options (1 to select an option, 0 for other options) +int32 cam_to_world +int32 cam_to_pixel +int32 world_to_pixel + +float32 x +float32 y +float32 z + +string camera_name +builtin_interfaces/Time stamp +--- +# Response +bool tf_success +uint8[] coords # returns data as a blob, must be deserialized after recieving \ No newline at end of file diff --git a/src/perception/frame_tf_service/frame_tf_service/__init__.py b/src/perception/frame_tf_service/frame_tf_service/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py new file mode 100644 index 000000000..0195a1f11 --- /dev/null +++ b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py @@ -0,0 +1,322 @@ +""" +Package: frame_tf_service +Filename: frame_tf_service.py +Authors: David Homiller, Saishravan Muthukrishnan, (AI was used for help with some debugging, math, learning information about ROS2, and packages.xml and CmakeLists.txt additions) +Email: david.homiller@utdallas.edu, saishravan.muthukrishnan@utdallas.edu +Copyright: 2021, Nova UTD +License: MIT License + +TODO: Description +""" + +# CV2 import for testing. Remove later. +import cv2 +from cv_bridge import CvBridge, CvBridgeError + +# Imports +import numpy as np + +import rclpy +from rclpy.node import Node +from tf2_ros import TransformException +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from scipy.spatial.transform import Rotation as R +import image_geometry +from rclpy.serialization import serialize_message, deserialize_message + + +# Message Imports +from rosgraph_msgs.msg import Clock +from geometry_msgs.msg import TransformStamped +from geometry_msgs.msg import Vector3 + +from builtin_interfaces.msg import Time +from sensor_msgs.msg import CameraInfo, Image, PointCloud2 +import sensor_msgs_py.point_cloud2 as pc2 +from std_msgs.msg import Header +from frame_tf_serv.srv import FrameTF + + +class FrameTFService(Node): + + def __init__(self): + super().__init__('FrameTFService') + + self.declare_parameter('seg_topic', '/semantics/semantic0') + self.srv = self.create_service( + FrameTF, + 'frame_tf', + self.callback + ) + self.serialized_message = None + self.timer = self.create_timer(0.02, self.timer_cb) # should i use a timer? what should the period be? + self.bridge = CvBridge() + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.ex = None + + # Declared for publishing msgs + self.stamp = Time() + + # Subcribes to raw lidar data + self.lidar_sub = self.create_subscription( + PointCloud2, '/lidar', self.lidar_callback, 10) + + # holds LIDAR transform stamp + self.t = None + + # Subscribes to camera info + rgb_center_camera_info_sub = self.create_subscription( + CameraInfo, '/carla/hero/rgb_center/camera_info', self.rgb_center_camera_info_cb, 10) + rgb_left_camera_info_sub = self.create_subscription( + CameraInfo, '/carla/hero/rgb_left/camera_info',self.rgb_left_camera_info_cb, 10) + rgb_right_camera_info_sub = self.create_subscription( + CameraInfo, '/carla/hero/rgb_right/camera_info',self.rgb_right_camera_info_cb,10) + rgb_back_camera_info_sub = self.create_subscription( + CameraInfo, '/carla/hero/rgb_back/camera_info', self.rgb_back_camera_info_cb,10) + # Subscribes to semantic segmented image topic + + + # Subscribes to clock + self.clock_sub = self.create_subscription( + Clock, '/clock', self.clock_cb, 10) + + self.cam_arr = None + + # stores camera info + self.rgb_center_cam_model = None + self.rgb_left_cam_model = None + self.rgb_right_cam_model = None + self.rgb_back_cam_model = None + + def timer_cb(self): + pass + + def rgb_center_camera_info_cb(self, msg: CameraInfo): + """Sets camera info for center camera + + Args: + msg (CameraInfo) + + Returns: + None + """ + self.rgb_center_cam_model = msg + + + def rgb_right_camera_info_cb(self, msg: CameraInfo): + """Sets camera info for right camera + + Args: + msg (CameraInfo) + + Returns: + None + """ + self.rgb_right_cam_model = image_geometry.PinholeCameraModel().fromCameraInfo(msg) + + def rgb_left_camera_info_cb(self, msg: CameraInfo): + """Sets camera info for left camera + + Args: + msg (CameraInfo) + + Returns: + None + """ + self.rgb_left_cam_model = image_geometry.PinholeCameraModel().fromCameraInfo(msg) + + def rgb_back_camera_info_cb(self, msg: CameraInfo): + """Sets camera info for back camera + + Args: + msg (CameraInfo) + + Returns: + None + """ + self.rgb_back_cam_model = image_geometry.PinholeCameraModel().fromCameraInfo(msg) + + def clock_cb(self, msg): + """! + Updates the clock for message headers. + @param msg[Clock] The clock message. + """ + + self.stamp.sec = msg.clock.sec + self.stamp.nanosec = msg.clock.nanosec + + def lidar_callback(self, lidar_msg: PointCloud2): + """ stores LIDAR transform information + + Args: + lidar_msg (PointCloud2) + + Returns: + None + """ + + # Get LIDAR points into np array + self.lid_arr = np.frombuffer(lidar_msg.data, dtype=np.float32) + self.lid_arr = np.reshape(self.lid_arr, (-1, 4)) + self.lid_arr = self.lid_arr[:,0:3] + + + def attach_depth(self, camera): + """ Applies camera intrinsics/extrinsics to LIDAR point cloud and stores it + + Args: + camera (string) + Returns: + None + """ + t = TransformStamped() + try: + t = self.tf_buffer.lookup_transform( + 'hero/'+camera, 'base_link', rclpy.time.Time(),timeout=rclpy.duration.Duration(seconds=2.0)) + #print("found transform!") + except TransformException as ex: + self.get_logger().info( + f'Could not transform to camera frameawefew: {ex}') + return + + # Rotate to camera frame + q = t.transform.rotation + tf_rotation: R = R.from_quat([q.x, q.y, q.z, q.w]) + cam_arr = tf_rotation.apply(self.lid_arr) + + # Translate to camera frame + cam_arr += [t.transform.translation.x, + t.transform.translation.y, + t.transform.translation.z] + + # Only keep points in front of the camera + cam_arr = cam_arr[cam_arr[:, 2] > 0] + self.cam_arr = cam_arr + + def camImage_to_world(self,camera,stamp): + """ Converts all pixels in a camera image into world coordinates and stores in a blob + + Args: + camera (string) + stamp (Time) + Returns: + None + """ + while True: + try: + # get rotation matrix + r_wc = self.tf_buffer.lookup_transform('hero/'+camera, 'base_link', stamp,timeout=rclpy.duration.Duration(seconds=2.0)).transform.rotation + R_wc = R.from_quat([r_wc.x, r_wc.y, r_wc.z, r_wc.w]).as_matrix() + + # get translation matrix + t_wc = self.tf_buffer.lookup_transform('hero/'+camera, 'base_link', stamp,timeout=rclpy.duration.Duration(seconds=2.0)).transform.translation + t_wc = np.array([t_wc.x,t_wc.y,t_wc.z]) + + # get tranformed LIDAR coords + self.attach_depth(camera) + + # Apply camera rotation and translation to transformed LIDAR points + x_w = R_wc.T@self.cam_arr.T + t_wc.reshape(3,1) + + # store as blob + self.serialized_message = x_w.tobytes() + break; + except TransformException as ex: + self.get_logger().info( + f'Could not transform to camera frameawefew: {ex}') + continue + + def world_to_pixel(self, camera, stamp, world_coord): + """ Converts given world coordinates into pixel coordinates and stores in a blob + + Args: + camera (string) + stamp (Time) + world_coord (3 Tuple) + Returns: + None + """ + while True: + try: + # get rotation matrix + r_lc = self.tf_buffer.lookup_transform('hero/'+camera, 'map', stamp, timeout=rclpy.duration.Duration(seconds=2.0)).transform.rotation + R_lc = R.from_quat([r_lc.x, r_lc.y, r_lc.z, r_lc.w]).as_matrix() + + # get translation matrix + t_wc = self.tf_buffer.lookup_transform('hero/'+camera, 'map', stamp, timeout=rclpy.duration.Duration(seconds=2.0)).transform.translation + t_wc = np.array([t_wc.x,t_wc.y,t_wc.z]) + + # choose correct set of camera intrinsics + selected_camera = None + if camera == "rgb_center": + selected_camera = self.rgb_center_cam_model + elif camera == "rgb_left": + selected_camera = self.rgb_left_cam_model + elif camera == "rgb_right": + selected_camera = self.rgb_right_cam_model + elif camera == "rgb_back": + selected_camera = self.rgb_back_cam_model + + fx = selected_camera.k[0] + cx = selected_camera.k[2] + fy = selected_camera.k[4] + cy = selected_camera.k[5] + + # transform coordinates + p_cam = R_lc*world_coord + t_wc + + # divide transformed world coordinates into pixel coordinates and apply intrinsics + x = world_coord[0] / world_coord[2] + y = world_coord[1] / world_coord[2]; + x_pixel = fx * x + cx + y_pixel = fy * y + cy + coords = (x_pixel, y_pixel) + #print(coords) + + # store as blob + self.serialized_message = bytes(coords); + break; + + except TransformException as ex: + self.get_logger().info( + f'Could not convert world to pixel: {ex}') + continue + + + def callback(self,request, response): + """ Calls selected transformation function based on recieved message parameters, and returns serialized output + + Args: + camera (string) + stamp (Time) + world_coord (3 Tuple) + Returns: + None + """ + # execute frame_tf operation based on selection + if len(request.camera_name) > 1: + if request.cam_to_world == 1: + self.camImage_to_world(request.camera_name,request.stamp) + response.coords = self.serialized_message + response.tf_success = True + elif request.world_to_pixel == 1: + self.world_to_pixel (request.camera_name, request.stamp, (request.x, request.y, request.z)) + response.coords = self.serialized_message + response.tf_success = True + else: + response.tf_success = False + print("transform failed") + else: + print("Camera name not provided: ", request.cam_to_world) + return response + +def main(args=None): + rclpy.init(args=args) + node = FrameTFService() + rclpy.spin(node) + rclpy.shutdown() + +if __name__ == "__main__": + main() diff --git a/src/perception/frame_tf_service/package.xml b/src/perception/frame_tf_service/package.xml new file mode 100644 index 000000000..49bbeb334 --- /dev/null +++ b/src/perception/frame_tf_service/package.xml @@ -0,0 +1,18 @@ + + + + frame_tf_service + 0.0.0 + TODO: Package description + root + TODO: License declaration + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/perception/frame_tf_service/resource/frame_tf_service b/src/perception/frame_tf_service/resource/frame_tf_service new file mode 100644 index 000000000..e69de29bb diff --git a/src/perception/frame_tf_service/setup.cfg b/src/perception/frame_tf_service/setup.cfg new file mode 100644 index 000000000..926a5e7c6 --- /dev/null +++ b/src/perception/frame_tf_service/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/frame_tf_service +[install] +install_scripts=$base/lib/frame_tf_service diff --git a/src/perception/frame_tf_service/setup.py b/src/perception/frame_tf_service/setup.py new file mode 100644 index 000000000..1ea47290a --- /dev/null +++ b/src/perception/frame_tf_service/setup.py @@ -0,0 +1,30 @@ +from setuptools import find_packages, setup + +package_name = 'frame_tf_service' + +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': [ + 'service = frame_tf_service.frame_tf_service_node:main' + ], + }, +) diff --git a/src/perception/frame_tf_service/srv/FrameTF.srv b/src/perception/frame_tf_service/srv/FrameTF.srv new file mode 100644 index 000000000..0dd5fc0e6 --- /dev/null +++ b/src/perception/frame_tf_service/srv/FrameTF.srv @@ -0,0 +1,17 @@ +# Select Frame TF Options (1 to select an option, 0 for other options) +int32 CAM_TO_WORLD +int32 CAM_TO_PIXEL +int32 WORLD_TO_PIXEL + +int32 x +int32 y +int32 z + +string camera_name + +builtin_interfaces/Time stamp + +--- +# Response +bool tf_success +uint8[] coords # returns data as a blob, must be deserialized after recieving \ No newline at end of file diff --git a/src/perception/frame_tf_service/test/test_copyright.py b/src/perception/frame_tf_service/test/test_copyright.py new file mode 100644 index 000000000..97a39196e --- /dev/null +++ b/src/perception/frame_tf_service/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' diff --git a/src/perception/frame_tf_service/test/test_flake8.py b/src/perception/frame_tf_service/test/test_flake8.py new file mode 100644 index 000000000..27ee1078f --- /dev/null +++ b/src/perception/frame_tf_service/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) diff --git a/src/perception/frame_tf_service/test/test_pep257.py b/src/perception/frame_tf_service/test/test_pep257.py new file mode 100644 index 000000000..b234a3840 --- /dev/null +++ b/src/perception/frame_tf_service/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' From c649b654367d70aad340c776ce2963135840d428 Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Tue, 14 Jul 2026 07:29:49 -0500 Subject: [PATCH 02/11] ignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index c6e2b0a7b..8e8484ca3 100644 --- a/.gitignore +++ b/.gitignore @@ -16,5 +16,6 @@ complex_yolov4_mse_loss.pth *.bt *.pyc trace* +ignore/ rosbag2* From 847bac7feda72a7c5e7401921da3312817996748 Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Wed, 15 Jul 2026 09:04:25 -0500 Subject: [PATCH 03/11] added pixel_to_world --- .../frame_tf_service/frame_tf_service_node.py | 90 ++++++++++++++++++- 1 file changed, 87 insertions(+), 3 deletions(-) diff --git a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py index 0195a1f11..46c944fde 100644 --- a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py +++ b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py @@ -15,6 +15,8 @@ # Imports import numpy as np +from math import hypot +from typing import Optional, Tuple import rclpy from rclpy.node import Node @@ -28,15 +30,15 @@ # Message Imports from rosgraph_msgs.msg import Clock -from geometry_msgs.msg import TransformStamped -from geometry_msgs.msg import Vector3 - +from geometry_msgs.msg import TransformStamped, Vector3, Point from builtin_interfaces.msg import Time from sensor_msgs.msg import CameraInfo, Image, PointCloud2 import sensor_msgs_py.point_cloud2 as pc2 from std_msgs.msg import Header from frame_tf_serv.srv import FrameTF +MAX_PIXEL_RADIUS = 5.0 + class FrameTFService(Node): @@ -283,6 +285,88 @@ def world_to_pixel(self, camera, stamp, world_coord): self.get_logger().info( f'Could not convert world to pixel: {ex}') continue + + + def get_intrinsics(self, camera_info: CameraInfo) -> Tuple[float, float, float, float]: + """ + Get focal length and principal point from the camera info matrix. + Returns (fx, fy, cx, cy). + """ + k = camera_info.k + fx, fy = k[0], k[4] + cx, cy = k[2], k[5] + return fx, fy, cx, cy + + def cloud_to_xyz(self, cloud: PointCloud2) -> np.ndarray: + # extracts the X, Y, and Z 3D spatial coordinates from a ROS 2 PointCloud2 message and loads them into a numpy array + return pc2.read_points_numpy(cloud, field_names=('x','y','z')) + + def transform_cloud( + self, + pts: np.ndarray, + target_frame: str, + source_frame: str, + stamp: Time, + ) -> np.ndarray: + """ + pts: (N,3) in source_frame -> (N,3) in target_frame. + returns the transform that takes a point from source into target; applied as R · p + t + """ + # get the transform that converts a point from source into target at time stamp + tf = self.tf_buffer.lookup_transform( + target_frame, source_frame, stamp, + timeout=rclpy.duration.Duration(seconds=0.2)) # NOT while True + q = tf.transform.rotation # quaternion (orientation of source relative to target) + t = tf.transform.translation # 3-vector (where the source origin sits in the target frame) + Rm = R.from_quat([q.x, q.y, q.z, q.w]) # convert quaternion into managed scipy Rotation object + return Rm.apply(pts) + np.array([t.x, t.y, t.z]) # apply the rotation to every point in pts, then add the translation + + def pixel_to_world( + self, + camera_info: CameraInfo, + image_stamp: Time, + cloud: PointCloud2, + u: float, + v: float, + ) -> Optional[np.ndarray]: + """ + Converts pixel coordinate to world coordinate ("what 3d map point is this pixel looking at?") + """ + fx, fy, cx, cy = self.get_intrinsics(camera_info) + camera_frame = camera_info.header.frame_id # where the camera lives in TF + src = cloud.header.frame_id # where the LiDAR points currently live + + points = self.cloud_to_xyz(cloud) # (N,3) in the cloud's own frame + + # cloud_cam[i] and cloud_map[i] are the same laser hit, just in different coordinates + # when we pick a winner in image space, we can easily get the map XYZ + cloud_cam = self.transform_cloud(points, camera_frame, src, cloud.header.stamp) + cloud_map = self.transform_cloud(points, "map", src, cloud.header.stamp) + + best = None # (d_pixels, z_c, p_map) + for p_cam, p_map in zip(cloud_cam, cloud_map): # each is [x, y, z] + x_c, y_c, z_c = p_cam + if not np.all(np.isfinite(p_cam)) or z_c <= 0: # skip bad / behind-camera points + continue + + # project with pinhole formula + ui = fx * (x_c / z_c) + cx + vi = fy * (y_c / z_c) + cy + if not (0 <= ui < camera_info.width and 0 <= vi < camera_info.height): + continue + + # distance (how many pixels away the projected LiDAR hit (ui, vi) is from the query pixel (u, v)) + d = hypot(ui - u, vi - v) + + if d <= MAX_PIXEL_RADIUS: + # nearest pixel first; break near-ties by frontmost depth + if best is None or (round(d, 1), z_c) < (round(best[0], 1), best[1]): + best = (d, z_c, p_map) + + if best is None: + return None # no LiDAR near this pixel + return best[2] # map point measured at cloud time + def callback(self,request, response): From c064ecb3036cff279d30e2018af00a925ccaa150 Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Wed, 15 Jul 2026 09:05:58 -0500 Subject: [PATCH 04/11] add pixel_to_world to srv --- src/perception/frame_tf_serv/srv/FrameTF.srv | 1 + 1 file changed, 1 insertion(+) diff --git a/src/perception/frame_tf_serv/srv/FrameTF.srv b/src/perception/frame_tf_serv/srv/FrameTF.srv index 095e4d049..792221131 100644 --- a/src/perception/frame_tf_serv/srv/FrameTF.srv +++ b/src/perception/frame_tf_serv/srv/FrameTF.srv @@ -2,6 +2,7 @@ int32 cam_to_world int32 cam_to_pixel int32 world_to_pixel +int32 pixel_to_world float32 x float32 y From 907ac716be256a432e7d08bc31209e6f96b35253 Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Wed, 15 Jul 2026 09:28:16 -0500 Subject: [PATCH 05/11] Refactor send_request method to include pixel_to_world parameter and improve argument handling. Update main function to reflect changes in request parameters and enhance usage instructions. --- .../frame_tf_client/frame_tf_client_node.py | 79 ++++++++++++++----- 1 file changed, 59 insertions(+), 20 deletions(-) diff --git a/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py b/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py index 6e32d10a4..7f9286fd2 100644 --- a/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py +++ b/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py @@ -15,37 +15,52 @@ def __init__(self): 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,camera_name,stamp,x,y,z): + 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 (float32) - y (float32) - z (float32) - world_to_pixel + 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) - desserialized () + 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.stamp = stamp - self.req.x = x - self.req.y = y - self.req.z = z + 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() - self.deserialized = np.frombuffer(self.response.coords,dtype=float) + 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 @@ -53,16 +68,40 @@ def send_request(self, cam_to_world,cam_to_pixel,world_to_pixel,camera_name,stam def main(args=None): rclpy.init(args=args) + if len(sys.argv) < 9: + print( + "Usage: ros2 run frame_tf_client client " + " " + " " + ) + print( + "pixel_to_world example: " + "0 0 0 1 rgb_center 512.0 256.0 0.0" + ) + rclpy.shutdown() + return + minimal_client = FrameTFClient() - stamp = Time() - ## Example Time input (change it up as you see fit) - example_time = rclpy.time.Time() - example_time.seconds = 1781736843 - example_time.nanosec = 163943087 - response = minimal_client.send_request(int(sys.argv[1]), int(sys.argv[2]), int(sys.argv[3]), sys.argv[4],example_time, int(sys.argv[5]),int(sys.argv[6]),int(sys.argv[7])) + 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, CAMERA: %s, Provided Time = %d seconds, %d nanoseconds' % - (int(sys.argv[1]), int(sys.argv[2]), int(sys.argv[3]), sys.argv[4],example_time.seconds, example_time.nanosec)) + '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) @@ -70,4 +109,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() From 1718c7fbecd6a56a0c037f7094c695b51612f8aa Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Wed, 15 Jul 2026 09:28:33 -0500 Subject: [PATCH 06/11] delete duplicate srv --- src/perception/frame_tf_service/srv/FrameTF.srv | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 src/perception/frame_tf_service/srv/FrameTF.srv diff --git a/src/perception/frame_tf_service/srv/FrameTF.srv b/src/perception/frame_tf_service/srv/FrameTF.srv deleted file mode 100644 index 0dd5fc0e6..000000000 --- a/src/perception/frame_tf_service/srv/FrameTF.srv +++ /dev/null @@ -1,17 +0,0 @@ -# Select Frame TF Options (1 to select an option, 0 for other options) -int32 CAM_TO_WORLD -int32 CAM_TO_PIXEL -int32 WORLD_TO_PIXEL - -int32 x -int32 y -int32 z - -string camera_name - -builtin_interfaces/Time stamp - ---- -# Response -bool tf_success -uint8[] coords # returns data as a blob, must be deserialized after recieving \ No newline at end of file From 35e00b95604177b141dc6d75846f4a284bbf3e3f Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Wed, 15 Jul 2026 09:28:58 -0500 Subject: [PATCH 07/11] Enhance FrameTFService with camera info caching and pixel-to-world transformation. Added methods to retrieve camera info and store the latest LiDAR cloud. Updated transformation logic to handle new parameters and improve error handling. --- .../frame_tf_service/frame_tf_service_node.py | 99 ++++++++++++++----- 1 file changed, 74 insertions(+), 25 deletions(-) diff --git a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py index 46c944fde..acdfd05d3 100644 --- a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py +++ b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py @@ -85,15 +85,27 @@ def __init__(self): Clock, '/clock', self.clock_cb, 10) self.cam_arr = None + self.latest_cloud: Optional[PointCloud2] = None + self.lid_arr: Optional[np.ndarray] = None - # stores camera info - self.rgb_center_cam_model = None - self.rgb_left_cam_model = None - self.rgb_right_cam_model = None - self.rgb_back_cam_model = None + # stores CameraInfo per camera (raw msg — has .k, .width, .height, .header.frame_id) + self.rgb_center_cam_model: Optional[CameraInfo] = None + self.rgb_left_cam_model: Optional[CameraInfo] = None + self.rgb_right_cam_model: Optional[CameraInfo] = None + self.rgb_back_cam_model: Optional[CameraInfo] = None def timer_cb(self): pass + + def get_camera_info(self, camera_name: str) -> Optional[CameraInfo]: + """Look up cached CameraInfo by camera label.""" + cameras = { + "rgb_center": self.rgb_center_cam_model, + "rgb_left": self.rgb_left_cam_model, + "rgb_right": self.rgb_right_cam_model, + "rgb_back": self.rgb_back_cam_model, + } + return cameras.get(camera_name) def rgb_center_camera_info_cb(self, msg: CameraInfo): """Sets camera info for center camera @@ -116,7 +128,7 @@ def rgb_right_camera_info_cb(self, msg: CameraInfo): Returns: None """ - self.rgb_right_cam_model = image_geometry.PinholeCameraModel().fromCameraInfo(msg) + self.rgb_right_cam_model = msg def rgb_left_camera_info_cb(self, msg: CameraInfo): """Sets camera info for left camera @@ -127,7 +139,7 @@ def rgb_left_camera_info_cb(self, msg: CameraInfo): Returns: None """ - self.rgb_left_cam_model = image_geometry.PinholeCameraModel().fromCameraInfo(msg) + self.rgb_left_cam_model = msg def rgb_back_camera_info_cb(self, msg: CameraInfo): """Sets camera info for back camera @@ -138,7 +150,7 @@ def rgb_back_camera_info_cb(self, msg: CameraInfo): Returns: None """ - self.rgb_back_cam_model = image_geometry.PinholeCameraModel().fromCameraInfo(msg) + self.rgb_back_cam_model = msg def clock_cb(self, msg): """! @@ -164,6 +176,9 @@ def lidar_callback(self, lidar_msg: PointCloud2): self.lid_arr = np.reshape(self.lid_arr, (-1, 4)) self.lid_arr = self.lid_arr[:,0:3] + # store point cloud for pixel_to_world + self.latest_cloud = lidar_msg + def attach_depth(self, camera): """ Applies camera intrinsics/extrinsics to LIDAR point cloud and stores it @@ -337,11 +352,16 @@ def pixel_to_world( src = cloud.header.frame_id # where the LiDAR points currently live points = self.cloud_to_xyz(cloud) # (N,3) in the cloud's own frame + if points.size == 0: + return None + + # prefer image stamp for TF when provided; fall back to cloud stamp + stamp = image_stamp if (image_stamp.sec != 0 or image_stamp.nanosec != 0) else cloud.header.stamp # cloud_cam[i] and cloud_map[i] are the same laser hit, just in different coordinates # when we pick a winner in image space, we can easily get the map XYZ - cloud_cam = self.transform_cloud(points, camera_frame, src, cloud.header.stamp) - cloud_map = self.transform_cloud(points, "map", src, cloud.header.stamp) + cloud_cam = self.transform_cloud(points, camera_frame, src, stamp) + cloud_map = self.transform_cloud(points, "map", src, stamp) best = None # (d_pixels, z_c, p_map) for p_cam, p_map in zip(cloud_cam, cloud_map): # each is [x, y, z] @@ -357,7 +377,7 @@ def pixel_to_world( # distance (how many pixels away the projected LiDAR hit (ui, vi) is from the query pixel (u, v)) d = hypot(ui - u, vi - v) - + if d <= MAX_PIXEL_RADIUS: # nearest pixel first; break near-ties by frontmost depth if best is None or (round(d, 1), z_c) < (round(best[0], 1), best[1]): @@ -379,21 +399,50 @@ def callback(self,request, response): Returns: None """ - # execute frame_tf operation based on selection - if len(request.camera_name) > 1: - if request.cam_to_world == 1: - self.camImage_to_world(request.camera_name,request.stamp) - response.coords = self.serialized_message - response.tf_success = True - elif request.world_to_pixel == 1: - self.world_to_pixel (request.camera_name, request.stamp, (request.x, request.y, request.z)) - response.coords = self.serialized_message - response.tf_success = True - else: - response.tf_success = False - print("transform failed") + response.tf_success = False + response.coords = bytes() + + if len(request.camera_name) <= 1: + self.get_logger().info("Camera name not provided") + return response + + if request.cam_to_world == 1: + self.camImage_to_world(request.camera_name, request.stamp) + response.coords = self.serialized_message + response.tf_success = True + elif request.world_to_pixel == 1: + self.world_to_pixel(request.camera_name, request.stamp, (request.x, request.y, request.z)) + response.coords = self.serialized_message + response.tf_success = True + elif request.pixel_to_world == 1: + camera_info = self.get_camera_info(request.camera_name) + if camera_info is None: + self.get_logger().info( + f"No CameraInfo yet for '{request.camera_name}'") + return response + if self.latest_cloud is None: + self.get_logger().info("No LiDAR cloud received yet") + return response + try: + point = self.pixel_to_world( + camera_info, + request.stamp, + self.latest_cloud, + float(request.x), + float(request.y), + ) + except TransformException as ex: + self.get_logger().info(f"pixel_to_world TF failed: {ex}") + return response + if point is None: + self.get_logger().info( + f"No LiDAR near pixel ({request.x}, {request.y})") + return response + response.coords = np.asarray(point, dtype=np.float64).tobytes() + response.tf_success = True else: - print("Camera name not provided: ", request.cam_to_world) + self.get_logger().info("No transform mode selected") + return response def main(args=None): From 008e7e6f5cdecf6ddb6c06f5d35f63e698b5fdc2 Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Wed, 15 Jul 2026 09:44:24 -0500 Subject: [PATCH 08/11] use rgb front instead of rgb center --- .../frame_tf_client/frame_tf_client_node.py | 2 +- .../frame_tf_service/frame_tf_service_node.py | 59 ++++++------------- 2 files changed, 19 insertions(+), 42 deletions(-) diff --git a/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py b/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py index 7f9286fd2..3f3744f9d 100644 --- a/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py +++ b/src/perception/frame_tf_client/frame_tf_client/frame_tf_client_node.py @@ -76,7 +76,7 @@ def main(args=None): ) print( "pixel_to_world example: " - "0 0 0 1 rgb_center 512.0 256.0 0.0" + "0 0 0 1 rgb_front 512.0 256.0 0.0" ) rclpy.shutdown() return diff --git a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py index acdfd05d3..1688c0e0d 100644 --- a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py +++ b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py @@ -69,15 +69,17 @@ def __init__(self): self.t = None # Subscribes to camera info - rgb_center_camera_info_sub = self.create_subscription( + # NOTE: this CARLA setup publishes rgb_front, not rgb_center (center has 0 publishers). + self.create_subscription( + CameraInfo, '/carla/hero/rgb_front/camera_info', self.rgb_front_camera_info_cb, 10) + self.create_subscription( CameraInfo, '/carla/hero/rgb_center/camera_info', self.rgb_center_camera_info_cb, 10) - rgb_left_camera_info_sub = self.create_subscription( - CameraInfo, '/carla/hero/rgb_left/camera_info',self.rgb_left_camera_info_cb, 10) - rgb_right_camera_info_sub = self.create_subscription( - CameraInfo, '/carla/hero/rgb_right/camera_info',self.rgb_right_camera_info_cb,10) - rgb_back_camera_info_sub = self.create_subscription( - CameraInfo, '/carla/hero/rgb_back/camera_info', self.rgb_back_camera_info_cb,10) - # Subscribes to semantic segmented image topic + self.create_subscription( + CameraInfo, '/carla/hero/rgb_left/camera_info', self.rgb_left_camera_info_cb, 10) + self.create_subscription( + CameraInfo, '/carla/hero/rgb_right/camera_info', self.rgb_right_camera_info_cb, 10) + self.create_subscription( + CameraInfo, '/carla/hero/rgb_back/camera_info', self.rgb_back_camera_info_cb, 10) # Subscribes to clock @@ -89,6 +91,7 @@ def __init__(self): self.lid_arr: Optional[np.ndarray] = None # stores CameraInfo per camera (raw msg — has .k, .width, .height, .header.frame_id) + self.rgb_front_cam_model: Optional[CameraInfo] = None self.rgb_center_cam_model: Optional[CameraInfo] = None self.rgb_left_cam_model: Optional[CameraInfo] = None self.rgb_right_cam_model: Optional[CameraInfo] = None @@ -100,56 +103,28 @@ def timer_cb(self): def get_camera_info(self, camera_name: str) -> Optional[CameraInfo]: """Look up cached CameraInfo by camera label.""" cameras = { + "rgb_front": self.rgb_front_cam_model, "rgb_center": self.rgb_center_cam_model, "rgb_left": self.rgb_left_cam_model, "rgb_right": self.rgb_right_cam_model, "rgb_back": self.rgb_back_cam_model, } return cameras.get(camera_name) + + def rgb_front_camera_info_cb(self, msg: CameraInfo): + self.rgb_front_cam_model = msg def rgb_center_camera_info_cb(self, msg: CameraInfo): - """Sets camera info for center camera - - Args: - msg (CameraInfo) - - Returns: - None - """ self.rgb_center_cam_model = msg def rgb_right_camera_info_cb(self, msg: CameraInfo): - """Sets camera info for right camera - - Args: - msg (CameraInfo) - - Returns: - None - """ self.rgb_right_cam_model = msg def rgb_left_camera_info_cb(self, msg: CameraInfo): - """Sets camera info for left camera - - Args: - msg (CameraInfo) - - Returns: - None - """ self.rgb_left_cam_model = msg def rgb_back_camera_info_cb(self, msg: CameraInfo): - """Sets camera info for back camera - - Args: - msg (CameraInfo) - - Returns: - None - """ self.rgb_back_cam_model = msg def clock_cb(self, msg): @@ -267,7 +242,9 @@ def world_to_pixel(self, camera, stamp, world_coord): # choose correct set of camera intrinsics selected_camera = None - if camera == "rgb_center": + if camera == "rgb_front": + selected_camera = self.rgb_front_cam_model + elif camera == "rgb_center": selected_camera = self.rgb_center_cam_model elif camera == "rgb_left": selected_camera = self.rgb_left_cam_model From ae0f7f62d7124bad65b1f51173b9ff0af2660b9c Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Mon, 27 Jul 2026 09:39:53 -0500 Subject: [PATCH 09/11] keyboard controller --- launches/launch.vehicle.py | 5 ++++- launches/launch_node_definitions.py | 5 +++++ 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/launches/launch.vehicle.py b/launches/launch.vehicle.py index 6aacdf4b5..9afda0caa 100644 --- a/launches/launch.vehicle.py +++ b/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 ], ) ), diff --git a/launches/launch_node_definitions.py b/launches/launch_node_definitions.py index 09271e7b6..cf294ddb2 100644 --- a/launches/launch_node_definitions.py +++ b/launches/launch_node_definitions.py @@ -248,4 +248,9 @@ road_user_detector = Node( package='road_user_detection', executable='road_user_detection' +) + +keyboard_controller = Node( + package='keyboard_control', + executable='keyboard_control_node' ) \ No newline at end of file From 22dc1e90c1b840687ac079e9d6c2a1c55961ef05 Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Mon, 27 Jul 2026 10:27:45 -0500 Subject: [PATCH 10/11] Add dependencies for frame_tf_client and introduce pixel overlay script --- .../frame_tf_client/pixel_overlay.py | 109 ++++++++++++++++++ src/perception/frame_tf_client/package.xml | 5 + src/perception/frame_tf_client/setup.py | 3 +- 3 files changed, 116 insertions(+), 1 deletion(-) create mode 100644 src/perception/frame_tf_client/frame_tf_client/pixel_overlay.py diff --git a/src/perception/frame_tf_client/frame_tf_client/pixel_overlay.py b/src/perception/frame_tf_client/frame_tf_client/pixel_overlay.py new file mode 100644 index 000000000..4f5918800 --- /dev/null +++ b/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 [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() diff --git a/src/perception/frame_tf_client/package.xml b/src/perception/frame_tf_client/package.xml index 7e6a8b665..bf29134c1 100644 --- a/src/perception/frame_tf_client/package.xml +++ b/src/perception/frame_tf_client/package.xml @@ -7,6 +7,11 @@ root TODO: License declaration + rclpy + sensor_msgs + cv_bridge + frame_tf_serv + ament_copyright ament_flake8 ament_pep257 diff --git a/src/perception/frame_tf_client/setup.py b/src/perception/frame_tf_client/setup.py index e102f2242..2a3746e86 100644 --- a/src/perception/frame_tf_client/setup.py +++ b/src/perception/frame_tf_client/setup.py @@ -24,7 +24,8 @@ }, entry_points={ 'console_scripts': [ - 'client = frame_tf_client.frame_tf_client_node:main' + 'client = frame_tf_client.frame_tf_client_node:main', + 'overlay = frame_tf_client.pixel_overlay:main', ], }, ) From 9606af26a2c2b806292bfa6444495b7513b19d65 Mon Sep 17 00:00:00 2001 From: yfshaikh Date: Wed, 29 Jul 2026 15:38:55 -0500 Subject: [PATCH 11/11] fix world to pixel --- .../frame_tf_service/frame_tf_service_node.py | 185 ++++++++++-------- 1 file changed, 108 insertions(+), 77 deletions(-) diff --git a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py index 1688c0e0d..e077de27e 100644 --- a/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py +++ b/src/perception/frame_tf_service/frame_tf_service/frame_tf_service_node.py @@ -1,8 +1,8 @@ """ Package: frame_tf_service Filename: frame_tf_service.py -Authors: David Homiller, Saishravan Muthukrishnan, (AI was used for help with some debugging, math, learning information about ROS2, and packages.xml and CmakeLists.txt additions) -Email: david.homiller@utdallas.edu, saishravan.muthukrishnan@utdallas.edu +Authors: David Homiller, Saishravan Muthukrishnan, Yusuf Shaikh (AI was used for help with some debugging, math, learning information about ROS2, and packages.xml and CmakeLists.txt additions) +Email: david.homiller@utdallas.edu, saishravan.muthukrishnan@utdallas.edu, yusuf.shaikh@utdallas.edu Copyright: 2021, Nova UTD License: MIT License @@ -220,64 +220,72 @@ def camImage_to_world(self,camera,stamp): f'Could not transform to camera frameawefew: {ex}') continue - def world_to_pixel(self, camera, stamp, world_coord): - """ Converts given world coordinates into pixel coordinates and stores in a blob - - Args: - camera (string) - stamp (Time) - world_coord (3 Tuple) + def world_to_pixel(self, camera: str, stamp: Time, world_coord) -> bool: + """ + Project a map-frame 3D point onto the camera image. + + Process: + 1. Load CameraInfo for `camera` (intrinsics + optical frame id). + 2. Look up TF that expresses map points in the camera frame + (stamp 0/0 → latest TF; otherwise use the request stamp). + 3. Apply the rigid transform: p_cam = R · P_map + t. + 4. Reject non-finite points or points behind the lens (z_c <= 0). + 5. Pinhole-project with K: u = fx·x_c/z_c + cx, v = fy·y_c/z_c + cy. + 6. Store (u, v) as float64 bytes in self.serialized_message. + Returns: - None + True on success (message filled), False if CameraInfo/TF/geometry fails. """ - while True: - try: - # get rotation matrix - r_lc = self.tf_buffer.lookup_transform('hero/'+camera, 'map', stamp, timeout=rclpy.duration.Duration(seconds=2.0)).transform.rotation - R_lc = R.from_quat([r_lc.x, r_lc.y, r_lc.z, r_lc.w]).as_matrix() - - # get translation matrix - t_wc = self.tf_buffer.lookup_transform('hero/'+camera, 'map', stamp, timeout=rclpy.duration.Duration(seconds=2.0)).transform.translation - t_wc = np.array([t_wc.x,t_wc.y,t_wc.z]) - - # choose correct set of camera intrinsics - selected_camera = None - if camera == "rgb_front": - selected_camera = self.rgb_front_cam_model - elif camera == "rgb_center": - selected_camera = self.rgb_center_cam_model - elif camera == "rgb_left": - selected_camera = self.rgb_left_cam_model - elif camera == "rgb_right": - selected_camera = self.rgb_right_cam_model - elif camera == "rgb_back": - selected_camera = self.rgb_back_cam_model - - fx = selected_camera.k[0] - cx = selected_camera.k[2] - fy = selected_camera.k[4] - cy = selected_camera.k[5] - - # transform coordinates - p_cam = R_lc*world_coord + t_wc - - # divide transformed world coordinates into pixel coordinates and apply intrinsics - x = world_coord[0] / world_coord[2] - y = world_coord[1] / world_coord[2]; - x_pixel = fx * x + cx - y_pixel = fy * y + cy - coords = (x_pixel, y_pixel) - #print(coords) - - # store as blob - self.serialized_message = bytes(coords); - break; - - except TransformException as ex: - self.get_logger().info( - f'Could not convert world to pixel: {ex}') - continue - + # Get intrinsics (fx, fy, cx, cy) and the camera's TF frame name + camera_info = self.get_camera_info(camera) + if camera_info is None: + self.get_logger().info(f"No CameraInfo yet for '{camera}'") + return False + + # p_map = the 3D point in the world/map frame (what the caller asked about) + p_map = np.asarray(world_coord, dtype=np.float64) + camera_frame = camera_info.header.frame_id + + # if client sends time 0/0, use the latest pose + tf_time = ( + rclpy.time.Time() + if (stamp.sec == 0 and stamp.nanosec == 0) + else stamp + ) + + # express map points in this camera's frame + try: + tf = self.tf_buffer.lookup_transform( + camera_frame, "map", tf_time, + timeout=rclpy.duration.Duration(seconds=0.2), + ) + except TransformException as ex: + self.get_logger().info(f"world_to_pixel TF failed: {ex}") + return False + + # Rigid transform: rotate the point, then slide it (R · p_map + t) + # Result p_cam is the same physical point, but measured from the camera + q = tf.transform.rotation + t = tf.transform.translation + Rm = R.from_quat([q.x, q.y, q.z, q.w]) + p_cam = Rm.apply(p_map) + np.array([t.x, t.y, t.z]) + + # In camera coords, +Z points out through the lens. reject points behind camera + x_c, y_c, z_c = p_cam + if not np.all(np.isfinite(p_cam)) or z_c <= 0: + self.get_logger().info( + f"world_to_pixel: point not in front of camera (z_c={z_c})") + return False + + # Pinhole projection: divide by depth, then scale/shift into pixel coords + fx, fy, cx, cy = self.get_intrinsics(camera_info) + u = fx * (x_c / z_c) + cx + v = fy * (y_c / z_c) + cy + + # Save (u, v) as float64 bytes so the client can unpack them + self.serialized_message = np.asarray([u, v], dtype=np.float64).tobytes() + self.get_logger().info(f"world_to_pixel → (u={u:.2f}, v={v:.2f})") + return True def get_intrinsics(self, camera_info: CameraInfo) -> Tuple[float, float, float, float]: """ @@ -322,47 +330,64 @@ def pixel_to_world( v: float, ) -> Optional[np.ndarray]: """ - Converts pixel coordinate to world coordinate ("what 3d map point is this pixel looking at?") + Associate an image pixel with a nearby LiDAR return and return its map XYZ. + + A pixel alone is underdetermined (a ray). This does nearby-ray association: + project the latest cloud into the image and pick the nearest return within + MAX_PIXEL_RADIUS (not an exact ray–surface intersection). + + Process: + 1. Read intrinsics (fx, fy, cx, cy) and camera / cloud frame ids. + 2. Parse the PointCloud2 into an (N,3) XYZ array. + 3. TF the cloud into the camera frame (for projection) and into map + (for the answer). Prefer image_stamp for TF; else cloud stamp. + 4. For each return: drop non-finite / behind-camera (z_c <= 0) / + off-image projections; project with the pinhole model. + 5. Among returns within MAX_PIXEL_RADIUS of (u, v), keep the nearest + in image space; break near-ties with smaller z_c (frontmost). + 6. Return that return's map XYZ, or None if nothing is close enough. + + Returns: + np.ndarray shape (3,) in map, or None on failure / no association. """ fx, fy, cx, cy = self.get_intrinsics(camera_info) - camera_frame = camera_info.header.frame_id # where the camera lives in TF - src = cloud.header.frame_id # where the LiDAR points currently live + camera_frame = camera_info.header.frame_id # where the camera lives in TF + src = cloud.header.frame_id # where the LiDAR points currently live - points = self.cloud_to_xyz(cloud) # (N,3) in the cloud's own frame + points = self.cloud_to_xyz(cloud) # (N,3) in the cloud's own frame if points.size == 0: return None - # prefer image stamp for TF when provided; fall back to cloud stamp + # Prefer image stamp for TF when provided; fall back to cloud stamp stamp = image_stamp if (image_stamp.sec != 0 or image_stamp.nanosec != 0) else cloud.header.stamp - # cloud_cam[i] and cloud_map[i] are the same laser hit, just in different coordinates - # when we pick a winner in image space, we can easily get the map XYZ + # Same laser hit in two frames: project in camera, answer in map cloud_cam = self.transform_cloud(points, camera_frame, src, stamp) cloud_map = self.transform_cloud(points, "map", src, stamp) best = None # (d_pixels, z_c, p_map) - for p_cam, p_map in zip(cloud_cam, cloud_map): # each is [x, y, z] + for p_cam, p_map in zip(cloud_cam, cloud_map): # each is [x, y, z] x_c, y_c, z_c = p_cam - if not np.all(np.isfinite(p_cam)) or z_c <= 0: # skip bad / behind-camera points + if not np.all(np.isfinite(p_cam)) or z_c <= 0: # skip bad / behind-camera continue - - # project with pinhole formula + + # Project with pinhole formula ui = fx * (x_c / z_c) + cx vi = fy * (y_c / z_c) + cy if not (0 <= ui < camera_info.width and 0 <= vi < camera_info.height): continue - - # distance (how many pixels away the projected LiDAR hit (ui, vi) is from the query pixel (u, v)) + + # Image distance from this projected hit to the query pixel d = hypot(ui - u, vi - v) if d <= MAX_PIXEL_RADIUS: - # nearest pixel first; break near-ties by frontmost depth + # Nearest pixel first; break near-ties by frontmost depth if best is None or (round(d, 1), z_c) < (round(best[0], 1), best[1]): best = (d, z_c, p_map) if best is None: - return None # no LiDAR near this pixel - return best[2] # map point measured at cloud time + return None # no LiDAR near this pixel + return best[2] # map point measured at cloud time @@ -388,9 +413,15 @@ def callback(self,request, response): response.coords = self.serialized_message response.tf_success = True elif request.world_to_pixel == 1: - self.world_to_pixel(request.camera_name, request.stamp, (request.x, request.y, request.z)) - response.coords = self.serialized_message - response.tf_success = True + ok = self.world_to_pixel( + request.camera_name, request.stamp, + (request.x, request.y, request.z)) + if ok and self.serialized_message is not None: + response.coords = self.serialized_message + response.tf_success = True + else: + response.tf_success = False + response.coords = bytes() elif request.pixel_to_world == 1: camera_info = self.get_camera_info(request.camera_name) if camera_info is None: