[Fawkes Git] branch/common/navgraph-timed: 175 revs updated. (0.5.0-3103-g2c442f7)

Christoph Henke christoph.henke at rwth-aachen.de
Mon May 9 09:46:11 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, common/navgraph-timed has been updated
  discards  e2dceab098e182a056fc9414c49498f6ff0f4b74 (commit)
  discards  3edbb3f9705d8983c0d57f3eff5c9a4b3bac09e1 (commit)
  discards  8016cb1e041cef8a481d8415398c6e034efb2e14 (commit)
  discards  2ab0de9b4201c97bf39db6966a217133773f2ffe (commit)
  discards  eadb5eaf4b712e839dc08ed074310df9370f5051 (commit)
  discards  cf0a0e117485d4bd21fbf623e24e715ee4855108 (commit)
  discards  f6ab0a8877c72983adc90b7a82f6c6db78312499 (commit)
  discards  c4336ce9ea9f550b3b3fcbeea8defbca63c5a163 (commit)
  discards  dadb85c12dd929cb83d6fe4dc62c1b51fa7ae9ba (commit)
  discards  870ae34bf8c532ffbbe3cac6ce9b7a94abf13ae6 (commit)
  discards  a1ef0234303fbca18e488d361cfebca13ca85ad0 (commit)
  discards  8e08b2f912c292361d14640fb188a9e0961329a3 (commit)
  discards  76742a10ec0c5874d629802e78c81b54921be23b (commit)
  discards  83a201b94cbbff0eed742a512d99cd7214ffa5cc (commit)
  discards  81aaec386565f0f34e598e5176425e95963a1966 (commit)
  discards  a8de941561901ad36d299a709e94217b88135bac (commit)
  discards  523d7fba408ba9d80faa221b98a24b557132a796 (commit)
  discards  76b50afd002691c84543ad6cfb3233d4f87875d2 (commit)
  discards  81d08a7534053662b2756cd43f29f4573de07d97 (commit)
  discards  39a0c1e628271e71ee416deea52f3080197ee0f1 (commit)
  discards  4f6fddb692643250589de85014ac05b7d89e987a (commit)
  discards  1b865c2a1c6b4ca6bdc41121ed40402bb90594a5 (commit)
        to  2c442f726a8c336835865eb87b24ce70234dff21 (commit)
       via  607fc46142431e68d8ef0e175b32d0439a86a525 (commit)
       via  ba83ccb23051e83ed24068c89af9efca6dabbf5b (commit)
       via  776812eb95c2200b435c7e208929e5f7d1ff6c6c (commit)
       via  b42453ca2a5255e18fb82e1056f300f1747dbba7 (commit)
       via  e36233f9f611bdec8031b18e2721c6003b8d0c94 (commit)
       via  ff03885e6671b92c24b1e9337ca5af912e588cff (commit)
       via  8b1e8874e608f471914f59e4ba912a65738b8797 (commit)
       via  4147781f1acc2c514c52042b20ed3bcd32b71270 (commit)
       via  d85bc00d7970921c7f851e2e777326189bb23c3c (commit)
       via  27625709c8d82994c4c8c181ea05b8a0fa291b9d (commit)
       via  04dcc91b0811e907768220682d271ec02f722f53 (commit)
       via  d230248eff4b3585060b428b2ea21fdb573dae3d (commit)
       via  5827ef5b37c7998e02c63124aec0fdbbc713769d (commit)
       via  2de806eb3498dd0698dc46eef6e7bd64d887fe9f (commit)
       via  a32eba8fa0ed9b8c7252b86afa2e6dc5694370ed (commit)
       via  71e4b2f2a9ed0b7b7e0f9296077c825d883fe9d1 (commit)
       via  d05d9b7eec225f7405802bb5f98e85b7f85c0863 (commit)
       via  180f478d0a8a4f704adc90366f4f8ed487e042ba (commit)
       via  5240d1d369ce0f59e02fa73d12070802c9e72cb7 (commit)
       via  7adddf9bc5236902ef6bbd6ce0dd3c9d4f2bae9e (commit)
       via  3f25efa5a5295fd4872e8b175b6899ea9adbc7e0 (commit)
       via  14a33397eaa1f78f29677ca0921fead1368e6bd4 (commit)
       via  a95c0e28a3ee76a4e3b946199fe14b74a1494e5f (commit)
       via  b90747e6065b00e08cf69dacac71dd508fc3cd28 (commit)
       via  6eb2b9a49c1eac2460284e3ceef0db22f67b9a0e (commit)
       via  89af27634082f8c434b3a814d93be68ad2d85677 (commit)
       via  0627fb6ba1d5e3808f09c81e0634ff7b5d013e69 (commit)
       via  95a6dd89b3b882db7cb90e98994b9362dd2fc54e (commit)
       via  72e4b768ac7a642bc774bc6ac4378e1295e94d94 (commit)
       via  88f267cec2575ddf18f909ded253f73c92637211 (commit)
       via  1660419ecad2614fb0aec5973ae37dc6c4c331f4 (commit)
       via  dcc9e4f715bf8fc2491f2809e05ff0acf4964c78 (commit)
       via  5d0dffd2bb41eb0cca42b68b02ab240c7a33b662 (commit)
       via  75e1161ff5d400fec46fe4492398f06a2f00a494 (commit)
       via  636b1131e007c39178f253c166c72e973851dbc7 (commit)
       via  9c505e9571b2bfffea5b582f50dd00a7104bf698 (commit)
       via  28a5fce8ba2b4fb8eb37b59be5e23e75f83755ab (commit)
       via  b836efea761380879a25db35638efef96a4c835f (commit)
       via  7ad884aadb71b230426c01907da42fd6c282dac5 (commit)
       via  b858a46ca9d8656b3e17a7103cff9d1b1fe03360 (commit)
       via  eb87fe3cbacda84d00e87082eb5f10786b44f2ba (commit)
       via  101017a51fa90700704c5bffd79ad88fb3172a4e (commit)
       via  5f23b4c8b654a23b24a57bbeaacf27ce605a4684 (commit)
       via  fec5f1070fba43b07b2f3d461a3abdcd99491b2e (commit)
       via  b3e0fa103425325624ad6138c8e74356bb9d2978 (commit)
       via  1a280daf55afdb8190a6226e0720333f1f13109b (commit)
       via  d6b5da2311a1861ac93d3bf6cbe8adc7dca2dce3 (commit)
       via  945b06c4ecc654641cd64540a48f75ce4afbcff9 (commit)
       via  cc731368ad2d382a9aefafd3a645af58639f98a9 (commit)
       via  e84a8631e7807dd1d838cb4290425ce888976d35 (commit)
       via  027bfae50d39e00aadb7d5b2e8d68ae33e3b4486 (commit)
       via  0252049a5b872580ce051ed1ee052d7c18c22636 (commit)
       via  d47081dbf4c90c2d1c679b5fbbd8915f66646aa7 (commit)
       via  32b089a9a142eb2ade7322f796b016abc689f205 (commit)
       via  bf12c756f342a7c4e91a6e69493529382e18042e (commit)
       via  c4cf06cdf3414a69a955b4327f4329efd1616221 (commit)
       via  de2016ee260201352925473f27e23b1aef93c07c (commit)
       via  973cffc83b71096038e57a8d7eb1e553359321b9 (commit)
       via  b0b2e915f5d80b24b35ed829b8c1dfbe6b20c249 (commit)
       via  f17a36d357edf2c040ba4cf1adcdda357cc5f291 (commit)
       via  4d7e27e39d157de45c188396f0bda627b43b697e (commit)
       via  d0eaf1475556061200e77d9ef70abecd7dd09e84 (commit)
       via  980b5d153fdabca7951befd84db5870fe7aee0e0 (commit)
       via  524e0c7ab04c31f88b75bfa0c781f3885e50c90f (commit)
       via  fa2e6e7b646cad0d84fb5aeea183dfc4d39cf049 (commit)
       via  ff87641af595750bae95811e4ff92b099d251d0b (commit)
       via  bfb16c4facdfb13688bb35d1aeecafe4c6207a5a (commit)
       via  9b50e9894a61441c01b850df15a5340ebbd36fe6 (commit)
       via  99cf97b0ba21a835a2c04eec6aa810abb3407f4d (commit)
       via  fe1ed26cff879a7cfc62f28b3912419c44c72061 (commit)
       via  3e4f1eb88ae0ae3df39762f2c373b5c23be5f3ad (commit)
       via  3861b2b2b99f79004a52a6099b544a4189990274 (commit)
       via  8440f28868807b87ff3b01f6d96717f30ee48c47 (commit)
       via  6d76eede970a8b3d108abec5e7252bb00c4fe3ef (commit)
       via  793b676ac236918efa7bdce6143ff74146ee5185 (commit)
       via  0548f14c41be9e5a6ee635288895d1111e9863f5 (commit)
       via  ab4a24d4bd31a04ba270a0d17dcedd9847410b3e (commit)
       via  3ad434a2e8ad279a23442fa28620acc23a2a59d9 (commit)
       via  ccd71f7a666d07c64015a5afc31daf577e8763eb (commit)
       via  d0a2b7f34daccf6e61e0c8cf1585d1b433307e2d (commit)
       via  c9caeaf87dbd9a498ba28bf4a1f34bfb09eba467 (commit)
       via  fa6ed69d5c86af7d5ff50e57cdfef50c296eddb7 (commit)
       via  15a2c6fa03fc97b1b62db4490570457ffda0ef63 (commit)
       via  bbfe1485e20c2f537b9d6d3c8577fae436481374 (commit)
       via  8f8c0b29a753595921226e8797dfbc60fd23c951 (commit)
       via  3ab02b187e1c33f2da297a1ba990e329d675328f (commit)
       via  85bba076eebeea8bd2d2b95d4c099a2a9ec68960 (commit)
       via  2b8a4542d1b19e513874fe347b7604eb5ce9163d (commit)
       via  fd65940dfe6e70a3cfa8b8db872a94310b5a0e36 (commit)
       via  01000b84b2ed3d3fd0a22e887701e9eb44d80ef5 (commit)
       via  45f2190d578cedefed154f56b97c4040891eee05 (commit)
       via  94d8f045d3d5df09b9f52a3459f671f7f661cf88 (commit)
       via  153994eb36a9c783f3f830e85ac18f2b45786efc (commit)
       via  aa34182e37c6f7ec151414ce2fdcc92190bd319d (commit)
       via  d079f6a6e51436860cfce9687862559e414f50fd (commit)
       via  ee1b544ac631ba4778c33c7511422ab0d0dfc0b6 (commit)
       via  f8174bf8e8f7f841c96bd30d154e806bafc93db1 (commit)
       via  a41b9405292dc89362acfdf723ac8d969f756e9f (commit)
       via  4b517197ce742490b550731f26e8fbaba30f494b (commit)
       via  ecd232ca92faf98a67134776bd7175dad6270cfd (commit)
       via  f4f2f821eb66038ac5820bd22f6fe9124366ea89 (commit)
       via  f67bc4a989fc8c9309ec4d2654fcbd0d3931a59b (commit)
       via  b6a43184ad47a80ed39a7df6b08fbcb96c34729b (commit)
       via  2efe170dfea3930e4d9008cff2404200a7b87c89 (commit)
       via  eba06ac9e6042e7685588a0dbe1332e14f52f564 (commit)
       via  0912bdc9744ccb5bba852834906faebecb6ef3e4 (commit)
       via  144884f6756b3adbff0a3333df399a5d458ef25c (commit)
       via  bb45875ddaad34051162834511ea0c7b23bd8992 (commit)
       via  59d32b9ccc828c2492f573ebce7a2d7ae5c0280a (commit)
       via  e97d2473e73af4e1e75d8fc0a7f516847ddbc102 (commit)
       via  4ed92791edcfbc38b0c698ecfd23102f67c3f016 (commit)
       via  9f5977a078604a5d325ecb3a53dcaf87529b2beb (commit)
       via  7fefb2d0a99a1b3b7c219dcf06b08a80275c85e0 (commit)
       via  37805c7bba610c3894eb8fc45a3fb6dc38f54ff7 (commit)
       via  5add554fddc8e14fb0866158929a750b7b3bf5c6 (commit)
       via  4ab90c40868caf406aeb5b500a73b8c51bac4511 (commit)
       via  1d133dfe368ed2db4dd91e80ef508aacf0012ddb (commit)
       via  f4d4ca508675f46f12dc6d0394c19f04f6bb920f (commit)
       via  63fa0c3a28ecc5d650068f40c72d02cfe23c1532 (commit)
       via  70800bb6f4cd6b9c487f5f383e2193bbd0b770b2 (commit)
       via  6b28191f0fc864c07cd987e67064f3e7f4549c47 (commit)
       via  b058a6fc117a99c1da01e5fc461d1cbc8af4cc65 (commit)
       via  3e8958c65d2ffd1c4af2c89b54edf2ec9c61f625 (commit)
       via  2ab4566dfd3fb81d36d399eab1d7d951af0f42db (commit)
       via  36fe5f807bd87008b1dc5982daf0b07b5196c0cc (commit)
       via  f81eb2e2dce08775803f4bac6805225e03830cb7 (commit)
       via  9df204b424347e9eca033c9c685a006d2d4793d7 (commit)
       via  395a30283815304b5f80e975c21a0292fd27432f (commit)
       via  3bc2a57bb7033d2b5de2d85951530f46b775d68b (commit)
       via  fc6f354abe5784bb1abe471828caca91a9d7fd83 (commit)
       via  26d9f036dfb118137c9c731f27b34dcc244ccd73 (commit)
       via  ec22c8fefa8c5f8246b5745db8056273fc4f4197 (commit)
       via  73afbe66fc741e4d3781c0c9174226233bdd9650 (commit)
       via  7ffb5c29aa35d3261b131a11ca602f9d438b0a81 (commit)
       via  3556a06be49b93cbfb3b77a97636ce6d5975e317 (commit)
       via  69a776a92f515b95ff8aa3ee9766b44d7f5dec2d (commit)
       via  57dd007eab226df1d60f5aa7af980454e02849a9 (commit)
       via  7e84b02da221505199cfa413a676169cb1fde738 (commit)
       via  b4e55c4b0f68d38294b6237a79303af51e8f1fa1 (commit)
       via  fab38fdb7f2ab1868d221da4888501d787078a99 (commit)
       via  ef928eb955b6cf5577d7973db0f177fde231a24d (commit)
       via  46ca3c0035335d52077a3ca0f7cdadd70092d598 (commit)
       via  90420b82ccea6170105703bf151fd62833c9f5db (commit)
       via  d0d7ce9e7458553541d7ce87ca6fd3cda90769c4 (commit)
       via  844c187ee920943d589f8cd7ac154a008c8cd5d3 (commit)
       via  6658acd309bbece93431eb14bba702620237b386 (commit)
       via  a2208c70475057b20eec43f5fd107be1d1ad0e6d (commit)
       via  7962b51766f3eefcd89de75152cb76cdecd264ae (commit)
       via  109d3be0306349bc1ce998c530cf1d838c6df0bc (commit)
       via  1817c727bbbcaa1b0f0f0be8a7fd8760fff9704d (commit)
       via  1ecc0ff58b1632e280f0ce8dc039a81e639a49b4 (commit)
       via  b7f33af8e9900fd9ee615b008c17e6f821146f60 (commit)
       via  76189f564b35fb8eb67b8060b0db99bfa7707d9b (commit)
       via  a232b5e5ff72745ae1fcd63f44e428c2848849b5 (commit)
       via  63474c0a585cd2f05af55b0629eaadf2a7c311fc (commit)
       via  adfe75a869a315d8449fe1024b42461537dca837 (commit)
       via  3ffa5d3159c7c4417ff606628fde73ab14f625a7 (commit)
       via  23e4fc446edb44df0770617338919ca3fd70d9a1 (commit)
       via  0da4e95759a996e0a9e7ef194bd2f97940342323 (commit)
       via  908ec4d438f316c93f1259b993b535b2def3487f (commit)
       via  fd9f734057e2fd0126546f5c40d0bc490c927536 (commit)
       via  32a4682fb1c2935fff1dd2706ae99b8e0526fb11 (commit)
       via  0d3227df3d0d3d070aa604d636bdf70d81df296f (commit)
       via  d7c2294fd04bce6bec1e945084baf2e9b7b785b1 (commit)
       via  4626be753b2e0be021ef83329bc1729ac44e206d (commit)
       via  6174f3e7aaa580d0f40d8ef4c0ded5b10a7b1104 (commit)
       via  95e4889740591f47a96124be5a238b8983f3531b (commit)
       via  a2e3dbeb0639974afdd5b609ea15e11f138b13f2 (commit)
       via  03965f294b70cd50a1e0bbb744c0a9e691a85b58 (commit)
       via  7339c2ec18273505d99e87f9f0ad7eb491985199 (commit)
       via  3581ce9d86872e2ea4abf25c579f9e1e9ad57747 (commit)
       via  0af92e540405f109a432c39952fa97926698d883 (commit)
       via  f6ef958ca7af8add8bc7e83aaeda90d93168c1e4 (commit)
       via  6574bbdcad7f066ddd5938416879a88a2871dab3 (commit)

This update added new revisions after undoing existing revisions.  That is
to say, the old revision is not a strict subset of the new revision.  This
situation occurs when you --force push a change and generate a repository
containing something like this:

 * -- * -- B -- O -- O -- O (e2dceab098e182a056fc9414c49498f6ff0f4b74)
            \
             N -- N -- N (2c442f726a8c336835865eb87b24ce70234dff21)

When this happens we assume that you've already had alert emails for all
of the O revisions, and so we here report only the revisions in the N
branch from the common base, B.

http://git.fawkesrobotics.org/fawkes.git/common/navgraph-timed

Those revisions listed above that are new to this repository have
not appeared on any other notification email; so we list those
revisions in full, below.

- *Log* ---------------------------------------------------------------
commit 3f25efa5a5295fd4872e8b175b6899ea9adbc7e0
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Mon Mar 9 22:00:03 2015 +0100
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: initial skeleton for timed navgraph extension plugin
    
    plugin only contains already existent distance based cost and estimate
    functions

http://git.fawkesrobotics.org/fawkes.git/commit/3f25efa
http://trac.fawkesrobotics.org/changeset/3f25efa

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 7adddf9bc5236902ef6bbd6ce0dd3c9d4f2bae9e
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Wed Mar 11 15:09:10 2015 +0100
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: reset cost functions to default when plugin is unloaded

http://git.fawkesrobotics.org/fawkes.git/commit/7adddf9
http://trac.fawkesrobotics.org/changeset/7adddf9

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 5240d1d369ce0f59e02fa73d12070802c9e72cb7
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Wed Mar 11 16:11:12 2015 +0100
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: changed method to unset search functions

http://git.fawkesrobotics.org/fawkes.git/commit/5240d1d
http://trac.fawkesrobotics.org/changeset/5240d1d

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 180f478d0a8a4f704adc90366f4f8ed487e042ba
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Wed Mar 11 15:09:10 2015 +0100
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: reset cost functions to default when plugin is unloaded

http://git.fawkesrobotics.org/fawkes.git/commit/180f478
http://trac.fawkesrobotics.org/changeset/180f478

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit d05d9b7eec225f7405802bb5f98e85b7f85c0863
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Wed Mar 11 16:11:12 2015 +0100
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: changed method to unset search functions

http://git.fawkesrobotics.org/fawkes.git/commit/d05d9b7
http://trac.fawkesrobotics.org/changeset/d05d9b7

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 71e4b2f2a9ed0b7b7e0f9296077c825d883fe9d1
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Mon Jun 8 17:42:28 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: moved plugin to fawkes-robotino repository

http://git.fawkesrobotics.org/fawkes.git/commit/71e4b2f
http://trac.fawkesrobotics.org/changeset/71e4b2f

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit a32eba8fa0ed9b8c7252b86afa2e6dc5694370ed
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Tue Jun 23 18:15:41 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph: added methods for cost estimation based on whole paths

http://git.fawkesrobotics.org/fawkes.git/commit/a32eba8
http://trac.fawkesrobotics.org/changeset/a32eba8

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 2de806eb3498dd0698dc46eef6e7bd64d887fe9f
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Thu Jun 25 11:57:49 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph: corrected comments

http://git.fawkesrobotics.org/fawkes.git/commit/2de806e
http://trac.fawkesrobotics.org/changeset/2de806e

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 5827ef5b37c7998e02c63124aec0fdbbc713769d
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Fri Jul 3 14:32:52 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: moved navpath name generation
    
    moved navpath name generation to the add-methods to perform the name
    genration only on a path change

http://git.fawkesrobotics.org/fawkes.git/commit/5827ef5
http://trac.fawkesrobotics.org/changeset/5827ef5

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit d230248eff4b3585060b428b2ea21fdb573dae3d
Author:     Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Mon Jul 6 16:30:40 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: added call by reference for NavPath in constructors
    
    NavPaths and Nodes are now called by reference in the constructors

http://git.fawkesrobotics.org/fawkes.git/commit/d230248
http://trac.fawkesrobotics.org/changeset/d230248

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 04dcc91b0811e907768220682d271ec02f722f53
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Fri Jul 17 18:30:09 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-constraints: added trajectory-node constraint
    
    trajectory-node-constraints holds a list of TrajectoryNodes
    TrajectoryNodes consist of a fawkes::Time and a NavGraphNode
    fawkes::Time specifies the time estimate at which a dynamic object is on a NavGraphNode

http://git.fawkesrobotics.org/fawkes.git/commit/04dcc91
http://trac.fawkesrobotics.org/changeset/04dcc91

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 27625709c8d82994c4c8c181ea05b8a0fa291b9d
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Mon Jul 20 11:25:57 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: first draft of trajectory-constraint

http://git.fawkesrobotics.org/fawkes.git/commit/2762570
http://trac.fawkesrobotics.org/changeset/2762570

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit d85bc00d7970921c7f851e2e777326189bb23c3c
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Tue Jul 21 09:31:36 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: remove trajectory-nodes when estimated time has passed
    
    constraint now erases nodes from the constraint if the planned time has passed

http://git.fawkesrobotics.org/fawkes.git/commit/d85bc00
http://trac.fawkesrobotics.org/changeset/d85bc00

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 4147781f1acc2c514c52042b20ed3bcd32b71270
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Fri Jul 31 17:31:02 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: new search-state constructor gets etimated-time
    
    search-state constructor now gets estimated-time for child-node
    added some doxygen-conform comments

http://git.fawkesrobotics.org/fawkes.git/commit/4147781
http://trac.fawkesrobotics.org/changeset/4147781

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 8b1e8874e608f471914f59e4ba912a65738b8797
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Fri Jul 31 17:33:37 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: changed from flaot to fawkes::Time
    
    path now holds list of fawkes::Time
    times specify estimated time the robot will be on a node of the path
    the position of the node in the node-vectors corresponds to the position of the time in the time-vector

