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..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,14 +205,20 @@ - /odom - /cmd_vel + /odom_noisy odom base_link - 0.005 - 0.005 - 0.003 - 0.003 + + + 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 b4d97a1ae..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,14 +205,20 @@ - /odom - /cmd_vel + /odom_noisy odom base_link - 0.0005 - 0.0005 - 0.0002 - 0.0002 + + + 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 1345dff32..cc65c1158 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf @@ -558,14 +558,19 @@ /turtlebot3/odom_noisy - /turtlebot3/cmd_vel odom base_footprint - 0.005 - 0.005 - 0.003 - 0.003 + + wheel_left_joint + wheel_right_joint + 0.033 + 0.287 + + 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 c47fc7de2..72ab50e14 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf @@ -558,14 +558,19 @@ /turtlebot3/odom_noisy - /turtlebot3/cmd_vel odom base_footprint - 0.0005 - 0.0005 - 0.0002 - 0.0002 + + wheel_left_joint + wheel_right_joint + 0.033 + 0.287 + + 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 5f3714754..179604928 100644 --- a/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf +++ b/CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf @@ -558,14 +558,19 @@ /turtlebot3/odom_noisy - /turtlebot3/cmd_vel odom base_footprint - 0.002 - 0.002 - 0.001 - 0.001 + + wheel_left_joint + wheel_right_joint + 0.033 + 0.287 + + 0.5 + 0.5 + 0.25 + 0.25 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 68a5fe9d6..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,55 +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 are still stored because they are useful - // as auxiliary information and to keep compatibility with the original plugin. - 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_; + bool initialized_{false}; - // Legacy generic noise parameter kept for compatibility with the original SDF. - double gaussian_noise_coeff_{0.0}; + // Accumulated noisy odometry estimate. + double noisy_x_{0.0}; + double noisy_y_{0.0}; + double noisy_yaw_{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}; + // 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 89c43ab25..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,7 +1,7 @@ #include "gz_noisy_odometry/NoisyOdometryPlugin.hpp" +#include #include #include -#include #include @@ -19,7 +19,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,20 +32,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; - gz_node_.Subscribe(gz_cmd_vel_topic_, &NoisyOdometryPlugin::OnCmdVel, this); + // 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); + + 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; + } + + _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)); @@ -55,32 +73,37 @@ 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_); + + if (!leftComp || !rightComp || + leftComp->Data().empty() || rightComp->Data().empty()) return; - const gz::math::Pose3d current_true_pose = poseComp->Data(); + 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; + // 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; - initialized_ = true; + initialized_ = true; return; } @@ -90,118 +113,76 @@ 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; - - const double delta_yaw = NormalizeAngle(current_true_yaw - previous_true_yaw); - 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()); - - 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; + // 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_; - nav_msgs::msg::Odometry odom_msg; - odom_msg.header.stamp = ros_node_->now(); - odom_msg.header.frame_id = frame_id_; - odom_msg.child_frame_id = child_frame_id_; + last_left_pos_ = left_pos; + last_right_pos_ = right_pos; + + // 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_; - 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(); + // 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); - 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(); + 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); - 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; + // 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.frame_id = frame_id_; + 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. - const double pose_var_xy = sigma_trans * sigma_trans; - const double pose_var_yaw = - (sigma_rot1 * sigma_rot1) + (sigma_rot2 * sigma_rot2); + // Covariance entries consistent with the motion-model sigmas. + 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_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] = 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 +}