Skip to content

Commit cc4b434

Browse files
javizqhjavizqh
authored
Merge pull request #728 from JdeRobot/odometry-publisher-gzharmonic
Odometry publisher: slip factor
2 parents 753eead + acff3d4 commit cc4b434

9 files changed

Lines changed: 179 additions & 195 deletions

File tree

‎CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_high.urdf‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -205,14 +205,20 @@
205205
</plugin>
206206

207207
<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
208-
<ros_topic>/odom</ros_topic>
209-
<gz_cmd_vel_topic>/cmd_vel</gz_cmd_vel_topic>
208+
<ros_topic>/odom_noisy</ros_topic>
210209
<frame_id>odom</frame_id>
211210
<child_frame_id>base_link</child_frame_id>
212-
<alpha1>0.005</alpha1>
213-
<alpha2>0.005</alpha2>
214-
<alpha3>0.003</alpha3>
215-
<alpha4>0.003</alpha4>
211+
212+
<!-- must match the DiffDrive plugin values above — one joint per side -->
213+
<left_joint>fl_wheel_to_chassis</left_joint>
214+
<right_joint>fr_wheel_to_chassis</right_joint>
215+
<wheel_separation>0.24</wheel_separation>
216+
<wheel_radius>0.122</wheel_radius>
217+
218+
<alpha1>1.5</alpha1>
219+
<alpha2>1.5</alpha2>
220+
<alpha3>0.8</alpha3>
221+
<alpha4>0.8</alpha4>
216222
</plugin>
217223
</gazebo>
218224

‎CustomRobots/rover_4wd_description/urdf/rover_4wd_noisy_low.urdf‎

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -205,14 +205,20 @@
205205
</plugin>
206206

207207
<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
208-
<ros_topic>/odom</ros_topic>
209-
<gz_cmd_vel_topic>/cmd_vel</gz_cmd_vel_topic>
208+
<ros_topic>/odom_noisy</ros_topic>
210209
<frame_id>odom</frame_id>
211210
<child_frame_id>base_link</child_frame_id>
212-
<alpha1>0.0005</alpha1>
213-
<alpha2>0.0005</alpha2>
214-
<alpha3>0.0002</alpha3>
215-
<alpha4>0.0002</alpha4>
211+
212+
<!-- must match the DiffDrive plugin values above — one joint per side -->
213+
<left_joint>fl_wheel_to_chassis</left_joint>
214+
<right_joint>fr_wheel_to_chassis</right_joint>
215+
<wheel_separation>0.24</wheel_separation>
216+
<wheel_radius>0.122</wheel_radius>
217+
218+
<alpha1>0.1</alpha1>
219+
<alpha2>0.1</alpha2>
220+
<alpha3>0.05</alpha3>
221+
<alpha4>0.05</alpha4>
216222
</plugin>
217223
</gazebo>
218224

‎CustomRobots/turtlebot3/models/turtlebot3_noisy_high/model.sdf‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -558,14 +558,19 @@
558558

559559
<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
560560
<ros_topic>/turtlebot3/odom_noisy</ros_topic>
561-
<gz_cmd_vel_topic>/turtlebot3/cmd_vel</gz_cmd_vel_topic>
562561
<frame_id>odom</frame_id>
563562
<child_frame_id>base_footprint</child_frame_id>
564563

565-
<alpha1>0.005</alpha1>
566-
<alpha2>0.005</alpha2>
567-
<alpha3>0.003</alpha3>
568-
<alpha4>0.003</alpha4>
564+
<!-- must match the DiffDrive plugin values above -->
565+
<left_joint>wheel_left_joint</left_joint>
566+
<right_joint>wheel_right_joint</right_joint>
567+
<wheel_radius>0.033</wheel_radius>
568+
<wheel_separation>0.287</wheel_separation>
569+
570+
<alpha1>1.5</alpha1>
571+
<alpha2>1.5</alpha2>
572+
<alpha3>0.8</alpha3>
573+
<alpha4>0.8</alpha4>
569574
</plugin>
570575

571576
</model>

‎CustomRobots/turtlebot3/models/turtlebot3_noisy_low/model.sdf‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -558,14 +558,19 @@
558558

559559
<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
560560
<ros_topic>/turtlebot3/odom_noisy</ros_topic>
561-
<gz_cmd_vel_topic>/turtlebot3/cmd_vel</gz_cmd_vel_topic>
562561
<frame_id>odom</frame_id>
563562
<child_frame_id>base_footprint</child_frame_id>
564563

565-
<alpha1>0.0005</alpha1>
566-
<alpha2>0.0005</alpha2>
567-
<alpha3>0.0002</alpha3>
568-
<alpha4>0.0002</alpha4>
564+
<!-- must match the DiffDrive plugin values above -->
565+
<left_joint>wheel_left_joint</left_joint>
566+
<right_joint>wheel_right_joint</right_joint>
567+
<wheel_radius>0.033</wheel_radius>
568+
<wheel_separation>0.287</wheel_separation>
569+
570+
<alpha1>0.1</alpha1>
571+
<alpha2>0.1</alpha2>
572+
<alpha3>0.05</alpha3>
573+
<alpha4>0.05</alpha4>
569574
</plugin>
570575

571576
</model>

‎CustomRobots/turtlebot3/models/turtlebot3_noisy_med/model.sdf‎

Lines changed: 10 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -558,14 +558,19 @@
558558

559559
<plugin filename="libNoisyOdometryPlugin.so" name="custom_plugins::NoisyOdometryPlugin">
560560
<ros_topic>/turtlebot3/odom_noisy</ros_topic>
561-
<gz_cmd_vel_topic>/turtlebot3/cmd_vel</gz_cmd_vel_topic>
562561
<frame_id>odom</frame_id>
563562
<child_frame_id>base_footprint</child_frame_id>
564563