http://git.fawkesrobotics.org/fawkes.git/commit/8b1e887
http://trac.fawkesrobotics.org/changeset/8b1e887

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit ff03885e6671b92c24b1e9337ca5af912e588cff
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Fri Jul 31 17:36:55 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: changed to references in parameter list
    
    time is handed as a reference to the constraint

http://git.fawkesrobotics.org/fawkes.git/commit/ff03885
http://trac.fawkesrobotics.org/changeset/ff03885

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit e36233f9f611bdec8031b18e2721c6003b8d0c94
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Fri Jul 31 17:39:44 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: constraint_repo.blocks use fawkes::Time
    
    blocks() function now gets fawkes::Time instead of float
    
    we decided to use fawkes::Time to check for trajectory-constraints
    as high-level componentes in fawkes could also use the check

http://git.fawkesrobotics.org/fawkes.git/commit/e36233f
http://trac.fawkesrobotics.org/changeset/e36233f

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit b42453ca2a5255e18fb82e1056f300f1747dbba7
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Mon Aug 3 15:25:59 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: search_state fix constructor
    
    constructor used AStarState(cost_sofar, parent) in initialisation list
    - parent is member of super-class
    - parameter handed to constructor is parent_state
    - generated seg-fault in generation of AStar-sequence

http://git.fawkesrobotics.org/fawkes.git/commit/b42453c
http://trac.fawkesrobotics.org/changeset/b42453c

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 776812eb95c2200b435c7e208929e5f7d1ff6c6c
Author:     Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
AuthorDate: Mon Aug 3 18:14:22 2015 +0800
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: call AStar Search with fawkes::Time(now)
    
    navgraph-thread now calls AStar search with a timestamp
    could be the current or a future timestamp
    
    the returned path now also holds times (nodes+times)

http://git.fawkesrobotics.org/fawkes.git/commit/776812e
http://trac.fawkesrobotics.org/changeset/776812e

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit ba83ccb23051e83ed24068c89af9efca6dabbf5b
Author:     Christoph Henke <christoph.henke at rwth-aachen.de>
AuthorDate: Thu Sep 24 21:00:21 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: New NavPathInterface. Using navgraph-timed.
    
    Created new NavPathInterface with path times. SearchState edit for calling cost and heuristic function apllied by navgraph-timed. Running with fix priorities.

http://git.fawkesrobotics.org/fawkes.git/commit/ba83ccb
http://trac.fawkesrobotics.org/changeset/ba83ccb

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 607fc46142431e68d8ef0e175b32d0439a86a525
Author:     Christoph Henke <christoph.henke at rwth-aachen.de>
AuthorDate: Mon Oct 19 11:32:42 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph-timed: Trajectory constraint for visu.
    
    Changed ConstraintRepo for using the TrajectoryNodeConstraint. Changes in SearchState for using the overwritten cost and heuristic functions from navgraph-timed. Adding ClockAspect to visualization thread, for using the trajectory constraint.

http://git.fawkesrobotics.org/fawkes.git/commit/607fc46
http://trac.fawkesrobotics.org/changeset/607fc46

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 2c442f726a8c336835865eb87b24ce70234dff21
Author:     Christoph Henke <christoph.henke at rwth-aachen.de>
AuthorDate: Sat Oct 24 21:20:52 2015 +0200
Commit:     Christoph Henke <christoph.henke at rwth-aachen.de>
CommitDate: Mon May 9 09:45:47 2016 +0200

    navgraph: Deleted debugging messages in navgraph

http://git.fawkesrobotics.org/fawkes.git/commit/2c442f7
http://trac.fawkesrobotics.org/changeset/2c442f7

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 cfg/conf.d/amcl.yaml                               |    8 +-
 cfg/conf.d/bumblebee2.yaml                         |    4 +-
 cfg/conf.d/colli.yaml                              |    6 +-
 cfg/conf.d/firevision.yaml                         |    2 +-
 cfg/conf.d/frames.yaml                             |    6 +-
 cfg/conf.d/gazsim.yaml                             |    4 +-
 cfg/conf.d/imu.yaml                                |    2 +-
 cfg/conf.d/katana.yaml                             |    6 +-
 cfg/conf.d/laser-cluster.yaml                      |    2 +-
 cfg/conf.d/laser-lines.yaml                        |    2 +-
 cfg/conf.d/laser.yaml                              |    4 +-
 cfg/conf.d/navgraph.yaml                           |    6 -
 cfg/conf.d/openni.yaml                             |    7 +
 cfg/conf.d/pantilt.yaml                            |    6 +-
 cfg/conf.d/pcl.yaml                                |   10 +
 cfg/conf.d/pcl_db.yaml                             |   12 +-
 cfg/conf.d/robotino.yaml                           |    6 +-
 cfg/conf.d/ros.yaml                                |   15 +-
 cfg/conf.d/static_transforms.yaml                  |    4 +-
 cfg/conf.d/tabletop_objects.yaml                   |    2 +-
 cfg/examples/fake_localization.yaml                |    4 +-
 cfg/examples/tripod_kinect_static_transforms.yaml  |   20 +-
 etc/buildsys/btypes/config_sysinstall.mk           |    4 +-
 etc/buildsys/btypes/config_syswide.mk              |    4 +-
 etc/buildsys/cgal.mk                               |    4 +-
 etc/buildsys/config.mk                             |    9 +-
 etc/buildsys/eigen3.mk                             |    2 +-
 etc/buildsys/fvconf.mk                             |    5 +-
 etc/buildsys/lua.mk                                |    2 +-
 etc/buildsys/lua_check.mk                          |    2 +-
 etc/buildsys/ros.mk                                |   46 +-
 etc/buildsys/rules.mk                              |    2 +-
 res/webview/css/blackboard.css                     |    2 +-
 src/libs/aspect/inifins/pointcloud.cpp             |    8 +-
 src/libs/aspect/inifins/pointcloud.h               |    2 +-
 src/libs/aspect/inifins/tf.cpp                     |    4 +-
 src/libs/aspect/manager.cpp                        |    2 +-
 src/libs/aspect/tf.cpp                             |  131 ++-
 src/libs/aspect/tf.h                               |   15 +-
 src/libs/baseapp/run.cpp                           |   10 +-
 src/libs/baseapp/run.h                             |    6 +-
 src/libs/blackboard/internal/instance_factory.cpp  |   15 +
 src/libs/config/net_handler.cpp                    |    4 +-
 src/libs/config/netconf.cpp                        |   32 +-
 src/libs/config/netconf.h                          |    2 +-
 src/libs/config/yaml.cpp                           |   23 +-
 src/libs/config/yaml.h                             |    2 +-
 src/libs/fvfilters/gauss.cpp                       |    5 +-
 src/libs/fvfilters/hipass.cpp                      |    5 +-
 src/libs/fvfilters/laplace.cpp                     |    5 +-
 src/libs/fvfilters/median.cpp                      |    5 +-
 src/libs/fvfilters/morphology/dilation.cpp         |    5 +-
 src/libs/fvfilters/morphology/erosion.cpp          |    5 +-
 src/libs/fvfilters/or.cpp                          |    5 +-
 src/libs/fvfilters/sharpen.cpp                     |    5 +-
 src/libs/fvfilters/sobel.cpp                       |    5 +-
 src/libs/fvfilters/threshold.cpp                   |    5 +-
 src/libs/interfaces/FacerInterface.cpp             |  291 +++++-
 src/libs/interfaces/FacerInterface.h               |   74 ++-
 src/libs/interfaces/FacerInterface.tolua           |   30 +-
 src/libs/interfaces/FacerInterface.xml             |   22 +
 src/libs/interfaces/KeyValueInterface.cpp          |  388 +++++++
 src/libs/interfaces/KeyValueInterface.h            |  114 ++
 src/libs/interfaces/KeyValueInterface.tolua        |   81 ++
 src/libs/interfaces/KeyValueInterface.xml          |   26 +
 src/libs/interfaces/TransformInterface.cpp         |   40 +-
 src/libs/interfaces/TransformInterface.h           |    7 +
 src/libs/interfaces/TransformInterface.tolua       |    3 +
 src/libs/interfaces/TransformInterface.xml         |    4 +
 .../navgraph/constraints/polygon_constraint.cpp    |    5 +
 src/libs/navgraph/generators/voronoi.cpp           |    2 +-
 src/libs/navgraph/navgraph_edge.cpp                |   15 +
 src/libs/navgraph/navgraph_path.cpp                |   25 +
 src/libs/navgraph/navgraph_path.h                  |    3 +
 src/libs/netcomm/fawkes/server_thread.cpp          |   81 +-
 src/libs/pcl_utils/utils.h                         |   29 +
 src/libs/tf/Makefile                               |   17 +-
 src/libs/tf/buffer_core.cpp                        | 1203 ++++++++++++++++++++
 src/libs/tf/buffer_core.h                          |  228 ++++
 src/libs/tf/static_cache.cpp                       |  154 +++
 src/libs/tf/tf.mk                                  |   13 +-
 src/libs/tf/time_cache.cpp                         |  150 ++--
 src/libs/tf/time_cache.h                           |  123 ++-
 src/libs/tf/transform_listener.cpp                 |   54 +-
 src/libs/tf/transform_listener.h                   |   13 +-
 src/libs/tf/transform_publisher.cpp                |   41 +-
 src/libs/tf/transform_publisher.h                  |   13 +-
 src/libs/tf/transform_storage.h                    |  101 ++
 src/libs/tf/transformer.cpp                        | 1153 +++----------------
 src/libs/tf/transformer.h                          |  126 +--
 src/libs/tf/types.h                                |   25 +-
 src/libs/tf/utils.h                                |   25 +
 src/libs/utils/math/lines.h                        |    8 +
 src/lua/fawkes/fsm/skill_jumpstate.lua             |   42 +
 src/lua/luaagent/agentenv.lua                      |   25 +-
 src/lua/luaagent/agenthsm.lua                      |    5 +
 src/lua/luaagent/jumpstates.lua                    |   16 +-
 src/lua/skiller/skill_jumpstate.lua                |  378 ------
 src/lua/skiller/skillhsm.lua                       |    7 +-
 src/lua/skiller/subskill_jumpstate.lua             |  389 +++++++
 src/lua/skills/generic/align_at_table.lua          |    6 +-
 src/lua/skills/generic/goto.lua                    |   14 +-
 src/lua/skills/generic/jaco.lua                    |    2 +-
 src/lua/skills/generic/katana.lua                  |    8 +-
 src/lua/skills/generic/katana_rel.lua              |    8 +-
 src/plugins/Makefile                               |    2 +-
 src/plugins/amcl/amcl_plugin.cpp                   |    1 -
 src/plugins/amcl/amcl_thread.cpp                   |   48 +-
 src/plugins/amcl/amcl_thread.h                     |    1 -
 src/plugins/amcl/map_lasergen_thread.cpp           |    6 +-
 src/plugins/amcl/ros_thread.cpp                    |    5 +-
 src/plugins/clips-tf/clips_tf_thread.cpp           |    5 +-
 src/plugins/colli/act_thread.cpp                   |   67 --
 src/plugins/colli/act_thread.h                     |   11 -
 src/plugins/colli/colli_thread.cpp                 |   25 +-
 src/plugins/colli/search/og_laser.cpp              |    2 +-
 src/plugins/colli/visualization_thread.cpp         |    9 +-
 src/plugins/colli/visualization_thread.h           |    2 +
 src/plugins/eclipse-clp/utils/skiller.ecl          |    4 +
 src/plugins/eclipse-clp/utils/timing.ecl           |   40 +
 .../examples/tf_example/tf_example_thread.cpp      |   21 +-
 .../examples/tf_example/tf_example_thread.h        |    2 +
 .../gazsim-robotino/gazsim_robotino_thread.cpp     |   10 +-
 .../gazsim-robotino/gazsim_robotino_thread.h       |    2 +
 .../gazsim_vis_localization_thread.cpp             |   18 +
 src/plugins/joystick/joystick_teleop_thread.cpp    |    2 +-
 src/plugins/laser-cluster/laser-cluster-thread.cpp |   46 +-
 src/plugins/laser-filter/filters/map_filter.cpp    |    7 +-
 src/plugins/laser-lines/laser-lines-thread.cpp     |    9 +
 src/plugins/mongodb/mongodb.mk                     |   21 +-
 src/plugins/mongodb/mongodb_thread.cpp             |   20 +
 src/plugins/mongodb_log/mongodb_log_tf_thread.cpp  |   54 +-
 src/plugins/mongodb_log/mongodb_log_tf_thread.h    |    2 +-
 src/plugins/mongodb_log/tools/Makefile             |    2 +-
 .../mongodb_log/tools/ffmongodb_save_imgs.cpp      |    4 +
 .../navgraph_generator_thread.cpp                  |   21 +-
 src/plugins/navgraph/navgraph_thread.cpp           |    4 +-
 src/plugins/navgraph/visualization_thread.cpp      |   66 +-
 src/plugins/navgraph/visualization_thread.h        |    2 +
 src/plugins/openni/pcl_frombuf_thread.cpp          |    2 +-
 src/plugins/openni/pointcloud_thread.cpp           |   14 +-
 src/plugins/openni/pointcloud_thread.h             |    3 +
 src/plugins/openrave/openrave.mk                   |    5 +-
 src/plugins/pantilt/robotis/rx28_thread.cpp        |    2 +-
 .../perception/pcl-db/mongodb_tf_transformer.cpp   |    2 +-
 src/plugins/perception/tabletop-objects/Makefile   |    6 +
 .../tabletop-objects/tabletop_objects_thread.cpp   |   29 +-
 .../tabletop-objects/tabletop_objects_thread.h     |    1 +
 .../tabletop-objects/visualization_thread.cpp      |   12 +-
 .../tabletop-objects/visualization_thread.h        |    1 +
 src/plugins/robot_state_publisher/Makefile         |    7 +-
 src/plugins/robotino/ir_pcl_thread.cpp             |    2 +-
 src/plugins/robotino/ir_pcl_thread.h               |    3 +-
 src/plugins/robotino/robotino.mk                   |   33 +-
 src/plugins/ros/Makefile                           |   17 +-
 src/plugins/ros/laserscan_thread.cpp               |   29 +-
 src/plugins/ros/laserscan_thread.h                 |    4 +-
 src/plugins/ros/move_base_thread.cpp               |    9 +-
 src/plugins/ros/move_base_thread.h                 |    4 +-
 src/plugins/ros/navigator_thread.cpp               |    2 +-
 src/plugins/ros/tf_thread.cpp                      |  291 ++++--
 src/plugins/ros/tf_thread.h                        |   31 +-
 .../static_transforms/static_transforms_thread.cpp |   46 +-
 .../static_transforms/static_transforms_thread.h   |    4 -
 src/tools/ffswitch/Makefile                        |   28 +
 src/tools/ffswitch/ffswitch.txt                    |   57 +
 src/tools/ffswitch/main.cpp                        |  103 ++
 src/tools/set_pose/Makefile                        |    2 +-
 src/tools/set_pose/ffset_pose.cpp                  |   17 +-
 169 files changed, 5104 insertions(+), 2316 deletions(-)
 create mode 100644 cfg/conf.d/pcl.yaml
 create mode 100644 src/libs/interfaces/KeyValueInterface.cpp
 create mode 100644 src/libs/interfaces/KeyValueInterface.h
 create mode 100644 src/libs/interfaces/KeyValueInterface.tolua
 create mode 100644 src/libs/interfaces/KeyValueInterface.xml
 create mode 100644 src/libs/tf/buffer_core.cpp
 create mode 100644 src/libs/tf/buffer_core.h
 create mode 100644 src/libs/tf/static_cache.cpp
 create mode 100644 src/libs/tf/transform_storage.h
 create mode 100644 src/lua/fawkes/fsm/skill_jumpstate.lua
 delete mode 100644 src/lua/skiller/skill_jumpstate.lua
 create mode 100644 src/lua/skiller/subskill_jumpstate.lua
 create mode 100644 src/plugins/eclipse-clp/utils/timing.ecl
 create mode 100644 src/tools/ffswitch/Makefile
 create mode 100644 src/tools/ffswitch/ffswitch.txt
 create mode 100644 src/tools/ffswitch/main.cpp


- *Diffs* -------------------------------------------------------------

- *commit* 3f25efa5a5295fd4872e8b175b6899ea9adbc7e0 - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Mon Mar 9 22:00:03 2015 +0100
Subject: navgraph-timed: initial skeleton for timed navgraph extension plugin

 .../interfaces => plugins/navgraph-timed}/Makefile |   21 ++++---
 .../navgraph_timed_plugin.cpp}                     |   48 ++++++++--------
 .../navgraph_timed_thread.cpp}                     |   51 +++++++++-------
 src/plugins/navgraph-timed/navgraph_timed_thread.h |   63 ++++++++++++++++++++
 4 files changed, 129 insertions(+), 54 deletions(-)


- *commit* 7adddf9bc5236902ef6bbd6ce0dd3c9d4f2bae9e - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Wed Mar 11 15:09:10 2015 +0100
Subject: navgraph-timed: reset cost functions to default when plugin is unloaded

 .../navgraph-timed/navgraph_timed_thread.cpp       |    1 +
 src/plugins/navgraph-timed/navgraph_timed_thread.h |    1 +
 2 files changed, 2 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/navgraph-timed/navgraph_timed_thread.cpp b/src/plugins/navgraph-timed/navgraph_timed_thread.cpp
index cfbd4e0..58ad2e2 100644
--- a/src/plugins/navgraph-timed/navgraph_timed_thread.cpp
+++ b/src/plugins/navgraph-timed/navgraph_timed_thread.cpp
@@ -44,6 +44,7 @@ NavGraphTimedThread::init()
 void
 NavGraphTimedThread::finalize()
 {
+  navgraph->set_search_funcs(NavGraphSearchState::straight_line_estimate, NavGraphSearchState::euclidean_cost);
 }
 
 void
diff --git a/src/plugins/navgraph-timed/navgraph_timed_thread.h b/src/plugins/navgraph-timed/navgraph_timed_thread.h
index aa3f786..fac6a39 100644
--- a/src/plugins/navgraph-timed/navgraph_timed_thread.h
+++ b/src/plugins/navgraph-timed/navgraph_timed_thread.h
@@ -28,6 +28,7 @@
 #include <aspect/blackboard.h>
 #include <navgraph/aspect/navgraph.h>
 #include <navgraph/navgraph.h>
+#include <navgraph/search_state.h>
 
 class NavGraphTimedThread
 : public fawkes::Thread,

- *commit* 5240d1d369ce0f59e02fa73d12070802c9e72cb7 - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Wed Mar 11 16:11:12 2015 +0100
Subject: navgraph-timed: changed method to unset search functions

 .../navgraph-timed/navgraph_timed_thread.cpp       |    2 +-
 src/plugins/navgraph-timed/navgraph_timed_thread.h |    1 -
 2 files changed, 1 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/navgraph-timed/navgraph_timed_thread.cpp b/src/plugins/navgraph-timed/navgraph_timed_thread.cpp
index 58ad2e2..3cef3f5 100644
--- a/src/plugins/navgraph-timed/navgraph_timed_thread.cpp
+++ b/src/plugins/navgraph-timed/navgraph_timed_thread.cpp
@@ -44,7 +44,7 @@ NavGraphTimedThread::init()
 void
 NavGraphTimedThread::finalize()
 {
-  navgraph->set_search_funcs(NavGraphSearchState::straight_line_estimate, NavGraphSearchState::euclidean_cost);
+  navgraph->unset_search_funcs();
 }
 
 void
diff --git a/src/plugins/navgraph-timed/navgraph_timed_thread.h b/src/plugins/navgraph-timed/navgraph_timed_thread.h
index fac6a39..aa3f786 100644
--- a/src/plugins/navgraph-timed/navgraph_timed_thread.h
+++ b/src/plugins/navgraph-timed/navgraph_timed_thread.h
@@ -28,7 +28,6 @@
 #include <aspect/blackboard.h>
 #include <navgraph/aspect/navgraph.h>
 #include <navgraph/navgraph.h>
-#include <navgraph/search_state.h>
 
 class NavGraphTimedThread
 : public fawkes::Thread,

- *commit* 180f478d0a8a4f704adc90366f4f8ed487e042ba - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Wed Mar 11 15:09:10 2015 +0100
Subject: navgraph-timed: reset cost functions to default when plugin is unloaded

 src/plugins/navgraph-timed/navgraph_timed_thread.h |    1 +
 1 files changed, 1 insertions(+), 0 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/navgraph-timed/navgraph_timed_thread.h b/src/plugins/navgraph-timed/navgraph_timed_thread.h
index aa3f786..fac6a39 100644
--- a/src/plugins/navgraph-timed/navgraph_timed_thread.h
+++ b/src/plugins/navgraph-timed/navgraph_timed_thread.h
@@ -28,6 +28,7 @@
 #include <aspect/blackboard.h>
 #include <navgraph/aspect/navgraph.h>
 #include <navgraph/navgraph.h>
+#include <navgraph/search_state.h>
 
 class NavGraphTimedThread
 : public fawkes::Thread,

- *commit* d05d9b7eec225f7405802bb5f98e85b7f85c0863 - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Wed Mar 11 16:11:12 2015 +0100
Subject: navgraph-timed: changed method to unset search functions

 src/plugins/navgraph-timed/navgraph_timed_thread.h |    1 -
 1 files changed, 0 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/navgraph-timed/navgraph_timed_thread.h b/src/plugins/navgraph-timed/navgraph_timed_thread.h
index fac6a39..aa3f786 100644
--- a/src/plugins/navgraph-timed/navgraph_timed_thread.h
+++ b/src/plugins/navgraph-timed/navgraph_timed_thread.h
@@ -28,7 +28,6 @@
 #include <aspect/blackboard.h>
 #include <navgraph/aspect/navgraph.h>
 #include <navgraph/navgraph.h>
-#include <navgraph/search_state.h>
 
 class NavGraphTimedThread
 : public fawkes::Thread,

- *commit* 71e4b2f2a9ed0b7b7e0f9296077c825d883fe9d1 - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Mon Jun 8 17:42:28 2015 +0200
Subject: navgraph-timed: moved plugin to fawkes-robotino repository

 src/plugins/navgraph-timed/Makefile                |   31 ----------
 .../navgraph-timed/navgraph_timed_plugin.cpp       |   45 --------------
 .../navgraph-timed/navgraph_timed_thread.cpp       |   54 -----------------
 src/plugins/navgraph-timed/navgraph_timed_thread.h |   63 --------------------
 4 files changed, 0 insertions(+), 193 deletions(-)


