git commit -m "first commit"

simulators/third_party/rviz_plugins/spencer_messages/.gitignore

@@ -0,0 +1,29 @@
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

simulators/third_party/rviz_plugins/spencer_messages/LICENSE

@@ -0,0 +1,23 @@
+Copyright (c) 2016, spencer.eu
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

simulators/third_party/rviz_plugins/spencer_messages/README.md

@@ -0,0 +1 @@
+# spencer_messages

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/CHANGELOG.rst

@@ -0,0 +1,35 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_control_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Adding missing spencer_control_msgs package
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/CMakeLists.txt

@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_control_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+  FILES
+  ComponentStatus.msg
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs message_runtime
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2013-2015 Timm Linder, Social Robotics Laboratory, University of Freiburg
+Copyright (c) 2014-2015 Stefan Breuers, Lucas Beyer, Computer Vision Group, RWTH Aachen University
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/msg/ComponentStatus.msg

@@ -0,0 +1,3 @@
+string name
+bool alive
+string detail

simulators/third_party/rviz_plugins/spencer_messages/spencer_control_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_control_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for controlling the SPENCER robot platform.</description>
+
+  <maintainer email="linder@cs.uni-freiburg.de">Timm Linder</maintainer>
+  <author email="linder@cs.uni-freiburg.de">Timm Linder</author>
+  
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/CHANGELOG.rst

@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_human_attribute_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#2 <https://github.com/LCAS/spencer_people_tracking/issues/2>`_ from spencer-project/master
+  Integrate multiple fixes from upstream
+* Specify correct license in package.xmls
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Marc Hanheide, Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Updated licenses
+* Adding message definitions, RViz plugins and mock scripts
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_human_attribute_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   CategoricalAttribute.msg
+   ScalarAttribute.msg
+   HumanAttributes.msg
+)
+
+# Generate services in the 'srv' folder
+#add_service_files(
+#  FILES
+#)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs message_runtime 
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/LICENSE

@@ -0,0 +1,28 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2014-2015 Timm Linder, Social Robotics Laboratory, University of Freiburg
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/msg/CategoricalAttribute.msg

@@ -0,0 +1,32 @@
+uint64                  subject_id              # either an observation ID or a track ID (if information has been integrated over time); should be encoded in topic name
+string                  type                    # type of the attribute, see constants below
+
+string[]                values                  # values, each value also should have a confidence, highest-confidence attribute should come first
+float32[]               confidences             # corresponding confidences should sum up to 1.0, highest confidence comes first
+
+
+##################################################
+### Constants for categorical attribute types. ###
+##################################################
+
+string      GENDER        = gender
+string      AGE_GROUP     = age_group
+
+###################################################
+### Constants for categorical attribute values. ###
+###################################################
+
+string      GENDER_MALE             = male
+string      GENDER_FEMALE           = female
+
+# Age groups are based upon the categories from the "Images Of Groups" RGB database
+string      AGE_GROUP_0_TO_2        = 0-2
+string      AGE_GROUP_3_TO_7        = 3-7
+string      AGE_GROUP_8_TO_12       = 8-12
+string      AGE_GROUP_13_TO_19      = 13-19
+string      AGE_GROUP_20_TO_36      = 20-36
+string      AGE_GROUP_37_TO_65      = 37-65
+string      AGE_GROUP_66_TO_99      = 66-99
+
+
+

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/msg/HumanAttributes.msg

@@ -0,0 +1,5 @@
+std_msgs/Header header
+
+# One entry per attribute type per person
+CategoricalAttribute[]   categoricalAttributes
+ScalarAttribute[]       scalarAttributes

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/msg/ScalarAttribute.msg

@@ -0,0 +1,12 @@
+uint64                  subject_id              # either an observation ID or a track ID (if information has been integrated over time); should be encoded in topic name
+string                  type                    # type of the attribute, see constants below
+
+float32[]               values                  # values, each value also should have a confidence; highest-confidence value comes first!
+float32[]               confidences             # corresponding confidences should sum up to 1.0
+
+
+###########################################
+### Constants for scalar attribute types. #
+###########################################
+
+string      PERSON_HEIGHT        = person_height

simulators/third_party/rviz_plugins/spencer_messages/spencer_human_attribute_msgs/package.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_human_attribute_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for Human Attribute Recognition</description>
+
+  <maintainer email="linder@informatik.uni-freiburg.de">Timm Linder</maintainer>
+  <author email="linder@informatik.uni-freiburg.de">Timm Linder</author>
+  
+  <license>BSD</license>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/CHANGELOG.rst

@@ -0,0 +1,40 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_social_relation_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#2 <https://github.com/LCAS/spencer_people_tracking/issues/2>`_ from spencer-project/master
+  Integrate multiple fixes from upstream
+* Specify correct license in package.xmls
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Marc Hanheide, Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Updating lots of utility packages to latest version from SPENCER repo. Licenses updated.
+* Updated licenses
+* Adding message definitions, RViz plugins and mock scripts
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/CMakeLists.txt

@@ -0,0 +1,46 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_social_relation_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  sensor_msgs
+  nav_msgs
+  geometry_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   SocialRelation.msg
+   SocialRelations.msg
+   SocialActivity.msg
+   SocialActivities.msg
+)
+
+# Generate services in the 'srv' folder
+#add_service_files(
+#  FILES
+#)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+   sensor_msgs
+   nav_msgs
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs sensor_msgs nav_msgs geometry_msgs message_runtime 
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/LICENSE

@@ -0,0 +1,28 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2014-2015 Timm Linder, Billy Okal, Social Robotics Laboratory, University of Freiburg
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialActivities.msg

@@ -0,0 +1,5 @@
+std_msgs/Header     header
+
+# All social activities that have been detected in the current time step,
+# within sensor range of the robot.
+SocialActivity[]    elements

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialActivity.msg

@@ -0,0 +1,24 @@
+string      type        # see constants below
+float32     confidence  # detection confidence
+
+uint64[]      track_ids   # IDs of all person tracks involved in the activity, might be one or multiple
+
+
+# Constants for social activity type (just examples at the moment)
+string      TYPE_SHOPPING = shopping
+string      TYPE_STANDING = standing
+string      TYPE_INDIVIDUAL_MOVING = individual_moving
+string      TYPE_WAITING_IN_QUEUE = waiting_in_queue
+string      TYPE_LOOKING_AT_INFORMATION_SCREEN = looking_at_information_screen
+string      TYPE_LOOKING_AT_KIOSK = looking_at_kiosk
+string      TYPE_GROUP_ASSEMBLING = group_assembling
+string      TYPE_GROUP_MOVING = group_moving
+string      TYPE_FLOW_WITH_ROBOT = flow
+string      TYPE_ANTIFLOW_AGAINST_ROBOT = antiflow
+string      TYPE_WAITING_FOR_OTHERS = waiting_for_others
+string      TYPE_LOOKING_FOR_HELP = looking_for_help
+
+
+#string      TYPE_COMMUNICATING = communicating
+#string      TYPE_TAKING_PHOTOGRAPH = taking_photograph
+#string      TYPE_TALKING_ON_PHONE = talking_on_phone

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialRelation.msg

@@ -0,0 +1,12 @@
+string      type        # e.g. mother-son relationship, romantic relationship, etc.
+float32     strength    # relationship strength between 0.0 and 1.0
+
+uint32      track1_id
+uint32      track2_id
+
+
+# Constants for type (just examples at the moment)
+string      TYPE_SPATIAL  = "spatial"
+string      TYPE_ROMANTIC = "romantic"
+string      TYPE_PARENT_CHILD = "parent_child"
+string      TYPE_FRIENDSHIP = "friendship"

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/msg/SocialRelations.msg

@@ -0,0 +1,5 @@
+std_msgs/Header     header
+
+# All social relations of all tracks in the current time step.
+# There might be multiple social relations per pair of tracks.
+SocialRelation[]    elements

simulators/third_party/rviz_plugins/spencer_messages/spencer_social_relation_msgs/package.xml

@@ -0,0 +1,28 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_social_relation_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for Social Activity Detection and Social Relation Analysis</description>
+
+  <maintainer email="linder@informatik.uni-freiburg.de">Timm Linder</maintainer>
+  <license>BSD</license>
+  
+  <author email="linder@informatik.uni-freiburg.de">Timm Linder</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>nav_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+  <run_depend>nav_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/CHANGELOG.rst

@@ -0,0 +1,41 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_tracking_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#2 <https://github.com/LCAS/spencer_people_tracking/issues/2>`_ from spencer-project/master
+  Integrate multiple fixes from upstream
+* Specify correct license in package.xmls
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Marc Hanheide, Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Updating lots of utility packages to latest version from SPENCER repo. Licenses updated.
+* Updated licenses
+* Adding is_matched field in TrackedPerson message to distinguish between misdetections and physical occlusions
+* Adding message definitions, RViz plugins and mock scripts
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/CMakeLists.txt

@@ -0,0 +1,52 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_tracking_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  geometry_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   DetectedPerson.msg
+   DetectedPersons.msg
+   CompositeDetectedPerson.msg
+   CompositeDetectedPersons.msg
+   TrackedPerson.msg
+   TrackedPersons.msg
+   TrackedPerson2d.msg
+   TrackedPersons2d.msg
+   TrackedGroup.msg
+   TrackedGroups.msg
+   ImmDebugInfo.msg
+   ImmDebugInfos.msg
+   TrackingTimingMetrics.msg
+)
+
+# Generate services in the 'srv' folder
+add_service_files(
+  FILES
+  GetPersonTrajectories.srv
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs geometry_msgs message_runtime 
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2013-2015 Timm Linder, Social Robotics Laboratory, University of Freiburg
+Copyright (c) 2014-2015 Stefan Breuers, Lucas Beyer, Computer Vision Group, RWTH Aachen University
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/CompositeDetectedPerson.msg

@@ -0,0 +1,20 @@
+# Specifies which detected persons have been merged into a composite detection by the spencer_detected_person_association module.
+
+# TODO: Do we need a composite person-specific timestamp (or even a full message header including frame ID)?
+# Having a separate timestamp per person could be useful if the timestamps of the merged DetectedPersons messages vary a lot,
+# and some persons are only seen by a single sensor (so averaging over all input timestamps would have a detrimental effect). 
+
+uint64      composite_detection_id          # ID of the fused detection
+
+float64     mean_confidence                 # mean of the confidences of the original detections
+float64     max_confidence                  # maximum confidence of original detections
+float64     min_confidence                  # minimum confidence of original detections
+
+
+geometry_msgs/PoseWithCovariance    pose    # Merged 3D pose (position + orientation) of the detection center
+                                            # check covariance to see which dimensions are actually set!
+                                            # unset dimensions shall have a covariance > 9999
+
+DetectedPerson[] original_detections        # The original detections from individual sensor-specific detectors that have been combined into a composite detection
+                                            # We are copying the entire DetectedPersons messages, *with poses transformed into the target frame*, such that subscribers
+                                            # do not have to subscribe to all the original DetectedPersons topics.

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/CompositeDetectedPersons.msg

@@ -0,0 +1,8 @@
+# Message specifying which original detected persons (from all kinds of sensors) have been merged into one fused detection before being processed by the person tracker, in the current time step.
+#
+# This information is processed by the spencer_detected_person_association module, such that other perception components can associate their results (e.g. person age, gender)
+# with a particular spencer_tracking_msgs/TrackedPerson (which contains a reference to a composite person detection ID).
+
+Header                      header          # Header timestamp is set to the *latest* timestamp of all fused DetectedPerson messages.
+                                            # Before fusion, all detections are transformed into a common coordinate frame (usually base_footprint).
+CompositeDetectedPerson[]   elements        # Fused (merged) detected persons

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/DetectedPerson.msg

@@ -0,0 +1,23 @@
+# Message describing a detection of a person
+#
+
+# Unique id of the detection, monotonically increasing over time
+uint64          detection_id
+
+# (Pseudo-)probabilistic value between 0.0 and 1.0 describing the detector's confidence in the detection
+float64         confidence
+
+# 3D pose (position + orientation) of the *center* of the detection
+# check covariance to see which dimensions are actually set! unset dimensions shall have a covariance > 9999
+geometry_msgs/PoseWithCovariance    pose    
+
+# Sensor modality / detector type, see example constants below. 
+# used e.g. later in tracking to check which tracks have been visually confirmed
+string          modality            
+
+                                    
+string          MODALITY_GENERIC_LASER_2D = laser2d
+string          MODALITY_GENERIC_LASER_3D = laser3d
+string          MODALITY_GENERIC_MONOCULAR_VISION = mono
+string          MODALITY_GENERIC_STEREO_VISION = stereo
+string          MODALITY_GENERIC_RGBD = rgbd

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/DetectedPersons.msg

@@ -0,0 +1,5 @@
+# Message with all currently detected persons
+#
+
+Header                  header          # Header containing timestamp etc. of this message
+DetectedPerson[]        detections      # All persons that are currently being detected

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/ImmDebugInfo.msg

@@ -0,0 +1,10 @@
+# Message for passing debug information of filter performance
+#
+
+uint64      track_id        # unique identifier of the target, consistent over time
+float64      innovation      # innovation of prediction and associated observation
+float64      CpXX            # variance of prediction acc. to x
+float64      CpYY            # variance of prediction acc. to y
+float64      CXX             # variance of state acc. to x
+float64      CYY             # variance of state acc. to y
+float64[]    modeProbabilities# array containing mode probabilities

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/ImmDebugInfos.msg

@@ -0,0 +1,5 @@
+# Message with all debug infos per frame
+#
+
+Header              header      # Header containing timestamp etc. of this message
+ImmDebugInfo[]      infos      # All persons that are currently being tracked

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/PersonTrajectory.msg

@@ -0,0 +1,10 @@
+# Message defining the trajectory of a tracked person.
+#
+# The distinction between track and trajectory is that, depending on the
+# implementation of the tracker, a single track (i.e. tracked person) might
+# change the trajectory if at some point a new trajectory "fits" that track (person)
+# better.
+#
+
+uint64                   track_id   # Unique identifier of the tracked person.
+PersonTrajectoryEntry[]  trajectory # All states of the last T frames of the most likely trajectory of that tracked person.

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/PersonTrajectoryEntry.msg

@@ -0,0 +1,11 @@
+# Message defining an entry of a person trajectory.
+#
+
+time     stamp           # age of the track
+bool     is_occluded     # if the track is currently not matched by a detection
+uint64   detection_id    # id of the corresponding detection in the current cycle (undefined if occluded)
+
+# The following fields are extracted from the Kalman state x and its covariance C
+
+geometry_msgs/PoseWithCovariance    pose   # pose of the track (z value and orientation might not be set, check if corresponding variance on diagonal is > 99999)
+geometry_msgs/TwistWithCovariance   twist  # velocity of the track (z value and rotational velocities might not be set, check if corresponding variance on diagonal is > 99999)

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedGroup.msg

@@ -0,0 +1,10 @@
+# Message defining a tracked group
+#
+
+uint64      group_id        # unique identifier of the target, consistent over time
+
+duration    age             # age of the group
+
+geometry_msgs/PoseWithCovariance    centerOfGravity   # mean and covariance of the group (calculated from its person tracks)
+
+uint64[]    track_ids       # IDs of the tracked persons in this group. See srl_tracking_msgs/TrackedPersons

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedGroups.msg

@@ -0,0 +1,5 @@
+# Message with all currently tracked groups 
+#
+
+Header              header      # Header containing timestamp etc. of this message
+TrackedGroup[]      groups      # All groups that are currently being tracked

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedPerson.msg

@@ -0,0 +1,14 @@
+# Message defining a tracked person
+#
+
+uint64      track_id        # unique identifier of the target, consistent over time
+bool        is_occluded     # if the track is currently not observable in a physical way
+bool        is_matched      # if the track is currently matched by a detection
+uint64      detection_id    # id of the corresponding detection in the current cycle (undefined if occluded)
+duration    age             # age of the track
+
+# The following fields are extracted from the Kalman state x and its covariance C
+
+geometry_msgs/PoseWithCovariance       pose   # pose of the track (z value and orientation might not be set, check if corresponding variance on diagonal is > 99999)
+
+geometry_msgs/TwistWithCovariance   twist     # velocity of the track (z value and rotational velocities might not be set, check if corresponding variance on diagonal is > 99999)

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedPerson2d.msg

@@ -0,0 +1,10 @@
+# Message defining a 2d image bbox of a tracked person
+#
+
+uint64      track_id        # unique identifier of the target, consistent over time
+float32     person_height   # 3d height of person in m
+int32       x               # top left corner x of 2d image bbox
+int32       y               # top left corner y of 2d image bbox
+uint32      w               # width of 2d image bbox
+uint32      h               # height of 2d image bbox
+float32     depth           # distance from the camera in m

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedPersons.msg

@@ -0,0 +1,5 @@
+# Message with all currently tracked persons 
+#
+
+Header              header      # Header containing timestamp etc. of this message
+TrackedPerson[]     tracks      # All persons that are currently being tracked

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackedPersons2d.msg

@@ -0,0 +1,5 @@
+# Message with all 2d bbox in image of currently tracked persons 
+#
+
+Header                header      # Header containing timestamp etc. of this message
+TrackedPerson2d[]     boxes       # All persons that are currently being tracked (2d image bbox)

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/msg/TrackingTimingMetrics.msg

@@ -0,0 +1,13 @@
+# Message with timing values for analyzing the efficency 
+#
+
+Header              header                  # Header containing timestamp etc. of this message
+uint64              track_count             # number of person currentl tracked
+uint64              cycle_no                # incremented number of cycles        
+float32             average_cycle_time      # average time for one cycle of tracker
+float32             cycle_time              # cycle time of current cycle
+float32             average_processing_rate # average frequency of processing data
+float32             cpu_load                # current cpu load
+float32             average_cpu_load        # average cpu load
+float32             elapsed_time            # elapsed seconds since start
+float32             elapsed_cpu_time        # elapsed cpu time since start

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_tracking_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for Person Detection and Tracking</description>
+
+  <maintainer email="linder@informatik.uni-freiburg.de">Timm Linder</maintainer>
+  <maintainer email="breuers@vision.rwth-aachen.de">Stefan Breuers</maintainer>
+  <maintainer email="beyer@vision.rwth-aachen.de">Lucas Beyer</maintainer>
+  
+  <license>BSD</license>
+  
+  <author email="linder@informatik.uni-freiburg.de">Timm Linder</author>
+  <author email="breuers@vision.rwth-aachen.de">Stefan Breuers</author>
+  <author email="beyer@vision.rwth-aachen.de">Lucas Beyer</author>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_messages/spencer_tracking_msgs/srv/GetPersonTrajectories.srv

@@ -0,0 +1,4 @@
+uint64[] requested_ids           # The IDs of the tracks you are interested in getting the trajectories of. An empty array means all available tracks.
+duration max_age                 # The maximum age of a trajectory you want to get. A duration of 0 means "since the beginning of times."
+---
+PersonTrajectory[] trajectories  # The trajectories of the tracks that have been asked for in requested_ids, in the same order.

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/CHANGELOG.rst

@@ -0,0 +1,39 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package spencer_vision_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.0.8 (2017-09-22)
+------------------
+* Merge pull request `#2 <https://github.com/LCAS/spencer_people_tracking/issues/2>`_ from spencer-project/master
+  Integrate multiple fixes from upstream
+* Specify correct license in package.xmls
+* Merge pull request `#41 <https://github.com/LCAS/spencer_people_tracking/issues/41>`_ from LCAS/master
+  1.0.7
+* Contributors: Marc Hanheide, Timm Linder
+
+1.0.7 (2017-06-09)
+------------------
+
+1.0.6 (2017-06-09)
+------------------
+
+1.0.5 (2017-05-12)
+------------------
+
+1.0.4 (2017-05-12)
+------------------
+
+1.0.3 (2017-05-11)
+------------------
+
+1.0.2 (2017-05-09)
+------------------
+* added missing roslib deps
+* Contributors: Marc Hanheide
+
+1.0.1 (2017-05-09)
+------------------
+* homogenised all version strings to 1.0.0
+* Updated licenses
+* Adding message definitions, RViz plugins and mock scripts
+* Contributors: Marc Hanheide, Timm Linder

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/CMakeLists.txt

@@ -0,0 +1,44 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_vision_msgs)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  roslib
+  std_msgs
+  sensor_msgs
+  geometry_msgs
+  message_generation
+)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# Generate messages in the 'msg' folder
+add_message_files(
+   FILES
+   PersonImage.msg
+   PersonImages.msg
+   PersonROI.msg
+   PersonROIs.msg
+)
+
+# Generate services in the 'srv' folder
+#add_service_files(
+#  FILES
+#)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   geometry_msgs
+   sensor_msgs
+   std_msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+catkin_package(
+  CATKIN_DEPENDS roscpp std_msgs sensor_msgs geometry_msgs message_runtime 
+)

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/LICENSE

@@ -0,0 +1,29 @@
+Software License Agreement (BSD License)
+
+Copyright (c) 2013-2015 Timm Linder, Social Robotics Laboratory, University of Freiburg
+Copyright (c) 2014-2015 Stefan Breuers, Lucas Beyer, Computer Vision Group, RWTH Aachen University
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+* Redistributions of source code must retain the above copyright notice, this
+  list of conditions and the following disclaimer.
+
+* Redistributions in binary form must reproduce the above copyright notice,
+  this list of conditions and the following disclaimer in the documentation
+  and/or other materials provided with the distribution.
+
+* Neither the name of the copyright holder nor the names of its contributors
+  may be used to endorse or promote products derived from this software
+  without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/msg/PersonImage.msg

@@ -0,0 +1,6 @@
+# Message describing a depth or RGB image containing a part of a person (e.g. head, face, full body...), which is usually encoded in the topic title
+#
+
+uint64              detection_id
+sensor_msgs/Image   image
+

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/msg/PersonImages.msg

@@ -0,0 +1,5 @@
+# Message describing an array of depth or RGB images containing a part of a person (e.g. head, face, full body...), which is usually encoded in the topic title
+#
+
+std_msgs/Header     header
+PersonImage[]       elements

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/msg/PersonROI.msg

@@ -0,0 +1,7 @@
+# Message describing a rectangular region of interest in a depth or RGB image containing a part of a person (e.g. head, face, full body...), which is usually encoded in the topic title
+#
+
+uint64          detection_id
+
+sensor_msgs/RegionOfInterest    roi
+

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/msg/PersonROIs.msg

@@ -0,0 +1,9 @@
+# Message describing an array of rectangular regions of interest in a depth or RGB image containing a part of a person (e.g. head, face, full body...), which is usually encoded in the topic title
+#
+
+std_msgs/Header     header
+
+string              rgb_topic       # not necessarily the raw camera output; might be preprocessed for rectification etc.
+string              depth_topic     # might not be set if depth is not available, otherwise it is the registered depth image
+
+PersonROI[]         elements

simulators/third_party/rviz_plugins/spencer_messages/spencer_vision_msgs/package.xml

@@ -0,0 +1,31 @@
+<?xml version="1.0"?>
+<package>
+
+  <name>spencer_vision_msgs</name>
+  <version>1.0.8</version>
+  <description>Messages used for Vision-related tasks in SPENCER</description>
+
+  <maintainer email="beyer@vision.rwth-aachen.de">Lucas Beyer</maintainer>
+  <maintainer email="breuers@vision.rwth-aachen.de">Stefan Breuers</maintainer>
+  <maintainer email="linder@informatik.uni-freiburg.de">Timm Linder</maintainer>
+  
+  <license>BSD</license>
+  
+  <author email="linder@informatik.uni-freiburg.de">Timm Linder</author>
+  <author email="breuers@vision.rwth-aachen.de">Stefan Breuers</author>
+  <author email="beyer@vision.rwth-aachen.de">Lucas Beyer</author>
+
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>geometry_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  
+  <run_depend>message_runtime</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>std_msgs</run_depend>
+  <run_depend>geometry_msgs</run_depend>
+  <run_depend>sensor_msgs</run_depend>
+
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/.gitignore

@@ -0,0 +1,32 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/CMakeLists.txt

