1+ #ifndef NOISY_ODOMETRY_PLUGIN_HPP_
2+ #define NOISY_ODOMETRY_PLUGIN_HPP_
3+
4+ #include < gz/sim/System.hh>
5+ #include < gz/sim/Model.hh>
6+ #include < gz/transport/Node.hh>
7+ #include < gz/msgs/twist.pb.h>
8+ #include < gz/math/Pose3.hh>
9+ #include < gz/math/Rand.hh>
10+
11+ #include < rclcpp/rclcpp.hpp>
12+ #include < nav_msgs/msg/odometry.hpp>
13+
14+ #include < mutex>
15+ #include < string>
16+ #include < memory>
17+
18+ namespace custom_plugins
19+ {
20+ class NoisyOdometryPlugin :
21+ public gz::sim::System,
22+ public gz::sim::ISystemConfigure,
23+ public gz::sim::ISystemPostUpdate
24+ {
25+ public:
26+ NoisyOdometryPlugin ();
27+ ~NoisyOdometryPlugin () override = default ;
28+
29+ void Configure (const gz::sim::Entity &_entity,
30+ const std::shared_ptr<const sdf::Element> &_sdf,
31+ gz::sim::EntityComponentManager &_ecm,
32+ gz::sim::EventManager &_eventMgr) override ;
33+
34+ void PostUpdate (const gz::sim::UpdateInfo &_info,
35+ const gz::sim::EntityComponentManager &_ecm) override ;
36+
37+ private:
38+ void OnCmdVel (const gz::msgs::Twist &_msg);
39+
40+ // Keeps angles bounded in [-pi, pi] so yaw differences remain well behaved.
41+ double NormalizeAngle (double _angle) const ;
42+
43+ gz::sim::Model model_;
44+ gz::transport::Node gz_node_;
45+
46+ std::shared_ptr<rclcpp::Node> ros_node_;
47+ rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr ros_pub_;
48+
49+ std::string ros_topic_;
50+ std::string gz_cmd_vel_topic_;
51+ std::string frame_id_;
52+ std::string child_frame_id_;
53+
54+ bool initialized_{false };
55+
56+ // Internal odometric estimate that is propagated only with noisy increments.
57+ // This is the state published as odom_noisy and it is allowed to drift.
58+ gz::math::Pose3d noisy_pose_internal_;
59+
60+ // True simulator pose used only to extract the real motion increment
61+ // between consecutive simulation steps.
62+ gz::math::Pose3d last_true_pose_;
63+
64+ std::chrono::steady_clock::duration last_update_time_{0 };
65+
66+ // Latest commanded velocities are still stored because they are useful
67+ // as auxiliary information and to keep compatibility with the original plugin.
68+ double current_v_{0.0 };
69+ double current_w_{0.0 };
70+
71+ // Last noisy body-frame velocities published in the odometry message.
72+ double last_noisy_linear_velocity_{0.0 };
73+ double last_noisy_angular_velocity_{0.0 };
74+
75+ std::mutex mutex_;
76+
77+ // Legacy generic noise parameter kept for compatibility with the original SDF.
78+ double gaussian_noise_coeff_{0.0 };
79+
80+ // Standard odometry motion-model coefficients.
81+ // They control how rotation and translation uncertainties affect each other.
82+ double alpha1_{0.0 };
83+ double alpha2_{0.0 };
84+ double alpha3_{0.0 };
85+ double alpha4_{0.0 };
86+ };
87+ }
88+
89+ #endif
0 commit comments