carlsondc-ceva
commented
1 year ago Not sure if this is due to some quirk of the RPLidar A2 or if this is likely to affect the A1 that the example specifies, but I noticed that during constant-velocity rotations, the map <-> odom transform updated and introduced a step error that resolved once the rotation stopped and another SLAM update occurred. I tracked this down to an unreported 50ms of latency in the LIDAR data (timestamps were 50ms later than they should have been).
Top plot is the rotation in the map<-> odom transform, middle is the heading of base_footprint and motion capture ("MC"), bottom is residuals from various heading sources. The two odometry sources (c3_odom and pi_scout_odom) have a small flat residual which shows that they were well-aligned in time with the ground truth. The SLAM sources all have a step in their residual mid-turn, and then once we go back to straight driving, they drop back down.
The height of this step corresponded to ~50 ms of rotation, so it looks like SLAM was matching up the scan at time t with odometry at time t - 50 ms. I put together a ROS node to modify the laser scan header, remapped a few topics, and got rid of this effect.
Anyway, I'm leaving this here in case somebody else tries to connect an RPLidar A2 and sees this behavior. Here's the node that I used to shift the timestamp:
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import LaserScan
import rclpy.time
class AdjustLaserScanTime(Node):
def __init__(self):
super().__init__('adjust_LaserScan_time')
self.publisher = self.create_publisher(LaserScan, "scan_out", 10)
self.declare_parameter('time_shift', 0.0)
self.subscription = self.create_subscription(LaserScan,
"scan_in",
self.callback,
10)
self.time_shift = rclpy.time.Duration(seconds=self.get_parameter('time_shift').get_parameter_value().double_value)
def callback(self, msg):
msg.header.stamp = rclpy.time.Time.to_msg(rclpy.time.Time.from_msg(msg.header.stamp) + self.time_shift)
self.publisher.publish(msg)
def main(args=None):
rclpy.init(args=args)
adjustLaserScanTime = AdjustLaserScanTime()
rclpy.spin(adjustLaserScanTime)
adjustLaserScanTime.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
bluepra
Suggestions for the documentation:
Note that for an RPLidar A2, if the cable is coming out towards the tray, you need to set
flip_axistotrueinconfig/rplidar_node.yaml. I'm assuming this value is fine as it is specified for the A1 which is used in the readme. You can tell that Something Is Wrong by looking at the /scan topic in rviz and driving it towards a wall (set fixed frame to base_link)-- the scan hits will move the wrong direction unless you flip the x axis.You can make the following changes to ensure that the LIDAR is always available at a fixed device file:
sudo lsusb. Look for the section describing the UART bridge:Note the "idVendor" and "idProduct" fields (0x10c4, 0xea60 respectively)
/etc/udev/rules.dto handle usb-serial device connections and automatically symlink to a predictable locationCreate
/etc/udev/rules.d/99-usb-serial.rules(or similar file)SUBSYSTEM=="tty", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", SYMLINK+="lidar_0"serial_portentry inconfig/rplidar_node.yamlto/dev/lidar_0