@@ -0,0 +1,116 @@
+# Software License Agreement (BSD License)
+#
+#  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+#  Copyright (c) 2012, Willow Garage, Inc.
+#  All rights reserved.
+#
+#  Redistribution and use in source and binary forms, with or without
+#  modification, are permitted provided that the following conditions are met:
+#
+#  * Redistributions of source code must retain the above copyright notice, this
+#    list of conditions and the following disclaimer.
+#  * Redistributions in binary form must reproduce the above copyright notice,
+#    this list of conditions and the following disclaimer in the documentation
+#    and/or other materials provided with the distribution.
+#  * Neither the name of the copyright holder nor the names of its contributors
+#    may be used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+#  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+#  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+#  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+#  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+#  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+#  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+#  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+#  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+#  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+#  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+
+cmake_minimum_required(VERSION 2.8.3)
+project(spencer_tracking_rviz_plugin)
+
+find_package(catkin REQUIRED COMPONENTS rviz spencer_tracking_msgs spencer_human_attribute_msgs spencer_social_relation_msgs)
+catkin_package()
+include_directories(${catkin_INCLUDE_DIRS})
+link_directories(${catkin_LIBRARY_DIRS})
+
+
+
+## This plugin includes Qt widgets, so we must include Qt.
+## We'll use the version that rviz used so they are compatible.
+if(rviz_QT_VERSION VERSION_LESS "5")
+  message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui)
+  ## pull in all required include dirs, define QT_LIBRARIES, etc.
+  include(${QT_USE_FILE})
+  qt4_wrap_cpp(MOC_FILES
+  src/detected_persons_display.h
+  src/tracked_persons_display.h
+  src/tracked_groups_display.h
+  src/social_relations_display.h
+  src/social_activities_display.h
+  src/human_attributes_display.h
+  src/person_display_common.h
+  src/additional_topic_subscriber.h
+)
+else()
+  message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}")
+  find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets)
+  ## make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies
+  set(QT_LIBRARIES Qt5::Widgets)
+  qt5_wrap_cpp(MOC_FILES
+  src/detected_persons_display.h
+  src/tracked_persons_display.h
+  src/tracked_groups_display.h
+  src/social_relations_display.h
+  src/social_activities_display.h
+  src/human_attributes_display.h
+  src/person_display_common.h
+  src/additional_topic_subscriber.h
+)
+endif()
+
+add_definitions(-DQT_NO_KEYWORDS)
+
+
+
+set(SOURCE_FILES
+  src/detected_persons_display.cpp
+  src/tracked_persons_display.cpp
+  src/tracked_groups_display.cpp
+  src/social_relations_display.cpp
+  src/social_activities_display.cpp
+  src/human_attributes_display.cpp
+  src/person_display_common.cpp
+  src/tracked_persons_cache.cpp
+  src/visuals/person_visual.cpp
+  ${MOC_FILES}
+)
+
+
+add_library(${PROJECT_NAME} ${SOURCE_FILES})
+add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) # for generation of message dependencies
+target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES})
+
+
+install(TARGETS
+  ${PROJECT_NAME}
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(FILES 
+  plugin_description.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+
+install(DIRECTORY media/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/media)
+
+install(DIRECTORY icons/
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/icons)
+
+install(PROGRAMS scripts/send_test_msgs.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/package.xml

@@ -0,0 +1,30 @@
+<package>
+  <name>spencer_tracking_rviz_plugin</name>
+  <version>1.0.8</version>
+
+  <description>
+     RViz plugin for visualizing tracked and detected persons and groups
+  </description>
+
+  <maintainer email="linder@cs.uni-freiburg.de">Timm Linder</maintainer>
+  <author email="linder@cs.uni-freiburg.de">Timm Linder</author>
+
+  <license>BSD; CC BY 3.0 (Fugue Icons by Yusuke Kamiyamane)</license>
+  
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <build_depend>rviz</build_depend>
+  <build_depend>spencer_tracking_msgs</build_depend>
+  <build_depend>spencer_social_relation_msgs</build_depend>
+  <build_depend>spencer_human_attribute_msgs</build_depend>
+  
+  <run_depend>rviz</run_depend>
+  <run_depend>spencer_tracking_msgs</run_depend>
+  <run_depend>spencer_social_relation_msgs</run_depend>
+  <run_depend>spencer_human_attribute_msgs</run_depend>
+
+
+  <export>
+      <rviz plugin="${prefix}/plugin_description.xml"/>
+  </export>
+<build_depend>roslib</build_depend><run_depend>roslib</run_depend></package>

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/plugin_description.xml

@@ -0,0 +1,45 @@
+<library path="lib/libspencer_tracking_rviz_plugin">
+
+  <class name="spencer_tracking_rviz_plugin/DetectedPersons" type="spencer_tracking_rviz_plugin::DetectedPersonsDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays detected persons from spencer_tracking_msgs/DetectedPersons messages.
+    </description>
+    <message_type>spencer_tracking_msgs/DetectedPersons</message_type>
+  </class>
+  
+  <class name="spencer_tracking_rviz_plugin/TrackedPersons" type="spencer_tracking_rviz_plugin::TrackedPersonsDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays tracked persons from spencer_tracking_msgs/TrackedPersons messages.
+    </description>
+    <message_type>spencer_tracking_msgs/TrackedPersons</message_type>
+  </class>
+
+  <class name="spencer_tracking_rviz_plugin/TrackedGroups" type="spencer_tracking_rviz_plugin::TrackedGroupsDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays tracked groups from spencer_tracking_msgs/TrackedGroups messages.
+    </description>
+    <message_type>spencer_tracking_msgs/TrackedGroups</message_type>
+  </class>
+
+  <class name="spencer_tracking_rviz_plugin/SocialRelations" type="spencer_tracking_rviz_plugin::SocialRelationsDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays social relations from spencer_social_relation_msgs/SocialRelations messages.
+    </description>
+    <message_type>spencer_social_relation_msgs/SocialRelations</message_type>
+  </class>
+
+  <class name="spencer_tracking_rviz_plugin/SocialActivities" type="spencer_tracking_rviz_plugin::SocialActivitiesDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays social activities from spencer_social_relation_msgs/SocialActivities messages.
+    </description>
+    <message_type>spencer_social_relation_msgs/SocialActivities</message_type>
+  </class>
+
+  <class name="spencer_tracking_rviz_plugin/HumanAttributes" type="spencer_tracking_rviz_plugin::HumanAttributesDisplay" base_class_type="rviz::Display">
+    <description>
+      Displays human attributes from spencer_human_attribute_msgs/HumanAttributes messages.
+    </description>
+    <message_type>spencer_human_attribute_msgs/HumanAttributes</message_type>
+  </class>
+
+</library>

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/scripts/send_test_msgs.py

@@ -0,0 +1,125 @@
+#!/usr/bin/env python
+
+import roslib; roslib.load_manifest( 'spencer_tracking_rviz_plugin' )
+from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson, DetectedPersons, DetectedPerson
+import rospy
+from math import cos, sin, tan, pi
+import tf
+import random
+import copy
+
+def setPoseAndTwistFromAngle( pose, twist, angle, radius ) :
+    currentx = radius * cos(angle)
+    currenty = radius * sin(angle)
+
+    nextx = radius * cos(angle + angleStep)
+    nexty = radius * sin(angle + angleStep)
+    
+    pose.pose.position.x = currentx
+    pose.pose.position.y = currenty
+    pose.pose.position.z = 0.0
+
+    quaternion = tf.transformations.quaternion_from_euler(0, 0, angle + pi/2.0)
+    pose.pose.orientation.x = quaternion[0]
+    pose.pose.orientation.y = quaternion[1]
+    pose.pose.orientation.z = quaternion[2]
+    pose.pose.orientation.w = quaternion[3]
+    
+    pose.covariance[0 + 0 * 6] = 0.4 # x
+    pose.covariance[1 + 1 * 6] = 0.2 # y
+    pose.covariance[2 + 2 * 6] = 999999 # z
+    pose.covariance[3 + 3 * 6] = 0.0 # x rotation
+    pose.covariance[4 + 5 * 6] = 0.0 # y rotation
+    pose.covariance[4 + 5 * 6] = 0.1 # z rotation
+
+    twist.twist.linear.x = nextx - currentx
+    twist.twist.linear.y = nexty - currenty
+    twist.twist.linear.z = 0
+    
+    for i in range(0, 3):
+        twist.covariance[i + i * 6] = 1.0 # linear velocity
+    for i in range(3, 6):
+        twist.covariance[i + i * 6] = float("inf") # rotational velocity
+
+def createTrackAndDetection( tracks, detections, track_id, detection_id, angle, radius ) :
+    trackedPerson = TrackedPerson()
+    trackedPerson.track_id = track_id
+
+    if detection_id >= 0 :
+        trackedPerson.detection_id = detection_id
+        trackedPerson.is_occluded = False
+    else :
+        trackedPerson.is_occluded = True
+
+    trackedPerson.age = rospy.Time.now() - startTime
+
+    setPoseAndTwistFromAngle(trackedPerson.pose, trackedPerson.twist, angle, radius)
+    tracks.append(trackedPerson)
+
+    if detection_id >= 0:
+        detectedPerson = DetectedPerson()
+        detectedPerson.detection_id = detection_id
+        detectedPerson.confidence = random.random()
+
+        detectedPerson.pose = copy.deepcopy(trackedPerson.pose)
+        detectedPerson.pose.pose.position.x += random.random() * 0.5 - 0.25 # introduce some noise on observation position
+        detectedPerson.pose.pose.position.y += random.random() * 0.5 - 0.25
+
+        detections.append(detectedPerson)
+
+    return
+
+# Main code
+trackTopic = '/spencer/tracked_persons'
+trackPublisher = rospy.Publisher( trackTopic, TrackedPersons )
+
+observationTopic = '/spencer/detected_persons' 
+observationPublisher = rospy.Publisher( observationTopic, DetectedPersons )
+
+rospy.init_node( 'publish_test_tracks_and_detections' )
+br = tf.TransformBroadcaster()
+
+# State variables
+startTime = rospy.Time.now()
+currentCycle = 0
+currentAngle = 0.0
+angleStep = 4.5 * pi / 180.
+idShift = 0
+updateRateHz = 10
+
+# Test coordinate frame for checking if mapping into the fixed frame works correctly
+frameOffset = (0, 0, 0)
+frameOrientation = tf.transformations.quaternion_from_euler(0,  0,  0) # 90.0 / 180.0 * pi
+
+print("Sending test messages on " + observationTopic + " and " + trackTopic)
+rate = rospy.Rate(updateRateHz)
+while not rospy.is_shutdown():
+    br.sendTransform(frameOffset, frameOrientation, rospy.Time.now(), "test_tf_frame", "odom")
+
+    trackedPersons = TrackedPersons()
+    trackedPersons.header.frame_id = "/test_tf_frame"
+    trackedPersons.header.stamp = rospy.Time.now()
+
+    detectedPersons = DetectedPersons()
+    detectedPersons.header = trackedPersons.header
+
+    tracks = trackedPersons.tracks;
+    detections = detectedPersons.detections;
+
+    createTrackAndDetection(tracks, detections, idShift+0, 3, currentAngle, 2.0)
+    createTrackAndDetection(tracks, detections, idShift+1, 7, currentAngle + pi / 2, 2.5)
+    createTrackAndDetection(tracks, detections, idShift+2, -1, currentAngle + pi / 1, 3.0)
+    createTrackAndDetection(tracks, detections, idShift+3, -1, currentAngle + pi * 1.5, cos(currentAngle) * 3.5  + 7.0)
+    createTrackAndDetection(tracks, detections, idShift+4, 88, 0.0, 0.0)
+
+    trackPublisher.publish( trackedPersons )
+    observationPublisher.publish( detectedPersons )
+
+    currentAngle += angleStep
+    currentCycle += 1
+
+    # Periodically shift the IDs to simulate tracks being removed and new tracks being added
+    if(currentCycle % (updateRateHz * 15) == 0) :
+        idShift += len(tracks)
+
+    rate.sleep()

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/additional_topic_subscriber.h

@@ -0,0 +1,221 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  Copyright (c) 2012, Willow Garage, Inc.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef ADDITIONAL_TOPIC_SUBSCRIBER_H
+#define ADDITIONAL_TOPIC_SUBSCRIBER_H
+
+#ifndef Q_MOC_RUN
+#include <message_filters/subscriber.h>
+#if ROS_VERSION_MINIMUM(1, 14, 0)
+#include <tf2_ros/message_filter.h>
+#else
+#include <tf/message_filter.h>
+#endif
+
+#include <boost/bind.hpp>
+#include <boost/function.hpp>
+#include <iostream>
+#include <OGRE/OgreSceneManager.h>
+#include <OGRE/OgreSceneNode.h>
+#endif
+
+#include <rviz/display.h>
+#include <rviz/display_context.h>
+#include <rviz/frame_manager.h>
+#include <rviz/properties/ros_topic_property.h>
+
+
+using namespace boost;
+
+namespace rviz
+{
+
+/** @brief Helper superclass for AdditionalTopicSubscriber, needed because
+ * Qt's moc and c++ templates don't work nicely together.  Not
+ * intended to be used directly. */
+class _AdditionalTopicSubscriber : public QObject
+{
+Q_OBJECT
+public:
+    void initialize(Display* display, FrameManager* frameManager)
+    {
+        QObject::connect(display, SIGNAL(changed()), this, SLOT(displayEnableChanged()));
+        QObject::connect(frameManager, SIGNAL(fixedFrameChanged()), this, SLOT(fixedFrameChanged()));
+        additional_topic_property_ = new RosTopicProperty("Additional topic", "", "", "", display, SLOT(updateTopic()), this);
+    }
+
+protected Q_SLOTS:
+    virtual void updateTopic() = 0;
+    virtual void displayEnableChanged() = 0;
+    virtual void fixedFrameChanged() = 0;
+
+protected:
+    RosTopicProperty* additional_topic_property_;
+};
+
+/** @brief Display subclass using a tf::MessageFilter, templated on the ROS message type.
+ *
+ * This class brings together some common things used in many Display
+ * types.  It has a tf::MessageFilter to filter incoming messages, and
+ * it handles subscribing and unsubscribing when the display is
+ * enabled or disabled.  It also has an Ogre::SceneNode which  */
+template<class MessageType>
+class AdditionalTopicSubscriber: public _AdditionalTopicSubscriber
+{
+// No Q_OBJECT macro here, moc does not support Q_OBJECT in a templated class.
+public:
+    /** @brief Convenience typedef so subclasses don't have to use
+     * the long templated class name to refer to their super class. */
+    typedef AdditionalTopicSubscriber<MessageType> ATSClass;
+
+    AdditionalTopicSubscriber(const QString& propertyName, Display* display, DisplayContext* context, ros::NodeHandle& update_nh, const boost::function<void (boost::shared_ptr<const MessageType>)>& messageCallback)
+            : tf_filter(NULL), m_messagesReceived(0), m_display(display), m_context(context), m_updateNodeHandle(update_nh), m_enabled(false), m_messageCallback(messageCallback)
+    {
+        _AdditionalTopicSubscriber::initialize(display, context->getFrameManager());
+
+        additional_topic_property_->setName(propertyName);
+        QString message_type = QString::fromStdString(ros::message_traits::datatype<MessageType>());
+        additional_topic_property_->setMessageType(message_type);
+        additional_topic_property_->setDescription(message_type + " topic to subscribe to.");
+
+#if ROS_VERSION_MINIMUM(1, 14, 0)
+        tf_filter = new tf2_ros::MessageFilter<MessageType>(*m_context->getTF2BufferPtr(), "map", 10, m_updateNodeHandle);
+#else
+        tf_filter = new tf::MessageFilter<MessageType>(*m_context->getTFClient(), "map", 10, m_updateNodeHandle);
+#endif
+
+        tf_filter->connectInput(m_subscriber);
+        tf_filter->registerCallback(boost::bind(&AdditionalTopicSubscriber<MessageType>::incomingMessage, this, _1));
+        m_context->getFrameManager()->registerFilterForTransformStatusCheck(tf_filter, display);
+
+        setEnabled(m_display->isEnabled());
+        updateTopic();
+        fixedFrameChanged();
+    }
+
+    virtual ~AdditionalTopicSubscriber()
+    {
+        unsubscribe();
+        delete tf_filter;
+    }
+
+    virtual void reset()
+    {
+        tf_filter->clear();
+        m_messagesReceived = 0;
+    }
+
+    void setEnabled(bool enabled)
+    {
+        m_enabled = enabled;
+        if (enabled) subscribe();
+    }
+
+protected:
+    virtual void updateTopic()
+    {
+        ROS_DEBUG_STREAM_NAMED("AdditionalTopicSubscriber", "AdditionalTopicSubscriber: Topic was changed to " << additional_topic_property_->getTopicStd());
+        unsubscribe();
+        reset();
+        subscribe();
+        m_context->queueRender();
+    }
+
+    virtual void displayEnableChanged()
+    {
+        ROS_DEBUG_STREAM_NAMED("AdditionalTopicSubscriber", "AdditionalTopicSubscriber: Display enabled = " << m_display->getBool());
+        setEnabled(m_display->getBool());
+    }
+
+    virtual void fixedFrameChanged()
+    {
+        ROS_DEBUG_STREAM_NAMED("AdditionalTopicSubscriber", "AdditionalTopicSubscriber: Fixed frame has changed for topic " << additional_topic_property_->getTopicStd());
+        tf_filter->setTargetFrame(m_context->getFixedFrame().toStdString());
+        reset();
+    }
+
+    virtual void subscribe()
+    {
+        if (!m_display->isEnabled()) {
+            return;
+        }
+
+        try {
+            ROS_DEBUG_STREAM_NAMED("AdditionalTopicSubscriber", "AdditionalTopicSubscriber: Subscribing to topic " << additional_topic_property_->getTopicStd());
+            m_subscriber.subscribe(m_updateNodeHandle, additional_topic_property_->getTopicStd(), 10);
+            m_display->setStatus(StatusProperty::Ok, additional_topic_property_->getName(), "OK");
+        }
+        catch (ros::Exception& e) {
+            m_display->setStatus(StatusProperty::Error, additional_topic_property_->getName(), QString("Error subscribing: ") + e.what());
+        }
+    }
+
+    virtual void unsubscribe()
+    {
+        m_subscriber.unsubscribe();
+    }
+
+    /** @brief Incoming message callback.  Checks if the message pointer
+     * is valid, increments m_messagesReceived, then calls
+     * processMessage(). */
+    void incomingMessage(const typename MessageType::ConstPtr& msg)
+    {
+        if (!msg) {
+            return;
+        }
+
+        ++m_messagesReceived;
+        m_display->setStatus(StatusProperty::Ok, additional_topic_property_->getName(), QString::number(m_messagesReceived) + " messages received");
+
+        // Callback for further processing
+        m_messageCallback(msg);
+    }
+
+#if ROS_VERSION_MINIMUM(1, 14, 0)
+    tf2_ros::MessageFilter<MessageType>* tf_filter;
+#else
+    tf::MessageFilter<MessageType>* tf_filter;
+#endif
+
+private:
+    std::string m_topic;
+    bool m_enabled;
+    Display* m_display;
+    DisplayContext* m_context;
+    ros::NodeHandle m_updateNodeHandle;
+    message_filters::Subscriber<MessageType> m_subscriber;
+    uint32_t m_messagesReceived;
+
+    const boost::function<void (boost::shared_ptr<const MessageType>)> m_messageCallback;
+};
+
+} // end namespace rviz
+
+#endif // ADDITIONAL_TOPIC_SUBSCRIBER_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/detected_persons_display.cpp

@@ -0,0 +1,279 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rviz/visualization_manager.h>
+#include <rviz/frame_manager.h>
+#include "rviz/selection/selection_manager.h"
+
+#include "detected_persons_display.h"
+#ifndef Q_MOC_RUN
+#include <boost/foreach.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+namespace spencer_tracking_rviz_plugin
+{
+
+// The constructor must have no arguments, so we can't give the
+// constructor the parameters it needs to fully initialize.
+void DetectedPersonsDisplay::onInitialize()
+{
+    PersonDisplayCommon::onInitialize();
+
+    QObject::connect(m_commonProperties->style, SIGNAL(changed()), this, SLOT(personVisualTypeChanged()) );
+
+    m_render_covariances_property       = new rviz::BoolProperty( "Render covariances", true, "Render track covariance ellipses", this, SLOT(stylesChanged()) );
+    m_render_detection_ids_property     = new rviz::BoolProperty( "Render detection IDs", true, "Render IDs of the detection that a track was matched against, if any", this, SLOT(stylesChanged()));
+    m_render_confidences_property       = new rviz::BoolProperty( "Render confidences", false, "Render detection confidences", this, SLOT(stylesChanged()));
+    m_render_orientations_property      = new rviz::BoolProperty( "Render orientation arrows", true, "Render orientation arrows (only if orientation covariances are finite!)", this, SLOT(stylesChanged()));
+    m_render_modality_text_property     = new rviz::BoolProperty( "Render modality text", false, "Render detection modality as text below detected person", this, SLOT(stylesChanged()));
+
+    m_text_spacing_property = new rviz::FloatProperty( "Text spacing", 1.0, "Factor for vertical spacing betweent texts", this, SLOT(stylesChanged()), this );
+    
+    m_low_confidence_threshold_property = new rviz::FloatProperty( "Low-confidence threshold", 0.5, "Detection confidence below which alpha will be reduced", this, SLOT(stylesChanged()));
+    m_low_confidence_alpha_property     = new rviz::FloatProperty( "Low-confidence alpha", 0.5, "Alpha multiplier for detections with confidence below low-confidence threshold", this, SLOT(stylesChanged()));
+
+    m_covariance_line_width_property = new rviz::FloatProperty( "Line width", 0.1, "Line width of covariance ellipses", m_render_covariances_property, SLOT(stylesChanged()), this );
+}
+
+DetectedPersonsDisplay::~DetectedPersonsDisplay()
+{
+    m_previousDetections.clear();
+}
+
+// Clear the visuals by deleting their objects.
+void DetectedPersonsDisplay::reset()
+{
+    PersonDisplayCommon::reset();
+    m_previousDetections.clear();
+}
+
+// Set the rendering style (cylinders, meshes, ...) of detected persons
+void DetectedPersonsDisplay::personVisualTypeChanged()
+{
+    foreach(boost::shared_ptr<DetectedPersonVisual>& detectedPersonVisual, m_previousDetections)
+    {
+        detectedPersonVisual->personVisual.reset();
+        createPersonVisualIfRequired(detectedPersonVisual->sceneNode.get(), detectedPersonVisual->personVisual);
+    }
+    stylesChanged();
+}
+
+// Update dynamically adjustable properties of all existing detections
+void DetectedPersonsDisplay::stylesChanged()
+{
+    foreach(boost::shared_ptr<DetectedPersonVisual> detectedPersonVisual, m_previousDetections)
+    {
+        bool personHidden = isPersonHidden(detectedPersonVisual->detectionId);
+
+        // Update common styles to person visual, such as line width
+        applyCommonStyles(detectedPersonVisual->personVisual);
+
+        // Get current detection color
+        Ogre::ColourValue detectionColor = getColorFromId(detectedPersonVisual->detectionId);
+        detectionColor.a *= m_commonProperties->alpha->getFloat(); // general alpha
+        if(personHidden) detectionColor.a = 0.0;
+        if(detectedPersonVisual->confidence < m_low_confidence_threshold_property->getFloat()) detectionColor.a *= m_low_confidence_alpha_property->getFloat();
+
+        if(detectedPersonVisual->personVisual) {
+            detectedPersonVisual->personVisual->setColor(detectionColor);
+        }
+
+        // Update texts
+        Ogre::ColourValue fontColor = m_commonProperties->font_color_style->getOptionInt() == FONT_COLOR_CONSTANT ? m_commonProperties->constant_font_color->getOgreColor() : detectionColor;
+        fontColor.a = m_commonProperties->alpha->getFloat();
+        if(personHidden) fontColor.a = 0.0;
+        
+        float textOffset = 0.0f;
+        detectedPersonVisual->detectionIdText->setCharacterHeight(0.18 * m_commonProperties->font_scale->getFloat());
+        detectedPersonVisual->detectionIdText->setPosition(Ogre::Vector3(0,0, -0.5*detectedPersonVisual->detectionIdText->getCharacterHeight() - textOffset));
+        detectedPersonVisual->detectionIdText->setVisible(m_render_detection_ids_property->getBool());
+        detectedPersonVisual->detectionIdText->setColor(fontColor);
+        if(m_render_detection_ids_property->getBool()) textOffset += m_text_spacing_property->getFloat() * detectedPersonVisual->detectionIdText->getCharacterHeight();
+
+        detectedPersonVisual->modalityText->setCharacterHeight(0.18 * m_commonProperties->font_scale->getFloat());
+        detectedPersonVisual->modalityText->setPosition(Ogre::Vector3(textOffset, 0, -0.5*detectedPersonVisual->modalityText->getCharacterHeight() - textOffset));
+        detectedPersonVisual->modalityText->setVisible(m_render_modality_text_property->getBool());
+        detectedPersonVisual->modalityText->setColor(fontColor);
+        if(m_render_modality_text_property->getBool()) textOffset += m_text_spacing_property->getFloat() * detectedPersonVisual->modalityText->getCharacterHeight();
+
+        detectedPersonVisual->confidenceText->setCharacterHeight(0.13 * m_commonProperties->font_scale->getFloat());
+        detectedPersonVisual->confidenceText->setPosition(Ogre::Vector3(textOffset, 0, -0.5*detectedPersonVisual->confidenceText->getCharacterHeight() - textOffset));
+        detectedPersonVisual->confidenceText->setVisible(m_render_confidences_property->getBool());
+        detectedPersonVisual->confidenceText->setColor(fontColor);
+        if(m_render_confidences_property->getBool()) textOffset += m_text_spacing_property->getFloat() * detectedPersonVisual->confidenceText->getCharacterHeight();
+
+        // Set color of covariance visualization
+        Ogre::ColourValue covarianceColor = detectionColor;
+        if(!m_render_covariances_property->getBool()) covarianceColor.a = 0.0;
+        detectedPersonVisual->covarianceVisual->setColor(covarianceColor);
+        detectedPersonVisual->covarianceVisual->setLineWidth(m_covariance_line_width_property->getFloat());
+
+        // Update orientation arrow
+        double arrowAlpha = m_render_orientations_property->getBool() && detectedPersonVisual->hasValidOrientation ? detectionColor.a : 0.0;
+        detectedPersonVisual->orientationArrow->setColor(Ogre::ColourValue(detectionColor.r, detectionColor.g, detectionColor.b, arrowAlpha));
+        const double shaftLength = 0.5, shaftDiameter = 0.05, headLength = 0.2, headDiameter = 0.2;
+        detectedPersonVisual->orientationArrow->set(shaftLength, shaftDiameter, headLength, headDiameter);
+    }
+}
+
+// This is our callback to handle an incoming message.
+void DetectedPersonsDisplay::processMessage(const spencer_tracking_msgs::DetectedPersons::ConstPtr& msg)
+{
+    // Get transforms into fixed frame etc.
+    if(!preprocessMessage(msg)) return;
+
+    const Ogre::Quaternion shapeQuaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ); // required to fix orientation of any Cylinder shapes
+    stringstream ss;
+
+    // Clear previous detections, this will also delete them from the scene graph
+    m_previousDetections.clear();
+
+    //
+    // Iterate over all detections in this message and create a visual representation
+    //
+    for (vector<spencer_tracking_msgs::DetectedPerson>::const_iterator detectedPersonIt = msg->detections.begin(); detectedPersonIt != msg->detections.end(); ++detectedPersonIt)
+    {
+        boost::shared_ptr<DetectedPersonVisual> detectedPersonVisual;
+
+        // Create a new visual representation of the detected person
+        detectedPersonVisual = boost::shared_ptr<DetectedPersonVisual>(new DetectedPersonVisual);
+        m_previousDetections.push_back(detectedPersonVisual);
+
+        // This scene node is the parent of all visualization elements for the detected person
+        detectedPersonVisual->sceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+        detectedPersonVisual->detectionId = detectedPersonIt->detection_id;
+        detectedPersonVisual->confidence = detectedPersonIt->confidence;
+        Ogre::SceneNode* currentSceneNode = detectedPersonVisual->sceneNode.get();
+
+
+        //
+        // Person visualization
+        //
+
+        // Create new visual for the person itself, if needed
+        boost::shared_ptr<PersonVisual> &personVisual = detectedPersonVisual->personVisual;
+        createPersonVisualIfRequired(currentSceneNode, personVisual);
+
+        const double personHeight = personVisual ? personVisual->getHeight() : 0;
+        const double halfPersonHeight = personHeight / 2.0;
+
+
+        //
+        // Position & visibility of entire detection
+        //
+
+        const Ogre::Matrix3 covXYZinTargetFrame = covarianceXYZIntoTargetFrame(detectedPersonIt->pose);
+        setPoseOrientation(currentSceneNode, detectedPersonIt->pose, covXYZinTargetFrame, personHeight);
+
+        //
+        // Texts
+        //
+        {
+            // Detection ID
+            if (!detectedPersonVisual->detectionIdText) {
+                detectedPersonVisual->detectionIdText.reset(new TextNode(context_->getSceneManager(), currentSceneNode));
+                detectedPersonVisual->detectionIdText->showOnTop();
+            }
+
+            ss.str(""); ss << "det " << detectedPersonIt->detection_id;
+            detectedPersonVisual->detectionIdText->setCaption(ss.str());
+
+            // Confidence value
+            if (!detectedPersonVisual->confidenceText) {
+                detectedPersonVisual->confidenceText.reset(new TextNode(context_->getSceneManager(), currentSceneNode));
+            }
+
+            ss.str(""); ss << fixed << setprecision(2) << detectedPersonIt->confidence;
+            detectedPersonVisual->confidenceText->setCaption(ss.str());
+            detectedPersonVisual->confidenceText->showOnTop();
+
+            // Modality text
+            if (!detectedPersonVisual->modalityText) {
+                detectedPersonVisual->modalityText.reset(new TextNode(context_->getSceneManager(), currentSceneNode));
+            }
+
+            ss.str(""); ss << detectedPersonIt->modality;
+            detectedPersonVisual->modalityText->setCaption(ss.str());
+            detectedPersonVisual->modalityText->showOnTop();
+        }
+
+        //
+        // Covariance visualization
+        //
+        if(!detectedPersonVisual->covarianceVisual) {
+            detectedPersonVisual->covarianceVisual.reset(new ProbabilityEllipseCovarianceVisual(context_->getSceneManager(), currentSceneNode));
+        }
+
+        // Update covariance ellipse
+        {
+            Ogre::Vector3 covarianceMean(0,0,0); // zero mean because parent node is already centered at pose mean
+            detectedPersonVisual->covarianceVisual->setOrientation(currentSceneNode->getOrientation().Inverse());
+            detectedPersonVisual->covarianceVisual->setMeanCovariance(covarianceMean, covXYZinTargetFrame);
+        }
+
+
+        //
+        // Orientation arrows
+        //
+        if (!detectedPersonVisual->orientationArrow) {
+            detectedPersonVisual->orientationArrow.reset(new rviz::Arrow(context_->getSceneManager(), currentSceneNode));
+        }
+
+        // Update orientation arrow
+        {
+            const Ogre::Vector3 forwardVector(1,0,0);
+
+            const double personRadius = 0.2;
+            const Ogre::Vector3 arrowAttachPoint(personRadius, 0, halfPersonHeight); // relative to tracked person's scene node
+            detectedPersonVisual->orientationArrow->setPosition(arrowAttachPoint);
+            detectedPersonVisual->orientationArrow->setOrientation(Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(forwardVector));
+            detectedPersonVisual->hasValidOrientation = hasValidOrientation(detectedPersonIt->pose);
+        }
+
+    } // end for loop over all detected persons
+
+    // Set all properties that can dynamically be adjusted in the GUI
+    stylesChanged();
+
+    //
+    // Update status (shown in property pane)
+    //
+    ss.str("");
+    ss << msg->detections.size() << " detections received";
+    setStatusStd(rviz::StatusProperty::Ok, "Tracks", ss.str());
+}
+
+} // end namespace spencer_tracking_rviz_plugin
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::DetectedPersonsDisplay, rviz::Display)

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/detected_persons_display.h

@@ -0,0 +1,114 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef DETECTED_PERSONS_DISPLAY_H
+#define DETECTED_PERSONS_DISPLAY_H
+
+#ifndef Q_MOC_RUN
+#include <map>
+#include <boost/circular_buffer.hpp>
+#include <spencer_tracking_msgs/DetectedPersons.h>
+#include "person_display_common.h"
+#endif
+
+namespace spencer_tracking_rviz_plugin
+{
+    /// The visual of a tracked person.
+    struct DetectedPersonVisual
+    {
+        boost::shared_ptr<Ogre::SceneNode> sceneNode;
+
+        boost::shared_ptr<PersonVisual> personVisual;
+        boost::shared_ptr<TextNode> detectionIdText, confidenceText, modalityText;
+        boost::shared_ptr<rviz::Arrow> orientationArrow;
+        boost::shared_ptr<CovarianceVisual> covarianceVisual;
+
+        float confidence;
+        bool hasValidOrientation;
+        unsigned int detectionId;
+    };
+
+    // The DetectedPersonsDisplay class itself just implements a circular buffer,
+    // editable parameters, and Display subclass machinery.
+    class DetectedPersonsDisplay: public PersonDisplayCommon<spencer_tracking_msgs::DetectedPersons>
+    {
+    Q_OBJECT
+    public:
+        // Constructor.  pluginlib::ClassLoader creates instances by calling
+        // the default constructor, so make sure you have one.
+        DetectedPersonsDisplay() {};
+        virtual ~DetectedPersonsDisplay();
+
+        // Overrides of protected virtual functions from Display.  As much
+        // as possible, when Displays are not enabled, they should not be
+        // subscribed to incoming data and should not show anything in the
+        // 3D view.  These functions are where these connections are made
+        // and broken.
+        
+        virtual void onInitialize();
+
+    protected:
+        // A helper to clear this display back to the initial state.
+        virtual void reset();
+
+        // Must be implemented by derived classes because MOC doesn't work in templates
+        virtual rviz::DisplayContext* getContext() {
+            return context_;
+        }
+
+    private Q_SLOTS:
+        void personVisualTypeChanged();
+
+        // Called whenever one of the properties in PersonDisplayCommonProperties has been changed
+        virtual void stylesChanged();
+
+    private:
+        // Function to handle an incoming ROS message.
+        void processMessage(const spencer_tracking_msgs::DetectedPersons::ConstPtr& msg);
+       
+        // All currently active tracks, with unique track ID as map key
+        vector<boost::shared_ptr<DetectedPersonVisual> > m_previousDetections;
+
+        // Properties
+        rviz::BoolProperty* m_render_covariances_property;
+        rviz::BoolProperty* m_render_detection_ids_property;
+        rviz::BoolProperty* m_render_confidences_property;
+        rviz::FloatProperty* m_low_confidence_threshold_property;
+        rviz::FloatProperty* m_low_confidence_alpha_property;
+        rviz::BoolProperty* m_render_orientations_property;
+        rviz::BoolProperty* m_render_modality_text_property;
+
+        rviz::FloatProperty* m_text_spacing_property;
+        rviz::FloatProperty* m_covariance_line_width_property;
+    };
+
+} // end namespace spencer_tracking_rviz_plugin
+
+#endif // DETECTED_PERSONS_DISPLAY_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/human_attributes_display.cpp

@@ -0,0 +1,289 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rviz/frame_manager.h>
+#include <rviz/selection/selection_manager.h>
+
+#ifndef Q_MOC_RUN
+#include "human_attributes_display.h"
+#include <boost/lexical_cast.hpp>
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#include <boost/range/adaptor/map.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+namespace spencer_tracking_rviz_plugin
+{
+
+void HumanAttributesDisplay::onInitialize()
+{
+    m_trackedPersonsCache.initialize(this, context_, update_nh_);
+    PersonDisplayCommon::onInitialize();
+    
+    m_render_gender_property = new rviz::BoolProperty( "Render gender", true, "Render gender visual", this, SLOT(stylesChanged()));
+    m_render_age_group_property = new rviz::BoolProperty( "Render age group", true, "Render age group visual", this, SLOT(stylesChanged()));
+    m_render_person_height_property = new rviz::BoolProperty( "Render person height", true, "Render person height", this, SLOT(stylesChanged()));
+    
+    m_occlusion_alpha_property = new rviz::FloatProperty( "Occlusion alpha", 0.5, "Alpha multiplier for occluded tracks", this, SLOT(stylesChanged()) );
+    m_occlusion_alpha_property->setMin( 0.0 );
+
+    m_commonProperties->z_offset->setFloat(2.7f);
+    m_commonProperties->style->setHidden(true);
+}
+
+HumanAttributesDisplay::~HumanAttributesDisplay()
+{
+}
+
+// Clear the visuals by deleting their objects.
+void HumanAttributesDisplay::reset()
+{
+    PersonDisplayCommon::reset();
+    m_trackedPersonsCache.reset();
+    m_humanAttributeVisuals.clear();
+    scene_node_->removeAndDestroyAllChildren(); // not sure if required?
+}
+
+void HumanAttributesDisplay::update(float wall_dt, float ros_dt)
+{}
+
+void HumanAttributesDisplay::stylesChanged()
+{
+    foreach(boost::shared_ptr<HumanAttributeVisual> humanAttributeVisual, m_humanAttributeVisuals | boost::adaptors::map_values) {
+        updateVisualStyles(humanAttributeVisual);
+    }
+}
+
+void HumanAttributesDisplay::updateVisualStyles(boost::shared_ptr<HumanAttributeVisual>& humanAttributeVisual)
+{
+    track_id trackId = humanAttributeVisual->trackId;
+    bool personHidden = isPersonHidden(trackId);
+
+    boost::shared_ptr<CachedTrackedPerson> trackedPerson = m_trackedPersonsCache.lookup(trackId);
+    float occlusionAlpha = trackedPerson->isOccluded ? m_occlusion_alpha_property->getFloat() : 1.0;
+
+    // Update text colors, size and visibility
+    Ogre::ColourValue fontColor = m_commonProperties->font_color_style->getOptionInt() == FONT_COLOR_CONSTANT ? m_commonProperties->constant_font_color->getOgreColor() : getColorFromId(trackId);
+    fontColor.a = m_commonProperties->alpha->getFloat() * occlusionAlpha;
+    if(personHidden) fontColor.a = 0;
+
+    humanAttributeVisual->ageGroupText->setVisible(m_render_age_group_property->getBool());
+    humanAttributeVisual->ageGroupText->setCharacterHeight(0.17 * m_commonProperties->font_scale->getFloat());
+    humanAttributeVisual->ageGroupText->setColor(fontColor);
+    humanAttributeVisual->ageGroupText->setPosition(Ogre::Vector3(0, 0, 0.17 * m_commonProperties->font_scale->getFloat()) );
+
+    humanAttributeVisual->personHeightText->setVisible(m_render_person_height_property->getBool());
+    humanAttributeVisual->personHeightText->setCharacterHeight(0.17 * m_commonProperties->font_scale->getFloat());
+    humanAttributeVisual->personHeightText->setColor(fontColor);
+    humanAttributeVisual->personHeightText->setPosition(Ogre::Vector3(0, 0, 0) );
+
+    if(humanAttributeVisual->genderMesh) {
+        humanAttributeVisual->genderMesh->setPosition(Ogre::Vector3(0, 0, 0.17 * 2 * m_commonProperties->font_scale->getFloat() + 0.3));
+        humanAttributeVisual->genderMesh->setVisible(m_render_gender_property->getBool());
+    }
+}
+
+boost::shared_ptr<HumanAttributesDisplay::HumanAttributeVisual> HumanAttributesDisplay::createVisualIfNotExists(track_id trackId)
+{
+    if(m_humanAttributeVisuals.find(trackId) == m_humanAttributeVisuals.end()) {
+        boost::shared_ptr<HumanAttributeVisual> humanAttributeVisual = boost::shared_ptr<HumanAttributeVisual>(new HumanAttributeVisual);
+
+        humanAttributeVisual->sceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+
+        humanAttributeVisual->ageGroupText = boost::shared_ptr<TextNode>(new TextNode(context_->getSceneManager(), humanAttributeVisual->sceneNode.get()));
+        humanAttributeVisual->ageGroupText->showOnTop();
+        humanAttributeVisual->ageGroupText->setCaption(" ");
+
+        humanAttributeVisual->personHeightText = boost::shared_ptr<TextNode>(new TextNode(context_->getSceneManager(), humanAttributeVisual->sceneNode.get()));
+        humanAttributeVisual->personHeightText->showOnTop();
+        humanAttributeVisual->personHeightText->setCaption(" ");
+
+        humanAttributeVisual->trackId = trackId;
+
+        m_humanAttributeVisuals[trackId] = humanAttributeVisual;
+    }
+
+
+    return m_humanAttributeVisuals[trackId];
+}
+
+// This is our callback to handle an incoming group message.
+void HumanAttributesDisplay::processMessage(const spencer_human_attribute_msgs::HumanAttributes::ConstPtr& msg)
+{
+    // Get transforms into fixed frame etc.
+    if(!preprocessMessage(msg)) return;
+
+    // Transform into Rviz fixed frame
+    m_frameTransform = Ogre::Matrix4(m_frameOrientation);
+    m_frameTransform.setTrans(m_framePosition);
+
+    const Ogre::Quaternion shapeQuaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ); // required to fix orientation of any Cylinder shapes
+    stringstream ss;
+
+    //
+    // Iterate over all categorical attributes in this message
+    //
+    foreach (const spencer_human_attribute_msgs::CategoricalAttribute& categoricalAttribute, msg->categoricalAttributes)
+    {
+        // Check if there is already a visual for this particular track
+        track_id trackId = categoricalAttribute.subject_id; // assumes subject_id is a track_id (not detection_id)
+        boost::shared_ptr<HumanAttributeVisual> humanAttributeVisual = createVisualIfNotExists(trackId);
+
+        if(categoricalAttribute.values.empty()) {
+            ROS_ERROR_STREAM("categoricalAttribute.values.empty() for track ID " << trackId << ", attribute " << categoricalAttribute.type);
+            continue;
+        }
+        if(categoricalAttribute.confidences.size() != categoricalAttribute.values.size()) {
+            ROS_WARN_STREAM("categoricalAttribute.confidences.size() != categoricalAttribute.values.size() for track ID " << trackId << ", attribute " << categoricalAttribute.type);
+        }
+
+        // Find highest-ranking attribute
+        size_t highestRankingIndex = 0; float highestConfidence = -999999;
+        for(size_t i = 0; i < categoricalAttribute.confidences.size(); i++) {
+            if(categoricalAttribute.confidences[i] > highestConfidence) {
+                highestConfidence = categoricalAttribute.confidences[i];
+                highestRankingIndex = i;
+            }
+        }
+
+        std::string valueWithHighestConfidence = categoricalAttribute.values[highestRankingIndex];
+
+        // Age group
+        if(categoricalAttribute.type == spencer_human_attribute_msgs::CategoricalAttribute::AGE_GROUP) {
+            ss.str(""); ss << valueWithHighestConfidence << "yrs";
+            humanAttributeVisual->ageGroupText->setCaption(ss.str());
+        }
+
+        // Gender
+        else if(categoricalAttribute.type == spencer_human_attribute_msgs::CategoricalAttribute::GENDER) {
+            ss.str(""); ss << "package://" ROS_PACKAGE_NAME "/media/" << valueWithHighestConfidence << "_symbol.dae";
+            std::string meshResource = ss.str();
+            
+            humanAttributeVisual->genderMesh = boost::shared_ptr<MeshNode>(new MeshNode(context_, humanAttributeVisual->sceneNode.get(), meshResource));
+            
+            Ogre::ColourValue meshColor(1, 1, 1, 1);
+            if(valueWithHighestConfidence == spencer_human_attribute_msgs::CategoricalAttribute::GENDER_MALE) meshColor = Ogre::ColourValue(0, 1, 1, 1);
+            if(valueWithHighestConfidence == spencer_human_attribute_msgs::CategoricalAttribute::GENDER_FEMALE) meshColor = Ogre::ColourValue(1, 0, 1, 1);
+            humanAttributeVisual->genderMesh->setColor(meshColor);
+
+            humanAttributeVisual->genderMesh->setScale(0.5);
+            humanAttributeVisual->genderMesh->setCameraFacing(true);            
+        }
+    }
+
+
+    //
+    // Iterate over all scalar attributes in this message
+    //
+    foreach (const spencer_human_attribute_msgs::ScalarAttribute& scalarAttribute, msg->scalarAttributes)
+    {
+        // Check if there is already a visual for this particular track
+        track_id trackId = scalarAttribute.subject_id; // assumes subject_id is a track_id (not detection_id)
+        boost::shared_ptr<HumanAttributeVisual> humanAttributeVisual = createVisualIfNotExists(trackId);
+        
+        if(scalarAttribute.values.empty()) {
+            ROS_ERROR_STREAM("scalarAttribute.values.empty() for track ID " << trackId << ", attribute " << scalarAttribute.type);
+            continue;
+        }
+        if(scalarAttribute.confidences.size() != scalarAttribute.values.size()) {
+            ROS_WARN_STREAM("scalarAttribute.confidences.size() != scalarAttribute.values.size() for track ID " << trackId << ", attribute " << scalarAttribute.type);
+        }
+
+        // Find highest-ranking attribute
+        size_t highestRankingIndex = 0; float highestConfidence = -999999;
+        for(size_t i = 0; i < scalarAttribute.confidences.size(); i++) {
+            if(scalarAttribute.confidences[i] > highestConfidence) {
+                highestConfidence = scalarAttribute.confidences[i];
+                highestRankingIndex = i;
+            }
+        }
+
+        float valueWithHighestConfidence = scalarAttribute.values[highestRankingIndex];
+
+        // Person height
+        if(scalarAttribute.type == spencer_human_attribute_msgs::ScalarAttribute::PERSON_HEIGHT) {
+            ss.str(""); ss << std::fixed << std::setprecision(2) << valueWithHighestConfidence << "m";
+            humanAttributeVisual->personHeightText->setCaption(ss.str());
+        }            
+    }
+
+
+    //
+    // Update position and style of all existing person visuals
+    //
+    set<track_id> tracksWithUnknownPosition;
+    foreach(boost::shared_ptr<HumanAttributeVisual> humanAttributeVisual, m_humanAttributeVisuals | boost::adaptors::map_values)
+    {
+        boost::shared_ptr<CachedTrackedPerson> trackedPerson = m_trackedPersonsCache.lookup(humanAttributeVisual->trackId);
+
+        // Get current track position
+        if(!trackedPerson) {
+            tracksWithUnknownPosition.insert(humanAttributeVisual->trackId);
+        }
+        else
+        {   // Track position is known
+            humanAttributeVisual->sceneNode->setPosition(trackedPerson->center + Ogre::Vector3(0, 0, m_commonProperties->z_offset->getFloat()));
+
+            // Update styles
+            updateVisualStyles(humanAttributeVisual);
+        }
+    }
+
+
+    // Remove visuals for tracks with unknown position
+    foreach(track_id trackId, tracksWithUnknownPosition) {
+        m_humanAttributeVisuals.erase(trackId);
+    }
+
+
+    //
+    // Update display status (shown in property pane)
+    //
+
+    ss.str("");
+    ss << msg->categoricalAttributes.size() << " categorical attribute(s)";
+    setStatusStd(rviz::StatusProperty::Ok, "Categorical attributes", ss.str());
+
+    ss.str("");
+    ss << msg->scalarAttributes.size() << " scalar attribute(s)";
+    setStatusStd(rviz::StatusProperty::Ok, "Scalar attributes", ss.str());
+
+    ss.str("");
+    ss << tracksWithUnknownPosition.size() << " track(s) with unknown position";
+    setStatusStd(0 == tracksWithUnknownPosition.size() ? rviz::StatusProperty::Ok : rviz::StatusProperty::Warn, "Attribute-to-track assignment", ss.str());
+}
+
+} // end namespace spencer_tracking_rviz_plugin
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::HumanAttributesDisplay, rviz::Display)

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/human_attributes_display.h

