Using odometry with motor encoders and visualize in RVIZ
Hello all,
After lots of work I have finaly my robot driving control with a joystick from my desktop. I have created an simple URDF of the robot which has 3 ultrasone sensors. This data I receive on RVIZ. But the next step is using odometry and see the bot moving in RVIZ. I wrote a script that computes the wheel velocities into linear and angular velocities and publish them as a nav_msgs.msg /Odometry and named the topic /odom.
When I drive the robot and do a rostopic ech /odom
I receive the data a screenshot below:
So this seems to work, I guess. But when I try to visualize it with RVIZ it tells me that there is no odom link to transform. The message I saw was:
Transform [sender=unknown_publisher]
I have also done a rosrun tf view_frames
this showed me the following:
Here it is clear that there is no /odom frame. So how could I fix this? Do I need to change my URDF file or do I make a mistake in RVIZ I don't know where to look at the moment anymore.
I hope someone can guide me a bit
Edit1: (Snippet of first attempt using odom from wheel encoders
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 # 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.last_time = current_time
def right_vel_callback(self, msg):
self.right_vel_rpm = msg.data
self.calculate_odometry(self.right_vel_rpm, self.left_vel_rpm)
def left_vel_callback(self, msg):
self.left_vel_rpm = msg.data
self.calculate_odometry(self.right_vel_rpm, self.left_vel_rpm)
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)