Drive PID from odometry stamps instead of a wall-clock poll - #22
Conversation
Fold pose updates into the controller callback and take dt from the odometry header so I/D terms follow sensor/sim time instead of a 100 ms Instant poll. Co-authored-by: Cursor <cursoragent@cursor.com>
![chatgpt-codex-connector[bot]](https://avatars.githubusercontent.com/in/1144995?s=60&v=4){kind=small}
There was a problem hiding this comment.
💡 Codex Review
Here are some automated review suggestions for this pull request.
Reviewed commit: a219aac4d3
ℹ️ About Codex in GitHub
Your team has set up Codex to review pull requests in this repo. Reviews are triggered when you
- Open a pull request for review
- Mark a draft as ready
- Comment "@codex review".
If Codex has suggestions, it will comment; otherwise it will react with 👍.
Codex can also answer questions or update the PR. Try commenting "@codex address that feedback".
|
|
||
| tokio::spawn(consume_inotify_stream()); | ||
| tokio::spawn(consume_map_sub(Arc::clone(&td))); | ||
| tokio::spawn(consume_odometry_sub(Arc::clone(&td))); |
There was a problem hiding this comment.
Deleted the consumer_odometry callback. Now we consume odometry inside go_to_goal.
There was a problem hiding this comment.
I would probably do it the other way around, so just do the "go to goal" stuff inside consume_odometry_sub
but really don't matter
| let current_cfg = cfg.load(); | ||
| let PidConfig { kp, ki, kd } = current_cfg.pid[&bridge_name]; | ||
| let tam_x_y_z_roll_pitch_yaw = ¤t_cfg.tam; | ||
|
|
There was a problem hiding this comment.
Deleted the instant::now() inside the loop. Now we get the dt from the msg.header.stamp msg::Time
This is to gaurrantee that we are using the clock of the odometry source. That its not the same in the simulation if we increase the clock rate.
…os2 fork. Point the submodule at Rumarino-Team/stonefish_ros2, pass fast_fixed_step / sim stamps / RTF cap from bringup, and run headless CI at 5x so PID dt follows pose time. Co-authored-by: Cursor <cursoragent@cursor.com>
2 participants
Summary
/bridge/odometrymessage instead of polling pose every 100 ms in a separate loop. Pose is still stored ontd.poseformove_to.dtfrom the odometry header stamp so I/D follow sensor/sim time rather thanInstant. Skip I/D on the first sample; if a later stamp is not increasing, skip I/D for that tick and log a warning.stonefish.launch.py(fast_fixed_step,use_sim_time_stamps,realtime_factor_cap). Headless CI/README call that launch file with CLI args; no extra wrapper launch file.This is the control-timing slice only. The larger
mission_executorrewrite stays on the localmainworking tree.Test plan
/bridge/thrusterspublishes at the odometry rate, not 10 Hz.move_to(missions still seetd.pose).dtshould track pose stamps, not wall clock.go_to_goalwarn and does not spike I/D.ros2 launch bringup stonefish.launch.pywithheadless:=true,hydrus_env_headless.scn,fast_fixed_step:=true,use_sim_time_stamps:=true,realtime_factor_cap:=5.0.