Skip to content

Odometry publisher: slip factor #728

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
javizqh merged 14 commits into from
May 26, 2026
Merged

Odometry publisher: slip factor #728

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
66966a8
fix odometry plug-in
aquintan4 May 7, 2026
fb869db
add a slip factor to add more realism
aquintan4 May 8, 2026
172d02a
Merge branch 'humble-devel' into odometry-publisher-gzharmonic
aquintan4 May 22, 2026
e8f988e
fix odometry plugin
aquintan4 May 22, 2026
24420e7
Merge remote-tracking branch 'upstream/humble-devel' into odometry-pu…
aquintan4 May 22, 2026
767d983
remove gazebo tick counter
aquintan4 May 22, 2026
e6dfdb1
odometry fix
aquintan4 May 22, 2026
e3b0fbb
add slip factor to turtlebot models
aquintan4 May 22, 2026
cab84ee
Merge branch 'humble-devel' into odometry-publisher-gzharmonic
javizqh May 22, 2026
3c51ab6
gazebo noisy odometry, using diff drive plugin
aquintan4 May 25, 2026
d8a744e
Merge remote-tracking branch 'upstream/humble-devel' into odometry-pu…
aquintan4 May 25, 2026
c3f9a5f
Merge remote-tracking branch 'upstream/odometry-publisher-gzharmonic'…
aquintan4 May 25, 2026
5998e47
init pose + diferent noise levels
aquintan4 May 25, 2026
acff3d4
add more noise + fix rover odometry plugin
aquintan4 May 26, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
18 changes: 12 additions & 6 deletions CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -205,14 +205,20 @@
</plugin>

<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
<ros_topic>/odom</ros_topic>
<gz_cmd_vel_topic>/cmd_vel</gz_cmd_vel_topic>
<ros_topic>/odom_noisy</ros_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_link</child_frame_id>
<alpha1>0.005</alpha1>
<alpha2>0.005</alpha2>
<alpha3>0.003</alpha3>
<alpha4>0.003</alpha4>

<!-- must match the DiffDrive plugin values above — one joint per side -->
<left_joint>fl_wheel_to_chassis</left_joint>
<right_joint>fr_wheel_to_chassis</right_joint>
<wheel_separation>0.24</wheel_separation>
<wheel_radius>0.122</wheel_radius>

<alpha1>1.5</alpha1>
<alpha2>1.5</alpha2>
<alpha3>0.8</alpha3>
<alpha4>0.8</alpha4>
</plugin>
</gazebo>

Expand Down
18 changes: 12 additions & 6 deletions CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -205,14 +205,20 @@
</plugin>

<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
<ros_topic>/odom</ros_topic>
<gz_cmd_vel_topic>/cmd_vel</gz_cmd_vel_topic>
<ros_topic>/odom_noisy</ros_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_link</child_frame_id>
<alpha1>0.0005</alpha1>
<alpha2>0.0005</alpha2>
<alpha3>0.0002</alpha3>
<alpha4>0.0002</alpha4>

<!-- must match the DiffDrive plugin values above — one joint per side -->
<left_joint>fl_wheel_to_chassis</left_joint>
<right_joint>fr_wheel_to_chassis</right_joint>
<wheel_separation>0.24</wheel_separation>
<wheel_radius>0.122</wheel_radius>

<alpha1>0.1</alpha1>
<alpha2>0.1</alpha2>
<alpha3>0.05</alpha3>
<alpha4>0.05</alpha4>
</plugin>
</gazebo>

Expand Down
15 changes: 10 additions & 5 deletions CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -558,14 +558,19 @@

<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
<ros_topic>/turtlebot3/odom_noisy</ros_topic>
<gz_cmd_vel_topic>/turtlebot3/cmd_vel</gz_cmd_vel_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_footprint</child_frame_id>

<alpha1>0.005</alpha1>
<alpha2>0.005</alpha2>
<alpha3>0.003</alpha3>
<alpha4>0.003</alpha4>
<!-- must match the DiffDrive plugin values above -->
<left_joint>wheel_left_joint</left_joint>
<right_joint>wheel_right_joint</right_joint>
<wheel_radius>0.033</wheel_radius>
<wheel_separation>0.287</wheel_separation>

<alpha1>1.5</alpha1>
<alpha2>1.5</alpha2>
<alpha3>0.8</alpha3>
<alpha4>0.8</alpha4>
</plugin>

</model>
Expand Down
15 changes: 10 additions & 5 deletions CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -558,14 +558,19 @@

<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
<ros_topic>/turtlebot3/odom_noisy</ros_topic>
<gz_cmd_vel_topic>/turtlebot3/cmd_vel</gz_cmd_vel_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_footprint</child_frame_id>

<alpha1>0.0005</alpha1>
<alpha2>0.0005</alpha2>
<alpha3>0.0002</alpha3>
<alpha4>0.0002</alpha4>
<!-- must match the DiffDrive plugin values above -->
<left_joint>wheel_left_joint</left_joint>
<right_joint>wheel_right_joint</right_joint>
<wheel_radius>0.033</wheel_radius>
<wheel_separation>0.287</wheel_separation>

<alpha1>0.1</alpha1>
<alpha2>0.1</alpha2>
<alpha3>0.05</alpha3>
<alpha4>0.05</alpha4>
</plugin>

</model>
Expand Down
15 changes: 10 additions & 5 deletions CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -558,14 +558,19 @@

<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
<ros_topic>/turtlebot3/odom_noisy</ros_topic>
<gz_cmd_vel_topic>/turtlebot3/cmd_vel</gz_cmd_vel_topic>
<frame_id>odom</frame_id>
<child_frame_id>base_footprint</child_frame_id>

<alpha1>0.002</alpha1>
<alpha2>0.002</alpha2>
<alpha3>0.001</alpha3>
<alpha4>0.001</alpha4>
<!-- must match the DiffDrive plugin values above -->
<left_joint>wheel_left_joint</left_joint>
<right_joint>wheel_right_joint</right_joint>
<wheel_radius>0.033</wheel_radius>
<wheel_separation>0.287</wheel_separation>

<alpha1>0.5</alpha1>
<alpha2>0.5</alpha2>
<alpha3>0.25</alpha3>
<alpha4>0.25</alpha4>
</plugin>

</model>
Expand Down
8 changes: 0 additions & 8 deletions Industrial/gz_noisy_odometry/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -42,22 +39,17 @@ target_include_directories(NoisyOdometryPlugin PUBLIC
$<INSTALL_INTERFACE:include>
)

# 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(
Expand Down
55 changes: 20 additions & 35 deletions Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3,15 +3,11 @@

#include <gz/sim/System.hh>
#include <gz/sim/Model.hh>
#include <gz/transport/Node.hh>
#include <gz/msgs/twist.pb.h>
#include <gz/math/Pose3.hh>
#include <gz/math/Rand.hh>

#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/odometry.hpp>

#include <mutex>
#include <string>
#include <memory>

Expand All @@ -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<rclcpp::Node> ros_node_;
rclcpp::Publisher<nav_msgs::msg::Odometry>::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
#endif
1 change: 0 additions & 1 deletion Industrial/gz_noisy_odometry/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,6 @@

<depend>rclcpp</depend>
<depend>nav_msgs</depend>
<depend>tf2</depend>

<export>
<build_type>ament_cmake</build_type>
Expand Down
Loading
Loading