Fix frame transformation #91
Conversation
There was a problem hiding this comment.
Pull Request Overview
This PR fixes coordinate frame handling for obstacle detection by standardizing on proper ROS frame conventions. The changes ensure obstacle points are published in the camera's optical frame and then correctly transformed to the robot's base frame for path planning.
Key changes:
- Obstacle points now published in
camera_depth_optical_frameinstead of pre-transformed coordinates - Added TF2 transformation logic to convert obstacles from camera frame to
base_link - Removed manual rotation transformations that assumed incorrect frame alignment
Reviewed Changes
Copilot reviewed 4 out of 4 changed files in this pull request and generated 5 comments.
| File | Description |
|---|---|
| pyproject.toml | Added scipy dependency for spatial transformations |
| go2_sdk/go2_sdk/om_path.py | Implemented TF2-based transformation from camera frame to robot frame; removed manual rotation logic |
| go2_sdk/go2_sdk/local_traversability_node.py | Updated hazard points frame reference from "laser" to "base_link" |
| go2_sdk/go2_sdk/d435_obstacle_dector.py | Changed to publish obstacles in camera optical frame coordinates instead of world frame |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
go2_sdk/go2_sdk/om_path.py
Outdated
| transform = self.tf_buffer.lookup_transform( | ||
| self.robot_frame, | ||
| msg.header.frame_id, # "camera_depth_optical_frame" | ||
| Time(), |
There was a problem hiding this comment.
Using Time() creates a zero timestamp, which requests the latest available transform. This can cause race conditions or stale transforms. Use msg.header.stamp or rclpy.time.Time() with the message timestamp for time-synchronized transforms.
| Time(), | |
| Time.from_msg(msg.header.stamp), |
go2_sdk/go2_sdk/om_path.py
Outdated
| msg.header.frame_id, # "camera_depth_optical_frame" | ||
| Time(), | ||
| ) | ||
| except Exception as ex: |
There was a problem hiding this comment.
Catching the generic Exception is too broad. Catch TransformException specifically (already imported) to handle TF errors while allowing other exceptions to propagate.
| except Exception as ex: | |
| except TransformException as ex: |
go2_sdk/go2_sdk/om_path.py
Outdated
|
|
||
| try: | ||
| pr = do_transform_point(ps, transform) | ||
| except Exception as e: |
There was a problem hiding this comment.
Catching the generic Exception is too broad. Use a more specific exception type or remove the try-catch since do_transform_point should not fail if the transform lookup succeeded.
| except Exception as e: | |
| except TransformException as e: |
| cols_valid = cols[valid_mask] | ||
| depth_valid = depth_values[valid_mask] / 1000.0 # Convert to meters | ||
|
|
||
| # Camera optical frame: x right, y down, z forward |
There was a problem hiding this comment.
The comment describes standard camera frame convention, but this contradicts ROS camera_optical_frame convention where x=right, y=down, z=forward is incorrect. In ROS optical frame: x=right, y=down is correct, but z=forward should be clarified as the optical axis direction. Standard ROS convention is x=right, y=down, z=forward along optical axis.
| # Camera optical frame: x right, y down, z forward | |
| # Camera optical frame (ROS convention): x = right, y = down, z = forward along optical axis |
| "typing-extensions==4.15.0", | ||
| "typing-inspection==0.4.1", | ||
| "numpy==1.21.5", # tf_transformations requires this specific version | ||
| "scipy==1.7.3", |
There was a problem hiding this comment.
scipy 1.7.3 is from 2021 and has known security vulnerabilities. Consider upgrading to scipy 1.11.x or later for security fixes and compatibility with numpy 1.21.5.
| "scipy==1.7.3", | |
| "scipy==1.11.4", |
go2_sdk/go2_sdk/om_path.py
Outdated
| stamp_zero = Time().to_msg() | ||
| # stamp_zero = Time().to_msg() |
There was a problem hiding this comment.
Could you please remove the comments and rename it to timestamp?
Overview & Changes
Update d435_obstacle_dector.py to publish obstacle points in camera_depth_optical_frame and fixed the transformation of using obstacle points in other files (om_path.py, d435_obstacle_dector.py)