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