/* -*- Mode: C -*-* */ /********************************************************************\ * File: track.hs * * Date: 12/02/1997 * * Author: Cem Unsal * * Robotics Institute, Carnegie-Mellon University * * unsal@ri.cmu.edu * * * * Description: * * Simple racetrack with left and right turns * * Also requires vrep.hs, highway.hs, weather.hs, and ufo.hs * * Description of the highway is at: * * http://www.cs.cmu.edu/~unsal/research/track.html * * * ********************************************************************** * This file is distributed under the following conditions: * * * * 1. The recipient shall refrain from disclosing the software, * * in any form, to third parties without prior written * * authorization from Carnegie-Mellon University. The * * recipient shall have the right to use and copy the * * software on, or in connection with the operation of, any * * computer system owned or operated by it. In addition, * * the recipient shall have the right to modify or merge * * the software to form updated works. * * * * 2. If the recipient receives a request from any third party * * to furnish all or a portion of the software to any third * * party, it will refer such a request to Carnegie-Mellon * * University. * * * * 3. Carnegie-Mellon University shall not be held liable for any * * damages resulting from the use or misuse of the software * * provided by it. Furthermore, Carnegie-Mellon University * * remains without obligation to assist in its installation * * or maintenance. * * * * 4. The recipient agrees to acknowledge Carnegie-Mellon * * University in appropriate citations appearing in public * * literature when reference is made to the software provided * * above. * * * * 5. If the recipient develops any enhancements to the software * * which materially improves its operation, the recipient * * agrees to make such enhancements available to Carnegie- * * Mellon University without charge, provided Carnegie- * * Mellon University agrees in writing to receive such * * enhancements in confidence, if requested to do so. * * * * 6. This header comment must remain attached to the source * * code of the provided software. * * * * Bug reports and suggestions can be mailed to Cem Unsal by * * electronic mail addressed to: "unsal@ri.cmu.edu". As mentioned * * in condition 3 above, the author is not obligated to fix any * * such bugs, or even to acknowledge receipt of the bug report. * * * \********************************************************************/ #include "highway.hs" #include "vrep_noexit.hs" // vrep.hs with transition "all -> exit" removed #include "ufo.hs" #include "weather.hs" #include "ext-functions.hs" #define lw 4 // lane width #define sl 150 // segment length #define pi 3.1415927 #define initiallanedev 0 global Lane lnLL1, lnLL2, lnLL3, lnRL1, lnRL2, lnRL3, lnRU1, lnRU2, lnRU3, lnLU1, lnLU2, lnLU3; global Segment segLL1, segLL2, segLL3, segLL4, segRL1, segRU1, segRU2, segRU3, segRU4, segLU1, segLU2, segLU3; global Section secLL, secRL, secRU, secLU; global HighwayBuilder hwy := create(HighwayBuilder); global UfoSource source1 := create(UfoSource, gxp := 2.5*lw-initiallanedev, gyp := sl, gzp :=0, section := secLU, lane := lnLU1, segment := segLU3, rxp := sl+(sl-3*lw)*pi/2, ryp := 0.5*lw+initiallanedev, rzp := 0, lyp := initiallanedev, vgam11 := 0, vgam12 := 1, vgam13 := 0, vgam21 := 1, vgam22 := 0, // Note alignment of y axis!!! vgam23 := 0, vgam31 := 0, vgam32 := 0, vgam33 := 1, vehiclesToCreate := 1, duration := 0, period := .2); global UfoSource source2 := create(UfoSource, gxp := 1.5*lw+initiallanedev, gyp := sl, gzp :=0, section := secLU, lane := lnLU2, segment := segLU3, rxp := sl+(sl-3*lw)*pi/2, ryp := 1.5*lw+initiallanedev, rzp := 0, lyp := initiallanedev, vgam11 := 0, vgam12 := 1, vgam13 := 0, vgam21 := 1, vgam22 := 0, // Note alignment of y axis!!! vgam23 := 0, vgam31 := 0, vgam32 := 0, vgam33 := 1, vehiclesToCreate := 1, duration := 0, period := .5); global UfoSource source3 := create(UfoSource, gxp := 0.5*lw+initiallanedev, gyp := sl, gzp :=0, section := secLU, lane := lnLU3, segment := segLU3, rxp := sl+(sl-3*lw)*pi/2, ryp := 2.5*lw+initiallanedev, rzp := 0, lyp := initiallanedev, vgam11 := 0, vgam12 := 1, vgam13 := 0, vgam21 := 1, vgam22 := 0, // Note alignment of y axis!!! vgam23 := 0, vgam31 := 0, vgam32 := 0, vgam33 := 1, vehiclesToCreate := 1, duration := 0, period := 1); type UfoSource { output number gxp; // vehicle's global x position number gyp; // vehicle's global y position number gzp; // vehicle's global z position Section section; // vehicle's section Segment segment; // vehicle's segment Lane lane; // vehicle's lane number rxp; // vehicle's road x position number ryp; // vehicle's road y position number rzp; // vehicle's road z position number lyp; // vehicle's lane y position number vgam11; // vehicle to global alignment matrix number vgam12; number vgam13; number vgam21; number vgam22; number vgam23; number vgam31; number vgam32; number vgam33; state number period; continuous number duration; number vehiclesToCreate; AutomatedVehicle new_av; export newVehicle; discrete start {duration' = 1;}; /* ' */ transition start -> start {newVehicle} when duration >= period and vehiclesToCreate > 0 do { duration := 0; vehiclesToCreate := vehiclesToCreate - 1; new_av := create(AutomatedVehicle, source := self); }; } type HighwayBuilder { setup define { /* segments */ /* LL */ Segment tsegLL1 := create(Segment, length := sl, xOffset := 0, gxa := 3*lw, gya := 2*sl, gza := 0, orientation := pi/2, curvature := 0, grade := 0, banking := 0, skid := 0); Segment tsegLL2 := create(Segment, length := (sl-3*lw)*pi/2, xOffset := sl, gxa := 3*lw, gya := 3*sl, gza := 0, orientation := pi/2, curvature := -1/(sl-3*lw), grade := 0, banking := 0, skid := 0); Segment tsegLL3 := create(Segment, length := (sl-3*lw)*pi/2, xOffset := sl+(sl-3*lw)*pi/2, gxa := sl, gya := 4*sl-3*lw, gza := 0, orientation := 0, curvature := -1/(sl-3*lw), grade := 0, banking := 0, skid := 0); Segment tsegLL4 := create(Segment, length := 3*lw, xOffset := sl+(sl-3*lw)*pi, gxa := sl+(sl-3*lw), gya := 3*sl, gza := 0, orientation := -pi/2, curvature := 0, grade := 0, banking := 0, skid := 0); /* RL */ Segment tsegRL1 := create(Segment, length := sl*pi/2, xOffset := 0, gxa := sl+(sl-3*lw), gya := 3*sl-3*lw, gza := 0, orientation := -pi/2, curvature := 1/sl, grade := 0, banking := 0, skid := 0); /* RU */ Segment tsegRU1 := create(Segment, length := 3*lw, xOffset := 0, gxa := 2*sl+(sl-3*lw), gya := 2*sl-3*lw, gza := 0, orientation := 0, curvature := 0, grade := 0, banking := 0, skid := 0); Segment tsegRU2 := create(Segment, length := (sl-3*lw)*pi/2, xOffset := 3*lw, gxa := 3*sl, gya := 2*sl-3*lw, gza := 0, orientation := 0, curvature := -1/(sl-3*lw), grade := 0, banking := 0, skid :=0); Segment tsegRU3 := create(Segment, length := (sl-3*lw)*pi/2, xOffset := 3*lw+(sl-3*lw)*pi/2, gxa := 4*sl-3*lw, gya := sl, gza := 0, orientation := -pi/2, curvature := -1/(sl-3*lw), grade := 0, banking := 0, skid := 0); Segment tsegRU4 := create(Segment, length := sl, xOffset := 3*lw+(sl-3*lw)*pi, gxa := 3*sl, gya := 3*lw, gza := 0, orientation := pi, curvature := 0, grade := 0, banking := 0, skid := 0); /* LU */ Segment tsegLU1 := create(Segment, length := sl, xOffset := 0, gxa := 2*sl, gya := 3*lw, gza := 0, orientation := pi, curvature := 0, grade := 0, banking := 0, skid := 0); Segment tsegLU2 := create(Segment, length := (sl-3*lw)*pi/2, xOffset := sl, gxa := sl, gya := 3*lw, gza := 0, orientation := pi, curvature := -1/(sl-3*lw), grade := 0, banking := 0, skid := 0); Segment tsegLU3 := create(Segment, length := sl, xOffset := sl+(sl-3*lw)*pi/2, gxa := 3*lw, gya := sl, gza := 0, orientation := pi/2, curvature := 0, grade := 0, banking := 0, skid := 0); /* lanes */ /* LL */ Lane tlnLL1 := create(Lane, width := lw, yOffset := 0, laneLeft := nil); Lane tlnLL2 := create(Lane, width := lw, yOffset := lw); Lane tlnLL3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil); Lane tlnRL1 := create(Lane, width := lw, yOffset := 0, laneLeft := nil); Lane tlnRL2 := create(Lane, width := lw, yOffset := lw); Lane tlnRL3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil); Lane tlnRU1 := create(Lane, width := lw, yOffset := 0, laneLeft := nil); Lane tlnRU2 := create(Lane, width := lw, yOffset := lw); Lane tlnRU3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil); Lane tlnLU1 := create(Lane, width := lw, yOffset := 0, laneLeft := nil); Lane tlnLU2 := create(Lane, width := lw, yOffset := lw); Lane tlnLU3 := create(Lane, width := lw, yOffset := 2*lw, laneRight := nil); /* sections */ Section tsecLL := create(Section, width := 3*lw, length := sl + (sl-3*lw)*pi + 3*lw, previousYOffsets := [0]); Section tsecRL := create(Section, width := 3*lw, length := sl*pi/2, previousYOffsets := [0]); Section tsecRU := create(Section, width := 3*lw, length := 3*lw + (sl-3*lw)*pi + sl, previousYOffsets := [0]); Section tsecLU := create(Section, width := 3*lw, length := sl + (sl-3*lw)*pi/2 + sl, previousYOffsets := [0]); Weather tweather := create (Weather, visibility := 100, precipitation := 0); } do { /* lanes */ lnLL1 := tlnLL1; lnLL2 := tlnLL2; lnLL3 := tlnLL3; lnRL1 := tlnRL1; lnRL2 := tlnRL2; lnRL3 := tlnRL3; lnRU1 := tlnRU1; lnRU2 := tlnRU2; lnRU3 := tlnRU3; lnLU1 := tlnLU1; lnLU2 := tlnLU2; lnLU3 := tlnLU3; /* sections */ secLL := tsecLL; secRL := tsecRL; secRU := tsecRU; secLU := tsecLU; /* segments */ segLL1 := tsegLL1; segLL2 := tsegLL2; segLL3 := tsegLL3; segLL4 := tsegLL4; segRL1 := tsegRL1; segRU1 := tsegRU1; segRU2 := tsegRU2; segRU3 := tsegRU3; segRU4 := tsegRU4; segLU1 := tsegLU1; segLU2 := tsegLU2; segLU3 := tsegLU3; /* additional definitions - connections */ /* lanes and segment arrays for sections */ lanes(tsecLL) := [tlnLL1, tlnLL2, tlnLL3]; lanes(tsecRL) := [tlnRL1, tlnRL2, tlnRL3]; lanes(tsecRU) := [tlnRU1, tlnRU2, tlnRU3]; lanes(tsecLU) := [tlnLU1, tlnLU2, tlnLU3]; segments(tsecLL) := [tsegLL1, tsegLL2, tsegLL3, tsegLL4]; segments(tsecRL) := [tsegRL1]; segments(tsecRU) := [tsegRU1, tsegRU2, tsegRU3, tsegRU4]; segments(tsecLU) := [tsegLU1, tsegLU2, tsegLU3]; /* section arrays for lanes */ section(tlnLL1) := tsecLL; section(tlnLL2) := tsecLL; section(tlnLL3) := tsecLL; section(tlnRL1) := tsecRL; section(tlnRL2) := tsecRL; section(tlnRL3) := tsecRL; section(tlnRU1) := tsecRU; section(tlnRU2) := tsecRU; section(tlnRU3) := tsecRU; section(tlnLU1) := tsecLU; section(tlnLU2) := tsecLU; section(tlnLU3) := tsecLU; /* up-down lane connections for lanes */ laneDown(tlnLL1) := [tlnRL1]; laneDown(tlnLL2) := [tlnRL2]; laneDown(tlnLL3) := [tlnRL3]; laneDown(tlnRL1) := [tlnRU1]; laneDown(tlnRL2) := [tlnRU2]; laneDown(tlnRL3) := [tlnRU3]; laneDown(tlnRU1) := [tlnLU1]; laneDown(tlnRU2) := [tlnLU2]; laneDown(tlnRU3) := [tlnLU3]; laneDown(tlnLU1) := [tlnLL1]; laneDown(tlnLU2) := [tlnLL2]; laneDown(tlnLU3) := [tlnLL3]; /* */ laneUp(tlnLL1) := [tlnLU1]; laneUp(tlnLL2) := [tlnLU2]; laneUp(tlnLL3) := [tlnLU3]; laneUp(tlnRL1) := [tlnLL1]; laneUp(tlnRL2) := [tlnLL2]; laneUp(tlnRL3) := [tlnLL3]; laneUp(tlnRU1) := [tlnRL1]; laneUp(tlnRU2) := [tlnRL2]; laneUp(tlnRU3) := [tlnRL3]; laneUp(tlnLU1) := [tlnRU1]; laneUp(tlnLU2) := [tlnRU2]; laneUp(tlnLU3) := [tlnRU3]; /* left-right lane connections for lanes */ laneRight(tlnLL1) := tlnLL2; laneRight(tlnLL2) := tlnLL3; laneLeft(tlnLL2) := tlnLL1; laneLeft(tlnLL3) := tlnLL2; laneRight(tlnRL1) := tlnRL2; laneRight(tlnRL2) := tlnRL3; laneLeft(tlnRL2) := tlnRL1; laneLeft(tlnRL3) := tlnRL2; laneRight(tlnRU1) := tlnRU2; laneRight(tlnRU2) := tlnRU3; laneLeft(tlnRU2) := tlnRU1; laneLeft(tlnRU3) := tlnRU2; laneRight(tlnLU1) := tlnLU2; laneRight(tlnLU2) := tlnLU3; laneLeft(tlnLU2) := tlnLU1; laneLeft(tlnLU3) := tlnLU2; /* section definitions for segment */ section(tsegLL1) := tsecLL; section(tsegLL2) := tsecLL; section(tsegLL3) := tsecLL; section(tsegLL4) := tsecLL; section(tsegRL1) := tsecRL; section(tsegRU1) := tsecRU; section(tsegRU2) := tsecRU; section(tsegRU3) := tsecRU; section(tsegRU4) := tsecRU; section(tsegLU1) := tsecLU; section(tsegLU2) := tsecLU; section(tsegLU3) := tsecLU; /* up/down section definitions */ sectionUp(tsecLL) := [tsecLU]; sectionDown(tsecLL) := [tsecRL]; sectionUp(tsecRL) := [tsecLL]; sectionDown(tsecRL) := [tsecRU]; sectionUp(tsecRU) := [tsecRL]; sectionDown(tsecRU) := [tsecLU]; sectionUp(tsecLU) := [tsecRU]; sectionDown(tsecLU) := [tsecLL]; /* up-down segment connections */ upSegment(tsegLL1) := nil; downSegment(tsegLL1) := tsegLL2; upSegment(tsegLL2) := tsegLL1; downSegment(tsegLL2) := tsegLL3; upSegment(tsegLL3) := tsegLL2; downSegment(tsegLL3) := tsegLL4; upSegment(tsegLL4) := tsegLL3; downSegment(tsegLL4) := nil; upSegment(tsegRL1) := nil; downSegment(tsegRL1) := nil; upSegment(tsegRU1) := nil; downSegment(tsegRU1) := tsegRU2; upSegment(tsegRU2) := tsegRU1; downSegment(tsegRU2) := tsegRU3; upSegment(tsegRU3) := tsegRU2; downSegment(tsegRU3) := tsegRU4; upSegment(tsegRU4) := tsegRU3; downSegment(tsegRU4) := nil; upSegment(tsegLU1) := nil; downSegment(tsegLU1) := tsegLU2; upSegment(tsegLU2) := tsegLU1; downSegment(tsegLU2) := tsegLU3; upSegment(tsegLU3) := tsegLU2; downSegment(tsegLU3) := nil; /* weather definitions for segments */ weather(tsegLL1) := tweather; weather(tsegLL2) := tweather; weather(tsegLL3) := tweather; weather(tsegLL4) := tweather; weather(tsegRL1) := tweather; weather(tsegRU1) := tweather; weather(tsegRU2) := tweather; weather(tsegRU3) := tweather; weather(tsegRU4) := tweather; weather(tsegLU1) := tweather; weather(tsegLU2) := tweather; weather(tsegLU3) := tweather; }; } /* end of file -- track.hs -- */