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, thofmann/tabletop-obj has been updated discards bf8d4717f6d6f27d73643b7c9b4d4ec73727941a (commit) discards 79778fe54a6dfd745e35be6927d975b6280235e4 (commit) discards 934415e7e1f47a6be024dba17a180fd342322814 (commit) discards d80ce2d6b6b530b80391e9bf1911587a90d77dfb (commit) to 551666e2b218e04c5c4ad6ee49573465c2448374 (commit) via 2240563d93762b8f81e2cd71605eafcb8803d6a4 (commit) via 19712baa70b9e7dd439419d1cf8c2f61fd8f2866 (commit) via 98130ccf1c59b7bc65f69b4e5337e393c5b961d0 (commit) via ad12e471f1ff2cfed0fc248d064644eaa5c6e99a (commit) via abfbecf896e7df3a69114269b6137f8d8f82d6fd (commit) via 8204e4c9c992d2aeca8c68654da8a98be37962fe (commit) via 6cd25b704ca94f2121fe874b45e6f5e50c2fb959 (commit) via 60dd0906132e5f78795313065435c19b57de6285 (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 (bf8d4717f6d6f27d73643b7c9b4d4ec73727941a) \ N -- N -- N (551666e2b218e04c5c4ad6ee49573465c2448374) 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/thofmann/tabletop-obj 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 60dd0906132e5f78795313065435c19b57de6285 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Mon Apr 28 15:10:10 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:23 2014 +0200 perception: extract plugin object-tracking This is part of the split-up of the tabletop object plugin. This plugin reads object positions from the blackboard and compares them to old positions. It reassigns the IDs such that the sum of all object movement distances is minimal. http://git.fawkesrobotics.org/fawkes.git/commit/60dd090 http://trac.fawkesrobotics.org/changeset/60dd090 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 6cd25b704ca94f2121fe874b45e6f5e50c2fb959 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed Apr 30 16:16:24 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:24 2014 +0200 perception: extract plugin object-fitting The object fitting plugin fits (previously clustered) objects to known shapes. It reads point clouds and publishes 3D Positions of the recognized shapes. This is part of the split-up of the tabletop-objects plugin. http://git.fawkesrobotics.org/fawkes.git/commit/6cd25b7 http://trac.fawkesrobotics.org/changeset/6cd25b7 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 8204e4c9c992d2aeca8c68654da8a98be37962fe Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed May 7 17:28:19 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:24 2014 +0200 perception: rename tabletop-recognition to tabletop-detection http://git.fawkesrobotics.org/fawkes.git/commit/8204e4c http://trac.fawkesrobotics.org/changeset/8204e4c - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit abfbecf896e7df3a69114269b6137f8d8f82d6fd Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed May 7 17:42:04 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:25 2014 +0200 perception: rename object-recognition to object-detection http://git.fawkesrobotics.org/fawkes.git/commit/abfbecf http://trac.fawkesrobotics.org/changeset/abfbecf - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit ad12e471f1ff2cfed0fc248d064644eaa5c6e99a Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Mon May 12 17:46:13 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:26 2014 +0200 tabletop-det: print time tracking info if time tracking is enabled http://git.fawkesrobotics.org/fawkes.git/commit/ad12e47 http://trac.fawkesrobotics.org/changeset/ad12e47 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 98130ccf1c59b7bc65f69b4e5337e393c5b961d0 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Mon May 12 17:50:32 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:26 2014 +0200 obj-det: print time tracking info, fix time tracking classes http://git.fawkesrobotics.org/fawkes.git/commit/98130cc http://trac.fawkesrobotics.org/changeset/98130cc - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 19712baa70b9e7dd439419d1cf8c2f61fd8f2866 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Mon May 12 18:40:27 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:27 2014 +0200 perception: use sync points to synchronize perception plugins The perception plugins now emit or wait for sync points in order to synchronize. The complete chain is: tabletop-detection -> object-detection -> object-fitting -> object-tracking -> tabletop-visualization http://git.fawkesrobotics.org/fawkes.git/commit/19712ba http://trac.fawkesrobotics.org/changeset/19712ba - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 2240563d93762b8f81e2cd71605eafcb8803d6a4 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed May 14 15:55:17 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:27 2014 +0200 obj-tracking: check old centroids again if an ID was wrongly assigned If an ID was wrongly assigned (i.e. the new object is too far away from the old object), we used to simply drop the object. In certain situations, this leads to a lot of dropped objects. Instead of dropping the object, check the old centroids for a match. If no old object is found, add the object as new object. http://git.fawkesrobotics.org/fawkes.git/commit/2240563 http://trac.fawkesrobotics.org/changeset/2240563 - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - commit 551666e2b218e04c5c4ad6ee49573465c2448374 Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> AuthorDate: Wed May 14 15:56:08 2014 +0200 Commit: Till Hofmann <hofm...@kbsg.rwth-aachen.de> CommitDate: Wed May 14 16:01:28 2014 +0200 obj-tracking: assert that the total number of IDs doesn't change The total number of IDs (used IDS + old centroid IDs + free IDs) should never change. http://git.fawkesrobotics.org/fawkes.git/commit/551666e http://trac.fawkesrobotics.org/changeset/551666e - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - *Summary* ----------------------------------------------------------- cfg/conf.d/object_detection.yaml | 4 + cfg/conf.d/object_fitting.yaml | 5 + cfg/conf.d/object_tracking.yaml | 7 +- cfg/conf.d/tabletop_detection.yaml | 3 + cfg/conf.d/tabletop_visualization.yaml | 4 + src/plugins/perception/object-detection/Makefile | 2 +- .../object-detection/object_detection_thread.cpp | 34 ++++- .../object-detection/object_detection_thread.h | 16 ++- src/plugins/perception/object-fitting/Makefile | 2 +- .../object-fitting/object_fitting_thread.cpp | 15 ++- .../object-fitting/object_fitting_thread.h | 11 ++- src/plugins/perception/object-tracking/Makefile | 3 +- .../object-tracking/object_tracking_thread.cpp | 159 ++++++++++++-------- .../object-tracking/object_tracking_thread.h | 15 ++- src/plugins/perception/tabletop-detection/Makefile | 2 +- .../tabletop_detection_thread.cpp | 14 ++ .../tabletop-detection/tabletop_detection_thread.h | 11 ++- .../perception/tabletop-visualization/Makefile | 2 +- .../visualization_thread.cpp | 10 ++ .../tabletop-visualization/visualization_thread.h | 13 ++- 20 files changed, 252 insertions(+), 80 deletions(-) - *Diffs* ------------------------------------------------------------- - *commit* 60dd0906132e5f78795313065435c19b57de6285 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Mon Apr 28 15:10:10 2014 +0200 Subject: perception: extract plugin object-tracking cfg/conf.d/object_tracking.yaml | 18 + src/plugins/perception/Makefile | 4 +- src/plugins/perception/object-tracking/Makefile | 70 ++++ .../object-tracking/object_tracking_plugin.cpp} | 21 +- .../object-tracking/object_tracking_thread.cpp | 337 ++++++++++++++++++++ .../object_tracking_thread.h} | 99 ++---- 6 files changed, 471 insertions(+), 78 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/Makefile b/src/plugins/perception/Makefile index 123d49d..7cb1285 100644 --- a/src/plugins/perception/Makefile +++ b/src/plugins/perception/Makefile @@ -19,10 +19,12 @@ include $(BASEDIR)/etc/buildsys/config.mk include $(BUILDSYSDIR)/fvconf.mk SUBDIRS = base fountain retriever tabletop-objects pcl-db \ - tabletop-recognition object-recognition common tabletop-visualization + tabletop-recognition object-recognition common tabletop-visualization \ + object-tracking tabletop-recognition: common object-recognition: common +object-tracking: common tabletop-visualization: common include $(BUILDSYSDIR)/rules.mk - *commit* 6cd25b704ca94f2121fe874b45e6f5e50c2fb959 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed Apr 30 16:16:24 2014 +0200 Subject: perception: extract plugin object-fitting cfg/conf.d/object_fitting.yaml | 19 + src/plugins/perception/Makefile | 3 +- .../perception/common/perception_common.cpp | 22 + src/plugins/perception/common/perception_common.h | 1 + src/plugins/perception/object-fitting/Makefile | 66 +++ .../object_fitting_plugin.cpp} | 18 +- .../object-fitting/object_fitting_thread.cpp | 419 ++++++++++++++++++++ .../object-fitting/object_fitting_thread.h | 106 +++++ 8 files changed, 644 insertions(+), 10 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/Makefile b/src/plugins/perception/Makefile index 7cb1285..2ee71fb 100644 --- a/src/plugins/perception/Makefile +++ b/src/plugins/perception/Makefile @@ -20,11 +20,12 @@ include $(BUILDSYSDIR)/fvconf.mk SUBDIRS = base fountain retriever tabletop-objects pcl-db \ tabletop-recognition object-recognition common tabletop-visualization \ - object-tracking + object-tracking object-fitting tabletop-recognition: common object-recognition: common object-tracking: common +object-fitting: common tabletop-visualization: common include $(BUILDSYSDIR)/rules.mk diff --git a/src/plugins/perception/common/perception_common.cpp b/src/plugins/perception/common/perception_common.cpp index 5f7c13e..d49d96e 100644 --- a/src/plugins/perception/common/perception_common.cpp +++ b/src/plugins/perception/common/perception_common.cpp @@ -106,5 +106,27 @@ Eigen::Vector3f quaternion_to_normal(Eigen::Quaternionf quat, return quat._transformVector(reference); } +void convert_colored_input(ColorCloudConstPtr input, CloudPtr output) +{ + output->header.seq = input->header.seq; + output->header.frame_id = input->header.frame_id; + output->header.stamp = input->header.stamp; + output->width = input->width; + output->height = input->height; + output->is_dense = input->is_dense; + + const size_t size = input->points.size(); + output->points.resize(size); + for (size_t i = 0; i < size; ++i) { + const ColorPointType &in = input->points[i]; + PointType &out = output->points[i]; + + out.x = in.x; + out.y = in.y; + out.z = in.z; + } +} + + } // namespace perception } // namespace fawkes diff --git a/src/plugins/perception/common/perception_common.h b/src/plugins/perception/common/perception_common.h index b9054b5..eaf3db3 100644 --- a/src/plugins/perception/common/perception_common.h +++ b/src/plugins/perception/common/perception_common.h @@ -62,6 +62,7 @@ Eigen::Quaternionf normal_to_quaternion(Eigen::Vector3f normal, Eigen::Vector3f reference = Eigen::Vector3f(0.f, 0.f, 1.f)); Eigen::Vector3f quaternion_to_normal(Eigen::Quaternionf quat, Eigen::Vector3f reference = Eigen::Vector3f(0.f, 0.f, 1.f)); +void convert_colored_input(ColorCloudConstPtr input, CloudPtr output); } } - *commit* 8204e4c9c992d2aeca8c68654da8a98be37962fe - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed May 7 17:28:19 2014 +0200 Subject: perception: rename tabletop-recognition to tabletop-detection ...op_recognition.yaml => tabletop_detection.yaml} | 2 +- src/plugins/perception/Makefile | 4 +- .../Makefile | 20 ++++++------ .../tabletop_detection_plugin.cpp} | 14 ++++---- .../tabletop_detection_thread.cpp} | 34 ++++++++++---------- .../tabletop_detection_thread.h} | 12 +++--- 6 files changed, 43 insertions(+), 43 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/Makefile b/src/plugins/perception/Makefile index 2ee71fb..6bf643b 100644 --- a/src/plugins/perception/Makefile +++ b/src/plugins/perception/Makefile @@ -19,10 +19,10 @@ include $(BASEDIR)/etc/buildsys/config.mk include $(BUILDSYSDIR)/fvconf.mk SUBDIRS = base fountain retriever tabletop-objects pcl-db \ - tabletop-recognition object-recognition common tabletop-visualization \ + tabletop-detection object-recognition common tabletop-visualization \ object-tracking object-fitting -tabletop-recognition: common +tabletop-detection: common object-recognition: common object-tracking: common object-fitting: common - *commit* abfbecf896e7df3a69114269b6137f8d8f82d6fd - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed May 7 17:42:04 2014 +0200 Subject: perception: rename object-recognition to object-detection ...ject_recognition.yaml => object_detection.yaml} | 2 +- src/plugins/perception/Makefile | 4 +- .../Makefile | 20 ++++++------ .../object_detection_plugin.cpp} | 14 ++++---- .../object_detection_thread.cpp} | 32 ++++++++++---------- .../object_detection_thread.h} | 14 ++++---- 6 files changed, 43 insertions(+), 43 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/Makefile b/src/plugins/perception/Makefile index 6bf643b..02f2114 100644 --- a/src/plugins/perception/Makefile +++ b/src/plugins/perception/Makefile @@ -19,11 +19,11 @@ include $(BASEDIR)/etc/buildsys/config.mk include $(BUILDSYSDIR)/fvconf.mk SUBDIRS = base fountain retriever tabletop-objects pcl-db \ - tabletop-detection object-recognition common tabletop-visualization \ + tabletop-detection object-detection common tabletop-visualization \ object-tracking object-fitting tabletop-detection: common -object-recognition: common +object-detection: common object-tracking: common object-fitting: common tabletop-visualization: common - *commit* ad12e471f1ff2cfed0fc248d064644eaa5c6e99a - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Mon May 12 17:46:13 2014 +0200 Subject: tabletop-det: print time tracking info if time tracking is enabled .../tabletop_detection_thread.cpp | 7 +++++++ 1 files changed, 7 insertions(+), 0 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp b/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp index c0ae71f..278dd44 100644 --- a/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp +++ b/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp @@ -955,6 +955,13 @@ TabletopDetectionThread::loop() TIMETRACK_END(ttc_table_to_output_); TIMETRACK_END(ttc_full_loop_); + +#ifdef USE_TIMETRACKER + if (++tt_loopcount_ >= 5) { + tt_loopcount_ = 0; + tt_->print_to_stdout(); + } +#endif } - *commit* 98130ccf1c59b7bc65f69b4e5337e393c5b961d0 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Mon May 12 17:50:32 2014 +0200 Subject: obj-det: print time tracking info, fix time tracking classes .../object-detection/object_detection_thread.cpp | 19 ++++++++++++++++++- .../object-detection/object_detection_thread.h | 4 ---- 2 files changed, 18 insertions(+), 5 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/object-detection/object_detection_thread.cpp b/src/plugins/perception/object-detection/object_detection_thread.cpp index 9983915..87774f2 100644 --- a/src/plugins/perception/object-detection/object_detection_thread.cpp +++ b/src/plugins/perception/object-detection/object_detection_thread.cpp @@ -132,6 +132,14 @@ ObjectDetectionThread::init() } centroids_.clear(); + +#ifdef USE_TIMETRACKER + tt_ = new TimeTracker(); + tt_loopcount_ = 0; + ttc_full_loop_ = tt_->add_class("Full Loop"); + ttc_obj_extraction_ = tt_->add_class("Object Extraction"); +#endif + } void @@ -169,6 +177,9 @@ ObjectDetectionThread::finalize() void ObjectDetectionThread::loop() { + + TIMETRACK_START(ttc_full_loop_); + table_pos_if_->read(); if (table_pos_if_->visibility_history() < 0) { logger->log_debug(name(), "No tabletop. Aborting object detection."); @@ -207,7 +218,6 @@ ObjectDetectionThread::loop() set_position(pos_ifs_[it->first], true, it->second, Eigen::Quaternionf(1, 0, 0, 0), "/base_link"); } - TIMETRACK_START(ttc_visualization_); for (unsigned int i = 0; i < f_obj_clusters_.size(); i++) { if (centroids_.count(i)) { *obj_clusters_[i] = *tmp_obj_clusters[i]; @@ -222,6 +232,13 @@ ObjectDetectionThread::loop() pcl_utils::copy_time(input_, f_obj_clusters_[i]); } + TIMETRACK_END(ttc_full_loop_); +#ifdef USE_TIMETRACKER + if (++tt_loopcount_ >= 5) { + tt_loopcount_ = 0; + tt_->print_to_stdout(); + } +#endif } unsigned int diff --git a/src/plugins/perception/object-detection/object_detection_thread.h b/src/plugins/perception/object-detection/object_detection_thread.h index bbdc59a..dda923c 100644 --- a/src/plugins/perception/object-detection/object_detection_thread.h +++ b/src/plugins/perception/object-detection/object_detection_thread.h @@ -161,10 +161,6 @@ class ObjectDetectionThread fawkes::TimeTracker *tt_; unsigned int tt_loopcount_; unsigned int ttc_full_loop_; - unsigned int ttc_cluster_objects_; - unsigned int ttc_visualization_; - unsigned int ttc_hungarian_; - unsigned int ttc_old_centroids_; unsigned int ttc_obj_extraction_; #endif }; - *commit* 19712baa70b9e7dd439419d1cf8c2f61fd8f2866 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Mon May 12 18:40:27 2014 +0200 Subject: perception: use sync points to synchronize perception plugins cfg/conf.d/object_detection.yaml | 4 ++++ cfg/conf.d/object_fitting.yaml | 5 +++++ cfg/conf.d/object_tracking.yaml | 7 ++++++- cfg/conf.d/tabletop_detection.yaml | 3 +++ cfg/conf.d/tabletop_visualization.yaml | 4 ++++ src/plugins/perception/object-detection/Makefile | 2 +- .../object-detection/object_detection_thread.cpp | 15 +++++++++++++++ .../object-detection/object_detection_thread.h | 12 +++++++++++- src/plugins/perception/object-fitting/Makefile | 2 +- .../object-fitting/object_fitting_thread.cpp | 13 +++++++++++++ .../object-fitting/object_fitting_thread.h | 11 ++++++++++- src/plugins/perception/object-tracking/Makefile | 3 ++- .../object-tracking/object_tracking_thread.cpp | 12 ++++++++++++ .../object-tracking/object_tracking_thread.h | 11 ++++++++++- src/plugins/perception/tabletop-detection/Makefile | 2 +- .../tabletop_detection_thread.cpp | 7 +++++++ .../tabletop-detection/tabletop_detection_thread.h | 11 ++++++++++- .../perception/tabletop-visualization/Makefile | 2 +- .../visualization_thread.cpp | 10 ++++++++++ .../tabletop-visualization/visualization_thread.h | 13 ++++++++++++- 20 files changed, 138 insertions(+), 11 deletions(-) _Diff for modified files_: diff --git a/cfg/conf.d/object_detection.yaml b/cfg/conf.d/object_detection.yaml index 2506128..b27719f 100644 --- a/cfg/conf.d/object_detection.yaml +++ b/cfg/conf.d/object_detection.yaml @@ -22,3 +22,7 @@ perception/object-detection: # max height of a centroid relative to the table centroid_max_height: 0.3 + # SyncPoint to wait for at the beginning of the loop + syncpoint_in: "/perception/tabletop-detection" + # SyncPoint to emit at the end of the loop + syncpoint_out: "/perception/object-detection" diff --git a/cfg/conf.d/object_fitting.yaml b/cfg/conf.d/object_fitting.yaml index c207eff..af78cf5 100644 --- a/cfg/conf.d/object_fitting.yaml +++ b/cfg/conf.d/object_fitting.yaml @@ -17,3 +17,8 @@ perception/object-fitting: # Print fitting results verbose: false + + # SyncPoint to wait for at the beginning of the loop + syncpoint_in: "/perception/object-detection" + # SyncPoint to emit at the end of the loop + syncpoint_out: "/perception/object-fitting" diff --git a/cfg/conf.d/object_tracking.yaml b/cfg/conf.d/object_tracking.yaml index b86d9bf..33b7d03 100644 --- a/cfg/conf.d/object_tracking.yaml +++ b/cfg/conf.d/object_tracking.yaml @@ -5,7 +5,7 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/tabletop-objects --- perception/object-tracking: # Input interface names (regexp) - interfaces_in: "Tabletop Object *" + interfaces_in: "Fitted Object *" # Output interface names (base name) interfaces_out: "Tracked Object %u" # max number of loops a centroid is saved @@ -16,3 +16,8 @@ perception/object-tracking: # min distance between two centroids, old centroids will be deleted if they are closer centroid_min_distance: 0.01 + + # SyncPoint to wait for at the beginning of the loop + syncpoint_in: "/perception/object-fitting" + # SyncPoint to emit at the end of the loop + syncpoint_out: "/perception/object-tracking" diff --git a/cfg/conf.d/tabletop_detection.yaml b/cfg/conf.d/tabletop_detection.yaml index cb9bb4b..47aa992 100644 --- a/cfg/conf.d/tabletop_detection.yaml +++ b/cfg/conf.d/tabletop_detection.yaml @@ -79,3 +79,6 @@ perception/tabletop-detection: # Publish the object pointcloud under this name # The object pointcloud is the pointcloud above the table (excluding the table itself) object_pointcloud: "objects" + + # The name of the sync point which is emitted at the end of the loop + syncpoint: "/perception/tabletop-detection" diff --git a/cfg/conf.d/tabletop_visualization.yaml b/cfg/conf.d/tabletop_visualization.yaml index baac52f..2aa9299 100644 --- a/cfg/conf.d/tabletop_visualization.yaml +++ b/cfg/conf.d/tabletop_visualization.yaml @@ -6,3 +6,7 @@ doc-url: !url http://trac.fawkesrobotics.org/wiki/Plugins/tabletop-objects perception/visualization: # Input interface names (regexp) object_name_pattern: "Tracked Object *" + # SyncPoint Identifier + # This should be the name of the SyncPoint which is released + # by the last thread which is visualized + syncpoint: "/perception/object-tracking" diff --git a/src/plugins/perception/object-detection/Makefile b/src/plugins/perception/object-detection/Makefile index dc0c00d..b3588f7 100644 --- a/src/plugins/perception/object-detection/Makefile +++ b/src/plugins/perception/object-detection/Makefile @@ -38,7 +38,7 @@ endif LIBS_object_detection = stdc++ fawkescore fawkesutils fawkesaspects fvutils \ fawkestf fawkesinterface fawkesblackboard fawkespcl_utils \ Position3DInterface SwitchInterface \ - fawkesperception_common + fawkesperception_common fawkessyncpoint OBJS_object_detection = object_detection_plugin.o object_detection_thread.o OBJS_all = $(OBJS_object_detection) diff --git a/src/plugins/perception/object-detection/object_detection_thread.cpp b/src/plugins/perception/object-detection/object_detection_thread.cpp index 87774f2..74cfa0e 100644 --- a/src/plugins/perception/object-detection/object_detection_thread.cpp +++ b/src/plugins/perception/object-detection/object_detection_thread.cpp @@ -67,6 +67,8 @@ ObjectDetectionThread::init() cfg_result_frame_ = config->get_string(CFG_PREFIX"result_frame"); cfg_input_pointcloud_ = config->get_string(CFG_PREFIX"input_pointcloud"); cfg_centroid_max_height_ = config->get_float(CFG_PREFIX"centroid_max_height"); + cfg_syncpoint_in_ = config->get_string(CFG_PREFIX"syncpoint_in"); + cfg_syncpoint_out_ = config->get_string(CFG_PREFIX"syncpoint_out"); if (pcl_manager->exists_pointcloud<PointType>(cfg_input_pointcloud_.c_str())) { finput_ = pcl_manager->get_pointcloud<PointType>(cfg_input_pointcloud_.c_str()); @@ -133,10 +135,14 @@ ObjectDetectionThread::init() centroids_.clear(); + syncpoint_in_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_in_.c_str()); + syncpoint_out_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_out_.c_str()); + #ifdef USE_TIMETRACKER tt_ = new TimeTracker(); tt_loopcount_ = 0; ttc_full_loop_ = tt_->add_class("Full Loop"); + ttc_syncpoint_wait_ = tt_->add_class("SyncPoint Wait Call"); ttc_obj_extraction_ = tt_->add_class("Object Extraction"); #endif @@ -172,6 +178,9 @@ ObjectDetectionThread::finalize() it != f_obj_clusters_.end(); it++) { it->reset(); } + + syncpoint_manager->release_syncpoint(name(), syncpoint_in_); + syncpoint_manager->release_syncpoint(name(), syncpoint_out_); } void @@ -180,6 +189,10 @@ ObjectDetectionThread::loop() TIMETRACK_START(ttc_full_loop_); + TIMETRACK_START(ttc_syncpoint_wait_); + syncpoint_in_->wait(name()); + TIMETRACK_END(ttc_syncpoint_wait_); + table_pos_if_->read(); if (table_pos_if_->visibility_history() < 0) { logger->log_debug(name(), "No tabletop. Aborting object detection."); @@ -232,6 +245,8 @@ ObjectDetectionThread::loop() pcl_utils::copy_time(input_, f_obj_clusters_[i]); } + syncpoint_out_->emit(name()); + TIMETRACK_END(ttc_full_loop_); #ifdef USE_TIMETRACKER if (++tt_loopcount_ >= 5) { diff --git a/src/plugins/perception/object-detection/object_detection_thread.h b/src/plugins/perception/object-detection/object_detection_thread.h index dda923c..06482ee 100644 --- a/src/plugins/perception/object-detection/object_detection_thread.h +++ b/src/plugins/perception/object-detection/object_detection_thread.h @@ -34,6 +34,7 @@ #include <aspect/blocked_timing.h> #include <aspect/tf.h> #include <aspect/pointcloud.h> +#include <aspect/syncpoint_manager.h> #include <Eigen/StdVector> @@ -48,6 +49,7 @@ namespace fawkes { class Position3DInterface; class SwitchInterface; class Time; + class SyncPoint; #ifdef USE_TIMETRACKER class TimeTracker; #endif @@ -104,7 +106,8 @@ class ObjectDetectionThread public fawkes::BlackBoardAspect, public fawkes::BlockedTimingAspect, public fawkes::TransformAspect, - public fawkes::PointCloudAspect + public fawkes::PointCloudAspect, + public fawkes::SyncPointManagerAspect { public: ObjectDetectionThread(); @@ -149,6 +152,10 @@ class ObjectDetectionThread PosIfsVector pos_ifs_; fawkes::Position3DInterface *table_pos_if_; + /* synchronization */ + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_in_; + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_out_; + /* configuration */ float cfg_cluster_tolerance_; unsigned int cfg_cluster_min_size_; @@ -156,11 +163,14 @@ class ObjectDetectionThread std::string cfg_result_frame_; std::string cfg_input_pointcloud_; float cfg_centroid_max_height_; + std::string cfg_syncpoint_in_; + std::string cfg_syncpoint_out_; #ifdef USE_TIMETRACKER fawkes::TimeTracker *tt_; unsigned int tt_loopcount_; unsigned int ttc_full_loop_; + unsigned int ttc_syncpoint_wait_; unsigned int ttc_obj_extraction_; #endif }; diff --git a/src/plugins/perception/object-fitting/Makefile b/src/plugins/perception/object-fitting/Makefile index 3a45678..af3787d 100644 --- a/src/plugins/perception/object-fitting/Makefile +++ b/src/plugins/perception/object-fitting/Makefile @@ -25,7 +25,7 @@ REQUIRED_PCL_LIBS = segmentation filters features LIBS_object_fitting = stdc++ fawkescore fawkesutils fawkesaspects fvutils \ fawkestf fawkesinterface fawkesblackboard fawkespcl_utils \ - Position3DInterface fawkesperception_common + Position3DInterface fawkesperception_common fawkessyncpoint OBJS_object_fitting = object_fitting_plugin.o object_fitting_thread.o diff --git a/src/plugins/perception/object-fitting/object_fitting_thread.cpp b/src/plugins/perception/object-fitting/object_fitting_thread.cpp index 1bfaef8..b219dcc 100644 --- a/src/plugins/perception/object-fitting/object_fitting_thread.cpp +++ b/src/plugins/perception/object-fitting/object_fitting_thread.cpp @@ -75,6 +75,8 @@ ObjectFittingThread::init() cfg_pointclouds_ = config->get_string(CFG_PREFIX"input_pointclouds"); cfg_output_prefix_ = config->get_string(CFG_PREFIX"output_prefix"); cfg_use_colored_input_ = config->get_bool(CFG_PREFIX"use_colored_input"); + cfg_syncpoint_in_ = config->get_string(CFG_PREFIX"syncpoint_in"); + cfg_syncpoint_out_ = config->get_string(CFG_PREFIX"syncpoint_out"); // find all pointclouds vector<string> pointclouds = pcl_manager->get_pointcloud_list(); @@ -158,6 +160,10 @@ ObjectFittingThread::init() std::vector<double> init_likelihoods; init_likelihoods.resize(known_obj_dimensions_.size() + 1, 0.0); // TODO obj_likelihoods_ initialization + + syncpoint_in_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_in_.c_str()); + syncpoint_out_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_out_.c_str()); + } void @@ -166,11 +172,16 @@ ObjectFittingThread::finalize() for (vector<Position3DInterface *>::iterator it = pos_ifs_.begin(); it != pos_ifs_.end(); it++) { blackboard->close(*it); } + + syncpoint_manager->release_syncpoint(name(), syncpoint_in_); + syncpoint_manager->release_syncpoint(name(), syncpoint_out_); } void ObjectFittingThread::loop() { + syncpoint_in_->wait(name()); + if (cfg_use_colored_input_) { for (uint i = 0; i < input_.size(); i++) { CloudPtr converted_input; @@ -205,6 +216,8 @@ ObjectFittingThread::loop() set_pos_interface(pos_ifs_[i], false); } } + + syncpoint_out_->emit(name()); } Eigen::Vector4f diff --git a/src/plugins/perception/object-fitting/object_fitting_thread.h b/src/plugins/perception/object-fitting/object_fitting_thread.h index 759871e..8192a4a 100644 --- a/src/plugins/perception/object-fitting/object_fitting_thread.h +++ b/src/plugins/perception/object-fitting/object_fitting_thread.h @@ -34,6 +34,7 @@ #include <aspect/blocked_timing.h> #include <aspect/tf.h> #include <aspect/pointcloud.h> +#include <aspect/syncpoint_manager.h> #include <interfaces/Position3DInterface.h> #include <interfaces/SwitchInterface.h> @@ -49,6 +50,7 @@ namespace fawkes { class Position3DInterface; + class SyncPoint; } class ObjectFittingThread @@ -59,7 +61,8 @@ class ObjectFittingThread public fawkes::BlackBoardAspect, public fawkes::BlockedTimingAspect, public fawkes::TransformAspect, - public fawkes::PointCloudAspect + public fawkes::PointCloudAspect, + public fawkes::SyncPointManagerAspect { public: ObjectFittingThread(); @@ -88,12 +91,18 @@ class ObjectFittingThread std::vector<fawkes::Position3DInterface *> pos_ifs_; + /* synchronization */ + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_in_; + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_out_; + /* configuration */ bool cfg_verbose_output_; std::string cfg_pointclouds_; std::string cfg_output_prefix_; bool cfg_use_colored_input_; std::string cfg_baselink_frame_id_; + std::string cfg_syncpoint_in_; + std::string cfg_syncpoint_out_; std::vector<fawkes::RefPtr<const pcl::PointCloud<fawkes::perception::PointType> > > finput_; std::vector<fawkes::perception::CloudConstPtr> input_; diff --git a/src/plugins/perception/object-tracking/Makefile b/src/plugins/perception/object-tracking/Makefile index 8e44e03..b077bf5 100644 --- a/src/plugins/perception/object-tracking/Makefile +++ b/src/plugins/perception/object-tracking/Makefile @@ -24,7 +24,8 @@ REQUIRED_PCL_LIBS = registration LIBS_object_tracking = stdc++ fawkescore fawkesutils fawkesaspects fvutils \ fawkestf fawkesinterface fawkesblackboard \ - Position3DInterface SwitchInterface fawkesperception_common + Position3DInterface SwitchInterface fawkesperception_common \ + fawkessyncpoint OBJS_object_tracking = object_tracking_plugin.o object_tracking_thread.o diff --git a/src/plugins/perception/object-tracking/object_tracking_thread.cpp b/src/plugins/perception/object-tracking/object_tracking_thread.cpp index 9aa15d2..14b133c 100644 --- a/src/plugins/perception/object-tracking/object_tracking_thread.cpp +++ b/src/plugins/perception/object-tracking/object_tracking_thread.cpp @@ -56,6 +56,8 @@ ObjectTrackingThread::init() cfg_ifs_out_ = config->get_string(CFG_PREFIX"interfaces_out"); cfg_centroid_min_distance_ = config->get_float(CFG_PREFIX"centroid_min_distance"); cfg_centroid_max_age_ = config->get_uint(CFG_PREFIX"centroid_max_age"); + cfg_syncpoint_in_ = config->get_string(CFG_PREFIX"syncpoint_in"); + cfg_syncpoint_out_ = config->get_string(CFG_PREFIX"syncpoint_out"); try { pos_ifs_in_ = blackboard->open_multiple_for_reading<Position3DInterface>(cfg_ifs_in_.c_str()); @@ -98,6 +100,10 @@ ObjectTrackingThread::init() centroids_.clear(); old_centroids_.clear(); + + syncpoint_in_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_in_.c_str()); + syncpoint_out_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_out_.c_str()); + } void @@ -109,12 +115,17 @@ ObjectTrackingThread::finalize() for(vector<Position3DInterface *>::iterator it = pos_ifs_out_.begin(); it != pos_ifs_out_.end(); it++) { blackboard->close(*it); } + + syncpoint_manager->release_syncpoint(name(), syncpoint_in_); + syncpoint_manager->release_syncpoint(name(), syncpoint_out_); } void ObjectTrackingThread::loop() { + syncpoint_in_->wait(name()); + // read the frame id from the first centroid // this is later used when writing the new positions to the blackboard // we assume all centroids have the same frame id @@ -180,6 +191,7 @@ ObjectTrackingThread::loop() if (!centroids_.empty()) first_run_ = false; + syncpoint_out_->emit(name()); } /** diff --git a/src/plugins/perception/object-tracking/object_tracking_thread.h b/src/plugins/perception/object-tracking/object_tracking_thread.h index 51ed027..c387f03 100644 --- a/src/plugins/perception/object-tracking/object_tracking_thread.h +++ b/src/plugins/perception/object-tracking/object_tracking_thread.h @@ -28,6 +28,7 @@ #include <aspect/logging.h> #include <aspect/blackboard.h> #include <aspect/blocked_timing.h> +#include <aspect/syncpoint_manager.h> #include <interfaces/Position3DInterface.h> #include <interfaces/SwitchInterface.h> @@ -41,6 +42,7 @@ namespace fawkes { class Position3DInterface; + class SyncPoint; } /** @class OldCentroid "tabletop_objects_thread.h" @@ -90,7 +92,8 @@ class ObjectTrackingThread public fawkes::LoggingAspect, public fawkes::ConfigurableAspect, public fawkes::BlackBoardAspect, - public fawkes::BlockedTimingAspect + public fawkes::BlockedTimingAspect, + public fawkes::SyncPointManagerAspect { public: ObjectTrackingThread(); @@ -127,12 +130,18 @@ class ObjectTrackingThread OldCentroidVector old_centroids_; std::string frame_id_; + /* synchronization */ + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_in_; + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_out_; + /* configuration */ uint cfg_centroid_max_age_; float cfg_centroid_max_distance_; float cfg_centroid_min_distance_; std::string cfg_ifs_in_; std::string cfg_ifs_out_; + std::string cfg_syncpoint_in_; + std::string cfg_syncpoint_out_; }; diff --git a/src/plugins/perception/tabletop-detection/Makefile b/src/plugins/perception/tabletop-detection/Makefile index 4a6e4d5..58cc34c 100644 --- a/src/plugins/perception/tabletop-detection/Makefile +++ b/src/plugins/perception/tabletop-detection/Makefile @@ -39,7 +39,7 @@ endif LIBS_tabletop_detection = fawkescore fawkesutils fawkesaspects fvutils \ fawkestf fawkesinterface fawkesblackboard fawkespcl_utils \ Position3DInterface SwitchInterface \ - fawkesperception_common + fawkesperception_common fawkessyncpoint OBJS_tabletop_detection = tabletop_detection_plugin.o tabletop_detection_thread.o OBJS_all = $(OBJS_tabletop_detection) diff --git a/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp b/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp index 278dd44..0411307 100644 --- a/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp +++ b/src/plugins/perception/tabletop-detection/tabletop_detection_thread.cpp @@ -90,6 +90,7 @@ TabletopDetectionThread::init() cfg_input_pointcloud_ = config->get_string(CFG_PREFIX"input_pointcloud"); cfg_cluster_min_size_ = config->get_uint(CFG_PREFIX"cluster_min_size"); cfg_object_pointcloud_ = config->get_string(CFG_PREFIX"object_pointcloud"); + cfg_syncpoint_ = config->get_string(CFG_PREFIX"syncpoint"); if (pcl_manager->exists_pointcloud<PointType>(cfg_input_pointcloud_.c_str())) { finput_ = pcl_manager->get_pointcloud<PointType>(cfg_input_pointcloud_.c_str()); @@ -164,6 +165,8 @@ TabletopDetectionThread::init() last_pcl_time_ = new Time(clock); + syncpoint_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_.c_str()); + #ifdef USE_TIMETRACKER tt_ = new TimeTracker(); tt_loopcount_ = 0; @@ -208,6 +211,8 @@ TabletopDetectionThread::finalize() fsimplified_polygon_.reset(); fobjects_.reset(); ftable_cluster_.reset(); + + syncpoint_manager->release_syncpoint(name(), syncpoint_); } void @@ -954,6 +959,8 @@ TabletopDetectionThread::loop() TIMETRACK_END(ttc_table_to_output_); + syncpoint_->emit(name()); + TIMETRACK_END(ttc_full_loop_); #ifdef USE_TIMETRACKER diff --git a/src/plugins/perception/tabletop-detection/tabletop_detection_thread.h b/src/plugins/perception/tabletop-detection/tabletop_detection_thread.h index ce22a50..603d87b 100644 --- a/src/plugins/perception/tabletop-detection/tabletop_detection_thread.h +++ b/src/plugins/perception/tabletop-detection/tabletop_detection_thread.h @@ -34,10 +34,13 @@ #include <aspect/blocked_timing.h> #include <aspect/pointcloud.h> #include <aspect/tf.h> +#include <aspect/syncpoint_manager.h> #include <interfaces/Position3DInterface.h> #include <interfaces/SwitchInterface.h> +#include <syncpoint/syncpoint.h> + #include <Eigen/StdVector> #include <pcl/segmentation/sac_segmentation.h> #include <pcl/filters/voxel_grid.h> @@ -48,6 +51,7 @@ namespace fawkes { class Position3DInterface; class SwitchInterface; class Time; + class SyncPoint; #ifdef USE_TIMETRACKER class TimeTracker; #endif @@ -61,7 +65,8 @@ class TabletopDetectionThread public fawkes::BlackBoardAspect, public fawkes::BlockedTimingAspect, public fawkes::TransformAspect, - public fawkes::PointCloudAspect + public fawkes::PointCloudAspect, + public fawkes::SyncPointManagerAspect { public: TabletopDetectionThread(); @@ -117,6 +122,7 @@ class TabletopDetectionThread std::string cfg_input_pointcloud_; unsigned int cfg_cluster_min_size_; std::string cfg_object_pointcloud_; + std::string cfg_syncpoint_; fawkes::SwitchInterface *switch_if_; fawkes::Position3DInterface *table_pos_if_; @@ -143,6 +149,9 @@ class TabletopDetectionThread fawkes::Time *last_pcl_time_; unsigned int loop_count_; + /* synchronization */ + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_; + #ifdef USE_TIMETRACKER fawkes::TimeTracker *tt_; unsigned int tt_loopcount_; diff --git a/src/plugins/perception/tabletop-visualization/Makefile b/src/plugins/perception/tabletop-visualization/Makefile index 80652ae..3b00a55 100644 --- a/src/plugins/perception/tabletop-visualization/Makefile +++ b/src/plugins/perception/tabletop-visualization/Makefile @@ -23,7 +23,7 @@ include $(BUILDCONFDIR)/tf/tf.mk LIBS_tabletop_visualization = fawkescore fawkesutils fawkesaspects fvutils \ fawkestf fawkesinterface fawkesblackboard \ - Position3DInterface fawkesperception_common + Position3DInterface fawkesperception_common fawkessyncpoint OBJS_tabletop_visualization = visualization_plugin.o visualization_thread.o diff --git a/src/plugins/perception/tabletop-visualization/visualization_thread.cpp b/src/plugins/perception/tabletop-visualization/visualization_thread.cpp index fdbe7bc..8333a3a 100644 --- a/src/plugins/perception/tabletop-visualization/visualization_thread.cpp +++ b/src/plugins/perception/tabletop-visualization/visualization_thread.cpp @@ -89,6 +89,10 @@ TabletopVisualizationThread::init() try { cfg_object_name_pattern_ = config->get_string(CFG_PREFIX_VIS"object_name_pattern"); } catch (Exception &e) {} // ignored, use default + cfg_syncpoint_ = "/perception/object-detection"; + try { + cfg_syncpoint_ = config->get_string(CFG_PREFIX_VIS"syncpoint"); + } catch (Exception &e) {} // ignored, use default vispub_ = new ros::Publisher(); *vispub_ = rosnode->advertise<visualization_msgs::MarkerArray>("visualization_marker_array", 100); @@ -101,6 +105,8 @@ TabletopVisualizationThread::init() obj_pos_ifs_ = blackboard->open_multiple_for_reading<Position3DInterface>(cfg_object_name_pattern_.c_str()); table_pos_if_ = blackboard->open_for_reading<Position3DInterface>("Tabletop"); + syncpoint_ = syncpoint_manager->get_syncpoint(name(), cfg_syncpoint_.c_str()); + } void @@ -130,12 +136,16 @@ TabletopVisualizationThread::finalize() blackboard->close(*it); } blackboard->close(table_pos_if_); + + syncpoint_manager->release_syncpoint(name(), syncpoint_); } void TabletopVisualizationThread::loop() { + syncpoint_->wait(name()); + MutexLocker lock(&mutex_); visualization_msgs::MarkerArray m; diff --git a/src/plugins/perception/tabletop-visualization/visualization_thread.h b/src/plugins/perception/tabletop-visualization/visualization_thread.h index a725fc1..0555980 100644 --- a/src/plugins/perception/tabletop-visualization/visualization_thread.h +++ b/src/plugins/perception/tabletop-visualization/visualization_thread.h @@ -36,6 +36,7 @@ #include <aspect/blackboard.h> #include <aspect/blocked_timing.h> #include <aspect/logging.h> +#include <aspect/syncpoint_manager.h> #include <plugins/ros/aspect/ros.h> #include <interfaces/Position3DInterface.h> @@ -49,6 +50,10 @@ #include <list> #include <string> +namespace fawkes { + class SyncPoint; +} + namespace ros { class Publisher; } @@ -60,7 +65,8 @@ class TabletopVisualizationThread public fawkes::ConfigurableAspect, public fawkes::ROSAspect, public fawkes::BlackBoardAspect, - public fawkes::LoggingAspect + public fawkes::LoggingAspect, + public fawkes::SyncPointManagerAspect { public: @@ -88,8 +94,13 @@ class TabletopVisualizationThread size_t last_id_num_; std::string frame_id_; + /* synchronization */ + fawkes::RefPtr<fawkes::SyncPoint> syncpoint_; + unsigned int cfg_duration_; std::string cfg_object_name_pattern_; + std::string cfg_syncpoint_; + public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; - *commit* 2240563d93762b8f81e2cd71605eafcb8803d6a4 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed May 14 15:55:17 2014 +0200 Subject: obj-tracking: check old centroids again if an ID was wrongly assigned .../object-tracking/object_tracking_thread.cpp | 16 +++++++++++++++- 1 files changed, 15 insertions(+), 1 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/object-tracking/object_tracking_thread.cpp b/src/plugins/perception/object-tracking/object_tracking_thread.cpp index 14b133c..9cbd76f 100644 --- a/src/plugins/perception/object-tracking/object_tracking_thread.cpp +++ b/src/plugins/perception/object-tracking/object_tracking_thread.cpp @@ -281,9 +281,23 @@ ObjectTrackingThread::track_objects( // (then, the old centroid is assigned to the new one) if (pcl::distances::l2(centroids_[id], new_centroids[row]) > cfg_centroid_max_distance_) { - // save the centroid because we don't use it now + // save the (old) centroid because we don't use it now old_centroids_.push_back(OldCentroid(id, centroids_[id])); id = -1; + // first check all old centroids if this is an old centroid reappearing + for (OldCentroidVector::iterator it = old_centroids_.begin(); + it != old_centroids_.end(); it++) { + if (pcl::distances::l2(new_centroids[row], it->getCentroid()) + <= cfg_centroid_max_distance_) { + id = it->getId(); + old_centroids_.erase(it); + break; + } + } + // assign a new id if no old centroid was found + if (id == -1) { + id = next_id(); + } } } final_assignment[row] = id; - *commit* 551666e2b218e04c5c4ad6ee49573465c2448374 - - - - - - - - - - Author: Till Hofmann <hofm...@kbsg.rwth-aachen.de> Date: Wed May 14 15:56:08 2014 +0200 Subject: obj-tracking: assert that the total number of IDs doesn't change .../object-tracking/object_tracking_thread.cpp | 3 +++ 1 files changed, 3 insertions(+), 0 deletions(-) _Diff for modified files_: diff --git a/src/plugins/perception/object-tracking/object_tracking_thread.cpp b/src/plugins/perception/object-tracking/object_tracking_thread.cpp index 9cbd76f..b94a3d9 100644 --- a/src/plugins/perception/object-tracking/object_tracking_thread.cpp +++ b/src/plugins/perception/object-tracking/object_tracking_thread.cpp @@ -126,6 +126,9 @@ ObjectTrackingThread::loop() syncpoint_in_->wait(name()); + // make sure we didn't 'lose' any IDs + assert(old_centroids_.size() + free_ids_.size() + centroids_.size() == pos_ifs_in_.size()); + // read the frame id from the first centroid // this is later used when writing the new positions to the blackboard // we assume all centroids have the same frame id -- 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
