public void RestartFootstepListener()
{
if (fl.Listening)
{
fl.Stop();
}
FootstepStatus.status = StatusButton.Status.Neutral;
fl = SetupFL();
}
void Start()
{
Debug.Log("Starting up! :D");
VisionPortField.text = VisionPort.ToString();
FootstepPortField.text = FootstepPort.ToString();
PosePortField.text = PosePort.ToString();
if (InitComms())
{
vl = SetupVL();
pl = SetupPL();
fl = SetupFL();
}
}
private LolaComms.FootstepListener SetupFL()
{
Debug.Log("Setting up FootstepListener...");
LolaComms.FootstepListener fl = new LolaComms.FootstepListener(FootstepPort, FootstepIP);
fl.onError += (errstr) =>
{
Debug.LogError("[Footsteps] " + errstr);
};
fl.onConnect += (host) =>
{
Debug.Log("[Footsteps] Connected to: " + host);
};
fl.onDisonnect += (host) =>
{
Debug.Log("[Footsteps] Disconnected from: " + host);
};
fl.onNewStep += (step) =>
{
Debug.Log("Got new footstep for " + (LolaComms.Foot)(step.stance) + "{" + step.stamp_gen + "} @ [" + step.start_x + ", " + step.start_y + ", " + step.start_z + "], start_phi_z: " + step.start_phi_z +
", phi0: " + step.phiO + ", phi_leg_rel: " + step.phi_leg_rel + ", [dy, dz]: [" + step.dy + ", " + step.dz_clear + "]");
var foot = (LolaComms.Foot)step.stance;
switch (foot)
{
case LolaComms.Foot.Left:
{
if (footsteps_L.Count == 0 ||
(footsteps_L.Count > 0 && footsteps_L.Count < max_steps &&
footsteps_L[0].step.stamp_gen == step.stamp_gen))
{
Debug.Log("Appending step " + footsteps_L.Count + " for stamp " + step.stamp_gen);
left_foot_angle += step.phiO;
AddFootstep(footsteps_L, step);
}
else // new timestamp
{
Debug.Log("Starting new step sequence for left foot @ stamp " + step.stamp_gen);
var renderers = footsteps_L[0].go.GetComponentsInChildren <Renderer>();
foreach (var r in renderers)
{
r.material = foot_left_permanent;
}
footsteps_L_real.Add(footsteps_L[0]); // first footstep from each stamp is robot's actual standing pose
left_foot_angle_real += footsteps_L[0].step.phiO;
left_foot_angle = left_foot_angle_real;
// delete all other steps
footsteps_L.RemoveAt(0);
foreach (var footstep in footsteps_L)
{
Destroy(footstep.go);
}
footsteps_L.Clear();
//footsteps_L.Add(footsteps_L_real[footsteps_L_real.Count - 1]);
// add new footstep as normal
left_foot_angle += step.phiO;
AddFootstep(footsteps_L, step);
}
}
break;
case LolaComms.Foot.Right:
{
if (footsteps_R.Count == 0 ||
(footsteps_R.Count > 0 && footsteps_R.Count < max_steps &&
footsteps_R[0].step.stamp_gen == step.stamp_gen))
{
Debug.Log("Appending step " + footsteps_R.Count + " for stamp " + step.stamp_gen);
right_foot_angle += step.phiO;
AddFootstep(footsteps_R, step);
}
else // new timestamp
{
Debug.Log("Starting new step sequence for left foot @ stamp " + step.stamp_gen);
var renderers = footsteps_R[0].go.GetComponentsInChildren <Renderer>();
foreach (var r in renderers)
{
r.material = foot_right_permanent;
}
footsteps_R_real.Add(footsteps_R[0]); // first footstep from each stamp is robot's actual standing pose
right_foot_angle_real += footsteps_R[0].step.phiO;
right_foot_angle = right_foot_angle_real;
// delete all other steps
footsteps_R.RemoveAt(0);
foreach (var footstep in footsteps_R)
{
Destroy(footstep.go);
}
footsteps_R.Clear();
// add new footstep as normal
right_foot_angle += step.phiO;
AddFootstep(footsteps_R, step);
}
}
break;
default:
{
Debug.LogError("Unknown foot stance value: " + step.stance);
}
break;
}
Debug.Log("Left foot rotations: curr == " + left_foot_angle + ", total real == " + left_foot_angle_real);
Debug.Log("Right foot rotations: curr == " + right_foot_angle + ", total real == " + right_foot_angle_real);
};
fl.Listen();
Debug.Log("FootstepListener: " + (fl.Listening ? "is listening" : "is NOT listening"));
return(fl);
}
