From 66966a858e09658b407a9c97c0f124ed5be53e0a Mon Sep 17 00:00:00 2001 From: aquintan Date: Thu, 7 May 2026 11:41:46 +0200 Subject: [PATCH 1/9] fix odometry plug-in --- .../gz_noisy_odometry/NoisyOdometryPlugin.hpp | 9 +-- .../gz_noisy_odometry/NoisyOdometryPlugin.cpp | 79 ++++++++----------- 2 files changed, 34 insertions(+), 54 deletions(-) diff --git a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp index 68a5fe9d6..c07d410c1 100644 --- a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp +++ b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp @@ -57,14 +57,11 @@ namespace custom_plugins // This is the state published as odom_noisy and it is allowed to drift. gz::math::Pose3d noisy_pose_internal_; - // True simulator pose used only to extract the real motion increment - // between consecutive simulation steps. - gz::math::Pose3d last_true_pose_; - std::chrono::steady_clock::duration last_update_time_{0}; - // Latest commanded velocities are still stored because they are useful - // as auxiliary information and to keep compatibility with the original plugin. + // Latest commanded velocities received from cmd_vel. + // These are integrated as the nominal motion, mirroring what wheel encoders + // would observe — including when the robot is physically blocked. double current_v_{0.0}; double current_w_{0.0}; diff --git a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp index 89c43ab25..a76d3bee5 100644 --- a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp +++ b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp @@ -78,7 +78,6 @@ namespace custom_plugins // After that, it evolves independently by integrating noisy motion // increments, so drift accumulates naturally over time. noisy_pose_internal_ = current_true_pose; - last_true_pose_ = current_true_pose; last_update_time_ = _info.simTime; initialized_ = true; return; @@ -90,28 +89,20 @@ namespace custom_plugins if (dt <= 0.0) return; last_update_time_ = _info.simTime; - // Compute the true relative motion between the last and current simulator pose. - // This gives a physically meaningful motion increment without publishing the - // simulator pose directly. - const gz::math::Vector3d delta_world = - current_true_pose.Pos() - last_true_pose_.Pos(); - - const double previous_true_yaw = last_true_pose_.Rot().Yaw(); - const double current_true_yaw = current_true_pose.Rot().Yaw(); - - // Express the translation increment in the previous robot frame so the - // odometry model operates in the body frame, as usual in planar robotics. - const gz::math::Vector3d delta_body = - last_true_pose_.Rot().RotateVectorReverse(delta_world); - - const double trans = std::sqrt( - delta_body.X() * delta_body.X() + delta_body.Y() * delta_body.Y()); - - const double rot1 = - (trans > 1e-9) ? NormalizeAngle(std::atan2(delta_body.Y(), delta_body.X())) : 0.0; + // Read the latest commanded velocities. These represent what the wheel + // encoders would observe, so they keep accumulating even when the robot is + // physically blocked by a wall — just as a real differential-drive robot would. + double v_cmd, w_cmd; + { + std::lock_guard lock(mutex_); + v_cmd = current_v_; + w_cmd = current_w_; + } - const double delta_yaw = NormalizeAngle(current_true_yaw - previous_true_yaw); - const double rot2 = NormalizeAngle(delta_yaw - rot1); + // Nominal motion increment derived from cmd_vel, equivalent to wheel-encoder + // dead-reckoning on a real robot. + const double trans = v_cmd * dt; + const double delta_rot = w_cmd * dt; // Odometry motion model: // - rotation uncertainty grows with both rotation and translation @@ -120,29 +111,25 @@ namespace custom_plugins // This is more robust than injecting noise directly on cmd_vel, and still // preserves the expected long-term drift because only noisy increments are // integrated into the internal odometric state. - const double sigma_rot1 = - alpha1_ * std::abs(rot1) + alpha2_ * std::abs(trans); const double sigma_trans = - alpha3_ * std::abs(trans) + alpha4_ * (std::abs(rot1) + std::abs(rot2)); - const double sigma_rot2 = - alpha1_ * std::abs(rot2) + alpha2_ * std::abs(trans); + alpha3_ * std::abs(trans) + alpha4_ * std::abs(delta_rot); + const double sigma_rot = + alpha1_ * std::abs(delta_rot) + alpha2_ * std::abs(trans); - const double rot1_noisy = rot1 + gz::math::Rand::DblNormal(0.0, sigma_rot1); - const double trans_noisy = trans + gz::math::Rand::DblNormal(0.0, sigma_trans); - const double rot2_noisy = rot2 + gz::math::Rand::DblNormal(0.0, sigma_rot2); + const double trans_noisy = trans + gz::math::Rand::DblNormal(0.0, sigma_trans); + const double rot_noisy = delta_rot + gz::math::Rand::DblNormal(0.0, sigma_rot); // Integrate the noisy relative motion over the internal odometry state. // This is the key point that makes the estimate drift over time instead of // snapping back to the true simulator pose. const double current_noisy_yaw = noisy_pose_internal_.Rot().Yaw(); - const double heading_after_rot1 = current_noisy_yaw + rot1_noisy; - const double new_noisy_yaw = - NormalizeAngle(current_noisy_yaw + rot1_noisy + rot2_noisy); + const double new_noisy_yaw = NormalizeAngle(current_noisy_yaw + rot_noisy); noisy_pose_internal_.Pos().X( - noisy_pose_internal_.Pos().X() + trans_noisy * std::cos(heading_after_rot1)); + noisy_pose_internal_.Pos().X() + trans_noisy * std::cos(current_noisy_yaw)); noisy_pose_internal_.Pos().Y( - noisy_pose_internal_.Pos().Y() + trans_noisy * std::sin(heading_after_rot1)); + noisy_pose_internal_.Pos().Y() + trans_noisy * std::sin(current_noisy_yaw)); + // Z is kept from the simulator so the message stays geometrically consistent. noisy_pose_internal_.Pos().Z(current_true_pose.Pos().Z()); noisy_pose_internal_.Rot() = gz::math::Quaterniond(0.0, 0.0, new_noisy_yaw); @@ -150,8 +137,8 @@ namespace custom_plugins // The published twist is consistent with the noisy increment that has just // been integrated. This keeps the message self-consistent. - last_noisy_linear_velocity_ = trans_noisy / dt; - last_noisy_angular_velocity_ = NormalizeAngle(rot1_noisy + rot2_noisy) / dt; + last_noisy_linear_velocity_ = trans_noisy / dt; + last_noisy_angular_velocity_ = rot_noisy / dt; nav_msgs::msg::Odometry odom_msg; odom_msg.header.stamp = ros_node_->now(); @@ -178,30 +165,26 @@ namespace custom_plugins // Fill the most relevant covariance entries with values consistent with the // motion model. The message remains simple, but no longer pretends to have // perfect certainty. - const double pose_var_xy = sigma_trans * sigma_trans; - const double pose_var_yaw = - (sigma_rot1 * sigma_rot1) + (sigma_rot2 * sigma_rot2); + const double pose_var_xy = sigma_trans * sigma_trans; + const double pose_var_yaw = sigma_rot * sigma_rot; - const double twist_var_v = pose_var_xy / (dt * dt); + const double twist_var_v = pose_var_xy / (dt * dt); const double twist_var_w = pose_var_yaw / (dt * dt); - odom_msg.pose.covariance[0] = pose_var_xy; - odom_msg.pose.covariance[7] = pose_var_xy; + odom_msg.pose.covariance[0] = pose_var_xy; + odom_msg.pose.covariance[7] = pose_var_xy; odom_msg.pose.covariance[14] = 1e-9; odom_msg.pose.covariance[21] = 1e-9; odom_msg.pose.covariance[28] = 1e-9; odom_msg.pose.covariance[35] = pose_var_yaw; - odom_msg.twist.covariance[0] = twist_var_v; - odom_msg.twist.covariance[7] = 1e-9; + odom_msg.twist.covariance[0] = twist_var_v; + odom_msg.twist.covariance[7] = 1e-9; odom_msg.twist.covariance[14] = 1e-9; odom_msg.twist.covariance[21] = 1e-9; odom_msg.twist.covariance[28] = 1e-9; odom_msg.twist.covariance[35] = twist_var_w; ros_pub_->publish(odom_msg); - - // Store the true pose only to extract the next real motion increment. - last_true_pose_ = current_true_pose; } } \ No newline at end of file From fb869db730151684236fe9043ca6977991cdf570 Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 8 May 2026 11:38:25 +0200 Subject: [PATCH 2/9] add a slip factor to add more realism --- .../gz_noisy_odometry/NoisyOdometryPlugin.hpp | 11 ++- .../gz_noisy_odometry/NoisyOdometryPlugin.cpp | 81 ++++++++++++++----- 2 files changed, 70 insertions(+), 22 deletions(-) diff --git a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp index c07d410c1..3b4d26a8c 100644 --- a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp +++ b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp @@ -57,11 +57,13 @@ namespace custom_plugins // This is the state published as odom_noisy and it is allowed to drift. gz::math::Pose3d noisy_pose_internal_; + // True simulator pose used only to extract the real motion increment + // between consecutive simulation steps. + gz::math::Pose3d last_true_pose_; + std::chrono::steady_clock::duration last_update_time_{0}; // Latest commanded velocities received from cmd_vel. - // These are integrated as the nominal motion, mirroring what wheel encoders - // would observe — including when the robot is physically blocked. double current_v_{0.0}; double current_w_{0.0}; @@ -80,6 +82,11 @@ namespace custom_plugins double alpha2_{0.0}; double alpha3_{0.0}; double alpha4_{0.0}; + + // Fraction of cmd_vel that leaks into the odometry when the robot is + // physically blocked (wall contact). Models wheel slip against the obstacle. + // Typical range: 0.01 – 0.05. + double slip_factor_{0.02}; }; } diff --git a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp index a76d3bee5..0b3fe05d7 100644 --- a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp +++ b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp @@ -45,6 +45,8 @@ namespace custom_plugins alpha3_ = _sdf->Get("alpha3", gaussian_noise_coeff_).first; alpha4_ = _sdf->Get("alpha4", gaussian_noise_coeff_).first; + slip_factor_ = _sdf->Get("slip_factor", 0.02).first; + gz_node_.Subscribe(gz_cmd_vel_topic_, &NoisyOdometryPlugin::OnCmdVel, this); if (!rclcpp::ok()) rclcpp::init(0, nullptr); @@ -78,6 +80,7 @@ namespace custom_plugins // After that, it evolves independently by integrating noisy motion // increments, so drift accumulates naturally over time. noisy_pose_internal_ = current_true_pose; + last_true_pose_ = current_true_pose; last_update_time_ = _info.simTime; initialized_ = true; return; @@ -89,9 +92,25 @@ namespace custom_plugins if (dt <= 0.0) return; last_update_time_ = _info.simTime; - // Read the latest commanded velocities. These represent what the wheel - // encoders would observe, so they keep accumulating even when the robot is - // physically blocked by a wall — just as a real differential-drive robot would. + // Compute the true relative motion between the last and current simulator pose. + // This gives a physically meaningful motion increment without publishing the + // simulator pose directly. + const gz::math::Vector3d delta_world = + current_true_pose.Pos() - last_true_pose_.Pos(); + + const double previous_true_yaw = last_true_pose_.Rot().Yaw(); + const double current_true_yaw = current_true_pose.Rot().Yaw(); + + // Express the translation increment in the previous robot frame so the + // odometry model operates in the body frame, as usual in planar robotics. + const gz::math::Vector3d delta_body = + last_true_pose_.Rot().RotateVectorReverse(delta_world); + + const double trans_true = std::sqrt( + delta_body.X() * delta_body.X() + delta_body.Y() * delta_body.Y()); + + const double delta_yaw_true = NormalizeAngle(current_true_yaw - previous_true_yaw); + double v_cmd, w_cmd; { std::lock_guard lock(mutex_); @@ -99,10 +118,24 @@ namespace custom_plugins w_cmd = current_w_; } - // Nominal motion increment derived from cmd_vel, equivalent to wheel-encoder - // dead-reckoning on a real robot. - const double trans = v_cmd * dt; - const double delta_rot = w_cmd * dt; + // Detect whether the robot is physically blocked: a non-negligible command + // is being sent but the simulator reports no real displacement. + // In that case a small fraction of the commanded motion leaks into the + // odometry, simulating wheel slip against the obstacle — exactly what real + // encoders would observe when the wheels spin without moving the chassis. + const double trans_cmd = v_cmd * dt; + const double delta_rot_cmd = w_cmd * dt; + + const bool is_blocked = + (std::abs(trans_cmd) > 1e-4 || std::abs(delta_rot_cmd) > 1e-4) && + (trans_true < 1e-4 && std::abs(delta_yaw_true) < 1e-4); + + const double trans = is_blocked ? trans_cmd * slip_factor_ : trans_true; + const double delta_yaw = is_blocked ? delta_rot_cmd * slip_factor_ : delta_yaw_true; + + const double rot1 = + (trans > 1e-9) ? NormalizeAngle(std::atan2(delta_body.Y(), delta_body.X())) : 0.0; + const double rot2 = NormalizeAngle(delta_yaw - rot1); // Odometry motion model: // - rotation uncertainty grows with both rotation and translation @@ -111,25 +144,29 @@ namespace custom_plugins // This is more robust than injecting noise directly on cmd_vel, and still // preserves the expected long-term drift because only noisy increments are // integrated into the internal odometric state. + const double sigma_rot1 = + alpha1_ * std::abs(rot1) + alpha2_ * std::abs(trans); const double sigma_trans = - alpha3_ * std::abs(trans) + alpha4_ * std::abs(delta_rot); - const double sigma_rot = - alpha1_ * std::abs(delta_rot) + alpha2_ * std::abs(trans); + alpha3_ * std::abs(trans) + alpha4_ * (std::abs(rot1) + std::abs(rot2)); + const double sigma_rot2 = + alpha1_ * std::abs(rot2) + alpha2_ * std::abs(trans); - const double trans_noisy = trans + gz::math::Rand::DblNormal(0.0, sigma_trans); - const double rot_noisy = delta_rot + gz::math::Rand::DblNormal(0.0, sigma_rot); + const double rot1_noisy = rot1 + gz::math::Rand::DblNormal(0.0, sigma_rot1); + const double trans_noisy = trans + gz::math::Rand::DblNormal(0.0, sigma_trans); + const double rot2_noisy = rot2 + gz::math::Rand::DblNormal(0.0, sigma_rot2); // Integrate the noisy relative motion over the internal odometry state. // This is the key point that makes the estimate drift over time instead of // snapping back to the true simulator pose. - const double current_noisy_yaw = noisy_pose_internal_.Rot().Yaw(); - const double new_noisy_yaw = NormalizeAngle(current_noisy_yaw + rot_noisy); + const double current_noisy_yaw = noisy_pose_internal_.Rot().Yaw(); + const double heading_after_rot1 = current_noisy_yaw + rot1_noisy; + const double new_noisy_yaw = + NormalizeAngle(current_noisy_yaw + rot1_noisy + rot2_noisy); noisy_pose_internal_.Pos().X( - noisy_pose_internal_.Pos().X() + trans_noisy * std::cos(current_noisy_yaw)); + noisy_pose_internal_.Pos().X() + trans_noisy * std::cos(heading_after_rot1)); noisy_pose_internal_.Pos().Y( - noisy_pose_internal_.Pos().Y() + trans_noisy * std::sin(current_noisy_yaw)); - // Z is kept from the simulator so the message stays geometrically consistent. + noisy_pose_internal_.Pos().Y() + trans_noisy * std::sin(heading_after_rot1)); noisy_pose_internal_.Pos().Z(current_true_pose.Pos().Z()); noisy_pose_internal_.Rot() = gz::math::Quaterniond(0.0, 0.0, new_noisy_yaw); @@ -137,8 +174,8 @@ namespace custom_plugins // The published twist is consistent with the noisy increment that has just // been integrated. This keeps the message self-consistent. - last_noisy_linear_velocity_ = trans_noisy / dt; - last_noisy_angular_velocity_ = rot_noisy / dt; + last_noisy_linear_velocity_ = trans_noisy / dt; + last_noisy_angular_velocity_ = NormalizeAngle(rot1_noisy + rot2_noisy) / dt; nav_msgs::msg::Odometry odom_msg; odom_msg.header.stamp = ros_node_->now(); @@ -166,7 +203,8 @@ namespace custom_plugins // motion model. The message remains simple, but no longer pretends to have // perfect certainty. const double pose_var_xy = sigma_trans * sigma_trans; - const double pose_var_yaw = sigma_rot * sigma_rot; + const double pose_var_yaw = + (sigma_rot1 * sigma_rot1) + (sigma_rot2 * sigma_rot2); const double twist_var_v = pose_var_xy / (dt * dt); const double twist_var_w = pose_var_yaw / (dt * dt); @@ -186,5 +224,8 @@ namespace custom_plugins odom_msg.twist.covariance[35] = twist_var_w; ros_pub_->publish(odom_msg); + + // Store the true pose only to extract the next real motion increment. + last_true_pose_ = current_true_pose; } } \ No newline at end of file From e8f988e7c9f6645f36439d5042a9da897bde9349 Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 22 May 2026 10:09:04 +0200 Subject: [PATCH 3/9] fix odometry plugin --- .../gz_noisy_odometry/NoisyOdometryPlugin.hpp | 13 ++++++ .../gz_noisy_odometry/NoisyOdometryPlugin.cpp | 44 ++++++++++++++++--- 2 files changed, 51 insertions(+), 6 deletions(-) diff --git a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp index 3b4d26a8c..68b2acfd4 100644 --- a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp +++ b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp @@ -87,6 +87,19 @@ namespace custom_plugins // physically blocked (wall contact). Models wheel slip against the obstacle. // Typical range: 0.01 – 0.05. double slip_factor_{0.02}; + + // Ratio of actual/commanded motion below which a robot is considered blocked. + // Avoids the fixed absolute threshold misfiring on slow robots or low-speed commands. + double block_ratio_threshold_{0.15}; + + // Fraction of longitudinal slip applied as zero-mean lateral noise when blocked. + // Models imperfect wheel-wall contact (small perpendicular micro-displacements). + double lateral_slip_ratio_{0.2}; + + // Consecutive simulation ticks the robot must appear blocked before slip + // activates, and the counter resets immediately on resumed motion. + int blocked_hysteresis_ticks_{3}; + int blocked_ticks_{0}; }; } diff --git a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp index 0b3fe05d7..93c266e94 100644 --- a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp +++ b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp @@ -45,7 +45,10 @@ namespace custom_plugins alpha3_ = _sdf->Get("alpha3", gaussian_noise_coeff_).first; alpha4_ = _sdf->Get("alpha4", gaussian_noise_coeff_).first; - slip_factor_ = _sdf->Get("slip_factor", 0.02).first; + slip_factor_ = _sdf->Get("slip_factor", 0.02).first; + block_ratio_threshold_ = _sdf->Get("block_ratio_threshold", 0.15).first; + lateral_slip_ratio_ = _sdf->Get("lateral_slip_ratio", 0.20).first; + blocked_hysteresis_ticks_ = _sdf->Get ("blocked_hysteresis_ticks", 3 ).first; gz_node_.Subscribe(gz_cmd_vel_topic_, &NoisyOdometryPlugin::OnCmdVel, this); @@ -123,18 +126,34 @@ namespace custom_plugins // In that case a small fraction of the commanded motion leaks into the // odometry, simulating wheel slip against the obstacle — exactly what real // encoders would observe when the wheels spin without moving the chassis. - const double trans_cmd = v_cmd * dt; + const double trans_cmd = v_cmd * dt; const double delta_rot_cmd = w_cmd * dt; - const bool is_blocked = - (std::abs(trans_cmd) > 1e-4 || std::abs(delta_rot_cmd) > 1e-4) && - (trans_true < 1e-4 && std::abs(delta_yaw_true) < 1e-4); + // Ratio-based detection: blocked when a commanded axis achieves less than + // block_ratio_threshold_ of the expected increment. More robust than a fixed + // absolute threshold, which can misfire on slow robots or tiny commands. + const bool is_commanded = std::abs(trans_cmd) > 1e-4 || std::abs(delta_rot_cmd) > 1e-4; + const bool linear_stalled = std::abs(trans_cmd) > 1e-4 && + trans_true < block_ratio_threshold_ * std::abs(trans_cmd); + const bool angular_stalled = std::abs(delta_rot_cmd) > 1e-4 && + std::abs(delta_yaw_true) < block_ratio_threshold_ * std::abs(delta_rot_cmd); + const bool blocked_raw = is_commanded && (linear_stalled || angular_stalled); + + // Hysteresis: require several consecutive blocked detections before + // activating slip, and reset immediately when motion resumes. + blocked_ticks_ = blocked_raw + ? std::min(blocked_ticks_ + 1, blocked_hysteresis_ticks_) + : 0; + const bool is_blocked = (blocked_ticks_ >= blocked_hysteresis_ticks_); const double trans = is_blocked ? trans_cmd * slip_factor_ : trans_true; const double delta_yaw = is_blocked ? delta_rot_cmd * slip_factor_ : delta_yaw_true; + // When blocked, delta_body is near-zero simulator noise so atan2 returns a + // random angle. Force rot1=0 so the slip integrates along the robot's + // current heading, which is the only physically meaningful direction. const double rot1 = - (trans > 1e-9) ? NormalizeAngle(std::atan2(delta_body.Y(), delta_body.X())) : 0.0; + (!is_blocked && trans > 1e-9) ? NormalizeAngle(std::atan2(delta_body.Y(), delta_body.X())) : 0.0; const double rot2 = NormalizeAngle(delta_yaw - rot1); // Odometry motion model: @@ -169,6 +188,19 @@ namespace custom_plugins noisy_pose_internal_.Pos().Y() + trans_noisy * std::sin(heading_after_rot1)); noisy_pose_internal_.Pos().Z(current_true_pose.Pos().Z()); + // When blocked, wheels spinning against a wall generate small random + // perpendicular micro-displacements (lateral slip). The direction is + // always 90° to the heading, so it cannot reverse the longitudinal slip. + if (is_blocked && lateral_slip_ratio_ > 0.0) + { + const double lateral = gz::math::Rand::DblNormal( + 0.0, std::abs(trans) * lateral_slip_ratio_); + noisy_pose_internal_.Pos().X( + noisy_pose_internal_.Pos().X() - lateral * std::sin(heading_after_rot1)); + noisy_pose_internal_.Pos().Y( + noisy_pose_internal_.Pos().Y() + lateral * std::cos(heading_after_rot1)); + } + noisy_pose_internal_.Rot() = gz::math::Quaterniond(0.0, 0.0, new_noisy_yaw); noisy_pose_internal_.Rot().Normalize(); From 767d983a43bf8a40242c79144fd4171ba29a1654 Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 22 May 2026 10:43:47 +0200 Subject: [PATCH 4/9] remove gazebo tick counter --- .../gz_noisy_odometry/NoisyOdometryPlugin.hpp | 4 ---- .../gz_noisy_odometry/NoisyOdometryPlugin.cpp | 19 +++++++------------ 2 files changed, 7 insertions(+), 16 deletions(-) diff --git a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp index 68b2acfd4..632cf7e29 100644 --- a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp +++ b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp @@ -96,10 +96,6 @@ namespace custom_plugins // Models imperfect wheel-wall contact (small perpendicular micro-displacements). double lateral_slip_ratio_{0.2}; - // Consecutive simulation ticks the robot must appear blocked before slip - // activates, and the counter resets immediately on resumed motion. - int blocked_hysteresis_ticks_{3}; - int blocked_ticks_{0}; }; } diff --git a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp index 93c266e94..906eeb8ba 100644 --- a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp +++ b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp @@ -45,10 +45,9 @@ namespace custom_plugins alpha3_ = _sdf->Get("alpha3", gaussian_noise_coeff_).first; alpha4_ = _sdf->Get("alpha4", gaussian_noise_coeff_).first; - slip_factor_ = _sdf->Get("slip_factor", 0.02).first; - block_ratio_threshold_ = _sdf->Get("block_ratio_threshold", 0.15).first; - lateral_slip_ratio_ = _sdf->Get("lateral_slip_ratio", 0.20).first; - blocked_hysteresis_ticks_ = _sdf->Get ("blocked_hysteresis_ticks", 3 ).first; + slip_factor_ = _sdf->Get("slip_factor", 0.02).first; + block_ratio_threshold_ = _sdf->Get("block_ratio_threshold", 0.15).first; + lateral_slip_ratio_ = _sdf->Get("lateral_slip_ratio", 0.20).first; gz_node_.Subscribe(gz_cmd_vel_topic_, &NoisyOdometryPlugin::OnCmdVel, this); @@ -132,19 +131,15 @@ namespace custom_plugins // Ratio-based detection: blocked when a commanded axis achieves less than // block_ratio_threshold_ of the expected increment. More robust than a fixed // absolute threshold, which can misfire on slow robots or tiny commands. + // Single-tick detection (no hysteresis): the physics engine makes the robot + // bounce on alternate ticks against a wall, so any multi-tick accumulator + // would never trigger. const bool is_commanded = std::abs(trans_cmd) > 1e-4 || std::abs(delta_rot_cmd) > 1e-4; const bool linear_stalled = std::abs(trans_cmd) > 1e-4 && trans_true < block_ratio_threshold_ * std::abs(trans_cmd); const bool angular_stalled = std::abs(delta_rot_cmd) > 1e-4 && std::abs(delta_yaw_true) < block_ratio_threshold_ * std::abs(delta_rot_cmd); - const bool blocked_raw = is_commanded && (linear_stalled || angular_stalled); - - // Hysteresis: require several consecutive blocked detections before - // activating slip, and reset immediately when motion resumes. - blocked_ticks_ = blocked_raw - ? std::min(blocked_ticks_ + 1, blocked_hysteresis_ticks_) - : 0; - const bool is_blocked = (blocked_ticks_ >= blocked_hysteresis_ticks_); + const bool is_blocked = is_commanded && (linear_stalled || angular_stalled); const double trans = is_blocked ? trans_cmd * slip_factor_ : trans_true; const double delta_yaw = is_blocked ? delta_rot_cmd * slip_factor_ : delta_yaw_true; From e6dfdb1afbb8878d3d0719f64eb2cbbdbaf683e3 Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 22 May 2026 11:01:00 +0200 Subject: [PATCH 5/9] odometry fix --- .../gz_noisy_odometry/NoisyOdometryPlugin.hpp | 4 ---- .../gz_noisy_odometry/NoisyOdometryPlugin.cpp | 23 +++++++------------ 2 files changed, 8 insertions(+), 19 deletions(-) diff --git a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp index 632cf7e29..fe0e4e417 100644 --- a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp +++ b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp @@ -88,10 +88,6 @@ namespace custom_plugins // Typical range: 0.01 – 0.05. double slip_factor_{0.02}; - // Ratio of actual/commanded motion below which a robot is considered blocked. - // Avoids the fixed absolute threshold misfiring on slow robots or low-speed commands. - double block_ratio_threshold_{0.15}; - // Fraction of longitudinal slip applied as zero-mean lateral noise when blocked. // Models imperfect wheel-wall contact (small perpendicular micro-displacements). double lateral_slip_ratio_{0.2}; diff --git a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp index 906eeb8ba..c642ff436 100644 --- a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp +++ b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp @@ -45,9 +45,8 @@ namespace custom_plugins alpha3_ = _sdf->Get("alpha3", gaussian_noise_coeff_).first; alpha4_ = _sdf->Get("alpha4", gaussian_noise_coeff_).first; - slip_factor_ = _sdf->Get("slip_factor", 0.02).first; - block_ratio_threshold_ = _sdf->Get("block_ratio_threshold", 0.15).first; - lateral_slip_ratio_ = _sdf->Get("lateral_slip_ratio", 0.20).first; + slip_factor_ = _sdf->Get("slip_factor", 0.02).first; + lateral_slip_ratio_ = _sdf->Get("lateral_slip_ratio", 0.20).first; gz_node_.Subscribe(gz_cmd_vel_topic_, &NoisyOdometryPlugin::OnCmdVel, this); @@ -128,18 +127,12 @@ namespace custom_plugins const double trans_cmd = v_cmd * dt; const double delta_rot_cmd = w_cmd * dt; - // Ratio-based detection: blocked when a commanded axis achieves less than - // block_ratio_threshold_ of the expected increment. More robust than a fixed - // absolute threshold, which can misfire on slow robots or tiny commands. - // Single-tick detection (no hysteresis): the physics engine makes the robot - // bounce on alternate ticks against a wall, so any multi-tick accumulator - // would never trigger. - const bool is_commanded = std::abs(trans_cmd) > 1e-4 || std::abs(delta_rot_cmd) > 1e-4; - const bool linear_stalled = std::abs(trans_cmd) > 1e-4 && - trans_true < block_ratio_threshold_ * std::abs(trans_cmd); - const bool angular_stalled = std::abs(delta_rot_cmd) > 1e-4 && - std::abs(delta_yaw_true) < block_ratio_threshold_ * std::abs(delta_rot_cmd); - const bool is_blocked = is_commanded && (linear_stalled || angular_stalled); + // Blocked when something is commanded but the simulator reports no real + // displacement. Single-tick, absolute threshold: ratio-based approaches + // misfire during acceleration because trans_true lags trans_cmd legitimately. + const bool is_blocked = + (std::abs(trans_cmd) > 1e-4 || std::abs(delta_rot_cmd) > 1e-4) && + (trans_true < 1e-4 && std::abs(delta_yaw_true) < 1e-4); const double trans = is_blocked ? trans_cmd * slip_factor_ : trans_true; const double delta_yaw = is_blocked ? delta_rot_cmd * slip_factor_ : delta_yaw_true; From e3b0fbbac88f2f23a9ea9d5bc0619e6c1331f363 Mon Sep 17 00:00:00 2001 From: aquintan Date: Fri, 22 May 2026 11:21:08 +0200 Subject: [PATCH 6/9] add slip factor to turtlebot models --- CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf | 2 ++ CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf | 2 ++ CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf | 2 ++ 3 files changed, 6 insertions(+) diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf index 1345dff32..c736406df 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf @@ -566,6 +566,8 @@ 0.005 0.003 0.003 + 0.15 + 0.25 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf index c47fc7de2..74f9cde88 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf @@ -566,6 +566,8 @@ 0.0005 0.0002 0.0002 + 0.05 + 0.15 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf index 5f3714754..dea88c5fb 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf @@ -566,6 +566,8 @@ 0.002 0.001 0.001 + 0.10 + 0.20 From 3c51ab62d2e5e7cada86d81f6dff0f107de74fd0 Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 25 May 2026 18:05:10 +0200 Subject: [PATCH 7/9] gazebo noisy odometry, using diff drive plugin --- .../urdf/rover_4wd_noisy_high.urdf | 2 + .../urdf/rover_4wd_noisy_low.urdf | 2 + .../models/turtlebot3_noisy_high/model.sdf | 9 +- .../models/turtlebot3_noisy_low/model.sdf | 9 +- .../models/turtlebot3_noisy_med/model.sdf | 9 +- Industrial/gz_noisy_odometry/CMakeLists.txt | 8 - .../gz_noisy_odometry/NoisyOdometryPlugin.hpp | 64 ++--- Industrial/gz_noisy_odometry/package.xml | 1 - .../gz_noisy_odometry/NoisyOdometryPlugin.cpp | 247 ++++++------------ 9 files changed, 128 insertions(+), 223 deletions(-) diff --git a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf index b461a8c5f..dc856c639 100644 --- a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf +++ b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf @@ -213,6 +213,8 @@ 0.005 0.003 0.003 + 0.15 + 0.25 diff --git a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf index b4d97a1ae..720f34418 100644 --- a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf +++ b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf @@ -213,6 +213,8 @@ 0.0005 0.0002 0.0002 + 0.05 + 0.15 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf index c736406df..38ced2c11 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf @@ -558,16 +558,19 @@ /turtlebot3/odom_noisy - /turtlebot3/cmd_vel odom base_footprint + + wheel_left_joint + wheel_right_joint + 0.033 + 0.287 + 0.005 0.005 0.003 0.003 - 0.15 - 0.25 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf index 74f9cde88..d6e363f5d 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf @@ -558,16 +558,19 @@ /turtlebot3/odom_noisy - /turtlebot3/cmd_vel odom base_footprint + + wheel_left_joint + wheel_right_joint + 0.033 + 0.287 + 0.0005 0.0005 0.0002 0.0002 - 0.05 - 0.15 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf index dea88c5fb..a38e1c246 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf @@ -558,16 +558,19 @@ /turtlebot3/odom_noisy - /turtlebot3/cmd_vel odom base_footprint + + wheel_left_joint + wheel_right_joint + 0.033 + 0.287 + 0.002 0.002 0.001 0.001 - 0.10 - 0.20 diff --git a/Industrial/gz_noisy_odometry/CMakeLists.txt b/Industrial/gz_noisy_odometry/CMakeLists.txt index 2980012bb..047dac573 100644 --- a/Industrial/gz_noisy_odometry/CMakeLists.txt +++ b/Industrial/gz_noisy_odometry/CMakeLists.txt @@ -21,14 +21,11 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(nav_msgs REQUIRED) -find_package(tf2 REQUIRED) find_package(gz-plugin2 REQUIRED COMPONENTS register) find_package(gz-sim8 REQUIRED) -find_package(gz-msgs10 REQUIRED) find_package(gz-math7 REQUIRED) find_package(gz-common5 REQUIRED) -find_package(gz-transport13 REQUIRED) # ============================================================================ # Noisy Odometry plugin @@ -42,22 +39,17 @@ target_include_directories(NoisyOdometryPlugin PUBLIC $ ) -# Dependencias de ROS 2 ament_target_dependencies(NoisyOdometryPlugin rclcpp nav_msgs - tf2 ) -# Enlace de librerías de Gazebo calcando la sintaxis del proyecto que funciona target_link_libraries(NoisyOdometryPlugin gz-plugin2::gz-plugin2 gz-plugin2::register gz-sim8::gz-sim8 - gz-msgs10::gz-msgs10 gz-math7::gz-math7 gz-common5::gz-common5 - gz-transport13::gz-transport13 ) install( diff --git a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp index fe0e4e417..d8a8feaed 100644 --- a/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp +++ b/Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp @@ -3,15 +3,11 @@ #include #include -#include -#include -#include #include #include #include -#include #include #include @@ -35,64 +31,44 @@ namespace custom_plugins const gz::sim::EntityComponentManager &_ecm) override; private: - void OnCmdVel(const gz::msgs::Twist &_msg); - - // Keeps angles bounded in [-pi, pi] so yaw differences remain well behaved. double NormalizeAngle(double _angle) const; gz::sim::Model model_; - gz::transport::Node gz_node_; std::shared_ptr ros_node_; rclcpp::Publisher::SharedPtr ros_pub_; std::string ros_topic_; - std::string gz_cmd_vel_topic_; std::string frame_id_; std::string child_frame_id_; - bool initialized_{false}; + // Wheel joint entities resolved once in Configure. + gz::sim::Entity left_joint_entity_{gz::sim::kNullEntity}; + gz::sim::Entity right_joint_entity_{gz::sim::kNullEntity}; - // Internal odometric estimate that is propagated only with noisy increments. - // This is the state published as odom_noisy and it is allowed to drift. - gz::math::Pose3d noisy_pose_internal_; + // Differential drive kinematics — must match the DiffDrive plugin values. + double wheel_radius_{0.033}; + double wheel_separation_{0.287}; - // True simulator pose used only to extract the real motion increment - // between consecutive simulation steps. - gz::math::Pose3d last_true_pose_; + // Last joint positions used to compute per-tick angle deltas. + double last_left_pos_{0.0}; + double last_right_pos_{0.0}; std::chrono::steady_clock::duration last_update_time_{0}; - // Latest commanded velocities received from cmd_vel. - double current_v_{0.0}; - double current_w_{0.0}; - - // Last noisy body-frame velocities published in the odometry message. - double last_noisy_linear_velocity_{0.0}; - double last_noisy_angular_velocity_{0.0}; - - std::mutex mutex_; - - // Legacy generic noise parameter kept for compatibility with the original SDF. - double gaussian_noise_coeff_{0.0}; - - // Standard odometry motion-model coefficients. - // They control how rotation and translation uncertainties affect each other. - double alpha1_{0.0}; - double alpha2_{0.0}; - double alpha3_{0.0}; - double alpha4_{0.0}; - - // Fraction of cmd_vel that leaks into the odometry when the robot is - // physically blocked (wall contact). Models wheel slip against the obstacle. - // Typical range: 0.01 – 0.05. - double slip_factor_{0.02}; + bool initialized_{false}; - // Fraction of longitudinal slip applied as zero-mean lateral noise when blocked. - // Models imperfect wheel-wall contact (small perpendicular micro-displacements). - double lateral_slip_ratio_{0.2}; + // Accumulated noisy odometry estimate. + double noisy_x_{0.0}; + double noisy_y_{0.0}; + double noisy_yaw_{0.0}; + // Standard odometry motion-model coefficients (Probabilistic Robotics, Ch. 5). + double alpha1_{0.0}; // rotation noise from rotation + double alpha2_{0.0}; // rotation noise from translation + double alpha3_{0.0}; // translation noise from translation + double alpha4_{0.0}; // translation noise from rotation }; } -#endif \ No newline at end of file +#endif diff --git a/Industrial/gz_noisy_odometry/package.xml b/Industrial/gz_noisy_odometry/package.xml index 6893a25c9..d1edcfbfa 100644 --- a/Industrial/gz_noisy_odometry/package.xml +++ b/Industrial/gz_noisy_odometry/package.xml @@ -11,7 +11,6 @@ rclcpp nav_msgs - tf2 ament_cmake diff --git a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp index c642ff436..e4d0949df 100644 --- a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp +++ b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp @@ -1,7 +1,6 @@ #include "gz_noisy_odometry/NoisyOdometryPlugin.hpp" -#include +#include #include -#include #include @@ -19,7 +18,7 @@ namespace custom_plugins double NoisyOdometryPlugin::NormalizeAngle(double _angle) const { - while (_angle > M_PI) _angle -= 2.0 * M_PI; + while (_angle > M_PI) _angle -= 2.0 * M_PI; while (_angle < -M_PI) _angle += 2.0 * M_PI; return _angle; } @@ -32,23 +31,38 @@ namespace custom_plugins model_ = gz::sim::Model(_entity); if (!model_.Valid(_ecm)) return; - gaussian_noise_coeff_ = _sdf->Get("gaussian_noise", 0.05).first; - ros_topic_ = _sdf->Get("ros_topic", "/turtlebot3/odom_noisy").first; - gz_cmd_vel_topic_ = _sdf->Get("gz_cmd_vel_topic", "/turtlebot3/cmd_vel").first; - frame_id_ = _sdf->Get("frame_id", "odom").first; + // Kinematics — must match the DiffDrive plugin values in the same SDF. + wheel_radius_ = _sdf->Get("wheel_radius", 0.033).first; + wheel_separation_ = _sdf->Get("wheel_separation", 0.287).first; + + const std::string left_joint_name = _sdf->Get("left_joint", "wheel_left_joint").first; + const std::string right_joint_name = _sdf->Get("right_joint", "wheel_right_joint").first; + + ros_topic_ = _sdf->Get("ros_topic", "/turtlebot3/odom_noisy").first; + frame_id_ = _sdf->Get("frame_id", "odom").first; child_frame_id_ = _sdf->Get("child_frame_id", "base_footprint").first; - // If the SDF does not provide the odometry motion-model coefficients - // explicitly, derive reasonable defaults from the legacy noise parameter. - alpha1_ = _sdf->Get("alpha1", gaussian_noise_coeff_).first; - alpha2_ = _sdf->Get("alpha2", gaussian_noise_coeff_).first; - alpha3_ = _sdf->Get("alpha3", gaussian_noise_coeff_).first; - alpha4_ = _sdf->Get("alpha4", gaussian_noise_coeff_).first; + // A single gaussian_noise value maps to all alphas as a convenience fallback. + const double default_noise = _sdf->Get("gaussian_noise", 0.05).first; + alpha1_ = _sdf->Get("alpha1", default_noise).first; + alpha2_ = _sdf->Get("alpha2", default_noise).first; + alpha3_ = _sdf->Get("alpha3", default_noise).first; + alpha4_ = _sdf->Get("alpha4", default_noise).first; + + // Resolve wheel joints and request JointPosition so Gazebo tracks them from step 0. + left_joint_entity_ = model_.JointByName(_ecm, left_joint_name); + right_joint_entity_ = model_.JointByName(_ecm, right_joint_name); - slip_factor_ = _sdf->Get("slip_factor", 0.02).first; - lateral_slip_ratio_ = _sdf->Get("lateral_slip_ratio", 0.20).first; + if (left_joint_entity_ == gz::sim::kNullEntity || + right_joint_entity_ == gz::sim::kNullEntity) + { + gzerr << "[NoisyOdometryPlugin] Wheel joints not found: '" + << left_joint_name << "', '" << right_joint_name << "'\n"; + return; + } - gz_node_.Subscribe(gz_cmd_vel_topic_, &NoisyOdometryPlugin::OnCmdVel, this); + _ecm.CreateComponent(left_joint_entity_, gz::sim::components::JointPosition()); + _ecm.CreateComponent(right_joint_entity_, gz::sim::components::JointPosition()); if (!rclcpp::ok()) rclcpp::init(0, nullptr); ros_node_ = rclcpp::Node::make_shared("noisy_odom_node_" + model_.Name(_ecm)); @@ -58,32 +72,26 @@ namespace custom_plugins ros_pub_ = ros_node_->create_publisher(ros_topic_, qos); } - void NoisyOdometryPlugin::OnCmdVel(const gz::msgs::Twist &_msg) - { - std::lock_guard lock(mutex_); - current_v_ = _msg.linear().x(); - current_w_ = _msg.angular().z(); - } - void NoisyOdometryPlugin::PostUpdate(const gz::sim::UpdateInfo &_info, const gz::sim::EntityComponentManager &_ecm) { if (_info.paused) return; - auto poseComp = _ecm.Component(model_.Entity()); - if (!poseComp) return; + auto *leftComp = _ecm.Component(left_joint_entity_); + auto *rightComp = _ecm.Component(right_joint_entity_); - const gz::math::Pose3d current_true_pose = poseComp->Data(); + if (!leftComp || !rightComp || + leftComp->Data().empty() || rightComp->Data().empty()) return; + + const double left_pos = leftComp->Data()[0]; + const double right_pos = rightComp->Data()[0]; if (!initialized_) { - // The noisy odometry starts aligned with the true pose only once. - // After that, it evolves independently by integrating noisy motion - // increments, so drift accumulates naturally over time. - noisy_pose_internal_ = current_true_pose; - last_true_pose_ = current_true_pose; + last_left_pos_ = left_pos; + last_right_pos_ = right_pos; last_update_time_ = _info.simTime; - initialized_ = true; + initialized_ = true; return; } @@ -93,141 +101,61 @@ namespace custom_plugins if (dt <= 0.0) return; last_update_time_ = _info.simTime; - // Compute the true relative motion between the last and current simulator pose. - // This gives a physically meaningful motion increment without publishing the - // simulator pose directly. - const gz::math::Vector3d delta_world = - current_true_pose.Pos() - last_true_pose_.Pos(); + // Per-tick wheel angle increments — the encoder analog. + // When the robot is blocked against a wall the wheel joints still rotate + // (the motor keeps spinning), so drift accumulates naturally without any + // special-case logic, exactly as real encoders would behave. + const double d_left = left_pos - last_left_pos_; + const double d_right = right_pos - last_right_pos_; - const double previous_true_yaw = last_true_pose_.Rot().Yaw(); - const double current_true_yaw = current_true_pose.Rot().Yaw(); + last_left_pos_ = left_pos; + last_right_pos_ = right_pos; - // Express the translation increment in the previous robot frame so the - // odometry model operates in the body frame, as usual in planar robotics. - const gz::math::Vector3d delta_body = - last_true_pose_.Rot().RotateVectorReverse(delta_world); + // Differential drive kinematics: arc lengths into linear and angular motion. + const double ds = wheel_radius_ * (d_right + d_left) / 2.0; + const double dtheta = wheel_radius_ * (d_right - d_left) / wheel_separation_; - const double trans_true = std::sqrt( - delta_body.X() * delta_body.X() + delta_body.Y() * delta_body.Y()); + // Odometry motion-model noise (Probabilistic Robotics, Ch. 5). + // Translation error grows with distance and heading change; rotation error + // grows with heading change and distance — both accumulate over time. + const double sigma_trans = alpha3_ * std::abs(ds) + alpha4_ * std::abs(dtheta); + const double sigma_rot = alpha1_ * std::abs(dtheta) + alpha2_ * std::abs(ds); - const double delta_yaw_true = NormalizeAngle(current_true_yaw - previous_true_yaw); + const double ds_noisy = ds + gz::math::Rand::DblNormal(0.0, sigma_trans); + const double dtheta_noisy = dtheta + gz::math::Rand::DblNormal(0.0, sigma_rot); - double v_cmd, w_cmd; - { - std::lock_guard lock(mutex_); - v_cmd = current_v_; - w_cmd = current_w_; - } - - // Detect whether the robot is physically blocked: a non-negligible command - // is being sent but the simulator reports no real displacement. - // In that case a small fraction of the commanded motion leaks into the - // odometry, simulating wheel slip against the obstacle — exactly what real - // encoders would observe when the wheels spin without moving the chassis. - const double trans_cmd = v_cmd * dt; - const double delta_rot_cmd = w_cmd * dt; - - // Blocked when something is commanded but the simulator reports no real - // displacement. Single-tick, absolute threshold: ratio-based approaches - // misfire during acceleration because trans_true lags trans_cmd legitimately. - const bool is_blocked = - (std::abs(trans_cmd) > 1e-4 || std::abs(delta_rot_cmd) > 1e-4) && - (trans_true < 1e-4 && std::abs(delta_yaw_true) < 1e-4); - - const double trans = is_blocked ? trans_cmd * slip_factor_ : trans_true; - const double delta_yaw = is_blocked ? delta_rot_cmd * slip_factor_ : delta_yaw_true; - - // When blocked, delta_body is near-zero simulator noise so atan2 returns a - // random angle. Force rot1=0 so the slip integrates along the robot's - // current heading, which is the only physically meaningful direction. - const double rot1 = - (!is_blocked && trans > 1e-9) ? NormalizeAngle(std::atan2(delta_body.Y(), delta_body.X())) : 0.0; - const double rot2 = NormalizeAngle(delta_yaw - rot1); - - // Odometry motion model: - // - rotation uncertainty grows with both rotation and translation - // - translation uncertainty grows with both translation and turning - // - // This is more robust than injecting noise directly on cmd_vel, and still - // preserves the expected long-term drift because only noisy increments are - // integrated into the internal odometric state. - const double sigma_rot1 = - alpha1_ * std::abs(rot1) + alpha2_ * std::abs(trans); - const double sigma_trans = - alpha3_ * std::abs(trans) + alpha4_ * (std::abs(rot1) + std::abs(rot2)); - const double sigma_rot2 = - alpha1_ * std::abs(rot2) + alpha2_ * std::abs(trans); - - const double rot1_noisy = rot1 + gz::math::Rand::DblNormal(0.0, sigma_rot1); - const double trans_noisy = trans + gz::math::Rand::DblNormal(0.0, sigma_trans); - const double rot2_noisy = rot2 + gz::math::Rand::DblNormal(0.0, sigma_rot2); - - // Integrate the noisy relative motion over the internal odometry state. - // This is the key point that makes the estimate drift over time instead of - // snapping back to the true simulator pose. - const double current_noisy_yaw = noisy_pose_internal_.Rot().Yaw(); - const double heading_after_rot1 = current_noisy_yaw + rot1_noisy; - const double new_noisy_yaw = - NormalizeAngle(current_noisy_yaw + rot1_noisy + rot2_noisy); - - noisy_pose_internal_.Pos().X( - noisy_pose_internal_.Pos().X() + trans_noisy * std::cos(heading_after_rot1)); - noisy_pose_internal_.Pos().Y( - noisy_pose_internal_.Pos().Y() + trans_noisy * std::sin(heading_after_rot1)); - noisy_pose_internal_.Pos().Z(current_true_pose.Pos().Z()); - - // When blocked, wheels spinning against a wall generate small random - // perpendicular micro-displacements (lateral slip). The direction is - // always 90° to the heading, so it cannot reverse the longitudinal slip. - if (is_blocked && lateral_slip_ratio_ > 0.0) - { - const double lateral = gz::math::Rand::DblNormal( - 0.0, std::abs(trans) * lateral_slip_ratio_); - noisy_pose_internal_.Pos().X( - noisy_pose_internal_.Pos().X() - lateral * std::sin(heading_after_rot1)); - noisy_pose_internal_.Pos().Y( - noisy_pose_internal_.Pos().Y() + lateral * std::cos(heading_after_rot1)); - } - - noisy_pose_internal_.Rot() = gz::math::Quaterniond(0.0, 0.0, new_noisy_yaw); - noisy_pose_internal_.Rot().Normalize(); - - // The published twist is consistent with the noisy increment that has just - // been integrated. This keeps the message self-consistent. - last_noisy_linear_velocity_ = trans_noisy / dt; - last_noisy_angular_velocity_ = NormalizeAngle(rot1_noisy + rot2_noisy) / dt; + // Integrate using the midpoint heading to reduce discretisation error. + const double heading_mid = noisy_yaw_ + dtheta_noisy / 2.0; + noisy_x_ += ds_noisy * std::cos(heading_mid); + noisy_y_ += ds_noisy * std::sin(heading_mid); + noisy_yaw_ = NormalizeAngle(noisy_yaw_ + dtheta_noisy); nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = ros_node_->now(); + odom_msg.header.stamp = ros_node_->now(); odom_msg.header.frame_id = frame_id_; - odom_msg.child_frame_id = child_frame_id_; - - odom_msg.pose.pose.position.x = noisy_pose_internal_.Pos().X(); - odom_msg.pose.pose.position.y = noisy_pose_internal_.Pos().Y(); - odom_msg.pose.pose.position.z = noisy_pose_internal_.Pos().Z(); - - odom_msg.pose.pose.orientation.x = noisy_pose_internal_.Rot().X(); - odom_msg.pose.pose.orientation.y = noisy_pose_internal_.Rot().Y(); - odom_msg.pose.pose.orientation.z = noisy_pose_internal_.Rot().Z(); - odom_msg.pose.pose.orientation.w = noisy_pose_internal_.Rot().W(); - - odom_msg.twist.twist.linear.x = last_noisy_linear_velocity_; - odom_msg.twist.twist.linear.y = 0.0; - odom_msg.twist.twist.linear.z = 0.0; - + odom_msg.child_frame_id = child_frame_id_; + + odom_msg.pose.pose.position.x = noisy_x_; + odom_msg.pose.pose.position.y = noisy_y_; + odom_msg.pose.pose.position.z = 0.0; + + // Planar robot: roll = pitch = 0, only yaw. + odom_msg.pose.pose.orientation.x = 0.0; + odom_msg.pose.pose.orientation.y = 0.0; + odom_msg.pose.pose.orientation.z = std::sin(noisy_yaw_ / 2.0); + odom_msg.pose.pose.orientation.w = std::cos(noisy_yaw_ / 2.0); + + // Twist consistent with the noisy increment just integrated. + odom_msg.twist.twist.linear.x = ds_noisy / dt; + odom_msg.twist.twist.linear.y = 0.0; + odom_msg.twist.twist.linear.z = 0.0; odom_msg.twist.twist.angular.x = 0.0; odom_msg.twist.twist.angular.y = 0.0; - odom_msg.twist.twist.angular.z = last_noisy_angular_velocity_; + odom_msg.twist.twist.angular.z = dtheta_noisy / dt; - // Fill the most relevant covariance entries with values consistent with the - // motion model. The message remains simple, but no longer pretends to have - // perfect certainty. + // Covariance entries consistent with the motion-model sigmas. const double pose_var_xy = sigma_trans * sigma_trans; - const double pose_var_yaw = - (sigma_rot1 * sigma_rot1) + (sigma_rot2 * sigma_rot2); - - const double twist_var_v = pose_var_xy / (dt * dt); - const double twist_var_w = pose_var_yaw / (dt * dt); + const double pose_var_yaw = sigma_rot * sigma_rot; odom_msg.pose.covariance[0] = pose_var_xy; odom_msg.pose.covariance[7] = pose_var_xy; @@ -236,16 +164,13 @@ namespace custom_plugins odom_msg.pose.covariance[28] = 1e-9; odom_msg.pose.covariance[35] = pose_var_yaw; - odom_msg.twist.covariance[0] = twist_var_v; + odom_msg.twist.covariance[0] = pose_var_xy / (dt * dt); odom_msg.twist.covariance[7] = 1e-9; odom_msg.twist.covariance[14] = 1e-9; odom_msg.twist.covariance[21] = 1e-9; odom_msg.twist.covariance[28] = 1e-9; - odom_msg.twist.covariance[35] = twist_var_w; + odom_msg.twist.covariance[35] = pose_var_yaw / (dt * dt); ros_pub_->publish(odom_msg); - - // Store the true pose only to extract the next real motion increment. - last_true_pose_ = current_true_pose; } -} \ No newline at end of file +} From 5998e47ef608b2a0c4098516b76d6160a442f4ec Mon Sep 17 00:00:00 2001 From: aquintan Date: Mon, 25 May 2026 18:42:10 +0200 Subject: [PATCH 8/9] init pose + diferent noise levels --- .../models/turtlebot3_noisy_high/model.sdf | 8 ++++---- .../turtlebot3/models/turtlebot3_noisy_low/model.sdf | 8 ++++---- .../turtlebot3/models/turtlebot3_noisy_med/model.sdf | 8 ++++---- .../src/gz_noisy_odometry/NoisyOdometryPlugin.cpp | 12 ++++++++++++ 4 files changed, 24 insertions(+), 12 deletions(-) diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf index 38ced2c11..41fe7b093 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf @@ -567,10 +567,10 @@ 0.033 0.287 - 0.005 - 0.005 - 0.003 - 0.003 + 0.5 + 0.5 + 0.3 + 0.3 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf index d6e363f5d..1df901c3d 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf @@ -567,10 +567,10 @@ 0.033 0.287 - 0.0005 - 0.0005 - 0.0002 - 0.0002 + 0.05 + 0.05 + 0.02 + 0.02 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf index a38e1c246..bf03f7773 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf @@ -567,10 +567,10 @@ 0.033 0.287 - 0.002 - 0.002 - 0.001 - 0.001 + 0.2 + 0.2 + 0.1 + 0.1 diff --git a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp index e4d0949df..8a05c2e02 100644 --- a/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp +++ b/Industrial/gz_noisy_odometry/src/gz_noisy_odometry/NoisyOdometryPlugin.cpp @@ -1,5 +1,6 @@ #include "gz_noisy_odometry/NoisyOdometryPlugin.hpp" #include +#include #include #include @@ -88,6 +89,17 @@ namespace custom_plugins if (!initialized_) { + // Seed the estimate with the robot's spawn pose so that odom_noisy starts + // at the same position as the perfect-odometry topic. After this point + // the estimate evolves purely from encoder increments — Pose is not read again. + auto *poseComp = _ecm.Component(model_.Entity()); + if (poseComp) + { + noisy_x_ = poseComp->Data().Pos().X(); + noisy_y_ = poseComp->Data().Pos().Y(); + noisy_yaw_ = poseComp->Data().Rot().Yaw(); + } + last_left_pos_ = left_pos; last_right_pos_ = right_pos; last_update_time_ = _info.simTime; From acff3d4dbfc3ae91a198d4d2c6ca6f7b4d784194 Mon Sep 17 00:00:00 2001 From: aquintan Date: Tue, 26 May 2026 12:02:04 +0200 Subject: [PATCH 9/9] add more noise + fix rover odometry plugin --- .../urdf/rover_4wd_noisy_high.urdf | 20 +++++++++++-------- .../urdf/rover_4wd_noisy_low.urdf | 20 +++++++++++-------- .../models/turtlebot3_noisy_high/model.sdf | 8 ++++---- .../models/turtlebot3_noisy_low/model.sdf | 8 ++++---- .../models/turtlebot3_noisy_med/model.sdf | 8 ++++---- 5 files changed, 36 insertions(+), 28 deletions(-) diff --git a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf index dc856c639..e2421addf 100644 --- a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf +++ b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf @@ -205,16 +205,20 @@ - /odom - /cmd_vel + /odom_noisy odom base_link - 0.005 - 0.005 - 0.003 - 0.003 - 0.15 - 0.25 + + + fl_wheel_to_chassis + fr_wheel_to_chassis + 0.24 + 0.122 + + 1.5 + 1.5 + 0.8 + 0.8 diff --git a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf index 720f34418..92c5b6541 100644 --- a/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf +++ b/CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf @@ -205,16 +205,20 @@ - /odom - /cmd_vel + /odom_noisy odom base_link - 0.0005 - 0.0005 - 0.0002 - 0.0002 - 0.05 - 0.15 + + + fl_wheel_to_chassis + fr_wheel_to_chassis + 0.24 + 0.122 + + 0.1 + 0.1 + 0.05 + 0.05 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf index 41fe7b093..cc65c1158 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf @@ -567,10 +567,10 @@ 0.033 0.287 - 0.5 - 0.5 - 0.3 - 0.3 + 1.5 + 1.5 + 0.8 + 0.8 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf index 1df901c3d..72ab50e14 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf @@ -567,10 +567,10 @@ 0.033 0.287 - 0.05 - 0.05 - 0.02 - 0.02 + 0.1 + 0.1 + 0.05 + 0.05 diff --git a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf index bf03f7773..179604928 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf @@ -567,10 +567,10 @@ 0.033 0.287 - 0.2 - 0.2 - 0.1 - 0.1 + 0.5 + 0.5 + 0.25 + 0.25