diff --git a/CMakeLists.txt b/CMakeLists.txt index e98b3c9..76d50ae 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,7 @@ target_link_libraries(odom_publisher ${catkin_LIBRARIES}) catkin_install_python(PROGRAMS nodes/odom_publisher_python.py nodes/odom_publisher_linus.py + nodes/odom_publisher_fromTwist.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/nodes/odom_publisher_fromTwist.py b/nodes/odom_publisher_fromTwist.py new file mode 100644 index 0000000..7d58cac --- /dev/null +++ b/nodes/odom_publisher_fromTwist.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python + +import rospy +from nav_msgs.msg import Odometry +from geometry_msgs.msg import TransformStamped +import tf2_ros +import math + +# Global variables for cumulative pose +x = 0.0 +y = 0.0 +theta = 0.0 # Orientation in radians + +def odom_callback(msg): + global x, y, theta, br + + # Time interval + current_time = rospy.Time.now() + dt = (current_time - odom_callback.last_time).to_sec() + odom_callback.last_time = current_time + + # Extract linear and angular velocities + linear_x = msg.twist.twist.linear.x + linear_y = msg.twist.twist.linear.y + angular_z = msg.twist.twist.angular.z + + # Compute the change in position and orientation + delta_x = (linear_x * math.cos(theta) - linear_y * math.sin(theta)) * dt + delta_y = (linear_x * math.sin(theta) + linear_y * math.cos(theta)) * dt + delta_theta = angular_z * dt + + # Update cumulative position and orientation + x += delta_x + y += delta_y + theta += delta_theta + + # Normalize theta to the range [-pi, pi] + theta = (theta + math.pi) % (2 * math.pi) - math.pi + + # Create a TransformStamped message + t = TransformStamped() + t.header.stamp = current_time + t.header.frame_id = "odom" + t.child_frame_id = "base_link" + + # Set translation + t.transform.translation.x = x + t.transform.translation.y = y + t.transform.translation.z = 0.0 + + # Set rotation (quaternion) + q = quaternion_from_euler(0, 0, theta) + t.transform.rotation.x = q[0] + t.transform.rotation.y = q[1] + t.transform.rotation.z = q[2] + t.transform.rotation.w = q[3] + + # Broadcast the transform + br.sendTransform(t) + +def main(): + global br + + rospy.init_node("odom_to_base_link_tf_from_twist") + + # Initialize TransformBroadcaster + br = tf2_ros.TransformBroadcaster() + + # Initialize last time for integration + odom_callback.last_time = rospy.Time.now() + + # Subscribe to the /odom topic + rospy.Subscriber("/odom", Odometry, odom_callback) + + rospy.loginfo("Broadcasting /odom to /base_link transform using twist") + rospy.spin() + +if __name__ == "__main__": + main()