@@ -0,0 +1,112 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef HUMAN_ATTRIBUTES_DISPLAY_H
+#define HUMAN_ATTRIBUTES_DISPLAY_H
+
+#ifndef Q_MOC_RUN
+#include <map>
+#include <boost/circular_buffer.hpp>
+#include <spencer_human_attribute_msgs/HumanAttributes.h>
+
+#include "person_display_common.h"
+#include "tracked_persons_cache.h"
+#include "visuals/mesh_node.h"
+#endif
+
+namespace spencer_tracking_rviz_plugin
+{
+    /// The display which can be added in RViz to display human attributes.
+    class HumanAttributesDisplay: public PersonDisplayCommon<spencer_human_attribute_msgs::HumanAttributes>
+    {
+    Q_OBJECT
+    public:
+        // Constructor.  pluginlib::ClassLoader creates instances by calling
+        // the default constructor, so make sure you have one.
+        HumanAttributesDisplay() {};
+        virtual ~HumanAttributesDisplay();
+
+        // Overrides of protected virtual functions from Display.  As much
+        // as possible, when Displays are not enabled, they should not be
+        // subscribed to incoming data and should not show anything in the
+        // 3D view.  These functions are where these connections are made
+        // and broken.
+
+        // Called after the constructors have run
+        virtual void onInitialize();
+
+        // Called periodically by the visualization manager
+        virtual void update(float wall_dt, float ros_dt);
+
+    protected:
+        // A helper to clear this display back to the initial state.
+        virtual void reset();
+
+        // Must be implemented by derived classes because MOC doesn't work in templates
+        virtual rviz::DisplayContext* getContext() {
+            return context_;
+        }
+
+    private:
+        struct HumanAttributeVisual {
+            boost::shared_ptr<Ogre::SceneNode> sceneNode;
+            boost::shared_ptr<MeshNode> genderMesh;
+            unsigned int trackId;
+            boost::shared_ptr<TextNode> ageGroupText;
+            boost::shared_ptr<TextNode> personHeightText;
+        };
+
+        // Functions to handle an incoming ROS message.
+        void processMessage(const spencer_human_attribute_msgs::HumanAttributes::ConstPtr& msg);
+       
+        // Helper functions
+        void updateVisualStyles(boost::shared_ptr<HumanAttributeVisual>& humanAttributeVisual);
+        boost::shared_ptr<HumanAttributeVisual> createVisualIfNotExists(track_id trackId);
+
+        // User-editable property variables.
+        rviz::BoolProperty* m_render_gender_property;
+        rviz::BoolProperty* m_render_person_height_property;
+        rviz::BoolProperty* m_render_age_group_property;
+       
+        rviz::FloatProperty* m_occlusion_alpha_property;        
+
+        // State variables
+        map<track_id, boost::shared_ptr<HumanAttributeVisual> > m_humanAttributeVisuals;
+
+        Ogre::Matrix4 m_frameTransform;
+        TrackedPersonsCache m_trackedPersonsCache;
+
+    private Q_SLOTS:
+        virtual void stylesChanged();
+    };
+
+} // end namespace spencer_tracking_rviz_plugin
+
+#endif // HUMAN_ATTRIBUTES_DISPLAY_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/person_display_common.cpp

@@ -0,0 +1,140 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef Q_MOC_RUN
+#include "person_display_common.h"
+#include <boost/lexical_cast.hpp>
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+
+namespace spencer_tracking_rviz_plugin
+{
+// The constructor must have no arguments, so we can't give the
+// constructor the parameters it needs to fully initialize.
+PersonDisplayCommonProperties::PersonDisplayCommonProperties(rviz::Display* display, StylesChangedSubscriber* stylesChangedSubscriber)
+    : m_display(display), m_stylesChangedSubscriber(stylesChangedSubscriber)
+{
+    style = new rviz::EnumProperty( "Style", "Cylinders", "Rendering mode to use, in order of computational complexity.", m_display, SLOT(stylesChanged()), this );
+    style->addOption( "Simple", STYLE_SIMPLE );
+    style->addOption( "Cylinders", STYLE_CYLINDER );
+    style->addOption( "Person meshes", STYLE_PERSON_MESHES );
+    style->addOption( "Bounding boxes", STYLE_BOUNDING_BOXES );
+    style->addOption( "Crosshairs", STYLE_CROSSHAIRS );
+
+    color_transform = new rviz::EnumProperty( "Color transform", "Rainbow", "How to color the tracked persons", m_display, SLOT(stylesChanged()), this );
+    color_transform->addOption( "SRL Tracking Colors", COLORS_SRL );
+    color_transform->addOption( "Alternative SRL colors", COLORS_SRL_ALTERNATIVE );
+    color_transform->addOption( "Rainbow", COLORS_RAINBOW );
+    color_transform->addOption( "Rainbow + B/W", COLORS_RAINBOW_BW );
+    color_transform->addOption( "Flat", COLORS_FLAT );
+    color_transform->addOption( "Vintage", COLORS_VINTAGE );
+    color_transform->addOption( "Constant color", COLORS_CONSTANT );
+
+    constant_color = new rviz::ColorProperty("Color", QColor( 130, 130, 130 ), "Color for tracked persons if using constant color transform.", m_display, SLOT(stylesChanged()), this );
+
+    color_map_offset = new rviz::IntProperty( "Color map offset", 0, "By how many indices to shift the offset in the color map (useful if not happy with the current colors)", m_display, SLOT(stylesChanged()), this);
+    color_map_offset->setMin( 0 );
+
+    alpha = new rviz::FloatProperty( "Alpha", 1.0, "0 is fully transparent, 1.0 is fully opaque.", m_display, SLOT(stylesChanged()), this);
+    alpha->setMin( 0.0 );
+    alpha->setMax( 1.0 );
+
+    line_width = new rviz::FloatProperty( "Line width", 0.05, "Line width for person visual", style, SLOT(stylesChanged()), this);
+    line_width->setMin( 0.0 );
+    line_width->setMax( 1.0 );
+    
+    scaling_factor = new rviz::FloatProperty( "Scaling factor", 1.0, "Scaling factor for person visual", style);
+    scaling_factor->setMin( 0.0 );
+    scaling_factor->setMax( 100.0 );
+
+    font_color_style = new rviz::EnumProperty( "Font color style", "Same color", "Which type of font coloring to use", m_display, SLOT(stylesChanged()), this );
+    font_color_style->addOption( "Same color", FONT_COLOR_FROM_PERSON );
+    font_color_style->addOption( "Constant color", FONT_COLOR_CONSTANT );
+
+    constant_font_color = new rviz::ColorProperty("Font color", QColor( 255, 255, 255 ), "Font color if using a constant color", m_display, SLOT(stylesChanged()), this );
+
+    font_scale = new rviz::FloatProperty( "Font scale", 2.0, "Larger values mean bigger font", m_display);
+    font_scale->setMin( 0.0 );
+
+    z_offset = new rviz::FloatProperty( "Z offset", 0.0, "Offset of all visualizations on the z (height) axis", m_display, SLOT(stylesChanged()), this);
+
+    use_actual_z_position = new rviz::BoolProperty( "Use Z position from message", false, "Use Z position from message (otherwise place above ground plane)", z_offset, SLOT(stylesChanged()), this);
+
+    m_excluded_person_ids_property = new rviz::StringProperty( "Excluded person IDs", "", "Comma-separated list of person IDs whose visualization should be hidden", m_display, SLOT(stylesChanged()), this );
+    m_included_person_ids_property = new rviz::StringProperty( "Included person IDs", "", "Comma-separated list of person IDs whose visualization should be visible. Overrides excluded IDs.", m_display, SLOT(stylesChanged()), this );
+
+    hideIrrelevantProperties();
+}
+
+void PersonDisplayCommonProperties::hideIrrelevantProperties()
+{
+    constant_color->setHidden(color_transform->getOptionInt() != COLORS_CONSTANT);
+    color_map_offset->setHidden(color_transform->getOptionInt() == COLORS_CONSTANT);
+    constant_font_color->setHidden(font_color_style->getOptionInt() != FONT_COLOR_CONSTANT);
+
+    line_width->setHidden(style->getOptionInt() != STYLE_BOUNDING_BOXES && style->getOptionInt() != STYLE_CROSSHAIRS);
+}
+
+// Callback for any changed style
+void PersonDisplayCommonProperties::stylesChanged()
+{
+    hideIrrelevantProperties();
+
+    // Update list of person IDs that shall be hidden or visible
+    m_excludedPersonIDs.clear();
+    {
+        string personIDString = m_excluded_person_ids_property->getStdString();
+        char_separator<char> separator(",");
+        tokenizer< char_separator<char> > tokens(personIDString, separator);
+        foreach(const string& token, tokens) {
+         try { m_excludedPersonIDs.insert(lexical_cast<person_id>(token)); }
+         catch(bad_lexical_cast &) {}
+        }
+    }
+    m_includedPersonIDs.clear();
+    {
+        string personIDString = m_included_person_ids_property->getStdString();
+        char_separator<char> separator(",");
+        tokenizer< char_separator<char> > tokens(personIDString, separator);
+        foreach(const string& token, tokens) {
+            try { m_includedPersonIDs.insert(lexical_cast<person_id>(token)); }
+            catch(bad_lexical_cast &) {}
+        }
+    }
+
+    // Relay change to other subscribers
+    m_stylesChangedSubscriber->stylesChanged();
+}
+
+
+} // end namespace spencer_tracking_rviz_plugin

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/person_display_common.h

@@ -0,0 +1,383 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef PERSON_DISPLAY_COMMON_H
+#define PERSON_DISPLAY_COMMON_H
+
+#ifndef Q_MOC_RUN
+#include <map>
+#include <set>
+#include <boost/circular_buffer.hpp>
+#include <spencer_tracking_msgs/TrackedPersons.h>
+#include <geometry_msgs/Twist.h>
+#include <rviz/message_filter_display.h>
+
+#include <OGRE/OgreSceneNode.h>
+#include <OGRE/OgreSceneManager.h>
+#include "visuals/person_visual.h"
+#include "visuals/text_node.h"
+#include "visuals/covariance_visual.h"
+#include <rviz/ogre_helpers/shape.h>
+#include <rviz/properties/bool_property.h>
+#include <rviz/properties/enum_property.h>
+#include <rviz/properties/color_property.h>
+#include <rviz/properties/float_property.h>
+#include <rviz/properties/int_property.h>
+#include <rviz/properties/string_property.h>
+#include <rviz/ogre_helpers/arrow.h>
+#include <rviz/ogre_helpers/movable_text.h>
+#endif
+
+
+
+
+
+
+
+using namespace std;
+using namespace boost;
+
+namespace spencer_tracking_rviz_plugin
+{
+    typedef unsigned int person_id;
+
+    /// Visualization style for a person
+    enum Styles {
+        STYLE_SIMPLE,
+        STYLE_CYLINDER,
+        STYLE_PERSON_MESHES,
+        STYLE_BOUNDING_BOXES,
+        STYLE_CROSSHAIRS
+    };
+
+    /// How to color persons
+    enum ColorTransforms {
+        COLORS_SRL,
+        COLORS_SRL_ALTERNATIVE,
+        COLORS_RAINBOW,
+        COLORS_RAINBOW_BW,
+        COLORS_FLAT,
+        COLORS_VINTAGE,
+        COLORS_CONSTANT,
+    };
+
+    /// Which font colors to use
+    enum FontColorStyle {
+        FONT_COLOR_FROM_PERSON,
+        FONT_COLOR_CONSTANT
+    };
+
+    /// Subclasses of PersonDisplayCommon can override stylesChanged() to get notified when one of the properties in PersonDisplayCommonProperties has changed.
+    class StylesChangedSubscriber {
+    public:
+        virtual ~StylesChangedSubscriber() {}
+        virtual void stylesChanged() {}
+    };
+
+    /// Common properties shared by multiple displays.
+    class PersonDisplayCommonProperties : public QObject {
+    Q_OBJECT
+    public:
+        PersonDisplayCommonProperties(rviz::Display* display, StylesChangedSubscriber* stylesChangedSubscriber);
+
+        // User-editable property variables.
+        rviz::EnumProperty* style;
+        rviz::EnumProperty* color_transform;
+        rviz::IntProperty* color_map_offset;
+
+        rviz::ColorProperty* constant_color;
+        rviz::FloatProperty* alpha;
+
+        rviz::FloatProperty* line_width;
+        rviz::FloatProperty* z_offset;
+        rviz::FloatProperty* scaling_factor;
+
+        rviz::BoolProperty* use_actual_z_position;
+
+        rviz::EnumProperty*  font_color_style;
+        rviz::ColorProperty* constant_font_color;
+        rviz::FloatProperty* font_scale;
+
+        rviz::StringProperty* m_excluded_person_ids_property;
+        rviz::StringProperty* m_included_person_ids_property;
+
+        /// These sets get updated automatically whenever the corresponding properties are updated.
+        set<person_id> m_excludedPersonIDs, m_includedPersonIDs;
+
+    private:
+        rviz::Display* m_display;
+        StylesChangedSubscriber* m_stylesChangedSubscriber;
+
+        void hideIrrelevantProperties();
+
+    private Q_SLOTS:
+        void stylesChanged();
+    };
+
+    /// A display with common properties that are shared by multiple specializations.
+    template<typename MessageType>
+    class PersonDisplayCommon: public rviz::MessageFilterDisplay<MessageType>, public StylesChangedSubscriber
+    {
+    public:
+        /// Constructor.  pluginlib::ClassLoader creates instances by calling
+        /// the default constructor, so make sure you have one.
+        PersonDisplayCommon() : m_commonProperties(0), m_veryLargeVariance(99999) {}
+        virtual ~PersonDisplayCommon() {}
+
+        /// Overrides base class method
+        virtual void onInitialize()
+        {
+            rviz::MessageFilterDisplay<MessageType>::onInitialize();
+            m_commonProperties = new PersonDisplayCommonProperties(this, this);
+        }
+
+    protected:
+        /// Common message processing. This method needs to be called by derived classes
+        bool preprocessMessage(const typename MessageType::ConstPtr& msg)
+        {
+            // Here we call the rviz::FrameManager to get the transform from the
+            // fixed frame to the frame in the header of this Imu message.  If
+            // it fails, we can't do anything else so we return.
+            if (!getContext()->getFrameManager()->getTransform(msg->header, m_framePosition, m_frameOrientation)) {
+                ROS_ERROR_THROTTLE(5.0, "Error transforming from frame '%s' into fixed frame!", msg->header.frame_id.c_str());
+                return false;
+            }
+
+            m_frameOrientation.ToRotationMatrix(m_frameRotationMatrix);
+            return true;
+        }
+
+        /// Create a visual representation of the person itself, if not set yet
+        void createPersonVisualIfRequired(Ogre::SceneNode* sceneNode, boost::shared_ptr<PersonVisual> &personVisual)
+        {
+            if (!personVisual) {
+                PersonVisualDefaultArgs defaultArgs(getContext()->getSceneManager(), sceneNode);
+                PersonVisual* newPersonVisual = 0;
+
+                if (m_commonProperties->style->getOptionInt() == STYLE_CYLINDER) newPersonVisual = new CylinderPersonVisual(defaultArgs);
+                if (m_commonProperties->style->getOptionInt() == STYLE_PERSON_MESHES) newPersonVisual = new MeshPersonVisual(defaultArgs);
+                if (m_commonProperties->style->getOptionInt() == STYLE_BOUNDING_BOXES) newPersonVisual = new BoundingBoxPersonVisual(defaultArgs);
+                if (m_commonProperties->style->getOptionInt() == STYLE_CROSSHAIRS) newPersonVisual = new CrosshairPersonVisual(defaultArgs);
+                personVisual.reset(newPersonVisual);
+            }
+
+            // Update position of the person visual
+            if (personVisual) {
+                personVisual->setPosition(Ogre::Vector3(0,0, personVisual->getHeight() * 0.5));
+            }
+        }
+
+        /// Applies common styles which apply to person visuals, such as line width etc.
+        void applyCommonStyles(boost::shared_ptr<PersonVisual> &personVisual) {
+            if(!personVisual) return;
+
+            // Set line width of wireframe visualization
+            HasLineWidth* hasLineWidth = dynamic_cast<HasLineWidth*>(personVisual.get());
+            if(hasLineWidth) {
+                hasLineWidth->setLineWidth(m_commonProperties->line_width->getFloat());
+            }
+
+            // Set scaling factor
+            personVisual->setScalingFactor(m_commonProperties->scaling_factor->getFloat());
+        }
+
+        // Builds velocity vector for a person from a twist message
+        Ogre::Vector3 getVelocityVector(const geometry_msgs::TwistWithCovariance& twist) {
+            const double zVelocityVariance = twist.covariance[2 * 6 + 2];
+            const double zVelocity = (isnan(zVelocityVariance) || isinf(zVelocityVariance)) ? 0.0 : twist.twist.linear.z;
+            return Ogre::Vector3(twist.twist.linear.x, twist.twist.linear.y, zVelocity);
+        }
+
+        /// Returns true if all xyz rotation variances are finite
+        bool hasValidOrientation(const geometry_msgs::PoseWithCovariance& pose)
+        {
+            // Check if quaternion has not been initialized, then it's invalid (all-zero elements)
+            if(pose.pose.orientation.x == 0 && pose.pose.orientation.y == 0 && pose.pose.orientation.z == 0 && pose.pose.orientation.w == 0) return false;
+
+            // According to ROS conventions, orientation covariance is always fixed-frame
+            // so no transform necessary!
+            const double xRotVariance = pose.covariance[3 * 6 + 3];
+            const double yRotVariance = pose.covariance[4 * 6 + 4];
+            const double zRotVariance = pose.covariance[5 * 6 + 5];
+            // Using logical OR instead of AND here because the orientation is given as a quaternion, instead of independent
+            // RPY angles. We assume if at least one RPY angle is valid (according to its covariance), the other angles are set to a
+            // reasonable default (as part of the quaternion) if their variance is non-finite.
+            return xRotVariance < m_veryLargeVariance || yRotVariance < m_veryLargeVariance || zRotVariance < m_veryLargeVariance;
+        }
+
+        /// Rotate the position (xyz) part of a pose covariance matrix into the fixed frame used for visualization
+        /// The covariance matrix needs to be transformed from the source (e.g. sensor) into the target (e.g. odometry) frame
+        /// This is mainly be a problem if the sensor is rotated vertically compared to the odometry frame, so that axes get swapped
+        Ogre::Matrix3 covarianceXYZIntoTargetFrame(const geometry_msgs::PoseWithCovariance& pose) {
+            Ogre::Matrix3 xyzcov;
+            for(int row = 0; row < 3; row++) for(int col = 0; col < 3; col++) xyzcov[row][col] = pose.covariance[row*6 + col]; // 6 = dimension of ROS covariance matrix
+            if(!isfinite(xyzcov.Determinant())) ROS_WARN_STREAM("Covariance matrix supplied to covarianceXYZIntoTargetFrame() contains non-finite elements: " << xyzcov);
+            return m_frameRotationMatrix * xyzcov * m_frameRotationMatrix.Transpose(); // cov(AX + a) = A cov(X) A^T
+        }
+
+        /// Set pose and orientation of person visual
+        void setPoseOrientation(Ogre::SceneNode* sceneNode, const geometry_msgs::PoseWithCovariance& pose, const Ogre::Matrix3& covXYZinTargetFrame, double personVisualHeight)
+        {
+            const geometry_msgs::Point& position = pose.pose.position;
+            const geometry_msgs::Quaternion& orientation = pose.pose.orientation;
+
+            Ogre::Matrix4 transform(m_frameOrientation);
+            transform.setTrans(m_framePosition);
+
+            Ogre::Vector3 originalPosition(position.x, position.y, position.z);
+            if(!isfinite(originalPosition.x) || !isfinite(originalPosition.y) || !isfinite(originalPosition.z)) {
+                ROS_WARN("Detected or tracked person has non-finite position! Something is wrong!");
+                return;
+            }
+
+            Ogre::Vector3 positionInTargetFrame = transform * originalPosition;
+
+            if(hasValidOrientation(pose)) {
+                Ogre::Quaternion detectionOrientation(orientation.w, orientation.x, orientation.y, orientation.z);
+                detectionOrientation.FromAngleAxis(detectionOrientation.getRoll(), Ogre::Vector3(0,0,1)); // only use yaw angle, ignore roll and pitch
+                sceneNode->setOrientation(m_frameOrientation * detectionOrientation);
+            }
+            else {
+                Ogre::Quaternion rotateTowardsCamera;
+                rotateTowardsCamera.FromAngleAxis(Ogre::Degree(180), Ogre::Vector3(0,0,1));
+                sceneNode->setOrientation(rotateTowardsCamera);
+            }
+
+            const double zVariance = covXYZinTargetFrame[2][2];
+            bool useActualZPosition = m_commonProperties->use_actual_z_position->getBool() && isfinite(zVariance) && zVariance >= 0 && zVariance < m_veryLargeVariance;
+
+            positionInTargetFrame.z = useActualZPosition ? positionInTargetFrame.z - personVisualHeight/2.0: 0.0;
+            positionInTargetFrame.z += m_commonProperties->z_offset->getFloat();
+
+            sceneNode->setPosition(positionInTargetFrame);
+        }
+
+        /// Get a color based upon track / detection ID.
+        Ogre::ColourValue getColorFromId(unsigned int object_id)
+        {
+            Ogre::ColourValue color;
+            const int colorScheme = m_commonProperties->color_transform->getOptionInt();
+            object_id += max(0, m_commonProperties->color_map_offset->getInt());
+
+            if(colorScheme == COLORS_SRL || colorScheme == COLORS_SRL_ALTERNATIVE)
+            {
+                // SRL People Tracker colors
+                const size_t NUM_SRL_COLORS = 6, NUM_SRL_COLOR_SHADES = 4, NUM_SRL_TOTAL_COLORS = NUM_SRL_COLORS * NUM_SRL_COLOR_SHADES;
+                const unsigned int spencer_colors[NUM_SRL_TOTAL_COLORS] = {
+                    0xC00000, 0xFF0000, 0xFF5050, 0xFFA0A0, // red
+                    0x00C000, 0x00FF00, 0x50FF50, 0xA0FFA0, // green
+                    0x0000C0, 0x0000FF, 0x5050FF, 0xA0A0FF, // blue
+                    0xF20A86, 0xFF00FF, 0xFF50FF, 0xFFA0FF, // magenta
+                    0x00C0C0, 0x00FFFF, 0x50FFFF, 0xA0FFFF, // cyan
+                    0xF5A316, 0xFFFF00, 0xFFFF50, 0xFFFFA0  // yellow
+                };
+
+                unsigned int rgb = 0;
+                const unsigned int tableId = object_id % NUM_SRL_TOTAL_COLORS;
+                if(m_commonProperties->color_transform->getOptionInt() == COLORS_SRL) {
+                    // Colors in original order (first vary shade, then vary color)
+                    rgb = spencer_colors[tableId];
+                }
+                else if(m_commonProperties->color_transform->getOptionInt() == COLORS_SRL_ALTERNATIVE) {
+                    // Colors in alternative order (first vary color, then vary shade)
+                    unsigned int shadeIndex = tableId / NUM_SRL_COLORS;
+                    unsigned int colorIndex = tableId % NUM_SRL_COLORS;
+                    rgb = spencer_colors[colorIndex * NUM_SRL_COLOR_SHADES + shadeIndex];
+                }
+
+                float r = ((rgb >> 16) & 0xff) / 255.0f,
+                       g = ((rgb >> 8)  & 0xff) / 255.0f,
+                       b = ((rgb >> 0)  & 0xff) / 255.0f;
+
+                color = Ogre::ColourValue(r, g, b, 1.0);
+            }
+            else if(colorScheme == COLORS_RAINBOW || colorScheme == COLORS_RAINBOW_BW)
+            {
+                const size_t NUM_COLOR = 10, NUM_BW = 4;
+                const unsigned int rainbow_colors[NUM_COLOR + NUM_BW] = {
+                    0xaf1f90, 0x000846, 0x00468a, 0x00953d, 0xb2c908, 0xfcd22a, 0xffa800, 0xff4500, 0xe0000b, 0xb22222,
+                    0xffffff, 0xb8b8b8, 0x555555, 0x000000
+                };
+
+                color.setAsARGB(rainbow_colors[object_id % (colorScheme == COLORS_RAINBOW ? NUM_COLOR : (NUM_COLOR+NUM_BW))]);
+            }
+            else if(colorScheme == COLORS_FLAT)
+            {
+                const size_t NUM_COLOR = 10;
+                const unsigned int flat_colors[NUM_COLOR] = {
+                    0x990033, 0xa477c4, 0x3498db, 0x1abc9c, 0x55e08f, 0xfff054, 0xef5523, 0xfe374a, 0xbaa891, 0xad5f43
+                };
+
+                color.setAsARGB(flat_colors[object_id % NUM_COLOR]);
+            }
+            else if(colorScheme == COLORS_VINTAGE)
+            {
+                const size_t NUM_COLOR = 10;
+                const unsigned int vintage_colors[NUM_COLOR] = {
+                    0xd05e56, 0x797d88, 0x448d7a, 0xa5d1cd, 0x88a764, 0xebe18c, 0xd8a027, 0xffcc66, 0xdc3f1c, 0xff9966
+                };
+
+                color.setAsARGB(vintage_colors[object_id % NUM_COLOR]);
+            }
+            else
+            {
+                // Constant color for all tracks
+                color = m_commonProperties->constant_color->getOgreColor();
+            }
+
+            color.a = 1.0f; // force full opacity
+            return color;
+        }
+
+        /// Checks if a person shall be hidden (can be set using include/exclude person ID properties in GUI)
+        bool isPersonHidden(person_id personId)
+        {
+            bool isIncluded = m_commonProperties->m_includedPersonIDs.find(personId) != m_commonProperties->m_includedPersonIDs.end();
+            if(isIncluded) return false;
+            if(!m_commonProperties->m_includedPersonIDs.empty()) return true;
+            return m_commonProperties->m_excludedPersonIDs.find(personId) != m_commonProperties->m_excludedPersonIDs.end();
+        }
+
+        /// Must be implemented by derived classes because MOC doesn't work in templates
+        virtual rviz::DisplayContext* getContext() = 0;
+
+        /// Common properties for the displays in this plugin
+        PersonDisplayCommonProperties* m_commonProperties;
+
+    protected:
+        Ogre::Quaternion m_frameOrientation;
+        Ogre::Matrix3 m_frameRotationMatrix;
+        Ogre::Vector3 m_framePosition;
+        const double m_veryLargeVariance;
+    };
+} // end namespace spencer_tracking_rviz_plugin
+
+
+#endif // PERSON_DISPLAY_COMMON_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/social_activities_display.cpp

