diff --git a/nodes/encoder_odom_publisher.py b/nodes/encoder_odom_publisher.py new file mode 100644 index 0000000..d1f7e55 --- /dev/null +++ b/nodes/encoder_odom_publisher.py @@ -0,0 +1,112 @@ +#!/usr/bin/env python + +import rospy +import tf +from nav_msgs.msg import Odometry +from std_msgs.msg import Header +from your_custom_msgs.msg import EncoderTicks # Replace with actual message type +import math + +class EncoderOdometry: + def __init__(self): + rospy.init_node("encoder_odom_publisher") + + # Robot parameters (update these according to your robot) + self.wheel_radius = 0.05 # Wheel radius in meters + self.wheel_base = 0.3 # Distance between wheels (m) + self.ticks_per_revolution = 1000 # Encoder ticks per wheel revolution + + # State variables + self.x = 0.0 + self.y = 0.0 + self.theta = 0.0 + self.last_l_ticks = None + self.last_r_ticks = None + self.last_time = None + + # ROS publishers + self.odom_pub = rospy.Publisher("/odom", Odometry, queue_size=10) + self.odom_broadcaster = tf.TransformBroadcaster() + + # ROS subscriber + rospy.Subscriber("/ticks_fb", EncoderTicks, self.ticks_callback) + + rospy.spin() + + def ticks_callback(self, msg): + if self.last_l_ticks is None or self.last_r_ticks is None or self.last_time is None: + self.last_l_ticks = msg.l_ticks + self.last_r_ticks = msg.r_ticks + self.last_time = msg.ticks_timestamp / 1e6 # Convert to seconds + return + + # Compute time delta + current_time = msg.ticks_timestamp / 1e6 # Convert to seconds + dt = current_time - self.last_time + if dt <= 0: + return # Skip if time is invalid + + # Compute tick differences + delta_l = msg.l_ticks - self.last_l_ticks + delta_r = msg.r_ticks - self.last_r_ticks + + # Convert ticks to meters + meters_per_tick = (2 * math.pi * self.wheel_radius) / self.ticks_per_revolution + delta_s_l = delta_l * meters_per_tick + delta_s_r = delta_r * meters_per_tick + + # Compute linear and angular velocity + v = (delta_s_l + delta_s_r) / 2.0 / dt + omega = (delta_s_r - delta_s_l) / self.wheel_base / dt + + # Integrate motion (basic differential drive kinematics) + delta_theta = omega * dt + self.theta += delta_theta + delta_x = v * math.cos(self.theta) * dt + delta_y = v * math.sin(self.theta) * dt + self.x += delta_x + self.y += delta_y + + # Create odometry message + odom = Odometry() + odom.header = Header(stamp=rospy.Time.from_sec(current_time), frame_id="odom") + odom.child_frame_id = "base_link" + + # Position + odom.pose.pose.position.x = self.x + odom.pose.pose.position.y = self.y + odom.pose.pose.position.z = 0.0 + + # Orientation (as quaternion) + q = tf.transformations.quaternion_from_euler(0, 0, self.theta) + odom.pose.pose.orientation.x = q[0] + odom.pose.pose.orientation.y = q[1] + odom.pose.pose.orientation.z = q[2] + odom.pose.pose.orientation.w = q[3] + + # Velocities + odom.twist.twist.linear.x = v + odom.twist.twist.angular.z = omega + + # Publish odometry + self.odom_pub.publish(odom) + + # Publish TF transform + self.odom_broadcaster.sendTransform( + (self.x, self.y, 0), + q, + rospy.Time.from_sec(current_time), + "base_link", + "odom" + ) + + # Update previous values + self.last_l_ticks = msg.l_ticks + self.last_r_ticks = msg.r_ticks + self.last_time = current_time + +if __name__ == "__main__": + try: + EncoderOdometry() + except rospy.ROSInterruptException: + pass