Vision ukf#87
Open
AnthonyHoneine wants to merge 37 commits into
Open
Conversation
…'t publish either
saransh323
requested changes
Mar 13, 2026
saransh323
left a comment
saransh323
left a comment
Member
There was a problem hiding this comment.
Overall looking great! Noted a few smaller changes, especially the copied/moved files.
| self.create_subscription(Odometry, "vision/other/state", self.update_camera, 1) | ||
| self.create_subscription(Odometry, "lidar/other/state", self.update_lidar, 1) | ||
|
|
||
| self.nand_publisher = self.create_publisher(NavSatFix, "other/vision_fusion", 10) |
Member
There was a problem hiding this comment.
Needs to be changed to an Odometry message eventually (after upcoming rolls). Publish odometry here and convert to navsatfix in telematics.py.
Contributor
There was a problem hiding this comment.
Pull request overview
Adds simulated “vision stack” sensor topics (camera + lidar) in the 2D double sim and introduces a new UKF-based fusion node intended to estimate NAND’s position from those sensors (without radio).
Changes:
- Publish simulated camera/lidar detections for NAND from SC with range/FOV gating and randomized dropouts/corruption.
- Add a Vision UKF node that consumes the simulated vision/lidar odometry and publishes a fused NavSatFix estimate.
- Update launch/config/install wiring to run the new sim + fusion nodes and adjust topic naming/starting poses.
Reviewed changes
Copilot reviewed 11 out of 11 changed files in this pull request and generated 15 comments.
Show a summary per file
| File | Description |
|---|---|
rb_ws/src/buggy/scripts/vision/detector_node.py |
Hardcodes vision output topic to /SC/... |
rb_ws/src/buggy/scripts/util/odomToNavsatFix.py |
New helper to convert Odometry (UTM) → NavSatFix |
rb_ws/src/buggy/scripts/simulator/visionstack_sim.py |
New camera/lidar simulator publishers with FOV/range logic and noise/dropouts |
rb_ws/src/buggy/scripts/simulator/engine.py |
Adjusts sim starting poses |
rb_ws/src/buggy/scripts/estimation/vision_ukf_node.py |
New vision/lidar fusion UKF node |
rb_ws/src/buggy/scripts/estimation/ukf_utils.py |
Changes UKF update return values + adds variance/stddev helpers |
rb_ws/src/buggy/scripts/estimation/ukf_node.py |
New UKF node script added in this PR |
rb_ws/src/buggy/launch/sim_2d_double.xml |
Launches visionstack sim + vision fusion node |
rb_ws/src/buggy/launch/sc-main.xml |
Launches vision fusion node |
rb_ws/src/buggy/config/sim_double.yaml |
Adds sim parameters (steering delay, navsat noise std) |
rb_ws/src/buggy/CMakeLists.txt |
Installs visionstack_sim.py (but not the new UKF executables) |
💡 Add Copilot custom instructions for smarter, more guided reviews. Learn how to get started.
You can also share your feedback on Copilot code review. Take the survey.
| from nav_msgs.msg import Odometry | ||
|
|
||
|
|
||
| from ukf import * |
Comment on lines
+47
to
+48
| self.x_hat, self.Sigma = ukf_predict(self.x_hat, self.Sigma, self.Q, [self.steering], 0.01, [1.3]) | ||
|
|
Comment on lines
+78
to
+84
| newMsg = Odometry() | ||
| newMsg.pose.pose.position.x = self.x_hat[0] | ||
| newMsg.pose.pose.position.y = self.x_hat[1] | ||
| newMsg.pose.pose.orientation.z = self.x_hat[2] | ||
| newMsg.twist.twist.linear.x = self.x_hat[3] | ||
| self.nand_publisher.publish(odom_to_navsat(newMsg)) | ||
|
|
| from util.odomToNavsatFix import odom_to_navsat | ||
|
|
||
|
|
||
| from ukf import * |
Comment on lines
+56
to
+70
| self.x_hat = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, -np.pi/2, 0]) | ||
| dist = np.linalg.norm(self.x_hat) | ||
| self.R_lidar = self.get_lidar_acc_matrix(dist=dist) | ||
|
|
||
| y = [msg.pose.pose.position.x, msg.pose.pose.position.y] | ||
| self.x_hat, self.Sigma = ukf_update(self.x_hat, self.Sigma, y, self.R_lidar) | ||
|
|
||
| def update_camera(self, msg): | ||
|
|
||
| if not self.start: | ||
| self.start = True | ||
| self.x_hat = np.array([msg.pose.pose.position.x, msg.pose.pose.position.y, -np.pi/2, 0]) | ||
| dist = np.linalg.norm(self.x_hat) | ||
| self.R_camera = self.get_camera_acc_matrix(dist=dist) | ||
| y = [msg.pose.pose.position.x, msg.pose.pose.position.y] |
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
saransh323
requested changes
Mar 14, 2026
Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com>
…(not absolute position)
TiaSinghania
approved these changes
Apr 1, 2026
| x_hat_next = x_hat + K @ (y - z_hat) | ||
| Sigma_next = Sigma - K @ S @ np.transpose(K) | ||
|
|
||
| _ = singular_flag |
Contributor
There was a problem hiding this comment.
remove singular flag entirely if its just not used
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Sim 2d double now has topics for a pretend camera a lidar that spit out NAND poses that are kinda bad randomly and also only update when NAND is close enough and for the camera within its FOV from SC. This allows sim of that data.
Also make a UKF for sensor fusion and interpolation of vision data to estimate NAND's position without radio. Can potentially be tuned still and has kinda slow startup in sim (~10 seconds).