The ROS community usually uses cpp for that kind of stuff. (For obvious reasons)

But do you also know a implementation in python. Maybe even optimized for runtime.