@Override
public TwistWithCovarianceStamped getVelocity() {
TwistWithCovarianceStamped velMsg = new TwistWithCovarianceStamped();
velMsg.twist.twist = _velocity.clone();
return velMsg;
}
@Override
public void setVelocity(Twist velocity) {
_velocity = velocity.clone();
}