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);
}
}