- *commit* a32eba8fa0ed9b8c7252b86afa2e6dc5694370ed - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Tue Jun 23 18:15:41 2015 +0200
Subject: navgraph: added methods for cost estimation based on whole paths

 src/libs/navgraph/navgraph.cpp      |   82 +++++++++++++++++++++++++++++------
 src/libs/navgraph/navgraph.h        |   15 ++++++-
 src/libs/navgraph/navgraph_path.cpp |   30 ++++++++++++-
 src/libs/navgraph/navgraph_path.h   |    2 +
 src/libs/navgraph/search_state.cpp  |   47 +++++++++++---------
 src/libs/navgraph/search_state.h    |   22 +++++-----
 6 files changed, 148 insertions(+), 50 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/navgraph.cpp b/src/libs/navgraph/navgraph.cpp
index 557067f..d4b7ccd 100644
--- a/src/libs/navgraph/navgraph.cpp
+++ b/src/libs/navgraph/navgraph.cpp
@@ -944,8 +944,8 @@ NavGraph::unset_search_funcs()
  * distance between nodes. The cost is the sum of costs of all edges
  * along the way from one node to another. The estimate is the straight line
  * distance from any given node to the goal node (which is provably admissible).
- * @param from node to search from
- * @param to goal node
+ * @param from name of node to search from
+ * @param to name of the goal node
  * @param use_constraints true to respect constraints imposed by the constraint
  * repository, false to ignore the repository searching as if there were no
  * constraints whatsoever.
@@ -956,7 +956,7 @@ NavGraph::unset_search_funcs()
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
+NavGraph::search_path(const std::string &from, const std::string &to,
 		      bool use_constraints, bool compute_constraints)
 {
   return search_path(from, to,
@@ -964,6 +964,44 @@ NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
 		     use_constraints, compute_constraints);
 }
 
+/** Search for a path between two nodes.
+ * This function executes an A* search to find an (optimal) path
+ * from node @p from to node @p to.
+ * @param from name of node to search from
+ * @param to name of the goal node
+ * @param estimate_func function to estimate the cost from any node to the goal.
+ * Note that the estimate function must be admissible for optimal A* search. That
+ * means that for no query may the calculated estimate be higher than the actual
+ * cost.
+ * @param cost_func function to calculate the cost from a node to another adjacent
+ * node. Note that the cost function is directly related to the estimate function.
+ * For example, the cost can be calculated in terms of distance between nodes, or in
+ * time that it takes to travel from one node to the other. The estimate function must
+ * match the cost function to be admissible.
+ * @param use_constraints true to respect constraints imposed by the constraint
+ * repository, false to ignore the repository searching as if there were no
+ * constraints whatsoever.
+ * @param compute_constraints if true re-compute constraints, otherwise use constraints
+ * as-is, for example if they have been computed before to check for changes.
+ * @return ordered vector of nodes which denote a path from @p from to @p to.
+ * Note that the vector is empty if no path could be found (i.e. there is non
+ * or it was prohibited when using constraints.
+ */
+fawkes::NavGraphPath
+NavGraph::search_path(const std::string &from, const std::string &to,
+		      navgraph::EstimateFunction estimate_func,
+		      navgraph::CostFunction cost_func,
+		      bool use_constraints, bool compute_constraints)
+{
+  NavGraphNode from_node(node(from));
+  std::vector<fawkes::NavGraphNode> nodes;
+  nodes.push_back(from_node);
+  NavGraphPath from_path(this, nodes);
+  NavGraphNode to_node(node(to));
+  return search_path(from_path, to_node, estimate_func, cost_func,
+		     use_constraints, compute_constraints);
+}
+
 /** Search for a path between two nodes with default distance costs.
  * This function executes an A* search to find an (optimal) path
  * from node @p from to node @p to.
@@ -984,7 +1022,7 @@ NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const std::string &from, const std::string &to,
+NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
 		      bool use_constraints, bool compute_constraints)
 {
   return search_path(from, to,
@@ -995,8 +1033,8 @@ NavGraph::search_path(const std::string &from, const std::string &to,
 /** Search for a path between two nodes.
  * This function executes an A* search to find an (optimal) path
  * from node @p from to node @p to.
- * @param from name of node to search from
- * @param to name of the goal node
+ * @param from node to search from
+ * @param to goal node
  * @param estimate_func function to estimate the cost from any node to the goal.
  * Note that the estimate function must be admissible for optimal A* search. That
  * means that for no query may the calculated estimate be higher than the actual
@@ -1016,21 +1054,22 @@ NavGraph::search_path(const std::string &from, const std::string &to,
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const std::string &from, const std::string &to,
+NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
 		      navgraph::EstimateFunction estimate_func,
 		      navgraph::CostFunction cost_func,
 		      bool use_constraints, bool compute_constraints)
 {
-  NavGraphNode from_node(node(from));
-  NavGraphNode to_node(node(to));
-  return search_path(from_node, to_node, estimate_func, cost_func,
+  std::vector<fawkes::NavGraphNode> nodes;
+  nodes.push_back(from);
+  NavGraphPath from_path(this, nodes);
+  return search_path(from_path, to, estimate_func, cost_func,
 		     use_constraints, compute_constraints);
 }
 
 /** Search for a path between two nodes.
  * This function executes an A* search to find an (optimal) path
  * from node @p from to node @p to.
- * @param from node to search from
+ * @param from path to search from
  * @param to goal node
  * @param estimate_func function to estimate the cost from any node to the goal.
  * Note that the estimate function must be admissible for optimal A* search. That
@@ -1051,7 +1090,7 @@ NavGraph::search_path(const std::string &from, const std::string &to,
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
+NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to,
 		      navgraph::EstimateFunction estimate_func,
 		      navgraph::CostFunction cost_func,
 		      bool use_constraints, bool compute_constraints)
@@ -1083,7 +1122,7 @@ NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
   NavGraphSearchState *solstate;
   for (unsigned int i = 0; i < a_star_solution.size(); ++i ) {
     solstate = dynamic_cast<NavGraphSearchState *>(a_star_solution[i]);
-    path[i] = solstate->node();
+    path[i] = solstate->path().nodes().back();
   }
 
   float cost =
@@ -1106,6 +1145,23 @@ NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
 float
 NavGraph::cost(const NavGraphNode &from, const NavGraphNode &to) const
 {
+  std::vector<fawkes::NavGraphNode> nodes;
+  nodes.push_back(from);
+  NavGraphPath path(this, nodes);
+  return search_cost_func_(path, to);
+}
+
+/** Calculate cost between two adjacent nodes.
+ * It is not verified whether the nodes are actually adjacent, but the cost
+ * function is simply applied. This is done to increase performance.
+ * The calculation will use the currently registered cost function.
+ * @param from path
+ * @param to second node
+ * @return cost from @p from to @p to
+ */
+float
+NavGraph::cost(const NavGraphPath &from, const NavGraphNode &to) const
+{
   return search_cost_func_(from, to);
 }
 
diff --git a/src/libs/navgraph/navgraph.h b/src/libs/navgraph/navgraph.h
index 6ce9926..9ee5f7f 100644
--- a/src/libs/navgraph/navgraph.h
+++ b/src/libs/navgraph/navgraph.h
@@ -43,10 +43,10 @@ namespace navgraph {
 }
 #endif
   typedef
-  std::function<float (const fawkes::NavGraphNode &, const fawkes::NavGraphNode &)>
+  std::function<float (const fawkes::NavGraphPath &, const fawkes::NavGraphNode &)>
       EstimateFunction;
   typedef
-  std::function<float (const fawkes::NavGraphNode &, const fawkes::NavGraphNode &)>
+  std::function<float (const fawkes::NavGraphPath &, const fawkes::NavGraphNode &)>
       CostFunction;
 
   extern const char *PROP_ORIENTATION;
@@ -145,6 +145,16 @@ class NavGraph
 				   navgraph::CostFunction cost_func,
 				   bool use_constraints = true, bool compute_constraints = true);
 
+  fawkes::NavGraphPath search_path(const NavGraphPath &from,
+				   const NavGraphNode &to,
+				   bool use_constraints = true, bool compute_constraints = true);
+
+  fawkes::NavGraphPath search_path(const NavGraphPath &from,
+				   const NavGraphNode &to,
+				   navgraph::EstimateFunction estimate_func,
+				   navgraph::CostFunction cost_func,
+				   bool use_constraints = true, bool compute_constraints = true);
+
   void add_node(const NavGraphNode &node);
   void add_node_and_connect(const NavGraphNode &node, ConnectionMode conn_mode);
   void connect_node_to_closest_node(const NavGraphNode &n);
@@ -195,6 +205,7 @@ class NavGraph
   void unset_search_funcs();
 
   float cost(const NavGraphNode &from, const NavGraphNode &to) const;
+  float cost(const NavGraphPath &from, const NavGraphNode &to) const;
 
   static std::string format_name(const char *format, ...);
   std::string gen_unique_name(const char *prefix = "U-");
diff --git a/src/libs/navgraph/navgraph_path.cpp b/src/libs/navgraph/navgraph_path.cpp
index 52539af..eb950dd 100644
--- a/src/libs/navgraph/navgraph_path.cpp
+++ b/src/libs/navgraph/navgraph_path.cpp
@@ -174,6 +174,18 @@ NavGraphPath::clear()
   cost_ = -1;
 }
 
+/** Get name of path
+ * @return name of path
+ */
+std::string
+NavGraphPath::name()
+{
+  std::string ret;
+  for (auto node : nodes_)
+    ret += node.name();
+  return ret;
+}
+
 
 /** Check if the path contains a given node.
  * @param node node to check for in current path
@@ -188,7 +200,7 @@ NavGraphPath::contains(const NavGraphNode &node) const
 
 /** Get goal of path.
  * @return goal of this path, i.e. the last node in the sequence of nodes.
- * @throw Exeption if there are no nodes in this path
+ * @throw Exception if there are no nodes in this path
  */
 const NavGraphNode &
 NavGraphPath::goal() const
@@ -200,6 +212,20 @@ NavGraphPath::goal() const
   return nodes_[nodes_.size() - 1];
 }
 
+/** Get last node of path.
+ * @return last node of this path
+ * @throw Exception of there are no nodes in this path
+ */
+const NavGraphNode &
+NavGraphPath::back() const
+{
+  if (nodes_.empty()) {
+    throw Exception("No nodes in plan, cannot retrieve last node");
+  }
+
+  return nodes_.back();
+}
+
 
 /** Get graph this path is based on.
  * @return const reference to graph this path is based on
@@ -396,7 +422,7 @@ NavGraphPath::Traversal::remaining_cost() const
 
   float cost = 0.;
   for (ssize_t i = current_; i < (ssize_t)path_->nodes_.size() - 1; ++i) {
-    cost += path_->graph_->cost(path_->nodes_[i], path_->nodes_[i+1]);
+    cost += path_->graph_->cost(*path_, path_->nodes_[i+1]);
   }
 
   return cost;
diff --git a/src/libs/navgraph/navgraph_path.h b/src/libs/navgraph/navgraph_path.h
index b6c72fb..c02795d 100644
--- a/src/libs/navgraph/navgraph_path.h
+++ b/src/libs/navgraph/navgraph_path.h
@@ -84,6 +84,7 @@ class NavGraphPath {
 
   const NavGraph &     graph() const;
   const NavGraphNode & goal() const;
+  const NavGraphNode & back() const;
 
   std::string	get_path_as_string(const char delim = ':') const;
   std::vector<std::string> get_node_names() const;
@@ -107,6 +108,7 @@ class NavGraphPath {
   bool   empty() const;
   size_t size() const;
   void   clear();
+  std::string name();
 
   Traversal traversal() const;
 
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index a2e4164..ff0662d 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -35,7 +35,7 @@ namespace fawkes {
  */
 
 /** Constructor.
- * @param node graph node this search state represents
+ * @param path graph path this search state represents
  * @param goal graph node of the goal
  * @param cost_sofar the cost until to this node from the start
  * @param parent parent search state
@@ -51,7 +51,7 @@ namespace fawkes {
  * match the cost function to be admissible.
  * @param constraint_repo constraint repository, null to plan only without constraints
  */
