From 1c072afdb1866ba397ee8e6155f5f68c6c7ab4da Mon Sep 17 00:00:00 2001 From: Mikko Rasa Date: Wed, 29 May 2013 20:48:38 +0300 Subject: [PATCH] New routing system for trains The earlier code routed each train separately and tried to deal with conflicts as they arose. The new system instead considers the entire layout and simultaneously finds routes for all trains. It is able to have trains wait for each other, and in the future many new tricks can be added. --- source/engineer/trainpanel.cpp | 4 +- source/libr2c2/timetable.cpp | 13 +- source/libr2c2/timetable.h | 2 +- source/libr2c2/train.cpp | 2 + source/libr2c2/trainrouteplanner.cpp | 366 +++++++++++++++++++++++++++ source/libr2c2/trainrouteplanner.h | 106 ++++++++ source/libr2c2/trainrouter.cpp | 195 ++++++-------- source/libr2c2/trainrouter.h | 26 +- source/libr2c2/zone.cpp | 5 + 9 files changed, 586 insertions(+), 133 deletions(-) create mode 100644 source/libr2c2/trainrouteplanner.cpp create mode 100644 source/libr2c2/trainrouteplanner.h diff --git a/source/engineer/trainpanel.cpp b/source/engineer/trainpanel.cpp index d6066cb..1076fed 100644 --- a/source/engineer/trainpanel.cpp +++ b/source/engineer/trainpanel.cpp @@ -279,6 +279,6 @@ void TrainPanel::go_to(Track *track, unsigned) pick_conn.disconnect(); TrainRouter *router = train.get_ai_of_type(); - if(!router || !router->go_to(*track)) - engineer.set_status("Could not set route"); + if(router) + router->set_destination(track->get_block()); } diff --git a/source/libr2c2/timetable.cpp b/source/libr2c2/timetable.cpp index 753aae8..44e859e 100644 --- a/source/libr2c2/timetable.cpp +++ b/source/libr2c2/timetable.cpp @@ -88,15 +88,15 @@ void Timetable::tick(const Time::TimeStamp &t, const Time::TimeDelta &) { case GOTO_SENSOR: arrived = false; - train.ai_message(Message("go-to-track", &get_sensor(row.get_param(0)))); + train.ai_message(Message("set-destination-block", &get_sensor(row.get_param(0)))); break; case GOTO_ZONE: arrived = false; - train.ai_message(Message("go-to-zone", &get_zone(row.get_param(0)))); + train.ai_message(Message("set-destination-zone", &get_zone(row.get_param(0)))); break; case TRAVEL_TO: { - Block *block = &get_sensor(row.get_param(0)).get_block(); + Block *block = &get_sensor(row.get_param(0)); if(block->get_train()!=&train || block->get_state()(0)); - Block *block = &get_sensor(row.get_param(1)).get_block(); + Block *block = &get_sensor(row.get_param(1)); if(block->get_train()!=other_train || block->get_state() &st) const st.push_back(i->save()); } -Track &Timetable::get_sensor(unsigned id) +Block &Timetable::get_sensor(unsigned id) { - Block &block = train.get_layout().get_block(id|0x1000); - return **block.get_tracks().begin(); + return train.get_layout().get_block(id|0x1000); } Track &Timetable::get_turnout(unsigned id) diff --git a/source/libr2c2/timetable.h b/source/libr2c2/timetable.h index 92a4d65..84a39cd 100644 --- a/source/libr2c2/timetable.h +++ b/source/libr2c2/timetable.h @@ -97,7 +97,7 @@ public: void tick(const Msp::Time::TimeStamp &, const Msp::Time::TimeDelta &); void save(std::list &) const; private: - Track &get_sensor(unsigned); + Block &get_sensor(unsigned); Track &get_turnout(unsigned); Zone &get_zone(const std::string &); void block_state_changed(Block &, Block::State); diff --git a/source/libr2c2/train.cpp b/source/libr2c2/train.cpp index 1022722..7de6236 100644 --- a/source/libr2c2/train.cpp +++ b/source/libr2c2/train.cpp @@ -258,6 +258,8 @@ void Train::unplace() void Train::stop_at(Block *block) { stop_at_block = block; + if(active && !stop_at_block) + reserve_more(); } bool Train::free_block(Block &block) diff --git a/source/libr2c2/trainrouteplanner.cpp b/source/libr2c2/trainrouteplanner.cpp new file mode 100644 index 0000000..ce38c4e --- /dev/null +++ b/source/libr2c2/trainrouteplanner.cpp @@ -0,0 +1,366 @@ +#include "layout.h" +#include "route.h" +#include "train.h" +#include "trainrouteplanner.h" +#include "trainrouter.h" +#include "vehicle.h" + +using namespace std; +using namespace Msp; + +namespace R2C2 { + +TrainRoutePlanner::TrainRoutePlanner(Layout &layout) +{ + const map &trains = layout.get_trains(); + for(map::const_iterator i=trains.begin(); i!=trains.end(); ++i) + { + TrainRoutingInfo info(*i->second); + if(info.router && info.router->has_destination()) + routed_trains.push_back(info); + } + + steps.push_back(RoutingStep()); + RoutingStep &start = steps.back(); + for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) + start.trains.push_back(TrainRoutingState(*i)); +} + +void TrainRoutePlanner::plan() +{ + RoutingStep *goal = 0; + for(list::iterator i=steps.begin(); i!=steps.end(); ++i) + { + if(i->is_goal()) + { + goal = &*i; + break; + } + + if(update_states(*i)) + { + int next_train = find_next_train(*i); + if(next_train>=0) + add_steps(*i, next_train); + } + } + + if(goal) + create_routes(*goal); +} + +bool TrainRoutePlanner::update_states(RoutingStep &step) +{ + RoutingStep next(&step); + bool changes = false; + for(vector::iterator i=next.trains.begin(); i!=next.trains.end(); ++i) + { + TrainState old_state = i->state; + if(i->state==BLOCKED) + i->state = MOVING; + + TrackIter next_track = i->track.next(i->path); + if(!next_track) + return false; + + for(vector::iterator j=next.trains.begin(); j!=next.trains.end(); ++j) + if(j!=i) + { + if(j->track.track()==next_track.track()) + { + unsigned other_exit = j->track.reverse(j->path).entry(); + if(next_track.entry()==other_exit) + return false; + } + else if(!j->is_occupied(*next_track)) + continue; + + i->state = BLOCKED; + } + + if(i->state!=old_state) + changes = true; + } + + if(changes) + { + list::iterator i; + for(i=steps.begin(); (i!=steps.end() && !(next<*i)); ++i) ; + steps.insert(i, next); + } + + return !changes; +} + +int TrainRoutePlanner::find_next_train(RoutingStep &step) +{ + Time::TimeDelta min_dt; + int next_train = -1; + for(unsigned i=0; i new_steps; + + RoutingStep next(&step); + next.advance(dt); + TrainRouter &router = *train.info->router; + if(router.is_destination(*train.track) && !router.is_destination(*next_track)) + { + next.trains[train_index].state = ARRIVED; + new_steps.push_back(next); + } + else + { + next.trains[train_index].advance_track(0); + + const TrackType::Endpoint &next_entry_ep = next_track.endpoint(); + for(unsigned i=0; next_entry_ep.paths>>i; ++i) + if(next_entry_ep.has_path(i)) + { + next.trains[train_index].path = i; + new_steps.push_back(next); + } + + if(next_entry_ep.paths!=next_track->get_type().get_paths()) + { + RoutingStep wait(&step); + wait.advance(dt); + wait.trains[train_index].state = WAITING; + new_steps.push_back(wait); + } + } + + new_steps.sort(); + steps.merge(new_steps); +} + +void TrainRoutePlanner::create_routes(RoutingStep &goal) +{ + for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) + { + i->route = new Route(i->train->get_layout()); + i->route->set_name("Router"); + } + + for(RoutingStep *i=&goal; i; i=i->prev) + { + for(vector::iterator j=i->trains.begin(); j!=i->trains.end(); ++j) + { + if(j->state==WAITING || j->state==BLOCKED) + j->info->waits.push_front(&*j); + j->info->route->add_track(*j->track); + } + } + + for(vector::iterator i=routed_trains.begin(); i!=routed_trains.end(); ++i) + { + i->router->set_route(i->route); + TrainRoutingState *current_wait = 0; + for(list::iterator j=i->waits.begin(); j!=i->waits.end(); ++j) + if(!current_wait || (*j)->track.track()!=current_wait->track.track()) + { + Block &block = (*j)->track.next()->get_block(); + i->router->add_wait(block, 0); + current_wait = *j; + } + } +} + + +TrainRoutePlanner::TrainRoutingInfo::TrainRoutingInfo(Train &t): + train(&t), + router(train->get_ai_of_type()), + route(0) +{ } + + +TrainRoutePlanner::OccupiedTrack::OccupiedTrack(Track &t, unsigned p, OccupiedTrack *n): + track(&t), + path_length(track->get_type().get_path_length(p)), + next(n), + n_tracks(next ? next->n_tracks+1 : 1), + refcount(1) +{ + if(next) + ++next->refcount; +} + +TrainRoutePlanner::OccupiedTrack::OccupiedTrack(const OccupiedTrack &other): + track(other.track), + path_length(other.path_length), + next(other.next), + n_tracks(other.n_tracks), + refcount(1) +{ + if(next) + ++next->refcount; +} + +TrainRoutePlanner::OccupiedTrack::~OccupiedTrack() +{ + if(next && !--next->refcount) + delete next; +} + + +TrainRoutePlanner::TrainRoutingState::TrainRoutingState(TrainRoutingInfo &inf): + info(&inf), + occupied_tracks(0), + state(MOVING) +{ + const Vehicle *veh = &info->train->get_vehicle(0); + track = TrackIter(veh->get_track(), veh->get_entry()); + // TODO margins + offset = veh->get_offset()+veh->get_type().get_length()/2; + path = track->get_active_path(); + + float path_length = track->get_type().get_path_length(path); + while(offset>path_length) + { + offset -= path_length; + track = track.next(); + path = track->get_active_path(); + path_length = track->get_type().get_path_length(path); + } + + while(Vehicle *next = veh->get_link(1)) + veh = next; + back_offset = veh->get_offset()-veh->get_type().get_length()/2; + + TrackIter iter(veh->get_track(), veh->get_entry()); + while(back_offset<0) + { + TrackIter prev = iter.flip().reverse(); + if(!prev) + break; + iter = prev; + back_offset += iter->get_type().get_path_length(iter->get_active_path()); + } + + while(1) + { + occupied_tracks = new OccupiedTrack(*iter, iter->get_active_path(), occupied_tracks); + if(iter.track()==track.track()) + break; + iter = iter.next(); + } +} + +TrainRoutePlanner::TrainRoutingState::TrainRoutingState(const TrainRoutingState &other): + info(other.info), + track(other.track), + path(other.path), + occupied_tracks(other.occupied_tracks), + offset(other.offset), + back_offset(other.back_offset), + state(other.state) +{ + ++occupied_tracks->refcount; +} + +TrainRoutePlanner::TrainRoutingState::~TrainRoutingState() +{ + if(!--occupied_tracks->refcount) + delete occupied_tracks; +} + +Time::TimeDelta TrainRoutePlanner::TrainRoutingState::get_time_to_next_track() const +{ + // TODO Consider the speed of the train + return (track->get_type().get_path_length(path)-offset)*Time::sec; +} + +bool TrainRoutePlanner::TrainRoutingState::is_occupied(Track &trk) const +{ + OccupiedTrack *occ = occupied_tracks; + for(unsigned n=occ->n_tracks; n>0; --n, occ=occ->next) + if(occ->track==&trk) + return true; + return false; +} + +void TrainRoutePlanner::TrainRoutingState::advance(float distance) +{ + offset += distance; + back_offset += distance; + + OccupiedTrack *last_occ = occupied_tracks; + for(unsigned n=occupied_tracks->n_tracks; n>1; --n) + last_occ = last_occ->next; + + // XXX What if there's multiple tracks to remove? + if(back_offset>last_occ->path_length) + { + back_offset -= last_occ->path_length; + if(occupied_tracks->refcount>1) + { + --occupied_tracks->refcount; + occupied_tracks = new OccupiedTrack(*occupied_tracks); + } + --occupied_tracks->n_tracks; + } +} + +void TrainRoutePlanner::TrainRoutingState::advance_track(unsigned next_path) +{ + float distance = occupied_tracks->path_length-offset; + track = track.next(path); + path = next_path; + occupied_tracks = new OccupiedTrack(*track, path, occupied_tracks); + advance(distance); + offset = 0; +} + + +TrainRoutePlanner::RoutingStep::RoutingStep(): + prev(0) +{ } + +TrainRoutePlanner::RoutingStep::RoutingStep(RoutingStep *p): + time(p->time), + trains(p->trains), + prev(p) +{ } + +void TrainRoutePlanner::RoutingStep::advance(const Time::TimeDelta &dt) +{ + time += dt; + for(vector::iterator i=trains.begin(); i!=trains.end(); ++i) + if(i->state==MOVING) + { + float distance = dt/Time::sec; + i->advance(distance); + } +} + +bool TrainRoutePlanner::RoutingStep::is_goal() const +{ + for(vector::const_iterator i=trains.begin(); i!=trains.end(); ++i) + if(i->state!=ARRIVED) + return false; + return true; +} + +bool TrainRoutePlanner::RoutingStep::operator<(const RoutingStep &other) const +{ + return time +#include +#include +#include "trackiter.h" + +namespace R2C2 { + +class Layout; +class Route; +class Track; +class Train; +class TrainRouter; + +class TrainRoutePlanner +{ +private: + struct TrainRoutingState; + + struct TrainRoutingInfo + { + Train *train; + TrainRouter *router; + Route *route; + std::list waits; + + TrainRoutingInfo(Train &); + }; + + struct OccupiedTrack + { + Track *track; + float path_length; + OccupiedTrack *next; + unsigned n_tracks; + unsigned refcount; + + OccupiedTrack(Track &, unsigned, OccupiedTrack *); + OccupiedTrack(const OccupiedTrack &); + ~OccupiedTrack(); + }; + + enum TrainState + { + MOVING, + WAITING, + BLOCKED, + ARRIVED + }; + + struct TrainRoutingState + { + TrainRoutingInfo *info; + TrackIter track; + unsigned path; + OccupiedTrack *occupied_tracks; + float offset; + float back_offset; + TrainState state; + + TrainRoutingState(TrainRoutingInfo &); + TrainRoutingState(const TrainRoutingState &); + ~TrainRoutingState(); + + Msp::Time::TimeDelta get_time_to_next_track() const; + bool is_occupied(Track &) const; + void advance(float); + void advance_track(unsigned); + }; + + struct RoutingStep + { + Msp::Time::TimeDelta time; + std::vector trains; + RoutingStep *prev; + + RoutingStep(); + RoutingStep(RoutingStep *); + + void advance(const Msp::Time::TimeDelta &); + bool is_goal() const; + + bool operator<(const RoutingStep &) const; + }; + + std::vector routed_trains; + std::list steps; + +public: + TrainRoutePlanner(Layout &); + + void plan(); +private: + bool update_states(RoutingStep &); + int find_next_train(RoutingStep &); + void add_steps(RoutingStep &, unsigned); + void add_waiting_step(RoutingStep &, unsigned); + void add_steps(RoutingStep &, TrainRoutingState &train); + void create_routes(RoutingStep &); +}; + +} // namespace R2C2 + +#endif diff --git a/source/libr2c2/trainrouter.cpp b/source/libr2c2/trainrouter.cpp index 6e588e7..77d6f71 100644 --- a/source/libr2c2/trainrouter.cpp +++ b/source/libr2c2/trainrouter.cpp @@ -2,6 +2,7 @@ #include "route.h" #include "trackiter.h" #include "train.h" +#include "trainrouteplanner.h" #include "trainrouter.h" #include "zone.h" @@ -14,7 +15,9 @@ TrainRouter::TrainRouter(Train &t): TrainAI(t), priority(0), arriving(false), - yielding_to(0) + dest_zone(0), + dest_block(0), + update_pending(false) { train.get_layout().signal_block_reserved.connect(sigc::mem_fun(this, &TrainRouter::block_reserved)); train.signal_advanced.connect(sigc::mem_fun(this, &TrainRouter::train_advanced)); @@ -25,11 +28,6 @@ void TrainRouter::set_priority(int p) priority = p; } -void TrainRouter::yield_to(const Train &t) -{ - yielding_to = &t; -} - bool TrainRouter::set_route(const Route *r) { train.free_noncritical_blocks(); @@ -67,65 +65,48 @@ bool TrainRouter::set_route(const Route *r) return true; } -bool TrainRouter::go_to(Track &to) +void TrainRouter::add_wait(Block &block, Train *tr) { - if(!train.get_speed()) - { - for(BlockIter i=train.get_tail_block(); (i && i->get_train()==&train); i=i.next()) - if(i->has_track(to)) - { - signal_arrived.emit(); - signal_event.emit(Message("arrived")); - return set_route(0); - } - } - - train.free_noncritical_blocks(); - - TrackIter next = train.get_head_block().next().track_iter(); - - Route *route = Route::find(next, to); - if(!route) - return false; - create_lead_route(route, route); - return set_route(route); + Wait wait; + wait.block = █ + wait.train = tr; + waits.push_back(wait); } -bool TrainRouter::go_to(const Zone &to) +const Route *TrainRouter::get_route() const { - set tracks; - for(BlockIter i=train.get_tail_block(); (i && i->get_train()==&train); i=i.next()) - tracks.insert(i->get_tracks().begin(), i->get_tracks().end()); - - const Zone::TrackSet &ztracks = to.get_tracks(); - unsigned union_size = 0; - for(Zone::TrackSet::const_iterator i=ztracks.begin(); i!=ztracks.end(); ++i) - union_size += tracks.count(*i); - - if(union_size==tracks.size() || union_size==ztracks.size()) - { - signal_arrived.emit(); - signal_event.emit(Message("arrived")); - return set_route(0); - } + if(routes.empty()) + return 0; + return routes.front(); +} - train.free_noncritical_blocks(); +void TrainRouter::set_destination(const Zone &zone) +{ + dest_zone = &zone; + dest_block = 0; + update_pending = true; +} - TrackIter next = train.get_head_block().next().track_iter(); +void TrainRouter::set_destination(const Block &block) +{ + dest_zone = 0; + dest_block = █ + update_pending = true; +} - Route *route = Route::find(next, to); - if(!route) - return false; - create_lead_route(route, route); - route->add_tracks(ztracks); - return set_route(route); +bool TrainRouter::has_destination() const +{ + return dest_zone || dest_block; } -const Route *TrainRouter::get_route() const +bool TrainRouter::is_destination(Track &track) const { - if(routes.empty()) - return 0; - return routes.front(); + if(dest_zone) + return dest_zone->has_track(track); + else if(dest_block) + return dest_block->has_track(track); + else + return false; } void TrainRouter::message(const Message &msg) @@ -139,19 +120,27 @@ void TrainRouter::message(const Message &msg) } else if(msg.type=="clear-route") set_route(0); - else if(msg.type=="go-to-track") - go_to(*msg.value.value()); - else if(msg.type=="go-to-zone") + else if(msg.type=="set-destination-block") + { + if(msg.value.check_type()) + set_destination(*msg.value.value()); + else + set_destination(*msg.value.value()); + } + else if(msg.type=="set-destination-zone") { if(msg.value.check_type()) - go_to(*msg.value.value()); + set_destination(*msg.value.value()); else - go_to(*msg.value.value()); + set_destination(*msg.value.value()); } } void TrainRouter::tick(const Time::TimeStamp &, const Time::TimeDelta &) { + if(update_pending) + create_plans(train.get_layout()); + if(arriving && !train.get_speed()) { train.set_active(false); @@ -177,12 +166,16 @@ void TrainRouter::save(list &st) const void TrainRouter::block_reserved(Block &block, Train *t) { if(t!=&train) + { + if(!waits.empty() && waits.front().block==&block) + { + train.stop_at(0); + waits.pop_front(); + } return; - - yielding_to = 0; + } BlockIter b_iter(&block, t->get_entry_to_block(block)); - BlockIter b_iter_next; RouteList::iterator route = routes.begin(); if(advance_route(route, block)) @@ -196,7 +189,7 @@ void TrainRouter::block_reserved(Block &block, Train *t) } // Check if the next block is still part of the designated route - b_iter_next = b_iter.next(*route); + BlockIter b_iter_next = b_iter.next(*route); RouteList::iterator next_route = route; if(!advance_route(next_route, *b_iter_next)) @@ -205,61 +198,8 @@ void TrainRouter::block_reserved(Block &block, Train *t) return; } - } - else - b_iter_next = b_iter.next(); - - // Check if there's another train and ask it to free the block if appropriate - if(b_iter_next) - { - if(Train *other_train = b_iter_next->get_train()) - { - /* There's another train ahead of us. If it wants to exit the block - from the same endpoint we're trying to enter from or the other way - around, treat it as coming towards us. Otherwise treat it as going - in the same direction. */ - int other_entry = other_train->get_entry_to_block(*b_iter_next); - if(other_entry<0) - throw logic_error("block reservation inconsistency"); - - unsigned exit = b_iter_next.reverse().entry(); - unsigned other_exit = BlockIter(b_iter_next.block(), other_entry).reverse().entry(); - bool entry_conflict = (b_iter_next.entry()==other_exit); - bool exit_conflict = (exit==static_cast(other_entry)); - // TODO: speed matching with preceding train - - TrainRouter *other_router = other_train->get_ai_of_type(); - int other_prio = (other_router ? other_router->get_priority() : 0); - - if(!entry_conflict && !exit_conflict && other_priofree_block(*b_iter_next); - } - else if(other_train!=yielding_to && (other_prioget_train()==other_train);) - { - if(!advance_route(j, *i)) - break; - last_contested = i; - i = i.next(*j); - } - - if(last_contested) - { - if(other_train->free_block(*last_contested)) - other_router->yield_to(train); - else - yield_to(*other_train); - } - } - } + if(!waits.empty() && waits.front().block==b_iter_next.block()) + train.stop_at(&block); } } @@ -339,6 +279,23 @@ bool TrainRouter::is_on_route(const Block &block) return advance_route(iter, block); } +void TrainRouter::create_plans(Layout &layout) +{ + TrainRoutePlanner planner(layout); + planner.plan(); + + const map &trains = layout.get_trains(); + for(map::const_iterator i=trains.begin(); i!=trains.end(); ++i) + if(TrainRouter *router = i->second->get_ai_of_type()) + router->update_pending = false; +} + + +TrainRouter::Wait::Wait(): + block(0), + train(0) +{ } + TrainRouter::Loader::Loader(TrainRouter &r): DataFile::ObjectLoader(r) diff --git a/source/libr2c2/trainrouter.h b/source/libr2c2/trainrouter.h index 28fc802..07afe2b 100644 --- a/source/libr2c2/trainrouter.h +++ b/source/libr2c2/trainrouter.h @@ -8,6 +8,7 @@ namespace R2C2 { class Block; +class Layout; class Route; class Track; class Zone; @@ -27,25 +28,40 @@ public: sigc::signal signal_arrived; private: + struct Wait + { + Block *block; + Train *train; + + Wait(); + }; + typedef std::list RouteList; int priority; RouteList routes; bool arriving; - const Train *yielding_to; + const Zone *dest_zone; + const Block *dest_block; + std::list waits; + + bool update_pending; public: TrainRouter(Train &); void set_priority(int); int get_priority() const { return priority; } - void yield_to(const Train &); bool set_route(const Route *); - bool go_to(Track &); - bool go_to(const Zone &); + void add_wait(Block &, Train *); const Route *get_route() const; + void set_destination(const Zone &); + void set_destination(const Block &); + bool has_destination() const; + bool is_destination(Track &) const; + virtual void message(const Message &); virtual void tick(const Msp::Time::TimeStamp &, const Msp::Time::TimeDelta &); @@ -59,6 +75,8 @@ private: Route *create_lead_route(Route *, const Route *); bool advance_route(RouteList::iterator &, const Block &); bool is_on_route(const Block &); + + static void create_plans(Layout &); }; } // namespace R2C2 diff --git a/source/libr2c2/zone.cpp b/source/libr2c2/zone.cpp index eddcfc9..72712a2 100644 --- a/source/libr2c2/zone.cpp +++ b/source/libr2c2/zone.cpp @@ -78,6 +78,11 @@ bool Zone::add_tracks(const TrackSet &trks) } } +bool Zone::has_track(Track &t) const +{ + return tracks.count(&t); +} + void Zone::save(list &st) const { st.push_back((DataFile::Statement("group"), group)); -- 2.45.2