565-
<alpha1>0.002</alpha1>
566-
<alpha2>0.002</alpha2>
567-
<alpha3>0.001</alpha3>
568-
<alpha4>0.001</alpha4>
564+
<!-- must match the DiffDrive plugin values above -->
565+
<left_joint>wheel_left_joint</left_joint>
566+
<right_joint>wheel_right_joint</right_joint>
567+
<wheel_radius>0.033</wheel_radius>
568+
<wheel_separation>0.287</wheel_separation>
569+
570+
<alpha1>0.5</alpha1>
571+
<alpha2>0.5</alpha2>
572+
<alpha3>0.25</alpha3>
573+
<alpha4>0.25</alpha4>
569574
</plugin>
570575

571576
</model>

‎Industrial/gz_noisy_odometry/CMakeLists.txt‎

Lines changed: 0 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -21,14 +21,11 @@ endif()
2121
find_package(ament_cmake REQUIRED)
2222
find_package(rclcpp REQUIRED)
2323
find_package(nav_msgs REQUIRED)
24-
find_package(tf2 REQUIRED)
2524

2625
find_package(gz-plugin2 REQUIRED COMPONENTS register)
2726
find_package(gz-sim8 REQUIRED)
28-
find_package(gz-msgs10 REQUIRED)
2927
find_package(gz-math7 REQUIRED)
3028
find_package(gz-common5 REQUIRED)
31-
find_package(gz-transport13 REQUIRED)
3229

3330
# ============================================================================
3431
# Noisy Odometry plugin
@@ -42,22 +39,17 @@ target_include_directories(NoisyOdometryPlugin PUBLIC
4239
$<INSTALL_INTERFACE:include>
4340
)
4441

45-
# Dependencias de ROS 2
4642
ament_target_dependencies(NoisyOdometryPlugin
4743
rclcpp
4844
nav_msgs
49-
tf2
5045
)
5146

52-
# Enlace de librerías de Gazebo calcando la sintaxis del proyecto que funciona
5347
target_link_libraries(NoisyOdometryPlugin
5448
gz-plugin2::gz-plugin2
5549
gz-plugin2::register
5650
gz-sim8::gz-sim8
57-
gz-msgs10::gz-msgs10
5851
gz-math7::gz-math7
5952
gz-common5::gz-common5
60-
gz-transport13::gz-transport13
6153
)
6254

6355
install(

‎Industrial/gz_noisy_odometry/include/gz_noisy_odometry/NoisyOdometryPlugin.hpp‎

Lines changed: 20 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -3,15 +3,11 @@
33

44
#include <gz/sim/System.hh>
55
#include <gz/sim/Model.hh>
6-
#include <gz/transport/Node.hh>
7-
#include <gz/msgs/twist.pb.h>
8-
#include <gz/math/Pose3.hh>
96
#include <gz/math/Rand.hh>
107

118
#include <rclcpp/rclcpp.hpp>
129
#include <nav_msgs/msg/odometry.hpp>
1310

14-
#include <mutex>
1511
#include <string>
1612
#include <memory>
1713

@@ -35,55 +31,44 @@ namespace custom_plugins
3531
const gz::sim::EntityComponentManager &_ecm) override;
3632

3733
private:
38-
void OnCmdVel(const gz::msgs::Twist &_msg);
39-
40-
// Keeps angles bounded in [-pi, pi] so yaw differences remain well behaved.
4134
double NormalizeAngle(double _angle) const;
4235

4336
gz::sim::Model model_;
44-
gz::transport::Node gz_node_;
4537

4638
std::shared_ptr<rclcpp::Node> ros_node_;
4739
rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr ros_pub_;
4840

4941
std::string ros_topic_;
50-
std::string gz_cmd_vel_topic_;
5142
std::string frame_id_;
5243
std::string child_frame_id_;
5344

54-
bool initialized_{false};
45+
// Wheel joint entities resolved once in Configure.
46+
gz::sim::Entity left_joint_entity_{gz::sim::kNullEntity};
47+
gz::sim::Entity right_joint_entity_{gz::sim::kNullEntity};
5548

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_;
49+
// Differential drive kinematics — must match the DiffDrive plugin values.
50+
double wheel_radius_{0.033};
51+
double wheel_separation_{0.287};
5952

60-
// True simulator pose used only to extract the real motion increment
61-
// between consecutive simulation steps.
62-
gz::math::Pose3d last_true_pose_;
53+
// Last joint positions used to compute per-tick angle deltas.
54+
double last_left_pos_{0.0};
55+
double last_right_pos_{0.0};
6356

6457
std::chrono::steady_clock::duration last_update_time_{0};
6558

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_;
59+
bool initialized_{false};
7660

77-
// Legacy generic noise parameter kept for compatibility with the original SDF.
78-
double gaussian_noise_coeff_{0.0};
61+
// Accumulated noisy odometry estimate.
62+
double noisy_x_{0.0};
63+
double noisy_y_{0.0};
64+
double noisy_yaw_{0.0};
7965

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};
66+
// Standard odometry motion-model coefficients (Probabilistic Robotics, Ch. 5).
67+
double alpha1_{0.0}; // rotation noise from rotation
68+
double alpha2_{0.0}; // rotation noise from translation
69+
double alpha3_{0.0}; // translation noise from translation
70+
double alpha4_{0.0}; // translation noise from rotation
8671
};
8772
}
8873

89-
#endif
74+
#endif

‎Industrial/gz_noisy_odometry/package.xml‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,6 @@
1111

1212
<depend>rclcpp</depend>
1313
<depend>nav_msgs</depend>
14-
<depend>tf2</depend>
1514

1615
<export>
1716
<build_type>ament_cmake</build_type>

0 commit comments

Comments
 (0)