44 lines
3.2 KiB
JavaScript
44 lines
3.2 KiB
JavaScript
|
const setup=require('./route_bench_setup.cjs');
|
||
|
|
const solverPath=process.argv[2];const N=+process.argv[3]||40;const live=process.argv[4]==='live';
|
||
|
|
const {S}=setup(solverPath);
|
||
|
|
const L=S.LIM,O=S.ORDER,w=S.wrap180;
|
||
|
|
let s=12345;const rnd=()=>{s=(s*1664525+1013904223)>>>0;return s/4294967296};
|
||
|
|
const rq=()=>O.map(id=>{const t=L[id];return t.lo+rnd()*(t.hi-t.lo)});
|
||
|
|
const d=(a,b)=>a.map((v,i)=>L[O[i]].cont?w(b[i]-v):b[i]-v);
|
||
|
|
function segOK(a,b){const dd=d(a,b);const m=Math.max(...dd.map(Math.abs));const n=Math.max(1,Math.ceil(m/1));for(let i=1;i<=n;i++){if(!S.collideFree(a.map((v,k)=>v+dd[k]*i/n)))return false}return true}
|
||
|
|
// build cases: free start, free goal config, straight blocked
|
||
|
|
const cases=[];let tries=0;
|
||
|
|
while(cases.length<N&&tries<20000){tries++;const a=rq(),b=rq();if(!S.collideFree(a)||!S.collideFree(b))continue;
|
||
|
|
if(segOK(a,b))continue; // only obstacle cases
|
||
|
|
cases.push({from:a,T:S.fk(b,0)});}
|
||
|
|
const W=[1,1,1,1,1,1];
|
||
|
|
let res={n:cases.length,reached:0,valid:0,blocked:0,len:0,lenDirect:0,ms:0,rev:0,segs:0,timeExec:0,fail:[]};
|
||
|
|
const VMAX=[60,60,90,90,120,180]; // deg/s assumed
|
||
|
|
function execTime(pts){let t=0;for(let i=1;i<pts.length;i++){const dd=d(pts[i-1],pts[i]);t+=Math.max(...dd.map((x,k)=>Math.abs(x)/VMAX[k]))+ (pts.length>2&&i<pts.length-1?0.25:0)}return t}
|
||
|
|
for(const c of cases){
|
||
|
|
const t0=performance.now();
|
||
|
|
const opt={tool:0,step:0.5,ref:c.from,from:c.from.slice()};
|
||
|
|
if(live){opt.noVia=true;opt.quick=true;opt.warm=c.from}
|
||
|
|
const r=live?S.solveRoute(c.T,opt):S.solveDyn(c.T,opt);
|
||
|
|
const ms=performance.now()-t0;res.ms+=ms;
|
||
|
|
const pts=[c.from].concat(r.via||[],[r.q]);
|
||
|
|
// unwrap
|
||
|
|
for(let i=1;i<pts.length;i++)pts[i]=pts[i-1].map((v,k)=>L[O[k]].cont?v+w(pts[i][k]-v):pts[i][k]);
|
||
|
|
let ok=true;for(let i=1;i<pts.length;i++)if(!segOK(pts[i-1],pts[i])){ok=false;break}
|
||
|
|
const tcp=S.fk(r.q,0);const err=Math.hypot(tcp[0]-c.T[0],tcp[1]-c.T[1],tcp[2]-c.T[2]);
|
||
|
|
const reached=err<1&&!r.blocked;
|
||
|
|
if(reached)res.reached++; if(ok)res.valid++; if(r.blocked)res.blocked++;
|
||
|
|
let len=0;for(let i=1;i<pts.length;i++)len+=d(pts[i-1],pts[i]).reduce((a,x)=>a+Math.abs(x),0);
|
||
|
|
const direct=d(c.from,r.q).reduce((a,x)=>a+Math.abs(x),0);
|
||
|
|
// reversals
|
||
|
|
let rev=0;for(let k=0;k<6;k++){let sg=0;for(let i=1;i<pts.length;i++){const x=d(pts[i-1],pts[i])[k];if(Math.abs(x)<2)continue;const s2=Math.sign(x);if(sg&&s2!==sg)rev++;sg=s2}}
|
||
|
|
res.per=res.per||[];res.per.push({ms:Math.round(ms),reached,ok,len:Math.round(len),rev,segs:pts.length-1,exec:+execTime(pts).toFixed(2),detour:r.detour||null});
|
||
|
|
if(reached&&ok){res.len+=len;res.lenDirect+=direct;res.rev+=rev;res.segs+=pts.length-1;res.timeExec+=execTime(pts)}
|
||
|
|
else res.fail.push({reached,ok,err:+err.toFixed(1),detour:r.detour||null,blocked:!!r.blocked});
|
||
|
|
}
|
||
|
|
const g=res.reached;const both=cases.length-res.fail.length;
|
||
|
|
require('fs').writeFileSync(process.argv[5]||require('path').join(require('os').tmpdir(),'route_bench_per.json'),JSON.stringify(res.per));console.log(JSON.stringify({n:res.n,reached:res.reached,validPath:res.valid,goodBoth:both,blocked:res.blocked,
|
||
|
|
avgPlanMs:+(res.ms/res.n).toFixed(0),avgJointTravel:+(res.len/both).toFixed(0),avgDirect:+(res.lenDirect/both).toFixed(0),
|
||
|
|
avgReversals:+(res.rev/both).toFixed(2),avgSegs:+(res.segs/both).toFixed(2),avgExecS:+(res.timeExec/both).toFixed(2),
|
||
|
|
failTypes:res.fail.slice(0,12)}));
|