Rosanswers logo

Hey everyone ! I'm building a diff drive robot using Ros2 Galactic , SLAM toolbox package and to be used in nav2 framework. I wrote the needed node for /odom to publish odometry msg, as follow in the snippet from odom node

        # publish the odom information
        quaternion = Quaternion()
        quaternion.x = 0.0
        quaternion.y = 0.0
        quaternion.z = sin(self.th / 2)
        quaternion.w = cos(self.th / 2)                
        transform_stamped_msg = TransformStamped()
        transform_stamped_msg.header.stamp = self.get_clock().now().to_msg()
        transform_stamped_msg.header.frame_id = self.odom_frame_id
        transform_stamped_msg.child_frame_id = self.base_frame_id
        transform_stamped_msg.transform.translation.x = self.x
        transform_stamped_msg.transform.translation.y = self.y
        transform_stamped_msg.transform.translation.z = 0.0
        transform_stamped_msg.transform.rotation.x = quaternion.x
        transform_stamped_msg.transform.rotation.y = quaternion.y
        transform_stamped_msg.transform.rotation.z = quaternion.z
        transform_stamped_msg.transform.rotation.w = quaternion.w
        odom = Odometry()
        odom.header.stamp = now.to_msg()
        odom.header.frame_id = self.odom_frame_id
        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 = quaternion
        odom.child_frame_id = self.base_frame_id
        odom.twist.twist.linear.x = self.dx
        odom.twist.twist.linear.y = 0.0
        odom.twist.twist.angular.z = self.dr

up to this point everythings seems ok, and made all the needed TF for the robot using a urdf file. (odom>>base_link and base_link>>lidar_frame) However, I noticed that when the robot moves and rotates the laserscan points moves with it, so it like all the room and walls rotate and it causing a drift when I try to build a map, I'm using rplidar and this slamteck package
note: 1. encoder reading are accurate 2. the tf between the odom>>base_link published from the odom node, while tf base_link>>lidar_frame published using ros description file . unfortunately I cant upload photos, so please refer to this video in YouTube that illustrate the issue. Does anyone has an idea what could be the problem? I feel like it's TF issue, but tf tree looks correct .

Originally posted by Husam on ROS Answers with karma: 26 on 2022-04-01

Post score: 1

Original comments

Comment by Mike Scheutzow on 2022-04-02:
Please describe how you are calculating self.x, .y and .th. Have you verified that these values are accurate at the beginning of operation? With axel encoders, it is expected that the error will slowly accumulate as the robot's travel distance increases.

Comment by Husam on 2022-04-19:
thank you for reply, here is my code

        now = self.get_clock().now()
        elapsed = now - self.then
        self.then = now
        elapsed = elapsed.nanoseconds / NS_TO_SEC
        self.dr = (self.wheel_radius/( 2* self.base_width))*(self.omega_right - self.omega_left)
        self.dx = 0.5 * self.wheel_radius * (self.omega_right + self.omega_left)

        delta_x = (self.dx * cos(self.th)) * elapsed
        delta_y = (self.dx * sin(self.th)) * elapsed
        delta_th = self.dr * elapsed

        self.x += delta_x
        self.y += delta_y
        self.th += delta_th

I verified my odom by comparing the desired wheels velocity that send by the keyboard on the /cmd_vel, and the actual measurements of the wheels velocity (encoder). also the robot has the same movement in rviz as the real robot. here a similar issue i think.

Comment by Husam on 2022-04-19:
More info, I'm using odrive and these encoder the encoder measurements are in turns and turns/sec, so I convert these to rad and rad/sec by multiply it by 2pi, so now the encoders provide (publish) four variables (angle right, angle left, omega right and omega left ) and as in my odom code above I'm using the omega left and right to calculate the odometry.

additional note : I multiplay velocity command and encoder data for the left wheel by -1 to make it positive when the robot moves forward. I also noticed when I'm doing slam, the map and odom TF looks fine, however when the robot rotates the new map start to rotate since the the scans rotate as well, and the tf of the odom jumps.


1 Answer 1


Rosanswers logo

Efter several tries and different solution , I found out that I'm using the base width (wheel separation) instead of half of it. Sorry for this and I really appreciate your help.

Originally posted by Husam with karma: 26 on 2022-04-21

This answer was ACCEPTED on the original site

Post score: 0


Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.