void replay_demo(int rateHertz) {
ros::Rate r(rateHertz);
for(int i = 0; i < cartesian_velocities.size(); i++) {
std::cout << "\n\ni = " << i << "\n";
if (!zeroVelocity(cartesian_velocities[i])) {
std::cout << "Non zero velocities at " << i << "\n";
}
int num_replayed_same_vel = 0;
do {
// std::cout << "Publishing velocity " << cartesian_velocities[i] << "\n";
pub_velocity.publish(cartesian_velocities[i]);
r.sleep();
ros::spinOnce();
num_replayed_same_vel++;
} while(!poseMatch(i+1) && !zeroVelocity(cartesian_velocities[i]) && !overshot(i+1) && num_replayed_same_vel < 3);
if (poseMatch(i+1)) {
std::cout << "Pose match at i = " << i << "\n";
}
if (overshot(i+1)) {
std::cout << "Overshot at i = " << i << "\n";
}
}
}
void plWalkingStrategy::Update(float delSecs)
{
if (fGroundHit || fFalseGround)
fTimeInAir = 0.0f;
else
{
fTimeInAir += delSecs;
if (fHeadHit)
{
// If we're airborne and hit our head, override achieved velocity to avoid being shoved sideways
hsVector3 velocity = fController->GetLinearVelocity();
hsVector3 achievedVelocity = fController->GetAchievedLinearVelocity();
achievedVelocity.fX = velocity.fX;
achievedVelocity.fY = velocity.fY;
if (achievedVelocity.fZ > 0.0f)
achievedVelocity.fZ = 0.0f;
fController->OverrideAchievedLinearVelocity(achievedVelocity);
}
}
hsVector3 zeroVelocity(0.f, 0.f, 0.f);
fController->SetLinearVelocity(zeroVelocity);
if (!fHitGroundInThisAge && IsOnGround())
fHitGroundInThisAge = true;
if (fClearImpact)
{
fImpactTime = 0.0f;
fImpactVelocity.Set(0.0f, 0.0f, 0.0f);
}
if (IsOnGround())
fClearImpact = true;
else
{
fImpactTime = fTimeInAir;
fImpactVelocity = fController->GetAchievedLinearVelocity();
// convert orientation from subworld to avatar-local coordinates
fImpactVelocity = (hsVector3)fController->GetLocalRotation().Rotate(&fImpactVelocity);
fClearImpact = false;
}
}
/**
* This is what actually shoots. Finally!
*/
void Energy::fire() {
if (!isReady())
return;
lastFiredFrame = curFrame;
if (ship->gameState->gsm == ClientMode) {
return;
}
chargingShot = dynamic_cast<EnergyShot*>(ship->gameState->custodian[chargingShotid]);
// If we haven't started a shot yet...
if (chargeStartTime == 0 && chargingShot == NULL) {
chargeStartTime = ship->gameState->getGameTime();
Vector3D zeroVelocity(0, 0, 0);
chargingShot = new EnergyShot(ship->shotOrigin, zeroVelocity, index, ship, ship->gameState);
ship->custodian->add(chargingShot);
chargingShotid = chargingShot->id;
}
}
