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, vmatare/laser-lines-matching has been updated
  discards  6a2f4ec95c06887ff7637e9e44e534b119ee3e34 (commit)
  discards  d4a5c134dbf5991c976f4011eb7794b72c67099d (commit)
  discards  702e914cdd1ae1ea878ce7a46e21753b7b859808 (commit)
  discards  22c0ea8a615c7d0a5d6994df5f115bf1f8610e1e (commit)
  discards  533b5cd707af5811892dcf9a50cb7c061541331a (commit)
  discards  9fc6d595454939ba7279f4e8f574a85fd866f57c (commit)
  discards  dd60bf7d4c1643987da75f2158156c18d519440c (commit)
  discards  8ce059365c185811610de51c75e9ebfd90d85099 (commit)
  discards  28621e28f8a0617fe58757e3356141a469304161 (commit)
  discards  b2f2d87bd5288c651e8426aa2eafa7a34408e4a1 (commit)
  discards  71b60ea0dbc63be456143f0b9f08b71780655de8 (commit)
  discards  38f1f7a18357dbee272bc68c7e572768c87ff155 (commit)
  discards  bed84c0ea7d9a60812209f9a32c3d04ce8cb98cd (commit)
  discards  4fe72c26926b29109deda44c39389d8e6f5ac10e (commit)
  discards  b2f096ba0ef43132675151721cd57a154169bc60 (commit)
  discards  84568886f6ebfed689ea64f8f010468de63d6c5c (commit)
  discards  4f6b39cc4b758f051ea907c9d6db5532ca34fe14 (commit)
  discards  3ba3a9f8fffea84fcb3d11113d052d8f746ba2b4 (commit)
  discards  f1856600e85634ba1ca203f0382c106c88e0b0e3 (commit)
  discards  909353843d1358053ff4f0c58e4026a5ad55eebe (commit)
  discards  30810872687f9b1136fbd8b3175ccdfff8fd7de2 (commit)
  discards  8fe984ba9ce01faa96544a0dd5878b58e654f37a (commit)
  discards  6e1885f4606b6f3475a39ec88e9d09a41533e2ad (commit)
  discards  762f9af309d8f46821b838547c10433798c95362 (commit)
  discards  497f55ae030e177b181cb894c1dceb3d9b7aa788 (commit)
  discards  029d9a2eb5c0e301b8dd096ca866ce545ecc2437 (commit)
  discards  d022bd332531762b28e5a25eaa5b1f2763c7e495 (commit)
  discards  a7368717c6becd8e146261a2143d04c6eff14ddf (commit)
  discards  11850b75b65468ccae033b9a1768fe43f3d113f3 (commit)
  discards  7aae7cfbde27ae218844cd12e6f0e0555c0dfa5c (commit)
  discards  597113280001a367f8c37ed2bbeaf6a31bade944 (commit)
  discards  0077f5559dd4586de654b551a1678f4b2c725f48 (commit)
  discards  19970f50261fb8052a832d0a95a0ec37afd6d1e4 (commit)
  discards  5af954f00da96369d08f7d5ae78222abba444518 (commit)
  discards  434c889532a428d8b0a04c4a31fac19a229c8aea (commit)
  discards  f28132bc865310b78ce0ea20bc4dc69ce9c3a07d (commit)
  discards  f653c1b5cdcd4e3012829b7ff90c1e81f8572be3 (commit)
  discards  199ca4372349001105634f8d08e62622c882a4c4 (commit)
  discards  4627edbafd3ae7e31335a90680688d7060aedbae (commit)
  discards  fd10f4d510501845070428938976598605d481e8 (commit)
  discards  ca9855de93f78fbe23ea36b65c12100a095090dd (commit)
  discards  f9473faa9cdde4b2526a0500b727209406c6283b (commit)
  discards  d40e1ccd54d9252791bd3a64ce4efa3928564cf3 (commit)
  discards  1429d7f404b4ee2a6e3a2ac1a6c6db1712bf1d81 (commit)
  discards  4c9c84966731aeb578613cd12951cfc05f208ccd (commit)
  discards  7d9de92414f9268b847db72a06a8493ef47ab148 (commit)
  discards  e413a9902aa836811103134e6a6f1c159465f33b (commit)
  discards  e4e6cce12c35a39477095701036e1e1982d92a17 (commit)
  discards  1187f710720064dc2ed5759cc5927d2d02fb3b72 (commit)
  discards  24540b8ba412220f6675146115e8417957270d5b (commit)
  discards  f5a95e9a498db4c4230084d0555de7a20bbb1d82 (commit)
  discards  ce7348c9d08c158f39bf42d988e9c4024739e1fb (commit)
  discards  df4248deb5ec8edd349b1b642c03cbcd28e45c82 (commit)
  discards  0aabc3cbcc30f5f6bbde1a92feb28fb3e8473d7c (commit)
  discards  accf06d19be740868434eda2b1f74c5183d728be (commit)
  discards  fa019abc62abce6e43c43b2848d3fe930abee732 (commit)
  discards  96798a48ac8376ceb2c4840325c0d8ed76515197 (commit)
  discards  599b51a644df68d4347316df55bdbd4270f41e98 (commit)
  discards  0e758a742bf54eb4e85479bb5282c30229c9efd1 (commit)
  discards  30810b68a4aca5d0f31f6c3e3ed5ba73614bcb6a (commit)
  discards  aa26d1ea265fb1fea6f2c722289c59a21ce5506d (commit)
  discards  d290644ddb63e89611eb1c0a18d137d10462ae32 (commit)
  discards  cdbc1a08699bebaf2d841cb9d2107fd755cc1846 (commit)
  discards  a54ce07c37f27024b1f8b8b2386e62dac1171a78 (commit)
  discards  852d27ad5ea40d106c296f7b1565cdc79fe503f1 (commit)
  discards  cceb762465160de2af9eee56d351663e88852e82 (commit)
  discards  8d9312fa6a0da7d7b50b4783fac6fa2a8a79f2db (commit)
  discards  48a251843f99b59fb62612b55341e26b12a5faa9 (commit)
  discards  6de356b670b4820025601297c07fd5181f0e7451 (commit)
  discards  25cb4fc4bf22a28bb78cf0b5a64cee0e5b98dd3e (commit)
  discards  1bcb9f08a375c4b148c5a474d55ebb36ae6407e2 (commit)
  discards  3968030a9cd0427c4ef26377dec10ad337d871f4 (commit)
  discards  ce0a8fcdb3b7ba739e619287bb23064724c179eb (commit)
  discards  aade921abb532f9d447855b69bdb0d1d15a3d761 (commit)
  discards  2ae9c2b9ff361f8906c09e3f27342c66acb2967a (commit)
  discards  173d02e3ff6460d92909f0ee093dab11467d4d68 (commit)
  discards  caca8c7357a982124ee614400290e33af2be3d97 (commit)
  discards  d766cc12e65515991b3142578a7f7f69193306c6 (commit)
  discards  514564c0606f7286975e801db07066f78bdbdb80 (commit)
  discards  b469c1ce3ea76c2ef00d57e795fe1f87d27f9086 (commit)
  discards  279836c58d9f9425942a78122fbde3807c525cc2 (commit)
  discards  b2276899d3eb8c41106ad2c052db011533eaa494 (commit)
  discards  0e8e3e2b717320526c870c678a053ed0227569a7 (commit)
  discards  b4ded1bc9721981dc7793e0e067be96f88290ba2 (commit)
  discards  8fd786fda1c049957a467b91dc675dae2cab54da (commit)
  discards  c4a99d154657d9a7284be18813f6c0a330578fa9 (commit)
  discards  636d76b2640a90582006efe163e47c08ed5cc919 (commit)
  discards  f057b95c53f6652033d74df50b0ac453cc6bc4f1 (commit)
  discards  fcaa24b7368abb1fa9f0389f1be995f6075e6b3f (commit)
  discards  19e65a99f415740d5b06ef8d3229845fa36341be (commit)
  discards  24d074dda025b423416f8552f790748e595c6d6f (commit)
  discards  5904ca521c1ea4be9d0a1eab893c7169a49efa25 (commit)
  discards  a5279a19b989430d6c57b9d15175796564bcd106 (commit)
  discards  c1043b7bd19c354e17da7a2fc696ab64cd723bba (commit)
  discards  ece8ea28533a9c66ccea41dee1ad11d877b6515e (commit)
  discards  4efd0a781bf34e908bfcd6cf3753fea174ca2b4c (commit)
  discards  e2abb6cddf6bc4c5afcddf86f4439f6ca604f005 (commit)
  discards  4edbd1315b79cbfb2a0f4a43e0b896f849f25ac6 (commit)
  discards  70b8a87ba5f994c22bc624d5dd99f611ba317f60 (commit)
  discards  1bd585a4325d13674d9338da493d8848885cb273 (commit)
  discards  d8a31299cc630023462d46c70f898e7251841089 (commit)
  discards  fd0d822429e46791c8d813a052b36ab93686ddfb (commit)
  discards  e2f2ec9b5b4abbd1a52eb333f52d422b3eb73ef3 (commit)
  discards  39a801bb75b3d117ab7682f14d6292cf3a6c00a4 (commit)
  discards  653e9da1728f5de542c86df1854ab88c39951ce8 (commit)
  discards  92ba576c50aeb3ccd3ff610d045a1c78613596aa (commit)
  discards  93a6c61ea2d72bde7e2d9a4167e093542a3f5827 (commit)
  discards  6114f9107c3747ad6d223a97df10ecd6dd00712b (commit)
  discards  ad29da79068fedcaa8cfb7192c4d73156022189a (commit)
  discards  03b21cd4758d52d6d6aff2eb16d55dd368257d8d (commit)
  discards  018256b91d12da67f70eef339fc46950e137742c (commit)
  discards  a9d8e7c18c5d473f833b8586859c1db57d3535b7 (commit)
  discards  e107ae1b8d1b5b3dff753aa67d8e4c1e0586da83 (commit)
  discards  df7e554407e74309289a9cb002c3ff2587417615 (commit)
  discards  5807fb7db8d2026e17dc4b87d9c8c19996288c34 (commit)
  discards  1461082897c75ec22028282124480419e3f68ccc (commit)
  discards  1aa57a4e4693c823a5c3a3bf6c69ce7bc1515451 (commit)
  discards  6a6d2f0334daa69dde5f121605ad5ae0602293bc (commit)
  discards  0f9cd36734d1b1a257c785ce97bfa50ab90ab7ee (commit)
  discards  b5390a24257b8f4bdfd8f03d244b12b0efec2e81 (commit)
  discards  ae82ab53b9f68e704d64ad08110cf74e304e5f11 (commit)
  discards  279841b904f8d494e2bee2c78f6dbf2c870d21f3 (commit)
  discards  13391ca51aad4cfa29c51a3559b32870f833c492 (commit)
  discards  173a2d8497e4d5a5bbad4b4b5c701036d41ebbeb (commit)
  discards  90bb6e55bbd89221a1bfbc9d802665f8beff608b (commit)
  discards  149ecaa0c38aec33ce985bb6be2d3c85d1fa0177 (commit)
  discards  7ba6d1c1054638d9a585e423e5d28f56f9ba429f (commit)
  discards  50684a488d6e6aeb89be21d3785167bd86146e84 (commit)
  discards  8294e05093da52dce553b664e2a3f1c42541d57e (commit)
  discards  258df8714f0ea2f95d705887e2d46647a142971c (commit)
  discards  2e93025c436627116c2e4a2b0e000cbad739baed (commit)
  discards  410db207d876623ad320015c321c1935bbbc20af (commit)
  discards  1145eb2b0027e711e321cc6bd4891c921de7ad00 (commit)
  discards  7d2a1951eb0eff20de57b38abcdccf8759999a40 (commit)
  discards  d2d8bf5fa324c6ed7b887e78b46569f7668d7635 (commit)
  discards  db9a5fd51570896f9fd88f1ae2f0beecb748fe5f (commit)
  discards  6636bc7d3b1655145c317a45c2cd1928d2f71fa1 (commit)
        to  0e29c04013678ddd2b8b880504213482db773cc2 (commit)
       via  3f9c34504025ec4d65565f7ab7399aebcb5849aa (commit)
       via  a499d79289aac8080cba4c888f9a68def953cbd9 (commit)
       via  0f07c689aaee14ef9e963b4c8977caf026945639 (commit)
       via  0fc8e4150162a60ae8211f9238db6f5f770fe22b (commit)
       via  c9d50b6de5255b7e7eb4774bf3d423a2cc457593 (commit)
       via  d951b71774f982b3fb9236a7b60da79069c8e476 (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 (6a2f4ec95c06887ff7637e9e44e534b119ee3e34)
            \
             N -- N -- N (0e29c04013678ddd2b8b880504213482db773cc2)

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/vmatare/laser-lines-matching

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 0f07c689aaee14ef9e963b4c8977caf026945639
Author:     Victor Mataré <mat...@lih.rwth-aachen.de>
AuthorDate: Sat Jun 25 19:59:27 2016 +0200
Commit:     Tim Niemueller <niemuel...@kbsg.rwth-aachen.de>
CommitDate: Wed Sep 21 15:04:20 2016 +0200

    laser-lines: match lines combinatorially by distance

http://git.fawkesrobotics.org/fawkes.git/commit/0f07c68
http://trac.fawkesrobotics.org/changeset/0f07c68

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit a499d79289aac8080cba4c888f9a68def953cbd9
Author:     Tim Niemueller <niemuel...@kbsg.rwth-aachen.de>
AuthorDate: Thu Sep 1 11:05:09 2016 +0200
Commit:     Tim Niemueller <niemuel...@kbsg.rwth-aachen.de>
CommitDate: Wed Sep 21 15:04:20 2016 +0200

    laser-lines: fix line deletion in visualization
    
    The refactoring of the visualization code (moving the loop out of the
    function) caused double creation of deletion messages. Bring back the
    old style and make the visualization function aware of raw and smoothed
    lines.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 3f9c34504025ec4d65565f7ab7399aebcb5849aa
Author:     Victor Mataré <mat...@lih.rwth-aachen.de>
AuthorDate: Fri Sep 2 15:03:28 2016 +0200
Commit:     Tim Niemueller <niemuel...@kbsg.rwth-aachen.de>
CommitDate: Wed Sep 21 15:04:20 2016 +0200

    laser-lines: catch TransformException
    
    Try to track in input frame as long as transform is unavailable.

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

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 0e29c04013678ddd2b8b880504213482db773cc2
Author:     Victor Mataré <mat...@lih.rwth-aachen.de>
AuthorDate: Fri Sep 2 15:18:10 2016 +0200
Commit:     Tim Niemueller <niemuel...@kbsg.rwth-aachen.de>
CommitDate: Wed Sep 21 15:04:20 2016 +0200

    laser-lines: use configurable odom frame for tracking

http://git.fawkesrobotics.org/fawkes.git/commit/0e29c04
http://trac.fawkesrobotics.org/changeset/0e29c04

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


- *Summary* -----------------------------------------------------------
 cfg/conf.d/gazsim.yaml                             |    2 +-
 cfg/conf.d/joystick.yaml                           |   57 +-
 cfg/conf.d/navgraph.yaml                           |   30 +-
 cfg/conf.d/robotino.yaml                           |  115 +--
 cfg/conf.d/ros.yaml                                |    4 -
 cfg/conf.d/skiller.yaml                            |   37 +-
 etc/buildsys/boost.mk                              |   15 +-
 etc/buildsys/lua.mk                                |    4 +-
 src/libs/blackboard/Makefile                       |   22 +-
 src/libs/blackboard/blackboard.tolua               |   65 --
 src/libs/blackboard/exceptions.h                   |   13 +-
 src/libs/blackboard/internal/interface_manager.cpp |   19 +-
 src/libs/blackboard/ownership.cpp                  |    6 -
 src/libs/config/net_handler.h                      |    4 +-
 src/libs/config/netconf.cpp                        |   14 +-
 src/libs/interface/interface.cpp                   |   17 +-
 src/libs/interface/interface.tolua                 |   24 +-
 src/libs/interface/message.tolua                   |    4 +-
 src/libs/interfaces/BatteryInterface.tolua         |   51 +-
 src/libs/interfaces/CameraControlInterface.tolua   |   68 +--
 src/libs/interfaces/EclipseDebuggerInterface.tolua |   34 +-
 src/libs/interfaces/FacerInterface.tolua           |  136 +---
 .../interfaces/FacialExpressionInterface.tolua     |   85 +--
 src/libs/interfaces/GameStateInterface.tolua       |   68 +--
 src/libs/interfaces/GripperInterface.tolua         |   51 +-
 src/libs/interfaces/HumanSkeletonInterface.tolua   |   17 +-
 src/libs/interfaces/HumanoidMotionInterface.tolua  |  204 +-----
 src/libs/interfaces/IMUInterface.tolua             |   17 +-
 src/libs/interfaces/JointInterface.tolua           |   17 +-
 src/libs/interfaces/JoystickInterface.tolua        |   68 +--
 src/libs/interfaces/KeyValueInterface.tolua        |   17 +-
 src/libs/interfaces/KickerInterface.tolua          |   68 +--
 src/libs/interfaces/Laser1080Interface.tolua       |   17 +-
 src/libs/interfaces/Laser360Interface.tolua        |   17 +-
 src/libs/interfaces/Laser720Interface.tolua        |   17 +-
 src/libs/interfaces/LedInterface.tolua             |   68 +--
 src/libs/interfaces/LocalizationInterface.tolua    |   34 +-
 src/libs/interfaces/MotorInterface.tolua           |  204 +-----
 src/libs/interfaces/NavPathInterface.tolua         |   17 +-
 src/libs/interfaces/NavigatorInterface.tolua       |  289 +------
 src/libs/interfaces/ObjectPositionInterface.tolua  |   17 +-
 src/libs/interfaces/Position2DTrackInterface.tolua |   17 +-
 src/libs/interfaces/Position3DInterface.tolua      |   17 +-
 src/libs/interfaces/SkillerInterface.tolua         |  102 +--
 src/libs/interfaces/SoccerPenaltyInterface.tolua   |   34 +-
 .../interfaces/SpeechRecognitionInterface.tolua    |   51 +-
 src/libs/interfaces/SpeechSynthInterface.tolua     |   34 +-
 src/libs/interfaces/SwitchInterface.tolua          |   85 +--
 src/libs/interfaces/TestInterface.tolua            |   68 +--
 src/libs/interfaces/TransformInterface.tolua       |   17 +-
 src/libs/interfaces/VisualDisplay2DInterface.tolua |  119 +---
 src/libs/interfaces/generator/tolua_generator.cpp  |   58 +--
 src/libs/interfaces/generator/tolua_generator.h    |    2 -
 src/libs/logging/component.cpp                     |   19 -
 src/libs/logging/component.h                       |    2 -
 src/libs/lua/context.cpp                           |  211 +----
 src/libs/lua/context.h                             |   17 +-
 src/libs/plugin/manager.cpp                        |   80 +-
 src/libs/plugin/net/handler.cpp                    |    4 +-
 src/libs/protobuf_clips/communicator.cpp           |   31 +-
 src/libs/protobuf_clips/communicator.h             |    2 +
 src/libs/tf/time_cache.cpp                         |   10 +-
 src/libs/utils/math/angle.h                        |   16 +-
 src/libs/utils/time/tracker.cpp                    |   20 -
 src/libs/utils/time/tracker.h                      |   11 -
 src/libs/utils/time/tracker_macros.h               |    4 -
 src/lua/fawkes/interface_initializer.lua           |  109 +--
 src/lua/fawkes/laser-lines_utils.lua               |    9 -
 src/lua/fawkes/mathext.lua                         |    8 +-
 src/lua/fawkes/tfutils.lua                         |   81 +--
 src/lua/fawkes/toluaext.lua                        |   42 -
 src/lua/skiller/fawkes/init.lua                    |  281 ------
 src/lua/skiller/fawkes/start.lua                   |   20 +-
 src/lua/skiller/skillenv.lua                       |  151 ++--
 src/lua/skills/generic/align_at_table.lua          |   18 +-
 src/lua/skills/generic/say.lua                     |    2 +-
 src/plugins/amcl/map_lasergen_thread.cpp           |    8 +-
 .../interfaces/OpenCVStereoParamsInterface.tolua   |  204 +-----
 src/plugins/clips/feature_blackboard.cpp           |   11 -
 src/plugins/colli/search/og_laser.cpp              |    2 +-
 src/plugins/colli/utils/rob/roboshape.cpp          |   18 +-
 .../interfaces/DynamixelServoInterface.tolua       |  340 +-------
 .../gazsim_vis_localization_thread.cpp             |    4 +-
 src/plugins/jaco/bimanual_goto_thread.cpp          |    4 +-
 src/plugins/jaco/goto_thread.cpp                   |   14 +-
 .../jaco/interfaces/JacoBimanualInterface.tolua    |   85 +--
 src/plugins/jaco/interfaces/JacoInterface.tolua    |  170 +----
 src/plugins/joystick/acquisition_thread.cpp        |  308 +++----
 src/plugins/joystick/acquisition_thread.h          |   56 +-
 src/plugins/joystick/joystick_teleop_thread.cpp    |  112 +---
 src/plugins/joystick/joystick_teleop_thread.h      |   10 +-
 .../katana/interfaces/KatanaInterface.tolua        |  289 +------
 .../interfaces/LaserClusterInterface.tolua         |   51 +-
 .../interfaces/LaserLineInterface.tolua            |   17 +-
 .../nao/interfaces/NaoJointPositionInterface.tolua |   85 +--
 .../interfaces/NaoJointStiffnessInterface.tolua    |   68 +--
 .../nao/interfaces/NaoSensorInterface.tolua        |   68 +--
 .../interfaces/NavGraphGeneratorInterface.cpp      |  194 ++--
 .../interfaces/NavGraphGeneratorInterface.h_ext    |   60 +-
 .../interfaces/NavGraphGeneratorInterface.tolua    |  320 +------
 .../interfaces/NavGraphGeneratorInterface.xml      |   12 +-
 .../navgraph_generator_thread.cpp                  |   14 +-
 src/plugins/navgraph/Makefile                      |   36 +-
 src/plugins/navgraph/navgraph_plugin.cpp           |   12 -
 src/plugins/navgraph/navgraph_thread.cpp           |    4 +-
 src/plugins/navgraph/rospub_thread.cpp             |  129 ---
 src/plugins/navgraph/rospub_thread.h               |   66 --
 src/plugins/navgraph/visualization_thread.cpp      |   25 -
 .../HumanSkeletonProjectionInterface.tolua         |   17 +-
 .../openrave/interfaces/OpenRaveInterface.tolua    |  204 +-----
 .../pantilt/interfaces/PanTiltInterface.tolua      |  170 +----
 .../interfaces/PclDatabaseMergeInterface.tolua     |   34 +-
 .../interfaces/PclDatabaseRetrieveInterface.tolua  |   34 +-
 .../interfaces/PclDatabaseStoreInterface.tolua     |   34 +-
 src/plugins/robotino/Makefile                      |   85 +--
 src/plugins/robotino/act_thread.cpp                |  675 +++++++-------
 src/plugins/robotino/act_thread.h                  |  155 ++--
 src/plugins/robotino/com_thread.cpp                |  863 ++++++++++++-------
 src/plugins/robotino/com_thread.h                  |  225 +++---
 src/plugins/robotino/direct_com_message.cpp        |  900 ------------------
 src/plugins/robotino/direct_com_message.h          |  232 -----
 src/plugins/robotino/direct_com_thread.cpp         |  951 --------------------
 src/plugins/robotino/direct_com_thread.h           |  136 ---
 .../interfaces/RobotinoSensorInterface.cpp         |  204 +-----
 .../interfaces/RobotinoSensorInterface.h_ext       |   43 +-
 .../interfaces/RobotinoSensorInterface.tolua       |   68 +--
 .../interfaces/RobotinoSensorInterface.xml         |   13 +-
 src/plugins/robotino/openrobotino_com_thread.cpp   |  545 -----------
 src/plugins/robotino/openrobotino_com_thread.h     |  149 ---
 src/plugins/robotino/robotino.mk                   |   22 -
 src/plugins/robotino/robotino_plugin.cpp           |   49 +-
 src/plugins/robotino/sensor_thread.cpp             |  137 +---
 src/plugins/robotino/sensor_thread.h               |   44 +-
 src/plugins/roomba/Roomba500Interface.tolua        |  119 +---
 src/plugins/ros/pcl_thread.cpp                     |  217 +-----
 src/plugins/ros/pcl_thread.h                       |   36 +-
 src/plugins/ros/skiller_thread.cpp                 |   11 +-
 src/plugins/skiller/SkillerDebugInterface.tolua    |   68 +--
 src/plugins/skiller/exec_thread.cpp                |  503 ++++++++++-
 src/plugins/skiller/exec_thread.h                  |   40 +-
 src/tools/skillgui/skillgui.cpp                    |    4 -
 141 files changed, 2474 insertions(+), 11094 deletions(-)
 delete mode 100644 src/libs/blackboard/blackboard.tolua
 delete mode 100644 src/lua/fawkes/toluaext.lua
 delete mode 100644 src/lua/skiller/fawkes/init.lua
 delete mode 100644 src/plugins/navgraph/rospub_thread.cpp
 delete mode 100644 src/plugins/navgraph/rospub_thread.h
 delete mode 100644 src/plugins/robotino/direct_com_message.cpp
 delete mode 100644 src/plugins/robotino/direct_com_message.h
 delete mode 100644 src/plugins/robotino/direct_com_thread.cpp
 delete mode 100644 src/plugins/robotino/direct_com_thread.h
 delete mode 100644 src/plugins/robotino/openrobotino_com_thread.cpp
 delete mode 100644 src/plugins/robotino/openrobotino_com_thread.h


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

- *commit* 0f07c689aaee14ef9e963b4c8977caf026945639 - - - - - - - - - -
Author:  Victor Mataré <mat...@lih.rwth-aachen.de>
Date:    Sat Jun 25 19:59:27 2016 +0200
Subject: laser-lines: match lines combinatorially by distance

 src/plugins/laser-lines/Makefile               |    2 +-
 src/plugins/laser-lines/laser-lines-thread.cpp |  153 +++++++++++-------------
 src/plugins/laser-lines/laser-lines-thread.h   |    4 +-
 src/plugins/laser-lines/line_info.cpp          |  111 +++++++++++++++++
 src/plugins/laser-lines/line_info.h            |   26 ++++
 5 files changed, 209 insertions(+), 87 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/Makefile b/src/plugins/laser-lines/Makefile
index 89e85ac..253a730 100644
--- a/src/plugins/laser-lines/Makefile
+++ b/src/plugins/laser-lines/Makefile
@@ -39,7 +39,7 @@ CFLAGS += $(CFLAGS_CPP11) -Wno-unknown-pragmas
 LIBS_laser_lines = fawkescore fawkesutils fawkesaspects fvutils fawkesbaseapp \
                        fawkestf fawkesinterface fawkesblackboard 
fawkespcl_utils \
                        Position3DInterface SwitchInterface LaserLineInterface
-OBJS_laser_lines = laser-lines-plugin.o laser-lines-thread.o
+OBJS_laser_lines = laser-lines-plugin.o laser-lines-thread.o  line_info.o
 
 OBJS_all    = $(OBJS_laser_lines)
 
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp 
b/src/plugins/laser-lines/laser-lines-thread.cpp
index 787b75a..b9faf65 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -117,7 +117,6 @@ LaserLinesThread::init()
     if(cfg_moving_avg_enabled_)
     {
       line_avg_ifs_.resize(cfg_max_num_lines_, NULL);
-      moving_average_windows_.resize(cfg_max_num_lines_);
     }
     for (unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
       char *tmp;
@@ -270,6 +269,7 @@ LaserLinesThread::loop()
   //logger->log_info(name(), "[L %u] total: %zu   finite: %zu",
   //                loop_count_, input_->points.size(), 
in_cloud->points.size());
 
+  {
   std::vector<LineInfo> linfos =
     calc_lines<PointType>(input_,
                          cfg_segm_min_inliers_, cfg_segm_max_iterations_,
@@ -289,96 +289,77 @@ LaserLinesThread::loop()
   lines_->height = 1;
   lines_->width  = num_points;
 
-  // sort lines by bearing to stabilize IDs
-  std::sort(linfos.begin(), linfos.end(),
-           [](const LineInfo &l1, const LineInfo &l2) -> bool
-           {
-             return l1.bearing < l2.bearing;
-           });
-
-  if (linfos.size() > cfg_max_num_lines_){
-    // Ignore lines if more than cfg_max_num_lines_ found.
-    // Since the lines are orderd by bearing, this will drop
-    // the line with the highest bearing.
-    linfos.resize(cfg_max_num_lines_);
+  vector<TrackedLineInfo>::iterator known_it = known_lines_.begin();
+  while (known_it != known_lines_.end()) {
+    btScalar min_dist = numeric_limits<btScalar>::max();
+    auto best_match = linfos.end();
+    for (vector<LineInfo>::iterator it_new = linfos.begin(); it_new != 
linfos.end(); ++it_new) {
+      btScalar d = known_it->distance(*it_new);
+      if (d < min_dist) {
+       min_dist = d;
+       best_match = it_new;
+      }
+    }
+    if (best_match != linfos.end() && min_dist < cfg_switch_tolerance_) {
+      known_it->update(*best_match);
+
+      // Important: erase line because all lines remaining after this are 
considered "new" (see below)
+      linfos.erase(best_match);
+      ++known_it;
+    }
+    else // No match for this line, so kill it
+      known_it = known_lines_.erase(known_it);
   }
 
+  for (LineInfo &l : linfos) {
+    // Only unmatched lines remaining, so these are the "new" lines
+    TrackedLineInfo tl(
+       tf_listener,
+       finput_->header.frame_id,
+       cfg_switch_tolerance_,
+       cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1);
+    tl.update(l);
+    known_lines_.push_back(tl);
+  }
 
-  std::vector<LineInfo> linfos_filtered;
-  linfos_filtered.resize(linfos.size());
-  if(cfg_moving_avg_enabled_)
-  {
-    
-    for(size_t info_cntr = 0; info_cntr < linfos.size(); ++info_cntr)
-    {
-      std::deque<LineInfo> &moving_avg_window = 
moving_average_windows_[info_cntr];
-      LineInfo &info = linfos[info_cntr];
-      LineInfo &filtered = linfos_filtered[info_cntr];
-      moving_avg_window.push_front(LineInfo(info));
-      if(moving_avg_window.size() > cfg_moving_avg_window_size_) {
-        moving_avg_window.pop_back();
+  }
+
+  // When there are too many lines, delete the ones farthest away
+  std::sort(known_lines_.begin(), known_lines_.end(),
+      [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool
+      {
+       return l1.raw.point_on_line.norm() < l2.raw.point_on_line.norm();
       }
-      
-      float if_point_on_line_avg[3] = {0,0,0};
-      float if_line_direction_avg[3] = {0,0,0};
-      float if_end_point_1_avg[3] = {0,0,0};
-      float if_end_point_2_avg[3] = {0,0,0};
-      float bearing_avg = 0;
-      float length_avg = 0;
-      
-      for(const LineInfo &avg_info: moving_avg_window) {
-        if_point_on_line_avg[0] += avg_info.base_point[0];
-        if_point_on_line_avg[1] += avg_info.base_point[1];
-        if_point_on_line_avg[2] += avg_info.base_point[2];
-        if_line_direction_avg[0] += avg_info.line_direction[0];
-        if_line_direction_avg[1] += avg_info.line_direction[1];
-        if_line_direction_avg[2] += avg_info.line_direction[2];
-        if_end_point_1_avg[0] += avg_info.end_point_1[0];
-        if_end_point_1_avg[1] += avg_info.end_point_1[1];
-        if_end_point_1_avg[2] += avg_info.end_point_1[2];
-        if_end_point_2_avg[0] += avg_info.end_point_2[0];
-        if_end_point_2_avg[1] += avg_info.end_point_2[1];
-        if_end_point_2_avg[2] += avg_info.end_point_2[2];
-        bearing_avg += avg_info.bearing;
-        length_avg += avg_info.length;
+  );
+  while (known_lines_.size() > cfg_max_num_lines_)
+    known_lines_.erase(known_lines_.end() - 1);
+
+  // Then sort by bearing to stabilize blackboard interface assignment
+  std::sort(known_lines_.begin(), known_lines_.end(),
+      [](const TrackedLineInfo &l1, const TrackedLineInfo &l2) -> bool
+      {
+       return l1.bearing_center < l2.bearing_center;
       }
-      
-      size_t queue_size = moving_avg_window.size();
-      filtered.base_point = {if_point_on_line_avg[0] / queue_size,
-                             if_point_on_line_avg[1] / queue_size,
-                             if_point_on_line_avg[2] / queue_size};
-      filtered.line_direction = {if_line_direction_avg[0] / queue_size,
-                                 if_line_direction_avg[1] / queue_size,
-                                 if_line_direction_avg[2] / queue_size};
-      filtered.end_point_1 = {if_end_point_1_avg[0] / queue_size,
-                              if_end_point_1_avg[1] / queue_size,
-                              if_end_point_1_avg[2] / queue_size};
-      filtered.end_point_2 = {if_end_point_2_avg[0] / queue_size,
-                              if_end_point_2_avg[1] / queue_size,
-                              if_end_point_2_avg[2] / queue_size};
-      filtered.bearing = bearing_avg / queue_size;
-      filtered.length = length_avg / queue_size;
-    }
-  }
+  );
+
   // set line parameters
   size_t oi = 0;
   unsigned int line_if_idx = 0;
-  for (size_t i = 0; i < linfos.size(); ++i) {
-    const LineInfo &info = linfos[i];
-    const LineInfo &info_avg = linfos_filtered[i];
+  for (size_t i = 0; i < known_lines_.size(); ++i) {
+    const TrackedLineInfo &info = known_lines_[i];
 
     if (line_if_idx < cfg_max_num_lines_) {
-           set_line(line_if_idx, line_ifs_[line_if_idx], true, 
finput_->header.frame_id, info);
+      set_line(line_if_idx, line_ifs_[line_if_idx], true, 
finput_->header.frame_id, info.raw);
       if(cfg_moving_avg_enabled_)
       {
-        set_line(line_if_idx, line_avg_ifs_[line_if_idx], true, 
finput_->header.frame_id, info_avg);
+        set_line(line_if_idx, line_avg_ifs_[line_if_idx], true, 
finput_->header.frame_id, info.smooth);
       }
       line_if_idx++;
     }
 
-    for (size_t p = 0; p < info.cloud->points.size(); ++p) {
+    for (size_t p = 0; p < info.raw.cloud->points.size(); ++p) {
       ColorPointType &out_point = lines_->points[oi++];
-      PointType &in_point  = info.cloud->points[p];
+      PointType &in_point  = info.raw.cloud->points[p];
       out_point.x = in_point.x;
       out_point.y = in_point.y;
       out_point.z = in_point.z;
@@ -394,17 +375,23 @@ LaserLinesThread::loop()
   }
 
   for (unsigned int i = line_if_idx; i < cfg_max_num_lines_; ++i) {
-         set_line(i, line_ifs_[i], false);
+    set_line(i, line_ifs_[i], false);
     if(cfg_moving_avg_enabled_)
     {
-           set_line(i, line_avg_ifs_[i], false);
+      set_line(i, line_avg_ifs_[i], false);
     }
   }
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  publish_visualization(linfos, "laser_lines");
-  if(cfg_moving_avg_enabled_) {
-         publish_visualization(linfos_filtered, "laser_lines_moving_average", 
"_avg");
+  { unsigned int line_id = 0;
+  for (size_t i = 0; i < known_lines_.size(); i++) {
+    publish_visualization(known_lines_[i].raw, i, line_id, "laser_lines");
+    line_id++;
+    if (cfg_moving_avg_enabled_) {
+      publish_visualization(known_lines_[i].smooth, i, line_id, 
"laser_lines_moving_average", "_avg");
+      line_id++;
+    }
+  }
   }
 #endif
 
@@ -429,6 +416,7 @@ LaserLinesThread::loop()
 }
 
 
+
 void
 LaserLinesThread::set_line(unsigned int idx,
                            fawkes::LaserLineInterface *iface,
@@ -523,15 +511,12 @@ LaserLinesThread::set_line(unsigned int idx,
 
 #ifdef HAVE_VISUAL_DEBUGGING
 void
-LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos,
+LaserLinesThread::publish_visualization(const LineInfo &info, size_t i, 
unsigned int idnum,
                                         std::string marker_namespace, 
std::string name_suffix)
 {
   visualization_msgs::MarkerArray m;
-  unsigned int idnum = 0;
 
-  for (size_t i = 0; i < linfos.size(); ++i) {
-    const LineInfo &info = linfos[i];
-    
+  {
     /*
     visualization_msgs::Marker basevec;
     basevec.header.frame_id = finput_->header.frame_id;
diff --git a/src/plugins/laser-lines/laser-lines-thread.h 
b/src/plugins/laser-lines/laser-lines-thread.h
index 136cf13..a531a44 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -105,7 +105,7 @@ class LaserLinesThread
 
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  void publish_visualization(const std::vector<LineInfo> &linfos,
+  void publish_visualization(const LineInfo &linfo, size_t i, unsigned int 
idnum,
                              std::string marker_namespace, std::string 
name_suffix = "");
 #endif
 
@@ -117,7 +117,7 @@ class LaserLinesThread
 
   std::vector<fawkes::LaserLineInterface *> line_ifs_;
   std::vector<fawkes::LaserLineInterface *> line_avg_ifs_;
-  std::vector<std::deque<LineInfo>> moving_average_windows_;
+  std::vector<TrackedLineInfo> known_lines_;
 
   fawkes::SwitchInterface *switch_if_;
 
diff --git a/src/plugins/laser-lines/line_info.h 
b/src/plugins/laser-lines/line_info.h
index 9c60765..e77c154 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -25,6 +25,10 @@
 #include <Eigen/Geometry>
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
+#include <boost/circular_buffer.hpp>
+#include <memory>
+#include <tf/types.h>
+#include <tf/transformer.h>
 
 /** Line information container.
  * All points and angles are in the sensor reference frame
@@ -49,4 +53,26 @@ class LineInfo {
                                                ///< points account to this line
 };
 
+
+class TrackedLineInfo {
+public:
+  LineInfo raw;        ///< the latest geometry of this line, i.e. unfiltered
+  LineInfo smooth;     ///< moving-average geometry of this line (cf. length 
of history buffer)
+  fawkes::tf::Stamped<fawkes::tf::Point> base_point_odom;      ///< last 
reference point (in odom frame) for line tracking
+  fawkes::tf::Transformer *transformer;        ///< Transformer used to 
transform from input_frame_id_to odom
+  std::string input_frame_id;  ///< Input frame ID of raw line infos 
(base_laser usually)
+  float cfg_switch_tolerance;  ///< Configured line jitter threshold
+  boost::circular_buffer<LineInfo> history;    ///< history of raw line 
geometries for computing moving average
+  float bearing_center;        ///< Bearing towards line center, used to 
select lines "in front of us" when there
+
+  TrackedLineInfo(
+      fawkes::tf::Transformer *tfer,
+      const std::string &input_frame_id,
+      float cfg_switch_tolerance,
+      unsigned int cfg_moving_avg_len);
+
+  btScalar distance(const LineInfo &linfo) const;
+  void update(LineInfo &new_linfo);
+};
+
 #endif

- *commit* a499d79289aac8080cba4c888f9a68def953cbd9 - - - - - - - - - -
Author:  Tim Niemueller <niemuel...@kbsg.rwth-aachen.de>
Date:    Thu Sep 1 11:05:09 2016 +0200
Subject: laser-lines: fix line deletion in visualization

 src/plugins/laser-lines/laser-lines-thread.cpp |  318 ++++++++++++------------
 src/plugins/laser-lines/laser-lines-thread.h   |   15 +-
 2 files changed, 173 insertions(+), 160 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp 
b/src/plugins/laser-lines/laser-lines-thread.cpp
index b9faf65..0581f61 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -39,7 +39,6 @@
 
 #ifdef HAVE_VISUAL_DEBUGGING
 #  include <ros/ros.h>
-#  include <visualization_msgs/MarkerArray.h>
 #endif
 
 #include <iostream>
@@ -383,16 +382,7 @@ LaserLinesThread::loop()
   }
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  { unsigned int line_id = 0;
-  for (size_t i = 0; i < known_lines_.size(); i++) {
-    publish_visualization(known_lines_[i].raw, i, line_id, "laser_lines");
-    line_id++;
-    if (cfg_moving_avg_enabled_) {
-      publish_visualization(known_lines_[i].smooth, i, line_id, 
"laser_lines_moving_average", "_avg");
-      line_id++;
-    }
-  }
-  }
+  publish_visualization(known_lines_, "laser_lines", 
"laser_lines_moving_average");
 #endif
 
   //*lines_ = *tmp_lines;
@@ -511,15 +501,16 @@ LaserLinesThread::set_line(unsigned int idx,
 
 #ifdef HAVE_VISUAL_DEBUGGING
 void
-LaserLinesThread::publish_visualization(const LineInfo &info, size_t i, 
unsigned int idnum,
-                                        std::string marker_namespace, 
std::string name_suffix)
+LaserLinesThread::publish_visualization_add_line(visualization_msgs::MarkerArray
 &m,
+                                                 unsigned int &idnum,
+                                                 const LineInfo &info,
+                                                 const size_t i,
+                                                 const std::string 
&marker_namespace,
+                                                 const std::string 
&name_suffix)
 {
-  visualization_msgs::MarkerArray m;
-
-  {
-    /*
-    visualization_msgs::Marker basevec;
-    basevec.header.frame_id = finput_->header.frame_id;
+       /*
+         visualization_msgs::Marker basevec;
+         basevec.header.frame_id = finput_->header.frame_id;
     basevec.header.stamp = ros::Time::now();
     basevec.ns = marker_namespace;
     basevec.id = idnum++;
@@ -537,145 +528,160 @@ LaserLinesThread::publish_visualization(const LineInfo 
&info, size_t i, unsigned
     basevec.color.a = 1.0;
     basevec.lifetime = ros::Duration(2, 0);
     m.markers.push_back(basevec);
-    */
-
-    visualization_msgs::Marker dirvec;
-    dirvec.header.frame_id = finput_->header.frame_id;
-    dirvec.header.stamp = ros::Time::now();
-    dirvec.ns = marker_namespace;
-    dirvec.id = idnum++;
-    dirvec.type = visualization_msgs::Marker::ARROW;
-    dirvec.action = visualization_msgs::Marker::ADD;
-    dirvec.points.resize(2);
-    dirvec.points[0].x = info.base_point[0];
-    dirvec.points[0].y = info.base_point[1];
-    dirvec.points[0].z = info.base_point[2];
-    dirvec.points[1].x = info.base_point[0] + info.line_direction[0];
-    dirvec.points[1].y = info.base_point[1] + info.line_direction[1];
-    dirvec.points[1].z = info.base_point[2] + info.line_direction[2];
-    dirvec.scale.x = 0.02;
-    dirvec.scale.y = 0.04;
-    dirvec.color.r = 0.0;
-    dirvec.color.g = 1.0;
-    dirvec.color.b = 0.f;
-    dirvec.color.a = 1.0;
-    dirvec.lifetime = ros::Duration(2, 0);
-    m.markers.push_back(dirvec);
-
-    visualization_msgs::Marker testvec;
-    testvec.header.frame_id = finput_->header.frame_id;
-    testvec.header.stamp = ros::Time::now();
-    testvec.ns = marker_namespace;
-    testvec.id = idnum++;
-    testvec.type = visualization_msgs::Marker::ARROW;
-    testvec.action = visualization_msgs::Marker::ADD;
-    testvec.points.resize(2);
-    testvec.points[0].x = 0; //info.point_on_line[0];
-    testvec.points[0].y = 0; //info.point_on_line[1];
-    testvec.points[0].z = 0; //info.point_on_line[2];
-    testvec.points[1].x = info.base_point[0];
-    testvec.points[1].y = info.base_point[1];
-    testvec.points[1].z = info.base_point[2];
-    testvec.scale.x = 0.02;
-    testvec.scale.y = 0.04;
-    testvec.color.r = line_colors[i][0] / 255.;
-    testvec.color.g = line_colors[i][1] / 255.;
-    testvec.color.b = line_colors[i][2] / 255.;
-    testvec.color.a = 1.0;
-    testvec.lifetime = ros::Duration(2, 0);
-    m.markers.push_back(testvec);
+       */
 
-    char *tmp;
-    if (asprintf(&tmp, "L_%zu%s", i+1, name_suffix.c_str()) != -1) {
-      // Copy to get memory freed on exception
-      std::string id = tmp;
-      free(tmp);
-
-      visualization_msgs::Marker text;
-      text.header.frame_id = finput_->header.frame_id;
-      text.header.stamp = ros::Time::now();
-      text.ns = marker_namespace;
-      text.id = idnum++;
-      text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
-      text.action = visualization_msgs::Marker::ADD;
-      text.pose.position.x = info.base_point[0];
-      text.pose.position.y = info.base_point[1];
-      text.pose.position.z = info.base_point[2] + .15;
-      text.pose.orientation.w = 1.;
-      text.scale.z = 0.15;
-      text.color.r = text.color.g = text.color.b = 1.0f;
-      text.color.a = 1.0;
-      text.lifetime = ros::Duration(2, 0);
-      text.text = id;
-      m.markers.push_back(text);
-    }
+       visualization_msgs::Marker dirvec;
+       dirvec.header.frame_id = finput_->header.frame_id;
+       dirvec.header.stamp = ros::Time::now();
+       dirvec.ns = marker_namespace;
+       dirvec.id = idnum++;
+       dirvec.type = visualization_msgs::Marker::ARROW;
+       dirvec.action = visualization_msgs::Marker::ADD;
+       dirvec.points.resize(2);
+       dirvec.points[0].x = info.base_point[0];
+       dirvec.points[0].y = info.base_point[1];
+       dirvec.points[0].z = info.base_point[2];
+       dirvec.points[1].x = info.base_point[0] + info.line_direction[0];
+       dirvec.points[1].y = info.base_point[1] + info.line_direction[1];
+       dirvec.points[1].z = info.base_point[2] + info.line_direction[2];
+       dirvec.scale.x = 0.02;
+       dirvec.scale.y = 0.04;
+       dirvec.color.r = 0.0;
+       dirvec.color.g = 1.0;
+       dirvec.color.b = 0.f;
+       dirvec.color.a = 1.0;
+       dirvec.lifetime = ros::Duration(2, 0);
+       m.markers.push_back(dirvec);
+
+       visualization_msgs::Marker testvec;
+       testvec.header.frame_id = finput_->header.frame_id;
+       testvec.header.stamp = ros::Time::now();
+       testvec.ns = marker_namespace;
+       testvec.id = idnum++;
+       testvec.type = visualization_msgs::Marker::ARROW;
+       testvec.action = visualization_msgs::Marker::ADD;
+       testvec.points.resize(2);
+       testvec.points[0].x = 0; //info.point_on_line[0];
+       testvec.points[0].y = 0; //info.point_on_line[1];
+       testvec.points[0].z = 0; //info.point_on_line[2];
+       testvec.points[1].x = info.base_point[0];
+       testvec.points[1].y = info.base_point[1];
+       testvec.points[1].z = info.base_point[2];
+       testvec.scale.x = 0.02;
+       testvec.scale.y = 0.04;
+       testvec.color.r = line_colors[i][0] / 255.;
+       testvec.color.g = line_colors[i][1] / 255.;
+       testvec.color.b = line_colors[i][2] / 255.;
+       testvec.color.a = 1.0;
+       testvec.lifetime = ros::Duration(2, 0);
+       m.markers.push_back(testvec);
+
+       char *tmp;
+       if (asprintf(&tmp, "L_%zu%s", i+1, name_suffix.c_str()) != -1) {
+               // Copy to get memory freed on exception
+               std::string id = tmp;
+               free(tmp);
+
+               visualization_msgs::Marker text;
+               text.header.frame_id = finput_->header.frame_id;
+               text.header.stamp = ros::Time::now();
+               text.ns = marker_namespace;
+               text.id = idnum++;
+               text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
+               text.action = visualization_msgs::Marker::ADD;
+               text.pose.position.x = info.base_point[0];
+               text.pose.position.y = info.base_point[1];
+               text.pose.position.z = info.base_point[2] + .15;
+               text.pose.orientation.w = 1.;
+               text.scale.z = 0.15;
+               text.color.r = text.color.g = text.color.b = 1.0f;
+               text.color.a = 1.0;
+               text.lifetime = ros::Duration(2, 0);
+               text.text = id;
+               m.markers.push_back(text);
+       }
 
-    if (cfg_min_length_ >= 0. || cfg_max_length_ >= 0.) {
-      visualization_msgs::Marker sphere_ep_1;
-      sphere_ep_1.header.frame_id = finput_->header.frame_id;
-      sphere_ep_1.header.stamp = ros::Time::now();
-      sphere_ep_1.ns = marker_namespace;
-      sphere_ep_1.id = idnum++;
-      sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
-      sphere_ep_1.action = visualization_msgs::Marker::ADD;
-      sphere_ep_1.pose.position.x = info.end_point_1[0];
-      sphere_ep_1.pose.position.y = info.end_point_1[1];
-      sphere_ep_1.pose.position.z = info.end_point_1[2];
-      sphere_ep_1.pose.orientation.w = 1.;
-      sphere_ep_1.scale.x = 0.05;
-      sphere_ep_1.scale.y = 0.05;
-      sphere_ep_1.scale.z = 0.05;
-      sphere_ep_1.color.r = line_colors[i][0] / 255.;
-      sphere_ep_1.color.g = line_colors[i][1] / 255.;
-      sphere_ep_1.color.b = line_colors[i][2] / 255.;
-      sphere_ep_1.color.a = 1.0;
-      sphere_ep_1.lifetime = ros::Duration(2, 0);
-      m.markers.push_back(sphere_ep_1);
-
-      visualization_msgs::Marker sphere_ep_2;
-      sphere_ep_2.header.frame_id = finput_->header.frame_id;
-      sphere_ep_2.header.stamp = ros::Time::now();
-      sphere_ep_2.ns = marker_namespace;
-      sphere_ep_2.id = idnum++;
-      sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
-      sphere_ep_2.action = visualization_msgs::Marker::ADD;
-      sphere_ep_2.pose.position.x = info.end_point_2[0];
-      sphere_ep_2.pose.position.y = info.end_point_2[1];
-      sphere_ep_2.pose.position.z = info.end_point_2[2];
-      sphere_ep_2.pose.orientation.w = 1.;
-      sphere_ep_2.scale.x = 0.05;
-      sphere_ep_2.scale.y = 0.05;
-      sphere_ep_2.scale.z = 0.05;
-      sphere_ep_2.color.r = line_colors[i][0] / 255.;
-      sphere_ep_2.color.g = line_colors[i][1] / 255.;
-      sphere_ep_2.color.b = line_colors[i][2] / 255.;
-      sphere_ep_2.color.a = 1.0;
-      sphere_ep_2.lifetime = ros::Duration(2, 0);
-      m.markers.push_back(sphere_ep_2);
-
-      visualization_msgs::Marker lineseg;
-      lineseg.header.frame_id = finput_->header.frame_id;
-      lineseg.header.stamp = ros::Time::now();
-      lineseg.ns = marker_namespace;
-      lineseg.id = idnum++;
-      lineseg.type = visualization_msgs::Marker::LINE_LIST;
-      lineseg.action = visualization_msgs::Marker::ADD;
-      lineseg.points.resize(2);
-      lineseg.points[0].x = info.end_point_1[0];
-      lineseg.points[0].y = info.end_point_1[1];
-      lineseg.points[0].z = info.end_point_1[2];
-      lineseg.points[1].x = info.end_point_2[0];
-      lineseg.points[1].y = info.end_point_2[1];
-      lineseg.points[1].z = info.end_point_2[2];
-      lineseg.scale.x = 0.02;
-      lineseg.scale.y = 0.04;
-      lineseg.color.r = line_colors[i][0] / 255.;
-      lineseg.color.g = line_colors[i][1] / 255.;
-      lineseg.color.b = line_colors[i][2] / 255.;
-      lineseg.color.a = 1.0;
-      lineseg.lifetime = ros::Duration(2, 0);
-      m.markers.push_back(lineseg);
-    }
+       if (cfg_min_length_ >= 0. || cfg_max_length_ >= 0.) {
+               visualization_msgs::Marker sphere_ep_1;
+               sphere_ep_1.header.frame_id = finput_->header.frame_id;
+               sphere_ep_1.header.stamp = ros::Time::now();
+               sphere_ep_1.ns = marker_namespace;
+               sphere_ep_1.id = idnum++;
+               sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
+               sphere_ep_1.action = visualization_msgs::Marker::ADD;
+               sphere_ep_1.pose.position.x = info.end_point_1[0];
+               sphere_ep_1.pose.position.y = info.end_point_1[1];
+               sphere_ep_1.pose.position.z = info.end_point_1[2];
+               sphere_ep_1.pose.orientation.w = 1.;
+               sphere_ep_1.scale.x = 0.05;
+               sphere_ep_1.scale.y = 0.05;
+               sphere_ep_1.scale.z = 0.05;
+               sphere_ep_1.color.r = line_colors[i][0] / 255.;
+               sphere_ep_1.color.g = line_colors[i][1] / 255.;
+               sphere_ep_1.color.b = line_colors[i][2] / 255.;
+               sphere_ep_1.color.a = 1.0;
+               sphere_ep_1.lifetime = ros::Duration(2, 0);
+               m.markers.push_back(sphere_ep_1);
+
+               visualization_msgs::Marker sphere_ep_2;
+               sphere_ep_2.header.frame_id = finput_->header.frame_id;
+               sphere_ep_2.header.stamp = ros::Time::now();
+               sphere_ep_2.ns = marker_namespace;
+               sphere_ep_2.id = idnum++;
+               sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
+               sphere_ep_2.action = visualization_msgs::Marker::ADD;
+               sphere_ep_2.pose.position.x = info.end_point_2[0];
+               sphere_ep_2.pose.position.y = info.end_point_2[1];
+               sphere_ep_2.pose.position.z = info.end_point_2[2];
+               sphere_ep_2.pose.orientation.w = 1.;
+               sphere_ep_2.scale.x = 0.05;
+               sphere_ep_2.scale.y = 0.05;
+               sphere_ep_2.scale.z = 0.05;
+               sphere_ep_2.color.r = line_colors[i][0] / 255.;
+               sphere_ep_2.color.g = line_colors[i][1] / 255.;
+               sphere_ep_2.color.b = line_colors[i][2] / 255.;
+               sphere_ep_2.color.a = 1.0;
+               sphere_ep_2.lifetime = ros::Duration(2, 0);
+               m.markers.push_back(sphere_ep_2);
+
+               visualization_msgs::Marker lineseg;
+               lineseg.header.frame_id = finput_->header.frame_id;
+               lineseg.header.stamp = ros::Time::now();
+               lineseg.ns = marker_namespace;
+               lineseg.id = idnum++;
+               lineseg.type = visualization_msgs::Marker::LINE_LIST;
+               lineseg.action = visualization_msgs::Marker::ADD;
+               lineseg.points.resize(2);
+               lineseg.points[0].x = info.end_point_1[0];
+               lineseg.points[0].y = info.end_point_1[1];
+               lineseg.points[0].z = info.end_point_1[2];
+               lineseg.points[1].x = info.end_point_2[0];
+               lineseg.points[1].y = info.end_point_2[1];
+               lineseg.points[1].z = info.end_point_2[2];
+               lineseg.scale.x = 0.02;
+               lineseg.scale.y = 0.04;
+               lineseg.color.r = line_colors[i][0] / 255.;
+               lineseg.color.g = line_colors[i][1] / 255.;
+               lineseg.color.b = line_colors[i][2] / 255.;
+               lineseg.color.a = 1.0;
+               lineseg.lifetime = ros::Duration(2, 0);
+               m.markers.push_back(lineseg);
+       }
+}
+
+void
+LaserLinesThread::publish_visualization(const std::vector<TrackedLineInfo> 
&linfos,
+                                        const std::string &marker_namespace,
+                                        const std::string 
&avg_marker_namespace)
+{
+  visualization_msgs::MarkerArray m;
+
+  unsigned int idnum = 0;
+ 
+  for (size_t i = 0; i < linfos.size(); ++i) {
+         const TrackedLineInfo &info = linfos[i];
+         publish_visualization_add_line(m, idnum, info.raw, i, 
marker_namespace);
+         publish_visualization_add_line(m, idnum, info.smooth, i, 
avg_marker_namespace, "_avg");
   }
 
   for (size_t i = idnum; i < last_id_num_; ++i) {
diff --git a/src/plugins/laser-lines/laser-lines-thread.h 
b/src/plugins/laser-lines/laser-lines-thread.h
index a531a44..a7e4138 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -40,10 +40,9 @@
 #include <Eigen/StdVector>
 #include <pcl/ModelCoefficients.h>
 
-#include <deque>
-
 #ifdef HAVE_VISUAL_DEBUGGING
 #  include <plugins/ros/aspect/ros.h>
+#  include <visualization_msgs/MarkerArray.h>
 
 namespace ros {
   class Publisher;
@@ -105,8 +104,16 @@ class LaserLinesThread
 
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  void publish_visualization(const LineInfo &linfo, size_t i, unsigned int 
idnum,
-                             std::string marker_namespace, std::string 
name_suffix = "");
+  void publish_visualization(const std::vector<TrackedLineInfo> &linfos,
+                             const std::string &marker_namespace,
+                             const std::string &avg_marker_namespace);
+
+  void publish_visualization_add_line(visualization_msgs::MarkerArray &m,
+                                      unsigned int &idnum,
+                                      const LineInfo &info,
+                                      const size_t i,
+                                      const std::string &marker_namespace,
+                                      const std::string &name_suffix = "");
 #endif
 
  private:

- *commit* 3f9c34504025ec4d65565f7ab7399aebcb5849aa - - - - - - - - - -
Author:  Victor Mataré <mat...@lih.rwth-aachen.de>
Date:    Fri Sep 2 15:03:28 2016 +0200
Subject: laser-lines: catch TransformException

 src/plugins/laser-lines/laser-lines-thread.cpp |    3 ++-
 src/plugins/laser-lines/line_info.cpp          |   22 ++++++++++++++++++----
 src/plugins/laser-lines/line_info.h            |    7 ++++++-
 3 files changed, 26 insertions(+), 6 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp 
b/src/plugins/laser-lines/laser-lines-thread.cpp
index 0581f61..a51b438 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -316,7 +316,8 @@ LaserLinesThread::loop()
        tf_listener,
        finput_->header.frame_id,
        cfg_switch_tolerance_,
-       cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1);
+       cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1,
+       logger, name());
     tl.update(l);
     known_lines_.push_back(tl);
   }
diff --git a/src/plugins/laser-lines/line_info.cpp 
b/src/plugins/laser-lines/line_info.cpp
index bd7689c..5307dd6 100644
--- a/src/plugins/laser-lines/line_info.cpp
+++ b/src/plugins/laser-lines/line_info.cpp
@@ -29,12 +29,16 @@ TrackedLineInfo::TrackedLineInfo(
     fawkes::tf::Transformer *tfer,
     const string &input_frame_id,
     float cfg_switch_tolerance,
-    unsigned int cfg_moving_avg_len)
+    unsigned int cfg_moving_avg_len,
+    fawkes::Logger *logger,
+    string plugin_name)
 : transformer(tfer),
   input_frame_id(input_frame_id),
   cfg_switch_tolerance(cfg_switch_tolerance),
   history(cfg_moving_avg_len),
-  bearing_center(0)
+  bearing_center(0),
+  logger(logger),
+  plugin_name(plugin_name)
 {}
 
 
@@ -49,7 +53,12 @@ btScalar TrackedLineInfo::distance(const LineInfo &linfo) 
const
              linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
          ), fawkes::Time(0,0), input_frame_id);
   fawkes::tf::Stamped<fawkes::tf::Point> bp_odom_new;
-  transformer->transform_point("/odom", bp_new, bp_odom_new);
+  try {
+    transformer->transform_point("/odom", bp_new, bp_odom_new);
+  } catch (fawkes::tf::TransformException &e) {
+    // Continue without tf, track in input frame instead. Warning follows on 
update() call.
+    bp_odom_new = bp_new;
+  }
 
   return (bp_odom_new - this->base_point_odom).length();
 }
@@ -66,7 +75,12 @@ void TrackedLineInfo::update(LineInfo &linfo)
          fawkes::tf::Point(
              linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
          ), fawkes::Time(0,0), input_frame_id);
-  transformer->transform_point("/odom", bp_new, this->base_point_odom);
+  try {
+    transformer->transform_point("/odom", bp_new, this->base_point_odom);
+  } catch (fawkes::tf::TransformException &e) {
+    logger->log_warn(plugin_name.c_str(), "Can't transform to odom. Attempting 
to track in %s.", input_frame_id);
+    this->base_point_odom = bp_new;
+  }
   this->history.push_back(linfo);
 
   Eigen::Vector3f base_point_sum(0,0,0), end_point_1_sum(0,0,0),
diff --git a/src/plugins/laser-lines/line_info.h 
b/src/plugins/laser-lines/line_info.h
index e77c154..d5e427e 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -29,6 +29,7 @@
 #include <memory>
 #include <tf/types.h>
 #include <tf/transformer.h>
+#include <logging/logger.h>
 
 /** Line information container.
  * All points and angles are in the sensor reference frame
@@ -64,12 +65,16 @@ public:
   float cfg_switch_tolerance;  ///< Configured line jitter threshold
   boost::circular_buffer<LineInfo> history;    ///< history of raw line 
geometries for computing moving average
   float bearing_center;        ///< Bearing towards line center, used to 
select lines "in front of us" when there
+  fawkes::Logger *logger;
+  std::string plugin_name;
 
   TrackedLineInfo(
       fawkes::tf::Transformer *tfer,
       const std::string &input_frame_id,
       float cfg_switch_tolerance,
-      unsigned int cfg_moving_avg_len);
+      unsigned int cfg_moving_avg_len,
+      fawkes::Logger *logger,
+      std::string plugin_name);
 
   btScalar distance(const LineInfo &linfo) const;
   void update(LineInfo &new_linfo);

- *commit* 0e29c04013678ddd2b8b880504213482db773cc2 - - - - - - - - - -
Author:  Victor Mataré <mat...@lih.rwth-aachen.de>
Date:    Fri Sep 2 15:18:10 2016 +0200
Subject: laser-lines: use configurable odom frame for tracking

 src/plugins/laser-lines/laser-lines-thread.cpp |    3 +++
 src/plugins/laser-lines/laser-lines-thread.h   |    1 +
 src/plugins/laser-lines/line_info.cpp          |    9 ++++++---
 src/plugins/laser-lines/line_info.h            |    6 ++++--
 4 files changed, 14 insertions(+), 5 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp 
b/src/plugins/laser-lines/laser-lines-thread.cpp
index a51b438..a6092d8 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -107,6 +107,8 @@ LaserLinesThread::init()
   cfg_result_frame_          = config->get_string(CFG_PREFIX"result_frame");
   cfg_max_num_lines_         = config->get_uint(CFG_PREFIX"max_num_lines");
 
+  cfg_tracking_frame_id_     = config->get_string("/frames/odom");
+
   finput_ = pcl_manager->get_pointcloud<PointType>(cfg_input_pcl_.c_str());
   input_ = pcl_utils::cloudptr_from_refptr(finput_);
 
@@ -315,6 +317,7 @@ LaserLinesThread::loop()
     TrackedLineInfo tl(
        tf_listener,
        finput_->header.frame_id,
+       cfg_tracking_frame_id_,
        cfg_switch_tolerance_,
        cfg_moving_avg_enabled_ ? cfg_moving_avg_window_size_ : 1,
        logger, name());
diff --git a/src/plugins/laser-lines/laser-lines-thread.h 
b/src/plugins/laser-lines/laser-lines-thread.h
index a7e4138..b8a461e 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -149,6 +149,7 @@ class LaserLinesThread
   float        cfg_max_dist_;
   bool         cfg_moving_avg_enabled_;
   unsigned int cfg_moving_avg_window_size_;
+  std::string  cfg_tracking_frame_id_;
 
   unsigned int loop_count_;
 
diff --git a/src/plugins/laser-lines/line_info.cpp 
b/src/plugins/laser-lines/line_info.cpp
index 5307dd6..97cd804 100644
--- a/src/plugins/laser-lines/line_info.cpp
+++ b/src/plugins/laser-lines/line_info.cpp
@@ -28,12 +28,14 @@ using namespace std;
 TrackedLineInfo::TrackedLineInfo(
     fawkes::tf::Transformer *tfer,
     const string &input_frame_id,
+    const string &tracking_frame_id,
     float cfg_switch_tolerance,
     unsigned int cfg_moving_avg_len,
     fawkes::Logger *logger,
     string plugin_name)
 : transformer(tfer),
   input_frame_id(input_frame_id),
+  tracking_frame_id(tracking_frame_id),
   cfg_switch_tolerance(cfg_switch_tolerance),
   history(cfg_moving_avg_len),
   bearing_center(0),
@@ -54,7 +56,7 @@ btScalar TrackedLineInfo::distance(const LineInfo &linfo) 
const
          ), fawkes::Time(0,0), input_frame_id);
   fawkes::tf::Stamped<fawkes::tf::Point> bp_odom_new;
   try {
-    transformer->transform_point("/odom", bp_new, bp_odom_new);
+    transformer->transform_point(tracking_frame_id, bp_new, bp_odom_new);
   } catch (fawkes::tf::TransformException &e) {
     // Continue without tf, track in input frame instead. Warning follows on 
update() call.
     bp_odom_new = bp_new;
@@ -76,9 +78,10 @@ void TrackedLineInfo::update(LineInfo &linfo)
              linfo.base_point[0], linfo.base_point[1], linfo.base_point[2]
          ), fawkes::Time(0,0), input_frame_id);
   try {
-    transformer->transform_point("/odom", bp_new, this->base_point_odom);
+    transformer->transform_point(tracking_frame_id, bp_new, 
this->base_point_odom);
   } catch (fawkes::tf::TransformException &e) {
-    logger->log_warn(plugin_name.c_str(), "Can't transform to odom. Attempting 
to track in %s.", input_frame_id);
+    logger->log_warn(plugin_name.c_str(), "Can't transform to %s. Attempting 
to track in %s.",
+       tracking_frame_id, input_frame_id);
     this->base_point_odom = bp_new;
   }
   this->history.push_back(linfo);
diff --git a/src/plugins/laser-lines/line_info.h 
b/src/plugins/laser-lines/line_info.h
index d5e427e..7dddbc9 100644
--- a/src/plugins/laser-lines/line_info.h
+++ b/src/plugins/laser-lines/line_info.h
@@ -62,15 +62,17 @@ public:
   fawkes::tf::Stamped<fawkes::tf::Point> base_point_odom;      ///< last 
reference point (in odom frame) for line tracking
   fawkes::tf::Transformer *transformer;        ///< Transformer used to 
transform from input_frame_id_to odom
   std::string input_frame_id;  ///< Input frame ID of raw line infos 
(base_laser usually)
+  std::string tracking_frame_id;       ///< Track lines relative to this frame 
(e.g. odom helps compensate movement)
   float cfg_switch_tolerance;  ///< Configured line jitter threshold
   boost::circular_buffer<LineInfo> history;    ///< history of raw line 
geometries for computing moving average
   float bearing_center;        ///< Bearing towards line center, used to 
select lines "in front of us" when there
-  fawkes::Logger *logger;
-  std::string plugin_name;
+  fawkes::Logger *logger;      ///< Logger pointer of the calling class
+  std::string plugin_name;     ///< Plugin name of the calling class
 
   TrackedLineInfo(
       fawkes::tf::Transformer *tfer,
       const std::string &input_frame_id,
+      const std::string &tracking_frame_id,
       float cfg_switch_tolerance,
       unsigned int cfg_moving_avg_len,
       fawkes::Logger *logger,




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org
_______________________________________________
fawkes-commits mailing list
fawkes-commits@lists.kbsg.rwth-aachen.de
https://lists.kbsg.rwth-aachen.de/listinfo/fawkes-commits

Reply via email to