Skip to content

Commit 913e575

Browse files
joepuzzojoepuzzo
committedDec 17, 2015
Awesome new resolver tool
1 parent 0ec1faf commit 913e575

File tree

5 files changed

+148
-52
lines changed

5 files changed

+148
-52
lines changed
 

‎lib/collision-system.js

+65-40
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,8 @@ var Planner = require('./planner.js'), // Plans and Trajectories
77
CollisionDetector = require('./algorithms/collision/collision-detector.js'),
88
Trajectory = require('./planner.js').Trajectory;
99
/*----------------------------Constants------------------------------*/
10-
const LOG = false;
10+
var LOG = false;
11+
const DEBUG = false
1112
const DSAFE = 200;
1213

1314
/*----------------------------CollisionSystem-------------------------------*/
@@ -21,8 +22,9 @@ const DSAFE = 200;
2122
* @param id
2223
* @param plan
2324
*/
24-
function CollisionSystem( messenger, gps, id) {
25-
25+
function CollisionSystem( messenger, gps, id, log ) {
26+
27+
LOG = log;
2628
this.messenger = messenger;
2729
this.gps = gps;
2830
this.id = id;
@@ -61,7 +63,7 @@ CollisionSystem.prototype.handleHIA = function( message, from ) {
6163

6264
//console.log("MYPOS: ",myLocation);
6365
//console.log("THEIRPOS: ",message);
64-
console.log("DISTANCE from",this.id , "to", from.id, d );
66+
if(LOG) console.log("DISTANCE from",this.id , "to", from.id, d );
6567

6668
// Assume that collision avoidance with from is alreadhy
6769
// being taken care of if from is in cordlinks already
@@ -72,7 +74,7 @@ CollisionSystem.prototype.handleHIA = function( message, from ) {
7274
// If I have larger address, I send roll assign
7375
if( from.id < this.id ){
7476

75-
console.log(this.id,"NEW LINK CREATED WITH", from.id);
77+
if(DEBUG) console.log(this.id,"NEW LINK CREATED WITH", from.id);
7678

7779
// randomly generate 0 or 1
7880
// 0 is "you are coordinator I am partner"
@@ -124,12 +126,12 @@ CollisionSystem.prototype.handleHIA = function( message, from ) {
124126
*/
125127
CollisionSystem.prototype.handleRollAssignment = function( message, from ) {
126128

127-
console.log(this.id,"RECIEVED ROLL ASSIGNMENT FROM",from.id);
129+
if(DEBUG) console.log(this.id,"RECIEVED ROLL ASSIGNMENT FROM",from.id);
128130

129131
// If we don't have a coord link with this guy
130132
if( !this.coordLinks.get(from.id) ){
131133
// Create a new cord link with this guy
132-
console.log(this.id,"NEW LINK ESTABLISHED WITH", from.id);
134+
if(DEBUG) console.log(this.id,"NEW LINK ESTABLISHED WITH", from.id);
133135
this.coordLinks.set( from.id, new CoordLink( from, message.roll ) );
134136
}
135137

@@ -158,13 +160,13 @@ CollisionSystem.prototype.handlePlanRequest = function( message, from ) {
158160
// Get a coordlink if we have one
159161
var clink = this.coordLinks.get( from.id );
160162

161-
console.log(this.id,"HANDLING PLAN REQUEST FOR PLAN", message.scheduleid, "FROM",from.id);
163+
if(DEBUG) console.log(this.id,"HANDLING PLAN REQUEST FOR PLAN", message.scheduleid, "FROM",from.id);
162164
//console.log("CURRENT ID", clink.scheduleid );
163165

164166
// If we have a clink with this guy and we have not already recieved this plan request
165167
if( clink && clink.scheduleid < message.scheduleid ){
166168

167-
console.log(this.id,"ORIGINAL PLAN REQUEST!");
169+
if(DEBUG) console.log(this.id,"ORIGINAL PLAN REQUEST!");
168170

169171
// Get the next segments to be send in the schedule
170172
var nextSegs = this.gps.getNextSegments();
@@ -191,12 +193,11 @@ CollisionSystem.prototype.handlePlanResponse = function( message, from ) {
191193
// Get a coordlink if we have one
192194
var clink = this.coordLinks.get( from.id );
193195

194-
console.log(this.id,"HANDLING PLAN RESPONSE FOR PLAN", message.scheduleid, "FROM",from.id);
195-
//console.log("CURRENT ID", clink.scheduleid );
196+
if(DEBUG) console.log(this.id,"HANDLING PLAN RESPONSE FOR PLAN", message.scheduleid, "FROM",from.id);
196197

197198
// If we have a coord link with this guy and we have not already recieved this plan response
198199
if( this.coordLinks.get(from.id) && clink.scheduleid < message.scheduleid ){
199-
console.log(this.id,"NEW PLAN RECIVED");
200+
if(DEBUG) console.log(this.id,"NEW PLAN RECIVED");
200201

201202
// Update the schedule id
202203
clink.scheduleid = message.scheduleid;
@@ -232,9 +233,11 @@ CollisionSystem.prototype.handlePlanResponse = function( message, from ) {
232233
}
233234

234235

235-
console.log(this.id,"MYPLAN ",myPlan);
236-
console.log(this.id,"THEIRPLAN",from.id, theirPlan);
237-
console.log(this.id,"COLLLLLLISION",collision);
236+
if(DEBUG) {
237+
console.log(this.id,"MYPLAN ",myPlan);
238+
console.log(this.id,"THEIRPLAN",from.id, theirPlan);
239+
console.log(this.id,"COLLLLLLISION",collision);
240+
}
238241

239242
// If we have a collision
240243
if(collision){
@@ -284,7 +287,7 @@ CollisionSystem.prototype.handlePlanResponse = function( message, from ) {
284287

285288
// When we get out of the collision sphere let our parner know
286289
this.gps.when( col.isect1, col.trajectory1.trajectory, dcol, function(){
287-
console.log("TO2:",from);
290+
if( DEBUG ) console.log("TO2:",from);
288291
my.messenger.sendLocal( [permission], from );
289292
//my.cleanupCoordLink(fr.id);
290293
});
@@ -322,16 +325,18 @@ CollisionSystem.prototype.removeFromSegPerm = function( granter, trajectory ){
322325
if(waitingOn.length==0){
323326
this.gps.unpause();
324327
}else{
325-
console.log("UGGGGGG REMOVED LINK AND NOW SEGPERM IS",waitingOn);
328+
if( DEBUG ) console.log("UGGGGGG REMOVED LINK AND NOW SEGPERM IS",waitingOn);
326329
}
327330
}
328331
}
329332
}
330333
return;
331334
}
332335

333-
console.log("GRANTER:",granter);
334-
console.log( "ALWAYS STRING:", trajectory );
336+
if( DEBUG ) {
337+
console.log("GRANTER:",granter);
338+
console.log( "ALWAYS STRING:", trajectory );
339+
}
335340

336341
// Get this permissions for the sent trajectory
337342
var waitingOn = this.segPermissions.get( trajectory );
@@ -350,7 +355,7 @@ CollisionSystem.prototype.removeFromSegPerm = function( granter, trajectory ){
350355
if( waitingOn.length === 0 ){
351356
this.gps.unpause();
352357
}else{
353-
console.log("UGGGGGG REMOVED LINK AND NOW SEGPERM IS",waitingOn);
358+
if(DEBUG) console.log("UGGGGGG REMOVED LINK AND NOW SEGPERM IS",waitingOn);
354359
}
355360
}
356361
}
@@ -387,7 +392,7 @@ CollisionSystem.prototype.handleScheduleReply = function( message, from ) {
387392
var collision = message.collision;
388393
var collisions = message.collisions;
389394

390-
console.log(this.id,"Recieved Schedule from:",from.id, schedule);
395+
if(DEBUG) console.log(this.id,"Recieved Schedule from:",from.id, schedule);
391396

392397
// myPlan is a map from trajseg -> 0 || 1 || 2
393398
for( var trajSeg of schedule ){
@@ -398,8 +403,11 @@ CollisionSystem.prototype.handleScheduleReply = function( message, from ) {
398403

399404
// If the action is 1
400405
if( act === 1 ){
401-
console.log(this.id,"NEED TO UPDATE SEGPERM OF ",mySeg);
402-
console.log(this.id,"MY SEGPERMS ARE: ",this.segPermissions);
406+
407+
if(DEBUG){
408+
console.log(this.id,"NEED TO UPDATE SEGPERM OF ",mySeg);
409+
console.log(this.id,"MY SEGPERMS ARE: ",this.segPermissions);
410+
}
403411

404412
// Get the seg permissions for this trajectory and update it
405413
// TODO WHY IS THIS NESSISARY
@@ -432,14 +440,16 @@ CollisionSystem.prototype.handleScheduleReply = function( message, from ) {
432440
//var fr = from;
433441

434442
this.gps.when( collision.isect2, collision.trajectory2.trajectory, dcol, function(){
435-
console.log("TO:",from);
443+
if(DEBUG) console.log("TO:",from);
436444
my.messenger.sendLocal( [permission], from );
437445
//my.cleanupCoordLink( fr.id );
438446
});
439447
}
440448
}
441-
console.log(this.id,"HERE IS WHAT WE HAVE BEEN WAITING FOR!! A MODIFIED PLAN!!!!!");
442-
console.log( this.segPermissions );
449+
if(DEBUG){
450+
console.log(this.id,"HERE IS WHAT WE HAVE BEEN WAITING FOR!! A MODIFIED PLAN!!!!!");
451+
console.log( this.segPermissions );
452+
}
443453
}
444454
}
445455

@@ -514,33 +524,48 @@ CollisionSystem.prototype.registerMessages = function() {
514524
var messenger = this.messenger;
515525
var my = this;
516526

517-
/*this.gps.on("PlanComplete",function(){
527+
this.gps.on("PlanComplete",function(){
518528
// Itterate over all the coordlinks and remove them
519529
for( var clink of my.coordLinks ){
530+
531+
532+
var gp = new CMessages.GrantPermission( );
533+
messenger.sendLocal( [gp], clink[1].linkPartner );
520534

521-
// Grant permission to everyone waiting for me!
522-
var gp = new CMessages.GrantPermission();
523-
messenger.sendLocal( [gp], clink[1].linkPartner );
535+
536+
//Itterate over all the trajectories in their plan
537+
/*if( clink[1].schedule !== undefined ){
538+
for( var traj of clink[1].schedule ) {
539+
540+
// If this trajectory is a partner waiting on me
541+
if( traj[1] === 2 ) {
542+
543+
// Grant permission to everyone waiting for me!
544+
var gp = new CMessages.GrantPermission( traj[0] );
545+
messenger.sendLocal( [gp], clink[1].linkPartner );
546+
}
524547
548+
}
549+
}*/
525550
// Clean up my partner
526551
my.cleanupCoordLink( clink[1].linkPartner.id );
527552
}
528-
});*/
553+
});
529554

530555
this.gps.on("TrajectoryChange", function(traj){
531556

532-
console.log("CHANGED TRAJ TO",traj);
557+
if(DEBUG) console.log("CHANGED TRAJ TO",traj);
533558

534559
// If the trajectory that we are about to execute
535560
// has a non zero permissions length then pause
536561
var waitingOn = my.segPermissions.get( Trajectory.stringify(traj) );
537562

538-
console.log( "WAITINGON FOR THIS SEG", waitingOn );
563+
if(DEBUG) console.log( "WAITINGON FOR THIS SEG", waitingOn );
539564

540565
// If we are waiting on this segment
541566
if( waitingOn.length > 0 ){
542567
// Pause the gps
543-
console.log("SHOULD PAUSE GPS NOW!!!!!!!!!!!!!!!!!!!!!!");
568+
if(DEBUG) console.log("SHOULD PAUSE GPS NOW!!!!!!!!!!!!!!!!!!!!!!");
544569
my.gps.pause();
545570
}
546571

@@ -553,12 +578,12 @@ CollisionSystem.prototype.registerMessages = function() {
553578
// Get the c link out
554579
link = links[1];
555580

556-
console.log("LINKS SCHEDULE:",link.schedule);
581+
if(DEBUG)console.log("LINKS SCHEDULE:",link.schedule);
557582

558583
// Check to see if this link contains the trajectory
559584
if( link.schedule && link.schedule.get( Trajectory.stringify( traj ) ) !== undefined ) {
560585

561-
console.log("WE HAVE A SCHEDULE WITH THIS TRAJECTORY");
586+
if(DEBUG)console.log("WE HAVE A SCHEDULE WITH THIS TRAJECTORY");
562587

563588
// Update the schedule steps
564589
link.steps++;
@@ -567,7 +592,7 @@ CollisionSystem.prototype.registerMessages = function() {
567592
// TODO 2 ??
568593
if( link.steps === 3 ){
569594

570-
console.log(my.id, "UPDATING LINK");
595+
if(DEBUG)console.log(my.id, "UPDATING LINK");
571596

572597
// Reset the steps
573598
link.steps = 0;
@@ -605,7 +630,7 @@ CollisionSystem.prototype.registerMessages = function() {
605630

606631
CollisionSystem.prototype.cleanupCoordLink = function(linkpartnerid){
607632

608-
console.log("cleaningupcordlinkwith ",linkpartnerid);
633+
if(DEBUG)console.log("cleaningupcordlinkwith ",linkpartnerid);
609634

610635
// Remove this link partner from all segments waiting on him
611636
this.removeFromSegPerm( linkpartnerid, "all" );
@@ -620,8 +645,8 @@ CollisionSystem.prototype.cleanupCoordLink = function(linkpartnerid){
620645
* @param amcoord
621646
*/
622647
function CoordLink(linkPartner,amcoord){
623-
var schedule; // my current schedule
624-
var partnerPlan; // don't let the name fool you, this is your plan if you are the parner
648+
var schedule; // my current schedule
649+
var partnerPlan; // don't let the name fool you, this is your plan if you are the parner
625650
this.linkPartner = linkPartner; // my link partner
626651
this.amcoord = amcoord; // 0 if not, 1 if am
627652
this.scheduleid = 0; // The id of the schedule we are working on

‎lib/data-node.js

+13-4
Original file line numberDiff line numberDiff line change
@@ -30,7 +30,14 @@ function DataNode( params, callback ) {
3030

3131
// Register for complete message ( this will shut down the advertisments )
3232
var my = this;
33-
this.gps.once("PlanComplete", function(){ my.cleanup; } );
33+
this.gps.once("PlanComplete", function(){ my.cleanup(); } );
34+
35+
// Print log infor when done
36+
if( params.log ) {
37+
this.gps.once( "PlanComplete", function( p ) {
38+
console.log(params.id,"has completed his route");
39+
});
40+
}
3441

3542
// A node is required to have a messenger for communication
3643
this.messenger = params.messenger || new Messenger( params.id );
@@ -41,7 +48,7 @@ function DataNode( params, callback ) {
4148
// A unique id
4249
this.id = params.id;
4350

44-
this.collisionSystem = new CollisionSystem(this.messenger,this.gps,this.id);
51+
this.collisionSystem = new CollisionSystem(this.messenger,this.gps,this.id,params.log);
4552

4653
// Register for all messages
4754
this.registerMessages();
@@ -80,9 +87,11 @@ DataNode.prototype.cleanup = function() {
8087
for( var i of this.intervals ) {
8188
clearInterval( i[1] );
8289
}
83-
8490
// Now shut down the messenger
85-
this.messenger.close();
91+
var my = this;
92+
setTimeout(function () {
93+
my.messenger.close();
94+
}, 4000);
8695
}
8796

8897

‎lib/resolve.js

+7-4
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,8 @@ const LOG = false;
1515
* Resolver constructor
1616
* @param plans an array of plans, one for each node.
1717
*/
18-
function Resolver( plans, speed ) {
18+
function Resolver( plans, speed, log ) {
19+
this.log = log;
1920
this.originalPlans = plans;
2021
this.resolutePlans = [];
2122
this.nodes = [];
@@ -66,10 +67,12 @@ Resolver.prototype.initNodes = function( plans, callback ) {
6667

6768
// Loop through and create a node for every plan
6869
var nnodes = plans.length;
70+
var my = this;
6971
for( i = 0; i < nnodes; i++ ) {
7072
this.nodes.push( new DataNode( {
7173
gps: new FakeGPS( plans[i], this.speed ),
72-
id: i
74+
id: i,
75+
log: my.log
7376
}, callback ) );
7477
}
7578
}
@@ -78,13 +81,13 @@ Resolver.prototype.initNodes = function( plans, callback ) {
7881
/*----------------------------Exports-------------------------------*/
7982

8083
// This is the resolve function
81-
exports.resolve = function( input, speed, callback ) {
84+
exports.resolve = function( input, speed, log, callback ) {
8285

8386
// Parse the input into the original plans
8487
Parser.parse( input, function( plans ){
8588

8689
// Once we have the plan create a new resolver
87-
var resolver = new Resolver( plans, speed );
90+
var resolver = new Resolver( plans, speed, log );
8891

8992
// Call the resolvers resolve function, passing in the callback
9093
resolver.resolve( callback );

‎resolver.js

+59
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,59 @@
1+
/*-----------------------------Imports-------------------------------*/
2+
var Parser = require('./lib/node-by-line.js'),
3+
Resolver = require('./lib/resolve.js'),
4+
fs = require('fs');
5+
6+
/*----------------------------Constants------------------------------*/
7+
8+
const LOG = false;
9+
const sep = "---------------------------------------------------------"
10+
11+
12+
/*------------------------------Input--------------------------------*/
13+
// Variables for argumentes
14+
var config = {}
15+
var filename = process.argv[2];
16+
var verbose = false;
17+
var speed = 1;
18+
19+
// Process the arguments
20+
process.argv.forEach(function (val, index, array) {
21+
switch( val ) {
22+
case "-s":
23+
case "--speed":
24+
speed = array[index+1];
25+
break;
26+
case "-v":
27+
case "--verbose":
28+
verbose = true;
29+
break;
30+
default:
31+
}
32+
});
33+
34+
// Validate arguments
35+
if( !filename ) {
36+
console.log( "Usage node resolver.js [path2file] [--speed|-s] speed [--verbose|-v] ");
37+
return;
38+
}
39+
40+
// Create a read stream
41+
var readStream = fs.createReadStream( filename );
42+
43+
// This will wait until we know the readable stream is actually valid before piping
44+
readStream.on('open', function () {
45+
// Pass the readStream to the resolver and let the "magic" happen!
46+
Resolver.resolve( readStream, speed, verbose, function( resolution ) {
47+
for( p of resolution.resolutePlans ) {
48+
console.log(p.toBonString().trim());
49+
}
50+
});
51+
});
52+
53+
// This catches any errors that happen while creating the readable stream (usually invalid names)
54+
readStream.on('error', function(err) {
55+
console.error(err);
56+
});
57+
58+
/*-----------------------VisplotFunction-----------------------------*/
59+

‎test/resolve-spec.js

+4-4
Original file line numberDiff line numberDiff line change
@@ -11,21 +11,21 @@ describe("Resolve", function() {
1111
beforeEach( function() {
1212
// Specify before logic here
1313
//file = fs.createReadStream("test/sample_input/sample_6n.movements");
14-
file = fs.createReadStream("test/sample_input/sample_10n.movements");
15-
//file = fs.createReadStream("test/sample_input/sample_2ntiny.movements");
14+
//file = fs.createReadStream("test/sample_input/sample_10n.movements");
15+
file = fs.createReadStream("test/sample_input/sample_2ntiny.movements");
1616
//file = fs.createReadStream("test/sample_input/sample_2n_2.movements");
1717
//file = fs.createReadStream("test/sample_input/simulation2015121510.movements");
1818
});
1919

2020
it("should create valid resolution ", function(done) {
2121
this.timeout(500000);
22-
Resolver.resolve( file, 3, function( resolution ) {
22+
Resolver.resolve( file, 3, false, function( resolution ) {
2323
//console.log(resolution);
2424
//expect(resolution.originalPlans.length).to.equal(2);
2525
//expect(resolution.resolutePlans.length).to.equal(2);
2626
//expect(resolution.nodes.length).to.equal(2);
2727
for( p of resolution.resolutePlans ) {
28-
console.log("\t", p.toBonString() );
28+
console.log(p.toBonString().trim());
2929
}
3030
done();
3131
});

0 commit comments

Comments
 (0)
Please sign in to comment.