private void Move(Vector3 position)
{
_rosLogger.PublishData(new StringMsg("Moving to: " + position.ToUTM().ToWGS84()));
GeoPointWGS84 point = position.ToUTM().ToWGS84();
_rosLocomotionWaypoint.PublishData(point);
_currentWaypoint = position;
CurrenLocomotionType = RobotLocomotionType.WAYPOINT;
_rosLocomotionWaypointState.PublishData(ROSLocomotionWaypointState.RobotWaypointState.RUNNING);
CurrentRobotLocomotionState = RobotLocomotionState.MOVING;
}
