So at the moment I’m trying the get the hang of this Transformation Frames. But I get very confused when I’m trying to apply it in my own project. I’m able to get data into RVIZ (which is allready a nice step) but they are completely of and also I get errors in RVIZ (sometimes RVIZ even crahses) so I’m doing something really wrong here. And I don;t know if it is because wrong URDF coding or wrong Python script for computing the odom and transformation.
I will post some snippet of my script here maybe someone sees here an error:
def publish_tf(self):
self.tf_broadcaster.sendTransform(
(self.x, self.y, 0), # Translation
tf.transformations.quaternion_from_euler(0, 0, self.yaw), # Rotation
rospy.Time.now(),
'base_link',
'odom'
)
def calculate_odometry(self, right_vel_rpm, left_vel_rpm):
wheelbase = 0.205 # Wheelbase in m
wheel_radius = 0.036 # Radius of wheel in m
right_vel_mps = (2 * math.pi * wheel_radius * right_vel_rpm) / 60.0 # Convert rpm to m/s right side
left_vel_mps = (2 * math.pi * wheel_radius * left_vel_rpm) / 60.0 # Convert rpm to m/s left side
linear_velocity = (right_vel_mps + left_vel_mps) * 0.5 # Compute the linear velocity
angular_velocity = (right_vel_mps - left_vel_mps) / wheelbase # Compute angular velocity in rad/s
current_time = rospy.Time.now()
dt = (current_time - self.last_time).to_sec() # Compute integration time and covert from us to sec
self.x += linear_velocity * math.cos(self.yaw) * dt
self.y += linear_velocity * math.sin(self.yaw) * dt
self.yaw += angular_velocity * dt
self.publish_odom()
self.publish_tf()
self.last_time = current_time
def publish_odom(self):
odom = Odometry()
odom.header.stamp = rospy.Time.now()
odom.header.frame_id = 'odom'
odom.child_frame_id = 'base_link'
# Populate position
odom.pose.pose.position.x = self.x
odom.pose.pose.position.y = self.y
odom.pose.pose.position.z = 0.0
odom.pose.pose.orientation.z = math.sin(self.yaw / 2.0)
odom.pose.pose.orientation.w = math.cos(self.yaw / 2.0)
# Populate velocities
odom.twist.twist.linear.x = 0.0
odom.twist.twist.linear.y = 0.0
odom.twist.twist.angular.z = 0.0
self.pub_odom.publish(odom)
Above are the three main functions for publishing and computing odometry and the transformation. I compute velocities in the function calculate_odometry and call in that function the function to publish the transformation publish_tf, which is base_link to odom.
Now my frames.pdf looks as follows:
The errors in RVIZ tell me there is no transform from base_footprint to base_link. But hat I don’t understand where should I do this transform and why?
Also the tree in RVIZ looks different as the frames.pdf:
So I’m really confused, should I look for the error in my URDF/XACRO file or is my coding wrong?
My odom frame is completely off from my base_link
Thanks in advance
