Use /vehicle/status/velocity_status as the input for lat/lon controller instead of /localization/kinematic_state

[JianKangEgon]

Checklist

• I've read the contribution guidelines.
• I've searched other issues and no duplicate issues were found.
• I've agreed with the maintainers that I can plan this task.

Description

Currently, the control module takes the kinematic states from localization module as the input. In other words, the performance of control module relies on the localization results by a large margin. In simulator, it works fine indeed, because the localization module and vehicle module are actually fake.

However, during road test on real vehicle, the robustness of localization module performs less reliable as vehicle driving-by-wire module, or wheel odometry sensor for robot. From my understanding, the algorithms of localization module (ICP / NDT matcher) are iterate computation progress. Sometimes, the steering wheel jitters due to the lost localization matcher. In comparison, the can bus data or odometer could provide relatively more helpful information.

By communication with the vehicle can bus or wheel encoder, the control module is able to acquire a much more accurate value. Thus, I'd love to propose this issue and discuss if it's reasonable to do so.

Purpose

Read vehicle velocity from can bus (or wheel encoder of robot) if these information are available.

Possible approaches

• Remap the input topic for control module from /localization/kinematic_state to /vehicle/status/velocity_status as the velocity source.
• Change the corresponding message type

Definition of done

• Rewrite the node file to change the topic message type.

• Build successfully without any conflict

• Validation and verification in both planning simulator and road test on real vehicle

[TakaHoribe]

Thank you for your post.

The can bus speed information ( /vehicle/status/velocity_status) is passed to the localization module for proper processing. If the can bus information is clearly more accurate than the localization matching, you can tell the information to the localization module through the error covariance.

The localization module also handles cases where the speed information from the can bus is disconnected. In such cases, the speed is calculated from the position estimation results. Therefore, I would suggest tuning the covariance parameters in Twist message sent to the EKF instead of sending the can bus information directly to the control module.

[JianKangEgon]

@TakaHoribe Thanks for your quick reply. This solves my concern.

[Sharrrrk]

I would suggest to analyze from three perspectives: accuracy and delay and safety.

Aaccuracy

vehicle can speed: in normal passenger cars, vehicle speed is calculated by ESP/ABS ECU based on WSS (wheel speed sensor), take narmal sedan for example, the accuracy of vehicle speed is around 0.002 m/s.
EKF output: the accuracy the EKF could reach depends on the input source with best accuracy, RTK take xsens 680G as example has velocity accuracy of 0.05 m/s; while for NDT, I can not provide solid data, but it could not be better than 0.03m in range which leads to ~0.003m/s in speed with 10hz LiDAR scans.
Result We could say that they have similar accuracy performance.

Delay

vehicle can speed: The delay of vehicle can speed is the processing time in ECU plus transmission time to AD computer, which is defined by n*10ms.
EKF output: The EKF output delay depends on multiple sources, for EKF the delay is much smaller but with less confidence level, while with vehicle speed as input, the confidence level is high yet additional processing time is required for EKF algorism.
Result Overall similar performance of both.

Safety

vehicle can speed: The vehicle ECU and CAN netork is designed to have high safety level, as long as the AD computer can communicate via CAN, the safety of the signal chain is solid. Also as long as the AD computer could send out CAN control message, it definitely reads back the vehicle can speed.
EKF output: With additional input sources and addtional nodes, more risks is introduced to the whole pipeline and makes it a less safe solution .
Result vehicle CAN speed safer.

Summary

Item	vehicle CAN speed	EKF twist output
Accuracy	Good	Good
Delay	Good	Good
Safety	Good	Not good

So in my oppinion, it's better to use vehicle CAN speed directly in control module for bettery safety, while using EKF twist output does not introduce better perfomance in accuracy or delay.

[plane-li]

Thank you for your post.

The can bus speed information ( /vehicle/status/velocity_status) is passed to the localization module for proper processing. If the can bus information is clearly more accurate than the localization matching, you can tell the information to the localization module through the error covariance.

The localization module also handles cases where the speed information from the can bus is disconnected. In such cases, the speed is calculated from the position estimation results. Therefore, I would suggest tuning the covariance parameters in Twist message sent to the EKF instead of sending the can bus information directly to the control module.

I agree with you! However,sometimes, the output of localization will shake in the longitudinal direction,which will lead to small different in velocity between EKF and Can bus. I think it may caused by the ndt when vehicle run on the road which has little variation in the longitudinal direction.

[Sharrrrk]

Refer to https://github.com/orgs/autowarefoundation/discussions/291#discussioncomment-2731408
Will close this issue.