Background:
I’m working on ROS 2 Humble (Ubuntu 22.04) with Gazebo Classic and a differential drive robot.
I’m trying to simulate my own encoder and odometry nodes instead of using the odometry output from the Gazebo libgazebo_ros_diff_drive.so plugin.
In my SDF file, I’ve set:
<publish_odom>false</publish_odom> <publish_odom_tf>false</publish_odom_tf> <publish_wheel_tf>true</publish_wheel_tf>
so that Gazebo only publishes wheel joint states, while my custom nodes handle encoder pulse generation and odometry calculation.
Problem:
The robot navigates normally and reaches its Nav2 goals, but the /odom pose from my custom odometry node is far from the true Gazebo world position.
Example:
[INFO] [pose_data_collector_final]: odom : x= 2.3969, y= 1.6180, angle=176.81° [INFO] [pose_data_collector_final]: world : x= 1.9294, y=-0.5328, angle=71.24°
While my Target goal (map frame): x=1.95, y=-0.50, yaw=86.33°
The robot visually stops at the correct location in Gazebo, but /odom is offset by almost 2 meters and rotated by ~100°, which seems abnormal.
Current setup:
Wheel radius: ✅ correct and matches URDF
Encoder ticks: ✅ computed from wheel joint position in radians
Using JointState topic from Gazebo to compute ticks
Robot TF tree: map → odom → base_footprint → base_link → sensors
Using Nav2 stack for navigation
Using Python nodes for encoder + odom
Integration equations:
delta_s = (right_dist + left_dist) / 2 delta_theta = (right_dist - left_dist) / wheel_separation x += delta_s * cos(theta + delta_theta / 2) y += delta_s * sin(theta + delta_theta / 2) theta += delta_theta
I can share the encoder + odom node snippets if useful.
#!/usr/bin/env python3 import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState from nav_msgs.msg import Odometry from geometry_msgs.msg import Quaternion import math import tf_transformations class CustomOdomNode(Node): def __init__(self): super().__init__('custom_odom_node') # Robot parameters self.wheel_radius = 0.033 # meters self.wheel_separation = 0.160 # meters self.encoder_ppr = 1024 # pulses per revolution # State variables self.last_left_pos = None self.last_right_pos = None self.x = 0.0 self.y = 0.0 self.theta = 0.0 # ROS interfaces self.create_subscription(JointState, '/joint_states', self.joint_callback, 10) self.odom_pub = self.create_publisher(Odometry, '/odom', 10) self.last_time = self.get_clock().now() self.get_logger().info("Custom odometry node started") def joint_callback(self, msg): try: left_index = msg.name.index('wheel_left_joint') right_index = msg.name.index('wheel_right_joint') except ValueError: return # joint names not found yet left_pos = msg.position[left_index] right_pos = msg.position[right_index] # Initialize previous readings if self.last_left_pos is None: self.last_left_pos = left_pos self.last_right_pos = right_pos return # Compute wheel rotation differences delta_left = left_pos - self.last_left_pos delta_right = right_pos - self.last_right_pos self.last_left_pos = left_pos self.last_right_pos = right_pos # Convert to linear distances left_dist = delta_left * self.wheel_radius right_dist = delta_right * self.wheel_radius # Differential drive integration delta_s = (right_dist + left_dist) / 2.0 delta_theta = (right_dist - left_dist) / self.wheel_separation self.theta += delta_theta self.x += delta_s * math.cos(self.theta) self.y += delta_s * math.sin(self.theta) # Compute velocity current_time = self.get_clock().now() dt = (current_time - self.last_time).nanoseconds / 1e9 self.last_time = current_time vx = delta_s / dt if dt > 0 else 0.0 vth = delta_theta / dt if dt > 0 else 0.0 # Publish Odometry odom = Odometry() odom.header.stamp = current_time.to_msg() odom.header.frame_id = 'odom' odom.child_frame_id = 'base_footprint' odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y q = tf_transformations.quaternion_from_euler(0, 0, self.theta) odom.pose.pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) odom.twist.twist.linear.x = vx odom.twist.twist.angular.z = vth self.odom_pub.publish(odom) What is the causing the problem and how can I solve it?