-NavGraphSearchState::NavGraphSearchState(NavGraphNode node, NavGraphNode goal,
+NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
 					 double cost_sofar, NavGraphSearchState *parent,
 					 NavGraph *map_graph,
 					 navgraph::EstimateFunction estimate_func,
@@ -59,31 +59,31 @@ NavGraphSearchState::NavGraphSearchState(NavGraphNode node, NavGraphNode goal,
 					 fawkes::NavGraphConstraintRepo *constraint_repo)
   : AStarState(cost_sofar, parent), estimate_func_(estimate_func), cost_func_(cost_func)
 {
-  node_ = node;
+  path_ = path;
   goal_ = goal;
   map_graph_ = map_graph;
 
   total_estimated_cost = path_cost + estimate();
 
   std::hash<std::string> h;
-  key_ = h(node_.name());
+  key_ = h(path_.name());
 
   constraint_repo_ = constraint_repo;
 }
 
 
 /** Constructor.
- * @param node graph node this search state represents
+ * @param path graph path this search state represents
  * @param goal graph node of the goal
  * @param map_graph map graph
  * @param constraint_repo constraint repository, null to plan only without constraints
  */
-NavGraphSearchState::NavGraphSearchState(NavGraphNode node, NavGraphNode goal,
+NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
 					 NavGraph *map_graph,
 					 fawkes::NavGraphConstraintRepo *constraint_repo)
   : AStarState(0, NULL)
 {
-  node_ = node;
+  path_ = path;
   goal_ = goal;
   map_graph_ = map_graph;
 
@@ -93,14 +93,14 @@ NavGraphSearchState::NavGraphSearchState(NavGraphNode node, NavGraphNode goal,
   total_estimated_cost = path_cost + estimate();
 
   std::hash<std::string> h;
-  key_ = h(node_.name());
+  key_ = h(path_.name());
 
   constraint_repo_ = constraint_repo;
 }
 
 
 /** Constructor.
- * @param node graph node this search state represents
+ * @param path graph path this search state represents
  * @param goal graph node of the goal
  * @param map_graph map graph
  * @param estimate_func function to estimate the cost from any node to the goal.
@@ -114,21 +114,21 @@ NavGraphSearchState::NavGraphSearchState(NavGraphNode node, NavGraphNode goal,
  * match the cost function to be admissible.
  * @param constraint_repo constraint repository, null to plan only without constraints
  */
-NavGraphSearchState::NavGraphSearchState(NavGraphNode node, NavGraphNode goal,
+NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
 					 NavGraph *map_graph,
 					 navgraph::EstimateFunction estimate_func,
 					 navgraph::CostFunction cost_func,
 					 fawkes::NavGraphConstraintRepo *constraint_repo)
   : AStarState(0, NULL), estimate_func_(estimate_func), cost_func_(cost_func)
 {
-  node_ = node;
+  path_ = path;
   goal_ = goal;
   map_graph_ = map_graph;
 
   total_estimated_cost = path_cost + estimate();
 
   std::hash<std::string> h;
-  key_ = h(node_.name());
+  key_ = h(path_.name());
 
   constraint_repo_ = constraint_repo;
 }
@@ -143,24 +143,24 @@ NavGraphSearchState::~NavGraphSearchState()
 /** Get graph node corresponding to this search state.
  * @return graph node corresponding to this search state
  */
-fawkes::NavGraphNode &
-NavGraphSearchState::node()
+fawkes::NavGraphPath &
+NavGraphSearchState::path()
 {
-  return node_;
+  return path_;
 }
 
 
 float
 NavGraphSearchState::estimate()
 {
-  return estimate_func_(node_, goal_);
+  return estimate_func_(path_, goal_);
 }
 
 
 bool
 NavGraphSearchState::is_goal()
 {
-  return (node_.name() == goal_.name());
+  return (path_.nodes().back().name() == goal_.name());
 }
 
 
@@ -170,7 +170,7 @@ NavGraphSearchState::children()
   std::vector< AStarState * > children;
   children.clear();
 
-  std::vector<std::string> descendants = node_.reachable_nodes();
+  std::vector<std::string> descendants = path_.nodes().back().reachable_nodes();
 
   for (unsigned int i = 0; i < descendants.size(); ++i) {
     NavGraphNode d = map_graph_->node(descendants[i]);
@@ -179,22 +179,25 @@ NavGraphSearchState::children()
     if (constraint_repo_) {
       if (constraint_repo_->blocks(d)) {
 	expand = false;
-      } else if (constraint_repo_->blocks(node_, d)) {
+      } else if (constraint_repo_->blocks(path_.back(), d)) {
 	expand = false;
       }
     }
 
     if (expand) {
-      float d_cost = cost_func_(node_, d);
+      float d_cost = cost_func_(path_, d);
 
       if (constraint_repo_) {
 	float cost_factor = 0.;
-	if (constraint_repo_->increases_cost(node_, d, cost_factor)) {
+	if (constraint_repo_->increases_cost(path_.back(), d, cost_factor)) {
 	  d_cost *= cost_factor;
 	}
       }
 
-      children.push_back(new NavGraphSearchState(d, goal_, path_cost + d_cost, this,
+      std::vector<NavGraphNode> new_nodes(path_.nodes());
+      NavGraphPath new_path(map_graph_, new_nodes);
+      new_path.add_node(d);
+      children.push_back(new NavGraphSearchState(new_path, goal_, path_cost + d_cost, this,
 						 map_graph_, estimate_func_, cost_func_,
 						 constraint_repo_));
     }
diff --git a/src/libs/navgraph/search_state.h b/src/libs/navgraph/search_state.h
index 2eff519..c3427ea 100644
--- a/src/libs/navgraph/search_state.h
+++ b/src/libs/navgraph/search_state.h
@@ -38,11 +38,11 @@ namespace fawkes {
 class NavGraphSearchState : public fawkes::AStarState
 {
  public:
-  NavGraphSearchState(fawkes::NavGraphNode node, fawkes::NavGraphNode goal,
+  NavGraphSearchState(fawkes::NavGraphPath path, fawkes::NavGraphNode goal,
 		      fawkes::NavGraph *map_graph,
 		      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
 
-  NavGraphSearchState(fawkes::NavGraphNode node, fawkes::NavGraphNode goal,
+  NavGraphSearchState(fawkes::NavGraphPath path, fawkes::NavGraphNode goal,
 		      fawkes::NavGraph *map_graph,
 		      navgraph::EstimateFunction estimate_func,
 		      navgraph::CostFunction cost_func = NavGraphSearchState::euclidean_cost,
@@ -50,7 +50,7 @@ class NavGraphSearchState : public fawkes::AStarState
 
   ~NavGraphSearchState();
 
-  fawkes::NavGraphNode & node();
+  fawkes::NavGraphPath & path();
 
   virtual size_t key() { return key_; }
   virtual float  estimate();
@@ -63,11 +63,11 @@ class NavGraphSearchState : public fawkes::AStarState
    * @return cost from @p from to @p to.
    */
   static float
-  euclidean_cost(const fawkes::NavGraphNode &from,
+  euclidean_cost(const fawkes::NavGraphPath &from,
 		 const fawkes::NavGraphNode &to)
   {
-    return sqrtf(powf(to.x() - from.x(), 2) +
-		 powf(to.y() - from.y(), 2) );
+    return sqrtf(powf(to.x() - from.nodes().back().x(), 2) +
+        powf(to.y() - from.nodes().back().y(), 2) );
   }
 
   /** Determine straight line estimate between two nodes.
@@ -76,15 +76,15 @@ class NavGraphSearchState : public fawkes::AStarState
    * @return estimate of cost from @p node to @p goal.
    */
   static float
-  straight_line_estimate(const fawkes::NavGraphNode &node,
+  straight_line_estimate(const fawkes::NavGraphPath &path,
 			 const fawkes::NavGraphNode &goal)
   {
-    return sqrtf(powf(goal.x() - node.x(), 2) +
-		 powf(goal.y() - node.y(), 2) );
+    return sqrtf(powf(goal.x() - path.nodes().back().x(), 2) +
+        powf(goal.y() - path.nodes().back().y(), 2) );
   }
 
  private:
-  NavGraphSearchState(fawkes::NavGraphNode node, fawkes::NavGraphNode goal,
+  NavGraphSearchState(fawkes::NavGraphPath path, fawkes::NavGraphNode goal,
 		      double cost_sofar, NavGraphSearchState *parent_state,
 		      fawkes::NavGraph *map_graph,
 		      navgraph::EstimateFunction estimate_func,
@@ -95,7 +95,7 @@ class NavGraphSearchState : public fawkes::AStarState
   std::vector<AStarState *> children();
 
   // state information
-  fawkes::NavGraphNode  node_;
+  fawkes::NavGraphPath  path_;
 
   // goal information
   fawkes::NavGraphNode  goal_;

- *commit* 2de806eb3498dd0698dc46eef6e7bd64d887fe9f - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Thu Jun 25 11:57:49 2015 +0200
Subject: navgraph: corrected comments

 src/libs/navgraph/search_state.h |    4 ++--
 1 files changed, 2 insertions(+), 2 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/search_state.h b/src/libs/navgraph/search_state.h
index c3427ea..5164c3d 100644
--- a/src/libs/navgraph/search_state.h
+++ b/src/libs/navgraph/search_state.h
@@ -58,7 +58,7 @@ class NavGraphSearchState : public fawkes::AStarState
 
   /** Determine euclidean cost between two nodes.
    * Note that the given notes are assumed to be adjacent nodes.
-   * @param from originating node
+   * @param from originating path
    * @param to destination node
    * @return cost from @p from to @p to.
    */
@@ -71,7 +71,7 @@ class NavGraphSearchState : public fawkes::AStarState
   }
 
   /** Determine straight line estimate between two nodes.
-   * @param node node to query heuristic value for
+   * @param path path to query heuristic value for
    * @param goal goal node to get estimate for
    * @return estimate of cost from @p node to @p goal.
    */

- *commit* 5827ef5b37c7998e02c63124aec0fdbbc713769d - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Fri Jul 3 14:32:52 2015 +0200
Subject: navgraph-timed: moved navpath name generation

 src/libs/navgraph/navgraph_path.cpp |    9 +++++----
 src/libs/navgraph/navgraph_path.h   |    2 +-
 2 files changed, 6 insertions(+), 5 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/navgraph_path.cpp b/src/libs/navgraph/navgraph_path.cpp
index eb950dd..ed214b0 100644
--- a/src/libs/navgraph/navgraph_path.cpp
+++ b/src/libs/navgraph/navgraph_path.cpp
@@ -125,6 +125,7 @@ NavGraphPath::add_node(const NavGraphNode &node, float cost_from_end)
   if (cost_from_end > 0) {
     cost_ += cost_from_end;
   }
+  name_ += node.name();
 }
 
 
@@ -140,6 +141,9 @@ NavGraphPath::set_nodes(const std::vector<NavGraphNode> &nodes, float cost)
 {
   nodes_        = nodes;
   cost_         = cost;
+  name_.clear();
+  for (auto node : nodes_)
+    name_ += node.name();
 }
 
 
@@ -180,10 +184,7 @@ NavGraphPath::clear()
 std::string
 NavGraphPath::name()
 {
-  std::string ret;
-  for (auto node : nodes_)
-    ret += node.name();
-  return ret;
+  return name_;
 }
 
 
diff --git a/src/libs/navgraph/navgraph_path.h b/src/libs/navgraph/navgraph_path.h
index c02795d..8f7544b 100644
--- a/src/libs/navgraph/navgraph_path.h
+++ b/src/libs/navgraph/navgraph_path.h
@@ -130,7 +130,7 @@ class NavGraphPath {
   const NavGraph            *graph_;
   std::vector<NavGraphNode>  nodes_;
   float                      cost_;
- 
+  std::string                name_;
 };
 
 

- *commit* d230248eff4b3585060b428b2ea21fdb573dae3d - - - - - - - - - -
Author:  Matthias Loebach <matthias.loebach at ima-zlw-ifu.rwth-aachen.de>
Date:    Mon Jul 6 16:30:40 2015 +0200
Subject: navgraph-timed: added call by reference for NavPath in constructors

 src/libs/navgraph/search_state.cpp |   64 +++++++++++++++++------------------
 src/libs/navgraph/search_state.h   |   42 +++++++++++++++--------
 2 files changed, 58 insertions(+), 48 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index ff0662d..c66a738 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -51,16 +51,19 @@ namespace fawkes {
  * match the cost function to be admissible.
  * @param constraint_repo constraint repository, null to plan only without constraints
  */
-NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
-					 double cost_sofar, NavGraphSearchState *parent,
-					 NavGraph *map_graph,
-					 navgraph::EstimateFunction estimate_func,
-					 navgraph::CostFunction cost_func,
-					 fawkes::NavGraphConstraintRepo *constraint_repo)
-  : AStarState(cost_sofar, parent), estimate_func_(estimate_func), cost_func_(cost_func)
+NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
+                                         const NavGraphNode &node,
+                                         const NavGraphNode &goal,
+                                         double cost_sofar,
+                                         NavGraphSearchState *parent_state,
+                                         NavGraph *map_graph,
+                                         navgraph::EstimateFunction estimate_func,
+                                         navgraph::CostFunction cost_func,
+                                         fawkes::NavGraphConstraintRepo *constraint_repo)
+  : AStarState(cost_sofar, parent), path_(path),
+    goal_(goal), estimate_func_(estimate_func), cost_func_(cost_func)
 {
-  path_ = path;
-  goal_ = goal;
+  path_.add_node(node);
   map_graph_ = map_graph;
 
   total_estimated_cost = path_cost + estimate();
@@ -78,13 +81,12 @@ NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
  * @param map_graph map graph
  * @param constraint_repo constraint repository, null to plan only without constraints
  */
-NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
-					 NavGraph *map_graph,
-					 fawkes::NavGraphConstraintRepo *constraint_repo)
-  : AStarState(0, NULL)
+NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
+                                         const NavGraphNode &goal,
+                                         NavGraph *map_graph,
+                                         fawkes::NavGraphConstraintRepo *constraint_repo)
+  : AStarState(0, NULL), path_(path), goal_(goal)
 {
-  path_ = path;
-  goal_ = goal;
   map_graph_ = map_graph;
 
   estimate_func_ = straight_line_estimate;
@@ -114,15 +116,15 @@ NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
  * match the cost function to be admissible.
  * @param constraint_repo constraint repository, null to plan only without constraints
  */
-NavGraphSearchState::NavGraphSearchState(NavGraphPath path, NavGraphNode goal,
-					 NavGraph *map_graph,
-					 navgraph::EstimateFunction estimate_func,
-					 navgraph::CostFunction cost_func,
-					 fawkes::NavGraphConstraintRepo *constraint_repo)
-  : AStarState(0, NULL), estimate_func_(estimate_func), cost_func_(cost_func)
+NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
+                                         const NavGraphNode &goal,
+                                         NavGraph *map_graph,
+                                         navgraph::EstimateFunction estimate_func,
+                                         navgraph::CostFunction cost_func,
+                                         fawkes::NavGraphConstraintRepo *constraint_repo)
+  : AStarState(0, NULL), path_(path), goal_(goal),
+    estimate_func_(estimate_func), cost_func_(cost_func)
 {
-  path_ = path;
-  goal_ = goal;
   map_graph_ = map_graph;
 
   total_estimated_cost = path_cost + estimate();
@@ -188,18 +190,14 @@ NavGraphSearchState::children()
       float d_cost = cost_func_(path_, d);
 
       if (constraint_repo_) {
-	float cost_factor = 0.;
-	if (constraint_repo_->increases_cost(path_.back(), d, cost_factor)) {
-	  d_cost *= cost_factor;
-	}
+        float cost_factor = 0.;
+        if (constraint_repo_->increases_cost(path_.back(), d, cost_factor)) {
+          d_cost *= cost_factor;
+        }
       }
 
-      std::vector<NavGraphNode> new_nodes(path_.nodes());
-      NavGraphPath new_path(map_graph_, new_nodes);
-      new_path.add_node(d);
-      children.push_back(new NavGraphSearchState(new_path, goal_, path_cost + d_cost, this,
-						 map_graph_, estimate_func_, cost_func_,
-						 constraint_repo_));
+      children.push_back(new NavGraphSearchState(path_, d, goal_, path_cost + d_cost, this,
+						 map_graph_, estimate_func_, cost_func_, constraint_repo_));
     }
   }
 
diff --git a/src/libs/navgraph/search_state.h b/src/libs/navgraph/search_state.h
index 5164c3d..c9f2ea0 100644
--- a/src/libs/navgraph/search_state.h
+++ b/src/libs/navgraph/search_state.h
@@ -38,15 +38,25 @@ namespace fawkes {
 class NavGraphSearchState : public fawkes::AStarState
 {
  public:
-  NavGraphSearchState(fawkes::NavGraphPath path, fawkes::NavGraphNode goal,
-		      fawkes::NavGraph *map_graph,
-		      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
-
-  NavGraphSearchState(fawkes::NavGraphPath path, fawkes::NavGraphNode goal,
-		      fawkes::NavGraph *map_graph,
-		      navgraph::EstimateFunction estimate_func,
-		      navgraph::CostFunction cost_func = NavGraphSearchState::euclidean_cost,
-		      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
+  NavGraphSearchState(const fawkes::NavGraphPath &path,
+                      const fawkes::NavGraphNode &goal,
+                      fawkes::NavGraph *map_graph,
+                      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
+
+  NavGraphSearchState(const fawkes::NavGraphPath &path,
+                      const fawkes::NavGraphNode &node,
+                      const fawkes::NavGraphNode &goal,
+                      fawkes::NavGraph *map_graph,
+                      navgraph::EstimateFunction estimate_func,
+                      navgraph::CostFunction cost_func = NavGraphSearchState::euclidean_cost,
+                      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
+
+  NavGraphSearchState(const fawkes::NavGraphPath &path,
+                      const fawkes::NavGraphNode &goal,
+                      fawkes::NavGraph *map_graph,
+                      navgraph::EstimateFunction estimate_func,
+                      navgraph::CostFunction cost_func = NavGraphSearchState::euclidean_cost,
+                      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
 
   ~NavGraphSearchState();
 
@@ -84,12 +94,14 @@ class NavGraphSearchState : public fawkes::AStarState
   }
 
  private:
-  NavGraphSearchState(fawkes::NavGraphPath path, fawkes::NavGraphNode goal,
-		      double cost_sofar, NavGraphSearchState *parent_state,
-		      fawkes::NavGraph *map_graph,
-		      navgraph::EstimateFunction estimate_func,
-		      navgraph::CostFunction cost_func,
-		      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
+  NavGraphSearchState(const fawkes::NavGraphPath &path,
+                      const fawkes::NavGraphNode &node,
+                      const fawkes::NavGraphNode &goal,
+                      double cost_sofar, NavGraphSearchState *parent_state,
+                      fawkes::NavGraph *map_graph,
+                      navgraph::EstimateFunction estimate_func,
+                      navgraph::CostFunction cost_func,
+                      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
 
  private:
   std::vector<AStarState *> children();

- *commit* 04dcc91b0811e907768220682d271ec02f722f53 - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Fri Jul 17 18:30:09 2015 +0800
Subject: navgraph-constraints: added trajectory-node constraint

 .../constraints/trajectory_node_constraint.cpp     |  186 ++++++++++++++++++++
 ...e_constraint.h => trajectory_node_constraint.h} |   46 +++--
 2 files changed, 216 insertions(+), 16 deletions(-)


- *commit* 27625709c8d82994c4c8c181ea05b8a0fa291b9d - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Mon Jul 20 11:25:57 2015 +0800
Subject: navgraph-timed: first draft of trajectory-constraint

 src/libs/navgraph/constraints/constraint_repo.cpp  |   83 ++++++++++++++++-
 src/libs/navgraph/constraints/constraint_repo.h    |   17 +++-
 .../constraints/trajectory_node_constraint.cpp     |   97 ++++++++++++++++++-
 .../constraints/trajectory_node_constraint.h       |   27 ++++-
 src/libs/navgraph/navgraph_path.cpp                |    8 ++-
 src/libs/navgraph/navgraph_path.h                  |    9 ++-
 src/libs/navgraph/search_state.cpp                 |   34 +++++--
 src/libs/navgraph/search_state.h                   |    6 +-
 8 files changed, 248 insertions(+), 33 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/constraints/constraint_repo.cpp b/src/libs/navgraph/constraints/constraint_repo.cpp
index c9b309c..e038f28 100644
--- a/src/libs/navgraph/constraints/constraint_repo.cpp
+++ b/src/libs/navgraph/constraints/constraint_repo.cpp
@@ -78,6 +78,17 @@ NavGraphConstraintRepo::register_constraint(NavGraphEdgeCostConstraint* constrai
   edge_cost_constraints_.push_back(constraint);
 }
 
+/** Register a constraint.
+ * @param constraint trajectory-node constraint to register
+ */
+void
+NavGraphConstraintRepo::register_constraint(NavGraphTrajectoryNodeConstraint* constraint)
+{
+  modified_ = true;
+  trajectory_node_constraints_.push_back(constraint);
+}
+
+
 
 /** Unregister a constraint by name.
  * @param name name of constraint to remove.
@@ -113,6 +124,16 @@ NavGraphConstraintRepo::unregister_constraint(std::string name)
   if (ecc != edge_cost_constraints_.end()) {
     edge_cost_constraints_.erase(ecc);
   }
+
+  TrajectoryNodeConstraintList::iterator tnc =
+    std::find_if(trajectory_node_constraints_.begin(), trajectory_node_constraints_.end(),
+		 [&name](const NavGraphTrajectoryNodeConstraint *c) {
+		   return *c == name;
+		 });
+  if (tnc != trajectory_node_constraints_.end()) {
+	  trajectory_node_constraints_.erase(tnc);
+  }
+
 }
 
 
@@ -145,6 +166,13 @@ NavGraphConstraintRepo::has_constraint(std::string &name)
 		 });
   if (ecc != edge_cost_constraints_.end()) return true;
 
+  TrajectoryNodeConstraintList::iterator tnc =
+    std::find_if(trajectory_node_constraints_.begin(), trajectory_node_constraints_.end(),
+		 [&name](const NavGraphTrajectoryNodeConstraint *c) {
+		   return *c == name;
+		 });
+  if (tnc != trajectory_node_constraints_.end()) return true;
+
   return false;
 }
 
@@ -207,6 +235,25 @@ NavGraphConstraintRepo::get_edge_cost_constraint(std::string &name)
   return NULL;
 }
 
+/** Get an trajectory node constraint by name.
+ * @param name name of constraint to retrieve
+ * @return if found returns a pointer to the trajectory node constraint, NULL if not found
+ */
+fawkes::NavGraphTrajectoryNodeConstraint *
+NavGraphConstraintRepo::get_trajectory_node_constraint(std::string &name)
+{
+  TrajectoryNodeConstraintList::iterator it =
+    std::find_if(trajectory_node_constraints_.begin(), trajectory_node_constraints_.end(),
+		 [&name](const NavGraphTrajectoryNodeConstraint *c) {
+		   return *c == name;
+		 });
+  if (it != trajectory_node_constraints_.end()) {
+    return *it;
+  }
+
+  return NULL;
+}
+
 
 /** Get a list of registered node constraints.
  * @return list of node constraints
@@ -236,6 +283,14 @@ NavGraphConstraintRepo::edge_cost_constraints() const
   return edge_cost_constraints_;
 }
 
+/** Get a list of registered trajectory node constraints.
+ * @return list of trajectory node constraints
+ */
+const NavGraphConstraintRepo::TrajectoryNodeConstraintList &
+NavGraphConstraintRepo::trajectory_node_constraints() const
+{
+  return trajectory_node_constraints_;
+}
 
 /** Check if there are any constraints at all.
  * @return true if constraints have been registered, false otherwise
@@ -245,7 +300,8 @@ NavGraphConstraintRepo::has_constraints() const
 {
   return (! (node_constraints_.empty() &&
 	     edge_constraints_.empty() &&
-	     edge_cost_constraints_.empty()));
+	     edge_cost_constraints_.empty() &&
+	     trajectory_node_constraints_.empty() ));
 }
 
 
@@ -265,6 +321,9 @@ NavGraphConstraintRepo::compute()
   for (fawkes::NavGraphEdgeCostConstraint *c : edge_cost_constraints_) {
     if (c->compute())  modified = true;
   }
+  for (fawkes::NavGraphTrajectoryNodeConstraint *c : trajectory_node_constraints_) {
+    if (c->compute())  modified = true;
+  }
 
   return modified;
 }
@@ -287,6 +346,12 @@ NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node)
   return NULL;
 }
 
+/** Check if any constraint in the repo blocks a node at a certain timepoint.
+ * @param node Node to check for a block
+ * @param Estimated_time time estimate at which the node will be blocked
+ * @return the (first) node constraint that blocked the node,
+ * NULL if the node is not blocked
+ */
 
 /** Check if any constraint in the repo blocks (some) nodes.
  * @param nodes vector of nodes to check for a block
@@ -309,7 +374,6 @@ NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphNode> &nodes)
   return rv;
 }
 
-
 /** Check if any constraint in the repo blocks the edge.
  * @param from node from which the edge originates
  * @param to node to which the edge leads
@@ -329,6 +393,18 @@ NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &from,
   return NULL;
 }
 
+fawkes::NavGraphTrajectoryNodeConstraint *
+NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node, const float estimated_time){
+
+	for (fawkes::NavGraphTrajectoryNodeConstraint *c : trajectory_node_constraints_) {
+	    if (c->blocks(node, estimated_time)) {
+	      return c;
+	    }
+	  }
+
+	  return NULL;
+}
+
 /** Check if any constraint in the repo increases the cost of the edge.
  * @param from node from which the edge originates
  * @param to node to which the edge leads
@@ -348,7 +424,6 @@ NavGraphConstraintRepo::increases_cost(const fawkes::NavGraphNode &from,
   return NULL;
 }
 
-
 /** Check if any constraint in the repo increases the cost of the edge.
  * @param from node from which the edge originates
  * @param to node to which the edge leads
@@ -401,8 +476,6 @@ NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphEdge> &edges)
   return rv;
 }
 
-
-
 /** Get the highest increasing cost factor for an edge.
  * This methods goes through all of the given edges and queries all
  * edge cost constraints. If any constraint increases the cost of an
diff --git a/src/libs/navgraph/constraints/constraint_repo.h b/src/libs/navgraph/constraints/constraint_repo.h
index 16053df..02d64e0 100644
--- a/src/libs/navgraph/constraints/constraint_repo.h
+++ b/src/libs/navgraph/constraints/constraint_repo.h
@@ -25,6 +25,7 @@
 #include <navgraph/constraints/node_constraint.h>
 #include <navgraph/constraints/edge_constraint.h>
 #include <navgraph/constraints/edge_cost_constraint.h>
+#include <navgraph/constraints/trajectory_node_constraint.h>
 
 #include <navgraph/navgraph_edge.h>
 
@@ -47,6 +48,8 @@ class NavGraphConstraintRepo
   typedef std::vector<fawkes::NavGraphEdgeConstraint*> EdgeConstraintList;
   /** List of navgraph edge cost constraints. */
   typedef std::vector<fawkes::NavGraphEdgeCostConstraint*> EdgeCostConstraintList;
+  /** List of navgraph trajectory node constraints. */
+  typedef std::vector<fawkes::NavGraphTrajectoryNodeConstraint*> TrajectoryNodeConstraintList;
 
   NavGraphConstraintRepo();
   ~NavGraphConstraintRepo();
@@ -54,16 +57,19 @@ class NavGraphConstraintRepo
   void register_constraint(NavGraphNodeConstraint *constraint);
   void register_constraint(NavGraphEdgeConstraint *constraint);
   void register_constraint(NavGraphEdgeCostConstraint *constraint);
+  void register_constraint(NavGraphTrajectoryNodeConstraint* constraint);
   void unregister_constraint(std::string name);
 
   bool has_constraint(std::string &name);
   fawkes::NavGraphNodeConstraint *      get_node_constraint(std::string &name);
   fawkes::NavGraphEdgeConstraint *      get_edge_constraint(std::string &name);
   fawkes::NavGraphEdgeCostConstraint *  get_edge_cost_constraint(std::string &name);
+  fawkes::NavGraphTrajectoryNodeConstraint *  get_trajectory_node_constraint(std::string &name);
 
   const NodeConstraintList &      node_constraints() const;
   const EdgeConstraintList &      edge_constraints() const;
   const EdgeCostConstraintList &  edge_cost_constraints() const;
+  const TrajectoryNodeConstraintList &  trajectory_node_constraints() const;
 
   bool has_constraints() const;
 
@@ -73,6 +79,9 @@ class NavGraphConstraintRepo
   NavGraphEdgeConstraint *     blocks(const fawkes::NavGraphNode &from,
 				      const fawkes::NavGraphNode &to);
 
+  NavGraphTrajectoryNodeConstraint *     blocks(const fawkes::NavGraphNode &node,
+		  	  	  	  const float estimated_time);
+
   NavGraphEdgeCostConstraint * increases_cost(const fawkes::NavGraphNode &from,
 					      const fawkes::NavGraphNode &to);
 
@@ -95,10 +104,10 @@ class NavGraphConstraintRepo
   bool modified(bool reset_modified = false);
 
  private:
-
-  NodeConstraintList node_constraints_;
-  EdgeConstraintList edge_constraints_;
-  EdgeCostConstraintList edge_cost_constraints_;
+  NodeConstraintList 			node_constraints_;
+  EdgeConstraintList 			edge_constraints_;
+  EdgeCostConstraintList 		edge_cost_constraints_;
+  TrajectoryNodeConstraintList 	trajectory_node_constraints_;
   bool    modified_;
 };
 } // namespace
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
index b9a5ae9..c4087bc 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
@@ -33,6 +33,17 @@ namespace fawkes{
  * @author Sebastian Reuter
  */
 
+/** Constructor.
+ * @param name name of node constraint
+ */
+NavGraphTrajectoryNodeConstraint::NavGraphTrajectoryNodeConstraint(Logger *logger, std::string name, fawkes::Clock *clock)
+  : NavGraphNodeConstraint(name)
+{
+  logger_ = logger;
+  clock_ = clock;
+  modified_ = false;
+}
+
 
 /** Constructor.
  * @param name name of node constraint
@@ -63,20 +74,60 @@ NavGraphTrajectoryNodeConstraint::~NavGraphTrajectoryNodeConstraint()
 {
 }
 
+void
+NavGraphTrajectoryNodeConstraint::print_Constraint(){
+
+	logger_->log_info("TarjectoryNodeConstraint", "Trajectory is:");
+
+	for(auto &node : node_list_){
+		std::string output;
+		output += node.node.name();
+		output += " at ";
+		output += std::to_string( node.estimated_time );
+
+		logger_->log_info("TarjectoryNodeConstraint", "%s", output.c_str());
+	}
+
+}
 
 bool
 NavGraphTrajectoryNodeConstraint::compute(void) throw()
 {
-  // do nothing but  return feedback
+  print_Constraint();
+
+  // check and erase old TrajectoryNodes
+  fawkes::Time now(clock_);
+  for( std::vector<TrajectoryNode>::iterator it = node_list_.begin(); it == node_list_.end(); it++){
+	  if(now > it->estimated_time) {
+		logger_->log_info("TarjectoryNodeConstraint", "Delete node %s as its time %f is too old", it->node.name().c_str(), it->estimated_time);
+		node_list_.erase(it);
+	  }
+  }
 
   if (modified_) {
     modified_ = false;
+
+    print_Constraint();
+
     return true;
   } else {
     return false;
   }
 }
 
+/** Add a single trajectory-node to constraint list.
+ * @param node trajectory-node to add to constraint list
+ */
+void
+NavGraphTrajectoryNodeConstraint::add_node(const NavGraphNode &node, const fawkes::Time estimated_time)
+{
+  if (! has_node(node)) {
+    modified_ = true;
+    TrajectoryNode tnode(node,estimated_time.in_sec());
+    node_list_.push_back(tnode);
+  }
+}
+
 
 /** Add a single trajectory-node to constraint list.
  * @param node trajectory-node to add to constraint list
@@ -123,7 +174,7 @@ void
 NavGraphTrajectoryNodeConstraint::remove_node(const TrajectoryNode &node)
 {
   for( std::vector<TrajectoryNode>::iterator n = node_list_.begin(); n == node_list_.end(); n++){
-	  if( node.node == n->node && node.time == n->time ){
+	  if( node.node == n->node && node.estimated_time == n->estimated_time ){
 		    modified_ = true;
 		    node_list_.erase(n);
 		    continue;
@@ -136,10 +187,16 @@ NavGraphTrajectoryNodeConstraint::remove_node(const TrajectoryNode &node)
  * @return true if node is in list, false otherwise
  */
 bool
-NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node)
+NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, float time)
 {
+	/*
+	 *
+	 * TODO:
+	 * Create range - puffer before and after the estimated time
+	 *
+	 */
   for(auto& n : node_list_){
-	  if(node == n.node){
+	  if(node == n.node && time == n.estimated_time){
 		  return true;
 	  }
   }
@@ -153,7 +210,13 @@ NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node)
 bool
 NavGraphTrajectoryNodeConstraint::has_node(const TrajectoryNode &trajectory_node)
 {
-	  for(auto& n : node_list_){
+	/*
+	 *
+	 * TODO:
+	 * Create range - puffer before and after the estimated time
+	 *
+	 */
+	for(auto& n : node_list_){
 		  if(trajectory_node.node == n.node){
 			  return true;
 		  }
@@ -161,6 +224,28 @@ NavGraphTrajectoryNodeConstraint::has_node(const TrajectoryNode &trajectory_node
 	  return false;
 }
 
+/** Check if constraint has a specific node.
+ * @param node node to check
+ * @return true if node is in list, false otherwise
+ */
+bool
+NavGraphTrajectoryNodeConstraint::has_node(const NavGraphNode &node)
+{
+	/*
+	 *
+	 * TODO:
+	 * Create range - puffer before and after the estimated time
+	 *
+	 */
+	for(auto& n : node_list_){
+		  if(node == n.node){
+			  return true;
+		  }
+	  }
+	  return false;
+}
+
+
 
 /** Get list of blocked trajectory-nodes.
  * @return list of blocked nodes
@@ -174,7 +259,7 @@ NavGraphTrajectoryNodeConstraint::node_list() const
 
 /** Remove all trajectory-nodes. */
 void
-NavGraphTrajectoryNodeConstraint::clear_nodes()
+NavGraphTrajectoryNodeConstraint::clear_trajectory()
 {
   if (! node_list_.empty()) {
     modified_ = true;
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.h b/src/libs/navgraph/constraints/trajectory_node_constraint.h
index ac86806..7f783be 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.h
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.h
@@ -27,6 +27,8 @@
 #include <vector>
 #include <string>
 #include <utils/time/time.h>
+#include <logging/logger.h>
+
 
 #include <navgraph/navgraph.h>
 
@@ -41,13 +43,15 @@ class NavGraphTrajectoryNodeConstraint : public NavGraphNodeConstraint
   typedef struct TrajectoryNode_ {
 	 /** Constructor.
 	   * @param node node on trajectory
-	   * @param time estimated time on node
+	   * @param estimated_time estimated time on node
 	 */
-	 TrajectoryNode_ (fawkes::NavGraphNode node, fawkes::Time	time) : node(node), time(time) {}
+	 TrajectoryNode_ (fawkes::NavGraphNode node, float	estimated_time) : node(node), estimated_time(estimated_time) {}
      fawkes::NavGraphNode 		node;
-     fawkes::Time		  		time;
+     float		  				estimated_time;
   } TrajectoryNode;
 
+  NavGraphTrajectoryNodeConstraint(Logger *logger, std::string name, fawkes::Clock *clock);
+
   NavGraphTrajectoryNodeConstraint(std::string name);
 
   NavGraphTrajectoryNodeConstraint(std::string name,
@@ -57,25 +61,36 @@ class NavGraphTrajectoryNodeConstraint : public NavGraphNodeConstraint
 
   const std::vector<TrajectoryNode> &  node_list() const;
 
+  void add_node(const NavGraphNode &node, const fawkes::Time estimated_time);
   void add_node(const TrajectoryNode &trajectory_node);
   void add_nodes(const std::vector<TrajectoryNode> &trajectory_node);
 
   void remove_node(const TrajectoryNode &node);
   void remove_node(const fawkes::NavGraphNode &node);
-  void clear_nodes();
 
+  void clear_trajectory();
+
+  bool has_node(const NavGraphNode &node);
   bool has_node(const TrajectoryNode &trajectory_node);
-  bool has_node(const fawkes::NavGraphNode &node);
+  bool has_node(const fawkes::NavGraphNode &node, float time);
 
   virtual bool compute(void) throw();
-
   virtual bool blocks(const fawkes::NavGraphNode &node) throw()
+    { return has_node(node); }
+
+  bool blocks(const fawkes::NavGraphNode &node, float time) throw()
+  { return has_node(node, time); }
+  bool blocks(const TrajectoryNode &node) throw()
   { return has_node(node); }
 
  protected:
   std::vector<TrajectoryNode> node_list_;	///< Node list
   bool modified_;	///< Set to true if changes are made to the constraint.
 
+  fawkes::Clock *clock_;
+  Logger *logger_;
+  void print_Constraint();
+
 };
 
 } // end namespace fawkes
diff --git a/src/libs/navgraph/navgraph_path.cpp b/src/libs/navgraph/navgraph_path.cpp
index ed214b0..3c6aaea 100644
--- a/src/libs/navgraph/navgraph_path.cpp
+++ b/src/libs/navgraph/navgraph_path.cpp
@@ -119,12 +119,15 @@ NavGraphPath::operator==(const NavGraphPath &p) const
  * ignored if it is less than zero.
  */
 void
-NavGraphPath::add_node(const NavGraphNode &node, float cost_from_end)
+NavGraphPath::add_node(const NavGraphNode &node, float cost_from_end, float estimated_time)
 {
   nodes_.push_back(node);
   if (cost_from_end > 0) {
     cost_ += cost_from_end;
   }
+  if( estimated_time > 0){
+	times_.push_back(estimated_time);
+  }
   name_ += node.name();
 }
 
@@ -154,6 +157,8 @@ NavGraphPath::set_nodes(const std::vector<NavGraphNode> &nodes, float cost)
 bool
 NavGraphPath::empty() const
 {
+  times_.empty();
+
   return nodes_.empty();
 }
 
@@ -174,6 +179,7 @@ NavGraphPath::size() const
 void
 NavGraphPath::clear()
 {
+  times_.clear();
   nodes_.clear();
   cost_ = -1;
 }
diff --git a/src/libs/navgraph/navgraph_path.h b/src/libs/navgraph/navgraph_path.h
index 8f7544b..bcad8e3 100644
--- a/src/libs/navgraph/navgraph_path.h
+++ b/src/libs/navgraph/navgraph_path.h
@@ -79,7 +79,7 @@ class NavGraphPath {
   NavGraphPath(); 
   NavGraphPath(const NavGraph *graph, std::vector<NavGraphNode> &nodes, float cost = -1);
 
-  void add_node(const NavGraphNode &node, float cost_from_end = 0);
+  void add_node(const NavGraphNode &node, float cost_from_end = 0, float estimated_time = 0);
   void set_nodes(const std::vector<NavGraphNode> &nodes, float cost = -1);
 
   const NavGraph &     graph() const;
@@ -95,6 +95,12 @@ class NavGraphPath {
   const std::vector<NavGraphNode> &  nodes() const
   { return nodes_; }
 
+  /** Get times of nodes along the path.
+   * @return sequence of timed nodes that compose the path
+   */
+  const std::vector<float> &  times() const
+  { return times_; }
+
   /** Get nodes along the path as mutable vector.
    * Use this with caution. Modifying the nodes invalidates any
    * running traversal.
@@ -129,6 +135,7 @@ class NavGraphPath {
  private:
   const NavGraph            *graph_;
   std::vector<NavGraphNode>  nodes_;
+  std::vector<float>  		 times_;
   float                      cost_;
   std::string                name_;
 };
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index c66a738..0d2731d 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -54,6 +54,7 @@ namespace fawkes {
 NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
                                          const NavGraphNode &node,
                                          const NavGraphNode &goal,
+                                         float estimated_time,
                                          double cost_sofar,
                                          NavGraphSearchState *parent_state,
                                          NavGraph *map_graph,
@@ -63,7 +64,7 @@ NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
   : AStarState(cost_sofar, parent), path_(path),
     goal_(goal), estimate_func_(estimate_func), cost_func_(cost_func)
 {
-  path_.add_node(node);
+  path_.add_node(node, 0.0, estimated_time);
   map_graph_ = map_graph;
 
   total_estimated_cost = path_cost + estimate();
@@ -74,7 +75,6 @@ NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
   constraint_repo_ = constraint_repo;
 }
 
-
 /** Constructor.
  * @param path graph path this search state represents
  * @param goal graph node of the goal
@@ -158,6 +158,17 @@ NavGraphSearchState::estimate()
   return estimate_func_(path_, goal_);
 }
 
+float
+NavGraphSearchState::estimate_time(fawkes::NavGraphPath &path, fawkes::NavGraphNode &n)
+{
+
+	  float max_v = 3.0;
+	  float time = max_v * (sqrtf(powf(n.x() - path.back().x(), 2) +
+	                       powf(n.y() - path.back().y(), 2) ) );
+	  return time;
+
+}
+
 
 bool
 NavGraphSearchState::is_goal()
@@ -177,26 +188,31 @@ NavGraphSearchState::children()
   for (unsigned int i = 0; i < descendants.size(); ++i) {
     NavGraphNode d = map_graph_->node(descendants[i]);
 
+    float d_time = path_.times().back() + estimate_time(path_, d);
+
     bool expand = true;
-    if (constraint_repo_) {
-      if (constraint_repo_->blocks(d)) {
-	expand = false;
-      } else if (constraint_repo_->blocks(path_.back(), d)) {
-	expand = false;
-      }
+    /*
+     * check for all possible constraints of node
+     *    given path so far
+     *    path enables to check node as well as edge constraint
+     *    path holds times - thus, it enables to check for trajectory constraints
+     */
+    if (constraint_repo_ && constraint_repo_->blocks(d, d_time)) {
+	    expand = false;
     }
 
     if (expand) {
       float d_cost = cost_func_(path_, d);
 
       if (constraint_repo_) {
+
         float cost_factor = 0.;
         if (constraint_repo_->increases_cost(path_.back(), d, cost_factor)) {
           d_cost *= cost_factor;
         }
       }
 
-      children.push_back(new NavGraphSearchState(path_, d, goal_, path_cost + d_cost, this,
+      children.push_back(new NavGraphSearchState(path_, d, goal_, d_time, path_cost + d_cost, this,
 						 map_graph_, estimate_func_, cost_func_, constraint_repo_));
     }
   }
diff --git a/src/libs/navgraph/search_state.h b/src/libs/navgraph/search_state.h
index c9f2ea0..a4961da 100644
--- a/src/libs/navgraph/search_state.h
+++ b/src/libs/navgraph/search_state.h
@@ -66,6 +66,8 @@ class NavGraphSearchState : public fawkes::AStarState
   virtual float  estimate();
   virtual bool   is_goal();
 
+   float estimate_time(fawkes::NavGraphPath &path, fawkes::NavGraphNode &n);
+
   /** Determine euclidean cost between two nodes.
    * Note that the given notes are assumed to be adjacent nodes.
    * @param from originating path
@@ -97,7 +99,9 @@ class NavGraphSearchState : public fawkes::AStarState
   NavGraphSearchState(const fawkes::NavGraphPath &path,
                       const fawkes::NavGraphNode &node,
                       const fawkes::NavGraphNode &goal,
-                      double cost_sofar, NavGraphSearchState *parent_state,
+                      float estimated_time,
+                      double cost_sofar,
+                      NavGraphSearchState *parent_state,
                       fawkes::NavGraph *map_graph,
                       navgraph::EstimateFunction estimate_func,
                       navgraph::CostFunction cost_func,

- *commit* d85bc00d7970921c7f851e2e777326189bb23c3c - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Tue Jul 21 09:31:36 2015 +0800
Subject: navgraph-timed: remove trajectory-nodes when estimated time has passed

 .../constraints/trajectory_node_constraint.cpp     |   78 ++++++--------------
 .../constraints/trajectory_node_constraint.h       |   16 ++--
 2 files changed, 31 insertions(+), 63 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
index c4087bc..c0ab330 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
@@ -83,32 +83,32 @@ NavGraphTrajectoryNodeConstraint::print_Constraint(){
 		std::string output;
 		output += node.node.name();
 		output += " at ";
-		output += std::to_string( node.estimated_time );
+		output += std::to_string( node.estimated_time.in_sec() );
 
 		logger_->log_info("TarjectoryNodeConstraint", "%s", output.c_str());
 	}
+	logger_->log_info("TarjectoryNodeConstraint", "Current T:%f", fawkes::Time(clock_).in_sec());
 
 }
 
 bool
 NavGraphTrajectoryNodeConstraint::compute(void) throw()
 {
-  print_Constraint();
 
-  // check and erase old TrajectoryNodes
+  // check for trajectory-node whether time has passed
   fawkes::Time now(clock_);
-  for( std::vector<TrajectoryNode>::iterator it = node_list_.begin(); it == node_list_.end(); it++){
-	  if(now > it->estimated_time) {
-		logger_->log_info("TarjectoryNodeConstraint", "Delete node %s as its time %f is too old", it->node.name().c_str(), it->estimated_time);
-		node_list_.erase(it);
-	  }
+  for( std::vector<TrajectoryNode>::iterator n = node_list_.begin(); n != node_list_.end();){
+    if(now > n->estimated_time){
+      modified_=true;
+      n = node_list_.erase(n);
+	}
+    else{
+      n++;
+    }
   }
 
   if (modified_) {
     modified_ = false;
-
-    print_Constraint();
-
     return true;
   } else {
     return false;
@@ -121,9 +121,9 @@ NavGraphTrajectoryNodeConstraint::compute(void) throw()
 void
 NavGraphTrajectoryNodeConstraint::add_node(const NavGraphNode &node, const fawkes::Time estimated_time)
 {
-  if (! has_node(node)) {
+  if (! has_node(node,estimated_time)) {
     modified_ = true;
-    TrajectoryNode tnode(node,estimated_time.in_sec());
+    TrajectoryNode tnode(node,estimated_time);
     node_list_.push_back(tnode);
   }
 }
@@ -153,33 +153,25 @@ NavGraphTrajectoryNodeConstraint::add_nodes(
   }
 }
 
-/** Remove a single node from the constraint list.
- * @param node node to remote
- */
-void
-NavGraphTrajectoryNodeConstraint::remove_node(const fawkes::NavGraphNode &node)
-{
-  for( std::vector<TrajectoryNode>::iterator n = node_list_.begin(); n == node_list_.end(); n++){
-	  if(node == n->node){
-		    modified_ = true;
-		    node_list_.erase(n);
-	  }
-  }
-}
-
 /** Remove a single trajectory-node from the constraint list.
  * @param node trajectory-node to remote
+ * @return bool returns whether or not the node could be found in the list
  */
-void
+bool
 NavGraphTrajectoryNodeConstraint::remove_node(const TrajectoryNode &node)
 {
-  for( std::vector<TrajectoryNode>::iterator n = node_list_.begin(); n == node_list_.end(); n++){
-	  if( node.node == n->node && node.estimated_time == n->estimated_time ){
+  for( std::vector<TrajectoryNode>::iterator n = node_list_.begin(); n != node_list_.end();){
+	  if( node.node.name() == n->node.name() && node.estimated_time == n->estimated_time ){
 		    modified_ = true;
 		    node_list_.erase(n);
-		    continue;
+		    return true;
 	  }
+	  else{
+      n++;
+    }
   }
+  return false;
+
 }
 
 /** Check if constraint has a specific node.
@@ -187,7 +179,7 @@ NavGraphTrajectoryNodeConstraint::remove_node(const TrajectoryNode &node)
  * @return true if node is in list, false otherwise
  */
 bool
-NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, float time)
+NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, const fawkes::Time time)
 {
 	/*
 	 *
@@ -224,28 +216,6 @@ NavGraphTrajectoryNodeConstraint::has_node(const TrajectoryNode &trajectory_node
 	  return false;
 }
 
-/** Check if constraint has a specific node.
- * @param node node to check
- * @return true if node is in list, false otherwise
- */
-bool
-NavGraphTrajectoryNodeConstraint::has_node(const NavGraphNode &node)
-{
-	/*
-	 *
-	 * TODO:
-	 * Create range - puffer before and after the estimated time
-	 *
-	 */
-	for(auto& n : node_list_){
-		  if(node == n.node){
-			  return true;
-		  }
-	  }
-	  return false;
-}
-
-
 
 /** Get list of blocked trajectory-nodes.
  * @return list of blocked nodes
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.h b/src/libs/navgraph/constraints/trajectory_node_constraint.h
index 7f783be..34de9b2 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.h
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.h
@@ -45,9 +45,9 @@ class NavGraphTrajectoryNodeConstraint : public NavGraphNodeConstraint
 	   * @param node node on trajectory
 	   * @param estimated_time estimated time on node
 	 */
-	 TrajectoryNode_ (fawkes::NavGraphNode node, float	estimated_time) : node(node), estimated_time(estimated_time) {}
+	 TrajectoryNode_ (fawkes::NavGraphNode node, fawkes::Time	estimated_time) : node(node), estimated_time(estimated_time) {}
      fawkes::NavGraphNode 		node;
-     float		  				estimated_time;
+     fawkes::Time  				estimated_time;
   } TrajectoryNode;
 
   NavGraphTrajectoryNodeConstraint(Logger *logger, std::string name, fawkes::Clock *clock);
@@ -65,23 +65,21 @@ class NavGraphTrajectoryNodeConstraint : public NavGraphNodeConstraint
   void add_node(const TrajectoryNode &trajectory_node);
   void add_nodes(const std::vector<TrajectoryNode> &trajectory_node);
 
-  void remove_node(const TrajectoryNode &node);
-  void remove_node(const fawkes::NavGraphNode &node);
+  bool remove_node(const TrajectoryNode &node);
 
   void clear_trajectory();
 
-  bool has_node(const NavGraphNode &node);
   bool has_node(const TrajectoryNode &trajectory_node);
-  bool has_node(const fawkes::NavGraphNode &node, float time);
+  bool has_node(const fawkes::NavGraphNode &node, const fawkes::Time time);
 
   virtual bool compute(void) throw();
   virtual bool blocks(const fawkes::NavGraphNode &node) throw()
-    { return has_node(node); }
+    { return false; }
 
   bool blocks(const fawkes::NavGraphNode &node, float time) throw()
   { return has_node(node, time); }
-  bool blocks(const TrajectoryNode &node) throw()
-  { return has_node(node); }
+  bool blocks(const TrajectoryNode &tnode) throw()
+  { return has_node(tnode); }
 
  protected:
   std::vector<TrajectoryNode> node_list_;	///< Node list

- *commit* 4147781f1acc2c514c52042b20ed3bcd32b71270 - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Fri Jul 31 17:31:02 2015 +0800
Subject: navgraph-timed: new search-state constructor gets etimated-time

 src/libs/navgraph/search_state.cpp |   53 +++++++++++++++++++++++++-----------
 src/libs/navgraph/search_state.h   |   26 ++++++++++-------
 2 files changed, 52 insertions(+), 27 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index 0d2731d..a995634 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -54,7 +54,7 @@ namespace fawkes {
 NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
                                          const NavGraphNode &node,
                                          const NavGraphNode &goal,
-                                         float estimated_time,
+                                         const fawkes::Time &estimated_time,
                                          double cost_sofar,
                                          NavGraphSearchState *parent_state,
                                          NavGraph *map_graph,
@@ -62,7 +62,7 @@ NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
                                          navgraph::CostFunction cost_func,
                                          fawkes::NavGraphConstraintRepo *constraint_repo)
   : AStarState(cost_sofar, parent), path_(path),
-    goal_(goal), estimate_func_(estimate_func), cost_func_(cost_func)
+    goal_(goal), estimated_time_(estimated_time), estimate_func_(estimate_func), cost_func_(cost_func)
 {
   path_.add_node(node, 0.0, estimated_time);
   map_graph_ = map_graph;
@@ -142,8 +142,8 @@ NavGraphSearchState::~NavGraphSearchState()
 }
 
 
-/** Get graph node corresponding to this search state.
- * @return graph node corresponding to this search state
+/** Get path corresponding to this search state.
+ * @return path path corresponding to this search state
  */
 fawkes::NavGraphPath &
 NavGraphSearchState::path()
@@ -151,32 +151,48 @@ NavGraphSearchState::path()
   return path_;
 }
 
-
+/** Estimate costs-to-go till goal.
+ * @return estimate estimate of the costs to the goal
+ */
 float
 NavGraphSearchState::estimate()
 {
   return estimate_func_(path_, goal_);
 }
 
-float
+/** Estimate time from last node on path to next node.
+ * @param path path of the path-plan so far
+ * @param node next node on the path
+ * @return time estimated time estimate the robots needs till the next node
+ */
+fawkes::Time
 NavGraphSearchState::estimate_time(fawkes::NavGraphPath &path, fawkes::NavGraphNode &n)
 {
+	//TESTING CODE - HARDCODED VELOCITY
 
-	  float max_v = 3.0;
-	  float time = max_v * (sqrtf(powf(n.x() - path.back().x(), 2) +
+	  double max_v = 3.0;
+	  double time = max_v * (sqrtf(powf(n.x() - path.back().x(), 2) +
 	                       powf(n.y() - path.back().y(), 2) ) );
-	  return time;
 
+	  return (path.times().back() + time);
 }
 
 
+/** Test if current node is goal-node.
+ * @return true true if the current node is goal-node, false otherwise
+ */
 bool
 NavGraphSearchState::is_goal()
 {
   return (path_.nodes().back().name() == goal_.name());
 }
 
-
+/** Get children-state of current-state.
+ * The children are evaluated whether or not they are constrained.
+ * Children with blocking-constraints are not returned.
+ * Children with cost-increasing-constraints are increased by a defined cost factor
+ * @return children list of children-states
+ */
 std::vector<AStarState *>
 NavGraphSearchState::children()
 {
@@ -188,19 +204,24 @@ NavGraphSearchState::children()
   for (unsigned int i = 0; i < descendants.size(); ++i) {
     NavGraphNode d = map_graph_->node(descendants[i]);
 
-    float d_time = path_.times().back() + estimate_time(path_, d);
+    fawkes::Time d_time = path_.times().back() + estimate_time(path_, d);
 
     bool expand = true;
+
     /*
-     * check for all possible constraints of node
-     *    given path so far
-     *    path enables to check node as well as edge constraint
-     *    path holds times - thus, it enables to check for trajectory constraints
+     * check if constraint_repo should be used
+     * check if there is a blocking-constraint (node,edge) for the node
      */
-    if (constraint_repo_ && constraint_repo_->blocks(d, d_time)) {
+    if (constraint_repo_ and constraint_repo_->blocks(path_, d, d_time)) {
 	    expand = false;
     }
 
+    /*
+	 * calculate costs by cost-function
+	 * check if there is a constraint that increases the cost of the state
+	 * create a new children-state
+	 * add children to children-vector and return vector
+	 */
     if (expand) {
       float d_cost = cost_func_(path_, d);
 
diff --git a/src/libs/navgraph/search_state.h b/src/libs/navgraph/search_state.h
index a4961da..b93d4e3 100644
--- a/src/libs/navgraph/search_state.h
+++ b/src/libs/navgraph/search_state.h
@@ -26,6 +26,7 @@
 #include <navgraph/constraints/constraint_repo.h>
 #include <navgraph/navgraph.h>
 #include <core/utils/lockptr.h>
+#include <utils/time/time.h>
 
 #include <functional>
 #include <cmath>
@@ -66,7 +67,7 @@ class NavGraphSearchState : public fawkes::AStarState
   virtual float  estimate();
   virtual bool   is_goal();
 
-   float estimate_time(fawkes::NavGraphPath &path, fawkes::NavGraphNode &n);
+   fawkes::Time estimate_time(fawkes::NavGraphPath &path, fawkes::NavGraphNode &n);
 
   /** Determine euclidean cost between two nodes.
    * Note that the given notes are assumed to be adjacent nodes.
@@ -96,16 +97,16 @@ class NavGraphSearchState : public fawkes::AStarState
   }
 
  private:
-  NavGraphSearchState(const fawkes::NavGraphPath &path,
-                      const fawkes::NavGraphNode &node,
-                      const fawkes::NavGraphNode &goal,
-                      float estimated_time,
-                      double cost_sofar,
-                      NavGraphSearchState *parent_state,
-                      fawkes::NavGraph *map_graph,
-                      navgraph::EstimateFunction estimate_func,
-                      navgraph::CostFunction cost_func,
-                      fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
+  NavGraphSearchState(const NavGraphPath &path,
+					  const NavGraphNode &node,
+					  const NavGraphNode &goal,
+					  const fawkes::Time &estimated_time,
+					  double cost_sofar,
+					  NavGraphSearchState *parent_state,
+					  NavGraph *map_graph,
+					  navgraph::EstimateFunction estimate_func,
+					  navgraph::CostFunction cost_func,
+					  fawkes::NavGraphConstraintRepo *constraint_repo = NULL);
 
  private:
   std::vector<AStarState *> children();
@@ -116,6 +117,9 @@ class NavGraphSearchState : public fawkes::AStarState
   // goal information
   fawkes::NavGraphNode  goal_;
 
+  // estimated time
+  fawkes::Time estimated_time_;
+
   fawkes::NavGraph *map_graph_;
 
   fawkes::NavGraphConstraintRepo *constraint_repo_;

- *commit* 8b1e8874e608f471914f59e4ba912a65738b8797 - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Fri Jul 31 17:33:37 2015 +0800
Subject: navgraph-timed: changed from flaot to fawkes::Time

 src/libs/navgraph/navgraph_path.cpp |   77 ++++++++++++++++++++++++++++++++--
 src/libs/navgraph/navgraph_path.h   |   24 +++++++++--
 2 files changed, 92 insertions(+), 9 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/navgraph_path.cpp b/src/libs/navgraph/navgraph_path.cpp
index 3c6aaea..e302be0 100644
--- a/src/libs/navgraph/navgraph_path.cpp
+++ b/src/libs/navgraph/navgraph_path.cpp
@@ -54,7 +54,6 @@ NavGraphPath::NavGraphPath()
   cost_ = -1;
 }
 
-
 /** Constructor.
  * @param graph navgraph this path is based on
  * @param nodes nodes that the path should follow. The nodes must build
@@ -66,6 +65,26 @@ NavGraphPath::NavGraphPath(const NavGraph *graph,
 			   std::vector<NavGraphNode> &nodes, float cost)
   : graph_(graph), nodes_(nodes), cost_(cost)
 {
+	// put elements into times_ vector to prevent missbehavior
+	for(uint i=0; i<nodes.size();){
+		times_.push_back(fawkes::Time(0,0));
+		i++; // to prevent warning message
+	}
+}
+
+
+/** Constructor.
+ * @param graph navgraph this path is based on
+ * @param nodes nodes that the path should follow.
+ * a sequence where each node is directly reachable from its predecessor.
+ * This is not verified internally.
+ * @param estimated time time estimates for the nodes along the path
+ * @param cost cost of the path, set to a value less than zero if unknown
+ */
+NavGraphPath::NavGraphPath(const NavGraph *graph, std::vector<NavGraphNode> &nodes,
+        					const std::vector<fawkes::Time> &estimated_time, float cost)
+  : graph_(graph), nodes_(nodes), times_(estimated_time), cost_(cost)
+{
 }
 
 /** Check if this path is cheaper than the other path.
@@ -119,13 +138,13 @@ NavGraphPath::operator==(const NavGraphPath &p) const
  * ignored if it is less than zero.
  */
 void
-NavGraphPath::add_node(const NavGraphNode &node, float cost_from_end, float estimated_time)
+NavGraphPath::add_node(const NavGraphNode &node, float cost_from_end, fawkes::Time estimated_time)
 {
   nodes_.push_back(node);
   if (cost_from_end > 0) {
     cost_ += cost_from_end;
   }
-  if( estimated_time > 0){
+  if( estimated_time > fawkes::Time(0,0)){
 	times_.push_back(estimated_time);
   }
   name_ += node.name();
@@ -140,10 +159,11 @@ NavGraphPath::add_node(const NavGraphNode &node, float cost_from_end, float esti
  * @param cost cost of the path, set to a value less than zero if unknown
  */
 void
-NavGraphPath::set_nodes(const std::vector<NavGraphNode> &nodes, float cost)
+NavGraphPath::set_nodes(const std::vector<NavGraphNode> &nodes, const std::vector<fawkes::Time> times, float cost)
 {
   nodes_        = nodes;
   cost_         = cost;
+  times_		= times;
   name_.clear();
   for (auto node : nodes_)
     name_ += node.name();
@@ -158,7 +178,6 @@ bool
 NavGraphPath::empty() const
 {
   times_.empty();
-
   return nodes_.empty();
 }
 
@@ -204,6 +223,54 @@ NavGraphPath::contains(const NavGraphNode &node) const
   return (std::find(nodes_.begin(), nodes_.end(), node) != nodes_.end());
 }
 
+/** Check if the trajectory contains a given node at a certain time.
+ * @param node node to check for in current path
+ * @param time time of node on the trajectory
+ * @return true if the node is contained in the current path at the spec time, false otherwise
+ */
+bool
+NavGraphPath::contains_at_time(const NavGraphNode &node, const fawkes::Time time) const
+{
+  for(uint i=0; i<nodes_.size(); i++){
+    if(nodes_[i].name() == node.name() && times_[i] == time){
+      return true;
+    }
+ }
+ return false;
+}
+
+/** Check if the trajectory contains a given node within a certain time-span.
+ * @param node node to check for in current path
+ * @param start_time start time of time-span
+ * @param end_time end time of time-span
+ * @return true if the node is contained in the current path in the spec time-span, false otherwise
+ */
+bool
+NavGraphPath::contains_in_timespan(const NavGraphNode &node, const fawkes::Time start_time, const fawkes::Time end_time) const
+{
+  for(uint i=0; i<nodes_.size(); i++){
+    if(nodes_[i].name() == node.name() && start_time < times_[i] && times_[i] < end_time ){
+      return true;
+    }
+ }
+ return false;
+}
+
+/** get time of node.
+ * @param node node along a path
+ * @return time time of node
+ */
+fawkes::Time
+NavGraphPath::time_at(const NavGraphNode &node) const
+{
+  for(uint i=0; i<nodes_.size(); i++){
+    if(nodes_[i].name() == node.name()){
+      return times_[i];
+    }
+ }
+ return fawkes::Time(0,0);
+}
+
 
 /** Get goal of path.
  * @return goal of this path, i.e. the last node in the sequence of nodes.
diff --git a/src/libs/navgraph/navgraph_path.h b/src/libs/navgraph/navgraph_path.h
index bcad8e3..ee6f1a4 100644
--- a/src/libs/navgraph/navgraph_path.h
+++ b/src/libs/navgraph/navgraph_path.h
@@ -28,6 +28,7 @@
 #include <vector>
 #include <string>
 #include <cstdlib>
+#include <utils/time/time.h>
 
 namespace fawkes {
 #if 0 /* just to make Emacs auto-indent happy */
@@ -78,9 +79,13 @@ class NavGraphPath {
 
   NavGraphPath(); 
   NavGraphPath(const NavGraph *graph, std::vector<NavGraphNode> &nodes, float cost = -1);
+  NavGraphPath(const NavGraph *graph, std::vector<NavGraphNode> &nodes,
+		        const std::vector<fawkes::Time> &estimated_time, float cost = -1);
 
-  void add_node(const NavGraphNode &node, float cost_from_end = 0, float estimated_time = 0);
-  void set_nodes(const std::vector<NavGraphNode> &nodes, float cost = -1);
+  void add_node(const NavGraphNode &node, float cost_from_end = 0,
+		           const fawkes::Time estimated_time = fawkes::Time(0,0));
+  void set_nodes(const std::vector<NavGraphNode> &nodes,
+		  	       const std::vector<fawkes::Time> times, float cost = -1);
 
   const NavGraph &     graph() const;
   const NavGraphNode & goal() const;
@@ -98,7 +103,7 @@ class NavGraphPath {
   /** Get times of nodes along the path.
    * @return sequence of timed nodes that compose the path
    */
-  const std::vector<float> &  times() const
+  const std::vector<fawkes::Time> &  times() const
   { return times_; }
 
   /** Get nodes along the path as mutable vector.
@@ -109,7 +114,15 @@ class NavGraphPath {
   std::vector<NavGraphNode> &  nodes_mutable()
   { return nodes_; }
 
+  /** Get times of nodes along the path as a mutable vector.
+   * @return sequence of timed nodes that compose the path
+   */
+  std::vector<fawkes::Time> &  times_mutable()
+  { return times_; }
+
   bool contains(const NavGraphNode &node) const;
+  bool contains_at_time(const NavGraphNode &node, fawkes::Time estimated_time) const;
+  bool contains_in_timespan(const NavGraphNode &node, fawkes::Time start_time, fawkes::Time end_time) const;
 
   bool   empty() const;
   size_t size() const;
@@ -133,9 +146,12 @@ class NavGraphPath {
   bool operator==(const NavGraphPath &p) const;
 
  private:
+  fawkes::Time time_at(const NavGraphNode &node) const;
+
+ private:
   const NavGraph            *graph_;
   std::vector<NavGraphNode>  nodes_;
-  std::vector<float>  		 times_;
+  std::vector<fawkes::Time>	 times_;
   float                      cost_;
   std::string                name_;
 };

- *commit* ff03885e6671b92c24b1e9337ca5af912e588cff - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Fri Jul 31 17:36:55 2015 +0800
Subject: navgraph-timed: changed to references in parameter list

 .../constraints/trajectory_node_constraint.cpp     |   70 +++++++++++++------
 .../constraints/trajectory_node_constraint.h       |   28 ++++----
 2 files changed, 63 insertions(+), 35 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
index c0ab330..9a06e35 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
@@ -37,8 +37,8 @@ namespace fawkes{
  * @param name name of node constraint
  */
 NavGraphTrajectoryNodeConstraint::NavGraphTrajectoryNodeConstraint(Logger *logger, std::string name, fawkes::Clock *clock)
-  : NavGraphNodeConstraint(name)
 {
+  name_ = name;
   logger_ = logger;
   clock_ = clock;
   modified_ = false;
@@ -49,8 +49,8 @@ NavGraphTrajectoryNodeConstraint::NavGraphTrajectoryNodeConstraint(Logger *logge
  * @param name name of node constraint
  */
 NavGraphTrajectoryNodeConstraint::NavGraphTrajectoryNodeConstraint(std::string name)
-  : NavGraphNodeConstraint(name)
 {
+  name_ = name;
   modified_ = false;
 }
 
@@ -63,8 +63,8 @@ NavGraphTrajectoryNodeConstraint::NavGraphTrajectoryNodeConstraint(std::string n
 NavGraphTrajectoryNodeConstraint::NavGraphTrajectoryNodeConstraint(
     std::string name,
     std::vector<TrajectoryNode> &node_list)
-  : NavGraphNodeConstraint(name)
 {
+  name_ = name;
   node_list_ = node_list;
   modified_  = false;
 }
@@ -83,7 +83,7 @@ NavGraphTrajectoryNodeConstraint::print_Constraint(){
 		std::string output;
 		output += node.node.name();
 		output += " at ";
-		output += std::to_string( node.estimated_time.in_sec() );
+		output += std::to_string( node.time.in_sec() );
 
 		logger_->log_info("TarjectoryNodeConstraint", "%s", output.c_str());
 	}
@@ -98,7 +98,7 @@ NavGraphTrajectoryNodeConstraint::compute(void) throw()
   // check for trajectory-node whether time has passed
   fawkes::Time now(clock_);
   for( std::vector<TrajectoryNode>::iterator n = node_list_.begin(); n != node_list_.end();){
-    if(now > n->estimated_time){
+    if(now > n->time){
       modified_=true;
       n = node_list_.erase(n);
 	}
@@ -119,25 +119,31 @@ NavGraphTrajectoryNodeConstraint::compute(void) throw()
  * @param node trajectory-node to add to constraint list
  */
 void
-NavGraphTrajectoryNodeConstraint::add_node(const NavGraphNode &node, const fawkes::Time estimated_time)
+NavGraphTrajectoryNodeConstraint::add_node(const NavGraphNode &node, const fawkes::Time &time)
 {
-  if (! has_node(node,estimated_time)) {
+  if (! has_node(node,time)) {
     modified_ = true;
-    TrajectoryNode tnode(node,estimated_time);
+    TrajectoryNode tnode(node,time);
     node_list_.push_back(tnode);
+
+	logger_->log_error("TrajectoryNodeConstraint", "Added trajectory-constraint for node %s at time %f", node.name().c_str(), time.in_sec() );
   }
 }
 
-
-/** Add a single trajectory-node to constraint list.
- * @param node trajectory-node to add to constraint list
+/** Add multiple trajectory-nodes to constraint list.
+ * @param nodes vector of nodes to add to constraint list
+ * @param times vector of times to add to constraint list
  */
 void
-NavGraphTrajectoryNodeConstraint::add_node(const TrajectoryNode &node)
+NavGraphTrajectoryNodeConstraint::add_nodes(const std::vector<NavGraphNode> &nodes, const std::vector<fawkes::Time> &times)
 {
-  if (! has_node(node)) {
-    modified_ = true;
-    node_list_.push_back(node);
+  if(nodes.size() == times.size()){
+	  for (uint i=0; i<nodes.size(); i++){
+		add_node(nodes[i],times[i]);
+	  }
+  }
+  else{
+	  logger_->log_error("TrajectoryNodeConstraint", "tried to add uneven number of nodes and corresponding times. Will do nothing!");
   }
 }
 
@@ -145,11 +151,10 @@ NavGraphTrajectoryNodeConstraint::add_node(const TrajectoryNode &node)
  * @param nodes trajectory-nodes to add to constraint list
  */
 void
-NavGraphTrajectoryNodeConstraint::add_nodes(
-  const std::vector<TrajectoryNode> &nodes)
+NavGraphTrajectoryNodeConstraint::add_nodes(const std::vector<TrajectoryNode> &nodes)
 {
-  for (const TrajectoryNode &n : nodes) {
-    add_node(n);
+  for(auto& node: nodes){
+		node_list_.push_back(node);
   }
 }
 
@@ -161,7 +166,7 @@ bool
 NavGraphTrajectoryNodeConstraint::remove_node(const TrajectoryNode &node)
 {
   for( std::vector<TrajectoryNode>::iterator n = node_list_.begin(); n != node_list_.end();){
-	  if( node.node.name() == n->node.name() && node.estimated_time == n->estimated_time ){
+	  if( node.node.name() == n->node.name() && node.time == n->time ){
 		    modified_ = true;
 		    node_list_.erase(n);
 		    return true;
@@ -179,7 +184,7 @@ NavGraphTrajectoryNodeConstraint::remove_node(const TrajectoryNode &node)
  * @return true if node is in list, false otherwise
  */
 bool
-NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, const fawkes::Time time)
+NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, const fawkes::Time &time)
 {
 	/*
 	 *
@@ -188,8 +193,9 @@ NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, con
 	 *
 	 */
   for(auto& n : node_list_){
-	  if(node == n.node && time == n.estimated_time){
+	  if(node == n.node && time == n.time){
 		  return true;
+		  logger_->log_error("TrajectoryNodeConstraint", "Blocking node %s at time %f", n.node.name().c_str(), n.time.in_sec());
 	  }
   }
   return false;
@@ -237,5 +243,25 @@ NavGraphTrajectoryNodeConstraint::clear_trajectory()
   }
 }
 
+/** Check if constraint matches name.
+ * @param name name string to compare this constraints name to
+ * @return true if the given name is the same as this constraint's name,
+ * false otherwise
+ */
+bool
+NavGraphTrajectoryNodeConstraint::operator==(const std::string &name) const
+{
+  return name_ == name;
+}
+
+/** Get name of constraint.
+ * @return name of constraint
+ */
+std::string
+NavGraphTrajectoryNodeConstraint::name()
+{
+  return name_;
+}
+
 
 } // end of namespace fawkes
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.h b/src/libs/navgraph/constraints/trajectory_node_constraint.h
index 34de9b2..68279da 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.h
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.h
@@ -37,17 +37,17 @@ namespace fawkes{
 }
 #endif
 
-class NavGraphTrajectoryNodeConstraint : public NavGraphNodeConstraint
+class NavGraphTrajectoryNodeConstraint
 {
  public:
   typedef struct TrajectoryNode_ {
 	 /** Constructor.
 	   * @param node node on trajectory
-	   * @param estimated_time estimated time on node
+	   * @param time estimated time on node
 	 */
-	 TrajectoryNode_ (fawkes::NavGraphNode node, fawkes::Time	estimated_time) : node(node), estimated_time(estimated_time) {}
+	 TrajectoryNode_ (fawkes::NavGraphNode node, fawkes::Time time) : node(node), time(time) {}
      fawkes::NavGraphNode 		node;
-     fawkes::Time  				estimated_time;
+     fawkes::Time  				time;
   } TrajectoryNode;
 
   NavGraphTrajectoryNodeConstraint(Logger *logger, std::string name, fawkes::Clock *clock);
@@ -57,12 +57,13 @@ class NavGraphTrajectoryNodeConstraint : public NavGraphNodeConstraint
   NavGraphTrajectoryNodeConstraint(std::string name,
 				   std::vector<TrajectoryNode> &node_list);
 
-  virtual ~NavGraphTrajectoryNodeConstraint();
+  ~NavGraphTrajectoryNodeConstraint();
 
   const std::vector<TrajectoryNode> &  node_list() const;
 
-  void add_node(const NavGraphNode &node, const fawkes::Time estimated_time);
+  void add_node(const NavGraphNode &node, const fawkes::Time &time);
   void add_node(const TrajectoryNode &trajectory_node);
+  void add_nodes(const std::vector<NavGraphNode> &trajectory_node, const std::vector<fawkes::Time> &times);
   void add_nodes(const std::vector<TrajectoryNode> &trajectory_node);
 
   bool remove_node(const TrajectoryNode &node);
@@ -70,20 +71,21 @@ class NavGraphTrajectoryNodeConstraint : public NavGraphNodeConstraint
   void clear_trajectory();
 
   bool has_node(const TrajectoryNode &trajectory_node);
-  bool has_node(const fawkes::NavGraphNode &node, const fawkes::Time time);
+  bool has_node(const fawkes::NavGraphNode &node, const fawkes::Time &time);
 
-  virtual bool compute(void) throw();
-  virtual bool blocks(const fawkes::NavGraphNode &node) throw()
-    { return false; }
+  bool compute(void) throw();
 
-  bool blocks(const fawkes::NavGraphNode &node, float time) throw()
+  bool blocks(const fawkes::NavGraphNode &node, const fawkes::Time &time) throw()
   { return has_node(node, time); }
-  bool blocks(const TrajectoryNode &tnode) throw()
-  { return has_node(tnode); }
+
+  std::string name();
+
+  bool operator==(const std::string &name) const;
 
  protected:
   std::vector<TrajectoryNode> node_list_;	///< Node list
   bool modified_;	///< Set to true if changes are made to the constraint.
+  std::string name_;
 
   fawkes::Clock *clock_;
   Logger *logger_;

- *commit* e36233f9f611bdec8031b18e2721c6003b8d0c94 - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Fri Jul 31 17:39:44 2015 +0800
Subject: navgraph-timed: constraint_repo.blocks use fawkes::Time

 src/libs/navgraph/constraints/constraint_repo.cpp |   63 ++++++++++++++++----
 src/libs/navgraph/constraints/constraint_repo.h   |   26 +++++---
 2 files changed, 66 insertions(+), 23 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/constraints/constraint_repo.cpp b/src/libs/navgraph/constraints/constraint_repo.cpp
index e038f28..e42c2f8 100644
--- a/src/libs/navgraph/constraints/constraint_repo.cpp
+++ b/src/libs/navgraph/constraints/constraint_repo.cpp
@@ -298,10 +298,10 @@ NavGraphConstraintRepo::trajectory_node_constraints() const
 bool
 NavGraphConstraintRepo::has_constraints() const
 {
-  return (! (node_constraints_.empty() &&
-	     edge_constraints_.empty() &&
-	     edge_cost_constraints_.empty() &&
-	     trajectory_node_constraints_.empty() ));
+  return (! (node_constraints_.empty()
+	         and edge_constraints_.empty()
+	         and edge_cost_constraints_.empty()
+	         and trajectory_node_constraints_.empty() ));
 }
 
 
@@ -328,6 +328,21 @@ NavGraphConstraintRepo::compute()
   return modified;
 }
 
+bool
+NavGraphConstraintRepo::blocks(const fawkes::NavGraphPath &path, const fawkes::NavGraphNode &node, const fawkes::Time &time){
+
+	if(   blocks(node)   						// evaluate node-constraint
+	   or blocks(path.back(), node) 	  		// evaluate edge-constraint
+	   or blocks(node, time)					// evaluate trajectory-node-constraint
+//	   or blocks(path.nodes().back(),node,		// evaluate trajectory-edge-constraint
+//	              path.times().back(),time)
+	  ){
+	    return true;
+	}
+
+	return false;
+}
+
 
 /** Check if any constraint in the repo blocks the node.
  * @param node Node to check for a block
@@ -346,13 +361,6 @@ NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node)
   return NULL;
 }
 
-/** Check if any constraint in the repo blocks a node at a certain timepoint.
- * @param node Node to check for a block
- * @param Estimated_time time estimate at which the node will be blocked
- * @return the (first) node constraint that blocked the node,
- * NULL if the node is not blocked
- */
-
 /** Check if any constraint in the repo blocks (some) nodes.
  * @param nodes vector of nodes to check for a block
  * @return map of blocked nodes, first element is the node name,
@@ -393,8 +401,14 @@ NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &from,
   return NULL;
 }
 
+/** Check if any constraint in the repo blocks a node at a certain timepoint.
+ * @param node Node to check for a block
+ * @param Estimated_time time estimate at which the node will be blocked
+ * @return the (first) node constraint that blocked the node,
+ * NULL if the node is not blocked
+ */
 fawkes::NavGraphTrajectoryNodeConstraint *
-NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node, const float estimated_time){
+NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node, const fawkes::Time &estimated_time){
 
 	for (fawkes::NavGraphTrajectoryNodeConstraint *c : trajectory_node_constraints_) {
 	    if (c->blocks(node, estimated_time)) {
@@ -402,9 +416,32 @@ NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &node, const float est
 	    }
 	  }
 
-	  return NULL;
+	return NULL;
 }
 
+/** Check if any constraint in the repo blocks an edge at a certain timepoint.
+ * @param from node from which the edge originates
+ * @param to node to which the edge leads
+ * @param first_time time of first node
+ * @param second_time time of second node
+ * @return the (first) edge constraint that blocked the node,
+ * NULL if the node is not blocked
+ */
+//fawkes::NavGraphTrajectoryEdgeConstraint *
+//NavGraphConstraintRepo::blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to
+//								const fawkes::Time first_time, const fawkes::Time second_time )
+//{
+//  for (fawkes::NavGraphTrajectoryEdgeConstraint *c : edge_constraints_) {
+//    if (c->blocks(from, to, first_time, second_time)) {
+//      return c;
+//    }
+//  }
+//
+//  return NULL;
+//}
+
+
+
 /** Check if any constraint in the repo increases the cost of the edge.
  * @param from node from which the edge originates
  * @param to node to which the edge leads
diff --git a/src/libs/navgraph/constraints/constraint_repo.h b/src/libs/navgraph/constraints/constraint_repo.h
index 02d64e0..b3aebd2 100644
--- a/src/libs/navgraph/constraints/constraint_repo.h
+++ b/src/libs/navgraph/constraints/constraint_repo.h
@@ -70,27 +70,33 @@ class NavGraphConstraintRepo
   const EdgeConstraintList &      edge_constraints() const;
   const EdgeCostConstraintList &  edge_cost_constraints() const;
   const TrajectoryNodeConstraintList &  trajectory_node_constraints() const;
+//  const TrajectoryEdgeConstraintList &  trajectory_edge_constraints() const;
 
   bool has_constraints() const;
 
   bool compute();
 
-  NavGraphNodeConstraint *     blocks(const fawkes::NavGraphNode &node);
-  NavGraphEdgeConstraint *     blocks(const fawkes::NavGraphNode &from,
-				      const fawkes::NavGraphNode &to);
+  bool blocks(const fawkes::NavGraphPath &path, const fawkes::NavGraphNode &node, const fawkes::Time &time = fawkes::Time(0,0));
 
-  NavGraphTrajectoryNodeConstraint *     blocks(const fawkes::NavGraphNode &node,
-		  	  	  	  const float estimated_time);
+  NavGraphNodeConstraint * blocks(const fawkes::NavGraphNode &node);
+  NavGraphEdgeConstraint * blocks(const fawkes::NavGraphNode &from,
+				      	  	  	  	  const fawkes::NavGraphNode &to);
+
+  NavGraphTrajectoryNodeConstraint * blocks(const fawkes::NavGraphNode &node,
+		  	  	  	  	  	  	  	  	  	  	const fawkes::Time &time);
+
+//  NavGraphTrajectoryEdgeConstraint * 	blocks(const fawkes::NavGraphNode &from, const fawkes::NavGraphNode &to
+//  					  const fawkes::Time first_time, const fawkes::Time second_time );
 
   NavGraphEdgeCostConstraint * increases_cost(const fawkes::NavGraphNode &from,
-					      const fawkes::NavGraphNode &to);
+					      	  	  	  	  	  const fawkes::NavGraphNode &to);
 
   NavGraphEdgeCostConstraint * increases_cost(const fawkes::NavGraphNode &from,
-					      const fawkes::NavGraphNode &to,
-					      float & cost_factor);
+					      	  	  	  	  	  const fawkes::NavGraphNode &to,
+					      	  	  	  	  	  float & cost_factor);
 
-  float                        cost_factor(const fawkes::NavGraphNode &from,
-					   const fawkes::NavGraphNode &to);
+  float cost_factor(const fawkes::NavGraphNode &from,
+		  		   const fawkes::NavGraphNode &to);
 
   std::map<std::string, std::string>
     blocks(const std::vector<fawkes::NavGraphNode> &nodes);

- *commit* b42453ca2a5255e18fb82e1056f300f1747dbba7 - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Mon Aug 3 15:25:59 2015 +0800
Subject: navgraph-timed: search_state fix constructor

 src/libs/navgraph/search_state.cpp |    2 +-
 1 files changed, 1 insertions(+), 1 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index a995634..ae7fbac 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -61,7 +61,7 @@ NavGraphSearchState::NavGraphSearchState(const NavGraphPath &path,
                                          navgraph::EstimateFunction estimate_func,
                                          navgraph::CostFunction cost_func,
                                          fawkes::NavGraphConstraintRepo *constraint_repo)
-  : AStarState(cost_sofar, parent), path_(path),
+  : AStarState(cost_sofar, parent_state), path_(path),
     goal_(goal), estimated_time_(estimated_time), estimate_func_(estimate_func), cost_func_(cost_func)
 {
   path_.add_node(node, 0.0, estimated_time);

- *commit* 776812eb95c2200b435c7e208929e5f7d1ff6c6c - - - - - - - - - -
Author:  Sebastian Reuter <sebastian.reuter at ima-zlw-ifu.rwth-aachen.de>
Date:    Mon Aug 3 18:14:22 2015 +0800
Subject: navgraph-timed: call AStar Search with fawkes::Time(now)

 src/libs/navgraph/navgraph.cpp           |   33 +++++++++++++++---------
 src/libs/navgraph/navgraph.h             |    9 ++++++
 src/libs/navgraph/search_state.cpp       |    3 ++
 src/plugins/navgraph/navgraph_thread.cpp |   40 ++++++++++++-----------------
 4 files changed, 49 insertions(+), 36 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/navgraph.cpp b/src/libs/navgraph/navgraph.cpp
index d4b7ccd..19ce112 100644
--- a/src/libs/navgraph/navgraph.cpp
+++ b/src/libs/navgraph/navgraph.cpp
@@ -956,10 +956,10 @@ NavGraph::unset_search_funcs()
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const std::string &from, const std::string &to,
+NavGraph::search_path(const std::string &from, const std::string &to, const fawkes::Time time,
 		      bool use_constraints, bool compute_constraints)
 {
-  return search_path(from, to,
+  return search_path(from, to, time,
 		     search_estimate_func_, search_cost_func_,
 		     use_constraints, compute_constraints);
 }
@@ -988,7 +988,7 @@ NavGraph::search_path(const std::string &from, const std::string &to,
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const std::string &from, const std::string &to,
+NavGraph::search_path(const std::string &from, const std::string &to, const fawkes::Time time,
 		      navgraph::EstimateFunction estimate_func,
 		      navgraph::CostFunction cost_func,
 		      bool use_constraints, bool compute_constraints)
@@ -998,7 +998,7 @@ NavGraph::search_path(const std::string &from, const std::string &to,
   nodes.push_back(from_node);
   NavGraphPath from_path(this, nodes);
   NavGraphNode to_node(node(to));
-  return search_path(from_path, to_node, estimate_func, cost_func,
+  return search_path(from_path, to_node, time, estimate_func, cost_func,
 		     use_constraints, compute_constraints);
 }
 
@@ -1022,10 +1022,10 @@ NavGraph::search_path(const std::string &from, const std::string &to,
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
+NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to, const fawkes::Time time,
 		      bool use_constraints, bool compute_constraints)
 {
-  return search_path(from, to,
+  return search_path(from, to, time,
 		     search_estimate_func_, search_cost_func_,
 		     use_constraints, compute_constraints);
 }
@@ -1054,15 +1054,17 @@ NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
+NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to, const fawkes::Time time,
 		      navgraph::EstimateFunction estimate_func,
 		      navgraph::CostFunction cost_func,
 		      bool use_constraints, bool compute_constraints)
 {
   std::vector<fawkes::NavGraphNode> nodes;
+  std::vector<fawkes::Time> times;
   nodes.push_back(from);
-  NavGraphPath from_path(this, nodes);
-  return search_path(from_path, to, estimate_func, cost_func,
+  times.push_back(time);
+  NavGraphPath from_path(this, nodes, times);
+  return search_path(from_path, to, time, estimate_func, cost_func,
 		     use_constraints, compute_constraints);
 }
 
@@ -1090,7 +1092,7 @@ NavGraph::search_path(const NavGraphNode &from, const NavGraphNode &to,
  * or it was prohibited when using constraints.
  */
 fawkes::NavGraphPath
-NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to,
+NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to, const fawkes::Time time,
 		      navgraph::EstimateFunction estimate_func,
 		      navgraph::CostFunction cost_func,
 		      bool use_constraints, bool compute_constraints)
@@ -1118,11 +1120,13 @@ NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to,
     a_star_solution =  astar.solve(initial_state);
   }
 
-  std::vector<fawkes::NavGraphNode> path(a_star_solution.size());
+  std::vector<fawkes::NavGraphNode> nodes(a_star_solution.size());
+  std::vector<fawkes::Time> times(a_star_solution.size());
   NavGraphSearchState *solstate;
   for (unsigned int i = 0; i < a_star_solution.size(); ++i ) {
     solstate = dynamic_cast<NavGraphSearchState *>(a_star_solution[i]);
-    path[i] = solstate->path().nodes().back();
+    nodes[i] = solstate->path().nodes().back();
+    times[i] = solstate->path().times().back();
   }
 
   float cost =
@@ -1130,7 +1134,10 @@ NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to,
       ? a_star_solution[a_star_solution.size() - 1]->total_estimated_cost
       : -1;
 
-  return NavGraphPath(this, path, cost);
+  // Could also be done with (?):
+  // NavGraphPath myPath = dynamic_cast<NavGraphSearchState *>(a_star_solution.back())->path();
+
+  return NavGraphPath(this, nodes, times, cost);
 }
 
 
diff --git a/src/libs/navgraph/navgraph.h b/src/libs/navgraph/navgraph.h
index 9ee5f7f..70dd4b1 100644
--- a/src/libs/navgraph/navgraph.h
+++ b/src/libs/navgraph/navgraph.h
@@ -27,12 +27,15 @@
 #include <navgraph/navgraph_edge.h>
 #include <navgraph/navgraph_path.h>
 #include <core/utils/lockptr.h>
+#include <utils/time/time.h>
 
 #include <vector>
 #include <list>
 #include <string>
 #include <functional>
 
+#include <iostream>
+
 namespace fawkes {
 #if 0 /* just to make Emacs auto-indent happy */
 }
@@ -128,29 +131,35 @@ class NavGraph
   std::vector<std::string>  reachable_nodes(const std::string &node_name) const;
 
   fawkes::NavGraphPath search_path(const std::string &from, const std::string &to,
+		  	  	   const fawkes::Time time,
 				   bool use_constraints = true, bool compute_constraints = true);
 
   fawkes::NavGraphPath search_path(const std::string &from, const std::string &to,
+		           const fawkes::Time time,
 				   navgraph::EstimateFunction estimate_func,
 				   navgraph::CostFunction cost_func,
 				   bool use_constraints = true, bool compute_constraints = true);
 
   fawkes::NavGraphPath search_path(const NavGraphNode &from,
 				   const NavGraphNode &to,
+		           const fawkes::Time time,
 				   bool use_constraints = true, bool compute_constraints = true);
 
   fawkes::NavGraphPath search_path(const NavGraphNode &from,
 				   const NavGraphNode &to,
+		           const fawkes::Time time,
 				   navgraph::EstimateFunction estimate_func,
 				   navgraph::CostFunction cost_func,
 				   bool use_constraints = true, bool compute_constraints = true);
 
   fawkes::NavGraphPath search_path(const NavGraphPath &from,
 				   const NavGraphNode &to,
+		           const fawkes::Time time,
 				   bool use_constraints = true, bool compute_constraints = true);
 
   fawkes::NavGraphPath search_path(const NavGraphPath &from,
 				   const NavGraphNode &to,
+				   const fawkes::Time time,
 				   navgraph::EstimateFunction estimate_func,
 				   navgraph::CostFunction cost_func,
 				   bool use_constraints = true, bool compute_constraints = true);
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index ae7fbac..f78aeaf 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -170,11 +170,14 @@ NavGraphSearchState::estimate_time(fawkes::NavGraphPath &path, fawkes::NavGraphN
 {
 	//TESTING CODE - HARDCODED VELOCITY
 
+	  std::cout<< "SearchState - Node " << n.name().c_str() << " gets time " << path.times().back().in_sec() << std::endl;
+
 	  double max_v = 3.0;
 	  double time = max_v * (sqrtf(powf(n.x() - path.back().x(), 2) +
 	                       powf(n.y() - path.back().y(), 2) ) );
 
 	  return (path.times().back() + time);
+
 }
 
 
diff --git a/src/plugins/navgraph/navgraph_thread.cpp b/src/plugins/navgraph/navgraph_thread.cpp
index 3d528f6..0050ce3 100644
--- a/src/plugins/navgraph/navgraph_thread.cpp
+++ b/src/plugins/navgraph/navgraph_thread.cpp
@@ -27,6 +27,7 @@
 #include <tf/utils.h>
 #include <core/utils/lockptr.h>
 
+#include <iostream>
 #include <fstream>
 #include <cmath>
 
@@ -451,29 +452,15 @@ NavGraphThread::generate_plan(std::string goal_name)
 	logger->log_debug(name(), "Starting at (%f,%f), closest node is '%s'",
 	                  pose_.getOrigin().x(), pose_.getOrigin().y(), init.name().c_str());
 
-	try {
-		path_ = graph_->search_path(init, goal, /* use constraints */ true);
-	} catch (Exception &e) {
-		logger->log_error(name(), "Failed to generate path from (%.2f,%.2f) to %s: %s",
-		                  init.x(), init.y(), goal_name.c_str(), e.what_no_backtrace());
-    pp_nav_if_->set_final(true);
-    pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
-		return false;
-	}
+  path_ = graph_->search_path(init, goal, fawkes::Time(clock), /* use constraints */ true);
 
-	if (! path_.empty()) {
-		constrained_plan_ = true;
-	} else {
-		constrained_plan_ = false;
-		logger->log_warn(name(), "Failed to generate plan, will try without constraints");
-		try {
-			path_ = graph_->search_path(init, goal, /* use constraints */ false);
-		} catch (Exception &e) {
-			pp_nav_if_->set_final(true);
-			pp_nav_if_->set_error_code(NavigatorInterface::ERROR_PATH_GEN_FAIL);
-			return false;
-		}
-	}
+  if (! path_.empty()) {
+    constrained_plan_ = true;
+  } else {
+    constrained_plan_ = false;
+    logger->log_warn(name(), "Failed to generate plan, will try without constraints");
+    path_ = graph_->search_path(init, goal, fawkes::Time(clock), /* use constraints */ false);
+  }
 
 	if (path_.empty()) {
 		logger->log_error(name(), "Failed to generate plan to travel to '%s'",
@@ -539,7 +526,7 @@ NavGraphThread::replan(const NavGraphNode &start, const NavGraphNode &goal)
   }
 
   NavGraphPath new_path =
-    graph_->search_path(start, act_goal,
+    graph_->search_path(start, act_goal, fawkes::Time(clock),
 			  /* use constraints */ true, /* compute constraints */ false);
 
   if (! new_path.empty()) {
@@ -1000,6 +987,13 @@ NavGraphThread::log_graph()
 void
 NavGraphThread::publish_path()
 {
+
+  for(uint i=0; i<path_.nodes().size(); i++){
+
+	  logger->log_info(name(), "Robot will be on node %s at %f", path_.nodes()[i].name().c_str(), path_.times()[i].in_sec());
+  }
+
+
   std::vector<std::string> vpath(40, "");
 
   if (traversal_) {

- *commit* ba83ccb23051e83ed24068c89af9efca6dabbf5b - - - - - - - - - -
Author:  Christoph Henke <christoph.henke at rwth-aachen.de>
Date:    Thu Sep 24 21:00:21 2015 +0200
Subject: navgraph-timed: New NavPathInterface. Using navgraph-timed.

 src/libs/interfaces/NavPathTimedInterface.cpp   | 2710 +++++++++++++++++++++++
 src/libs/interfaces/NavPathTimedInterface.h     |  394 ++++
 src/libs/interfaces/NavPathTimedInterface.tolua |  291 +++
 src/libs/interfaces/NavPathTimedInterface.xml   |  104 +
 src/libs/navgraph/navgraph.cpp                  |    3 +
 src/libs/navgraph/search_state.cpp              |    2 +-
 src/plugins/navgraph/Makefile                   |    2 +-
 src/plugins/navgraph/navgraph_thread.cpp        |   92 +-
 src/plugins/navgraph/navgraph_thread.h          |    4 +
 9 files changed, 3598 insertions(+), 4 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/navgraph.cpp b/src/libs/navgraph/navgraph.cpp
index 19ce112..0087c11 100644
--- a/src/libs/navgraph/navgraph.cpp
+++ b/src/libs/navgraph/navgraph.cpp
@@ -1104,9 +1104,11 @@ NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to, const fa
   std::vector<AStarState *> a_star_solution;
 
   if (use_constraints) {
+      std::cout << "NavGraph: Use Constraints" << std::endl;
     constraint_repo_.lock();
     if (compute_constraints && constraint_repo_->has_constraints()) {
       constraint_repo_->compute();
+      std::cout << "NavGraph: Compute Constraints" << std::endl;
     }
 
     NavGraphSearchState *initial_state =
@@ -1115,6 +1117,7 @@ NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to, const fa
     a_star_solution =  astar.solve(initial_state);
     constraint_repo_.unlock();
   } else {
+      std::cout << "NavGraph: Don't Use Constraints" << std::endl;
     NavGraphSearchState *initial_state =
       new NavGraphSearchState(from, to, this, estimate_func, cost_func);
     a_star_solution =  astar.solve(initial_state);
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index f78aeaf..e9a68c1 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -207,7 +207,7 @@ NavGraphSearchState::children()
   for (unsigned int i = 0; i < descendants.size(); ++i) {
     NavGraphNode d = map_graph_->node(descendants[i]);
 
-    fawkes::Time d_time = path_.times().back() + estimate_time(path_, d);
+    fawkes::Time d_time = path_.times().back() + estimate();//path_.times().back() + estimate_time(path_, d);
 
     bool expand = true;
 
diff --git a/src/plugins/navgraph/Makefile b/src/plugins/navgraph/Makefile
index 9fc68f1..99bab04 100644
--- a/src/plugins/navgraph/Makefile
+++ b/src/plugins/navgraph/Makefile
@@ -26,7 +26,7 @@ CFLAGS += $(CFLAGS_CPP11)
 
 LIBS_navgraph = m fawkescore fawkesutils fawkesaspects fawkesinterface \
 		fawkesblackboard fawkesnavgraph \
-		NavigatorInterface NavPathInterface
+		NavigatorInterface NavPathInterface NavPathTimedInterface
 OBJS_navgraph = navgraph_plugin.o navgraph_thread.o
 OBJS_all    = $(OBJS_navgraph)
 
diff --git a/src/plugins/navgraph/navgraph_thread.cpp b/src/plugins/navgraph/navgraph_thread.cpp
index 0050ce3..efde909 100644
--- a/src/plugins/navgraph/navgraph_thread.cpp
+++ b/src/plugins/navgraph/navgraph_thread.cpp
@@ -109,7 +109,7 @@ NavGraphThread::init()
   pp_nav_if_ = blackboard->open_for_writing<NavigatorInterface>("Pathplan");
   nav_if_    = blackboard->open_for_reading<NavigatorInterface>(cfg_nav_if_id_.c_str());
   path_if_   = blackboard->open_for_writing<NavPathInterface>("NavPath");
-
+  path_timed_if_ = blackboard->open_for_writing<NavPathTimedInterface>("NavPathTimed");
 
   if (! cfg_graph_file_.empty()) {
     if (cfg_graph_file_[0] != '/') {
@@ -197,6 +197,7 @@ NavGraphThread::finalize()
   blackboard->close(pp_nav_if_);
   blackboard->close(nav_if_);
   blackboard->close(path_if_);
+  blackboard->close(path_timed_if_);
 }
 
 void
@@ -995,12 +996,14 @@ NavGraphThread::publish_path()
 
 
   std::vector<std::string> vpath(40, "");
+  std::vector<double> vpathtimes(40, 0.0);
 
   if (traversal_) {
     size_t ind = 0;
     size_t r = traversal_.running() ? traversal_.current_index() : traversal_.remaining();
     for (; r < traversal_.path().size(); ++r) {
-      vpath[ind++] = traversal_.path().nodes()[r].name();
+      vpath[ind] = traversal_.path().nodes()[r].name();
+      vpathtimes[ind++] = traversal_.path().times()[r].in_msec();
     }
   }
 
@@ -1046,4 +1049,89 @@ NavGraphThread::publish_path()
   path_if_->set_path_node_40(vpath[39].c_str());
   path_if_->set_path_length(traversal_.remaining());
   path_if_->write();
+
+  path_timed_if_->set_path_node_1(vpath[0].c_str());
+  path_timed_if_->set_path_node_2(vpath[1].c_str());
+  path_timed_if_->set_path_node_3(vpath[2].c_str());
+  path_timed_if_->set_path_node_4(vpath[3].c_str());
+  path_timed_if_->set_path_node_5(vpath[4].c_str());
+  path_timed_if_->set_path_node_6(vpath[5].c_str());
+  path_timed_if_->set_path_node_7(vpath[6].c_str());
+  path_timed_if_->set_path_node_8(vpath[7].c_str());
+  path_timed_if_->set_path_node_9(vpath[8].c_str());
+  path_timed_if_->set_path_node_10(vpath[9].c_str());
+  path_timed_if_->set_path_node_11(vpath[10].c_str());
+  path_timed_if_->set_path_node_12(vpath[11].c_str());
+  path_timed_if_->set_path_node_13(vpath[12].c_str());
+  path_timed_if_->set_path_node_14(vpath[13].c_str());
+  path_timed_if_->set_path_node_15(vpath[14].c_str());
+  path_timed_if_->set_path_node_16(vpath[15].c_str());
+  path_timed_if_->set_path_node_17(vpath[16].c_str());
+  path_timed_if_->set_path_node_18(vpath[17].c_str());
+  path_timed_if_->set_path_node_19(vpath[18].c_str());
+  path_timed_if_->set_path_node_20(vpath[19].c_str());
+  path_timed_if_->set_path_node_21(vpath[20].c_str());
+  path_timed_if_->set_path_node_22(vpath[21].c_str());
+  path_timed_if_->set_path_node_23(vpath[22].c_str());
+  path_timed_if_->set_path_node_24(vpath[23].c_str());
+  path_timed_if_->set_path_node_25(vpath[24].c_str());
+  path_timed_if_->set_path_node_26(vpath[25].c_str());
+  path_timed_if_->set_path_node_27(vpath[26].c_str());
+  path_timed_if_->set_path_node_28(vpath[27].c_str());
+  path_timed_if_->set_path_node_29(vpath[28].c_str());
+  path_timed_if_->set_path_node_30(vpath[29].c_str());
+  path_timed_if_->set_path_node_31(vpath[30].c_str());
+  path_timed_if_->set_path_node_32(vpath[31].c_str());
+  path_timed_if_->set_path_node_33(vpath[32].c_str());
+  path_timed_if_->set_path_node_34(vpath[33].c_str());
+  path_timed_if_->set_path_node_35(vpath[34].c_str());
+  path_timed_if_->set_path_node_36(vpath[35].c_str());
+  path_timed_if_->set_path_node_37(vpath[36].c_str());
+  path_timed_if_->set_path_node_38(vpath[37].c_str());
+  path_timed_if_->set_path_node_39(vpath[38].c_str());
+  path_timed_if_->set_path_node_40(vpath[39].c_str());
+
+  path_timed_if_->set_path_time_1(vpathtimes[0]);
+  path_timed_if_->set_path_time_2(vpathtimes[1]);
+  path_timed_if_->set_path_time_3(vpathtimes[2]);
+  path_timed_if_->set_path_time_4(vpathtimes[3]);
+  path_timed_if_->set_path_time_5(vpathtimes[4]);
+  path_timed_if_->set_path_time_6(vpathtimes[5]);
+  path_timed_if_->set_path_time_7(vpathtimes[6]);
+  path_timed_if_->set_path_time_8(vpathtimes[7]);
+  path_timed_if_->set_path_time_9(vpathtimes[8]);
+  path_timed_if_->set_path_time_10(vpathtimes[9]);
+  path_timed_if_->set_path_time_11(vpathtimes[10]);
+  path_timed_if_->set_path_time_12(vpathtimes[11]);
+  path_timed_if_->set_path_time_13(vpathtimes[12]);
+  path_timed_if_->set_path_time_14(vpathtimes[13]);
+  path_timed_if_->set_path_time_15(vpathtimes[14]);
+  path_timed_if_->set_path_time_16(vpathtimes[15]);
+  path_timed_if_->set_path_time_17(vpathtimes[16]);
+  path_timed_if_->set_path_time_18(vpathtimes[17]);
+  path_timed_if_->set_path_time_19(vpathtimes[18]);
+  path_timed_if_->set_path_time_20(vpathtimes[19]);
+  path_timed_if_->set_path_time_21(vpathtimes[20]);
+  path_timed_if_->set_path_time_22(vpathtimes[21]);
+  path_timed_if_->set_path_time_23(vpathtimes[22]);
+  path_timed_if_->set_path_time_24(vpathtimes[23]);
+  path_timed_if_->set_path_time_25(vpathtimes[24]);
+  path_timed_if_->set_path_time_26(vpathtimes[25]);
+  path_timed_if_->set_path_time_27(vpathtimes[26]);
+  path_timed_if_->set_path_time_28(vpathtimes[27]);
+  path_timed_if_->set_path_time_29(vpathtimes[28]);
+  path_timed_if_->set_path_time_30(vpathtimes[29]);
+  path_timed_if_->set_path_time_31(vpathtimes[30]);
+  path_timed_if_->set_path_time_32(vpathtimes[31]);
+  path_timed_if_->set_path_time_33(vpathtimes[32]);
+  path_timed_if_->set_path_time_34(vpathtimes[33]);
+  path_timed_if_->set_path_time_35(vpathtimes[34]);
+  path_timed_if_->set_path_time_36(vpathtimes[35]);
+  path_timed_if_->set_path_time_37(vpathtimes[36]);
+  path_timed_if_->set_path_time_38(vpathtimes[37]);
+  path_timed_if_->set_path_time_39(vpathtimes[38]);
+  path_timed_if_->set_path_time_40(vpathtimes[39]);
+
+  path_timed_if_->set_path_length(traversal_.remaining());
+  path_timed_if_->write();
 }
diff --git a/src/plugins/navgraph/navgraph_thread.h b/src/plugins/navgraph/navgraph_thread.h
index a2b51b1..ed19334 100644
--- a/src/plugins/navgraph/navgraph_thread.h
+++ b/src/plugins/navgraph/navgraph_thread.h
@@ -37,6 +37,8 @@
 
 #include <interfaces/NavigatorInterface.h>
 #include <interfaces/NavPathInterface.h>
+// test broker
+#include <interfaces/NavPathTimedInterface.h>
 
 #include <navgraph/navgraph.h>
 #include <utils/system/fam.h>
@@ -116,6 +118,8 @@ class NavGraphThread
   fawkes::NavigatorInterface *nav_if_;
   fawkes::NavigatorInterface *pp_nav_if_;
   fawkes::NavPathInterface *path_if_;
+  // test broker
+  fawkes::NavPathTimedInterface *path_timed_if_;
 
   fawkes::LockPtr<fawkes::NavGraph> graph_;
 

- *commit* 607fc46142431e68d8ef0e175b32d0439a86a525 - - - - - - - - - -
Author:  Christoph Henke <christoph.henke at rwth-aachen.de>
Date:    Mon Oct 19 11:32:42 2015 +0200
Subject: navgraph-timed: Trajectory constraint for visu.

 src/libs/navgraph/constraints/constraint_repo.cpp  |   21 ++++
 src/libs/navgraph/constraints/constraint_repo.h    |    3 +
 .../constraints/trajectory_node_constraint.cpp     |   97 +++++++++++++++-----
 .../constraints/trajectory_node_constraint.h       |   19 +++-
 src/libs/navgraph/search_state.cpp                 |   11 ++-
 src/plugins/navgraph/visualization_thread.cpp      |    3 +-
 src/plugins/navgraph/visualization_thread.h        |    2 +
 7 files changed, 126 insertions(+), 30 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/constraints/constraint_repo.cpp b/src/libs/navgraph/constraints/constraint_repo.cpp
index e42c2f8..8ce17f4 100644
--- a/src/libs/navgraph/constraints/constraint_repo.cpp
+++ b/src/libs/navgraph/constraints/constraint_repo.cpp
@@ -382,6 +382,27 @@ NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphNode> &nodes)
   return rv;
 }
 
+/** Check if any constraint in the repo blocks (some) nodes.
+ * @param nodes vector of nodes to check for a block
+ * @return map of blocked nodes, first element is the node name,
+ * second element is the name of the constraint that blocks the node.
+ * Nodes from @p nodes that are not blocked will not appear in the map.
+ */
+std::map<std::string, std::string>
+NavGraphConstraintRepo::blocks(const std::vector<fawkes::NavGraphNode> &nodes, const fawkes::Time &now )
+{
+  std::map<std::string, std::string> rv;
+  for (const fawkes::NavGraphNode &n : nodes) {
+    for (fawkes::NavGraphTrajectoryNodeConstraint *c : trajectory_node_constraints_) {
+      if (c->blocks(n, now)) {
+	rv[n.name()] = c->name();
+      }
+    }
+  }
+
+  return rv;
+}
+
 /** Check if any constraint in the repo blocks the edge.
  * @param from node from which the edge originates
  * @param to node to which the edge leads
diff --git a/src/libs/navgraph/constraints/constraint_repo.h b/src/libs/navgraph/constraints/constraint_repo.h
index b3aebd2..762397a 100644
--- a/src/libs/navgraph/constraints/constraint_repo.h
+++ b/src/libs/navgraph/constraints/constraint_repo.h
@@ -101,6 +101,9 @@ class NavGraphConstraintRepo
   std::map<std::string, std::string>
     blocks(const std::vector<fawkes::NavGraphNode> &nodes);
 
+  std::map<std::string, std::string>
+   blocks(const std::vector<fawkes::NavGraphNode> &nodes, const fawkes::Time &now);
+
   std::map<std::pair<std::string, std::string>, std::string>
     blocks(const std::vector<fawkes::NavGraphEdge> &edges);
 
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
index 9a06e35..7ae789e 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.cpp
@@ -84,6 +84,10 @@ NavGraphTrajectoryNodeConstraint::print_Constraint(){
 		output += node.node.name();
 		output += " at ";
 		output += std::to_string( node.time.in_sec() );
+		output += " from ";
+		output += std::to_string( node.time.in_sec() - node.timeframe.in_sec() );
+		output += " to ";
+		output += std::to_string( node.time.in_sec() + node.timeframe.in_sec() );
 
 		logger_->log_info("TarjectoryNodeConstraint", "%s", output.c_str());
 	}
@@ -117,16 +121,32 @@ NavGraphTrajectoryNodeConstraint::compute(void) throw()
 
 /** Add a single trajectory-node to constraint list.
  * @param node trajectory-node to add to constraint list
+ * @param time trajectory-time to add to constraint list
+ * @param timeframe trajectory-time-frame (default value) to add to constraint list
  */
 void
 NavGraphTrajectoryNodeConstraint::add_node(const NavGraphNode &node, const fawkes::Time &time)
 {
   if (! has_node(node,time)) {
+    fawkes::Time timeframe;
+    timeframe.set_time((long int) 1000);
+    add_node(node, time, timeframe);
+  }
+}
+
+/** Add a single trajectory-node to constraint list.
+ * @param node trajectory-node to add to constraint list
+ * @param time trajectory-time to add to constraint list
+ * @param timeframe trajectory-time-frame to add to constraint list
+ */
+void
+NavGraphTrajectoryNodeConstraint::add_node(const NavGraphNode &node, const fawkes::Time &time, const fawkes::Time &timeframe)
+{
+  if (! has_node(node,time)) {
     modified_ = true;
-    TrajectoryNode tnode(node,time);
+    TrajectoryNode tnode(node,time,timeframe);
     node_list_.push_back(tnode);
-
-	logger_->log_error("TrajectoryNodeConstraint", "Added trajectory-constraint for node %s at time %f", node.name().c_str(), time.in_sec() );
+    logger_->log_error("TrajectoryNodeConstraint", "Added trajectory-constraint for node %s from %f till %f", node.name().c_str(), time.in_sec()-timeframe.in_sec(), time.in_sec()+timeframe.in_sec());
   }
 }
 
@@ -148,13 +168,31 @@ NavGraphTrajectoryNodeConstraint::add_nodes(const std::vector<NavGraphNode> &nod
 }
 
 /** Add multiple trajectory-nodes to constraint list.
+ * @param nodes vector of nodes to add to constraint list
+ * @param times vector of times to add to constraint list
+ * @param timeframes vector of time-frames to add to constraint list
+ */
+void
+NavGraphTrajectoryNodeConstraint::add_nodes(const std::vector<NavGraphNode> &nodes, const std::vector<fawkes::Time> &times, const std::vector<fawkes::Time> &timeframes)
+{
+  if(nodes.size() == times.size() && nodes.size() == timeframes.size()){
+	  for (uint i=0; i<nodes.size(); i++){
+		add_node(nodes[i],times[i],timeframes[i]);
+	  }
+  }
+  else{
+	  logger_->log_error("TrajectoryNodeConstraint", "tried to add uneven number of nodes, corresponding times and corresponding time-frames. Will do nothing!");
+  }
+}
+
+/** Add multiple trajectory-nodes to constraint list.
  * @param nodes trajectory-nodes to add to constraint list
  */
 void
-NavGraphTrajectoryNodeConstraint::add_nodes(const std::vector<TrajectoryNode> &nodes)
+NavGraphTrajectoryNodeConstraint::add_nodes(const std::vector<TrajectoryNode> &tnodes)
 {
-  for(auto& node: nodes){
-		node_list_.push_back(node);
+  for(auto& tnode: tnodes){
+		node_list_.push_back(tnode);
   }
 }
 
@@ -181,21 +219,40 @@ NavGraphTrajectoryNodeConstraint::remove_node(const TrajectoryNode &node)
 
 /** Check if constraint has a specific node.
  * @param node node to check
- * @return true if node is in list, false otherwise
+ * @return true if node is in list and registered within time frame, false otherwise
  */
 bool
 NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, const fawkes::Time &time)
 {
-	/*
-	 *
-	 * TODO:
-	 * Create range - puffer before and after the estimated time
-	 *
-	 */
   for(auto& n : node_list_){
-	  if(node == n.node && time == n.time){
+	  logger_->log_error("TrajectoryNodeConstraint", "Tested node %s at %f, which is blocked from %f till %f",
+      		                     n.node.name().c_str(),
+      		                     time.in_sec(),
+      		                     n.time.in_sec() - n.timeframe.in_sec(),
+      		                     n.time.in_sec() + n.timeframe.in_sec());
+	  if(node == n.node && time.in_sec() > (n.time.in_sec() - n.timeframe.in_sec()) && time.in_sec() < (n.time.in_sec() + n.timeframe.in_sec())){
+		  logger_->log_error("TrajectoryNodeConstraint", "Blocking node %s from %f till %f",
+		                     n.node.name().c_str(),
+		                     n.time.in_sec() - n.timeframe.in_sec(),
+		                     n.time.in_sec() + n.timeframe.in_sec());
+		  return true;
+	  }
+  }
+  return false;
+}
+
+/** Check if constraint has a specific node.
+ * @param node node to check
+ * @return true if node is in list and registered within time frame, false otherwise
+ */
+bool
+NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node)
+{
+  for(auto& n : node_list_){
+	  if(node == n.node){
 		  return true;
-		  logger_->log_error("TrajectoryNodeConstraint", "Blocking node %s at time %f", n.node.name().c_str(), n.time.in_sec());
+		  logger_->log_error("TrajectoryNodeConstraint", "Blocking node %s",
+		                     n.node.name().c_str());
 	  }
   }
   return false;
@@ -203,19 +260,13 @@ NavGraphTrajectoryNodeConstraint::has_node(const fawkes::NavGraphNode &node, con
 
 /** Check if constraint has a specific trajectory-node.
  * @param trajectory_node node to check
- * @return true if node is in list, false otherwise
+ * @return true if node is in list and registered within time frame, false otherwise
  */
 bool
 NavGraphTrajectoryNodeConstraint::has_node(const TrajectoryNode &trajectory_node)
 {
-	/*
-	 *
-	 * TODO:
-	 * Create range - puffer before and after the estimated time
-	 *
-	 */
 	for(auto& n : node_list_){
-		  if(trajectory_node.node == n.node){
+		  if(trajectory_node.node == n.node && trajectory_node.time > (n.time - n.timeframe) && trajectory_node.time < (n.time + n.timeframe)){
 			  return true;
 		  }
 	  }
diff --git a/src/libs/navgraph/constraints/trajectory_node_constraint.h b/src/libs/navgraph/constraints/trajectory_node_constraint.h
index 68279da..4a59ac3 100644
--- a/src/libs/navgraph/constraints/trajectory_node_constraint.h
+++ b/src/libs/navgraph/constraints/trajectory_node_constraint.h
@@ -44,10 +44,12 @@ class NavGraphTrajectoryNodeConstraint
 	 /** Constructor.
 	   * @param node node on trajectory
 	   * @param time estimated time on node
+	   * @param timeframe estimated reservation time-frame
 	 */
-	 TrajectoryNode_ (fawkes::NavGraphNode node, fawkes::Time time) : node(node), time(time) {}
+	 TrajectoryNode_ (fawkes::NavGraphNode node, fawkes::Time time, fawkes::Time timeframe) : node(node), time(time), timeframe(timeframe) {}
      fawkes::NavGraphNode 		node;
-     fawkes::Time  				time;
+     fawkes::Time  			time;
+     fawkes::Time			timeframe;
   } TrajectoryNode;
 
   NavGraphTrajectoryNodeConstraint(Logger *logger, std::string name, fawkes::Clock *clock);
@@ -57,13 +59,16 @@ class NavGraphTrajectoryNodeConstraint
   NavGraphTrajectoryNodeConstraint(std::string name,
 				   std::vector<TrajectoryNode> &node_list);
 
-  ~NavGraphTrajectoryNodeConstraint();
+  virtual ~NavGraphTrajectoryNodeConstraint();
 
   const std::vector<TrajectoryNode> &  node_list() const;
 
   void add_node(const NavGraphNode &node, const fawkes::Time &time);
+  void add_node(const NavGraphNode &node, const fawkes::Time &time, const fawkes::Time &timeframe);
   void add_node(const TrajectoryNode &trajectory_node);
+
   void add_nodes(const std::vector<NavGraphNode> &trajectory_node, const std::vector<fawkes::Time> &times);
+  void add_nodes(const std::vector<NavGraphNode> &trajectory_nodes, const std::vector<fawkes::Time> &times, const std::vector<fawkes::Time> &timeframes);
   void add_nodes(const std::vector<TrajectoryNode> &trajectory_node);
 
   bool remove_node(const TrajectoryNode &node);
@@ -72,12 +77,16 @@ class NavGraphTrajectoryNodeConstraint
 
   bool has_node(const TrajectoryNode &trajectory_node);
   bool has_node(const fawkes::NavGraphNode &node, const fawkes::Time &time);
+  bool has_node(const fawkes::NavGraphNode &node);
 
-  bool compute(void) throw();
+  virtual bool compute(void) throw();
 
-  bool blocks(const fawkes::NavGraphNode &node, const fawkes::Time &time) throw()
+  virtual bool blocks(const fawkes::NavGraphNode &node, const fawkes::Time &time) throw()
   { return has_node(node, time); }
 
+  virtual bool blocks(const fawkes::NavGraphNode &node) throw()
+    { return has_node(node); }
+
   std::string name();
 
   bool operator==(const std::string &name) const;
diff --git a/src/libs/navgraph/search_state.cpp b/src/libs/navgraph/search_state.cpp
index e9a68c1..4db954c 100644
--- a/src/libs/navgraph/search_state.cpp
+++ b/src/libs/navgraph/search_state.cpp
@@ -207,16 +207,25 @@ NavGraphSearchState::children()
   for (unsigned int i = 0; i < descendants.size(); ++i) {
     NavGraphNode d = map_graph_->node(descendants[i]);
 
-    fawkes::Time d_time = path_.times().back() + estimate();//path_.times().back() + estimate_time(path_, d);
+    fawkes::Time d_time = path_.times().back() + estimate();
 
     bool expand = true;
 
     /*
      * check if constraint_repo should be used
      * check if there is a blocking-constraint (node,edge) for the node
+     * add the actual node with an cost factor for waiting
      */
     if (constraint_repo_ and constraint_repo_->blocks(path_, d, d_time)) {
 	    expand = false;
+	    // TODO Add the actual node with an cost factor for waiting
+	    //children.push_back(new NavGraphSearchState(path_,
+	                                               //path_.back() /*the actual node*/,
+	    //goal_,
+	    //                                         /*set the heuristic cost from actual to goal node*/ ,
+	    //                                         /*set the cost function for waiting till the blocked node is free*/,
+	    //                                         this, map_graph_, estimate_func_, cost_func_, constraint_repo_));
+
     }
 
     /*
diff --git a/src/plugins/navgraph/visualization_thread.cpp b/src/plugins/navgraph/visualization_thread.cpp
index b80e033..b9afd7a 100644
--- a/src/plugins/navgraph/visualization_thread.cpp
+++ b/src/plugins/navgraph/visualization_thread.cpp
@@ -229,8 +229,9 @@ NavGraphVisualizationThread::publish()
   std::vector<fawkes::NavGraphEdge> edges = graph_->edges();
   graph_.unlock();
 
+  fawkes::Time now(clock);
   crepo_.lock();
-  std::map<std::string, std::string> bl_nodes = crepo_->blocks(nodes);
+  std::map<std::string, std::string> bl_nodes = crepo_->blocks(nodes, now);
   std::map<std::pair<std::string, std::string>, std::string> bl_edges =
     crepo_->blocks(edges);
   std::list<std::tuple<std::string, std::string, std::string, float>> edge_cfs =
diff --git a/src/plugins/navgraph/visualization_thread.h b/src/plugins/navgraph/visualization_thread.h
index 4445165..5f61f3a 100644
--- a/src/plugins/navgraph/visualization_thread.h
+++ b/src/plugins/navgraph/visualization_thread.h
@@ -30,6 +30,7 @@
 #include <plugins/ros/aspect/ros.h>
 #include <navgraph/navgraph.h>
 #include <navgraph/navgraph_path.h>
+#include <aspect/clock.h>
 
 #include <ros/publisher.h>
 #include <visualization_msgs/MarkerArray.h>
@@ -40,6 +41,7 @@ class NavGraphVisualizationThread
   public fawkes::ConfigurableAspect,
   public fawkes::LoggingAspect,
   public fawkes::ROSAspect,
+  public fawkes::ClockAspect,
   public fawkes::NavGraph::ChangeListener
 {
  public:

- *commit* 2c442f726a8c336835865eb87b24ce70234dff21 - - - - - - - - - -
Author:  Christoph Henke <christoph.henke at rwth-aachen.de>
Date:    Sat Oct 24 21:20:52 2015 +0200
Subject: navgraph: Deleted debugging messages in navgraph

 src/libs/navgraph/navgraph.cpp         |    3 ---
 src/plugins/navgraph/navgraph_thread.h |    2 --
 2 files changed, 0 insertions(+), 5 deletions(-)

_Diff for modified files_:
diff --git a/src/libs/navgraph/navgraph.cpp b/src/libs/navgraph/navgraph.cpp
index 0087c11..19ce112 100644
--- a/src/libs/navgraph/navgraph.cpp
+++ b/src/libs/navgraph/navgraph.cpp
@@ -1104,11 +1104,9 @@ NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to, const fa
   std::vector<AStarState *> a_star_solution;
 
   if (use_constraints) {
-      std::cout << "NavGraph: Use Constraints" << std::endl;
     constraint_repo_.lock();
     if (compute_constraints && constraint_repo_->has_constraints()) {
       constraint_repo_->compute();
-      std::cout << "NavGraph: Compute Constraints" << std::endl;
     }
 
     NavGraphSearchState *initial_state =
@@ -1117,7 +1115,6 @@ NavGraph::search_path(const NavGraphPath &from, const NavGraphNode &to, const fa
     a_star_solution =  astar.solve(initial_state);
     constraint_repo_.unlock();
   } else {
-      std::cout << "NavGraph: Don't Use Constraints" << std::endl;
     NavGraphSearchState *initial_state =
       new NavGraphSearchState(from, to, this, estimate_func, cost_func);
     a_star_solution =  astar.solve(initial_state);
diff --git a/src/plugins/navgraph/navgraph_thread.h b/src/plugins/navgraph/navgraph_thread.h
index ed19334..7c3b48d 100644
--- a/src/plugins/navgraph/navgraph_thread.h
+++ b/src/plugins/navgraph/navgraph_thread.h
@@ -37,7 +37,6 @@
 
 #include <interfaces/NavigatorInterface.h>
 #include <interfaces/NavPathInterface.h>
-// test broker
 #include <interfaces/NavPathTimedInterface.h>
 
 #include <navgraph/navgraph.h>
@@ -118,7 +117,6 @@ class NavGraphThread
   fawkes::NavigatorInterface *nav_if_;
   fawkes::NavigatorInterface *pp_nav_if_;
   fawkes::NavPathInterface *path_if_;
-  // test broker
   fawkes::NavPathTimedInterface *path_timed_if_;
 
   fawkes::LockPtr<fawkes::NavGraph> graph_;




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list