laserscan data rotate with the robot rotation
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 .
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.
thank you for reply, here is my code
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.
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.