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)
Asked by ElectricRay on 2023-08-06 09:35:04 UTC
Answers
Along with the nav message ODOM you need to also publish the transform for baselink to ODOM. This should be described in the tutorials.
https://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom
Asked by billy on 2023-08-08 01:19:35 UTC
Comments
Thanks I'll have a better at that tutorial and see if I can figure out what I do wrong regarding this transofrmation.
Asked by ElectricRay on 2023-08-08 03:17:14 UTC
So @billy, as far I understand what I dod wrong in my code is not adding any transformation here? I should use something like: odom_trans.header.frame_id = "odom"
the example code in the tutorial is C++ I need to do this in Python but I think this is easy to figure out.
But one thing I don't understand in the example they start with vx = 0.1, vy = -0.1 and vth = 0.1. is this just an example and should I replace this with subscribers to my computed velocities. I have my velocities in rpm but I could write a script that converts it to m/s and publish those to those variables.
I think that would be the way right?
Asked by ElectricRay on 2023-08-09 04:46:58 UTC
Here is what I've seen used to publish the trsnform from odom:
geometry_msgs::Quaternion quat = tf::createQuaternionMsgFromYaw(odom_yaw);
tf_msg.header.seq++;
tf_msg.header.stamp = ros::Time::now();
tf_msg.transform.translation.x = odom_x;
tf_msg.transform.translation.y = odom_y;
tf_msg.transform.translation.z = 0.0;
tf_msg.transform.rotation = quat;
odom_broadcaster.sendTransform(tf_msg);
Asked by chased11 on 2023-08-10 21:05:35 UTC
Well I think I need to understand the code better. But I get the point that I need to apply the transformation.
Asked by ElectricRay on 2023-08-11 12:52:27 UTC
Comments