From 9c1eeefb8d9722c1bfb428e6cd11d4a336a046a4 Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 3 Apr 2026 02:14:02 +0000 Subject: [PATCH 1/6] Reduce SimRobot charging radius --- src/mower_simulation/src/SimRobot.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp index 7f77d6bb0..835979a58 100644 --- a/src/mower_simulation/src/SimRobot.cpp +++ b/src/mower_simulation/src/SimRobot.cpp @@ -145,7 +145,7 @@ void SimRobot::SimulationStep(const ros::TimerEvent& te) { // Update Charger Status if (sqrt((docking_pos_x_ - pos_x_) * (docking_pos_x_ - pos_x_) + - (docking_pos_y_ - pos_y_) * (docking_pos_y_ - pos_y_)) < 0.5) { + (docking_pos_y_ - pos_y_) * (docking_pos_y_ - pos_y_)) < 0.025) { if (!is_charging_) { spdlog::info("Charging"); is_charging_ = true; From 266569329b3731e3f4dec6015f7f0b8f9fa7189a Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 3 Apr 2026 02:09:56 +0000 Subject: [PATCH 2/6] Fix diagonal SimRobot movement --- src/mower_simulation/src/SimRobot.cpp | 28 ++++++++++++++++----------- src/mower_simulation/src/SimRobot.h | 4 ++++ 2 files changed, 21 insertions(+), 11 deletions(-) diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp index 835979a58..264879c71 100644 --- a/src/mower_simulation/src/SimRobot.cpp +++ b/src/mower_simulation/src/SimRobot.cpp @@ -31,7 +31,8 @@ void SimRobot::Start() { pose_service_ = nh_.advertiseService("/xbot_positioning/set_robot_pose", &SimRobot::OnSetPose, this); odometry_pub_ = nh_.advertise("odom_out", 50); xbot_absolute_pose_pub_ = nh_.advertise("xb_pose_out", 50); - timer_ = nh_.createTimer(ros::Duration(0.1), &SimRobot::SimulationStep, this); + // Keep in sync with DiffDriveService::tick_schedule_ + timer_ = nh_.createTimer(ros::Duration(0.02), &SimRobot::SimulationStep, this); timer_.start(); } @@ -48,10 +49,8 @@ bool SimRobot::OnSetPose(xbot_positioning::SetPoseSrvRequest& req, xbot_position void SimRobot::GetTwist(double& vx, double& vr) { std::lock_guard lk{state_mutex_}; - vx = vx_; - vr = vr_; - vx += linear_speed_noise(generator); - vr += angular_speed_noise(generator); + vx = last_noisy_vx_; + vr = last_noisy_vr_; } void SimRobot::ResetEmergency() { @@ -131,12 +130,19 @@ void SimRobot::SimulationStep(const ros::TimerEvent& te) { // Update Position if not in emergency mode if (!emergency_latch_) { double time_diff_s = (now - last_update_).toSec(); - double delta_x = (vx_ * cos(pos_heading_)) * time_diff_s; - double delta_y = (vx_ * sin(pos_heading_)) * time_diff_s; - double delta_th = vr_ * time_diff_s; - pos_x_ += delta_x; - pos_y_ += delta_y; - pos_heading_ += delta_th; + double noisy_vx = vx_ + linear_speed_noise(generator); + double noisy_vr = vr_ + angular_speed_noise(generator); + last_noisy_vx_ = noisy_vx; + last_noisy_vr_ = noisy_vr; + if (fabs(noisy_vr) > 1e-6) { + double r = noisy_vx / noisy_vr; + pos_x_ += r * (sin(pos_heading_ + noisy_vr * time_diff_s) - sin(pos_heading_)); + pos_y_ -= r * (cos(pos_heading_ + noisy_vr * time_diff_s) - cos(pos_heading_)); + pos_heading_ += noisy_vr * time_diff_s; + } else { + pos_x_ += noisy_vx * cos(pos_heading_) * time_diff_s; + pos_y_ += noisy_vx * sin(pos_heading_) * time_diff_s; + } pos_heading_ = fmod(pos_heading_, M_PI * 2.0); if (pos_heading_ < 0) { pos_heading_ += M_PI * 2.0; diff --git a/src/mower_simulation/src/SimRobot.h b/src/mower_simulation/src/SimRobot.h index c57c95f4e..7266e6b94 100644 --- a/src/mower_simulation/src/SimRobot.h +++ b/src/mower_simulation/src/SimRobot.h @@ -65,6 +65,10 @@ class SimRobot { double pos_y_ = 0; double pos_heading_ = 0; + // Last noisy twist values (for encoder/IMU readback) + double last_noisy_vx_ = 0; + double last_noisy_vr_ = 0; + // Current Emergency State bool emergency_active_ = false; // Latched Emergency From d7adff965e8de2f01261c0758599a396010e9c5a Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 24 Apr 2026 21:04:48 +0000 Subject: [PATCH 3/6] Use hysteresis for charging state --- src/mower_simulation/src/SimRobot.cpp | 23 +++++++++++------------ 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp index 264879c71..2c39c9f5a 100644 --- a/src/mower_simulation/src/SimRobot.cpp +++ b/src/mower_simulation/src/SimRobot.cpp @@ -150,18 +150,17 @@ void SimRobot::SimulationStep(const ros::TimerEvent& te) { } // Update Charger Status - if (sqrt((docking_pos_x_ - pos_x_) * (docking_pos_x_ - pos_x_) + - (docking_pos_y_ - pos_y_) * (docking_pos_y_ - pos_y_)) < 0.025) { - if (!is_charging_) { - spdlog::info("Charging"); - is_charging_ = true; - charging_started_time = ros::Time::now(); - } - } else { - if (is_charging_) { - spdlog::info("Stopped Charging"); - is_charging_ = false; - } + // Hysteresis: engage when closer than 0.02 m, disengage only when farther than 0.03 m, + // preventing oscillation caused by noise in the integrated position. + const double dock_dist = sqrt((docking_pos_x_ - pos_x_) * (docking_pos_x_ - pos_x_) + + (docking_pos_y_ - pos_y_) * (docking_pos_y_ - pos_y_)); + if (!is_charging_ && dock_dist < 0.02) { + spdlog::info("Charging"); + is_charging_ = true; + charging_started_time = ros::Time::now(); + } else if (is_charging_ && dock_dist > 0.03) { + spdlog::info("Stopped Charging"); + is_charging_ = false; } if (is_charging_) { From 9a378dc256400b49ac02d3ab708b3aec666a2f48 Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 24 Apr 2026 20:56:42 +0000 Subject: [PATCH 4/6] Initialize last_update_ in SimRobot --- src/mower_simulation/src/SimRobot.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp index 2c39c9f5a..38781dff5 100644 --- a/src/mower_simulation/src/SimRobot.cpp +++ b/src/mower_simulation/src/SimRobot.cpp @@ -127,6 +127,11 @@ void SimRobot::GetIsCharging(bool& charging, double& seconds_since_start, std::s void SimRobot::SimulationStep(const ros::TimerEvent& te) { std::lock_guard lk{state_mutex_}; const auto now = ros::Time::now(); + if (last_update_.isZero()) { + last_update_ = now; + PublishPosition(); + return; + } // Update Position if not in emergency mode if (!emergency_latch_) { double time_diff_s = (now - last_update_).toSec(); From 972cf8d3c1c60b0ee4a8a4f2baa2413c48bfa876 Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 24 Apr 2026 21:23:18 +0000 Subject: [PATCH 5/6] Reset sim speed in emergency mode --- src/mower_simulation/src/SimRobot.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp index 38781dff5..ba178296a 100644 --- a/src/mower_simulation/src/SimRobot.cpp +++ b/src/mower_simulation/src/SimRobot.cpp @@ -133,7 +133,10 @@ void SimRobot::SimulationStep(const ros::TimerEvent& te) { return; } // Update Position if not in emergency mode - if (!emergency_latch_) { + if (emergency_latch_) { + last_noisy_vx_ = 0.0; + last_noisy_vr_ = 0.0; + } else { double time_diff_s = (now - last_update_).toSec(); double noisy_vx = vx_ + linear_speed_noise(generator); double noisy_vr = vr_ + angular_speed_noise(generator); From 3c62ef8a6ac941b1f3c6d6aa65762d0d614d5325 Mon Sep 17 00:00:00 2001 From: Robert Vollmer Date: Fri, 24 Apr 2026 21:20:32 +0000 Subject: [PATCH 6/6] No sim velocity noise at rest --- src/mower_simulation/src/SimRobot.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/mower_simulation/src/SimRobot.cpp b/src/mower_simulation/src/SimRobot.cpp index ba178296a..cf0973187 100644 --- a/src/mower_simulation/src/SimRobot.cpp +++ b/src/mower_simulation/src/SimRobot.cpp @@ -138,8 +138,12 @@ void SimRobot::SimulationStep(const ros::TimerEvent& te) { last_noisy_vr_ = 0.0; } else { double time_diff_s = (now - last_update_).toSec(); - double noisy_vx = vx_ + linear_speed_noise(generator); - double noisy_vr = vr_ + angular_speed_noise(generator); + // Skip noise when the robot is commanded to rest; a stationary mower does + // not random-walk its odometry, and unconditional noise integration would + // cause the simulated position to drift past the charging hysteresis window. + const bool at_rest = (vx_ == 0.0 && vr_ == 0.0); + double noisy_vx = at_rest ? 0.0 : vx_ + linear_speed_noise(generator); + double noisy_vr = at_rest ? 0.0 : vr_ + angular_speed_noise(generator); last_noisy_vx_ = noisy_vx; last_noisy_vr_ = noisy_vr; if (fabs(noisy_vr) > 1e-6) { @@ -166,6 +170,10 @@ void SimRobot::SimulationStep(const ros::TimerEvent& te) { spdlog::info("Charging"); is_charging_ = true; charging_started_time = ros::Time::now(); + // Snap to the exact docking pose to eliminate any pre-latch drift so the + // disengage threshold cannot be tripped while the robot sits on the dock. + pos_x_ = docking_pos_x_; + pos_y_ = docking_pos_y_; } else if (is_charging_ && dock_dist > 0.03) { spdlog::info("Stopped Charging"); is_charging_ = false;