public void TwistCallback(SIGVerse.ROSBridge.geometry_msgs.Twist twist)
{
// TODO: Add MaxSpeedBaseX and MaxSpeedBaseY to HRSCommon or replace with MaxSpeedBaseTrans and limit based on norm
this.linearVelX = Mathf.Sign(twist.linear.x) * Mathf.Clamp(Mathf.Abs(twist.linear.x), 0.0f, HSRCommon.MaxSpeedBase);
this.linearVelY = Mathf.Sign(twist.linear.y) * Mathf.Clamp(Mathf.Abs(twist.linear.y), 0.0f, HSRCommon.MaxSpeedBase);
this.angularVel = Mathf.Sign(twist.angular.z) * Mathf.Clamp(Mathf.Abs(twist.angular.z), 0.0f, HSRCommon.MaxSpeedBaseRad);
this.isMoving = Mathf.Abs(this.linearVelX) >= 0.001f || Mathf.Abs(this.linearVelY) >= 0.001f || Mathf.Abs(this.angularVel) >= 0.001f;
}
public void TwistCallback(SIGVerse.ROSBridge.geometry_msgs.Twist twist)
{
float linearVel = Mathf.Sqrt(Mathf.Pow(twist.linear.x, 2) + Mathf.Pow(twist.linear.y, 2));
float linearVelClamped = Mathf.Clamp(linearVel, 0.0f, HSRCommon.MaxSpeedBase);
if (linearVel >= 0.001)
{
this.linearVelX = twist.linear.x * linearVelClamped / linearVel;
this.linearVelY = twist.linear.y * linearVelClamped / linearVel;
}
else
{
this.linearVelX = 0.0f;
this.linearVelY = 0.0f;
}
this.angularVelZ = Mathf.Sign(twist.angular.z) * Mathf.Clamp(Mathf.Abs(twist.angular.z), 0.0f, HSRCommon.MaxSpeedBaseRad);
// Debug.Log("linearVel=" + linearVel + ", angularVel=" + angularVel);
this.isMoving = Mathf.Abs(this.linearVelX) >= 0.001f || Mathf.Abs(this.linearVelY) >= 0.001f || Mathf.Abs(this.angularVelZ) >= 0.001f;
}
