void StrategyAttackCL::enableBridgeCaptors() {
LOG_WARNING("enableBridgeCaptors %s\n", b2s(useBridgeBumpers_));
if (useBridgeBumpers_) {
Events->enable(EVENTS_NO_BRIDGE_BUMP_LEFT);
if (bridgeDetectionByCenter_ && (bridge_ == BRIDGE_POS_MIDDLE_CENTER || bridge_ == BRIDGE_POS_CENTER)) {
// on n'utilise pas le capteur droit si on passe en sioux
} else {
Events->enable(EVENTS_NO_BRIDGE_BUMP_RIGHT);
}
}
}
ll naive(ll l, ll r) {
ll ans = 0;
for (ll x = l; x <= r; ++x) {
string s = b2s(x);
int n = s.length(), k = init_kmp(s);
if (k < n && n % k == 0) {
cerr << " " << s << endl;
ans += 1;
}
}
return ans;
}
int SWMIopnexec (state_t *sp) {
char q[1024], opnstr[1024], *name, *origname, *s;
opn_t *opnp, *opnbasep, *opntimep, *opnfreqp;
obj_t *objp;
HRESULT hr;
BSTR propb, ctb, wqlb, qb;
SAFEARRAY *props;
VARIANT var;
LONG pu, pl, pi;
ULONG ul;
double v;
kvt_t *kvtp;
int kvti;
ctb = s2b ("countertype");
wqlb = s2b ("WQL");
for (
objp = (obj_t *) dtfirst (objdict); objp;
objp = (obj_t *) dtnext (objdict, objp)
) {
sfsprintf (q, 1024, "select * from %s", objp->obj);
if (verbose)
sfprintf (sfstderr, "query: %s\n", q);
if (!(qb = s2b (q))) {
sfprintf (sfstderr, "cannot convert obj name %s to bstr\n", objp->obj);
return -1;
}
if ((hr = sp->pSvc->ExecQuery (
wqlb, qb, WBEM_FLAG_FORWARD_ONLY | WBEM_FLAG_RETURN_IMMEDIATELY,
0, &sp->pEnum
)) != S_OK) {
sfprintf (
sfstderr, "cannot execute query %s, error %s\n", q, E2ccp (hr)
);
return -1;
}
if (SWMIproxysecurity (sp, sp->pEnum) < 0) {
sfprintf (sfstderr, "cannot enable security for query %s\n", q);
return -1;
}
while (
sp->pEnum->Next (30 * 1000, 1, &sp->pClsObj, &ul
) == S_OK && ul == 1) {
if ((hr = sp->pClsObj->GetNames (
0, WBEM_FLAG_ALWAYS | WBEM_FLAG_NONSYSTEM_ONLY, 0, &props
)) != S_OK) {
sfprintf (
sfstderr, "cannot get props for obj %s, error %s\n",
objp->obj, E2ccp (hr)
);
return -1;
}
if(
(hr = SafeArrayGetLBound (props, 1, &pl)) != S_OK ||
(hr = SafeArrayGetUBound (props, 1, &pu)) != S_OK
) {
sfprintf (
sfstderr, "cannot get props bounds for %s, error %s\n",
objp->obj, E2ccp (hr)
);
return -1;
}
if (pu - pl + 1 > kvtn) {
kvtn = pu - pl + 1;
if (!(kvts = (kvt_t *) vmresize (
Vmheap, kvts, kvtn * sizeof (kvt_t), VM_RSCOPY
))) {
sfprintf (sfstderr, "cannot grow kvt array\n");
return -1;
}
}
name = NULL;
for (pi = pl, kvtl = 0; pi <= pu; pi++, kvtl++) {
kvtp = &kvts[kvtl];
if ((hr = SafeArrayGetElement (props, &pi, &propb)) != S_OK) {
sfprintf (
sfstderr, "cannot get prop name for %d/%s, error %s\n",
pi, objp->obj, E2ccp (hr)
);
continue;
}
kvtp->kp = (char *) b2s (propb);
if ((hr = sp->pClsObj->Get (propb, 0, &var, 0, 0)) != S_OK) {
sfprintf (
sfstderr, "cannot get prop value for %d/%s, error %s\n",
pi, objp->obj, E2ccp (hr)
);
continue;
}
if (
!(kvtp->vp = (char *) V2ccp (&var)) ||
!(kvtp->vp = vmstrdup (Vmheap, kvtp->vp))
) {
kvtl--;
continue;
}
switch (var.vt) {
case VT_UI1: kvtp->v = var.bVal; break;
case VT_I2: kvtp->v = var.iVal; break;
case VT_I4: kvtp->v = var.lVal; break;
default: kvtp->v = strtoull (kvtp->vp, &s, 10); break;
}
if (strcmp (kvtp->kp, "Name") == 0) {
if (var.vt == VT_NULL)
name = "ALL";
else
name = kvtp->vp;
origname = name;
}
if ((hr = sp->pClsObj->GetPropertyQualifierSet (
propb, &sp->pQualSet
)) != S_OK) {
sfprintf (
sfstderr, "cannot get quals for %d/%s, error %s\n",
pi, objp->obj, E2ccp (hr)
);
continue;
}
kvtp->type = V_TYPE_STRING;
if ((hr = sp->pQualSet->Get (
ctb, 0, &var, 0
)) == S_OK && var.vt == VT_I4) {
switch (var.lVal) {
case PERF_COUNTER_RAWCOUNT:
case PERF_COUNTER_LARGE_RAWCOUNT:
kvtp->type = V_TYPE_SIMPLE; break;
case PERF_RAW_FRACTION:
kvtp->type = V_TYPE_WITHBASE; break;
case PERF_RAW_BASE:
case PERF_PRECISION_TIMESTAMP:
kvtp->type = V_TYPE_ISBASE; break;
case PERF_COUNTER_COUNTER:
case PERF_COUNTER_BULK_COUNT:
kvtp->type = V_TYPE_WITHTIMENFREQ; break;
case PERF_100NSEC_TIMER:
kvtp->type = V_TYPE_WITH100NSEC; break;
case PERF_100NSEC_TIMER_INV:
kvtp->type = V_TYPE_WITH100NSECINV; break;
default:
kvtp->type = V_TYPE_SIMPLE; break;
}
} else {
if (strcmp (kvtp->kp, "Timestamp_PerfTime") == 0)
kvtp->type = V_TYPE_ISTIME;
else if (strcmp (kvtp->kp, "Frequency_PerfTime") == 0)
kvtp->type = V_TYPE_ISFREQ;
else if (strcmp (kvtp->kp, "Timestamp_Sys100NS") == 0)
kvtp->type = V_TYPE_IS100NSEC;
else if (kvtp->vp[0] == 0 || isdigit (kvtp->vp[0]))
kvtp->type = V_TYPE_SIMPLE;
}
}
::SafeArrayDestroy (props);
sp->pClsObj->Release ();
for (kvti = 0; kvti < kvtl; kvti++) {
kvtp = &kvts[kvti];
sfsprintf (opnstr, 1024, "%s.%s.%s", objp->obj, kvtp->kp, name);
if (!(opnp = (opn_t *) dtmatch (opndict, opnstr))) {
sfsprintf (opnstr, 1024, "%s.%s.ALL", objp->obj, kvtp->kp);
if ((opnp = (opn_t *) dtmatch (opndict, opnstr)))
name = "ALL";
else
sfsprintf (opnstr, 1024, "%s.%s.%s", objp->obj, kvtp->kp, name);
}
if (!opnp) {
if (kvtp->type < V_TYPE_SUPPORT)
continue;
if (!(opnp = opninsert (
OPN_KIND_S, "", "", "", objp->obj, kvtp->kp, name, ""
))) {
sfprintf (sfstderr, "cannot insert opn\n");
return -1;
}
opnp->objp = objp;
}
if (opnp->ep && strstr (origname, opnp->ep))
continue;
if (opnp->type == 0)
opnp->type = kvtp->type;
opnp->vp = kvtp->vp;
opnp->cv += kvtp->v;
opnp->havec = TRUE;
}
for (kvti = 0; kvti < kvtl; kvti++) {
kvtp = &kvts[kvti];
sfsprintf (opnstr, 1024, "%s.%s.SUM", objp->obj, kvtp->kp);
if (!(opnp = (opn_t *) dtmatch (opndict, opnstr)))
continue;
if (opnp->ep && strstr (origname, opnp->ep))
continue;
if (opnp->type == 0)
opnp->type = kvtp->type;
opnp->vp = kvtp->vp;
opnp->cv += kvtp->v;
opnp->havec = TRUE;
}
}
}
for (
opnp = (opn_t *) dtfirst (opndict); opnp;
opnp = (opn_t *) dtnext (opndict, opnp)
) {
if (opnp->kind != OPN_KIND_P || !opnp->havec)
continue;
if (opnp->type >= V_TYPE_SUPPORT)
continue;
switch (opnp->type) {
case V_TYPE_STRING:
#if 0
sfprintf (
sfstdout, "rt=STAT type=string name=%s str=%s%s%s%s\n",
opnp->mname, opnp->vp,
(opnp->lp[0]) ? " label='" : "", (opnp->lp[0]) ? opnp->lp : "",
(opnp->lp[0]) ? "'" : ""
);
#endif
break;
case V_TYPE_SIMPLE:
sfprintf (
sfstdout, "rt=STAT type=%s name=%s unit=%s num=%lld%s%s%s\n",
opnp->mtype, opnp->mname, opnp->munit, opnp->cv,
(opnp->lp[0]) ? " label='" : "", (opnp->lp[0]) ? opnp->lp : "",
(opnp->lp[0]) ? "'" : ""
);
break;
case V_TYPE_WITHBASE:
sfsprintf (opnstr, 1024, "%s.%s_Base.%s", opnp->op, opnp->pp, opnp->np);
if (!(opnbasep = (opn_t *) dtmatch (opndict, opnstr))) {
sfprintf (sfstderr, "cannot find base property for %s\n", opnp->opn);
continue;
}
if (!opnbasep->havec)
continue;
if ((v = (opnbasep->cv == 0) ? 0.00 : 100.0 * (
opnp->cv / (double) opnbasep->cv
)) < 0.0)
continue;
sfprintf (
sfstdout, "rt=STAT type=%s name=%s unit=%s num=%lf%s%s%s\n",
opnp->mtype, opnp->mname, opnp->munit, v,
(opnp->lp[0]) ? " label='" : "", (opnp->lp[0]) ? opnp->lp : "",
(opnp->lp[0]) ? "'" : ""
);
break;
case V_TYPE_WITHTIMENFREQ:
if (!opnp->havep)
continue;
sfsprintf (opnstr, 1024, "%s.Timestamp_PerfTime.%s", opnp->op, opnp->np);
if (!(opntimep = (opn_t *) dtmatch (opndict, opnstr))) {
sfprintf (sfstderr, "cannot find time property for %s\n", opnp->opn);
continue;
}
if (!opntimep->havec || !opntimep->havep)
continue;
sfsprintf (opnstr, 1024, "%s.Frequency_PerfTime.%s", opnp->op, opnp->np);
if (!(opnfreqp = (opn_t *) dtmatch (opndict, opnstr))) {
sfprintf (sfstderr, "cannot find freq property for %s\n", opnp->opn);
continue;
}
if (!opnfreqp->havec)
continue;
if (
opnp->cv < opnp->pv || opntimep->cv <= opntimep->pv ||
opnfreqp->cv == 0
)
continue;
if ((v = (opnp->cv - opnp->pv) / (
(opntimep->cv - opntimep->pv) / (double) opnfreqp->cv
)) < 0.0) {
if (v > -1)
v = 0.0;
else
continue;
}
sfprintf (
sfstdout, "rt=STAT type=%s name=%s unit=%s num=%lf%s%s%s\n",
opnp->mtype, opnp->mname, opnp->munit, v,
(opnp->lp[0]) ? " label='" : "", (opnp->lp[0]) ? opnp->lp : "",
(opnp->lp[0]) ? "'" : ""
);
break;
case V_TYPE_WITH100NSEC:
case V_TYPE_WITH100NSECINV:
if (!opnp->havep)
continue;
sfsprintf (opnstr, 1024, "%s.Timestamp_Sys100NS.%s", opnp->op, opnp->np);
if (!(opntimep = (opn_t *) dtmatch (opndict, opnstr))) {
sfprintf (sfstderr, "cannot find time property for %s\n", opnp->opn);
continue;
}
if (!opntimep->havec || !opntimep->havep)
continue;
if (opnp->cv < opnp->pv || opntimep->cv <= opntimep->pv)
continue;
if (opnp->type == V_TYPE_WITH100NSEC) {
if ((v = 100.0 * (
(opnp->cv - opnp->pv) /
((double) opntimep->cv - opntimep->pv)
)) < 0.0) {
if (v > -1)
v = 0.0;
else
continue;
}
sfprintf (
sfstdout, "rt=STAT type=%s name=%s unit=%s num=%lf%s%s%s\n",
opnp->mtype, opnp->mname, opnp->munit, v,
(opnp->lp[0]) ? " label='" : "", (opnp->lp[0]) ? opnp->lp : "",
(opnp->lp[0]) ? "'" : ""
);
} else {
if ((v = 100.0 * (
1.0 - (opnp->cv - opnp->pv) /
((double) opntimep->cv - opntimep->pv)
)) < 0.0) {
if (v > -1)
v = 0.0;
else
continue;
}
sfprintf (
sfstdout, "rt=STAT type=%s name=%s unit=%s num=%lf%s%s%s\n",
opnp->mtype, opnp->mname, opnp->munit, v,
(opnp->lp[0]) ? " label='" : "", (opnp->lp[0]) ? opnp->lp : "",
(opnp->lp[0]) ? "'" : ""
);
}
break;
}
}
return 0;
}
void StrategyAttackCL::disableBridgeCaptors() {
LOG_WARNING("enableBridgeCaptors %s\n", b2s(useBridgeBumpers_));
Events->disable(EVENTS_NO_BRIDGE_BUMP_LEFT);
Events->disable(EVENTS_NO_BRIDGE_BUMP_RIGHT);
}
// --------------------------------------------------------------------------
// return true if there is no bridge here
// dummyBumperEvt return false if it must continue the movement
// --------------------------------------------------------------------------
bool StrategyAttackCL::checkBridgeBumperEvent(bool& dummyBumperEvt)
{
dummyBumperEvt = false;
bool holeLeft=Events->isInWaitResult(EVENTS_NO_BRIDGE_BUMP_LEFT);
bool holeRight=Events->isInWaitResult(EVENTS_NO_BRIDGE_BUMP_RIGHT);
if (holeLeft || holeRight) {
dummyBumperEvt = true;
if (RobotPos->x() < 1200 || RobotPos->x()> 1700) return false;
// le pont n'est pas la! Faut vite s'arreter!
Move->emergencyStop();
usleep(500000); // attend 0.5s et regarde a nouveau les bumpers pour voir
// si c'etait une fausse alerte
bool bridgeDetected=false;
BridgePosition oldBridgePos=bridge_;
if (getBridgePosByBumper(bridgeDetected)
&& !bridgeDetected) {
// il n'y a pas de pont ici, c'etait vrai!
LOG_WARNING("No bridge here! %s, current bridge=%s, holeLeft=%s holeRight=%s\n",
RobotPos->txt(), BridgePosTxt(bridge_), b2s(holeLeft), b2s(holeRight));
noBridgeHere();
BridgeCaptorStatus captors[BRIDGE_CAPTORS_NBR];
if (getBridgeCaptors(captors, false)) { // on n'est pas dans la merde les bumpers ne marchent pas
EnvDetectorDist leftSharpBridge = ENV_NO;
EnvDetectorDist rightSharpBridge = ENV_NO;
if (useEnvDetectorsForBridge_) {
if (!EnvDetector->getEnvDetector(ENV_DETECTOR_TOP_LEFT, leftSharpBridge)) {
LOG_ERROR("Can't get left sharp-bridge detector.\n");
leftSharpBridge = ENV_NO;
} else {
LOG_INFO("left-sharp-bridge: %d\n", leftSharpBridge);
}
if (!EnvDetector->getEnvDetector(ENV_DETECTOR_TOP_RIGHT, rightSharpBridge)) {
LOG_ERROR("Can't get right sharp-bridge detector.\n");
rightSharpBridge = ENV_NO;
} else {
LOG_INFO("right-sharp-bridge: %d\n", leftSharpBridge);
}
}
if (captors[BRIDGE_BUMPER_LEFT] != BRIDGE_DETECTED && leftSharpBridge <= ENV_FAR) {
// mark bridges, that are unavailable (current bridge
// is going to get discarded elsewhere)
if (oldBridgePos == BRIDGE_POS_MIDDLE_BORDURE) {
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_BORDURE_BIT));
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_MIDDLE_BORDURE_BIT));
}
if (oldBridgePos == BRIDGE_POS_MIDDLE_CENTER) {
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_BORDURE_BIT));
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_MIDDLE_BORDURE_BIT));
}
} else if (captors[BRIDGE_BUMPER_RIGHT] != BRIDGE_DETECTED && rightSharpBridge <= ENV_FAR) {
if (oldBridgePos == BRIDGE_POS_MIDDLE_BORDURE) {
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_CENTER_BIT));
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_MIDDLE_CENTER_BIT));
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_SIOUX_BIT));
}
if (oldBridgePos == BRIDGE_POS_MIDDLE_CENTER) {
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_CENTER_BIT));
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_MIDDLE_CENTER_BIT));
bridgeAvailibility_ &= (~(1<<BRIDGE_ENTRY_SIOUX_BIT));
}
} else {
LOG_INFO("bridge seems to be here");
dummyBumperEvt=true;
return false;
}
}
LOG_INFO("Availibility: 0x%2.2x\n", bridgeAvailibility_);
dummyBumperEvt=false;
return true;
}
}
LOG_INFO("checkBridgeBumperEvent: dummy=%s\n", b2s(dummyBumperEvt));
return false;
}
