Ask Your Question
1

laserscan data rotate with the robot rotation

asked 2022-04-01 06:39:17 -0500

Husam gravatar image

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
        self.odom_broadcaster.sendTransform(transform_stamped_msg)
        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
        self.odom_pub.publish(odom)

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 .

edit retag flag offensive close merge delete

Comments

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.

Mike Scheutzow gravatar image Mike Scheutzow  ( 2022-04-02 11:27:57 -0500 )edit

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.

Husam gravatar image Husam  ( 2022-04-19 07:47:17 -0500 )edit

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.

Husam gravatar image Husam  ( 2022-04-19 09:16:12 -0500 )edit

1 Answer

Sort by ยป oldest newest most voted
0

answered 2022-04-21 18:32:05 -0500

Husam gravatar image

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.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2022-04-01 06:39:17 -0500

Seen: 66 times

Last updated: Apr 21