public override void MoveDirect(Vector2 command) { if (CurrenLocomotionType != RobotLocomotionType.DIRECT) { _rosLocomotionWaypointState.PublishData(ROSLocomotionWaypointState.RobotWaypointState.STOP); } _rosLocomotionDirect.PublishData(command.y, command.x); CurrenLocomotionType = RobotLocomotionType.DIRECT; CurrentRobotLocomotionState = RobotLocomotionState.MOVING; }
void Update() { if (_rosBridge == null) { return; } if (!goal_set) { if (!_sentCommand) { _rosLocomotionState.PublishData(new StringMsg(subState)); _sentCommand = true; } return; } else { _sentCommand = false; } angle = Vector3.SignedAngle(goal - transform.position, transform.forward, transform.up) * Mathf.Deg2Rad; distance = Vector3.Distance(transform.position, goal); switch (state) { case "RUNNING": switch (subState) { default: subState = "TURNING"; break; case "TURNING": vel._linear._x = 0; vel._angular._z = k_alpha * angle; if (Mathf.Abs(angle) < 0.2f) { subState = "FORWARDING"; } break; case "FORWARDING": if (angle > Mathf.PI / 2) { vel._linear._x = -k_rho * distance; vel._angular._z = k_alpha * (angle - Mathf.PI); } else if (angle < -Mathf.PI / 2) { vel._linear._x = -k_rho * distance; vel._angular._z = k_alpha * (angle + Mathf.PI); } else { vel._linear._x = k_rho * distance; vel._angular._z = k_alpha * angle; } vel._linear._x = Mathf.Clamp((float)vel._linear._x, -vel_maxlin, vel_maxlin); vel._angular._z = Mathf.Clamp((float)vel._angular._z, -vel_maxang, vel_maxang); PublishVelocity(vel); break; } PublishVelocity(vel); break; case "PARK": subState = "STOP"; vel._linear._x = 0; vel._angular._z = 0; PublishVelocity(vel); break; default: if (prestate == "RUNNING") { subState = "STOP"; vel._linear._x = 0; vel._angular._z = 0; PublishVelocity(vel); } else { subState = "IDLE"; } break; } prestate = state; _publishTimer -= Time.deltaTime; if (_publishTimer <= 0) { _publishTimer = _publishInterval; _rosLocomotionWaypointState.PublishData(subState); } }