@@ -0,0 +1,566 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rviz/visualization_manager.h>
+#include <rviz/frame_manager.h>
+#include "rviz/selection/selection_manager.h"
+
+#ifndef Q_MOC_RUN
+#include "social_activities_display.h"
+
+#include <boost/lexical_cast.hpp>
+#include <boost/tokenizer.hpp>
+#include <boost/algorithm/string.hpp>
+#include <boost/range/adaptor/map.hpp>
+#include <boost/foreach.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+// required to fix orientation of any Cylinder shapes
+const Ogre::Quaternion shapeQuaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) );
+
+
+namespace sr = spencer_social_relation_msgs;
+namespace spencer_tracking_rviz_plugin
+{
+
+void SocialActivitiesDisplay::onInitialize()
+{
+    m_trackedPersonsCache.initialize(this, context_, update_nh_);
+    PersonDisplayCommon::onInitialize();
+
+    QObject::connect(m_commonProperties->style, SIGNAL(changed()), this, SLOT(personVisualTypeChanged()) );
+
+    m_excluded_activity_types_property = new rviz::StringProperty( "Excluded activity types", "", "Comma-separated list of activity types whose visualization should be hidden", this, SLOT(stylesChanged()) );
+    m_included_activity_types_property = new rviz::StringProperty( "Included activity types", "", "Comma-separated list of activity types whose visualization should be visible, overrides excluded", this, SLOT(stylesChanged()) );
+
+    m_min_confidence_property = new rviz::FloatProperty( "Min. confidence", 0.0, "Minimum confidence for a social activity to be shown", this, SLOT(stylesChanged()) );
+    m_min_confidence_property->setMin( 0.0 );
+
+    m_hide_with_no_activity_property = new rviz::BoolProperty( "Hide tracks with no activity", false, "Hide all tracks which do not have at least one social activity assigned", this, SLOT(stylesChanged()));
+
+    m_render_intraactivity_connections_property = new rviz::BoolProperty( "Connect tracks sharing the same activity", true, "Connect all tracks that share the same activity", this, SLOT(stylesChanged()));
+    m_line_width_property = new rviz::FloatProperty( "Line width", 0.05, "Line width of connecting lines", m_render_intraactivity_connections_property, SLOT(stylesChanged()), this );
+    m_line_width_property->setMin( 0.0 );
+
+    m_render_activity_types_property = new rviz::BoolProperty( "Render activity type texts", true, "Render activity types as text", this, SLOT(stylesChanged()));
+    m_activity_type_per_track_property = new rviz::BoolProperty( "Activity type per track", false, "Show activity type for each individual track", this, SLOT(stylesChanged()));
+    m_render_confidences_property = new rviz::BoolProperty( "Render confidences", true, "Render confidence values next to activity type", this, SLOT(stylesChanged()));
+
+    m_render_circles_property = new rviz::BoolProperty( "Render circles below person", true, "Render circles below person", this, SLOT(stylesChanged()));
+    m_circle_radius_property = new rviz::FloatProperty( "Radius", 0.45, "Radius of circles below person in meters", m_render_circles_property, SLOT(stylesChanged()), this );
+    m_circle_radius_property->setMin( 0.0 );
+
+    m_circle_alpha_property = new rviz::FloatProperty( "Alpha", 1.0, "Alpha value (opacity) of circles below person", m_render_circles_property, SLOT(stylesChanged()), this );
+    m_circle_alpha_property->setMin( 0.0 );
+    m_circle_alpha_property->setMax( 1.0 );
+
+    m_occlusion_alpha_property = new rviz::FloatProperty( "Occlusion alpha", 0.5, "Alpha multiplier for history of occluded tracks", this, SLOT(stylesChanged()) );
+    m_occlusion_alpha_property->setMin( 0.0 );
+
+    m_activity_type_offset = new rviz::FloatProperty( "Activity type Z offset", 2.0, "Offset in z position (height) of the activity type text", this, SLOT(stylesChanged()) );
+
+    m_activity_colors = new rviz::Property( "Activity colors", "", "Colors of different social activity types", this );
+
+    // Add colors for new activity types here, also adjust header file!
+    m_activity_color_unknown = new rviz::ColorProperty( "(Unknown activity)", QColor(255,255,255), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_none = new rviz::ColorProperty( "(No activity)", QColor(200,200,200), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_shopping = new rviz::ColorProperty( sr::SocialActivity::TYPE_SHOPPING.c_str(), QColor(0,0,255), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_standing = new rviz::ColorProperty( sr::SocialActivity::TYPE_STANDING.c_str(), QColor(0,0,0), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_individual_moving = new rviz::ColorProperty( sr::SocialActivity::TYPE_INDIVIDUAL_MOVING.c_str(), QColor(128,128,128), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_waiting_in_queue = new rviz::ColorProperty( sr::SocialActivity::TYPE_WAITING_IN_QUEUE.c_str(), QColor(255,0,255), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_looking_at_information_screen = new rviz::ColorProperty( sr::SocialActivity::TYPE_LOOKING_AT_INFORMATION_SCREEN.c_str(), QColor(255,255,0), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_looking_at_kiosk = new rviz::ColorProperty( sr::SocialActivity::TYPE_LOOKING_AT_KIOSK.c_str(), QColor(255,128,0), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_group_assembling = new rviz::ColorProperty( sr::SocialActivity::TYPE_GROUP_ASSEMBLING.c_str(), QColor(0,128,255), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_group_moving = new rviz::ColorProperty( sr::SocialActivity::TYPE_GROUP_MOVING.c_str(), QColor(0,255,255), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_flow = new rviz::ColorProperty( sr::SocialActivity::TYPE_FLOW_WITH_ROBOT.c_str(), QColor(0,255,0), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_antiflow = new rviz::ColorProperty( sr::SocialActivity::TYPE_ANTIFLOW_AGAINST_ROBOT.c_str(), QColor(255,0,0), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_waiting_for_others = new rviz::ColorProperty( sr::SocialActivity::TYPE_WAITING_FOR_OTHERS.c_str(), QColor(255,170,255), "", m_activity_colors, SLOT(stylesChanged()), this);
+    m_activity_color_looking_for_help = new rviz::ColorProperty( sr::SocialActivity::TYPE_LOOKING_FOR_HELP.c_str(), QColor(155,170,255), "", m_activity_colors, SLOT(stylesChanged()), this);
+
+    m_commonProperties->color_transform->setHidden(true);
+    m_commonProperties->color_map_offset->setHidden(true);
+
+    // Create a scene node for visualizing group affiliation history
+    m_socialActivitiesSceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+}
+
+SocialActivitiesDisplay::~SocialActivitiesDisplay()
+{
+}
+
+// Clear the visuals by deleting their objects.
+void SocialActivitiesDisplay::reset()
+{
+    PersonDisplayCommon::reset();
+    m_trackedPersonsCache.reset();
+    m_socialActivityVisuals.clear();
+    m_personVisualMap.clear();
+    m_highestConfidenceActivityPerTrack.clear();
+}
+
+void SocialActivitiesDisplay::update(float wall_dt, float ros_dt)
+{
+    // Update animations
+    foreach(PersonVisualContainer& personVisualContainer, m_personVisualMap | boost::adaptors::map_values) {
+        if(personVisualContainer.personVisual) {
+            personVisualContainer.personVisual->update(ros_dt);
+        }
+    }
+}
+
+void SocialActivitiesDisplay::stylesChanged()
+{
+    m_commonProperties->color_map_offset->setHidden(true);
+
+    // Get list of group IDs belonging to tracks that shall be hidden or visible
+    m_excludedActivityTypes.clear();
+    {
+        string inputString = m_excluded_activity_types_property->getStdString();
+        char_separator<char> separator(",");
+        tokenizer< char_separator<char> > tokens(inputString, separator);
+        foreach(const string& token, tokens) {
+            string tmp = token;
+            boost::algorithm::to_lower(tmp);
+            m_excludedActivityTypes.insert(tmp);
+        }
+    }
+    m_includedActivityTypes.clear();
+    {
+        string inputString = m_included_activity_types_property->getStdString();
+        char_separator<char> separator(",");
+        tokenizer< char_separator<char> > tokens(inputString, separator);
+        foreach(const string& token, tokens) {
+            string tmp = token;
+            boost::algorithm::to_lower(tmp);
+            m_includedActivityTypes.insert(tmp);
+        }
+    }
+
+    foreach(boost::shared_ptr<SocialActivityVisual> socialActivityVisual, m_socialActivityVisuals) {
+        updateSocialActivityVisualStyles(socialActivityVisual);
+    }
+
+    foreach(PersonVisualContainer& personVisualContainer, m_personVisualMap | boost::adaptors::map_values) {
+        if(personVisualContainer.personVisual) {
+            // Update common styles to person visual, such as line width
+            applyCommonStyles(personVisualContainer.personVisual);
+
+            // Update color according to highest-ranking social activity for this person
+            Ogre::ColourValue activityColor;
+
+            activity_type activityType = "";
+            float confidence = 1.0f;
+            if(m_highestConfidenceActivityPerTrack.find(personVisualContainer.trackId) != m_highestConfidenceActivityPerTrack.end()) {
+                activityType = m_highestConfidenceActivityPerTrack[personVisualContainer.trackId].type;
+                confidence   = m_highestConfidenceActivityPerTrack[personVisualContainer.trackId].confidence;
+            }
+            else {
+                if(m_hide_with_no_activity_property->getBool()) confidence = -999;
+            }
+            activityColor = getActivityColor(activityType, confidence);
+
+            personVisualContainer.personVisual->setColor(activityColor);
+        }
+    }
+}
+
+bool SocialActivitiesDisplay::isActivityTypeHidden(activity_type activityType) {
+    boost::algorithm::to_lower(activityType);
+
+    bool isIncluded = m_includedActivityTypes.find(activityType) != m_includedActivityTypes.end();
+    if(isIncluded) return false;
+    if(!m_includedActivityTypes.empty()) return true;
+
+    return m_excludedActivityTypes.find(activityType) != m_excludedActivityTypes.end();
+}
+
+Ogre::ColourValue SocialActivitiesDisplay::getActivityColor(activity_type activityType, float confidence) {
+    bool hideActivityType = isActivityTypeHidden(activityType);
+
+    // Determine color
+    rviz::ColorProperty* colorProperty = NULL;
+
+    // Add new social activity types here, and also add a property in constructor at top of file!
+    if(activityType.empty())
+        colorProperty = m_activity_color_none;
+    else if(activityType == sr::SocialActivity::TYPE_SHOPPING)
+        colorProperty = m_activity_color_shopping;
+    else if(activityType == sr::SocialActivity::TYPE_STANDING)
+        colorProperty = m_activity_color_standing;
+    else if(activityType == sr::SocialActivity::TYPE_INDIVIDUAL_MOVING)
+        colorProperty = m_activity_color_individual_moving;
+    else if(activityType == sr::SocialActivity::TYPE_WAITING_IN_QUEUE)
+        colorProperty = m_activity_color_waiting_in_queue;
+    else if(activityType == sr::SocialActivity::TYPE_LOOKING_AT_INFORMATION_SCREEN)
+        colorProperty = m_activity_color_looking_at_information_screen;
+    else if(activityType == sr::SocialActivity::TYPE_LOOKING_AT_KIOSK)
+        colorProperty = m_activity_color_looking_at_kiosk;
+    else if(activityType == sr::SocialActivity::TYPE_GROUP_ASSEMBLING)
+        colorProperty = m_activity_color_group_assembling;
+    else if(activityType == sr::SocialActivity::TYPE_GROUP_MOVING)
+        colorProperty = m_activity_color_group_moving;
+    else if(activityType == sr::SocialActivity::TYPE_FLOW_WITH_ROBOT)
+        colorProperty = m_activity_color_flow;
+    else if(activityType == sr::SocialActivity::TYPE_ANTIFLOW_AGAINST_ROBOT)
+        colorProperty = m_activity_color_antiflow;
+    else if(activityType == sr::SocialActivity::TYPE_WAITING_FOR_OTHERS)
+        colorProperty = m_activity_color_waiting_for_others;
+    else if(activityType == sr::SocialActivity::TYPE_LOOKING_FOR_HELP)
+        colorProperty = m_activity_color_looking_for_help;
+    else
+        colorProperty = m_activity_color_unknown;
+
+    Ogre::ColourValue activityColor = colorProperty->getOgreColor();
+    activityColor.a = 1.0f;
+
+    activityColor.a *= m_commonProperties->alpha->getFloat(); // general alpha
+    if(hideActivityType) activityColor.a = 0;
+
+    if(confidence < m_min_confidence_property->getFloat()) activityColor.a = 0;
+
+    return activityColor;
+}
+
+// Set the rendering style (cylinders, meshes, ...) of tracked persons
+void SocialActivitiesDisplay::personVisualTypeChanged()
+{
+    m_personVisualMap.clear();
+    foreach(PersonVisualContainer& personVisualContainer, m_personVisualMap | boost::adaptors::map_values) {
+        personVisualContainer.personVisual.reset();
+        createPersonVisualIfRequired(personVisualContainer.sceneNode.get(), personVisualContainer.personVisual);
+    }
+    stylesChanged();
+}
+
+void SocialActivitiesDisplay::updateSocialActivityVisualStyles(boost::shared_ptr<SocialActivityVisual>& socialActivityVisual)
+{
+    std::stringstream ss;
+
+    Ogre::ColourValue activityColor = getActivityColor(socialActivityVisual->activityType, socialActivityVisual->confidence);
+    bool hideActivity = isActivityTypeHidden(socialActivityVisual->activityType) || socialActivityVisual->confidence < m_min_confidence_property->getFloat();
+    bool showCircles = m_render_circles_property->getBool();
+
+    foreach(boost::shared_ptr<rviz::Shape> circle, socialActivityVisual->socialActivityAssignmentCircles) {
+        circle->setColor(activityColor.r, activityColor.g, activityColor.b, activityColor.a * m_circle_alpha_property->getFloat() * (showCircles ? 1.0f : 0.0f));
+        const double circleDiameter = m_circle_radius_property->getFloat() * 2, circleHeight = 0;
+        circle->setScale(shapeQuaternion * Ogre::Vector3(circleDiameter, circleDiameter, circleHeight));
+    }
+
+    double connectionLineVisibilityAlpha = m_render_intraactivity_connections_property->getBool() ? 1.0 : 0.0;
+    foreach(boost::shared_ptr<rviz::BillboardLine> connectionLine, socialActivityVisual->connectionLines) {
+        connectionLine->setColor(activityColor.r, activityColor.g, activityColor.b, activityColor.a * connectionLineVisibilityAlpha);
+        connectionLine->setLineWidth(m_line_width_property->getFloat());
+    }
+
+    // Update text colors, size and visibility
+    ss.str(""); ss << socialActivityVisual->activityType;
+    if(m_render_confidences_property->getBool()) ss << fixed << setprecision(1) << " (" << 100*socialActivityVisual->confidence << "%)";
+
+    for(int i = 0; i < socialActivityVisual->typeTexts.size(); i++) {
+        boost::shared_ptr<TextNode>& typeText = socialActivityVisual->typeTexts[i];
+
+        if(typeText) { // might be not set if center of activity could not be determined
+            typeText->setCaption(ss.str());
+
+            Ogre::Vector3 centerAt;
+            if(m_activity_type_per_track_property->getBool()) {
+                boost::shared_ptr<CachedTrackedPerson> trackedPerson = m_trackedPersonsCache.lookup(socialActivityVisual->trackIds[i]);
+                if(!trackedPerson) continue;
+                centerAt = Ogre::Vector3(trackedPerson->center.x, trackedPerson->center.y, m_commonProperties->z_offset->getFloat());
+            }
+            else centerAt = Ogre::Vector3(socialActivityVisual->socialActivityCenter.x, socialActivityVisual->socialActivityCenter.y, socialActivityVisual->socialActivityCenter.z);
+
+            Ogre::ColourValue fontColor = m_commonProperties->font_color_style->getOptionInt() == FONT_COLOR_CONSTANT ? m_commonProperties->constant_font_color->getOgreColor() : activityColor;
+            fontColor.a = m_commonProperties->alpha->getFloat();
+            if(hideActivity) fontColor.a = 0;
+            float characterHeight = 0.23 * m_commonProperties->font_scale->getFloat();
+            typeText->setVisible(m_render_activity_types_property->getBool());
+            typeText->setCharacterHeight(characterHeight);
+            typeText->setColor(fontColor);
+            typeText->setPosition(m_frameTransform * Ogre::Vector3(
+                centerAt.x,
+                centerAt.y,
+                centerAt.z + m_activity_type_offset->getFloat() + m_commonProperties->z_offset->getFloat()
+                    + socialActivityVisual->declutteringOffset * characterHeight /* this is for decluttering of overlapping labels */));
+        }
+    }
+}
+
+// Helper function for guaranteeing consistent ordering of activity labels
+bool CompareActivityByType (const SocialActivitiesDisplay::ActivityWithConfidence& first, const SocialActivitiesDisplay::ActivityWithConfidence& second) {
+    return first.type < second.type;
+}
+
+// This is our callback to handle an incoming group message.
+void SocialActivitiesDisplay::processMessage(const spencer_social_relation_msgs::SocialActivities::ConstPtr& msg)
+{
+    // Get transforms into fixed frame etc.
+    if(!preprocessMessage(msg)) return;
+
+    // Transform into Rviz fixed frame
+    m_frameTransform = Ogre::Matrix4(m_frameOrientation);
+    m_frameTransform.setTrans(m_framePosition);
+    stringstream ss;
+
+    // Clear previous visualization (not very efficient, but easier to implement)
+    // Note that person visuals are not cleared to allow walking animations to function properly
+    m_socialActivityVisuals.clear();
+    m_socialActivitiesSceneNode->removeAndDestroyAllChildren();
+
+    m_highestConfidenceActivityPerTrack.clear(); // used later on to determine color of person visuals
+    m_allActivitiesPerTrack.clear();
+
+    unsigned int numTracksWithUnknownPosition = 0;
+
+
+    //
+    // Iterate over all social activities in this message
+    //
+    foreach (const spencer_social_relation_msgs::SocialActivity& socialActivity, msg->elements)
+    {
+        // Create a new visual representation of the social activity
+        boost::shared_ptr<SocialActivityVisual> socialActivityVisual = boost::shared_ptr<SocialActivityVisual>(new SocialActivityVisual);
+        socialActivityVisual->activityType = socialActivity.type;
+        socialActivityVisual->confidence = socialActivity.confidence;
+        socialActivityVisual->personCount = socialActivity.track_ids.size();
+        m_socialActivityVisuals.push_back(socialActivityVisual);
+
+        geometry_msgs::Point socialActivityCenter;
+        size_t numGoodTracksInActivity = 0;
+
+        //
+        // Assignment circles, person visuals (if enabled) + connections between social activity members
+        //
+
+        for(size_t trackIndex = 0; trackIndex < socialActivity.track_ids.size(); trackIndex++)
+        {
+            const track_id trackId = socialActivity.track_ids[trackIndex];
+            boost::shared_ptr<CachedTrackedPerson> trackedPerson = m_trackedPersonsCache.lookup(trackId);
+
+            ActivityWithConfidence activityWithConfidence;
+            activityWithConfidence.type = socialActivity.type;
+            activityWithConfidence.confidence = socialActivity.confidence;
+
+            // Update map of highest-confidence activity per person
+            if(m_highestConfidenceActivityPerTrack.find(trackId) == m_highestConfidenceActivityPerTrack.end()
+                    || socialActivity.confidence > m_highestConfidenceActivityPerTrack[trackId].confidence) {
+                m_highestConfidenceActivityPerTrack[trackId] = activityWithConfidence;
+            }
+
+            // Update map of all activities per person
+            if(m_allActivitiesPerTrack.find(trackId) == m_allActivitiesPerTrack.end()) {
+                m_allActivitiesPerTrack[trackId] = vector<ActivityWithConfidence>();
+            }
+            m_allActivitiesPerTrack[trackId].push_back(activityWithConfidence);
+
+
+            // Get current track position
+            if(!trackedPerson) {
+                numTracksWithUnknownPosition++;
+            }
+            else
+            {
+                socialActivityVisual->trackIds.push_back(trackId);
+                numGoodTracksInActivity++;
+
+                Ogre::Vector3 trackCenterAtGroundPlane(trackedPerson->center.x, trackedPerson->center.y, m_commonProperties->z_offset->getFloat());
+                socialActivityCenter.x += trackCenterAtGroundPlane.x;
+                socialActivityCenter.y += trackCenterAtGroundPlane.y;
+                socialActivityCenter.z += trackCenterAtGroundPlane.z;
+
+
+                //
+                // Social activity assignment circles (below tracks)
+                //
+
+                if(m_render_circles_property->getBool()) // only create circles if they are enabled, for better performance
+                {
+                    boost::shared_ptr<rviz::Shape> circle = boost::shared_ptr<rviz::Shape>(new rviz::Shape(rviz::Shape::Cylinder, context_->getSceneManager(), m_socialActivitiesSceneNode.get()));
+
+                    const double circleHeight = 0;
+                    circle->setOrientation(shapeQuaternion);
+
+                    Ogre::Vector3 circlePos = trackCenterAtGroundPlane + Ogre::Vector3(0, 0, -0.5*circleHeight - 0.01);
+                    circle->setPosition(circlePos);
+
+                    socialActivityVisual->socialActivityAssignmentCircles.push_back(circle);
+                }
+
+                //
+                // Intra-activity connections
+                //
+
+                if(m_render_intraactivity_connections_property->getBool()) // only create circles if they are enabled, for better performance
+                {
+                    // Iterate over all tracks sharing the same activity to render intra-activity connections
+                    for(size_t otherTrackIndex = trackIndex + 1; otherTrackIndex < socialActivity.track_ids.size(); otherTrackIndex++)
+                    {
+                        const track_id otherTrackId = socialActivity.track_ids[otherTrackIndex];
+                        boost::shared_ptr<CachedTrackedPerson> otherTrackedPerson = m_trackedPersonsCache.lookup(otherTrackId);
+
+                        // Get other track's position
+                        if(otherTrackedPerson) {
+                            // Get positions. These are already in fixed frame coordinates!
+                            const Ogre::Vector3 verticalShift(0,0, 0.5 + m_commonProperties->z_offset->getFloat());
+                            const Ogre::Vector3& position1 = verticalShift + trackedPerson->center;
+                            const Ogre::Vector3& position2 = verticalShift + otherTrackedPerson->center;
+
+                            // Add line connecting the two tracks
+                            boost::shared_ptr<rviz::BillboardLine> connectionLine(new rviz::BillboardLine(context_->getSceneManager(), m_socialActivitiesSceneNode.get()));
+                            connectionLine->setMaxPointsPerLine(2);
+                            connectionLine->addPoint(position1);
+                            connectionLine->addPoint(position2);
+                            socialActivityVisual->connectionLines.push_back(connectionLine);
+                        }
+                    } // end loop over other tracks sharing same activity
+                }
+
+            } // end if track found
+        } // end for loop over tracks
+
+        //
+        // Texts
+        //
+        socialActivityCenter.x /= (double) numGoodTracksInActivity;
+        socialActivityCenter.y /= (double) numGoodTracksInActivity;
+        socialActivityCenter.z /= (double) numGoodTracksInActivity;
+        socialActivityVisual->socialActivityCenter = socialActivityCenter;
+
+        // Social activity type
+        if(numGoodTracksInActivity > 0) {
+            for(int i = 0; i < (m_activity_type_per_track_property->getBool() ? socialActivityVisual->trackIds.size() : 1); i++) {
+                boost::shared_ptr<TextNode> typeText(new TextNode(context_->getSceneManager(), m_socialActivitiesSceneNode.get()));
+                typeText->showOnTop();
+                socialActivityVisual->typeTexts.push_back(typeText);
+            }
+        }
+    } // end for loop over all social activities in msg
+
+
+    //
+    // Second iteration over all social activities and member tracks, required for decluttering
+    //
+    ROS_ASSERT(msg->elements.size() == m_socialActivityVisuals.size());
+    for(size_t i = 0; i < msg->elements.size(); i++)
+    {
+        const spencer_social_relation_msgs::SocialActivity& socialActivity = msg->elements[i];
+        boost::shared_ptr<SocialActivityVisual> socialActivityVisual = m_socialActivityVisuals[i];
+        size_t maxIndexOfThisActivity = 0;
+
+        for(size_t trackIndex = 0; trackIndex < socialActivity.track_ids.size(); trackIndex++)
+        {
+            const track_id trackId = socialActivity.track_ids[trackIndex];
+            vector<ActivityWithConfidence> activitiesOfTrack(m_allActivitiesPerTrack[trackId]);
+
+            // Sort to ensure consistency across multiple runs, even if msg->elements order changes
+            std::sort(activitiesOfTrack.begin(), activitiesOfTrack.end(), CompareActivityByType);
+
+            for(size_t j = 0; j < activitiesOfTrack.size(); j++) {
+                if(activitiesOfTrack[j].type == socialActivityVisual->activityType) {
+                    maxIndexOfThisActivity = std::max(maxIndexOfThisActivity, j);
+                    break;
+                }
+            }
+        }
+
+        socialActivityVisual->declutteringOffset = maxIndexOfThisActivity; // got it
+    }
+
+
+    //
+    // Create person visuals for all tracked persons (colored in color of activity with highest confidence)
+    //
+    set<track_id> seenTrackIds;
+    foreach(const TrackedPersonsCache::CachedTrackedPersonsMap::value_type& entry, m_trackedPersonsCache.getMap()) {
+        const track_id trackId = entry.first;
+        const boost::shared_ptr<CachedTrackedPerson> trackedPerson = entry.second;
+
+        PersonVisualContainer personVisualContainer;
+        if(m_personVisualMap.find(trackId) != m_personVisualMap.end()) {
+            personVisualContainer = m_personVisualMap[trackId];
+        }
+        else {
+            personVisualContainer.trackId = trackId;
+            personVisualContainer.sceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode()); // This scene node is the parent of all visualization elements for the tracked person
+        }
+
+        // Create new visual for the person itself, if needed
+        createPersonVisualIfRequired(personVisualContainer.sceneNode.get(), personVisualContainer.personVisual);
+
+        const double personHeight = personVisualContainer.personVisual ? personVisualContainer.personVisual->getHeight() : 0;
+        const Ogre::Matrix3 covXYZinTargetFrame = covarianceXYZIntoTargetFrame(trackedPerson->pose);
+        setPoseOrientation(personVisualContainer.sceneNode.get(), trackedPerson->pose, covXYZinTargetFrame, personHeight);
+
+        // Update walking animation if required
+        const Ogre::Vector3 velocityVector = getVelocityVector(trackedPerson->twist);
+        boost::shared_ptr<MeshPersonVisual> meshPersonVisual = boost::dynamic_pointer_cast<MeshPersonVisual>(personVisualContainer.personVisual);
+        if(meshPersonVisual) {
+            meshPersonVisual->setWalkingSpeed(velocityVector.length());
+        }
+
+        m_personVisualMap[trackId] = personVisualContainer; // to keep visuals alive across multiple frames, for walking animation
+        seenTrackIds.insert(trackId);
+    }
+
+    // Delete obsolete track visuals of tracks that have disappeared
+    set<track_id> trackIdsToDelete;
+    foreach(track_id trackId, m_personVisualMap | boost::adaptors::map_keys) {
+        if(seenTrackIds.find(trackId) == seenTrackIds.end()) trackIdsToDelete.insert(trackId);
+    }
+    foreach(track_id trackIdToDelete, trackIdsToDelete) {
+        m_personVisualMap.erase(trackIdToDelete);
+    }
+
+    //
+    // Update all styles (colors etc. which can also be reconfigured at runtime, even if no new messages are received)
+    //
+    stylesChanged();
+
+
+    //
+    // Update status (shown in property pane)
+    //
+
+    ss.str("");
+    ss << msg->elements.size() << " activities(s)";
+    setStatusStd(rviz::StatusProperty::Ok, "Social activities", ss.str());
+
+    ss.str("");
+    ss << numTracksWithUnknownPosition << " track(s) with unknown position";
+    setStatusStd(0 == numTracksWithUnknownPosition ? rviz::StatusProperty::Ok : rviz::StatusProperty::Warn, "Track-to-activity assignment", ss.str());
+}
+
+} // end namespace spencer_tracking_rviz_plugin
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::SocialActivitiesDisplay, rviz::Display)

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/social_activities_display.h

@@ -0,0 +1,167 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SOCIAL_ACTIVITIES_DISPLAY_H
+#define SOCIAL_ACTIVITIES_DISPLAY_H
+#ifndef Q_MOC_RUN
+#include <map>
+#include <vector>
+#include <boost/circular_buffer.hpp>
+#include <spencer_social_relation_msgs/SocialActivities.h>
+#endif
+#include "person_display_common.h"
+#include "tracked_persons_cache.h"
+
+namespace spencer_tracking_rviz_plugin
+{
+    typedef std::string activity_type;
+
+    /// The display which can be added in RViz to display social activities.
+    class SocialActivitiesDisplay: public PersonDisplayCommon<spencer_social_relation_msgs::SocialActivities>
+    {
+    Q_OBJECT
+    public:
+        // To determine, for persons involved in multiple social activities, their most likely one.
+        struct ActivityWithConfidence {
+            activity_type type;
+            float confidence;
+        };
+
+        // Constructor.  pluginlib::ClassLoader creates instances by calling
+        // the default constructor, so make sure you have one.
+        SocialActivitiesDisplay() {};
+        virtual ~SocialActivitiesDisplay();
+
+        // Overrides of protected virtual functions from Display.  As much
+        // as possible, when Displays are not enabled, they should not be
+        // subscribed to incoming data and should not show anything in the
+        // 3D view.  These functions are where these connections are made
+        // and broken.
+
+        // Called after the constructors have run
+        virtual void onInitialize();
+
+        // Called periodically by the visualization manager
+        virtual void update(float wall_dt, float ros_dt);
+
+    protected:
+        // A helper to clear this display back to the initial state.
+        virtual void reset();
+
+        // Must be implemented by derived classes because MOC doesn't work in templates
+        virtual rviz::DisplayContext* getContext() {
+            return context_;
+        }
+
+    private:
+        struct SocialActivityVisual {
+            vector<boost::shared_ptr<rviz::Shape> > socialActivityAssignmentCircles;
+            vector<boost::shared_ptr<rviz::BillboardLine> > connectionLines;
+            vector< boost::shared_ptr<TextNode> > typeTexts;
+            vector<track_id> trackIds;
+            activity_type activityType;
+            float confidence;
+            geometry_msgs::Point socialActivityCenter;
+            size_t personCount;
+            size_t declutteringOffset; // for decluttering of labels etc. in case of multiple activities per track, e.g. 1 = shift label by 1 row
+        };
+
+        // Functions to handle an incoming ROS message.
+        void processMessage(const spencer_social_relation_msgs::SocialActivities::ConstPtr& msg);
+
+        // Helper functions
+        void updateSocialActivityVisualStyles(boost::shared_ptr<SocialActivityVisual>& groupVisual);
+        bool isActivityTypeHidden(activity_type activityType);
+        Ogre::ColourValue getActivityColor(activity_type activityType, float confidence);
+
+        // Scene nodes
+        boost::shared_ptr<Ogre::SceneNode> m_socialActivitiesSceneNode;
+
+        // User-editable property variables.
+        rviz::StringProperty* m_excluded_activity_types_property;
+        rviz::StringProperty* m_included_activity_types_property;
+
+        rviz::BoolProperty* m_render_intraactivity_connections_property;
+        rviz::BoolProperty* m_render_activity_types_property;
+        rviz::BoolProperty* m_activity_type_per_track_property;
+        rviz::BoolProperty* m_render_confidences_property;
+        rviz::BoolProperty* m_render_circles_property;
+        rviz::BoolProperty* m_hide_with_no_activity_property;
+
+        rviz::FloatProperty* m_occlusion_alpha_property;
+        rviz::FloatProperty* m_min_confidence_property;
+        rviz::FloatProperty* m_circle_radius_property;
+        rviz::FloatProperty* m_circle_alpha_property;
+        rviz::FloatProperty* m_line_width_property;
+        rviz::FloatProperty* m_activity_type_offset; // z offset of the group ID text
+
+        rviz::Property* m_activity_colors;
+        rviz::ColorProperty* m_activity_color_none;
+        rviz::ColorProperty* m_activity_color_unknown;
+        rviz::ColorProperty* m_activity_color_shopping;
+        rviz::ColorProperty* m_activity_color_standing;
+        rviz::ColorProperty* m_activity_color_individual_moving;
+        rviz::ColorProperty* m_activity_color_waiting_in_queue;
+        rviz::ColorProperty* m_activity_color_looking_at_information_screen;
+        rviz::ColorProperty* m_activity_color_looking_at_kiosk;
+        rviz::ColorProperty* m_activity_color_group_assembling;
+        rviz::ColorProperty* m_activity_color_group_moving;
+        rviz::ColorProperty* m_activity_color_flow;
+        rviz::ColorProperty* m_activity_color_antiflow;
+        rviz::ColorProperty* m_activity_color_waiting_for_others;
+        rviz::ColorProperty* m_activity_color_looking_for_help;
+
+
+        // State variables
+        struct PersonVisualContainer {
+            boost::shared_ptr<PersonVisual> personVisual;
+            boost::shared_ptr<Ogre::SceneNode> sceneNode;
+            track_id trackId;
+        };
+
+        vector<boost::shared_ptr<SocialActivityVisual> > m_socialActivityVisuals;
+        map<track_id, PersonVisualContainer > m_personVisualMap; // to keep person visuals alive across multiple frames, for walking animation
+
+        map<track_id, ActivityWithConfidence> m_highestConfidenceActivityPerTrack; // only highest-confidence activity per person
+        map<track_id, vector<ActivityWithConfidence> > m_allActivitiesPerTrack; // all activities that a track is involved in
+
+        set<activity_type> m_excludedActivityTypes, m_includedActivityTypes;
+
+        Ogre::Matrix4 m_frameTransform;
+        TrackedPersonsCache m_trackedPersonsCache;
+
+    private Q_SLOTS:
+        void personVisualTypeChanged();
+        virtual void stylesChanged();
+    };
+
+} // end namespace spencer_tracking_rviz_plugin
+
+#endif // SOCIAL_ACTIVITIES_DISPLAY_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/social_relations_display.cpp

@@ -0,0 +1,167 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rviz/visualization_manager.h>
+#include <rviz/frame_manager.h>
+#include "rviz/selection/selection_manager.h"
+
+#ifndef Q_MOC_RUN
+#include "social_relations_display.h"
+#include <boost/foreach.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+namespace spencer_tracking_rviz_plugin
+{
+
+void SocialRelationsDisplay::onInitialize()
+{
+    m_trackedPersonsCache.initialize(this, context_, update_nh_);
+    PersonDisplayCommon::onInitialize();
+    
+    m_relation_type_filter_property = new rviz::StringProperty( "Relation type filter", "", "Type of social relations to display (see \"type\" field in message). No wildcards allowed. Leave empty to allow any type of relation.", this, SLOT(stylesChanged()));
+    
+    m_positive_person_relation_threshold = new rviz::FloatProperty( "Positive relation threshold", 0.5, "Above which probability threshold a social relation between tracks is considered as positive", this, SLOT(stylesChanged()));
+
+    m_positive_person_relations_color = new rviz::ColorProperty( "Positive relation color", QColor(0,255,0), "Color for positive track relations", this, SLOT(stylesChanged()));
+    m_negative_person_relations_color = new rviz::ColorProperty( "Negative relation color", QColor(255,0,0), "Color for negative track relations", this, SLOT(stylesChanged()));
+
+    m_render_positive_person_relations_property = new rviz::BoolProperty( "Render positive relations", true,  "Render positive person relations", this, SLOT(stylesChanged()));
+    m_render_negative_person_relations_property = new rviz::BoolProperty( "Render negative relations", false, "Render negative person relations", this, SLOT(stylesChanged()));
+   
+    // Create a scene node for visualizing social relations
+    m_socialRelationsSceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+}
+
+SocialRelationsDisplay::~SocialRelationsDisplay()
+{
+}
+
+// Clear the visuals by deleting their objects.
+void SocialRelationsDisplay::reset()
+{
+    PersonDisplayCommon::reset();
+    m_trackedPersonsCache.reset();
+    m_relationVisuals.clear();
+}
+
+void SocialRelationsDisplay::processMessage(const spencer_social_relation_msgs::SocialRelations::ConstPtr& msg)
+{
+    // Get transforms into fixed frame etc.
+    if(!preprocessMessage(msg)) return;
+
+    // Loop over all social relations between tracks
+    m_relationVisuals.clear();
+    m_socialRelationsSceneNode->removeAndDestroyAllChildren();
+
+    foreach(const spencer_social_relation_msgs::SocialRelation& socialRelation, msg->elements)
+    {
+        boost::shared_ptr<CachedTrackedPerson> personTrack1 = m_trackedPersonsCache.lookup(socialRelation.track1_id);
+        boost::shared_ptr<CachedTrackedPerson> personTrack2 = m_trackedPersonsCache.lookup(socialRelation.track2_id);
+
+        // Cannot draw relations for tracks with unknown position
+        if(!personTrack1 || !personTrack2) continue;
+
+        // Create a new visual representation of the tracked person
+        boost::shared_ptr<RelationVisual> relationVisual = boost::shared_ptr<RelationVisual>(new RelationVisual);
+        relationVisual->type = socialRelation.type;
+        relationVisual->relationStrength = socialRelation.strength;
+        m_relationVisuals.push_back(relationVisual);
+
+        // Get positions. These are already in fixed frame coordinates!
+        const Ogre::Vector3 verticalShift(0,0, 1.0 + m_commonProperties->z_offset->getFloat()), textShift(0,0, 0.3);
+        const Ogre::Vector3& position1 = verticalShift + personTrack1->center;
+        const Ogre::Vector3& position2 = verticalShift + personTrack2->center;
+        const Ogre::Vector3& centerPosition = (position1 + position2) / 2.0;
+
+        // Add line connecting the two tracks
+        boost::shared_ptr<rviz::BillboardLine> relationLine(new rviz::BillboardLine(context_->getSceneManager(), m_socialRelationsSceneNode.get()));
+        relationLine->setMaxPointsPerLine(2);
+        relationLine->addPoint(position1);
+        relationLine->addPoint(position2);
+        relationVisual->relationLine = relationLine;
+
+        // Add relationship strength text
+        stringstream ss;
+        boost::shared_ptr<TextNode> relationText(new TextNode(context_->getSceneManager(), m_socialRelationsSceneNode.get()));
+        ss.str(""); ss << std::fixed << std::setprecision(0) << socialRelation.strength * 100 << "%";
+        relationText->setCaption(ss.str());
+        relationText->setPosition(centerPosition + textShift);
+        relationText->showOnTop();
+        relationVisual->relationText = relationText;
+
+        // Remember to which groups the tracks belong, to be able to hide certain groups and their track-to-track relations
+        relationVisual->trackId1 = socialRelation.track1_id;
+        relationVisual->trackId2 = socialRelation.track2_id;
+
+        // Update adjustable styles
+        updateRelationVisualStyles(relationVisual);
+    }
+}
+
+void SocialRelationsDisplay::stylesChanged()
+{
+    foreach(boost::shared_ptr<RelationVisual> relationVisual, m_relationVisuals) {
+        updateRelationVisualStyles(relationVisual);
+    }
+}
+
+void SocialRelationsDisplay::updateRelationVisualStyles(boost::shared_ptr<RelationVisual>& relationVisual)
+{
+    std::string typeFilter = m_relation_type_filter_property->getStdString();
+    bool validRelationType = relationVisual->type.find(typeFilter) != std::string::npos;
+    bool hideRelation = !validRelationType || isPersonHidden(relationVisual->trackId1) || isPersonHidden(relationVisual->trackId2);
+
+    // Determine type of the relationship
+    bool isPositiveRelation = relationVisual->relationStrength > m_positive_person_relation_threshold->getFloat();
+
+    // Get color
+    Ogre::ColourValue relationColor = isPositiveRelation ? m_positive_person_relations_color->getOgreColor() : m_negative_person_relations_color->getOgreColor();
+    relationColor.a *= m_commonProperties->alpha->getFloat(); // general alpha
+    if(hideRelation) relationColor.a = 0;
+
+    if(isPositiveRelation && !m_render_positive_person_relations_property->getBool()) relationColor.a = 0;
+    if(!isPositiveRelation && !m_render_negative_person_relations_property->getBool()) relationColor.a = 0;
+
+
+    relationVisual->relationLine->setLineWidth(0.03 * (isPositiveRelation ? 1.0 : 0.3));
+    relationVisual->relationLine->setColor(relationColor.r, relationColor.g, relationColor.b, relationColor.a);
+
+    relationVisual->relationText->setCharacterHeight(0.15 * m_commonProperties->font_scale->getFloat());
+    relationVisual->relationText->setColor(relationColor);
+}
+
+
+} // end namespace spencer_tracking_rviz_plugin
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::SocialRelationsDisplay, rviz::Display)

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/social_relations_display.h

@@ -0,0 +1,108 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef SOCIAL_RELATIONS_DISPLAY_H
+#define SOCIAL_RELATIONS_DISPLAY_H
+
+#ifndef Q_MOC_RUN
+#include <spencer_social_relation_msgs/SocialRelations.h>
+#include "person_display_common.h"
+#include "tracked_persons_cache.h"
+#endif
+
+namespace spencer_tracking_rviz_plugin
+{
+    /// The display which can be added in RViz to display social relations.
+    class SocialRelationsDisplay: public PersonDisplayCommon<spencer_social_relation_msgs::SocialRelations>
+    {
+    Q_OBJECT
+    public:
+        // Constructor.  pluginlib::ClassLoader creates instances by calling
+        // the default constructor, so make sure you have one.
+        SocialRelationsDisplay() {};
+        virtual ~SocialRelationsDisplay();
+
+        // Overrides of protected virtual functions from Display.  As much
+        // as possible, when Displays are not enabled, they should not be
+        // subscribed to incoming data and should not show anything in the
+        // 3D view.  These functions are where these connections are made
+        // and broken.
+
+        // Called after the constructors have run
+        virtual void onInitialize();
+
+    protected:
+        // A helper to clear this display back to the initial state.
+        virtual void reset();
+
+        // Must be implemented by derived classes because MOC doesn't work in templates
+        virtual rviz::DisplayContext* getContext() {
+            return context_;
+        }
+
+    private:
+        struct RelationVisual {
+            std::string type;
+            double relationStrength;
+            boost::shared_ptr<rviz::BillboardLine> relationLine;
+            boost::shared_ptr<TextNode> relationText;
+            track_id trackId1, trackId2; // required to hide certain tracks
+        };
+
+        // Functions to handle an incoming ROS message.
+        void processMessage(const spencer_social_relation_msgs::SocialRelations::ConstPtr& msg);
+        
+        // Helper functions
+        void updateRelationVisualStyles(boost::shared_ptr<RelationVisual>& relationVisual);
+        
+        // Scene node for group affiliation history visualization
+        boost::shared_ptr<Ogre::SceneNode> m_socialRelationsSceneNode;
+
+        // User-editable property variables.
+        rviz::StringProperty* m_relation_type_filter_property;
+        
+        rviz::BoolProperty* m_render_positive_person_relations_property;
+        rviz::BoolProperty* m_render_negative_person_relations_property;
+
+        rviz::FloatProperty* m_positive_person_relation_threshold;
+        rviz::ColorProperty* m_positive_person_relations_color;
+        rviz::ColorProperty* m_negative_person_relations_color;
+
+        // State variables
+        vector<boost::shared_ptr<RelationVisual> > m_relationVisuals;
+        TrackedPersonsCache m_trackedPersonsCache;
+
+    private Q_SLOTS:
+        virtual void stylesChanged();
+    };
+
+} // end namespace spencer_tracking_rviz_plugin
+
+#endif // SOCIAL_RELATIONS_DISPLAY_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/tracked_groups_display.cpp

@@ -0,0 +1,405 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include <rviz/visualization_manager.h>
+#include <rviz/frame_manager.h>
+#include "rviz/selection/selection_manager.h"
+
+#include "tracked_groups_display.h"
+#ifndef Q_MOC_RUN
+#include <boost/lexical_cast.hpp>
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+namespace spencer_tracking_rviz_plugin
+{
+
+void TrackedGroupsDisplay::onInitialize()
+{
+    m_realFixedFrame = "map";
+
+    m_trackedPersonsCache.initialize(this, context_, update_nh_);
+    PersonDisplayCommon::onInitialize();
+    
+    QObject::connect(m_commonProperties->style, SIGNAL(changed()), this, SLOT(personVisualTypeChanged()) );
+
+    m_excluded_group_ids_property = new rviz::StringProperty( "Excluded group IDs", "", "Comma-separated list of group IDs whose group visualization should be hidden", this, SLOT(stylesChanged()) );
+    m_included_group_ids_property = new rviz::StringProperty( "Included group IDs", "", "Comma-separated list of group IDs whose group visualization should be visible, overrides excluded", this, SLOT(stylesChanged()) );
+
+    m_render_intragroup_connections_property = new rviz::BoolProperty( "Connect group members", true, "Connect all members of a group by lines", this, SLOT(stylesChanged()));
+    m_render_ids_property = new rviz::BoolProperty( "Render group IDs", true, "Render group IDs as text", this, SLOT(stylesChanged()));
+    m_render_history_property = new rviz::BoolProperty( "Render history", false, "Render group affiliation history", this, SLOT(stylesChanged()));
+    
+    m_single_person_groups_in_constant_color_property  = new rviz::BoolProperty( "Single-person groups in constant color", true, "Render single-person groups in constant color", this, SLOT(stylesChanged()));
+    m_hide_ids_of_single_person_groups_property = new rviz::BoolProperty( "Hide IDs of single-person groups", false, "Hide IDs of single-person groups", m_render_ids_property, SLOT(stylesChanged()), this);
+
+    m_history_length_property = new rviz::IntProperty( "Global history size", 1000, "Global number of group affiliation history entries to display.", this, SLOT(stylesChanged()));
+    m_history_length_property->setMin( 1 );
+    m_history_length_property->setMax( 10000000 );
+
+    m_occlusion_alpha_property = new rviz::FloatProperty( "Occlusion alpha", 0.5, "Alpha multiplier for history of occluded tracks", this, SLOT(stylesChanged()) );
+    m_occlusion_alpha_property->setMin( 0.0 );
+
+    m_group_id_offset = new rviz::FloatProperty( "Group ID Z offset", 2.0, "Offset in z position (height) of the group ID text", this, SLOT(stylesChanged()) );
+
+    // Create a scene node for visualizing group affiliation history
+    m_groupAffiliationHistorySceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+    m_groupsSceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+}
+
+TrackedGroupsDisplay::~TrackedGroupsDisplay()
+{
+}
+
+// Clear the visuals by deleting their objects.
+void TrackedGroupsDisplay::reset()
+{
+    PersonDisplayCommon::reset();
+    m_trackedPersonsCache.reset();
+    m_groupVisuals.clear();
+    m_groupAffiliationHistory.clear();
+    m_groupAffiliations.clear();
+}
+
+void TrackedGroupsDisplay::update(float wall_dt, float ros_dt)
+{
+    // Move map scene node
+    Ogre::Vector3 mapFramePosition; Ogre::Quaternion mapFrameOrientation;
+    getContext()->getFrameManager()->getTransform(m_realFixedFrame, ros::Time(0), mapFramePosition, mapFrameOrientation);
+    Ogre::Matrix4 mapFrameTransform(mapFrameOrientation); mapFrameTransform.setTrans(mapFramePosition);
+    m_groupAffiliationHistorySceneNode->setPosition(mapFramePosition);
+    m_groupAffiliationHistorySceneNode->setOrientation(mapFrameOrientation);
+}
+
+bool TrackedGroupsDisplay::isGroupHidden(group_id groupId) {
+    bool isIncluded = m_includedGroupIDs.find(groupId) != m_includedGroupIDs.end();
+    if(isIncluded) return false;
+    if(!m_includedGroupIDs.empty()) return true;
+
+    return m_excludedGroupIDs.find(groupId) != m_excludedGroupIDs.end();
+}
+
+void TrackedGroupsDisplay::stylesChanged()
+{
+     // Get list of group IDs belonging to tracks that shall be hidden or visible
+     m_excludedGroupIDs.clear();
+     {
+         string groupIDString = m_excluded_group_ids_property->getStdString();
+         char_separator<char> separator(",");
+         tokenizer< char_separator<char> > tokens(groupIDString, separator);
+         foreach(const string& token, tokens) {
+             try { m_excludedGroupIDs.insert(lexical_cast<track_id>(token)); }
+             catch(bad_lexical_cast &) {}
+         }
+     }
+     m_includedGroupIDs.clear();
+     {
+        string groupIDString = m_included_group_ids_property->getStdString();
+        char_separator<char> separator(",");
+        tokenizer< char_separator<char> > tokens(groupIDString, separator);
+        foreach(const string& token, tokens) {
+            try { m_includedGroupIDs.insert(lexical_cast<track_id>(token)); }
+            catch(bad_lexical_cast &) {}
+        }
+    }
+
+    foreach(boost::shared_ptr<GroupVisual> groupVisual, m_groupVisuals) {
+        updateGroupVisualStyles(groupVisual);
+    }
+
+     // Update history size
+    m_groupAffiliationHistory.rset_capacity(m_history_length_property->getInt());
+    
+    // Update history color etc.
+    updateHistoryStyles();
+}
+
+
+// Set the rendering style (cylinders, meshes, ...) of tracked persons
+void TrackedGroupsDisplay::personVisualTypeChanged()
+{
+    foreach(boost::shared_ptr<GroupVisual> groupVisual, m_groupVisuals) {
+        foreach(boost::shared_ptr<PersonVisual>& personVisual, groupVisual->personVisuals) {
+            Ogre::SceneNode* parentSceneNode = personVisual->getParentSceneNode();
+            personVisual.reset();
+            createPersonVisualIfRequired(parentSceneNode, personVisual);
+        }
+    }
+    stylesChanged();
+}
+
+void TrackedGroupsDisplay::updateGroupVisualStyles(boost::shared_ptr<GroupVisual>& groupVisual)
+{
+    bool hideGroup = isGroupHidden(groupVisual->groupId);
+
+    // Apply current group color
+    Ogre::ColourValue groupColor = m_commonProperties->constant_color->getOgreColor();
+
+    if(groupVisual->personCount > 1 || !m_single_person_groups_in_constant_color_property->getBool()) {
+        groupColor = getColorFromId(groupVisual->groupId);
+    }   
+
+    groupColor.a *= m_commonProperties->alpha->getFloat(); // general alpha
+    if(hideGroup) groupColor.a = 0;
+
+    foreach(boost::shared_ptr<rviz::Shape> groupAssignmentCircle, groupVisual->groupAssignmentCircles) {
+        groupAssignmentCircle->setColor(groupColor.r, groupColor.g, groupColor.b, groupColor.a);
+    }
+
+    foreach(boost::shared_ptr<PersonVisual>& personVisual, groupVisual->personVisuals) {
+        if(personVisual) {
+            // Update common styles to person visual, such as line width
+            applyCommonStyles(personVisual);
+        
+            personVisual->setColor(groupColor);
+        }
+    }
+
+    double connectionLineVisibilityAlpha = m_render_intragroup_connections_property->getBool() ? 1.0 : 0.0;
+    foreach(boost::shared_ptr<rviz::BillboardLine> connectionLine, groupVisual->connectionLines) {
+        connectionLine->setColor(groupColor.r, groupColor.g, groupColor.b, groupColor.a * connectionLineVisibilityAlpha);
+        connectionLine->setLineWidth(0.05);
+    }
+
+    // Update text colors, size and visibility
+    Ogre::ColourValue fontColor = m_commonProperties->font_color_style->getOptionInt() == FONT_COLOR_CONSTANT ? m_commonProperties->constant_font_color->getOgreColor() : groupColor;
+    fontColor.a = m_commonProperties->alpha->getFloat();
+    if(hideGroup) fontColor.a = 0;
+    bool groupIdVisible = groupVisual->personCount > 1 ? true : !m_hide_ids_of_single_person_groups_property->getBool();
+    groupVisual->idText->setVisible(m_render_ids_property->getBool() && groupIdVisible);
+    groupVisual->idText->setCharacterHeight(0.23 * m_commonProperties->font_scale->getFloat());
+    groupVisual->idText->setColor(fontColor);
+    groupVisual->idText->setPosition(m_frameTransform * Ogre::Vector3(
+        groupVisual->groupCenter.x,
+        groupVisual->groupCenter.y,
+        groupVisual->groupCenter.z + m_group_id_offset->getFloat() + m_commonProperties->z_offset->getFloat()));
+}
+
+void TrackedGroupsDisplay::updateHistoryStyles()
+{
+    const Ogre::Quaternion shapeQuaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) );
+    foreach(boost::shared_ptr<GroupAffiliationHistoryEntry> groupAffiliationHistoryEntry, m_groupAffiliationHistory) {
+        bool hideGroup = isGroupHidden(groupAffiliationHistoryEntry->groupId);
+        const double historyShapeDiameter = 0.1;
+
+        Ogre::ColourValue historyColor = m_commonProperties->constant_color->getOgreColor();
+
+        if(!groupAffiliationHistoryEntry->wasSinglePersonGroup || !m_single_person_groups_in_constant_color_property->getBool()) {
+            historyColor = getColorFromId(groupAffiliationHistoryEntry->groupId);
+        }   
+
+
+        historyColor.a = m_commonProperties->alpha->getFloat();
+
+        if(groupAffiliationHistoryEntry->wasOccluded) historyColor.a *= m_occlusion_alpha_property->getFloat();
+        if(hideGroup) historyColor.a = 0;
+        if(!m_render_history_property->getBool()) historyColor.a = 0;
+
+        groupAffiliationHistoryEntry->shape->setColor(historyColor);
+        groupAffiliationHistoryEntry->shape->setScale(shapeQuaternion * Ogre::Vector3(historyShapeDiameter, historyShapeDiameter, 0.05));
+    }
+}
+
+// This is our callback to handle an incoming group message.
+void TrackedGroupsDisplay::processMessage(const spencer_tracking_msgs::TrackedGroups::ConstPtr& msg)
+{
+    // Get transforms into fixed frame etc.
+    if(!preprocessMessage(msg)) return;
+
+    // Transform from map/odometry frame into fixed frame, required to display track history if the fixed frame is not really "fixed" (e.g. base_link)
+    Ogre::Vector3 mapFramePosition; Ogre::Quaternion mapFrameOrientation;
+    getContext()->getFrameManager()->getTransform(m_realFixedFrame, msg->header.stamp, mapFramePosition, mapFrameOrientation);
+    Ogre::Matrix4 mapFrameTransform(mapFrameOrientation); mapFrameTransform.setTrans(mapFramePosition);
+
+    // Transform into Rviz fixed frame
+    m_frameTransform = Ogre::Matrix4(m_frameOrientation);
+    m_frameTransform.setTrans(m_framePosition);
+
+    const Ogre::Quaternion shapeQuaternion( Ogre::Degree(90), Ogre::Vector3(1,0,0) ); // required to fix orientation of any Cylinder shapes
+    stringstream ss;
+
+    // Clear previous visualization
+    m_groupVisuals.clear();
+    m_groupsSceneNode->removeAndDestroyAllChildren();
+
+    unsigned int numTracksWithUnknownPosition = 0;
+    m_groupAffiliations.clear();
+
+    //
+    // Iterate over all groups in this message
+    //
+    foreach (const spencer_tracking_msgs::TrackedGroup& trackedGroup, msg->groups)
+    {
+        // Create a new visual representation of the tracked group
+        boost::shared_ptr<GroupVisual> groupVisual = boost::shared_ptr<GroupVisual>(new GroupVisual);
+        groupVisual->groupId = trackedGroup.group_id;
+        groupVisual->personCount = trackedGroup.track_ids.size();
+        m_groupVisuals.push_back(groupVisual);
+
+        //
+        // Group visualization circles, person visuals (if enabled) + connections between group members
+        //
+
+        for(size_t trackIndex = 0; trackIndex < trackedGroup.track_ids.size(); trackIndex++)
+        {
+            const track_id trackId = trackedGroup.track_ids[trackIndex];
+            boost::shared_ptr<CachedTrackedPerson> trackedPerson = m_trackedPersonsCache.lookup(trackId);
+
+            // Get current track position
+            if(!trackedPerson) {
+                numTracksWithUnknownPosition++;
+            }
+            else
+            {
+                Ogre::Vector3 trackCenterAtGroundPlane(trackedPerson->center.x, trackedPerson->center.y, m_commonProperties->z_offset->getFloat());
+
+                m_groupAffiliations[trackId] = trackedGroup.group_id; // required to hide certain groups later on
+
+
+                //
+                // Group visualization circles (below tracks)
+                //
+
+                const double groupAssignmentCircleHeight = 0;
+                const double groupAssignmentCircleDiameter = 0.9;
+                boost::shared_ptr<rviz::Shape> groupAssignmentCircle = boost::shared_ptr<rviz::Shape>(new rviz::Shape(rviz::Shape::Cylinder, context_->getSceneManager(), m_groupsSceneNode.get()));
+
+                groupAssignmentCircle->setScale(shapeQuaternion * Ogre::Vector3(groupAssignmentCircleDiameter, groupAssignmentCircleDiameter, groupAssignmentCircleHeight));
+                groupAssignmentCircle->setOrientation(shapeQuaternion);
+
+                Ogre::Vector3 groupAssignmentCirclePos = trackCenterAtGroundPlane + Ogre::Vector3(0, 0, -0.5*groupAssignmentCircleHeight - 0.01);
+                groupAssignmentCircle->setPosition(groupAssignmentCirclePos);
+
+                groupVisual->groupAssignmentCircles.push_back(groupAssignmentCircle);
+
+
+                //
+                // Person visuals (colored in group color)
+                //
+
+                // This scene node is the parent of all visualization elements for the tracked person
+                boost::shared_ptr<Ogre::SceneNode> sceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+                groupVisual->personVisualSceneNodes.push_back(sceneNode);
+
+                // Create new visual for the person itself, if needed
+                boost::shared_ptr<PersonVisual> personVisual;
+                createPersonVisualIfRequired(sceneNode.get(), personVisual);
+                groupVisual->personVisuals.push_back(personVisual);
+
+                const double personHeight = personVisual ? personVisual->getHeight() : 0;
+                const Ogre::Matrix3 covXYZinTargetFrame = covarianceXYZIntoTargetFrame(trackedPerson->pose);
+                setPoseOrientation(sceneNode.get(), trackedPerson->pose, covXYZinTargetFrame, personHeight);
+
+
+                //
+                // Intra-group connections
+                //
+
+                // Iterate over all neighbouring group tracks to render intra-group connections
+                for(size_t otherTrackIndex = trackIndex + 1; otherTrackIndex < trackedGroup.track_ids.size(); otherTrackIndex++)
+                {
+                    const track_id otherTrackId = trackedGroup.track_ids[otherTrackIndex];
+                    boost::shared_ptr<CachedTrackedPerson> otherTrackedPerson = m_trackedPersonsCache.lookup(otherTrackId);
+
+                    // Get other track's position
+                    if(otherTrackedPerson) {
+                        // Get positions. These are already in fixed frame coordinates!
+                        const Ogre::Vector3 verticalShift(0,0, 0.5 + m_commonProperties->z_offset->getFloat());
+                        const Ogre::Vector3& position1 = verticalShift + trackedPerson->center;
+                        const Ogre::Vector3& position2 = verticalShift + otherTrackedPerson->center;
+
+                        // Add line connecting the two tracks
+                        boost::shared_ptr<rviz::BillboardLine> connectionLine(new rviz::BillboardLine(context_->getSceneManager(), m_groupsSceneNode.get()));
+                        connectionLine->setMaxPointsPerLine(2);
+                        connectionLine->addPoint(position1);
+                        connectionLine->addPoint(position2);
+                        groupVisual->connectionLines.push_back(connectionLine);
+                    }
+                } // end loop over neighbouring group tracks
+
+
+                //
+                // Group affiliation history
+                //
+              
+                boost::shared_ptr<GroupAffiliationHistoryEntry> newHistoryEntry(new GroupAffiliationHistoryEntry);
+                newHistoryEntry->shape = boost::shared_ptr<rviz::Shape>(new rviz::Shape(rviz::Shape::Cylinder, context_->getSceneManager(), m_groupAffiliationHistorySceneNode.get()));
+                newHistoryEntry->shape->setPosition(mapFrameTransform.inverse() * trackCenterAtGroundPlane);
+                newHistoryEntry->shape->setOrientation(shapeQuaternion);
+                newHistoryEntry->wasOccluded = trackedPerson->isOccluded;
+                newHistoryEntry->wasSinglePersonGroup = trackedGroup.track_ids.size() <= 1;
+                newHistoryEntry->groupId = trackedGroup.group_id;
+                m_groupAffiliationHistory.push_back(newHistoryEntry);
+
+
+            } // end if track found
+        } // end for loop over tracks
+
+
+        //
+        // Texts
+        //
+        const geometry_msgs::Point& groupCenter = trackedGroup.centerOfGravity.pose.position;
+        groupVisual->groupCenter = groupCenter;
+
+        // Group ID
+        boost::shared_ptr<TextNode> idText(new TextNode(context_->getSceneManager(), m_groupsSceneNode.get()));
+        ss.str(""); ss << "group " << trackedGroup.group_id;
+        idText->setCaption(ss.str());
+        idText->showOnTop();
+        groupVisual->idText = idText;
+
+        // Set adjustable styles such as color etc.
+        updateGroupVisualStyles(groupVisual);
+        updateHistoryStyles();
+    } // end for loop over all tracked groups
+
+
+    //
+    // Update status (shown in property pane)
+    //
+
+    ss.str("");
+    ss << msg->groups.size() << " group(s)";
+    setStatusStd(rviz::StatusProperty::Ok, "Groups", ss.str());
+
+    ss.str("");
+    ss << numTracksWithUnknownPosition << " track(s) with unknown position";
+    setStatusStd(0 == numTracksWithUnknownPosition ? rviz::StatusProperty::Ok : rviz::StatusProperty::Warn, "Track-to-group assignment", ss.str());
+}
+
+} // end namespace spencer_tracking_rviz_plugin
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::TrackedGroupsDisplay, rviz::Display)

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/tracked_groups_display.h

@@ -0,0 +1,146 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef TRACKED_GROUPS_DISPLAY_H
+#define TRACKED_GROUPS_DISPLAY_H
+
+#ifndef Q_MOC_RUN
+#include <map>
+#include <boost/circular_buffer.hpp>
+#endif
+#include <spencer_tracking_msgs/TrackedGroups.h>
+
+#include "person_display_common.h"
+#include "tracked_persons_cache.h"
+
+namespace spencer_tracking_rviz_plugin
+{
+    typedef unsigned int group_id;
+
+    /// A single entry in the history of a tracked person, to show group affiliation.
+    struct GroupAffiliationHistoryEntry
+    {
+        group_id groupId;
+        boost::shared_ptr<rviz::Shape> shape;
+        bool wasOccluded, wasSinglePersonGroup;
+    };
+
+    /// History of a tracked person.
+    typedef circular_buffer<boost::shared_ptr<GroupAffiliationHistoryEntry> > GroupAffiliationHistory;
+
+    /// The display which can be added in RViz to display tracked groups.
+    class TrackedGroupsDisplay: public PersonDisplayCommon<spencer_tracking_msgs::TrackedGroups>
+    {
+    Q_OBJECT
+    public:
+        // Constructor.  pluginlib::ClassLoader creates instances by calling
+        // the default constructor, so make sure you have one.
+        TrackedGroupsDisplay() {};
+        virtual ~TrackedGroupsDisplay();
+
+        // Overrides of protected virtual functions from Display.  As much
+        // as possible, when Displays are not enabled, they should not be
+        // subscribed to incoming data and should not show anything in the
+        // 3D view.  These functions are where these connections are made
+        // and broken.
+
+        // Called after the constructors have run
+        virtual void onInitialize();
+
+        // Called periodically by the visualization manager
+        virtual void update(float wall_dt, float ros_dt);
+
+    protected:
+        // A helper to clear this display back to the initial state.
+        virtual void reset();
+
+        // Must be implemented by derived classes because MOC doesn't work in templates
+        virtual rviz::DisplayContext* getContext() {
+            return context_;
+        }
+
+    private:
+        struct GroupVisual {
+            vector<boost::shared_ptr<rviz::Shape> > groupAssignmentCircles;
+            vector<boost::shared_ptr<PersonVisual> > personVisuals;
+            vector<boost::shared_ptr<Ogre::SceneNode> > personVisualSceneNodes;
+            vector<boost::shared_ptr<rviz::BillboardLine> > connectionLines;
+            boost::shared_ptr<TextNode> idText;
+            group_id groupId;
+            geometry_msgs::Point groupCenter;
+            size_t personCount;
+        };
+
+        // Functions to handle an incoming ROS message.
+        void processMessage(const spencer_tracking_msgs::TrackedGroups::ConstPtr& msg);
+       
+        // Helper functions
+        void updateGroupVisualStyles(boost::shared_ptr<GroupVisual>& groupVisual);
+        void updateHistoryStyles();
+        bool isGroupHidden(group_id groupId);
+
+        // Scene node for group affiliation history visualization
+        boost::shared_ptr<Ogre::SceneNode> m_groupAffiliationHistorySceneNode, m_groupsSceneNode;
+
+        std::string m_realFixedFrame;
+
+        // User-editable property variables.
+        rviz::StringProperty* m_excluded_group_ids_property;
+        rviz::StringProperty* m_included_group_ids_property;
+
+        rviz::BoolProperty* m_render_intragroup_connections_property;
+        rviz::BoolProperty* m_render_ids_property;
+        rviz::BoolProperty* m_render_history_property;
+        rviz::BoolProperty* m_single_person_groups_in_constant_color_property;
+        rviz::BoolProperty* m_hide_ids_of_single_person_groups_property;
+
+        rviz::IntProperty*   m_history_length_property;
+
+        rviz::FloatProperty* m_occlusion_alpha_property;        
+        rviz::FloatProperty* m_group_id_offset; // z offset of the group ID text
+
+        // State variables
+        vector<boost::shared_ptr<GroupVisual> > m_groupVisuals;
+        
+        map<track_id, group_id> m_groupAffiliations;
+        GroupAffiliationHistory m_groupAffiliationHistory;
+        set<group_id> m_excludedGroupIDs, m_includedGroupIDs;
+
+        Ogre::Matrix4 m_frameTransform;
+        TrackedPersonsCache m_trackedPersonsCache;
+
+    private Q_SLOTS:
+        void personVisualTypeChanged();
+        virtual void stylesChanged();
+    };
+
+} // end namespace spencer_tracking_rviz_plugin
+
+#endif // TRACKED_GROUPS_DISPLAY_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/tracked_persons_cache.cpp

@@ -0,0 +1,98 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "tracked_persons_cache.h"
+
+#ifndef Q_MOC_RUN
+#include <sstream>
+#include <boost/foreach.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+
+namespace spencer_tracking_rviz_plugin
+{
+
+TrackedPersonsCache::~TrackedPersonsCache()
+{
+    m_cachedTrackedPersons.clear();
+    delete m_tracked_person_subscriber;
+}
+
+void TrackedPersonsCache::initialize(rviz::Display* display, rviz::DisplayContext* context, ros::NodeHandle update_nh)
+{
+    m_display = display;
+    m_context = context;
+
+    m_tracked_person_subscriber = new rviz::AdditionalTopicSubscriber<spencer_tracking_msgs::TrackedPersons>("Tracked persons topic", display, context, update_nh,
+        boost::bind(&TrackedPersonsCache::processTrackedPersonsMessage, this, _1));
+}
+
+void TrackedPersonsCache::reset()
+{
+    m_cachedTrackedPersons.clear();
+}
+
+const boost::shared_ptr<CachedTrackedPerson> TrackedPersonsCache::lookup(track_id trackId)
+{
+    CachedTrackedPersonsMap::const_iterator entry = m_cachedTrackedPersons.find(trackId);
+    if(entry == m_cachedTrackedPersons.end()) return boost::shared_ptr<CachedTrackedPerson>();
+    else return entry->second;
+}
+
+void TrackedPersonsCache::processTrackedPersonsMessage(const spencer_tracking_msgs::TrackedPersons::ConstPtr& msg)
+{
+    // Get transform of person tracks into fixed frame
+    Ogre::Vector3 frameOrigin; Ogre::Quaternion frameOrientation;
+    m_context->getFrameManager()->getTransform(msg->header, frameOrigin, frameOrientation);
+    Ogre::Matrix4 transform(frameOrientation);
+    transform.setTrans(frameOrigin);
+
+    // Now iterate over all tracks and store their positions
+    m_cachedTrackedPersons.clear();
+    foreach(spencer_tracking_msgs::TrackedPerson trackedPerson, msg->tracks)
+    {
+        m_cachedTrackedPersons[trackedPerson.track_id] = boost::shared_ptr<CachedTrackedPerson>(new CachedTrackedPerson);
+        CachedTrackedPerson& cachedTrackedPerson = *m_cachedTrackedPersons[trackedPerson.track_id];
+
+        const geometry_msgs::Point& position = trackedPerson.pose.pose.position;
+        cachedTrackedPerson.center = transform * Ogre::Vector3(position.x, position.y, position.z);
+        cachedTrackedPerson.pose = trackedPerson.pose;
+        cachedTrackedPerson.twist = trackedPerson.twist;
+        cachedTrackedPerson.isOccluded = trackedPerson.is_occluded;
+    }
+
+    std::stringstream ss;
+    ss << msg->tracks.size() << " track(s)";
+    m_display->setStatusStd(rviz::StatusProperty::Ok, "Tracks", ss.str());
+}
+
+
+} // end namespace spencer_tracking_rviz_plugin

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/tracked_persons_cache.h

@@ -0,0 +1,93 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef TRACKED_PERSONS_CACHE_H
+#define TRACKED_PERSONS_CACHE_H
+
+#ifndef Q_MOC_RUN
+#include <map>
+#include <geometry_msgs/PoseWithCovariance.h>
+#include <geometry_msgs/TwistWithCovariance.h>
+#include <spencer_tracking_msgs/TrackedPersons.h>
+#endif
+#include "additional_topic_subscriber.h"
+
+namespace spencer_tracking_rviz_plugin
+{    
+    typedef unsigned int track_id;
+
+    /// Data structure for storing information about individual person tracks
+    struct CachedTrackedPerson
+    {
+        Ogre::Vector3 center;
+        geometry_msgs::PoseWithCovariance pose;
+        geometry_msgs::TwistWithCovariance twist;
+        bool isOccluded;
+    };
+
+    /// Subscribes to a TrackedPersons topic and caches all TrackedPersons of the current cycle, so that
+    /// the owning rviz::Display can look up track positions etc for visualization.
+    class TrackedPersonsCache {
+    public:
+        typedef std::map<track_id, boost::shared_ptr<CachedTrackedPerson> > CachedTrackedPersonsMap;
+
+        // Destructor
+        ~TrackedPersonsCache();
+
+        /// Create TrackedPersons subscriber and setup RViz properties.
+        void initialize(rviz::Display* display, rviz::DisplayContext* context, ros::NodeHandle update_nh);
+
+        /// Clear internal state, including all cached track positions.
+        void reset();
+
+        /// Lookup information for the given tracked person ID. Returns a null pointer if no information is available.
+        const boost::shared_ptr<CachedTrackedPerson> lookup(track_id trackId);
+
+        /// Return internal map
+        const CachedTrackedPersonsMap& getMap() {
+            return m_cachedTrackedPersons;
+        }
+
+    private:
+        // Callback when a new TrackedPersons message has arrived
+        void processTrackedPersonsMessage(const spencer_tracking_msgs::TrackedPersons::ConstPtr& msg);
+
+        rviz::AdditionalTopicSubscriber<spencer_tracking_msgs::TrackedPersons>* m_tracked_person_subscriber;
+        rviz::Display* m_display;
+        rviz::DisplayContext* m_context;
+
+        // Our TrackedPerson memory
+        CachedTrackedPersonsMap m_cachedTrackedPersons;
+    };
+    
+
+}
+
+#endif // TRACKED_PERSONS_CACHE_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/tracked_persons_display.cpp

@@ -0,0 +1,608 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *  * Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef Q_MOC_RUN
+#include <rviz/visualization_manager.h>
+#include <rviz/frame_manager.h>
+#include "rviz/selection/selection_manager.h"
+#include "tracked_persons_display.h"
+#include <boost/lexical_cast.hpp>
+#include <boost/tokenizer.hpp>
+#include <boost/foreach.hpp>
+#endif
+#define foreach BOOST_FOREACH
+
+namespace spencer_tracking_rviz_plugin
+{
+// The constructor must have no arguments, so we can't give the
+// constructor the parameters it needs to fully initialize.
+void TrackedPersonsDisplay::onInitialize()
+{
+  PersonDisplayCommon::onInitialize();
+
+  m_realFixedFrame = "odom";
+  QObject::connect(m_commonProperties->style, SIGNAL(changed()), this, SLOT(personVisualTypeChanged()));
+
+  m_occlusion_alpha_property = new rviz::FloatProperty("Occlusion alpha", 0.3, "Alpha multiplier for occluded tracks",
+                                                       this, SLOT(stylesChanged()));
+  m_occlusion_alpha_property->setMin(0.0);
+
+  m_missed_alpha_property =
+      new rviz::FloatProperty("Missed alpha", 0.5, "Alpha multiplier for missed tracks", this, SLOT(stylesChanged()));
+  m_missed_alpha_property->setMin(0.0);
+
+  m_history_length_property = new rviz::IntProperty("History size", 100, "Number of prior track positions to display.",
+                                                    this, SLOT(stylesChanged()));
+  m_history_length_property->setMin(1);
+  m_history_length_property->setMax(10000000);
+
+  m_delete_after_ncycles_property = new rviz::IntProperty("Delete after no. cycles", 100,
+                                                          "After how many time steps to delete an old track that has "
+                                                          "not been seen again, including its history",
+                                                          this, SLOT(stylesChanged()));
+  m_delete_after_ncycles_property->setMin(0);
+  m_delete_after_ncycles_property->setMax(10000000);
+
+  m_show_deleted_property = new rviz::BoolProperty(
+      "Show DELETED tracks", false, "Show tracks which have been marked as deleted", this, SLOT(stylesChanged()));
+  m_show_occluded_property = new rviz::BoolProperty("Show OCCLUDED tracks", true,
+                                                    "Show tracks which could not be matched to an detection due to "
+                                                    "sensor occlusion",
+                                                    this, SLOT(stylesChanged()));
+  m_show_missed_property = new rviz::BoolProperty("Show MISSED tracks", true,
+                                                  "Show tracks which could not be matched to an detection but should "
+                                                  "be observable by the sensor",
+                                                  this, SLOT(stylesChanged()));
+  m_show_matched_property = new rviz::BoolProperty(
+      "Show MATCHED tracks", true, "Show tracks which could be matched to an detection", this, SLOT(stylesChanged()));
+
+  m_render_history_property =
+      new rviz::BoolProperty("Render history", true, "Render prior track positions", this, SLOT(stylesChanged()));
+  m_render_history_as_line_property = new rviz::BoolProperty(
+      "History as line", true, "Display history as line instead of dots", this, SLOT(stylesChanged()));
+  m_render_person_property =
+      new rviz::BoolProperty("Render person visual", true, "Render person visualization", this, SLOT(stylesChanged()));
+  m_render_covariances_property = new rviz::BoolProperty("Render covariances", true, "Render track covariance ellipses",
+                                                         this, SLOT(stylesChanged()));
+  m_render_velocities_property =
+      new rviz::BoolProperty("Render velocities", true, "Render track velocity arrows", this, SLOT(stylesChanged()));
+  m_render_ids_property =
+      new rviz::BoolProperty("Render track IDs", true, "Render track IDs as text", this, SLOT(stylesChanged()));
+  m_render_detection_ids_property = new rviz::BoolProperty("Render detection IDs", true,
+                                                           "Render IDs of the detection that a track was matched "
+                                                           "against, if any",
+                                                           this, SLOT(stylesChanged()));
+  m_render_track_state_property =
+      new rviz::BoolProperty("Render track state", true, "Render track state text", this, SLOT(stylesChanged()));
+
+  m_history_min_point_distance_property = new rviz::FloatProperty("Min. history point distance", 0.4,
+                                                                  "Minimum distance between history points before a "
+                                                                  "new one is placed",
+                                                                  this, SLOT(stylesChanged()));
+  m_history_line_width_property = new rviz::FloatProperty(
+      "Line width", 0.05, "Line width of history", m_render_history_as_line_property, SLOT(stylesChanged()), this);
+  m_covariance_line_width_property =
+      new rviz::FloatProperty("Line width", 0.1, "Line width of covariance ellipses", m_render_covariances_property,
+                              SLOT(stylesChanged()), this);
+
+  // TODO: Implement functionality
+  // m_render_state_prediction_property  = new rviz::BoolProperty( "Render state prediction", true, "Render state
+  // prediction from Kalman filter", this, SLOT( updateRenderFlags() ));
+
+  // Create a scene node for visualizing track history
+  m_trackHistorySceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+}
+
+TrackedPersonsDisplay::~TrackedPersonsDisplay()
+{
+  m_cachedTracks.clear();
+}
+
+// Clear the visuals by deleting their objects.
+void TrackedPersonsDisplay::reset()
+{
+  PersonDisplayCommon::reset();
+  m_cachedTracks.clear();
+}
+
+void TrackedPersonsDisplay::update(float wall_dt, float ros_dt)
+{
+  // Move map scene node
+  Ogre::Vector3 mapFramePosition;
+  Ogre::Quaternion mapFrameOrientation;
+  getContext()->getFrameManager()->getTransform(m_realFixedFrame, ros::Time(0), mapFramePosition, mapFrameOrientation);
+  Ogre::Matrix4 mapFrameTransform(mapFrameOrientation);
+  mapFrameTransform.setTrans(mapFramePosition);
+  m_trackHistorySceneNode->setPosition(mapFramePosition);
+  m_trackHistorySceneNode->setOrientation(mapFrameOrientation);
+
+  // Update position of deleted tracks (because they are not being updated by ROS messages any more)
+  foreach (const track_map::value_type& entry, m_cachedTracks)
+  {
+    const boost::shared_ptr<TrackedPersonVisual>& trackedPersonVisual = entry.second;
+    if (trackedPersonVisual->isDeleted)
+    {
+      Ogre::Matrix4 poseInCurrentFrame = mapFrameTransform * trackedPersonVisual->lastObservedPose;
+      Ogre::Vector3 position = poseInCurrentFrame.getTrans();
+      Ogre::Quaternion orientation = poseInCurrentFrame.extractQuaternion();
+      if (!position.isNaN() && !orientation.isNaN())
+      {
+        trackedPersonVisual->sceneNode->setPosition(position);
+        trackedPersonVisual->sceneNode->setOrientation(orientation);
+      }
+    }
+    else
+    {
+      // Update animation etc.
+      if (trackedPersonVisual->personVisual)
+        trackedPersonVisual->personVisual->update(ros_dt);
+    }
+  }
+}
+
+/// Update all dynamically adjusted visualization properties (colors, font sizes etc.) of all currently tracked persons
+void TrackedPersonsDisplay::stylesChanged()
+{
+  const Ogre::Quaternion shapeQuaternion(Ogre::Degree(90), Ogre::Vector3(1, 0, 0));
+
+  // Update each track
+  foreach (const track_map::value_type& entry, m_cachedTracks)
+  {
+    const track_id trackId = entry.first;
+    const boost::shared_ptr<TrackedPersonVisual>& trackedPersonVisual = entry.second;
+
+    // Update common styles to person visual, such as line width
+    applyCommonStyles(trackedPersonVisual->personVisual);
+
+    // Update track visibility
+    bool trackVisible = !isPersonHidden(trackId);
+
+    if (trackedPersonVisual->isDeleted)
+      trackVisible &= m_show_deleted_property->getBool();
+    else if (trackedPersonVisual->isOccluded)
+      trackVisible &= m_show_occluded_property->getBool();
+    else if (trackedPersonVisual->isMissed)
+      trackVisible &= m_show_missed_property->getBool();
+    else
+      trackVisible &= m_show_matched_property->getBool();
+
+    trackedPersonVisual->sceneNode->setVisible(trackVisible);
+    trackedPersonVisual->historySceneNode->setVisible(trackVisible && !m_render_history_as_line_property->getBool());
+    trackedPersonVisual->historyLineSceneNode->setVisible(trackVisible && m_render_history_as_line_property->getBool());
+
+    // Get current track color
+    Ogre::ColourValue trackColorWithFullAlpha = getColorFromId(trackId);
+    Ogre::ColourValue trackColor = getColorFromId(trackId);
+    trackColor.a *= m_commonProperties->alpha->getFloat();  // general alpha
+    if (trackedPersonVisual->isOccluded)
+      trackColor.a *= m_occlusion_alpha_property->getFloat();  // occlusion alpha
+    if (trackedPersonVisual->isMissed)
+      trackColor.a *= m_missed_alpha_property->getFloat();  // occlusion alpha
+
+    // Update person color
+    Ogre::ColourValue personColor = trackColor;
+    if (!m_render_person_property->getBool())
+      personColor.a = 0.0;
+
+    if (trackedPersonVisual->personVisual)
+    {
+      trackedPersonVisual->personVisual->setColor(personColor);
+    }
+
+    // Update history size
+    trackedPersonVisual->history.rset_capacity(m_history_length_property->getInt());
+
+    // Update history color
+    foreach (boost::shared_ptr<TrackedPersonHistoryEntry> historyEntry, trackedPersonVisual->history)
+    {
+      const double historyShapeDiameter = 0.1;
+      Ogre::ColourValue historyColor = trackColorWithFullAlpha;
+      historyColor.a *= m_commonProperties->alpha->getFloat();  // general alpha
+      if (historyEntry->wasOccluded)
+        historyColor.a *= m_occlusion_alpha_property->getFloat();
+      if (isPersonHidden(trackId) || m_render_history_as_line_property->getBool())
+        historyColor.a = 0;
+
+      if (historyEntry->shape)
+      {
+        historyEntry->shape->setColor(historyColor);
+        historyEntry->shape->setScale(shapeQuaternion *
+                                      Ogre::Vector3(historyShapeDiameter, historyShapeDiameter, 0.05));
+      }
+    }
+
+    if (trackedPersonVisual->historyLine)
+    {  // history-as-line mode (as opposed to history-as-dots)
+      Ogre::ColourValue historyColor = trackColorWithFullAlpha;
+      historyColor.a *= m_commonProperties->alpha->getFloat();  // general alpha
+      if (isPersonHidden(trackId))
+        historyColor.a = 0;
+      trackedPersonVisual->historyLine->setColor(historyColor.r, historyColor.g, historyColor.b, historyColor.a);
+    }
+
+    // Update text colors, font size and visibility
+    const double personHeight = trackedPersonVisual->personVisual ? trackedPersonVisual->personVisual->getHeight() : 0;
+    Ogre::ColourValue fontColor = m_commonProperties->font_color_style->getOptionInt() == FONT_COLOR_CONSTANT ?
+                                      m_commonProperties->constant_font_color->getOgreColor() :
+                                      trackColor;
+    fontColor.a = m_commonProperties->alpha->getFloat();
+
+    trackedPersonVisual->detectionIdText->setCharacterHeight(0.18 * m_commonProperties->font_scale->getFloat());
+    trackedPersonVisual->detectionIdText->setVisible(!trackedPersonVisual->isOccluded &&
+                                                     m_render_detection_ids_property->getBool() && trackVisible);
+    trackedPersonVisual->detectionIdText->setColor(fontColor);
+    trackedPersonVisual->detectionIdText->setPosition(
+        Ogre::Vector3(0, 0, -trackedPersonVisual->detectionIdText->getCharacterHeight()));
+
+    trackedPersonVisual->stateText->setCharacterHeight(0.18 * m_commonProperties->font_scale->getFloat());
+    trackedPersonVisual->stateText->setVisible(m_render_track_state_property->getBool() && trackVisible);
+    trackedPersonVisual->stateText->setColor(fontColor);
+    trackedPersonVisual->stateText->setPosition(
+        Ogre::Vector3(0, 0, personHeight + trackedPersonVisual->stateText->getCharacterHeight()));
+
+    const double stateTextOffset =
+        m_render_track_state_property->getBool() ? 1.2 * trackedPersonVisual->stateText->getCharacterHeight() : 0;
+    trackedPersonVisual->idText->setCharacterHeight(0.25 * m_commonProperties->font_scale->getFloat());
+    trackedPersonVisual->idText->setVisible(m_render_ids_property->getBool() && trackVisible);
+    trackedPersonVisual->idText->setColor(fontColor);
+    trackedPersonVisual->idText->setPosition(
+        Ogre::Vector3(0, 0, personHeight + trackedPersonVisual->idText->getCharacterHeight() + stateTextOffset));
+
+    // Update velocity arrow color
+    double arrowAlpha = m_render_velocities_property->getBool() ? trackColor.a : 0.0;
+    if (trackedPersonVisual->hasZeroVelocity)
+      arrowAlpha = 0.0;
+    trackedPersonVisual->velocityArrow->setColor(
+        Ogre::ColourValue(trackColor.r, trackColor.g, trackColor.b, arrowAlpha));
+
+    // Set color of covariance visualization
+    Ogre::ColourValue covarianceColor = trackColor;
+    if (!m_render_covariances_property->getBool())
+      covarianceColor.a = 0.0;
+    trackedPersonVisual->covarianceVisual->setColor(covarianceColor);
+    trackedPersonVisual->covarianceVisual->setLineWidth(m_covariance_line_width_property->getFloat());
+  }
+
+  // Update global history visibility
+  m_trackHistorySceneNode->setVisible(m_render_history_property->getBool());
+}
+
+// Set the rendering style (cylinders, meshes, ...) of tracked persons
+void TrackedPersonsDisplay::personVisualTypeChanged()
+{
+  foreach (const track_map::value_type& entry, m_cachedTracks)
+  {
+    const boost::shared_ptr<TrackedPersonVisual>& trackedPersonVisual = entry.second;
+    trackedPersonVisual->personVisual.reset();
+    createPersonVisualIfRequired(trackedPersonVisual->sceneNode.get(), trackedPersonVisual->personVisual);
+  }
+  stylesChanged();
+}
+
+// This is our callback to handle an incoming message.
+void TrackedPersonsDisplay::processMessage(const spencer_tracking_msgs::TrackedPersons::ConstPtr& msg)
+{
+  // Get transforms into fixed frame etc.
+  if (!preprocessMessage(msg))
+    return;
+
+  // Transform from map/odometry frame into fixed frame, required to display track history if the fixed frame is not
+  // really "fixed" (e.g. base_link)
+  Ogre::Vector3 mapFramePosition;
+  Ogre::Quaternion mapFrameOrientation;
+  getContext()->getFrameManager()->getTransform(m_realFixedFrame, msg->header.stamp, mapFramePosition,
+                                                mapFrameOrientation);
+  Ogre::Matrix4 mapFrameTransform(mapFrameOrientation);
+  mapFrameTransform.setTrans(mapFramePosition);
+
+  // Transform required to fix orientation of any Cylinder shapes
+  const Ogre::Quaternion shapeQuaternion(Ogre::Degree(90), Ogre::Vector3(1, 0, 0));
+  stringstream ss;
+
+  //
+  // Iterate over all tracks in this message, see if we have a cached visual (then update it) or create a new one.
+  //
+  set<unsigned int> encounteredTrackIds;
+  for (vector<spencer_tracking_msgs::TrackedPerson>::const_iterator trackedPersonIt = msg->tracks.begin();
+       trackedPersonIt != msg->tracks.end(); ++trackedPersonIt)
+  {
+    boost::shared_ptr<TrackedPersonVisual> trackedPersonVisual;
+
+    // See if we encountered this track ID before in this loop (means duplicate track ID)
+    if (encounteredTrackIds.find(trackedPersonIt->track_id) != encounteredTrackIds.end())
+    {
+      ROS_ERROR_STREAM("spencer_tracking_msgs::TrackedPersons contains duplicate track ID "
+                       << trackedPersonIt->track_id << "! Skipping duplicate track.");
+      continue;
+    }
+    else
+    {
+      encounteredTrackIds.insert(trackedPersonIt->track_id);
+    }
+
+    // See if we have cached a track with this ID
+    if (m_cachedTracks.find(trackedPersonIt->track_id) != m_cachedTracks.end())
+    {
+      trackedPersonVisual = m_cachedTracks[trackedPersonIt->track_id];
+    }
+    else
+    {
+      // Create a new visual representation of the tracked person
+      trackedPersonVisual = boost::shared_ptr<TrackedPersonVisual>(new TrackedPersonVisual);
+      m_cachedTracks[trackedPersonIt->track_id] = trackedPersonVisual;
+
+      // This scene node is the parent of all visualization elements for the tracked person
+      trackedPersonVisual->sceneNode = boost::shared_ptr<Ogre::SceneNode>(scene_node_->createChildSceneNode());
+      trackedPersonVisual->historySceneNode =
+          boost::shared_ptr<Ogre::SceneNode>(m_trackHistorySceneNode->createChildSceneNode());
+      trackedPersonVisual->historyLineSceneNode =
+          boost::shared_ptr<Ogre::SceneNode>(m_trackHistorySceneNode->createChildSceneNode());
+    }
+
+    // These values need to be remembered for later use in stylesChanged()
+    if (trackedPersonIt->is_occluded && !trackedPersonIt->is_matched)
+    {
+      trackedPersonVisual->isOccluded = true;
+      trackedPersonVisual->isMissed = false;
+    }
+    else if (!trackedPersonIt->is_occluded && !trackedPersonIt->is_matched)
+    {
+      trackedPersonVisual->isOccluded = false;
+      trackedPersonVisual->isMissed = true;
+    }
+    else
+    {
+      trackedPersonVisual->isOccluded = false;
+      trackedPersonVisual->isMissed = false;
+    }
+
+    trackedPersonVisual->isDeleted = false;
+    trackedPersonVisual->numCyclesNotSeen = 0;
+
+    Ogre::SceneNode* currentSceneNode = trackedPersonVisual->sceneNode.get();
+
+    //
+    // Person visualization
+    //
+
+    // Create new visual for the person itself, if needed
+    boost::shared_ptr<PersonVisual>& personVisual = trackedPersonVisual->personVisual;
+    createPersonVisualIfRequired(currentSceneNode, personVisual);
+
+    const double personHeight = personVisual ? personVisual->getHeight() : 0;
+    const double halfPersonHeight = personHeight / 2.0;
+
+    //
+    // Position of entire track
+    //
+
+    const Ogre::Matrix3 covXYZinTargetFrame = covarianceXYZIntoTargetFrame(trackedPersonIt->pose);
+    setPoseOrientation(currentSceneNode, trackedPersonIt->pose, covXYZinTargetFrame, personHeight);
+
+    //
+    // Track history
+    //
+
+    Ogre::Vector3 newHistoryEntryPosition = mapFrameTransform.inverse() * currentSceneNode->getPosition();
+
+    const float MIN_HISTORY_ENTRY_DISTANCE = m_history_min_point_distance_property->getFloat();  // in meters
+    if ((trackedPersonVisual->positionOfLastHistoryEntry - newHistoryEntryPosition).length() >
+        MIN_HISTORY_ENTRY_DISTANCE)
+    {
+      // General history
+      boost::shared_ptr<TrackedPersonHistoryEntry> newHistoryEntry(new TrackedPersonHistoryEntry);
+      newHistoryEntry->trackId = trackedPersonIt->track_id;
+      newHistoryEntry->position = newHistoryEntryPosition;  // used by history lines (below) even if no shape is set
+      newHistoryEntry->wasOccluded = trackedPersonIt->is_occluded;
+      trackedPersonVisual->history.push_back(newHistoryEntry);
+
+      // Always need to reset history line since history is like a queue, oldest element has to be removed but
+      // BillboardLine doesn't offer that functionality
+      trackedPersonVisual->historyLine.reset(
+          new rviz::BillboardLine(context_->getSceneManager(), trackedPersonVisual->historyLineSceneNode.get()));
+
+      if (m_render_history_as_line_property->getBool())
+      {
+        // History lines
+        if (trackedPersonVisual->history.size() >= 2)
+        {
+          trackedPersonVisual->historyLine->setLineWidth(m_history_line_width_property->getFloat());
+          trackedPersonVisual->historyLine->setMaxPointsPerLine(trackedPersonVisual->history.size());
+
+          foreach (const boost::shared_ptr<TrackedPersonHistoryEntry>& historyEntry, trackedPersonVisual->history)
+          {
+            historyEntry->shape.reset();  // remove existing dot shapes, if any, for better performance
+            trackedPersonVisual->historyLine->addPoint(historyEntry->position);
+          }
+        }
+      }
+      else
+      {
+        // History dots
+        newHistoryEntry->shape = boost::shared_ptr<rviz::Shape>(new rviz::Shape(
+            rviz::Shape::Cylinder, context_->getSceneManager(), trackedPersonVisual->historySceneNode.get()));
+        newHistoryEntry->shape->setPosition(newHistoryEntryPosition);
+        newHistoryEntry->shape->setOrientation(shapeQuaternion);
+      }
+
+      trackedPersonVisual->positionOfLastHistoryEntry = newHistoryEntryPosition;
+    }
+
+    //
+    // Texts
+    //
+    {
+      if (!trackedPersonVisual->idText)
+      {
+        trackedPersonVisual->idText.reset(new TextNode(context_->getSceneManager(), currentSceneNode));
+        trackedPersonVisual->stateText.reset(new TextNode(context_->getSceneManager(), currentSceneNode));
+        trackedPersonVisual->detectionIdText.reset(new TextNode(context_->getSceneManager(), currentSceneNode));
+      }
+
+      // Detection ID
+      ss.str("");
+      ss << "det " << trackedPersonIt->detection_id;
+      trackedPersonVisual->detectionIdText->setCaption(ss.str());
+
+      // Track state
+      ss.str("");
+
+      if (trackedPersonIt->is_occluded && !trackedPersonIt->is_matched)
+        ss << "OCCLUDED";
+      else if (!trackedPersonIt->is_occluded && !trackedPersonIt->is_matched)
+        ss << "MISSED";
+      else
+        ss << "MATCHED";
+
+      trackedPersonVisual->stateText->setCaption(ss.str());
+
+      // Track ID
+      ss.str("");
+      ss << "Human " << trackedPersonIt->track_id;
+      trackedPersonVisual->idText->setCaption(ss.str());
+    }
+
+    //
+    // Velocity arrows
+    //
+    if (!trackedPersonVisual->velocityArrow)
+    {
+      trackedPersonVisual->velocityArrow.reset(new rviz::Arrow(context_->getSceneManager(), currentSceneNode));
+    }
+
+    // Update velocity arrow
+    {
+      const Ogre::Vector3 velocityVector = getVelocityVector(trackedPersonIt->twist);
+
+      if (velocityVector.isZeroLength() || velocityVector.length() > 100 || velocityVector.isNaN())
+      {
+        if (!velocityVector.isZeroLength())
+        {  // do not show warning for zero velocity
+          ROS_WARN("Track %lu has suspicious velocity (%.1f m/s), not showing velocity vector!",
+                   trackedPersonIt->track_id, velocityVector.length());
+        }
+      }
+      else
+      {
+        const double personRadius = 0.2;
+        const Ogre::Vector3 velocityArrowAttachPoint(personRadius, 0,
+                                                     halfPersonHeight);  // relative to tracked person's scene node
+        trackedPersonVisual->velocityArrow->setPosition(velocityArrowAttachPoint);
+        trackedPersonVisual->velocityArrow->setOrientation(
+            m_frameOrientation * currentSceneNode->getOrientation().Inverse() *
+            Ogre::Vector3::NEGATIVE_UNIT_Z.getRotationTo(velocityVector));
+
+        const double shaftLength = velocityVector.length(), shaftDiameter = 0.05, headLength = 0.2, headDiameter = 0.2;
+        trackedPersonVisual->velocityArrow->set(shaftLength, shaftDiameter, headLength, headDiameter);
+        trackedPersonVisual->hasZeroVelocity = velocityVector.length() < 0.05;
+      }
+
+      boost::shared_ptr<MeshPersonVisual> meshPersonVisual =
+          boost::dynamic_pointer_cast<MeshPersonVisual>(personVisual);
+      if (meshPersonVisual)
+      {
+        meshPersonVisual->setWalkingSpeed(velocityVector.length());
+      }
+    }
+
+    //
+    // Covariance visualization
+    //
+    if (!trackedPersonVisual->covarianceVisual)
+    {
+      trackedPersonVisual->covarianceVisual.reset(
+          new ProbabilityEllipseCovarianceVisual(context_->getSceneManager(), currentSceneNode));
+    }
+
+    // Update covariance ellipse
+    {
+      Ogre::Vector3 covarianceMean(0, 0, 0);  // zero mean because parent node is already centered at pose mean
+      trackedPersonVisual->covarianceVisual->setOrientation(currentSceneNode->getOrientation().Inverse());
+      trackedPersonVisual->covarianceVisual->setMeanCovariance(covarianceMean, covXYZinTargetFrame);
+    }
+
+  }  // end for loop over all tracked persons
+
+  // Set all properties which can be dynamically in the GUI. This iterates over all tracks.
+  stylesChanged();
+
+  //
+  // First hide, then delete old cached tracks which have not been seen for a while
+  //
+  set<unsigned int> trackIdsToDelete;
+  for (map<unsigned int, boost::shared_ptr<TrackedPersonVisual> >::const_iterator cachedTrackIt =
+           m_cachedTracks.begin();
+       cachedTrackIt != m_cachedTracks.end(); ++cachedTrackIt)
+  {
+    if (encounteredTrackIds.end() == encounteredTrackIds.find(cachedTrackIt->first))
+    {
+      const boost::shared_ptr<TrackedPersonVisual>& trackedPersonVisual = cachedTrackIt->second;
+
+      // Update state and visibility
+      if (!trackedPersonVisual->isDeleted)
+      {
+        trackedPersonVisual->stateText->setCaption("DELETED");
+        trackedPersonVisual->isDeleted = true;
+
+        Ogre::Matrix4 lastObservedPose(trackedPersonVisual->sceneNode->getOrientation());
+        lastObservedPose.setTrans(trackedPersonVisual->sceneNode->getPosition());
+        trackedPersonVisual->lastObservedPose = mapFrameTransform.inverse() * lastObservedPose;
+      }
+
+      if (!m_show_deleted_property->getBool())
+        trackedPersonVisual->sceneNode->setVisible(false);
+
+      // Delete if too old
+      if (++trackedPersonVisual->numCyclesNotSeen > m_delete_after_ncycles_property->getInt())
+      {
+        trackIdsToDelete.insert(cachedTrackIt->first);
+      }
+    }
+  }
+
+  for (set<unsigned int>::const_iterator setIt = trackIdsToDelete.begin(); setIt != trackIdsToDelete.end(); ++setIt)
+  {
+    m_cachedTracks.erase(*setIt);
+  }
+
+  //
+  // Update status (shown in property pane)
+  //
+  ss.str("");
+  ss << msg->tracks.size() << " tracks received";
+  setStatusStd(rviz::StatusProperty::Ok, "Tracks", ss.str());
+}
+
+}  // end namespace spencer_tracking_rviz_plugin
+
+// Tell pluginlib about this class.  It is important to do this in
+// global scope, outside our package's namespace.
+#include <pluginlib/class_list_macros.h>
+PLUGINLIB_EXPORT_CLASS(spencer_tracking_rviz_plugin::TrackedPersonsDisplay, rviz::Display)

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/tracked_persons_display.h

@@ -0,0 +1,155 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef TRACKED_PERSONS_DISPLAY_H
+#define TRACKED_PERSONS_DISPLAY_H
+
+#ifndef Q_MOC_RUN
+#include <map>
+#include <boost/circular_buffer.hpp>
+#include <spencer_tracking_msgs/TrackedPersons.h>
+#include "person_display_common.h"
+#endif
+
+namespace spencer_tracking_rviz_plugin
+{
+    typedef unsigned int track_id;
+
+    /// A single entry in the history of a tracked person.
+    struct TrackedPersonHistoryEntry
+    {
+        Ogre::Vector3 position;
+        boost::shared_ptr<rviz::Shape> shape;
+        bool wasOccluded;
+        track_id trackId;
+    };
+
+    /// History of a tracked person.
+    typedef circular_buffer<boost::shared_ptr<TrackedPersonHistoryEntry> > TrackedPersonHistory;
+
+    /// The visual of a tracked person.
+    struct TrackedPersonVisual
+    {
+        TrackedPersonHistory history;
+        boost::shared_ptr<rviz::BillboardLine> historyLine;
+        Ogre::Vector3 positionOfLastHistoryEntry;
+
+        boost::shared_ptr<Ogre::SceneNode> sceneNode, historySceneNode, historyLineSceneNode;
+
+        boost::shared_ptr<PersonVisual> personVisual;
+        boost::shared_ptr<TextNode> idText, detectionIdText, stateText;
+        boost::shared_ptr<rviz::Arrow> velocityArrow;
+        boost::shared_ptr<CovarianceVisual> covarianceVisual;
+
+        Ogre::Matrix4 lastObservedPose;
+
+        bool isOccluded, isDeleted, isMissed, hasZeroVelocity;
+        int numCyclesNotSeen;
+    };
+
+    // The TrackedPersonsDisplay class itself just implements a circular buffer,
+    // editable parameters, and Display subclass machinery.
+    class TrackedPersonsDisplay: public PersonDisplayCommon<spencer_tracking_msgs::TrackedPersons>
+    {
+    Q_OBJECT
+    public:
+        // Constructor.  pluginlib::ClassLoader creates instances by calling
+        // the default constructor, so make sure you have one.
+        TrackedPersonsDisplay() {};
+        virtual ~TrackedPersonsDisplay();
+
+        // Overrides of protected virtual functions from Display.  As much
+        // as possible, when Displays are not enabled, they should not be
+        // subscribed to incoming data and should not show anything in the
+        // 3D view.  These functions are where these connections are made
+        // and broken.
+
+        // Called after the constructors have run
+        virtual void onInitialize();
+
+        // Called periodically by the visualization manager
+        virtual void update(float wall_dt, float ros_dt);
+
+    protected:
+        // A helper to clear this display back to the initial state.
+        virtual void reset();
+
+        // Must be implemented by derived classes because MOC doesn't work in templates
+        virtual rviz::DisplayContext* getContext() {
+            return context_;
+        }
+
+    private Q_SLOTS:
+        void personVisualTypeChanged();
+
+        // Called whenever one of the properties in PersonDisplayCommonProperties has been changed
+        virtual void stylesChanged();
+
+    private:
+        // Function to handle an incoming ROS message.
+        void processMessage(const spencer_tracking_msgs::TrackedPersons::ConstPtr& msg);
+       
+        // All currently active tracks, with unique track ID as map key
+        typedef map<track_id, boost::shared_ptr<TrackedPersonVisual> > track_map;
+        track_map m_cachedTracks;
+
+        // Scene node for track history visualization
+        boost::shared_ptr<Ogre::SceneNode> m_trackHistorySceneNode;
+        std::string m_realFixedFrame;
+
+        // User-editable property variables.
+        rviz::FloatProperty* m_occlusion_alpha_property;
+        rviz::FloatProperty* m_missed_alpha_property;
+        rviz::IntProperty*   m_history_length_property;
+        rviz::IntProperty*   m_delete_after_ncycles_property;
+
+        rviz::BoolProperty* m_show_deleted_property;
+        rviz::BoolProperty* m_show_occluded_property;
+        rviz::BoolProperty* m_show_missed_property;
+        rviz::BoolProperty* m_show_matched_property;
+    
+        rviz::BoolProperty* m_render_person_property;
+        rviz::BoolProperty* m_render_history_property;
+        rviz::BoolProperty* m_render_history_as_line_property;
+        rviz::BoolProperty* m_render_covariances_property;
+        rviz::BoolProperty* m_render_state_prediction_property;
+        rviz::BoolProperty* m_render_velocities_property;
+        rviz::BoolProperty* m_render_ids_property;
+        rviz::BoolProperty* m_render_detection_ids_property;
+        rviz::BoolProperty* m_render_track_state_property;
+
+        rviz::FloatProperty* m_history_line_width_property;
+        rviz::FloatProperty* m_history_min_point_distance_property;
+        rviz::FloatProperty* m_covariance_line_width_property;
+    };
+
+} // end namespace spencer_tracking_rviz_plugin
+
+#endif // TRACKED_PERSONS_DISPLAY_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/visuals/covariance_visual.h

@@ -0,0 +1,236 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+ *  Copyright (c) 2006, Kai O. Arras, Autonomous Intelligent Systems Lab, University of Freiburg
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions are met:
+ *
+ *  * Redistributions of source code must retain the above copyright notice, this
+ *    list of conditions and the following disclaimer.
+ *  * Redistributions in binary form must reproduce the above copyright notice,
+ *    this list of conditions and the following disclaimer in the documentation
+ *    and/or other materials provided with the distribution.
+ *  * Neither the name of the copyright holder nor the names of its contributors
+ *    may be used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ *  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ *  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+ *  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ *  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ *  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ *  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ *  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef COVARIANCE_VISUAL_H
+#define COVARIANCE_VISUAL_H
+
+#include <rviz/ogre_helpers/shape.h>
+#include <rviz/ogre_helpers/billboard_line.h>
+#include <cmath>
+
+namespace spencer_tracking_rviz_plugin
+{
+// Visualization of a covariance matrix
+class CovarianceVisual
+{
+public:
+  CovarianceVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode) : m_sceneManager(sceneManager)
+  {
+    m_sceneNode = parentNode->createChildSceneNode();
+  }
+
+  virtual ~CovarianceVisual()
+  {
+    m_sceneManager->destroySceneNode(m_sceneNode->getName());
+  };
+
+  void setPosition(const Ogre::Vector3& position)
+  {
+    m_sceneNode->setPosition(position);
+  }
+
+  void setOrientation(const Ogre::Quaternion& orientation)
+  {
+    m_sceneNode->setOrientation(orientation);
+  }
+
+  void setVisible(bool visible)
+  {
+    m_sceneNode->setVisible(visible, true);
+  }
+
+  virtual void setColor(const Ogre::ColourValue& c) = 0;
+
+  virtual void setLineWidth(float lineWidth) = 0;
+
+  /// NOTE: It is assumed that the covariance matrix is already rotated into the target frame of the sceneNode!
+  virtual void setMeanCovariance(const Ogre::Vector3& mean, const Ogre::Matrix3& cov) = 0;
+
+protected:
+  Ogre::SceneManager* m_sceneManager;
+  Ogre::SceneNode* m_sceneNode;
+};
+
+// 2D ellipse visualization of a covariance matrix
+class ProbabilityEllipseCovarianceVisual : public CovarianceVisual
+{
+public:
+  ProbabilityEllipseCovarianceVisual(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode)
+    : CovarianceVisual(sceneManager, parentNode)
+  {
+    m_line = new rviz::BillboardLine(m_sceneManager, m_sceneNode);
+  }
+
+  virtual ~ProbabilityEllipseCovarianceVisual()
+  {
+    delete m_line;
+  }
+
+  virtual void setLineWidth(float lineWidth)
+  {
+    m_line->setLineWidth(lineWidth);
+  }
+
+  virtual void setColor(const Ogre::ColourValue& c)
+  {
+    m_line->setColor(c.r, c.g, c.b, c.a);
+  }
+
+  virtual void setMeanCovariance(const Ogre::Vector3& mean, const Ogre::Matrix3& cov)
+  {
+    int numberOfPoints;
+    double *xs, *ys;
+    double determinant = cov[0][0] * cov[1][1] - cov[1][0] * cov[0][1];
+
+    m_line->clear();
+
+    if (!std::isfinite(determinant))
+    {
+      ROS_WARN_STREAM_THROTTLE(
+          5.0, "Covariance matrix has non-finite values in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): "
+                   << cov);
+      return;
+    }
+
+    if (std::abs(cov[0][1] - cov[1][0]) > 0.00001)
+    {
+      ROS_WARN_STREAM_THROTTLE(
+          5.0,
+          "Covariance matrix is not symmetric in ProbabilityEllipseCovarianceVisual::setMeanCovariance(): " << cov);
+      return;
+    }
+
+    if (determinant > 0 && cov[0][0] > 0 /* positive definite? */ ||
+        std::abs(determinant - 0.00001) == 0.0 && (cov[0][0] > 0 || cov[1][1] > 0) /* positive semidefinite? */)
+    {
+      calc_prob_elli_99(mean.x, mean.y, cov[0][0], cov[1][1], cov[0][1], numberOfPoints, xs, ys);
+
+      m_line->setMaxPointsPerLine(numberOfPoints);
+
+      for (int i = 0; i < numberOfPoints; i++)
+      {
+        Ogre::Vector3 vertex(xs[i], ys[i], mean.z);
+        m_line->addPoint(vertex);
+      }
+    }
+    else
+    {
+      if (cov[0][0] != 0 || cov[0][1] != 0 || cov[0][2] != 0 || cov[1][0] != 0 || cov[1][1] != 0 || cov[1][2] != 0 ||
+          cov[2][0] != 0 || cov[2][1] != 0 || cov[2][2] != 0)
+        ROS_WARN_STREAM_THROTTLE(5.0,
+                                 "Covariance matrix is not positive (semi-)definite in "
+                                 "ProbabilityEllipseCovarianceVisual::setMeanCovariance(): "
+                                     << cov);
+    }
+  }
+
+private:
+  rviz::BillboardLine* m_line;
+
+  // Puts angle alpha into the interval [min..min+2*pi[
+  double set_angle_to_range(double alpha, double min)
+  {
+    while (alpha >= min + 2.0 * M_PI)
+    {
+      alpha -= 2.0 * M_PI;
+    }
+    while (alpha < min)
+    {
+      alpha += 2.0 * M_PI;
+    }
+    return alpha;
+  }
+
+  // Calculates the points on a rotated ellipse given by center xc, yc, half axes a, b and angle phi.
+  // Returns number of points np and points in Cart. coordinates
+  void calc_ellipse(double xc, double yc, double a, double b, double phi, int& np, double*& xvec, double*& yvec)
+  {
+    const int N_ELLI_POINTS = 40;
+    int i, offset;
+    double t, cr, sr, ca, sa, xi, yi, reso;
+    static double x[N_ELLI_POINTS + 1];
+    static double y[N_ELLI_POINTS + 1];
+    reso = 2 * M_PI / N_ELLI_POINTS;
+    offset = N_ELLI_POINTS / 2;
+    ca = cos(phi);
+    sa = sin(phi);
+    i = 0;
+    t = 0;
+    while (t < M_PI)
+    {
+      cr = cos(t);
+      sr = sin(t);
+      xi = a * cr * ca - b * sr * sa;
+      yi = a * cr * sa + b * sr * ca;
+      x[i] = xi + xc;
+      y[i] = yi + yc;
+      x[offset + i] = -xi + xc;
+      y[offset + i] = -yi + yc;
+      t = t + reso;
+      i++;
+    }
+    x[N_ELLI_POINTS] = x[0];  // Close contour
+    y[N_ELLI_POINTS] = y[0];  // Close contour
+    np = N_ELLI_POINTS + 1;
+    xvec = x;
+    yvec = y;
+  }
+
+  // Calculates the points on a 95%-iso-probability ellipse given by the bivarate RV with mean xc, yc
+  // and covariance matrix sxx, syy, sxy. Returns number of points np and points in Cart. coordinates
+  void calc_prob_elli_95(double xc, double yc, double sxx, double syy, double sxy, int& np, double*& x, double*& y)
+  {
+    double la, lb, a, b, phi;
+    la = (sxx + syy + sqrt((sxx - syy) * (sxx - syy) + 4 * sxy * sxy)) / 2;
+    lb = (sxx + syy - sqrt((sxx - syy) * (sxx - syy) + 4 * sxy * sxy)) / 2;
+    a = sqrt(5.991464 * la);
+    b = sqrt(5.991464 * lb);
+    phi = set_angle_to_range(atan2(2 * sxy, sxx - syy) / 2, 0);
+    calc_ellipse(xc, yc, a, b, phi, np, x, y);
+  }
+
+  // Calculates the points on a 99%-iso-probability ellipse given by the bivarate RV with mean xc, yc
+  // and covariance matrix sxx, syy, sxy. Returns number of points np and points in Cart. coordinates
+  void calc_prob_elli_99(double xc, double yc, double sxx, double syy, double sxy, int& np, double*& x, double*& y)
+  {
+    double la, lb, a, b, phi;
+    la = (sxx + syy + sqrt((sxx - syy) * (sxx - syy) + 4 * sxy * sxy)) / 2;
+    lb = (sxx + syy - sqrt((sxx - syy) * (sxx - syy) + 4 * sxy * sxy)) / 2;
+    a = sqrt(9.210340 * la);
+    b = sqrt(9.210340 * lb);
+    phi = set_angle_to_range(atan2(2 * sxy, sxx - syy) / 2, 0);
+    calc_ellipse(xc, yc, a, b, phi, np, x, y);
+  }
+};
+
+}  // namespace spencer_tracking_rviz_plugin
+
+#endif  // COVARIANCE_VISUAL_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/visuals/mesh_node.h

@@ -0,0 +1,191 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef MESH_NODE_H
+#define MESH_NODE_H
+
+#include <rviz/mesh_loader.h>
+#ifndef Q_MOC_RUN
+#include <resource_retriever/retriever.h>
+#endif
+#include <rviz/visualization_manager.h>
+#include <rviz/render_panel.h> // hack to get camera position 
+
+#include <OgreSceneManager.h>
+#include <OgreSubEntity.h>
+#include <OgreMaterialManager.h>
+#include <OgreTextureManager.h>
+#include <OgreTechnique.h>
+#include <OgreRoot.h>
+#include <OgreFrameListener.h>
+
+
+namespace spencer_tracking_rviz_plugin {
+    class MeshNode : public Ogre::FrameListener {
+    public:
+        MeshNode(rviz::DisplayContext* displayContext, Ogre::SceneNode* parentNode, const std::string& meshResource, Ogre::Vector3 position = Ogre::Vector3::ZERO)
+        : m_sceneManager(displayContext->getSceneManager()), m_displayContext(displayContext), m_meshResource(meshResource)
+        {
+            m_cameraFacing = false;
+            m_sceneNode = parentNode->createChildSceneNode();
+            m_sceneNode->setVisible(false);
+
+            // Load mesh
+            assert(!rviz::loadMeshFromResource(meshResource).isNull());
+
+            // Create scene entity
+            std::stringstream ss;
+            static int counter = 0;
+            ss << "gender_symbol_" << counter++;
+            std::string id = ss.str();
+
+            m_entity = m_sceneManager->createEntity(id, meshResource);
+            m_sceneNode->attachObject(m_entity);
+
+            // set up material
+            ss << "Material";
+            // Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create( ss.str(), "rviz" );
+            Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+            default_material->setReceiveShadows(false);
+            default_material->getTechnique(0)->setLightingEnabled(true);
+            default_material->getTechnique(0)->setAmbient( 0.5, 0.5, 0.5 );
+            m_materials.insert( default_material );
+            m_entity->setMaterial( default_material );
+
+            setPosition(position);
+            setVisible(true);
+
+            // For camera-facing meshes
+            Ogre::Root::getSingleton().addFrameListener(this);
+        }
+
+        virtual ~MeshNode() {
+            Ogre::Root::getSingleton().removeFrameListener(this);
+            m_sceneManager->destroyEntity(m_entity);
+
+            // destroy all the materials we've created
+            std::set<Ogre::MaterialPtr>::iterator it;
+            for(it = m_materials.begin(); it != m_materials.end(); it++ )
+            {
+                Ogre::MaterialPtr material = *it;
+                if (!material.isNull())
+                {
+                  material->unload();
+                  Ogre::MaterialManager::getSingleton().remove(material->getName());
+                }
+            }
+            m_materials.clear();
+            m_sceneManager->destroySceneNode(m_sceneNode->getName());
+        }
+
+        void setOrientation(const Ogre::Quaternion& orientation) {
+            m_orientation = orientation;
+        }
+
+        void setPosition(const Ogre::Vector3& position) {
+            m_sceneNode->setPosition(position);
+        }
+
+        void setScale(const float scaleFactor) {
+            m_sceneNode->setScale(Ogre::Vector3(scaleFactor, scaleFactor, scaleFactor));
+        }
+
+        void setVisible(bool visible) {
+            m_sceneNode->setVisible(visible, true);
+        }
+
+        void setCameraFacing(bool cameraFacing) {
+            m_cameraFacing = cameraFacing;
+        }
+
+        void setColor(const Ogre::ColourValue& c) {
+            Ogre::SceneBlendType blending;
+            bool depth_write;
+
+            if ( c.a < 0.9998 )
+            {
+              blending = Ogre::SBT_TRANSPARENT_ALPHA;
+              depth_write = false;
+            }
+            else
+            {
+              blending = Ogre::SBT_REPLACE;
+              depth_write = true;
+            }
+
+            std::set<Ogre::MaterialPtr>::iterator it;
+            for(it = m_materials.begin(); it != m_materials.end(); it++)
+            {
+              Ogre::Technique* technique = (*it)->getTechnique( 0 );
+
+              technique->setAmbient( c.r*0.5, c.g*0.5, c.b*0.5 );
+              technique->setDiffuse( c.r, c.g, c.b, c.a );
+              technique->setSceneBlending( blending );
+              technique->setDepthWriteEnabled( depth_write );
+              technique->setLightingEnabled( true );
+            }
+        }
+
+        const std::string& getMeshResource() const {
+            return m_meshResource;
+        }
+
+        // We are using this FrameListener callback to orient the mesh towards the camera.
+        // Using a SceneManager::Listener and its preUpdateSceneGraph(SceneManager, Camera) method doesn't work because
+        // it is apparently never invoked by the Rviz render system.
+        virtual bool frameStarted(const Ogre::FrameEvent &evt)
+        {
+            Ogre::Quaternion cameraQuat;
+            if(m_cameraFacing) {
+                // Align with camera view direction
+                // FIXME: The following way of retrieving the camera and its position is a bit hacky, don't try this at home!
+                rviz::VisualizationManager* visualizationManager = dynamic_cast<rviz::VisualizationManager*>(m_displayContext);
+                assert(visualizationManager != NULL);
+                cameraQuat = visualizationManager->getRenderPanel()->getCamera()->getOrientation();
+            }
+            m_sceneNode->setOrientation(cameraQuat * m_orientation);
+            return true;
+        }
+
+    private:
+        Ogre::SceneManager* m_sceneManager;
+        Ogre::SceneNode* m_sceneNode;
+        rviz::DisplayContext* m_displayContext;
+
+        Ogre::Quaternion m_orientation;
+        Ogre::Entity* m_entity;
+        std::set<Ogre::MaterialPtr> m_materials;
+        std::string m_meshResource;
+        bool m_cameraFacing;
+    };
+
+}
+
+#endif // MESH_NODE_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/visuals/person_visual.cpp

@@ -0,0 +1,232 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include <rviz/mesh_loader.h>
+#ifndef Q_MOC_RUN
+#include <ros/console.h>
+#include <ros/package.h>
+#include <resource_retriever/retriever.h>
+
+#include <boost/algorithm/string/predicate.hpp>
+#include <boost/filesystem.hpp>
+#endif
+#include <OgreSceneManager.h>
+#include <OgreSubEntity.h>
+#include <OgreMaterialManager.h>
+#include <OgreTextureManager.h>
+#include <OgreTechnique.h>
+#include <OgreAnimation.h>
+
+#include "person_visual.h"
+
+
+namespace fs = boost::filesystem;
+
+namespace spencer_tracking_rviz_plugin {
+
+/** This helper class ensures that skeletons can be loaded from a package:// path **/
+class RosPackagePathResourceLoadingListener : public Ogre::ResourceLoadingListener
+{
+public:
+    RosPackagePathResourceLoadingListener(const fs::path& parentPath) : _parentPath(parentPath) {
+    }
+
+    /** This event is called when a resource beings loading. */
+    virtual Ogre::DataStreamPtr resourceLoading(const Ogre::String &name, const Ogre::String &group, Ogre::Resource *resource) {
+      fs::path absolutePath = _parentPath / name;
+      ROS_INFO_STREAM("RosPackagePathResourceLoadingListener loading resource: " << absolutePath.string());
+
+      try
+      {
+        resource_retriever::Retriever retriever;
+        _lastResource = retriever.get(absolutePath.string()); // not thread-safe!
+        return Ogre::DataStreamPtr(new Ogre::MemoryDataStream(_lastResource.data.get(), _lastResource.size));
+      }
+      catch (resource_retriever::Exception& e)
+      {
+        ROS_ERROR("In RosPackagePathResourceLoadingListener: %s", e.what());
+        return Ogre::DataStreamPtr();
+      }
+    }
+
+    virtual void resourceStreamOpened(const Ogre::String &name, const Ogre::String &group, Ogre::Resource *resource, Ogre::DataStreamPtr& dataStream) {
+    }
+
+    virtual bool resourceCollision(Ogre::Resource *resource, Ogre::ResourceManager *resourceManager) {
+      return false;
+    }
+
+private:
+    const fs::path& _parentPath;
+    resource_retriever::MemoryResource _lastResource;
+};
+
+
+
+MeshPersonVisual::MeshPersonVisual(const PersonVisualDefaultArgs& args) : PersonVisual(args), m_animationState(NULL), m_walkingSpeed(1.0), entity_(NULL)
+{
+    m_childSceneNode = m_sceneNode->createChildSceneNode();
+    m_childSceneNode->setVisible(false);
+
+    std::string meshResource = "package://" ROS_PACKAGE_NAME "/media/animated_walking_man.mesh";
+
+    std::cout<<meshResource;
+
+    /// This is required to load referenced skeletons from package:// path
+    fs::path model_path(meshResource);
+    fs::path parent_path(model_path.parent_path());
+
+    Ogre::ResourceLoadingListener *newListener = new RosPackagePathResourceLoadingListener(parent_path), 
+                                  *oldListener = Ogre::ResourceGroupManager::getSingleton().getLoadingListener();
+
+    Ogre::ResourceGroupManager::getSingleton().setLoadingListener(newListener);
+    bool loadFailed = rviz::loadMeshFromResource(meshResource).isNull();
+    Ogre::ResourceGroupManager::getSingleton().setLoadingListener(oldListener);
+
+    delete newListener;
+
+
+    // Create scene entity
+    static size_t count = 0;
+    std::stringstream ss;
+    ss << "mesh_person_visual" << count++;
+    std::string id = ss.str();
+
+    entity_ = m_sceneManager->createEntity(id, meshResource);
+    m_childSceneNode->attachObject(entity_);
+
+    // set up animation
+    setAnimationState("");
+
+    // set up material
+    ss << "Material";
+    // Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create( ss.str(), "rviz" );
+    Ogre::MaterialPtr default_material = Ogre::MaterialManager::getSingleton().create(ss.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+    default_material->setReceiveShadows(false);
+    default_material->getTechnique(0)->setLightingEnabled(true);
+    default_material->getTechnique(0)->setAmbient( 0.5, 0.5, 0.5 );
+    materials_.insert( default_material );
+    entity_->setMaterial( default_material );
+
+    // set position
+    Ogre::Quaternion quat1; quat1.FromAngleAxis(Ogre::Degree(90), Ogre::Vector3(0,1,0));
+    Ogre::Quaternion quat2; quat2.FromAngleAxis(Ogre::Degree(-90), Ogre::Vector3(0,0,1));
+    m_childSceneNode->setOrientation(quat1 * quat2);
+
+    double scaleFactor = 0.243 * 1.75;
+    m_childSceneNode->setScale(Ogre::Vector3(scaleFactor, scaleFactor, scaleFactor));
+    m_childSceneNode->setPosition(Ogre::Vector3(0, 0, -1));
+
+    m_childSceneNode->setVisible(true);
+}
+
+MeshPersonVisual::~MeshPersonVisual() {
+    m_sceneManager->destroyEntity( entity_ );
+
+    // destroy all the materials we've created
+    std::set<Ogre::MaterialPtr>::iterator it;
+    for ( it = materials_.begin(); it!=materials_.end(); it++ )
+    {
+        Ogre::MaterialPtr material = *it;
+        if (!material.isNull())
+        {
+          material->unload();
+          Ogre::MaterialManager::getSingleton().remove(material->getName());
+        }
+    }
+    materials_.clear();
+
+    m_sceneManager->destroySceneNode(m_childSceneNode->getName());
+}
+
+void MeshPersonVisual::setColor(const Ogre::ColourValue& c) {
+    Ogre::SceneBlendType blending;
+    bool depth_write;
+
+    if ( c.a < 0.9998 )
+    {
+      blending = Ogre::SBT_TRANSPARENT_ALPHA;
+      depth_write = false;
+    }
+    else
+    {
+      blending = Ogre::SBT_REPLACE;
+      depth_write = true;
+    }
+
+    std::set<Ogre::MaterialPtr>::iterator it;
+    for( it = materials_.begin(); it != materials_.end(); it++ )
+    {
+      Ogre::Technique* technique = (*it)->getTechnique( 0 );
+
+      technique->setAmbient( c.r*0.5, c.g*0.5, c.b*0.5 );
+      technique->setDiffuse( c.r, c.g, c.b, c.a );
+      technique->setSceneBlending( blending );
+      technique->setDepthWriteEnabled( depth_write );
+      technique->setLightingEnabled( true );
+    }
+}
+
+void MeshPersonVisual::setAnimationState(const std::string& nameOfAnimationState) {
+    Ogre::AnimationStateSet *animationStates = entity_->getAllAnimationStates();
+    if(animationStates != NULL)
+    {
+      Ogre::AnimationStateIterator animationsIterator = animationStates->getAnimationStateIterator();
+      while (animationsIterator.hasMoreElements())
+      {
+        Ogre::AnimationState *animationState = animationsIterator.getNext();
+        if(animationState->getAnimationName() == nameOfAnimationState || nameOfAnimationState.empty()) {
+          animationState->setLoop(true);
+          animationState->setEnabled(true);
+          m_animationState = animationState;
+          return;
+        }    
+      }
+
+      // Not found. Set first animation state then.
+      ROS_WARN_STREAM_ONCE("Person mesh animation state " << nameOfAnimationState << " does not exist in mesh!");
+      setAnimationState("");
+    }
+}
+
+void MeshPersonVisual::setWalkingSpeed(float walkingSpeed) {
+    m_walkingSpeed = walkingSpeed;
+}
+
+
+void MeshPersonVisual::update(float deltaTime) {
+    if(m_animationState) {
+        m_animationState->addTime(0.7 * deltaTime * m_walkingSpeed);
+    }
+}
+
+
+} // end of namespace spencer_tracking_rviz_plugin

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/visuals/person_visual.h

@@ -0,0 +1,319 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef PERSON_VISUAL_H
+#define PERSON_VISUAL_H
+
+#include <rviz/ogre_helpers/shape.h>
+#include <rviz/ogre_helpers/billboard_line.h>
+
+#include <OgreSceneNode.h>
+#include <OgreAnimation.h>
+#include <OgreSharedPtr.h>
+#include <OgreEntity.h>
+
+
+namespace spencer_tracking_rviz_plugin {
+    // Abstract class for visuals which have got an adjustable line width
+    class HasLineWidth {
+    public:
+        virtual void setLineWidth(double lineWidth) = 0;
+    };
+
+    // Default arguments that need to be supplied to all types of PersonVisual
+    struct PersonVisualDefaultArgs {
+        PersonVisualDefaultArgs(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode) : sceneManager(sceneManager), parentNode(parentNode) {}
+        Ogre::SceneManager* sceneManager;
+        Ogre::SceneNode* parentNode;
+    };
+
+    /// Base class for all person visualization types
+    class PersonVisual {
+    public:
+        PersonVisual(const PersonVisualDefaultArgs& args) :
+                m_sceneManager(args.sceneManager),
+                m_correctOrientation( Ogre::Degree(90), Ogre::Vector3(1,0,0) )
+        {
+            m_parentSceneNode = args.parentNode;
+            m_sceneNode = args.parentNode->createChildSceneNode();
+
+            Ogre::Vector3 scale(1,1,1);
+            m_sceneNode->setScale(m_correctOrientation * scale);
+        }
+
+        virtual ~PersonVisual() {
+            m_sceneManager->destroySceneNode(m_sceneNode->getName());
+        };
+
+        void setPosition(const Ogre::Vector3& position) {
+            m_sceneNode->setPosition(position);
+        }
+
+        const Ogre::Vector3& getPosition() const {
+            return m_sceneNode->getPosition();
+        }
+
+        void setOrientation(const Ogre::Quaternion& orientation) {
+            m_sceneNode->setOrientation(orientation * m_correctOrientation);
+        }
+
+        const Ogre::Quaternion& getOrientation() const {
+           return m_sceneNode->getOrientation();
+        }
+
+        virtual void setScalingFactor(double scalingFactor) {
+            m_sceneNode->setScale(scalingFactor, scalingFactor, scalingFactor);
+        }
+
+        void setVisible(bool visible) {
+            m_sceneNode->setVisible(visible, true);
+        }
+
+        Ogre::SceneNode* getParentSceneNode() {
+            return m_parentSceneNode;
+        }
+
+        virtual void update(float deltaTime) {}
+        virtual void setColor(const Ogre::ColourValue& c) = 0;
+        virtual double getHeight() = 0;
+
+    protected:
+        Ogre::SceneManager* m_sceneManager;
+        Ogre::SceneNode *m_sceneNode, *m_parentSceneNode;
+        Ogre::Quaternion m_correctOrientation;
+    };
+
+
+    /// Visualization of a person as cylinder (body) + sphere (head)
+    class CylinderPersonVisual : public PersonVisual {
+    public:
+        CylinderPersonVisual(const PersonVisualDefaultArgs& args) : PersonVisual(args)
+        {
+            m_bodyShape = new rviz::Shape(rviz::Shape::Cylinder, args.sceneManager, m_sceneNode);
+            m_headShape = new rviz::Shape(rviz::Shape::Sphere, args.sceneManager, m_sceneNode);
+
+            const float headDiameter = 0.4;
+            const float totalHeight = getHeight();
+            const float cylinderHeight = totalHeight - headDiameter;
+
+            m_bodyShape->setScale(Ogre::Vector3(headDiameter, headDiameter, cylinderHeight));
+            m_headShape->setScale(Ogre::Vector3(headDiameter, headDiameter, headDiameter));
+
+            m_bodyShape->setPosition(Ogre::Vector3(0, 0, cylinderHeight / 2 - totalHeight / 2));
+            m_headShape->setPosition(Ogre::Vector3(0, 0, totalHeight / 2 - headDiameter / 2 ));
+        }
+
+        virtual ~CylinderPersonVisual() {
+            delete m_bodyShape;
+            delete m_headShape;
+        }
+
+        virtual void setColor(const Ogre::ColourValue& c) {
+            m_bodyShape->setColor(c);
+            m_headShape->setColor(c);
+        }
+
+        virtual double getHeight() {
+            return 1.75;
+        }
+
+    private:
+        rviz::Shape *m_bodyShape, *m_headShape;
+    };
+
+
+    /// Visualization of a person as a wireframe bounding box
+    class BoundingBoxPersonVisual : public PersonVisual, public HasLineWidth {
+    public:
+        BoundingBoxPersonVisual(const PersonVisualDefaultArgs& args, double height = 1.75, double width = 0.6, double scalingFactor = 1.0) : PersonVisual(args)
+        {
+            m_width = width; m_height = height; m_scalingFactor = scalingFactor; m_lineWidth = 0.03;
+            m_wireframe = NULL;
+            generateWireframe();
+        }
+
+        virtual ~BoundingBoxPersonVisual() {
+            delete m_wireframe;
+        }
+
+        virtual void setColor(const Ogre::ColourValue& c) {
+            m_wireframe->setColor(c.r, c.g, c.b, c.a);
+        }
+
+        virtual double getHeight() {
+            return m_height;
+        }
+
+        virtual void setLineWidth(double lineWidth) {
+            m_wireframe->setLineWidth(lineWidth);
+        }
+
+        /*
+        virtual void setScalingFactor(double scalingFactor) {
+            if(scalingFactor != m_scalingFactor) {
+                m_scalingFactor = scalingFactor;
+                generateWireframe();
+            }
+        }
+        */
+
+    protected:
+        virtual void generateWireframe() {
+            delete m_wireframe;
+            m_wireframe = new rviz::BillboardLine(m_sceneManager, m_sceneNode);
+            
+            m_wireframe->setLineWidth(m_lineWidth);   
+            m_wireframe->setMaxPointsPerLine(2);
+            m_wireframe->setNumLines(12);
+
+            double w = m_width * m_scalingFactor, h = m_height * m_scalingFactor;
+            Ogre::Vector3 bottomLeft(0, -w, 0), bottomRight(0, 0, 0), topLeft(0, -w, h), topRight(0, 0, h);
+            Ogre::Vector3 rear(w, 0, 0);
+
+            // Front quad
+                                        m_wireframe->addPoint(bottomLeft);          m_wireframe->addPoint(bottomRight);
+            m_wireframe->newLine();     m_wireframe->addPoint(bottomRight);         m_wireframe->addPoint(topRight);
+            m_wireframe->newLine();     m_wireframe->addPoint(topRight);            m_wireframe->addPoint(topLeft);
+            m_wireframe->newLine();     m_wireframe->addPoint(topLeft);             m_wireframe->addPoint(bottomLeft);
+
+            // Rear quad
+            m_wireframe->newLine();     m_wireframe->addPoint(bottomLeft + rear);   m_wireframe->addPoint(bottomRight + rear);
+            m_wireframe->newLine();     m_wireframe->addPoint(bottomRight + rear);  m_wireframe->addPoint(topRight + rear);
+            m_wireframe->newLine();     m_wireframe->addPoint(topRight + rear);     m_wireframe->addPoint(topLeft + rear);
+            m_wireframe->newLine();     m_wireframe->addPoint(topLeft + rear);      m_wireframe->addPoint(bottomLeft + rear);
+
+            // Four connecting lines between front and rear
+            m_wireframe->newLine();     m_wireframe->addPoint(bottomLeft);          m_wireframe->addPoint(bottomLeft + rear);
+            m_wireframe->newLine();     m_wireframe->addPoint(bottomRight);         m_wireframe->addPoint(bottomRight + rear);
+            m_wireframe->newLine();     m_wireframe->addPoint(topRight);            m_wireframe->addPoint(topRight + rear);
+            m_wireframe->newLine();     m_wireframe->addPoint(topLeft);             m_wireframe->addPoint(topLeft + rear);
+        
+            m_wireframe->setPosition(Ogre::Vector3(-w/2, w/2, -h/2));
+        } 
+
+    private:
+        rviz::BillboardLine *m_wireframe;
+        double m_width, m_height, m_scalingFactor, m_lineWidth;
+    };
+
+
+    /// Visualization of a person as a crosshair
+    class CrosshairPersonVisual : public PersonVisual, public HasLineWidth {
+    public:
+        CrosshairPersonVisual(const PersonVisualDefaultArgs& args, double height = 1.0, double width = 1.0) : PersonVisual(args)
+        {
+            m_width = width; m_height = height; m_lineWidth = 0.03;
+            m_crosshair = NULL;
+            generateCrosshair();
+        }
+
+        virtual ~CrosshairPersonVisual() {
+            delete m_crosshair;
+        }
+
+        virtual void setColor(const Ogre::ColourValue& c) {
+            m_crosshair->setColor(c.r, c.g, c.b, c.a);
+        }
+
+        virtual double getHeight() {
+            return m_height;
+        }
+
+        virtual void setLineWidth(double lineWidth) {
+            m_crosshair->setLineWidth(lineWidth);
+        }
+
+
+    protected:
+        virtual void generateCrosshair() {
+            delete m_crosshair;
+            m_crosshair = new rviz::BillboardLine(m_sceneManager, m_sceneNode);
+            
+            m_crosshair->setLineWidth(m_lineWidth);   
+            m_crosshair->setMaxPointsPerLine(2);
+            m_crosshair->setNumLines(5);
+
+            double w = m_width / 2.0;
+            Ogre::Vector3 p1a(-w, 0, 0), p1b(+w, 0, 0);
+            Ogre::Vector3 p2a(0, -w, 0), p2b(0, +w, 0);
+            Ogre::Vector3 p3a(0, 0, -w), p3b(0, 0, +w);
+
+            Ogre::Vector3 arrow_a(0.7*w, -0.2*w, 0), arrow_m(w, 0, 0), arrow_b(0.7*w, +0.2*w, 0);
+
+                                        m_crosshair->addPoint(p1a);         m_crosshair->addPoint(p1b);
+            m_crosshair->newLine();     m_crosshair->addPoint(p2a);         m_crosshair->addPoint(p2b);
+            m_crosshair->newLine();     m_crosshair->addPoint(p3a);         m_crosshair->addPoint(p3b);
+
+            m_crosshair->newLine();     m_crosshair->addPoint(arrow_m);     m_crosshair->addPoint(arrow_a);
+            m_crosshair->newLine();     m_crosshair->addPoint(arrow_m);     m_crosshair->addPoint(arrow_b);
+
+            m_crosshair->setPosition(Ogre::Vector3(0, 0, 0));
+        } 
+
+    private:
+        rviz::BillboardLine *m_crosshair;
+        double m_width, m_height, m_lineWidth;
+    };
+
+
+    /// Visualization of a person as a mesh (walking human)
+    class MeshPersonVisual : public PersonVisual {
+    public:
+        MeshPersonVisual(const PersonVisualDefaultArgs& args);
+
+        virtual ~MeshPersonVisual();
+
+        virtual void update(float deltaTime);
+
+        virtual void setColor(const Ogre::ColourValue& c);
+
+        void setAnimationState(const std::string& nameOfAnimationState);
+
+        void setWalkingSpeed(float walkingSpeed);
+
+        virtual double getHeight() {
+            return 1.75;
+        }
+
+        virtual void setScalingFactor(double scalingFactor) {
+            // Not supported (for some reason causes the mesh to be mirrored vertically).
+        }
+
+    private:
+        Ogre::SceneNode *m_childSceneNode;
+        Ogre::Entity* entity_;
+        Ogre::AnimationState* m_animationState;
+        std::set<Ogre::MaterialPtr> materials_;
+        float m_walkingSpeed;
+    };
+
+}
+
+#endif // PERSON_VISUAL_H

simulators/third_party/rviz_plugins/spencer_tracking_rviz_plugin/src/visuals/text_node.h

@@ -0,0 +1,95 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2013-2015, Timm Linder, Social Robotics Lab, University of Freiburg
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions are met:
+*
+*  * Redistributions of source code must retain the above copyright notice, this
+*    list of conditions and the following disclaimer.
+*  * Redistributions in binary form must reproduce the above copyright notice,
+*    this list of conditions and the following disclaimer in the documentation
+*    and/or other materials provided with the distribution.
+*  * Neither the name of the copyright holder nor the names of its contributors
+*    may be used to endorse or promote products derived from this software
+*    without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+*  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+*  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+*  DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+*  FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+*  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+*  SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+*  OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+*  OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef TEXT_NODE_H
+#define TEXT_NODE_H
+
+#include <rviz/ogre_helpers/shape.h>
+#include <rviz/ogre_helpers/movable_text.h>
+
+namespace spencer_tracking_rviz_plugin {
+    class TextNode {
+    public:
+        TextNode(Ogre::SceneManager* sceneManager, Ogre::SceneNode* parentNode, Ogre::Vector3 position = Ogre::Vector3::ZERO) : m_sceneManager(sceneManager)
+        {
+            m_sceneNode = parentNode->createChildSceneNode();
+
+            m_text = new rviz::MovableText("text");
+            m_text->setTextAlignment(rviz::MovableText::H_CENTER, rviz::MovableText::V_BELOW);
+            m_sceneNode->attachObject(m_text);
+
+            setCharacterHeight(1.0);
+            setPosition(position);
+            setVisible(true);
+        }
+
+        virtual ~TextNode() {
+            m_sceneManager->destroySceneNode(m_sceneNode->getName());
+            delete m_text;
+        };
+
+        void setCharacterHeight(double characterHeight) {
+            m_text->setCharacterHeight(characterHeight);
+            m_text->setSpaceWidth(0.3 * characterHeight);
+        }
+
+        double getCharacterHeight() {
+            return m_text->getCharacterHeight();
+        }
+
+        void setCaption(const std::string& caption) {
+            m_text->setCaption(caption);
+        }
+
+        void setPosition(const Ogre::Vector3& position) {
+            m_sceneNode->setPosition(position);
+        }
+
+        void setVisible(bool visible) {
+            m_sceneNode->setVisible(visible, true);
+        }
+
+        void setColor(const Ogre::ColourValue& c) {
+            m_text->setColor(c);
+        }
+
+        void showOnTop(bool onTop = true) {
+            m_text->showOnTop(onTop);
+        }
+
+    private:
+        Ogre::SceneManager* m_sceneManager;
+        Ogre::SceneNode* m_sceneNode;
+        rviz::MovableText* m_text;
+    };
+
+}
+
+#endif // TEXT_NODE_H