diff --git a/messages/rwth_perception_people_msgs/CHANGELOG.rst b/messages/rwth_perception_people_msgs/CHANGELOG.rst new file mode 100755 index 0000000..e729b16 --- /dev/null +++ b/messages/rwth_perception_people_msgs/CHANGELOG.rst @@ -0,0 +1,62 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package rwth_perception_people_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Contributors: Timm Linder + +1.0.11 (2020-08-26) +------------------- +* Merge pull request `#81 `_ from LCAS/master + Changelog updates from LCAS buildfarm +* Contributors: Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#2 `_ from spencer-project/master + Integrate multiple fixes from upstream +* Specify correct license in package.xmls +* Merge pull request `#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. +* Improved directory structure for clarity +* Contributors: Marc Hanheide, Timm Linder diff --git a/messages/rwth_perception_people_msgs/CMakeLists.txt b/messages/rwth_perception_people_msgs/CMakeLists.txt new file mode 100755 index 0000000..e2f9fee --- /dev/null +++ b/messages/rwth_perception_people_msgs/CMakeLists.txt @@ -0,0 +1,136 @@ +cmake_minimum_required(VERSION 2.8.3) +project(rwth_perception_people_msgs) + +## Find catkin macros and libraries +## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) +## is used, also find other catkin packages +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs geometry_msgs) + +## System dependencies are found with CMake's conventions +# find_package(Boost REQUIRED COMPONENTS system) + + +## Uncomment this if the package has a setup.py. This macro ensures +## modules and global scripts declared therein get installed +## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html +# catkin_python_setup() + +####################################### +## Declare ROS messages and services ## +####################################### + +## Generate messages in the 'msg' folder +add_message_files( + FILES + VisualOdometry.msg + GroundHOGDetections.msg + UpperBodyDetector.msg + GroundPlane.msg + PedestrianTracking.msg + PedestrianTrackingArray.msg + PedestrianLocations.msg + AnnotatedFrame.msg + Annotation.msg +) + +## Generate services in the 'srv' folder +# add_service_files( +# FILES +# Service1.srv +# Service2.srv +# ) + +## Generate added messages and services with any dependencies listed here +generate_messages( + DEPENDENCIES + std_msgs + geometry_msgs +) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( +# INCLUDE_DIRS include +# LIBRARIES rwth_perception_people_msgs +# CATKIN_DEPENDS message_generation std_msgs +# DEPENDS system_lib +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +# include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) + +## Declare a cpp library +# add_library(rwth_perception_people_msgs +# src/${PROJECT_NAME}/rwth_perception_people_msgs.cpp +# ) + +## Declare a cpp executable +# add_executable(rwth_perception_people_msgs_node src/rwth_perception_people_msgs_node.cpp) + +## Add cmake target dependencies of the executable/library +## as an example, message headers may need to be generated before nodes +# add_dependencies(rwth_perception_people_msgs_node rwth_perception_people_msgs_generate_messages_cpp) + +## Specify libraries to link a library or executable target against +# target_link_libraries(rwth_perception_people_msgs_node +# ${catkin_LIBRARIES} +# ) + +############# +## Install ## +############# + +# all install targets should use catkin DESTINATION variables +# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html + +## Mark executable scripts (Python etc.) for installation +## in contrast to setup.py, you can choose the destination +# install(PROGRAMS +# scripts/my_python_script +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark executables and/or libraries for installation +# install(TARGETS rwth_perception_people_msgs rwth_perception_people_msgs_node +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +# ) + +## Mark cpp header files for installation +# install(DIRECTORY include/${PROJECT_NAME}/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +# FILES_MATCHING PATTERN "*.h" +# PATTERN ".svn" EXCLUDE +# ) + +## Mark other files for installation (e.g. launch and bag files, etc.) +# install(FILES +# # myfile1 +# # myfile2 +# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +# ) + +############# +## Testing ## +############# + +## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test test/test_rwth_perception_people_msgs.cpp) +# if(TARGET ${PROJECT_NAME}-test) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) +# endif() + +## Add folders to be run by python nosetests +# catkin_add_nosetests(test) diff --git a/messages/rwth_perception_people_msgs/LICENSE b/messages/rwth_perception_people_msgs/LICENSE new file mode 100755 index 0000000..5a7ca8f --- /dev/null +++ b/messages/rwth_perception_people_msgs/LICENSE @@ -0,0 +1,12 @@ +Copyright (c) 2014, Christian Dondrup, Dennis Mitzel +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + +2. 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. + +3. 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. \ No newline at end of file diff --git a/messages/rwth_perception_people_msgs/msg/AnnotatedFrame.msg b/messages/rwth_perception_people_msgs/msg/AnnotatedFrame.msg new file mode 100755 index 0000000..1557c38 --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/AnnotatedFrame.msg @@ -0,0 +1,8 @@ +Header header + +# frame (image) +int64 frame +# time stamp +time timestamp +# Array of annotation boxes +Annotation[] annotations diff --git a/messages/rwth_perception_people_msgs/msg/Annotation.msg b/messages/rwth_perception_people_msgs/msg/Annotation.msg new file mode 100755 index 0000000..cdb7ded --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/Annotation.msg @@ -0,0 +1,14 @@ +Header header + +# frame id (backref) +int64 frame +# box id +int64 id +# top left x +float64 tlx +# top left y +float64 tly +# bbox width +float64 width +# bbox height +float64 height diff --git a/messages/rwth_perception_people_msgs/msg/GroundHOGDetections.msg b/messages/rwth_perception_people_msgs/msg/GroundHOGDetections.msg new file mode 100755 index 0000000..da721b9 --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/GroundHOGDetections.msg @@ -0,0 +1,7 @@ +Header header +int64[] pos_x # upper left x corner of the bounding box +int64[] pos_y # upper left y corner of the bounding box +int64[] width # width of the bounding box +int64[] height # height of the bounding box +float64[] scale # scale of the detected object +float64[] score # score of the detected object diff --git a/messages/rwth_perception_people_msgs/msg/GroundPlane.msg b/messages/rwth_perception_people_msgs/msg/GroundPlane.msg new file mode 100755 index 0000000..4ab53cb --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/GroundPlane.msg @@ -0,0 +1,3 @@ +Header header +float64[] n # 3x1 vector containing the normal of the ground plane +float64 d # d is the ditance ax+by+cz = d diff --git a/messages/rwth_perception_people_msgs/msg/PedestrianLocations.msg b/messages/rwth_perception_people_msgs/msg/PedestrianLocations.msg new file mode 100755 index 0000000..83851d4 --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/PedestrianLocations.msg @@ -0,0 +1,7 @@ +std_msgs/Header header +int32[] ids +geometry_msgs/Pose[] poses +float64[] distances +float64[] angles +float64 min_distance +float64 min_distance_angle diff --git a/messages/rwth_perception_people_msgs/msg/PedestrianTracking.msg b/messages/rwth_perception_people_msgs/msg/PedestrianTracking.msg new file mode 100755 index 0000000..30e6e78 --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/PedestrianTracking.msg @@ -0,0 +1,13 @@ +Header header +# position projected on the GP in world coordinates +float64[] traj_x +float64[] traj_y +float64[] traj_z +# position projected on the GP in camera coordinates +float64[] traj_x_camera +float64[] traj_y_camera +float64[] traj_z_camera +float64[] dir +float64 speed +int64 id +float64 score diff --git a/messages/rwth_perception_people_msgs/msg/PedestrianTrackingArray.msg b/messages/rwth_perception_people_msgs/msg/PedestrianTrackingArray.msg new file mode 100755 index 0000000..acd397b --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/PedestrianTrackingArray.msg @@ -0,0 +1,2 @@ +Header header +PedestrianTracking[] pedestrians diff --git a/messages/rwth_perception_people_msgs/msg/UpperBodyDetector.msg b/messages/rwth_perception_people_msgs/msg/UpperBodyDetector.msg new file mode 100755 index 0000000..b30089a --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/UpperBodyDetector.msg @@ -0,0 +1,8 @@ +Header header +int64[] pos_x # upper left x corner of the bounding box +int64[] pos_y # upper left y corner of the bounding box +int64[] width # width of the bounding box +int64[] height # height of the bounding box +float64[] dist # distance of the overlaid area and the template +float64[] median_depth # median_depth inside the detected bounding box + diff --git a/messages/rwth_perception_people_msgs/msg/VisualOdometry.msg b/messages/rwth_perception_people_msgs/msg/VisualOdometry.msg new file mode 100755 index 0000000..af6f5d5 --- /dev/null +++ b/messages/rwth_perception_people_msgs/msg/VisualOdometry.msg @@ -0,0 +1,43 @@ +# Internal information on the +# fovis algorithm parameters +# and results + +Header header + +# True if in the next run the reference +# frame will be changed. This is the case +# when the number of inliers drops below +# a threshold or the previous motion estimate +# failed in last motion estimation. +bool change_reference_frame + +# The threshold that is currently +# used for the FAST feature detector. +int32 fast_threshold + +# total number of detected keypoints in raw image +int32 num_total_detected_keypoints + +# same as above per pyramid level, starting at 0 +int32[] num_detected_keypoints + +# total number of keypoints after bucketing and +# edge and depth filter +int32 num_total_keypoints + +# same as above per pyramid level, starting at 0 +int32[] num_keypoints + +# info from motion estimator +int32 motion_estimate_status_code +string motion_estimate_status +bool motion_estimate_valid +int32 num_matches +int32 num_inliers +int32 num_reprojection_failures + +# runtime of last iteration in seconds +float64 runtime + +# motion estimation - in matrix (4x4) form [R|t] +float64[] transformation_matrix diff --git a/messages/rwth_perception_people_msgs/package.xml b/messages/rwth_perception_people_msgs/package.xml new file mode 100755 index 0000000..67a6773 --- /dev/null +++ b/messages/rwth_perception_people_msgs/package.xml @@ -0,0 +1,26 @@ + + + rwth_perception_people_msgs + 1.2.1 + The rwth_perception_people_msgs package + + Christian Dondrup + + BSD + + http://www.vision.rwth-aachen.de/ + + Christian Dondrup + Dennis Mitzel + + catkin + + std_msgs + geometry_msgs + message_generation + + std_msgs + geometry_msgs + + +roslibroslib diff --git a/messages/spencer_control_msgs/CHANGELOG.rst b/messages/spencer_control_msgs/CHANGELOG.rst new file mode 100755 index 0000000..9b4d25b --- /dev/null +++ b/messages/spencer_control_msgs/CHANGELOG.rst @@ -0,0 +1,58 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package spencer_control_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Contributors: Timm Linder + +1.0.11 (2020-08-26) +------------------- +* Merge pull request `#81 `_ from LCAS/master + Changelog updates from LCAS buildfarm +* Contributors: Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#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 diff --git a/messages/spencer_control_msgs/CMakeLists.txt b/messages/spencer_control_msgs/CMakeLists.txt new file mode 100755 index 0000000..4380db0 --- /dev/null +++ b/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 +) diff --git a/messages/spencer_control_msgs/CMakeLists.txt~ b/messages/spencer_control_msgs/CMakeLists.txt~ new file mode 100755 index 0000000..4380db0 --- /dev/null +++ b/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 +) diff --git a/messages/spencer_control_msgs/LICENSE b/messages/spencer_control_msgs/LICENSE new file mode 100755 index 0000000..98f9a8a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_control_msgs/msg/ComponentStatus.msg b/messages/spencer_control_msgs/msg/ComponentStatus.msg new file mode 100755 index 0000000..76f3cba --- /dev/null +++ b/messages/spencer_control_msgs/msg/ComponentStatus.msg @@ -0,0 +1,3 @@ +string name +bool alive +string detail \ No newline at end of file diff --git a/messages/spencer_control_msgs/package.xml b/messages/spencer_control_msgs/package.xml new file mode 100755 index 0000000..c84283c --- /dev/null +++ b/messages/spencer_control_msgs/package.xml @@ -0,0 +1,22 @@ + + + + spencer_control_msgs + 1.2.1 + Messages used for controlling the SPENCER robot platform. + + Timm Linder + Timm Linder + + BSD + + catkin + message_generation + roscpp + std_msgs + + message_runtime + roscpp + std_msgs + +roslibroslib diff --git a/messages/spencer_human_attribute_msgs/CHANGELOG.rst b/messages/spencer_human_attribute_msgs/CHANGELOG.rst new file mode 100755 index 0000000..a3bf3ad --- /dev/null +++ b/messages/spencer_human_attribute_msgs/CHANGELOG.rst @@ -0,0 +1,62 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package spencer_human_attribute_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Contributors: Timm Linder + +1.0.11 (2020-08-26) +------------------- +* Merge pull request `#81 `_ from LCAS/master + Changelog updates from LCAS buildfarm +* Contributors: Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#2 `_ from spencer-project/master + Integrate multiple fixes from upstream +* Specify correct license in package.xmls +* Merge pull request `#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 diff --git a/messages/spencer_human_attribute_msgs/CMakeLists.txt b/messages/spencer_human_attribute_msgs/CMakeLists.txt new file mode 100755 index 0000000..2a9830c --- /dev/null +++ b/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 +) diff --git a/messages/spencer_human_attribute_msgs/LICENSE b/messages/spencer_human_attribute_msgs/LICENSE new file mode 100755 index 0000000..dddbbfb --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_human_attribute_msgs/msg/CategoricalAttribute.msg b/messages/spencer_human_attribute_msgs/msg/CategoricalAttribute.msg new file mode 100755 index 0000000..1259aa2 --- /dev/null +++ b/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 + + + diff --git a/messages/spencer_human_attribute_msgs/msg/HumanAttributes.msg b/messages/spencer_human_attribute_msgs/msg/HumanAttributes.msg new file mode 100755 index 0000000..c60bc27 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_human_attribute_msgs/msg/ScalarAttribute.msg b/messages/spencer_human_attribute_msgs/msg/ScalarAttribute.msg new file mode 100755 index 0000000..eea07b7 --- /dev/null +++ b/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 diff --git a/messages/spencer_human_attribute_msgs/package.xml b/messages/spencer_human_attribute_msgs/package.xml new file mode 100755 index 0000000..6eaf8c9 --- /dev/null +++ b/messages/spencer_human_attribute_msgs/package.xml @@ -0,0 +1,22 @@ + + + + spencer_human_attribute_msgs + 1.2.1 + Messages used for Human Attribute Recognition + + Timm Linder + Timm Linder + + BSD + + catkin + message_generation + roscpp + std_msgs + + message_runtime + roscpp + std_msgs + +roslibroslib diff --git a/messages/spencer_social_relation_msgs/CHANGELOG.rst b/messages/spencer_social_relation_msgs/CHANGELOG.rst new file mode 100755 index 0000000..a7d76ee --- /dev/null +++ b/messages/spencer_social_relation_msgs/CHANGELOG.rst @@ -0,0 +1,63 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package spencer_social_relation_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Contributors: Timm Linder + +1.0.11 (2020-08-26) +------------------- +* Merge pull request `#81 `_ from LCAS/master + Changelog updates from LCAS buildfarm +* Contributors: Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#2 `_ from spencer-project/master + Integrate multiple fixes from upstream +* Specify correct license in package.xmls +* Merge pull request `#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 diff --git a/messages/spencer_social_relation_msgs/CMakeLists.txt b/messages/spencer_social_relation_msgs/CMakeLists.txt new file mode 100755 index 0000000..7949160 --- /dev/null +++ b/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 +) diff --git a/messages/spencer_social_relation_msgs/LICENSE b/messages/spencer_social_relation_msgs/LICENSE new file mode 100755 index 0000000..7416d42 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_social_relation_msgs/msg/SocialActivities.msg b/messages/spencer_social_relation_msgs/msg/SocialActivities.msg new file mode 100755 index 0000000..b0b55a4 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_social_relation_msgs/msg/SocialActivity.msg b/messages/spencer_social_relation_msgs/msg/SocialActivity.msg new file mode 100755 index 0000000..fc1857b --- /dev/null +++ b/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 diff --git a/messages/spencer_social_relation_msgs/msg/SocialRelation.msg b/messages/spencer_social_relation_msgs/msg/SocialRelation.msg new file mode 100755 index 0000000..2159dc9 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/messages/spencer_social_relation_msgs/msg/SocialRelations.msg b/messages/spencer_social_relation_msgs/msg/SocialRelations.msg new file mode 100755 index 0000000..f30cd8d --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_social_relation_msgs/package.xml b/messages/spencer_social_relation_msgs/package.xml new file mode 100755 index 0000000..889faa8 --- /dev/null +++ b/messages/spencer_social_relation_msgs/package.xml @@ -0,0 +1,28 @@ + + + + spencer_social_relation_msgs + 1.2.1 + Messages used for Social Activity Detection and Social Relation Analysis + + Timm Linder + BSD + + Timm Linder + + catkin + message_generation + roscpp + std_msgs + sensor_msgs + nav_msgs + geometry_msgs + + message_runtime + roscpp + std_msgs + sensor_msgs + nav_msgs + geometry_msgs + +roslibroslib diff --git a/messages/spencer_tracking_msgs/CHANGELOG.rst b/messages/spencer_tracking_msgs/CHANGELOG.rst new file mode 100755 index 0000000..b7b3a59 --- /dev/null +++ b/messages/spencer_tracking_msgs/CHANGELOG.rst @@ -0,0 +1,64 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package spencer_tracking_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Contributors: Timm Linder + +1.0.11 (2020-08-26) +------------------- +* Merge pull request `#81 `_ from LCAS/master + Changelog updates from LCAS buildfarm +* Contributors: Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#2 `_ from spencer-project/master + Integrate multiple fixes from upstream +* Specify correct license in package.xmls +* Merge pull request `#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 diff --git a/messages/spencer_tracking_msgs/CMakeLists.txt b/messages/spencer_tracking_msgs/CMakeLists.txt new file mode 100755 index 0000000..2210c40 --- /dev/null +++ b/messages/spencer_tracking_msgs/CMakeLists.txt @@ -0,0 +1,58 @@ +cmake_minimum_required(VERSION 2.8.3) +project(spencer_tracking_msgs) + +find_package(catkin REQUIRED COMPONENTS + roscpp + roslib + std_msgs + geometry_msgs + sensor_msgs + message_generation +) + +################################################ +## Declare ROS messages, services and actions ## +################################################ + +# Generate messages in the 'msg' folder +add_message_files( + FILES + Box.msg + DetectedPerson.msg + DetectedPersons.msg + DetectedPerson2d.msg + DetectedPersons2d.msg + CompositeDetectedPerson.msg + CompositeDetectedPersons.msg + TargetPerson.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 + sensor_msgs +) + +################################### +## catkin specific configuration ## +################################### +catkin_package( + CATKIN_DEPENDS roscpp std_msgs geometry_msgs message_runtime +) diff --git a/messages/spencer_tracking_msgs/LICENSE b/messages/spencer_tracking_msgs/LICENSE new file mode 100755 index 0000000..98f9a8a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/Box.msg b/messages/spencer_tracking_msgs/msg/Box.msg new file mode 100755 index 0000000..7a34ea4 --- /dev/null +++ b/messages/spencer_tracking_msgs/msg/Box.msg @@ -0,0 +1,4 @@ +int32 x # top left of x +int32 y # top left of y +uint32 w # width of 2d image bbox +uint32 h # height of 2d image bbox \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/CompositeDetectedPerson.msg b/messages/spencer_tracking_msgs/msg/CompositeDetectedPerson.msg new file mode 100755 index 0000000..9a657e9 --- /dev/null +++ b/messages/spencer_tracking_msgs/msg/CompositeDetectedPerson.msg @@ -0,0 +1,24 @@ +# 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. + +Box bounding_box # Associated box (x_center, y_center, w, h) +Box pro_bounding_box # cylinder projection box (x_center, y_center, w, h) +# float64[] feature \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/CompositeDetectedPersons.msg b/messages/spencer_tracking_msgs/msg/CompositeDetectedPersons.msg new file mode 100755 index 0000000..cb7cfe0 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/DetectedPerson.msg b/messages/spencer_tracking_msgs/msg/DetectedPerson.msg new file mode 100755 index 0000000..b0aabc6 --- /dev/null +++ b/messages/spencer_tracking_msgs/msg/DetectedPerson.msg @@ -0,0 +1,28 @@ +# 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 + +# 2D prejection +Box bounding_box +# 2d feature +# float64[] feature + +# 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 diff --git a/messages/spencer_tracking_msgs/msg/DetectedPerson2d.msg b/messages/spencer_tracking_msgs/msg/DetectedPerson2d.msg new file mode 100755 index 0000000..51fad13 --- /dev/null +++ b/messages/spencer_tracking_msgs/msg/DetectedPerson2d.msg @@ -0,0 +1,15 @@ +# Message defining a 2d image bbox of a detected 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 +int32 x # central x of 2d image bbox +int32 y # cemtral 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 + +# float64[] feature diff --git a/messages/spencer_tracking_msgs/msg/DetectedPersons.msg b/messages/spencer_tracking_msgs/msg/DetectedPersons.msg new file mode 100755 index 0000000..e8ec21c --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/DetectedPersons2d.msg b/messages/spencer_tracking_msgs/msg/DetectedPersons2d.msg new file mode 100755 index 0000000..ce828ed --- /dev/null +++ b/messages/spencer_tracking_msgs/msg/DetectedPersons2d.msg @@ -0,0 +1,5 @@ +# Message with all currently detected persons +# + +Header header # Header containing timestamp etc. of this message +DetectedPerson2d[] detections # All persons that are currently being detected \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/ImmDebugInfo.msg b/messages/spencer_tracking_msgs/msg/ImmDebugInfo.msg new file mode 100755 index 0000000..57fbbb6 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/ImmDebugInfos.msg b/messages/spencer_tracking_msgs/msg/ImmDebugInfos.msg new file mode 100755 index 0000000..10792ea --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/PersonTrajectory.msg b/messages/spencer_tracking_msgs/msg/PersonTrajectory.msg new file mode 100755 index 0000000..604bb00 --- /dev/null +++ b/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. diff --git a/messages/spencer_tracking_msgs/msg/PersonTrajectoryEntry.msg b/messages/spencer_tracking_msgs/msg/PersonTrajectoryEntry.msg new file mode 100755 index 0000000..93a7da3 --- /dev/null +++ b/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) diff --git a/messages/spencer_tracking_msgs/msg/TargetPerson.msg b/messages/spencer_tracking_msgs/msg/TargetPerson.msg new file mode 100755 index 0000000..aa7c1c1 --- /dev/null +++ b/messages/spencer_tracking_msgs/msg/TargetPerson.msg @@ -0,0 +1,18 @@ +# Message defining a tracked person +# +Header header # Header containing timestamp etc. of this message +uint64 track_id # unique identifier of the target, consistent over time +float32 target_confidence # target confidence inferenced by the target classifier +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) + +Box bounding_box +# float64[] feature diff --git a/messages/spencer_tracking_msgs/msg/TrackedGroup.msg b/messages/spencer_tracking_msgs/msg/TrackedGroup.msg new file mode 100755 index 0000000..29cd536 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/TrackedGroups.msg b/messages/spencer_tracking_msgs/msg/TrackedGroups.msg new file mode 100755 index 0000000..4117c12 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/TrackedPerson.msg b/messages/spencer_tracking_msgs/msg/TrackedPerson.msg new file mode 100755 index 0000000..cc7688b --- /dev/null +++ b/messages/spencer_tracking_msgs/msg/TrackedPerson.msg @@ -0,0 +1,17 @@ +# 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) + +Box bounding_box +# float64[] feature diff --git a/messages/spencer_tracking_msgs/msg/TrackedPerson2d.msg b/messages/spencer_tracking_msgs/msg/TrackedPerson2d.msg new file mode 100755 index 0000000..4b370ec --- /dev/null +++ b/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 diff --git a/messages/spencer_tracking_msgs/msg/TrackedPersons.msg b/messages/spencer_tracking_msgs/msg/TrackedPersons.msg new file mode 100755 index 0000000..5c5a361 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/msg/TrackedPersons2d.msg b/messages/spencer_tracking_msgs/msg/TrackedPersons2d.msg new file mode 100755 index 0000000..b800c0e --- /dev/null +++ b/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) diff --git a/messages/spencer_tracking_msgs/msg/TrackingTimingMetrics.msg b/messages/spencer_tracking_msgs/msg/TrackingTimingMetrics.msg new file mode 100755 index 0000000..ced3bbc --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_tracking_msgs/package.xml b/messages/spencer_tracking_msgs/package.xml new file mode 100755 index 0000000..a934114 --- /dev/null +++ b/messages/spencer_tracking_msgs/package.xml @@ -0,0 +1,32 @@ + + + + spencer_tracking_msgs + 1.2.1 + Messages used for Person Detection and Tracking + + Timm Linder + Stefan Breuers + Lucas Beyer + + BSD + + Timm Linder + Stefan Breuers + Lucas Beyer + + catkin + message_generation + roscpp + std_msgs + geometry_msgs + sensor_msgs + + + message_runtime + roscpp + std_msgs + geometry_msgs + sensor_msgs + +roslibroslib diff --git a/messages/spencer_tracking_msgs/srv/GetPersonTrajectories.srv b/messages/spencer_tracking_msgs/srv/GetPersonTrajectories.srv new file mode 100755 index 0000000..fb3e767 --- /dev/null +++ b/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. diff --git a/messages/spencer_vision_msgs/CHANGELOG.rst b/messages/spencer_vision_msgs/CHANGELOG.rst new file mode 100755 index 0000000..56a5f9f --- /dev/null +++ b/messages/spencer_vision_msgs/CHANGELOG.rst @@ -0,0 +1,65 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package spencer_vision_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Contributors: Timm Linder + +1.0.11 (2020-08-26) +------------------- +* Merge pull request `#1 `_ from spencer-project/master + Adding confidence field to PersonROI message +* Contributors: Manuel Fernandez-Carmona, Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#3 `_ from spencer-project/master + Adding confidence field to PersonROI message +* Adding confidence field to PersonROI message +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Manuel Fernandez-Carmona, Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#2 `_ from spencer-project/master + Integrate multiple fixes from upstream +* Specify correct license in package.xmls +* Merge pull request `#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 diff --git a/messages/spencer_vision_msgs/CMakeLists.txt b/messages/spencer_vision_msgs/CMakeLists.txt new file mode 100755 index 0000000..79a25a8 --- /dev/null +++ b/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 +) diff --git a/messages/spencer_vision_msgs/LICENSE b/messages/spencer_vision_msgs/LICENSE new file mode 100755 index 0000000..98f9a8a --- /dev/null +++ b/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 \ No newline at end of file diff --git a/messages/spencer_vision_msgs/msg/PersonImage.msg b/messages/spencer_vision_msgs/msg/PersonImage.msg new file mode 100755 index 0000000..b64a80b --- /dev/null +++ b/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 + diff --git a/messages/spencer_vision_msgs/msg/PersonImages.msg b/messages/spencer_vision_msgs/msg/PersonImages.msg new file mode 100755 index 0000000..7cb3163 --- /dev/null +++ b/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 diff --git a/messages/spencer_vision_msgs/msg/PersonROI.msg b/messages/spencer_vision_msgs/msg/PersonROI.msg new file mode 100755 index 0000000..2aab830 --- /dev/null +++ b/messages/spencer_vision_msgs/msg/PersonROI.msg @@ -0,0 +1,8 @@ +# 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 +float64 confidence + +sensor_msgs/RegionOfInterest roi + diff --git a/messages/spencer_vision_msgs/msg/PersonROIs.msg b/messages/spencer_vision_msgs/msg/PersonROIs.msg new file mode 100755 index 0000000..6b197aa --- /dev/null +++ b/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 diff --git a/messages/spencer_vision_msgs/package.xml b/messages/spencer_vision_msgs/package.xml new file mode 100755 index 0000000..9a535fd --- /dev/null +++ b/messages/spencer_vision_msgs/package.xml @@ -0,0 +1,31 @@ + + + + spencer_vision_msgs + 1.2.1 + Messages used for Vision-related tasks in SPENCER + + Lucas Beyer + Stefan Breuers + Timm Linder + + BSD + + Timm Linder + Stefan Breuers + Lucas Beyer + + catkin + message_generation + roscpp + std_msgs + geometry_msgs + sensor_msgs + + message_runtime + roscpp + std_msgs + geometry_msgs + sensor_msgs + +roslibroslib diff --git a/messages/visualization/spencer_tracking_rviz_plugin/CHANGELOG.rst b/messages/visualization/spencer_tracking_rviz_plugin/CHANGELOG.rst new file mode 100755 index 0000000..f047dc0 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/CHANGELOG.rst @@ -0,0 +1,83 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package spencer_tracking_rviz_plugin +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ +* Fix Qt5 cmake module not being found +* Contributors: Timm Linder + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Fixes required for ROS Melodic support + - Replace deprecated pluginlib macro + - Qt5 support for Rviz plugins + - Replace tf by tf2 message_filter in rviz plugin (contributed by user @f-fl0 in srl-freiburg/spencer_tracking_rviz_plugin) + - Add boost namespace for shared_ptr (for rviz plugin, this was contributed by @makokal in srl-freiburg/spencer_tracking_rviz_plugin) +* Contributors: Timm Linder, Billy Okal, f-fl0 + +1.0.11 (2020-08-26) +------------------- +* Various smaller fixes for ROS Kinetic + - Use find_package(Eigen3) instead of find_package(Eigen) + - Configurable odom and base footprint frame IDs (fixes `#53 `_) +* Contributors: Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#2 `_ from spencer-project/master + Integrate multiple fixes from upstream +* Specify correct license in package.xmls +* Merge pull request `#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 +* Fixed segfault. You shouldnt mix QT4 and QT5 +* Compiling on kinetic. Rviz crashes with views (QT bug?) +* Updating lots of utility packages to latest version from SPENCER repo. Licenses updated. +* Ignore Z position in visualization by default +* Update README.md +* Update README.md +* Disable group history visualization per default since it is very CPU-intensive, reduce default history size +* Various fixes to TrackedPersons and DetectedPersons display in RViz + - Fix popping up of track / detection IDs when persons first appear + - Add option to ignore Z position from ROS message + - Prevent crash when covariance matrix is not positive (semi-)definite + - Add option to hide IDs of single-person groups + - Display MISSED (new) / MATCHED / OCCLUDED track status to distinguish between misdetections and physical occlusions +* Update README.md +* Adding message definitions, RViz plugins and mock scripts +* Contributors: Joao Avelino, Marc Hanheide, Timm Linder diff --git a/messages/visualization/spencer_tracking_rviz_plugin/CMakeLists.txt b/messages/visualization/spencer_tracking_rviz_plugin/CMakeLists.txt new file mode 100755 index 0000000..f57da97 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/CMakeLists.txt @@ -0,0 +1,119 @@ +# 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/target_person_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/target_person_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/target_person_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}) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/LICENSE b/messages/visualization/spencer_tracking_rviz_plugin/LICENSE new file mode 100755 index 0000000..74f57e8 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/LICENSE @@ -0,0 +1,31 @@ +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 +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. diff --git a/messages/visualization/spencer_tracking_rviz_plugin/README.md b/messages/visualization/spencer_tracking_rviz_plugin/README.md new file mode 100755 index 0000000..7ef689b --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/README.md @@ -0,0 +1,38 @@ +RViz plugins for visualization of detected and tracked persons, groups, social relations, activities and human attributes +------------------------------------------------------------------------------------------------------------------------- +Author: ©2013-2015 Timm Linder, Social Robotics Laboratory, Albert-Ludwigs-University Freiburg, Germany + +E-mail: linder@cs.uni-freiburg.de + +This ROS package adds the following display types to RViz: + +- Detected persons (spencer_tracking_msgs/DetectedPersons) +- Tracked persons (spencer_tracking_msgs/TrackedPersons) +- Tracked groups (spencer_tracking_msgs/TrackedGroups) +- Social relations (spencer_social_relation_msgs/SocialRelations) +- Social activities (spencer_social_relation_msgs/SocialActivities) +- Human attributes (spencer_human_attribute_msgs/HumanAttributes) + +These message types have been defined within the consortium of the SPENCER FP-7 European Research Project (http://www.spencer.eu). + +See the "screenshots" folder for some example images of these displays. + + +Installation instructions +------------------------- +Just check out the package into the "src" folder of your ROS catkin workspace, run "catkin_make", make sure you have run +"source devel/setup.sh", and then run RViz using "rosrun rviz rviz". The plugin will be discovered automatically. + +Usage +----- +Add displays for detected and tracked persons, groups or human attributes by clicking on "Add display" in the "Displays" +panel in RViz. Adjust the topic name if necessary, to point to the correct topic. + +The displays for groups, social relations, and human attributes require two topic to be set in order to function, +the second topic being the tracked persons topic. + +Known issues +------------ +Rviz may crash with a segfault when exiting. This usually can be ignored. +Person visuals in the tracked groups and human attributes display do not support walking animations. +However, these displays can be combined with the tracked persons display (which does support animations) by disabling their person visuals. diff --git a/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/DetectedPersons.png b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/DetectedPersons.png new file mode 100755 index 0000000..84b9ccc Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/DetectedPersons.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/HumanAttributes.png b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/HumanAttributes.png new file mode 100755 index 0000000..b8873ea Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/HumanAttributes.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/SocialActivities.png b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/SocialActivities.png new file mode 100755 index 0000000..66eb0b1 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/SocialActivities.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/SocialRelations.png b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/SocialRelations.png new file mode 100755 index 0000000..fae1feb Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/SocialRelations.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TargetPerson.png b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TargetPerson.png new file mode 100755 index 0000000..6e8cb7d Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TargetPerson.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TrackedGroups.png b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TrackedGroups.png new file mode 100755 index 0000000..8c32dab Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TrackedGroups.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TrackedPersons.png b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TrackedPersons.png new file mode 100755 index 0000000..6e8cb7d Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/icons/classes/TrackedPersons.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/media/animated_walking_man.mesh b/messages/visualization/spencer_tracking_rviz_plugin/media/animated_walking_man.mesh new file mode 100755 index 0000000..a9f8ec9 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/media/animated_walking_man.mesh differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/media/animated_walking_man.skeleton b/messages/visualization/spencer_tracking_rviz_plugin/media/animated_walking_man.skeleton new file mode 100755 index 0000000..f07386f Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/media/animated_walking_man.skeleton differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/media/female_symbol.dae b/messages/visualization/spencer_tracking_rviz_plugin/media/female_symbol.dae new file mode 100755 index 0000000..67ab4ed --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/media/female_symbol.dae @@ -0,0 +1,1817 @@ + + + Z_UP + + + + + + + + + + + + 0.000000 0.000000 0.000000 1.000000 + + + 1.000000 0.000000 1.000000 1.000000 + + + 0.000000 0.000000 0.000000 1.000000 + + + 2.000000 + + + 0.000000 0.000000 0.000000 1.000000 + + + 1.000000 + + + 1.000000 1.000000 1.000000 1.000000 + + + 0.000000 + + + + + + + + + + + +20.843750 19.600000 0.000000 +3.714062 19.600000 0.000000 +3.714062 23.120314 0.000000 +5.246523 23.503572 0.000000 +6.699265 24.002457 0.000000 +8.072289 24.616966 0.000000 +9.365592 25.347097 0.000000 +10.579176 26.192860 0.000000 +11.713041 27.154243 0.000000 +12.767185 28.231251 0.000000 +13.743845 29.450035 0.000000 +14.570248 30.726021 0.000000 +15.246396 32.059216 0.000000 +15.772289 33.449619 0.000000 +16.147928 34.897228 0.000000 +16.373310 36.402042 0.000000 +16.448437 37.964062 0.000000 +16.350063 39.814190 0.000000 +16.054941 41.569927 0.000000 +15.563073 43.231281 0.000000 +14.874457 44.798248 0.000000 +13.989096 46.270828 0.000000 +12.906984 47.649014 0.000000 +11.628124 48.932812 0.000000 +10.256124 50.038391 0.000000 +8.801722 50.973885 0.000000 +7.264925 51.739285 0.000000 +5.645729 52.334602 0.000000 +3.944134 52.759827 0.000000 +2.160141 53.014965 0.000000 +0.293751 53.100002 0.000000 +-1.587371 53.016968 0.000000 +-3.378061 52.767853 0.000000 +-5.078317 52.352676 0.000000 +-6.688138 51.771431 0.000000 +-8.207527 51.024117 0.000000 +-9.636481 50.110722 0.000000 +-10.975000 49.031250 0.000000 +-12.245153 47.761131 0.000000 +-13.319898 46.387375 0.000000 +-14.199234 44.909985 0.000000 +-14.883162 43.328953 0.000000 +-15.371685 41.644299 0.000000 +-15.664798 39.855999 0.000000 +-15.762501 37.964062 0.000000 +-15.694038 36.455837 0.000000 +-15.488647 35.000793 0.000000 +-15.146334 33.598946 0.000000 +-14.667093 32.250286 0.000000 +-14.050928 30.954817 0.000000 +-13.297833 29.712536 0.000000 +-12.407814 28.523438 0.000000 +-11.438203 27.468559 0.000000 +-10.390083 26.517315 0.000000 +-9.263457 25.669708 0.000000 +-8.058324 24.925734 0.000000 +-6.774683 24.285398 0.000000 +-5.412533 23.748697 0.000000 +-3.971876 23.315626 0.000000 +-3.971876 19.600000 0.000000 +-20.843750 19.600000 0.000000 +-20.843750 12.900001 0.000000 +-3.971876 12.900001 0.000000 +-3.971876 -5.565625 0.000000 +3.714062 -5.565625 0.000000 +3.714062 12.900001 0.000000 +20.843750 12.900001 0.000000 +20.843750 19.600000 5.905512 +3.714062 19.600000 5.905512 +3.714062 23.120314 5.905512 +5.246523 23.503572 5.905512 +6.699265 24.002457 5.905512 +8.072289 24.616966 5.905512 +9.365592 25.347097 5.905512 +10.579176 26.192860 5.905512 +11.713041 27.154243 5.905512 +12.767185 28.231251 5.905512 +13.743845 29.450035 5.905512 +14.570248 30.726021 5.905512 +15.246396 32.059216 5.905512 +15.772289 33.449619 5.905512 +16.147928 34.897228 5.905512 +16.373310 36.402042 5.905512 +16.448437 37.964062 5.905512 +16.350063 39.814190 5.905512 +16.054941 41.569927 5.905512 +15.563073 43.231281 5.905512 +14.874457 44.798248 5.905512 +13.989096 46.270828 5.905512 +12.906984 47.649014 5.905512 +11.628124 48.932812 5.905512 +10.256124 50.038391 5.905512 +8.801722 50.973885 5.905512 +7.264925 51.739285 5.905512 +5.645729 52.334602 5.905512 +3.944134 52.759827 5.905512 +2.160141 53.014965 5.905512 +0.293751 53.100002 5.905512 +-1.587371 53.016968 5.905512 +-3.378061 52.767853 5.905512 +-5.078317 52.352676 5.905512 +-6.688138 51.771431 5.905512 +-8.207527 51.024117 5.905512 +-9.636481 50.110722 5.905512 +-10.975000 49.031250 5.905512 +-12.245153 47.761131 5.905512 +-13.319898 46.387375 5.905512 +-14.199234 44.909985 5.905512 +-14.883162 43.328953 5.905512 +-15.371685 41.644299 5.905512 +-15.664798 39.855999 5.905512 +-15.762501 37.964062 5.905512 +-15.694038 36.455837 5.905512 +-15.488647 35.000793 5.905512 +-15.146334 33.598946 5.905512 +-14.667093 32.250286 5.905512 +-14.050928 30.954817 5.905512 +-13.297833 29.712536 5.905512 +-12.407814 28.523438 5.905512 +-11.438203 27.468559 5.905512 +-10.390083 26.517315 5.905512 +-9.263457 25.669708 5.905512 +-8.058324 24.925734 5.905512 +-6.774683 24.285398 5.905512 +-5.412533 23.748697 5.905512 +-3.971876 23.315626 5.905512 +-3.971876 19.600000 5.905512 +-20.843750 19.600000 5.905512 +-20.843750 12.900001 5.905512 +-3.971876 12.900001 5.905512 +-3.971876 -5.565625 5.905512 +3.714062 -5.565625 5.905512 +3.714062 12.900001 5.905512 +20.843750 12.900001 5.905512 +8.582062 37.066360 0.000000 +8.434502 36.127934 0.000000 +8.188567 35.234726 0.000000 +7.844260 34.386734 0.000000 +7.401578 33.583965 0.000000 +6.860522 32.826401 0.000000 +6.221092 32.114063 0.000000 +5.509885 31.473598 0.000000 +4.753491 30.931664 0.000000 +3.951913 30.488266 0.000000 +3.105150 30.143400 0.000000 +2.213202 29.897070 0.000000 +1.276069 29.749269 0.000000 +0.293751 29.700001 0.000000 +-0.679527 29.749268 0.000000 +-1.608291 29.897066 0.000000 +-2.492538 30.143398 0.000000 +-3.332271 30.488270 0.000000 +-4.127489 30.931669 0.000000 +-4.878190 31.473598 0.000000 +-5.584375 32.114063 0.000000 +-6.219452 32.826405 0.000000 +-6.756824 33.583958 0.000000 +-7.196493 34.386734 0.000000 +-7.538457 35.234726 0.000000 +-7.782718 36.127937 0.000000 +-7.929275 37.066360 0.000000 +-7.978127 38.049999 0.000000 +-7.929275 39.034042 0.000000 +-7.782718 39.972767 0.000000 +-7.538457 40.866184 0.000000 +-7.196493 41.714287 0.000000 +-6.756825 42.517082 0.000000 +-6.219452 43.274555 0.000000 +-5.584375 43.986721 0.000000 +-4.878190 44.626980 0.000000 +-4.127488 45.168736 0.000000 +-3.332271 45.611992 0.000000 +-2.492539 45.956745 0.000000 +-1.608291 46.203003 0.000000 +-0.679528 46.350758 0.000000 +0.293751 46.400002 0.000000 +1.276068 46.350754 0.000000 +2.213202 46.202995 0.000000 +3.105150 45.956745 0.000000 +3.951913 45.611996 0.000000 +4.753491 45.168739 0.000000 +5.509884 44.626980 0.000000 +6.221092 43.986721 0.000000 +6.860522 43.274555 0.000000 +7.401578 42.517075 0.000000 +7.844260 41.714287 0.000000 +8.188568 40.866184 0.000000 +8.434502 39.972771 0.000000 +8.582062 39.034042 0.000000 +8.631248 38.049999 0.000000 +8.582062 37.066360 5.905512 +8.434502 36.127934 5.905512 +8.188567 35.234726 5.905512 +7.844260 34.386734 5.905512 +7.401578 33.583965 5.905512 +6.860522 32.826401 5.905512 +6.221092 32.114063 5.905512 +5.509885 31.473598 5.905512 +4.753491 30.931664 5.905512 +3.951913 30.488266 5.905512 +3.105150 30.143400 5.905512 +2.213202 29.897070 5.905512 +1.276069 29.749269 5.905512 +0.293751 29.700001 5.905512 +-0.679527 29.749268 5.905512 +-1.608291 29.897066 5.905512 +-2.492538 30.143398 5.905512 +-3.332271 30.488270 5.905512 +-4.127489 30.931669 5.905512 +-4.878190 31.473598 5.905512 +-5.584375 32.114063 5.905512 +-6.219452 32.826405 5.905512 +-6.756824 33.583958 5.905512 +-7.196493 34.386734 5.905512 +-7.538457 35.234726 5.905512 +-7.782718 36.127937 5.905512 +-7.929275 37.066360 5.905512 +-7.978127 38.049999 5.905512 +-7.929275 39.034042 5.905512 +-7.782718 39.972767 5.905512 +-7.538457 40.866184 5.905512 +-7.196493 41.714287 5.905512 +-6.756825 42.517082 5.905512 +-6.219452 43.274555 5.905512 +-5.584375 43.986721 5.905512 +-4.878190 44.626980 5.905512 +-4.127488 45.168736 5.905512 +-3.332271 45.611992 5.905512 +-2.492539 45.956745 5.905512 +-1.608291 46.203003 5.905512 +-0.679528 46.350758 5.905512 +0.293751 46.400002 5.905512 +1.276068 46.350754 5.905512 +2.213202 46.202995 5.905512 +3.105150 45.956745 5.905512 +3.951913 45.611996 5.905512 +4.753491 45.168739 5.905512 +5.509884 44.626980 5.905512 +6.221092 43.986721 5.905512 +6.860522 43.274555 5.905512 +7.401578 42.517075 5.905512 +7.844260 41.714287 5.905512 +8.188568 40.866184 5.905512 +8.434502 39.972771 5.905512 +8.582062 39.034042 5.905512 +8.631248 38.049999 5.905512 + + + + + + + + + + + +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +0.242621 -0.970121 0.000000 +0.283967 -0.958834 0.000000 +0.283967 -0.958834 0.000000 +0.242621 -0.970121 0.000000 +0.283967 -0.958834 0.000000 +0.242621 -0.970121 0.000000 +0.283967 -0.958834 0.000000 +0.367023 -0.930212 0.000000 +0.367023 -0.930212 0.000000 +0.283967 -0.958834 0.000000 +0.367023 -0.930212 0.000000 +0.283967 -0.958834 0.000000 +0.367023 -0.930212 0.000000 +0.450552 -0.892750 0.000000 +0.450551 -0.892750 0.000000 +0.367023 -0.930212 0.000000 +0.450551 -0.892750 0.000000 +0.367023 -0.930212 0.000000 +0.450552 -0.892750 0.000000 +0.532285 -0.846565 0.000000 +0.532285 -0.846565 0.000000 +0.450552 -0.892750 0.000000 +0.532285 -0.846565 0.000000 +0.450551 -0.892750 0.000000 +0.532285 -0.846565 0.000000 +0.609918 -0.792465 0.000000 +0.609918 -0.792465 0.000000 +0.532285 -0.846565 0.000000 +0.609918 -0.792465 0.000000 +0.532285 -0.846565 0.000000 +0.609918 -0.792465 0.000000 +0.681415 -0.731897 0.000000 +0.681415 -0.731897 0.000000 +0.609918 -0.792465 0.000000 +0.681415 -0.731897 0.000000 +0.609918 -0.792465 0.000000 +0.681415 -0.731897 0.000000 +0.748424 -0.663221 0.000000 +0.748424 -0.663220 0.000000 +0.681415 -0.731897 0.000000 +0.748424 -0.663220 0.000000 +0.681415 -0.731897 0.000000 +0.748424 -0.663221 0.000000 +0.810880 -0.585212 0.000000 +0.810880 -0.585213 0.000000 +0.748424 -0.663221 0.000000 +0.810880 -0.585213 0.000000 +0.748424 -0.663220 0.000000 +0.810880 -0.585212 0.000000 +0.866801 -0.498654 0.000000 +0.866801 -0.498654 0.000000 +0.810880 -0.585212 0.000000 +0.866801 -0.498654 0.000000 +0.810880 -0.585213 0.000000 +0.866801 -0.498654 0.000000 +0.914922 -0.403630 0.000000 +0.914922 -0.403630 0.000000 +0.866801 -0.498654 0.000000 +0.914922 -0.403630 0.000000 +0.866801 -0.498654 0.000000 +0.914922 -0.403630 0.000000 +0.953019 -0.302910 0.000000 +0.953019 -0.302910 0.000000 +0.914922 -0.403630 0.000000 +0.953019 -0.302910 0.000000 +0.914922 -0.403630 0.000000 +0.953019 -0.302910 0.000000 +0.979812 -0.199923 0.000000 +0.979812 -0.199923 0.000000 +0.953019 -0.302910 0.000000 +0.979812 -0.199923 0.000000 +0.953019 -0.302910 0.000000 +0.979812 -0.199923 0.000000 +0.995166 -0.098205 0.000000 +0.995166 -0.098205 0.000000 +0.979812 -0.199923 0.000000 +0.995166 -0.098205 0.000000 +0.979812 -0.199923 0.000000 +0.995166 -0.098205 0.000000 +0.999997 0.002531 0.000000 +0.999997 0.002531 0.000000 +0.995166 -0.098205 0.000000 +0.999997 0.002531 0.000000 +0.995166 -0.098205 0.000000 +0.999997 0.002531 0.000000 +0.993975 0.109607 0.000000 +0.993975 0.109607 0.000000 +0.999997 0.002531 0.000000 +0.993975 0.109607 0.000000 +0.999997 0.002531 0.000000 +0.993975 0.109607 0.000000 +0.974304 0.225239 0.000000 +0.974304 0.225239 0.000000 +0.993975 0.109607 0.000000 +0.974304 0.225239 0.000000 +0.993975 0.109607 0.000000 +0.974304 0.225239 0.000000 +0.939047 0.343788 0.000000 +0.939047 0.343788 0.000000 +0.974304 0.225239 0.000000 +0.939047 0.343788 0.000000 +0.974304 0.225239 0.000000 +0.939047 0.343788 0.000000 +0.888060 0.459728 0.000000 +0.888060 0.459728 0.000000 +0.939047 0.343788 0.000000 +0.888060 0.459728 0.000000 +0.939047 0.343788 0.000000 +0.888060 0.459728 0.000000 +0.823366 0.567510 0.000000 +0.823366 0.567510 0.000000 +0.888060 0.459728 0.000000 +0.823366 0.567510 0.000000 +0.888060 0.459728 0.000000 +0.823366 0.567510 0.000000 +0.748796 0.662800 0.000000 +0.748796 0.662800 0.000000 +0.823366 0.567510 0.000000 +0.748796 0.662800 0.000000 +0.823366 0.567510 0.000000 +0.748796 0.662800 0.000000 +0.668954 0.743303 0.000000 +0.668954 0.743303 0.000000 +0.748796 0.662800 0.000000 +0.668954 0.743303 0.000000 +0.748796 0.662800 0.000000 +0.668954 0.743303 0.000000 +0.585044 0.811002 0.000000 +0.585044 0.811002 0.000000 +0.668954 0.743303 0.000000 +0.585044 0.811002 0.000000 +0.668954 0.743303 0.000000 +0.585044 0.811002 0.000000 +0.494134 0.869386 0.000000 +0.494134 0.869386 0.000000 +0.585044 0.811002 0.000000 +0.494134 0.869386 0.000000 +0.585044 0.811002 0.000000 +0.494134 0.869386 0.000000 +0.396043 0.918232 0.000000 +0.396044 0.918232 0.000000 +0.494134 0.869386 0.000000 +0.396044 0.918232 0.000000 +0.494134 0.869386 0.000000 +0.396043 0.918232 0.000000 +0.294185 0.955749 0.000000 +0.294185 0.955749 0.000000 +0.396043 0.918232 0.000000 +0.294185 0.955749 0.000000 +0.396044 0.918232 0.000000 +0.294185 0.955749 0.000000 +0.192263 0.981344 0.000000 +0.192263 0.981343 0.000000 +0.294185 0.955749 0.000000 +0.192263 0.981343 0.000000 +0.294185 0.955749 0.000000 +0.192263 0.981344 0.000000 +0.093654 0.995605 0.000000 +0.093654 0.995605 0.000000 +0.192263 0.981344 0.000000 +0.093654 0.995605 0.000000 +0.192263 0.981343 0.000000 +0.093654 0.995605 0.000000 +0.000709 1.000000 0.000000 +0.000709 1.000000 0.000000 +0.093654 0.995605 0.000000 +0.000709 1.000000 0.000000 +0.093654 0.995605 0.000000 +0.000709 1.000000 0.000000 +-0.091045 0.995847 0.000000 +-0.091045 0.995847 0.000000 +0.000709 1.000000 0.000000 +-0.091045 0.995847 0.000000 +0.000709 1.000000 0.000000 +-0.091045 0.995847 0.000000 +-0.187743 0.982218 0.000000 +-0.187743 0.982218 0.000000 +-0.091045 0.995847 0.000000 +-0.187743 0.982218 0.000000 +-0.091045 0.995847 0.000000 +-0.187743 0.982218 0.000000 +-0.288823 0.957383 0.000000 +-0.288823 0.957383 0.000000 +-0.187743 0.982218 0.000000 +-0.288823 0.957383 0.000000 +-0.187743 0.982218 0.000000 +-0.288823 0.957383 0.000000 +-0.391077 0.920358 0.000000 +-0.391077 0.920358 0.000000 +-0.288823 0.957383 0.000000 +-0.391077 0.920358 0.000000 +-0.288823 0.957383 0.000000 +-0.391077 0.920358 0.000000 +-0.490731 0.871311 0.000000 +-0.490731 0.871311 0.000000 +-0.391077 0.920358 0.000000 +-0.490731 0.871311 0.000000 +-0.391077 0.920358 0.000000 +-0.490731 0.871311 0.000000 +-0.584051 0.811717 0.000000 +-0.584051 0.811717 0.000000 +-0.490731 0.871311 0.000000 +-0.584051 0.811717 0.000000 +-0.490731 0.871311 0.000000 +-0.584051 0.811717 0.000000 +-0.668380 0.743820 0.000000 +-0.668380 0.743820 0.000000 +-0.584051 0.811717 0.000000 +-0.668380 0.743820 0.000000 +-0.584051 0.811717 0.000000 +-0.668380 0.743820 0.000000 +-0.748734 0.662870 0.000000 +-0.748734 0.662870 0.000000 +-0.668380 0.743820 0.000000 +-0.748734 0.662870 0.000000 +-0.668380 0.743820 0.000000 +-0.748734 0.662870 0.000000 +-0.825121 0.564956 0.000000 +-0.825121 0.564956 0.000000 +-0.748734 0.662870 0.000000 +-0.825121 0.564956 0.000000 +-0.748734 0.662870 0.000000 +-0.825121 0.564956 0.000000 +-0.890398 0.455183 0.000000 +-0.890398 0.455183 0.000000 +-0.825121 0.564956 0.000000 +-0.890398 0.455183 0.000000 +-0.825121 0.564956 0.000000 +-0.890398 0.455183 0.000000 +-0.940988 0.338441 0.000000 +-0.940988 0.338441 0.000000 +-0.890398 0.455183 0.000000 +-0.940988 0.338441 0.000000 +-0.890398 0.455183 0.000000 +-0.940988 0.338441 0.000000 +-0.975381 0.220524 0.000000 +-0.975381 0.220524 0.000000 +-0.940988 0.338441 0.000000 +-0.975381 0.220524 0.000000 +-0.940988 0.338441 0.000000 +-0.975381 0.220524 0.000000 +-0.994278 0.106824 0.000000 +-0.994278 0.106824 0.000000 +-0.975381 0.220524 0.000000 +-0.994278 0.106824 0.000000 +-0.975381 0.220524 0.000000 +-0.994278 0.106824 0.000000 +-0.999995 0.003117 0.000000 +-0.999995 0.003117 0.000000 +-0.994278 0.106824 0.000000 +-0.999995 0.003117 0.000000 +-0.994278 0.106824 0.000000 +-0.999995 0.003117 0.000000 +-0.995698 -0.092663 0.000000 +-0.995698 -0.092663 0.000000 +-0.999995 0.003117 0.000000 +-0.995698 -0.092663 0.000000 +-0.999995 0.003117 0.000000 +-0.995698 -0.092663 0.000000 +-0.982030 -0.188727 0.000000 +-0.982030 -0.188727 0.000000 +-0.995698 -0.092663 0.000000 +-0.982030 -0.188727 0.000000 +-0.995698 -0.092663 0.000000 +-0.982030 -0.188727 0.000000 +-0.958111 -0.286398 0.000000 +-0.958111 -0.286398 0.000000 +-0.982030 -0.188727 0.000000 +-0.958111 -0.286398 0.000000 +-0.982030 -0.188727 0.000000 +-0.958111 -0.286398 0.000000 +-0.923881 -0.382681 0.000000 +-0.923881 -0.382681 0.000000 +-0.958111 -0.286398 0.000000 +-0.923881 -0.382681 0.000000 +-0.958111 -0.286398 0.000000 +-0.923881 -0.382681 0.000000 +-0.880220 -0.474566 0.000000 +-0.880220 -0.474566 0.000000 +-0.923881 -0.382681 0.000000 +-0.880220 -0.474566 0.000000 +-0.923881 -0.382681 0.000000 +-0.880220 -0.474566 0.000000 +-0.828846 -0.559477 0.000000 +-0.828846 -0.559477 0.000000 +-0.880220 -0.474566 0.000000 +-0.828846 -0.559477 0.000000 +-0.880220 -0.474566 0.000000 +-0.828846 -0.559477 0.000000 +-0.769386 -0.638784 0.000000 +-0.769386 -0.638784 0.000000 +-0.828846 -0.559477 0.000000 +-0.769386 -0.638784 0.000000 +-0.828846 -0.559477 0.000000 +-0.769386 -0.638784 0.000000 +-0.704868 -0.709338 0.000000 +-0.704868 -0.709338 0.000000 +-0.769386 -0.638784 0.000000 +-0.704868 -0.709338 0.000000 +-0.769386 -0.638784 0.000000 +-0.704868 -0.709338 0.000000 +-0.637301 -0.770615 0.000000 +-0.637301 -0.770615 0.000000 +-0.704868 -0.709338 0.000000 +-0.637301 -0.770615 0.000000 +-0.704868 -0.709338 0.000000 +-0.637301 -0.770615 0.000000 +-0.563845 -0.825881 0.000000 +-0.563845 -0.825881 0.000000 +-0.637301 -0.770615 0.000000 +-0.563845 -0.825881 0.000000 +-0.637301 -0.770615 0.000000 +-0.563845 -0.825881 0.000000 +-0.486340 -0.873770 0.000000 +-0.486340 -0.873770 0.000000 +-0.563845 -0.825881 0.000000 +-0.486340 -0.873770 0.000000 +-0.563845 -0.825881 0.000000 +-0.486340 -0.873770 0.000000 +-0.406872 -0.913485 0.000000 +-0.406872 -0.913485 0.000000 +-0.486340 -0.873770 0.000000 +-0.406872 -0.913485 0.000000 +-0.486340 -0.873770 0.000000 +-0.406872 -0.913485 0.000000 +-0.327515 -0.944846 0.000000 +-0.327515 -0.944846 0.000000 +-0.406872 -0.913485 0.000000 +-0.327515 -0.944846 0.000000 +-0.406872 -0.913485 0.000000 +-0.327515 -0.944846 0.000000 +-0.287881 -0.957666 0.000000 +-0.287881 -0.957666 0.000000 +-0.327515 -0.944846 0.000000 +-0.287881 -0.957666 0.000000 +-0.327515 -0.944846 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +-0.994704 0.102782 0.000000 +-0.977544 0.210731 0.000000 +-0.977544 0.210731 0.000000 +-0.994704 0.102782 0.000000 +-0.977544 0.210731 0.000000 +-0.994704 0.102782 0.000000 +-0.977544 0.210731 0.000000 +-0.946951 0.321379 0.000000 +-0.946951 0.321379 0.000000 +-0.977544 0.210731 0.000000 +-0.946951 0.321379 0.000000 +-0.977544 0.210731 0.000000 +-0.946951 0.321379 0.000000 +-0.902688 0.430296 0.000000 +-0.902688 0.430296 0.000000 +-0.946951 0.321379 0.000000 +-0.902688 0.430296 0.000000 +-0.946951 0.321379 0.000000 +-0.902688 0.430296 0.000000 +-0.846152 0.532942 0.000000 +-0.846152 0.532942 0.000000 +-0.902688 0.430296 0.000000 +-0.846152 0.532942 0.000000 +-0.902688 0.430296 0.000000 +-0.846152 0.532942 0.000000 +-0.780172 0.625565 0.000000 +-0.780172 0.625565 0.000000 +-0.846152 0.532942 0.000000 +-0.780172 0.625565 0.000000 +-0.846152 0.532942 0.000000 +-0.780172 0.625565 0.000000 +-0.707670 0.706543 0.000000 +-0.707670 0.706543 0.000000 +-0.780172 0.625565 0.000000 +-0.707670 0.706543 0.000000 +-0.780172 0.625565 0.000000 +-0.707670 0.706543 0.000000 +-0.626771 0.779204 0.000000 +-0.626771 0.779203 0.000000 +-0.707670 0.706543 0.000000 +-0.626771 0.779203 0.000000 +-0.707670 0.706543 0.000000 +-0.626771 0.779204 0.000000 +-0.534131 0.845402 0.000000 +-0.534131 0.845402 0.000000 +-0.626771 0.779204 0.000000 +-0.534131 0.845402 0.000000 +-0.626771 0.779203 0.000000 +-0.534131 0.845402 0.000000 +-0.431372 0.902174 0.000000 +-0.431372 0.902174 0.000000 +-0.534131 0.845402 0.000000 +-0.431372 0.902174 0.000000 +-0.534131 0.845402 0.000000 +-0.431372 0.902174 0.000000 +-0.322253 0.946654 0.000000 +-0.322253 0.946654 0.000000 +-0.431372 0.902174 0.000000 +-0.322253 0.946654 0.000000 +-0.431372 0.902174 0.000000 +-0.322253 0.946654 0.000000 +-0.211335 0.977414 0.000000 +-0.211335 0.977414 0.000000 +-0.322253 0.946654 0.000000 +-0.211335 0.977414 0.000000 +-0.322253 0.946654 0.000000 +-0.211335 0.977414 0.000000 +-0.103087 0.994672 0.000000 +-0.103087 0.994672 0.000000 +-0.211335 0.977414 0.000000 +-0.103087 0.994672 0.000000 +-0.211335 0.977414 0.000000 +-0.103087 0.994672 0.000000 +0.000231 1.000000 0.000000 +0.000231 1.000000 0.000000 +-0.103087 0.994672 0.000000 +0.000231 1.000000 0.000000 +-0.103087 0.994672 0.000000 +0.000231 1.000000 0.000000 +0.104005 0.994577 0.000000 +0.104005 0.994577 0.000000 +0.000231 1.000000 0.000000 +0.104005 0.994577 0.000000 +0.000231 1.000000 0.000000 +0.104005 0.994577 0.000000 +0.213104 0.977030 0.000000 +0.213104 0.977030 0.000000 +0.104005 0.994577 0.000000 +0.213104 0.977030 0.000000 +0.104005 0.994577 0.000000 +0.213104 0.977030 0.000000 +0.324695 0.945819 0.000000 +0.324695 0.945819 0.000000 +0.213104 0.977030 0.000000 +0.324695 0.945819 0.000000 +0.213104 0.977030 0.000000 +0.324695 0.945819 0.000000 +0.434216 0.900809 0.000000 +0.434216 0.900809 0.000000 +0.324695 0.945819 0.000000 +0.434216 0.900809 0.000000 +0.324695 0.945819 0.000000 +0.434216 0.900809 0.000000 +0.537069 0.843538 0.000000 +0.537069 0.843538 0.000000 +0.434216 0.900809 0.000000 +0.537069 0.843538 0.000000 +0.434216 0.900809 0.000000 +0.537069 0.843538 0.000000 +0.629534 0.776973 0.000000 +0.629533 0.776973 0.000000 +0.537069 0.843538 0.000000 +0.629533 0.776973 0.000000 +0.537069 0.843538 0.000000 +0.629534 0.776973 0.000000 +0.710111 0.704090 0.000000 +0.710111 0.704090 0.000000 +0.629534 0.776973 0.000000 +0.710111 0.704090 0.000000 +0.629533 0.776973 0.000000 +0.710111 0.704090 0.000000 +0.782238 0.622980 0.000000 +0.782238 0.622980 0.000000 +0.710111 0.704090 0.000000 +0.782238 0.622980 0.000000 +0.710111 0.704090 0.000000 +0.782238 0.622980 0.000000 +0.847775 0.530355 0.000000 +0.847775 0.530355 0.000000 +0.782238 0.622980 0.000000 +0.847775 0.530355 0.000000 +0.782238 0.622980 0.000000 +0.847775 0.530355 0.000000 +0.903816 0.427920 0.000000 +0.903816 0.427920 0.000000 +0.847775 0.530355 0.000000 +0.903816 0.427920 0.000000 +0.847775 0.530355 0.000000 +0.903816 0.427920 0.000000 +0.947610 0.319429 0.000000 +0.947610 0.319429 0.000000 +0.903816 0.427920 0.000000 +0.947610 0.319429 0.000000 +0.903816 0.427920 0.000000 +0.947610 0.319429 0.000000 +0.977837 0.209369 0.000000 +0.977837 0.209369 0.000000 +0.947610 0.319429 0.000000 +0.977837 0.209369 0.000000 +0.947610 0.319429 0.000000 +0.977837 0.209369 0.000000 +0.994775 0.102095 0.000000 +0.994775 0.102095 0.000000 +0.977837 0.209369 0.000000 +0.994775 0.102095 0.000000 +0.977837 0.209369 0.000000 +0.994775 0.102095 0.000000 +1.000000 0.000010 0.000000 +1.000000 0.000010 0.000000 +0.994775 0.102095 0.000000 +1.000000 0.000010 0.000000 +0.994775 0.102095 0.000000 +1.000000 0.000010 0.000000 +0.994778 -0.102060 0.000000 +0.994778 -0.102060 0.000000 +1.000000 0.000010 0.000000 +0.994778 -0.102060 0.000000 +1.000000 0.000010 0.000000 +0.994778 -0.102060 0.000000 +0.977848 -0.209316 0.000000 +0.977848 -0.209316 0.000000 +0.994778 -0.102060 0.000000 +0.977848 -0.209316 0.000000 +0.994778 -0.102060 0.000000 +0.977848 -0.209316 0.000000 +0.947627 -0.319380 0.000000 +0.947627 -0.319380 0.000000 +0.977848 -0.209316 0.000000 +0.947627 -0.319380 0.000000 +0.977848 -0.209316 0.000000 +0.947627 -0.319380 0.000000 +0.903829 -0.427895 0.000000 +0.903829 -0.427895 0.000000 +0.947627 -0.319380 0.000000 +0.903829 -0.427895 0.000000 +0.947627 -0.319380 0.000000 +0.903829 -0.427895 0.000000 +0.847765 -0.530372 0.000000 +0.847765 -0.530372 0.000000 +0.903829 -0.427895 0.000000 +0.847765 -0.530372 0.000000 +0.903829 -0.427895 0.000000 +0.847765 -0.530372 0.000000 +0.782184 -0.623048 0.000000 +0.782184 -0.623048 0.000000 +0.847765 -0.530372 0.000000 +0.782184 -0.623048 0.000000 +0.847765 -0.530372 0.000000 +0.782184 -0.623048 0.000000 +0.710012 -0.704190 0.000000 +0.710012 -0.704190 0.000000 +0.782184 -0.623048 0.000000 +0.710012 -0.704190 0.000000 +0.782184 -0.623048 0.000000 +0.710012 -0.704190 0.000000 +0.629412 -0.777072 0.000000 +0.629412 -0.777072 0.000000 +0.710012 -0.704190 0.000000 +0.629412 -0.777072 0.000000 +0.710012 -0.704190 0.000000 +0.629412 -0.777072 0.000000 +0.536947 -0.843616 0.000000 +0.536947 -0.843616 0.000000 +0.629412 -0.777072 0.000000 +0.536947 -0.843616 0.000000 +0.629412 -0.777072 0.000000 +0.536947 -0.843616 0.000000 +0.434100 -0.900865 0.000000 +0.434100 -0.900865 0.000000 +0.536947 -0.843616 0.000000 +0.434100 -0.900865 0.000000 +0.536947 -0.843616 0.000000 +0.434100 -0.900865 0.000000 +0.324602 -0.945851 0.000000 +0.324602 -0.945851 0.000000 +0.434100 -0.900865 0.000000 +0.324602 -0.945851 0.000000 +0.434100 -0.900865 0.000000 +0.324602 -0.945851 0.000000 +0.213043 -0.977043 0.000000 +0.213043 -0.977043 0.000000 +0.324602 -0.945851 0.000000 +0.213043 -0.977043 0.000000 +0.324602 -0.945851 0.000000 +0.213043 -0.977043 0.000000 +0.103971 -0.994580 0.000000 +0.103971 -0.994580 0.000000 +0.213043 -0.977043 0.000000 +0.103971 -0.994580 0.000000 +0.213043 -0.977043 0.000000 +0.103971 -0.994580 0.000000 +0.000230 -1.000000 0.000000 +0.000230 -1.000000 0.000000 +0.103971 -0.994580 0.000000 +0.000230 -1.000000 0.000000 +0.103971 -0.994580 0.000000 +0.000230 -1.000000 0.000000 +-0.103055 -0.994676 0.000000 +-0.103055 -0.994676 0.000000 +0.000230 -1.000000 0.000000 +-0.103055 -0.994676 0.000000 +0.000230 -1.000000 0.000000 +-0.103055 -0.994676 0.000000 +-0.211273 -0.977427 0.000000 +-0.211273 -0.977427 0.000000 +-0.103055 -0.994676 0.000000 +-0.211273 -0.977427 0.000000 +-0.103055 -0.994676 0.000000 +-0.211273 -0.977427 0.000000 +-0.322158 -0.946686 0.000000 +-0.322158 -0.946686 0.000000 +-0.211273 -0.977427 0.000000 +-0.322158 -0.946686 0.000000 +-0.211273 -0.977427 0.000000 +-0.322158 -0.946686 0.000000 +-0.431258 -0.902229 0.000000 +-0.431258 -0.902229 0.000000 +-0.322158 -0.946686 0.000000 +-0.431258 -0.902229 0.000000 +-0.322158 -0.946686 0.000000 +-0.431258 -0.902229 0.000000 +-0.534009 -0.845479 0.000000 +-0.534009 -0.845479 0.000000 +-0.431258 -0.902229 0.000000 +-0.534009 -0.845479 0.000000 +-0.431258 -0.902229 0.000000 +-0.534009 -0.845479 0.000000 +-0.626649 -0.779302 0.000000 +-0.626649 -0.779302 0.000000 +-0.534009 -0.845479 0.000000 +-0.626649 -0.779302 0.000000 +-0.534009 -0.845479 0.000000 +-0.626649 -0.779302 0.000000 +-0.707571 -0.706642 0.000000 +-0.707571 -0.706642 0.000000 +-0.626649 -0.779302 0.000000 +-0.707571 -0.706642 0.000000 +-0.626649 -0.779302 0.000000 +-0.707571 -0.706642 0.000000 +-0.780118 -0.625632 0.000000 +-0.780118 -0.625632 0.000000 +-0.707571 -0.706642 0.000000 +-0.780118 -0.625632 0.000000 +-0.707571 -0.706642 0.000000 +-0.780118 -0.625632 0.000000 +-0.846141 -0.532959 0.000000 +-0.846141 -0.532960 0.000000 +-0.780118 -0.625632 0.000000 +-0.846141 -0.532960 0.000000 +-0.780118 -0.625632 0.000000 +-0.846141 -0.532959 0.000000 +-0.902699 -0.430272 0.000000 +-0.902699 -0.430272 0.000000 +-0.846141 -0.532959 0.000000 +-0.902699 -0.430272 0.000000 +-0.846141 -0.532960 0.000000 +-0.902699 -0.430272 0.000000 +-0.946967 -0.321330 0.000000 +-0.946967 -0.321330 0.000000 +-0.902699 -0.430272 0.000000 +-0.946967 -0.321330 0.000000 +-0.902699 -0.430272 0.000000 +-0.946967 -0.321330 0.000000 +-0.977556 -0.210678 0.000000 +-0.977556 -0.210678 0.000000 +-0.946967 -0.321330 0.000000 +-0.977556 -0.210678 0.000000 +-0.946967 -0.321330 0.000000 +-0.977556 -0.210678 0.000000 +-0.994707 -0.102748 0.000000 +-0.994707 -0.102748 0.000000 +-0.977556 -0.210678 0.000000 +-0.994707 -0.102748 0.000000 +-0.977556 -0.210678 0.000000 +-0.994707 -0.102748 0.000000 +-1.000000 0.000010 0.000000 +-1.000000 0.000010 0.000000 +-0.994707 -0.102748 0.000000 +-1.000000 0.000010 0.000000 +-0.994707 -0.102748 0.000000 +-1.000000 0.000010 0.000000 +-0.994704 0.102782 0.000000 +-0.994704 0.102782 0.000000 +-1.000000 0.000010 0.000000 +-0.994704 0.102782 0.000000 +-1.000000 0.000010 0.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 + + + + + + + + + + + + +

0 0 1 1 68 2 0 3 68 4 67 5 1 6 2 7 69 8 1 9 69 10 68 11 2 12 3 13 70 14 2 15 70 16 69 17 3 18 4 19 71 20 3 21 71 22 70 23 4 24 5 25 72 26 4 27 72 28 71 29 5 30 6 31 73 32 5 33 73 34 72 35 6 36 7 37 74 38 6 39 74 40 73 41 7 42 8 43 75 44 7 45 75 46 74 47 8 48 9 49 76 50 8 51 76 52 75 53 9 54 10 55 77 56 9 57 77 58 76 59 10 60 11 61 78 62 10 63 78 64 77 65 11 66 12 67 79 68 11 69 79 70 78 71 12 72 13 73 80 74 12 75 80 76 79 77 13 78 14 79 81 80 13 81 81 82 80 83 14 84 15 85 82 86 14 87 82 88 81 89 15 90 16 91 83 92 15 93 83 94 82 95 16 96 17 97 84 98 16 99 84 100 83 101 17 102 18 103 85 104 17 105 85 106 84 107 18 108 19 109 86 110 18 111 86 112 85 113 19 114 20 115 87 116 19 117 87 118 86 119 20 120 21 121 88 122 20 123 88 124 87 125 21 126 22 127 89 128 21 129 89 130 88 131 22 132 23 133 90 134 22 135 90 136 89 137 23 138 24 139 91 140 23 141 91 142 90 143 24 144 25 145 92 146 24 147 92 148 91 149 25 150 26 151 93 152 25 153 93 154 92 155 26 156 27 157 94 158 26 159 94 160 93 161 27 162 28 163 95 164 27 165 95 166 94 167 28 168 29 169 96 170 28 171 96 172 95 173 29 174 30 175 97 176 29 177 97 178 96 179 30 180 31 181 98 182 30 183 98 184 97 185 31 186 32 187 99 188 31 189 99 190 98 191 32 192 33 193 100 194 32 195 100 196 99 197 33 198 34 199 101 200 33 201 101 202 100 203 34 204 35 205 102 206 34 207 102 208 101 209 35 210 36 211 103 212 35 213 103 214 102 215 36 216 37 217 104 218 36 219 104 220 103 221 37 222 38 223 105 224 37 225 105 226 104 227 38 228 39 229 106 230 38 231 106 232 105 233 39 234 40 235 107 236 39 237 107 238 106 239 40 240 41 241 108 242 40 243 108 244 107 245 41 246 42 247 109 248 41 249 109 250 108 251 42 252 43 253 110 254 42 255 110 256 109 257 43 258 44 259 111 260 43 261 111 262 110 263 44 264 45 265 112 266 44 267 112 268 111 269 45 270 46 271 113 272 45 273 113 274 112 275 46 276 47 277 114 278 46 279 114 280 113 281 47 282 48 283 115 284 47 285 115 286 114 287 48 288 49 289 116 290 48 291 116 292 115 293 49 294 50 295 117 296 49 297 117 298 116 299 50 300 51 301 118 302 50 303 118 304 117 305 51 306 52 307 119 308 51 309 119 310 118 311 52 312 53 313 120 314 52 315 120 316 119 317 53 318 54 319 121 320 53 321 121 322 120 323 54 324 55 325 122 326 54 327 122 328 121 329 55 330 56 331 123 332 55 333 123 334 122 335 56 336 57 337 124 338 56 339 124 340 123 341 57 342 58 343 125 344 57 345 125 346 124 347 58 348 59 349 126 350 58 351 126 352 125 353 59 354 60 355 127 356 59 357 127 358 126 359 60 360 61 361 128 362 60 363 128 364 127 365 61 366 62 367 129 368 61 369 129 370 128 371 62 372 63 373 130 374 62 375 130 376 129 377 63 378 64 379 131 380 63 381 131 382 130 383 64 384 65 385 132 386 64 387 132 388 131 389 65 390 66 391 133 392 65 393 133 394 132 395 66 396 0 397 67 398 66 399 67 400 133 401 134 402 135 403 191 404 134 405 191 406 190 407 135 408 136 409 192 410 135 411 192 412 191 413 136 414 137 415 193 416 136 417 193 418 192 419 137 420 138 421 194 422 137 423 194 424 193 425 138 426 139 427 195 428 138 429 195 430 194 431 139 432 140 433 196 434 139 435 196 436 195 437 140 438 141 439 197 440 140 441 197 442 196 443 141 444 142 445 198 446 141 447 198 448 197 449 142 450 143 451 199 452 142 453 199 454 198 455 143 456 144 457 200 458 143 459 200 460 199 461 144 462 145 463 201 464 144 465 201 466 200 467 145 468 146 469 202 470 145 471 202 472 201 473 146 474 147 475 203 476 146 477 203 478 202 479 147 480 148 481 204 482 147 483 204 484 203 485 148 486 149 487 205 488 148 489 205 490 204 491 149 492 150 493 206 494 149 495 206 496 205 497 150 498 151 499 207 500 150 501 207 502 206 503 151 504 152 505 208 506 151 507 208 508 207 509 152 510 153 511 209 512 152 513 209 514 208 515 153 516 154 517 210 518 153 519 210 520 209 521 154 522 155 523 211 524 154 525 211 526 210 527 155 528 156 529 212 530 155 531 212 532 211 533 156 534 157 535 213 536 156 537 213 538 212 539 157 540 158 541 214 542 157 543 214 544 213 545 158 546 159 547 215 548 158 549 215 550 214 551 159 552 160 553 216 554 159 555 216 556 215 557 160 558 161 559 217 560 160 561 217 562 216 563 161 564 162 565 218 566 161 567 218 568 217 569 162 570 163 571 219 572 162 573 219 574 218 575 163 576 164 577 220 578 163 579 220 580 219 581 164 582 165 583 221 584 164 585 221 586 220 587 165 588 166 589 222 590 165 591 222 592 221 593 166 594 167 595 223 596 166 597 223 598 222 599 167 600 168 601 224 602 167 603 224 604 223 605 168 606 169 607 225 608 168 609 225 610 224 611 169 612 170 613 226 614 169 615 226 616 225 617 170 618 171 619 227 620 170 621 227 622 226 623 171 624 172 625 228 626 171 627 228 628 227 629 172 630 173 631 229 632 172 633 229 634 228 635 173 636 174 637 230 638 173 639 230 640 229 641 174 642 175 643 231 644 174 645 231 646 230 647 175 648 176 649 232 650 175 651 232 652 231 653 176 654 177 655 233 656 176 657 233 658 232 659 177 660 178 661 234 662 177 663 234 664 233 665 178 666 179 667 235 668 178 669 235 670 234 671 179 672 180 673 236 674 179 675 236 676 235 677 180 678 181 679 237 680 180 681 237 682 236 683 181 684 182 685 238 686 181 687 238 688 237 689 182 690 183 691 239 692 182 693 239 694 238 695 183 696 184 697 240 698 183 699 240 700 239 701 184 702 185 703 241 704 184 705 241 706 240 707 185 708 186 709 242 710 185 711 242 712 241 713 186 714 187 715 243 716 186 717 243 718 242 719 187 720 188 721 244 722 187 723 244 724 243 725 188 726 189 727 245 728 188 729 245 730 244 731 189 732 134 733 190 734 189 735 190 736 245 737 147 738 2 739 1 740 146 741 2 742 147 743 145 744 2 745 146 746 145 747 3 748 2 749 144 750 3 751 145 752 144 753 4 754 3 755 143 756 4 757 144 758 143 759 5 760 4 761 142 762 5 763 143 764 142 765 6 766 5 767 141 768 6 769 142 770 141 771 7 772 6 773 140 774 7 775 141 776 140 777 8 778 7 779 65 780 0 781 66 782 65 783 1 784 0 785 63 786 65 787 64 788 62 789 65 790 63 791 62 792 1 793 65 794 60 795 62 796 61 797 59 798 62 799 60 800 59 801 1 802 62 803 59 804 147 805 1 806 58 807 147 808 59 809 58 810 148 811 147 812 58 813 149 814 148 815 58 816 150 817 149 818 57 819 150 820 58 821 57 822 151 823 150 824 56 825 151 826 57 827 56 828 152 829 151 830 55 831 152 832 56 833 139 834 8 835 140 836 139 837 9 838 8 839 139 840 10 841 9 842 138 843 10 844 139 845 138 846 11 847 10 848 137 849 11 850 138 851 137 852 12 853 11 854 136 855 12 856 137 857 136 858 13 859 12 860 135 861 13 862 136 863 135 864 14 865 13 866 135 867 15 868 14 869 134 870 15 871 135 872 134 873 16 874 15 875 189 876 16 877 134 878 189 879 17 880 16 881 188 882 17 883 189 884 188 885 18 886 17 887 187 888 18 889 188 890 187 891 19 892 18 893 186 894 19 895 187 896 186 897 20 898 19 899 185 900 20 901 186 902 185 903 21 904 20 905 184 906 21 907 185 908 184 909 22 910 21 911 183 912 22 913 184 914 183 915 23 916 22 917 182 918 23 919 183 920 182 921 24 922 23 923 181 924 24 925 182 926 181 927 25 928 24 929 180 930 25 931 181 932 180 933 26 934 25 935 179 936 26 937 180 938 179 939 27 940 26 941 178 942 27 943 179 944 178 945 28 946 27 947 177 948 28 949 178 950 177 951 29 952 28 953 176 954 29 955 177 956 176 957 30 958 29 959 175 960 30 961 176 962 175 963 31 964 30 965 174 966 31 967 175 968 174 969 32 970 31 971 173 972 32 973 174 974 172 975 32 976 173 977 172 978 33 979 32 980 171 981 33 982 172 983 171 984 34 985 33 986 170 987 34 988 171 989 170 990 35 991 34 992 169 993 35 994 170 995 169 996 36 997 35 998 168 999 36 1000 169 1001 168 1002 37 1003 36 1004 167 1005 37 1006 168 1007 167 1008 38 1009 37 1010 166 1011 38 1012 167 1013 166 1014 39 1015 38 1016 166 1017 40 1018 39 1019 165 1020 40 1021 166 1022 165 1023 41 1024 40 1025 164 1026 41 1027 165 1028 164 1029 42 1030 41 1031 163 1032 42 1033 164 1034 163 1035 43 1036 42 1037 162 1038 43 1039 163 1040 162 1041 44 1042 43 1043 161 1044 44 1045 162 1046 161 1047 45 1048 44 1049 160 1050 45 1051 161 1052 160 1053 46 1054 45 1055 159 1056 46 1057 160 1058 159 1059 47 1060 46 1061 158 1062 47 1063 159 1064 158 1065 48 1066 47 1067 158 1068 49 1069 48 1070 157 1071 49 1072 158 1073 157 1074 50 1075 49 1076 156 1077 50 1078 157 1079 156 1080 51 1081 50 1082 155 1083 51 1084 156 1085 155 1086 52 1087 51 1088 154 1089 52 1090 155 1091 154 1092 53 1093 52 1094 154 1095 54 1096 53 1097 153 1098 54 1099 154 1100 153 1101 55 1102 54 1103 153 1104 152 1105 55 1106 203 1107 68 1108 69 1109 202 1110 203 1111 69 1112 201 1113 202 1114 69 1115 201 1116 69 1117 70 1118 200 1119 201 1120 70 1121 200 1122 70 1123 71 1124 199 1125 200 1126 71 1127 199 1128 71 1129 72 1130 198 1131 199 1132 72 1133 198 1134 72 1135 73 1136 197 1137 198 1138 73 1139 197 1140 73 1141 74 1142 196 1143 197 1144 74 1145 196 1146 74 1147 75 1148 132 1149 133 1150 67 1151 132 1152 67 1153 68 1154 130 1155 131 1156 132 1157 129 1158 130 1159 132 1160 129 1161 132 1162 68 1163 127 1164 128 1165 129 1166 126 1167 127 1168 129 1169 126 1170 129 1171 68 1172 126 1173 68 1174 203 1175 125 1176 126 1177 203 1178 125 1179 203 1180 204 1181 125 1182 204 1183 205 1184 125 1185 205 1186 206 1187 124 1188 125 1189 206 1190 124 1191 206 1192 207 1193 123 1194 124 1195 207 1196 123 1197 207 1198 208 1199 122 1200 123 1201 208 1202 195 1203 196 1204 75 1205 195 1206 75 1207 76 1208 195 1209 76 1210 77 1211 194 1212 195 1213 77 1214 194 1215 77 1216 78 1217 193 1218 194 1219 78 1220 193 1221 78 1222 79 1223 192 1224 193 1225 79 1226 192 1227 79 1228 80 1229 191 1230 192 1231 80 1232 191 1233 80 1234 81 1235 191 1236 81 1237 82 1238 190 1239 191 1240 82 1241 190 1242 82 1243 83 1244 245 1245 190 1246 83 1247 245 1248 83 1249 84 1250 244 1251 245 1252 84 1253 244 1254 84 1255 85 1256 243 1257 244 1258 85 1259 243 1260 85 1261 86 1262 242 1263 243 1264 86 1265 242 1266 86 1267 87 1268 241 1269 242 1270 87 1271 241 1272 87 1273 88 1274 240 1275 241 1276 88 1277 240 1278 88 1279 89 1280 239 1281 240 1282 89 1283 239 1284 89 1285 90 1286 238 1287 239 1288 90 1289 238 1290 90 1291 91 1292 237 1293 238 1294 91 1295 237 1296 91 1297 92 1298 236 1299 237 1300 92 1301 236 1302 92 1303 93 1304 235 1305 236 1306 93 1307 235 1308 93 1309 94 1310 234 1311 235 1312 94 1313 234 1314 94 1315 95 1316 233 1317 234 1318 95 1319 233 1320 95 1321 96 1322 232 1323 233 1324 96 1325 232 1326 96 1327 97 1328 231 1329 232 1330 97 1331 231 1332 97 1333 98 1334 230 1335 231 1336 98 1337 230 1338 98 1339 99 1340 229 1341 230 1342 99 1343 228 1344 229 1345 99 1346 228 1347 99 1348 100 1349 227 1350 228 1351 100 1352 227 1353 100 1354 101 1355 226 1356 227 1357 101 1358 226 1359 101 1360 102 1361 225 1362 226 1363 102 1364 225 1365 102 1366 103 1367 224 1368 225 1369 103 1370 224 1371 103 1372 104 1373 223 1374 224 1375 104 1376 223 1377 104 1378 105 1379 222 1380 223 1381 105 1382 222 1383 105 1384 106 1385 222 1386 106 1387 107 1388 221 1389 222 1390 107 1391 221 1392 107 1393 108 1394 220 1395 221 1396 108 1397 220 1398 108 1399 109 1400 219 1401 220 1402 109 1403 219 1404 109 1405 110 1406 218 1407 219 1408 110 1409 218 1410 110 1411 111 1412 217 1413 218 1414 111 1415 217 1416 111 1417 112 1418 216 1419 217 1420 112 1421 216 1422 112 1423 113 1424 215 1425 216 1426 113 1427 215 1428 113 1429 114 1430 214 1431 215 1432 114 1433 214 1434 114 1435 115 1436 214 1437 115 1438 116 1439 213 1440 214 1441 116 1442 213 1443 116 1444 117 1445 212 1446 213 1447 117 1448 212 1449 117 1450 118 1451 211 1452 212 1453 118 1454 211 1455 118 1456 119 1457 210 1458 211 1459 119 1460 210 1461 119 1462 120 1463 210 1464 120 1465 121 1466 209 1467 210 1468 121 1469 209 1470 121 1471 122 1472 209 1473 122 1474 208 1475

 + + + + + + + + 1.000000 1.000000 1.000000 + + + + + + + 1.000000 0.000000 0.000000 0.992424 0.000000 1.000000 0.000000 -23.446840 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 + + + + + + + + + diff --git a/messages/visualization/spencer_tracking_rviz_plugin/media/male_symbol.dae b/messages/visualization/spencer_tracking_rviz_plugin/media/male_symbol.dae new file mode 100755 index 0000000..adc9d6b --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/media/male_symbol.dae @@ -0,0 +1,1778 @@ + + + Z_UP + + + + + + + + + + + + 0.000000 0.000000 0.000000 1.000000 + + + 0.000000 1.000000 1.000000 1.000000 + + + 0.000000 1.000000 1.000000 1.000000 + + + 0.000000 0.000000 0.000000 1.000000 + + + 2.000000 + + + 0.000000 0.000000 0.000000 1.000000 + + + 1.000000 + + + 1.000000 1.000000 1.000000 1.000000 + + + 0.000000 + + + + + + + + + + + +26.607811 48.799999 0.000000 +1.921875 48.799999 0.000000 +1.921875 42.100002 0.000000 +14.070313 42.100002 0.000000 +-2.507813 25.515625 0.000000 +-3.563105 26.095154 0.000000 +-4.645057 26.585522 0.000000 +-5.753666 26.986734 0.000000 +-6.888935 27.298786 0.000000 +-8.050860 27.521687 0.000000 +-9.239446 27.655424 0.000000 +-10.454687 27.700001 0.000000 +-12.314348 27.602936 0.000000 +-14.096906 27.311735 0.000000 +-15.802360 26.826405 0.000000 +-17.430708 26.146940 0.000000 +-18.981955 25.273344 0.000000 +-20.456093 24.205614 0.000000 +-21.853127 22.943750 0.000000 +-23.114574 21.545570 0.000000 +-24.181952 20.068876 0.000000 +-25.055260 18.513681 0.000000 +-25.734503 16.879974 0.000000 +-26.219677 15.167763 0.000000 +-26.510777 13.377041 0.000000 +-26.607813 11.507813 0.000000 +-26.515434 9.704163 0.000000 +-26.238297 7.978922 0.000000 +-25.776403 6.332096 0.000000 +-25.129751 4.763681 0.000000 +-24.298346 3.273677 0.000000 +-23.282175 1.862086 0.000000 +-22.081249 0.528906 0.000000 +-20.748726 -0.672640 0.000000 +-19.337753 -1.689333 0.000000 +-17.848341 -2.521173 0.000000 +-16.280485 -3.168160 0.000000 +-14.634187 -3.630294 0.000000 +-12.909439 -3.907574 0.000000 +-11.106251 -4.000000 0.000000 +-9.237867 -3.902934 0.000000 +-7.447895 -3.611735 0.000000 +-5.736336 -3.126403 0.000000 +-4.103189 -2.446939 0.000000 +-2.548453 -1.573342 0.000000 +-1.072129 -0.505612 0.000000 +0.325783 0.756250 0.000000 +1.587437 2.153762 0.000000 +2.654991 3.628444 0.000000 +3.528444 5.180294 0.000000 +4.207796 6.809311 0.000000 +4.693048 8.515498 0.000000 +4.984200 10.298853 0.000000 +5.081249 12.159375 0.000000 +5.036702 13.384025 0.000000 +4.903060 14.582079 0.000000 +4.680325 15.753540 0.000000 +4.368495 16.898407 0.000000 +3.967570 18.016678 0.000000 +3.477551 19.108355 0.000000 +2.898438 20.173437 0.000000 +19.345314 36.654690 0.000000 +19.345314 24.600000 0.000000 +26.607811 24.600000 0.000000 +26.607811 48.799999 5.905512 +1.921875 48.799999 5.905512 +1.921875 42.100002 5.905512 +14.070313 42.100002 5.905512 +-2.507813 25.515625 5.905512 +-3.563105 26.095154 5.905512 +-4.645057 26.585522 5.905512 +-5.753666 26.986734 5.905512 +-6.888935 27.298786 5.905512 +-8.050860 27.521687 5.905512 +-9.239446 27.655424 5.905512 +-10.454687 27.700001 5.905512 +-12.314348 27.602936 5.905512 +-14.096906 27.311735 5.905512 +-15.802360 26.826405 5.905512 +-17.430708 26.146940 5.905512 +-18.981955 25.273344 5.905512 +-20.456093 24.205614 5.905512 +-21.853127 22.943750 5.905512 +-23.114574 21.545570 5.905512 +-24.181952 20.068876 5.905512 +-25.055260 18.513681 5.905512 +-25.734503 16.879974 5.905512 +-26.219677 15.167763 5.905512 +-26.510777 13.377041 5.905512 +-26.607813 11.507813 5.905512 +-26.515434 9.704163 5.905512 +-26.238297 7.978922 5.905512 +-25.776403 6.332096 5.905512 +-25.129751 4.763681 5.905512 +-24.298346 3.273677 5.905512 +-23.282175 1.862086 5.905512 +-22.081249 0.528906 5.905512 +-20.748726 -0.672640 5.905512 +-19.337753 -1.689333 5.905512 +-17.848341 -2.521173 5.905512 +-16.280485 -3.168160 5.905512 +-14.634187 -3.630294 5.905512 +-12.909439 -3.907574 5.905512 +-11.106251 -4.000000 5.905512 +-9.237867 -3.902934 5.905512 +-7.447895 -3.611735 5.905512 +-5.736336 -3.126403 5.905512 +-4.103189 -2.446939 5.905512 +-2.548453 -1.573342 5.905512 +-1.072129 -0.505612 5.905512 +0.325783 0.756250 5.905512 +1.587437 2.153762 5.905512 +2.654991 3.628444 5.905512 +3.528444 5.180294 5.905512 +4.207796 6.809311 5.905512 +4.693048 8.515498 5.905512 +4.984200 10.298853 5.905512 +5.081249 12.159375 5.905512 +5.036702 13.384025 5.905512 +4.903060 14.582079 5.905512 +4.680325 15.753540 5.905512 +4.368495 16.898407 5.905512 +3.967570 18.016678 5.905512 +3.477551 19.108355 5.905512 +2.898438 20.173437 5.905512 +19.345314 36.654690 5.905512 +19.345314 24.600000 5.905512 +26.607811 24.600000 5.905512 +-2.523852 10.888011 0.000000 +-2.670408 9.982843 0.000000 +-2.914669 9.118878 0.000000 +-3.256633 8.296111 0.000000 +-3.696301 7.514542 0.000000 +-4.233674 6.774171 0.000000 +-4.868750 6.075000 0.000000 +-5.572959 5.444899 0.000000 +-6.317729 4.911735 0.000000 +-7.103061 4.475511 0.000000 +-7.928954 4.136225 0.000000 +-8.795409 3.893878 0.000000 +-9.702424 3.748470 0.000000 +-10.650000 3.700000 0.000000 +-11.588871 3.748470 0.000000 +-12.488519 3.893878 0.000000 +-13.348947 4.136224 0.000000 +-14.170153 4.475510 0.000000 +-14.952137 4.911736 0.000000 +-15.694899 5.444899 0.000000 +-16.398438 6.075000 0.000000 +-17.033514 6.774171 0.000000 +-17.570885 7.514541 0.000000 +-18.010553 8.296110 0.000000 +-18.352520 9.118877 0.000000 +-18.596783 9.982845 0.000000 +-18.743338 10.888012 0.000000 +-18.792187 11.834375 0.000000 +-18.743336 12.780661 0.000000 +-18.596779 13.686480 0.000000 +-18.352520 14.551833 0.000000 +-18.010555 15.376724 0.000000 +-17.570887 16.161146 0.000000 +-17.033514 16.905104 0.000000 +-16.398438 17.608595 0.000000 +-15.694899 18.243050 0.000000 +-14.952135 18.779896 0.000000 +-14.170153 19.219135 0.000000 +-13.348948 19.560760 0.000000 +-12.488521 19.804787 0.000000 +-11.588871 19.951197 0.000000 +-10.650000 20.000000 0.000000 +-9.702424 19.951197 0.000000 +-8.795407 19.804781 0.000000 +-7.928954 19.560762 0.000000 +-7.103061 19.219133 0.000000 +-6.317729 18.779898 0.000000 +-5.572959 18.243050 0.000000 +-4.868750 17.608595 0.000000 +-4.233674 16.905104 0.000000 +-3.696301 16.161146 0.000000 +-3.256633 15.376723 0.000000 +-2.914669 14.551836 0.000000 +-2.670409 13.686481 0.000000 +-2.523852 12.780662 0.000000 +-2.475000 11.834375 0.000000 +-2.523852 10.888011 5.905512 +-2.670408 9.982843 5.905512 +-2.914669 9.118878 5.905512 +-3.256633 8.296111 5.905512 +-3.696301 7.514542 5.905512 +-4.233674 6.774171 5.905512 +-4.868750 6.075000 5.905512 +-5.572959 5.444899 5.905512 +-6.317729 4.911735 5.905512 +-7.103061 4.475511 5.905512 +-7.928954 4.136225 5.905512 +-8.795409 3.893878 5.905512 +-9.702424 3.748470 5.905512 +-10.650000 3.700000 5.905512 +-11.588871 3.748470 5.905512 +-12.488519 3.893878 5.905512 +-13.348947 4.136224 5.905512 +-14.170153 4.475510 5.905512 +-14.952137 4.911736 5.905512 +-15.694899 5.444899 5.905512 +-16.398438 6.075000 5.905512 +-17.033514 6.774171 5.905512 +-17.570885 7.514541 5.905512 +-18.010553 8.296110 5.905512 +-18.352520 9.118877 5.905512 +-18.596783 9.982845 5.905512 +-18.743338 10.888012 5.905512 +-18.792187 11.834375 5.905512 +-18.743336 12.780661 5.905512 +-18.596779 13.686480 5.905512 +-18.352520 14.551833 5.905512 +-18.010555 15.376724 5.905512 +-17.570887 16.161146 5.905512 +-17.033514 16.905104 5.905512 +-16.398438 17.608595 5.905512 +-15.694899 18.243050 5.905512 +-14.952135 18.779896 5.905512 +-14.170153 19.219135 5.905512 +-13.348948 19.560760 5.905512 +-12.488521 19.804787 5.905512 +-11.588871 19.951197 5.905512 +-10.650000 20.000000 5.905512 +-9.702424 19.951197 5.905512 +-8.795407 19.804781 5.905512 +-7.928954 19.560762 5.905512 +-7.103061 19.219133 5.905512 +-6.317729 18.779898 5.905512 +-5.572959 18.243050 5.905512 +-4.868750 17.608595 5.905512 +-4.233674 16.905104 5.905512 +-3.696301 16.161146 5.905512 +-3.256633 15.376723 5.905512 +-2.914669 14.551836 5.905512 +-2.670409 13.686481 5.905512 +-2.523852 12.780662 5.905512 +-2.475000 11.834375 5.905512 + + + + + + + + + + + +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +0.000000 1.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +-0.707240 0.706973 0.000000 +-0.707240 0.706973 0.000000 +-0.707240 0.706973 0.000000 +-0.707240 0.706973 0.000000 +-0.707240 0.706973 0.000000 +-0.707240 0.706973 0.000000 +0.481356 0.876525 0.000000 +0.447410 0.894329 0.000000 +0.447410 0.894329 0.000000 +0.481356 0.876525 0.000000 +0.447410 0.894329 0.000000 +0.481356 0.876525 0.000000 +0.447410 0.894329 0.000000 +0.376845 0.926276 0.000000 +0.376845 0.926276 0.000000 +0.447410 0.894329 0.000000 +0.376845 0.926276 0.000000 +0.447410 0.894329 0.000000 +0.376845 0.926276 0.000000 +0.302909 0.953019 0.000000 +0.302909 0.953019 0.000000 +0.376845 0.926276 0.000000 +0.302909 0.953019 0.000000 +0.376845 0.926276 0.000000 +0.302909 0.953019 0.000000 +0.226897 0.973919 0.000000 +0.226897 0.973919 0.000000 +0.302909 0.953019 0.000000 +0.226897 0.973919 0.000000 +0.302909 0.953019 0.000000 +0.226897 0.973919 0.000000 +0.150220 0.988653 0.000000 +0.150220 0.988653 0.000000 +0.226897 0.973919 0.000000 +0.150220 0.988653 0.000000 +0.226897 0.973919 0.000000 +0.150220 0.988653 0.000000 +0.074287 0.997237 0.000000 +0.074287 0.997237 0.000000 +0.150220 0.988653 0.000000 +0.074287 0.997237 0.000000 +0.150220 0.988653 0.000000 +0.074287 0.997237 0.000000 +-0.007741 0.999970 0.000000 +-0.007741 0.999970 0.000000 +0.074287 0.997237 0.000000 +-0.007741 0.999970 0.000000 +0.074287 0.997237 0.000000 +-0.007741 0.999970 0.000000 +-0.106835 0.994277 0.000000 +-0.106835 0.994277 0.000000 +-0.007741 0.999970 0.000000 +-0.106835 0.994277 0.000000 +-0.007741 0.999970 0.000000 +-0.106835 0.994277 0.000000 +-0.217828 0.975987 0.000000 +-0.217828 0.975987 0.000000 +-0.106835 0.994277 0.000000 +-0.217828 0.975987 0.000000 +-0.106835 0.994277 0.000000 +-0.217828 0.975987 0.000000 +-0.329975 0.943990 0.000000 +-0.329975 0.943990 0.000000 +-0.217828 0.975987 0.000000 +-0.329975 0.943990 0.000000 +-0.217828 0.975987 0.000000 +-0.329975 0.943990 0.000000 +-0.438652 0.898657 0.000000 +-0.438652 0.898657 0.000000 +-0.329975 0.943990 0.000000 +-0.438652 0.898657 0.000000 +-0.329975 0.943990 0.000000 +-0.438652 0.898657 0.000000 +-0.539524 0.841970 0.000000 +-0.539524 0.841970 0.000000 +-0.438652 0.898657 0.000000 +-0.539524 0.841970 0.000000 +-0.438652 0.898657 0.000000 +-0.539524 0.841970 0.000000 +-0.629360 0.777114 0.000000 +-0.629360 0.777114 0.000000 +-0.539524 0.841970 0.000000 +-0.629360 0.777114 0.000000 +-0.539524 0.841970 0.000000 +-0.629360 0.777114 0.000000 +-0.707309 0.706905 0.000000 +-0.707309 0.706905 0.000000 +-0.629360 0.777114 0.000000 +-0.707309 0.706905 0.000000 +-0.629360 0.777114 0.000000 +-0.707309 0.706905 0.000000 +-0.777602 0.628757 0.000000 +-0.777602 0.628757 0.000000 +-0.707309 0.706905 0.000000 +-0.777602 0.628757 0.000000 +-0.707309 0.706905 0.000000 +-0.777602 0.628757 0.000000 +-0.842565 0.538595 0.000000 +-0.842565 0.538595 0.000000 +-0.777602 0.628757 0.000000 +-0.842565 0.538595 0.000000 +-0.777602 0.628757 0.000000 +-0.842565 0.538595 0.000000 +-0.899207 0.437524 0.000000 +-0.899207 0.437524 0.000000 +-0.842565 0.538595 0.000000 +-0.899207 0.437524 0.000000 +-0.842565 0.538595 0.000000 +-0.899207 0.437524 0.000000 +-0.944386 0.328839 0.000000 +-0.944386 0.328839 0.000000 +-0.899207 0.437524 0.000000 +-0.944386 0.328839 0.000000 +-0.899207 0.437524 0.000000 +-0.944386 0.328839 0.000000 +-0.976194 0.216899 0.000000 +-0.976194 0.216899 0.000000 +-0.944386 0.328839 0.000000 +-0.976194 0.216899 0.000000 +-0.944386 0.328839 0.000000 +-0.976194 0.216899 0.000000 +-0.994333 0.106307 0.000000 +-0.994333 0.106307 0.000000 +-0.976194 0.216899 0.000000 +-0.994333 0.106307 0.000000 +-0.976194 0.216899 0.000000 +-0.994333 0.106307 0.000000 +-1.000000 0.000347 0.000000 +-1.000000 0.000347 0.000000 +-0.994333 0.106307 0.000000 +-1.000000 0.000347 0.000000 +-0.994333 0.106307 0.000000 +-1.000000 0.000347 0.000000 +-0.994469 -0.105031 0.000000 +-0.994469 -0.105031 0.000000 +-1.000000 0.000347 0.000000 +-0.994469 -0.105031 0.000000 +-1.000000 0.000347 0.000000 +-0.994469 -0.105031 0.000000 +-0.976685 -0.214678 0.000000 +-0.976685 -0.214678 0.000000 +-0.994469 -0.105031 0.000000 +-0.976685 -0.214678 0.000000 +-0.994469 -0.105031 0.000000 +-0.976685 -0.214678 0.000000 +-0.945309 -0.326176 0.000000 +-0.945309 -0.326176 0.000000 +-0.976685 -0.214678 0.000000 +-0.945309 -0.326176 0.000000 +-0.976685 -0.214678 0.000000 +-0.945309 -0.326176 0.000000 +-0.900443 -0.434974 0.000000 +-0.900443 -0.434974 0.000000 +-0.945309 -0.326176 0.000000 +-0.900443 -0.434974 0.000000 +-0.945309 -0.326176 0.000000 +-0.900443 -0.434974 0.000000 +-0.843812 -0.536639 0.000000 +-0.843812 -0.536639 0.000000 +-0.900443 -0.434974 0.000000 +-0.843812 -0.536639 0.000000 +-0.900443 -0.434974 0.000000 +-0.843812 -0.536639 0.000000 +-0.778453 -0.627703 0.000000 +-0.778453 -0.627703 0.000000 +-0.843812 -0.536639 0.000000 +-0.778453 -0.627703 0.000000 +-0.843812 -0.536639 0.000000 +-0.778453 -0.627703 0.000000 +-0.707284 -0.706929 0.000000 +-0.707284 -0.706929 0.000000 +-0.778453 -0.627703 0.000000 +-0.707284 -0.706929 0.000000 +-0.778453 -0.627703 0.000000 +-0.707284 -0.706929 0.000000 +-0.628074 -0.778154 0.000000 +-0.628074 -0.778154 0.000000 +-0.707284 -0.706929 0.000000 +-0.628074 -0.778154 0.000000 +-0.707284 -0.706929 0.000000 +-0.628074 -0.778154 0.000000 +-0.536995 -0.843586 0.000000 +-0.536994 -0.843586 0.000000 +-0.628074 -0.778154 0.000000 +-0.536994 -0.843586 0.000000 +-0.628074 -0.778154 0.000000 +-0.536995 -0.843586 0.000000 +-0.435288 -0.900291 0.000000 +-0.435288 -0.900291 0.000000 +-0.536995 -0.843586 0.000000 +-0.435288 -0.900291 0.000000 +-0.536994 -0.843586 0.000000 +-0.435288 -0.900291 0.000000 +-0.326425 -0.945223 0.000000 +-0.326425 -0.945223 0.000000 +-0.435288 -0.900291 0.000000 +-0.326425 -0.945223 0.000000 +-0.435288 -0.900291 0.000000 +-0.326425 -0.945223 0.000000 +-0.214847 -0.976648 0.000000 +-0.214846 -0.976648 0.000000 +-0.326425 -0.945223 0.000000 +-0.214846 -0.976648 0.000000 +-0.326425 -0.945223 0.000000 +-0.214847 -0.976648 0.000000 +-0.105112 -0.994460 0.000000 +-0.105112 -0.994460 0.000000 +-0.214847 -0.976648 0.000000 +-0.105112 -0.994460 0.000000 +-0.214846 -0.976648 0.000000 +-0.105112 -0.994460 0.000000 +0.000346 -1.000000 0.000000 +0.000346 -1.000000 0.000000 +-0.105112 -0.994460 0.000000 +0.000346 -1.000000 0.000000 +-0.105112 -0.994460 0.000000 +0.000346 -1.000000 0.000000 +0.106386 -0.994325 0.000000 +0.106386 -0.994325 0.000000 +0.000346 -1.000000 0.000000 +0.106386 -0.994325 0.000000 +0.000346 -1.000000 0.000000 +0.106386 -0.994325 0.000000 +0.217048 -0.976161 0.000000 +0.217048 -0.976161 0.000000 +0.106386 -0.994325 0.000000 +0.217048 -0.976161 0.000000 +0.106386 -0.994325 0.000000 +0.217048 -0.976161 0.000000 +0.329038 -0.944317 0.000000 +0.329038 -0.944317 0.000000 +0.217048 -0.976161 0.000000 +0.329038 -0.944317 0.000000 +0.217048 -0.976161 0.000000 +0.329038 -0.944317 0.000000 +0.437751 -0.899096 0.000000 +0.437751 -0.899096 0.000000 +0.329038 -0.944317 0.000000 +0.437751 -0.899096 0.000000 +0.329038 -0.944317 0.000000 +0.437751 -0.899096 0.000000 +0.538823 -0.842419 0.000000 +0.538823 -0.842419 0.000000 +0.437751 -0.899096 0.000000 +0.538823 -0.842419 0.000000 +0.437751 -0.899096 0.000000 +0.538823 -0.842419 0.000000 +0.628965 -0.777434 0.000000 +0.628965 -0.777434 0.000000 +0.538823 -0.842419 0.000000 +0.628965 -0.777434 0.000000 +0.538823 -0.842419 0.000000 +0.628965 -0.777434 0.000000 +0.707085 -0.707128 0.000000 +0.707086 -0.707128 0.000000 +0.628965 -0.777434 0.000000 +0.707086 -0.707128 0.000000 +0.628965 -0.777434 0.000000 +0.707085 -0.707128 0.000000 +0.777273 -0.629163 0.000000 +0.777273 -0.629163 0.000000 +0.707085 -0.707128 0.000000 +0.777273 -0.629163 0.000000 +0.707086 -0.707128 0.000000 +0.777273 -0.629163 0.000000 +0.842102 -0.539318 0.000000 +0.842102 -0.539318 0.000000 +0.777273 -0.629163 0.000000 +0.842102 -0.539318 0.000000 +0.777273 -0.629163 0.000000 +0.842102 -0.539318 0.000000 +0.898754 -0.438454 0.000000 +0.898754 -0.438454 0.000000 +0.842102 -0.539318 0.000000 +0.898754 -0.438454 0.000000 +0.842102 -0.539318 0.000000 +0.898754 -0.438454 0.000000 +0.944049 -0.329805 0.000000 +0.944049 -0.329805 0.000000 +0.898754 -0.438454 0.000000 +0.944049 -0.329805 0.000000 +0.898754 -0.438454 0.000000 +0.944049 -0.329805 0.000000 +0.976015 -0.217704 0.000000 +0.976015 -0.217704 0.000000 +0.944049 -0.329805 0.000000 +0.976015 -0.217704 0.000000 +0.944049 -0.329805 0.000000 +0.976015 -0.217704 0.000000 +0.994284 -0.106770 0.000000 +0.994284 -0.106770 0.000000 +0.976015 -0.217704 0.000000 +0.994284 -0.106770 0.000000 +0.976015 -0.217704 0.000000 +0.994284 -0.106770 0.000000 +0.999969 -0.007878 0.000000 +0.999969 -0.007878 0.000000 +0.994284 -0.106770 0.000000 +0.999969 -0.007878 0.000000 +0.994284 -0.106770 0.000000 +0.999969 -0.007878 0.000000 +0.997284 0.073658 0.000000 +0.997284 0.073658 0.000000 +0.999969 -0.007878 0.000000 +0.997284 0.073658 0.000000 +0.999969 -0.007878 0.000000 +0.997284 0.073658 0.000000 +0.988847 0.148935 0.000000 +0.988847 0.148935 0.000000 +0.997284 0.073658 0.000000 +0.988847 0.148935 0.000000 +0.997284 0.073658 0.000000 +0.988847 0.148935 0.000000 +0.974367 0.224965 0.000000 +0.974367 0.224965 0.000000 +0.988847 0.148935 0.000000 +0.974367 0.224965 0.000000 +0.988847 0.148935 0.000000 +0.974367 0.224965 0.000000 +0.953822 0.300373 0.000000 +0.953822 0.300373 0.000000 +0.974367 0.224965 0.000000 +0.953822 0.300373 0.000000 +0.974367 0.224965 0.000000 +0.953822 0.300373 0.000000 +0.927518 0.373778 0.000000 +0.927518 0.373778 0.000000 +0.953822 0.300373 0.000000 +0.927518 0.373778 0.000000 +0.953822 0.300373 0.000000 +0.927518 0.373778 0.000000 +0.896069 0.443915 0.000000 +0.896069 0.443915 0.000000 +0.927518 0.373778 0.000000 +0.896069 0.443915 0.000000 +0.927518 0.373778 0.000000 +0.896069 0.443915 0.000000 +0.878533 0.477682 0.000000 +0.878533 0.477682 0.000000 +0.896069 0.443915 0.000000 +0.878533 0.477682 0.000000 +0.896069 0.443915 0.000000 +0.707845 -0.706368 0.000000 +0.707845 -0.706368 0.000000 +0.707845 -0.706368 0.000000 +0.707845 -0.706368 0.000000 +0.707845 -0.706368 0.000000 +0.707845 -0.706368 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +-1.000000 0.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +0.000000 -1.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +1.000000 0.000000 0.000000 +-0.994382 0.105847 0.000000 +-0.976327 0.216300 0.000000 +-0.976327 0.216300 0.000000 +-0.994382 0.105847 0.000000 +-0.976327 0.216300 0.000000 +-0.994382 0.105847 0.000000 +-0.976327 0.216300 0.000000 +-0.944503 0.328502 0.000000 +-0.944503 0.328502 0.000000 +-0.976327 0.216300 0.000000 +-0.944503 0.328502 0.000000 +-0.976327 0.216300 0.000000 +-0.944503 0.328502 0.000000 +-0.899066 0.437813 0.000000 +-0.899066 0.437813 0.000000 +-0.944503 0.328502 0.000000 +-0.899066 0.437813 0.000000 +-0.944503 0.328502 0.000000 +-0.899066 0.437813 0.000000 +-0.841829 0.539744 0.000000 +-0.841829 0.539744 0.000000 +-0.899066 0.437813 0.000000 +-0.841829 0.539744 0.000000 +-0.899066 0.437813 0.000000 +-0.841829 0.539744 0.000000 +-0.775923 0.630828 0.000000 +-0.775923 0.630828 0.000000 +-0.841829 0.539744 0.000000 +-0.775923 0.630828 0.000000 +-0.841829 0.539744 0.000000 +-0.775923 0.630828 0.000000 +-0.704457 0.709747 0.000000 +-0.704457 0.709747 0.000000 +-0.775923 0.630828 0.000000 +-0.704457 0.709747 0.000000 +-0.775923 0.630828 0.000000 +-0.704457 0.709747 0.000000 +-0.625373 0.780326 0.000000 +-0.625373 0.780326 0.000000 +-0.704457 0.709747 0.000000 +-0.625373 0.780326 0.000000 +-0.704457 0.709747 0.000000 +-0.625373 0.780326 0.000000 +-0.534711 0.845035 0.000000 +-0.534711 0.845035 0.000000 +-0.625373 0.780326 0.000000 +-0.534711 0.845035 0.000000 +-0.625373 0.780326 0.000000 +-0.534711 0.845035 0.000000 +-0.433533 0.901138 0.000000 +-0.433533 0.901138 0.000000 +-0.534711 0.845035 0.000000 +-0.433533 0.901138 0.000000 +-0.534711 0.845035 0.000000 +-0.433533 0.901138 0.000000 +-0.325235 0.945633 0.000000 +-0.325235 0.945633 0.000000 +-0.433533 0.901138 0.000000 +-0.325235 0.945633 0.000000 +-0.433533 0.901138 0.000000 +-0.325235 0.945633 0.000000 +-0.214174 0.976796 0.000000 +-0.214174 0.976795 0.000000 +-0.325235 0.945633 0.000000 +-0.214174 0.976795 0.000000 +-0.325235 0.945633 0.000000 +-0.214174 0.976796 0.000000 +-0.104842 0.994489 0.000000 +-0.104842 0.994489 0.000000 +-0.214174 0.976796 0.000000 +-0.104842 0.994489 0.000000 +-0.214174 0.976795 0.000000 +-0.104842 0.994489 0.000000 +0.000236 1.000000 0.000000 +0.000236 1.000000 0.000000 +-0.104842 0.994489 0.000000 +0.000236 1.000000 0.000000 +-0.104842 0.994489 0.000000 +0.000236 1.000000 0.000000 +0.105713 0.994397 0.000000 +0.105713 0.994397 0.000000 +0.000236 1.000000 0.000000 +0.105713 0.994397 0.000000 +0.000236 1.000000 0.000000 +0.105713 0.994397 0.000000 +0.215685 0.976463 0.000000 +0.215685 0.976463 0.000000 +0.105713 0.994397 0.000000 +0.215685 0.976463 0.000000 +0.105713 0.994397 0.000000 +0.215685 0.976463 0.000000 +0.327041 0.945010 0.000000 +0.327041 0.945010 0.000000 +0.215685 0.976463 0.000000 +0.327041 0.945010 0.000000 +0.215685 0.976463 0.000000 +0.327041 0.945010 0.000000 +0.435254 0.900308 0.000000 +0.435254 0.900308 0.000000 +0.327041 0.945010 0.000000 +0.435254 0.900308 0.000000 +0.327041 0.945010 0.000000 +0.435254 0.900308 0.000000 +0.536018 0.844207 0.000000 +0.536018 0.844207 0.000000 +0.435254 0.900308 0.000000 +0.536018 0.844207 0.000000 +0.435254 0.900308 0.000000 +0.536018 0.844207 0.000000 +0.626056 0.779778 0.000000 +0.626056 0.779778 0.000000 +0.536018 0.844207 0.000000 +0.626056 0.779778 0.000000 +0.536018 0.844207 0.000000 +0.626056 0.779778 0.000000 +0.704624 0.709580 0.000000 +0.704624 0.709580 0.000000 +0.626056 0.779778 0.000000 +0.704624 0.709580 0.000000 +0.626056 0.779778 0.000000 +0.704624 0.709580 0.000000 +0.775923 0.630828 0.000000 +0.775923 0.630828 0.000000 +0.704624 0.709580 0.000000 +0.775923 0.630828 0.000000 +0.704624 0.709580 0.000000 +0.775923 0.630828 0.000000 +0.841829 0.539744 0.000000 +0.841829 0.539744 0.000000 +0.775923 0.630828 0.000000 +0.841829 0.539744 0.000000 +0.775923 0.630828 0.000000 +0.841829 0.539744 0.000000 +0.899066 0.437814 0.000000 +0.899066 0.437814 0.000000 +0.841829 0.539744 0.000000 +0.899066 0.437814 0.000000 +0.841829 0.539744 0.000000 +0.899066 0.437814 0.000000 +0.944503 0.328504 0.000000 +0.944503 0.328504 0.000000 +0.899066 0.437814 0.000000 +0.944503 0.328504 0.000000 +0.899066 0.437814 0.000000 +0.944503 0.328504 0.000000 +0.976327 0.216301 0.000000 +0.976327 0.216301 0.000000 +0.944503 0.328504 0.000000 +0.976327 0.216301 0.000000 +0.944503 0.328504 0.000000 +0.976327 0.216301 0.000000 +0.994383 0.105846 0.000000 +0.994383 0.105846 0.000000 +0.976327 0.216301 0.000000 +0.994383 0.105846 0.000000 +0.976327 0.216301 0.000000 +0.994383 0.105846 0.000000 +1.000000 -0.000003 0.000000 +1.000000 -0.000003 0.000000 +0.994383 0.105846 0.000000 +1.000000 -0.000003 0.000000 +0.994383 0.105846 0.000000 +1.000000 -0.000003 0.000000 +0.994388 -0.105793 0.000000 +0.994388 -0.105793 0.000000 +1.000000 -0.000003 0.000000 +0.994388 -0.105793 0.000000 +1.000000 -0.000003 0.000000 +0.994388 -0.105793 0.000000 +0.976385 -0.216039 0.000000 +0.976385 -0.216039 0.000000 +0.994388 -0.105793 0.000000 +0.976385 -0.216039 0.000000 +0.994388 -0.105793 0.000000 +0.976385 -0.216039 0.000000 +0.944722 -0.327872 0.000000 +0.944722 -0.327872 0.000000 +0.976385 -0.216039 0.000000 +0.944722 -0.327872 0.000000 +0.976385 -0.216039 0.000000 +0.944722 -0.327872 0.000000 +0.899606 -0.436703 0.000000 +0.899606 -0.436703 0.000000 +0.944722 -0.327872 0.000000 +0.899606 -0.436703 0.000000 +0.944722 -0.327872 0.000000 +0.899606 -0.436703 0.000000 +0.842867 -0.538122 0.000000 +0.842867 -0.538122 0.000000 +0.899606 -0.436703 0.000000 +0.842867 -0.538122 0.000000 +0.899606 -0.436703 0.000000 +0.842867 -0.538122 0.000000 +0.777611 -0.628746 0.000000 +0.777611 -0.628746 0.000000 +0.842867 -0.538122 0.000000 +0.777611 -0.628746 0.000000 +0.842867 -0.538122 0.000000 +0.777611 -0.628746 0.000000 +0.706923 -0.707291 0.000000 +0.706923 -0.707291 0.000000 +0.777611 -0.628746 0.000000 +0.706923 -0.707291 0.000000 +0.777611 -0.628746 0.000000 +0.706923 -0.707291 0.000000 +0.628659 -0.777681 0.000000 +0.628659 -0.777681 0.000000 +0.706923 -0.707291 0.000000 +0.628659 -0.777681 0.000000 +0.706923 -0.707291 0.000000 +0.628659 -0.777681 0.000000 +0.538632 -0.842541 0.000000 +0.538632 -0.842541 0.000000 +0.628659 -0.777681 0.000000 +0.538632 -0.842541 0.000000 +0.628659 -0.777681 0.000000 +0.538632 -0.842541 0.000000 +0.437669 -0.899136 0.000000 +0.437669 -0.899136 0.000000 +0.538632 -0.842541 0.000000 +0.437669 -0.899136 0.000000 +0.538632 -0.842541 0.000000 +0.437669 -0.899136 0.000000 +0.329043 -0.944315 0.000000 +0.329043 -0.944315 0.000000 +0.437669 -0.899136 0.000000 +0.329043 -0.944315 0.000000 +0.437669 -0.899136 0.000000 +0.329043 -0.944315 0.000000 +0.217098 -0.976150 0.000000 +0.217098 -0.976150 0.000000 +0.329043 -0.944315 0.000000 +0.217098 -0.976150 0.000000 +0.329043 -0.944315 0.000000 +0.217098 -0.976150 0.000000 +0.106429 -0.994320 0.000000 +0.106429 -0.994320 0.000000 +0.217098 -0.976150 0.000000 +0.106429 -0.994320 0.000000 +0.217098 -0.976150 0.000000 +0.106429 -0.994320 0.000000 +0.000238 -1.000000 0.000000 +0.000238 -1.000000 0.000000 +0.106429 -0.994320 0.000000 +0.000238 -1.000000 0.000000 +0.106429 -0.994320 0.000000 +0.000238 -1.000000 0.000000 +-0.105554 -0.994414 0.000000 +-0.105554 -0.994413 0.000000 +0.000238 -1.000000 0.000000 +-0.105554 -0.994413 0.000000 +0.000238 -1.000000 0.000000 +-0.105554 -0.994414 0.000000 +-0.215576 -0.976487 0.000000 +-0.215576 -0.976487 0.000000 +-0.105554 -0.994414 0.000000 +-0.215576 -0.976487 0.000000 +-0.105554 -0.994413 0.000000 +-0.215576 -0.976487 0.000000 +-0.327227 -0.944946 0.000000 +-0.327227 -0.944946 0.000000 +-0.215576 -0.976487 0.000000 +-0.327227 -0.944946 0.000000 +-0.215576 -0.976487 0.000000 +-0.327227 -0.944946 0.000000 +-0.435943 -0.899974 0.000000 +-0.435943 -0.899974 0.000000 +-0.327227 -0.944946 0.000000 +-0.435943 -0.899974 0.000000 +-0.327227 -0.944946 0.000000 +-0.435943 -0.899974 0.000000 +-0.537323 -0.843376 0.000000 +-0.537323 -0.843376 0.000000 +-0.435943 -0.899974 0.000000 +-0.537323 -0.843376 0.000000 +-0.435943 -0.899974 0.000000 +-0.537323 -0.843376 0.000000 +-0.627977 -0.778232 0.000000 +-0.627977 -0.778232 0.000000 +-0.537323 -0.843376 0.000000 +-0.627977 -0.778232 0.000000 +-0.537323 -0.843376 0.000000 +-0.627977 -0.778232 0.000000 +-0.706755 -0.707458 0.000000 +-0.706755 -0.707458 0.000000 +-0.627977 -0.778232 0.000000 +-0.706755 -0.707458 0.000000 +-0.627977 -0.778232 0.000000 +-0.706755 -0.707458 0.000000 +-0.777611 -0.628746 0.000000 +-0.777611 -0.628746 0.000000 +-0.706755 -0.707458 0.000000 +-0.777611 -0.628746 0.000000 +-0.706755 -0.707458 0.000000 +-0.777611 -0.628746 0.000000 +-0.842867 -0.538122 0.000000 +-0.842867 -0.538122 0.000000 +-0.777611 -0.628746 0.000000 +-0.842867 -0.538122 0.000000 +-0.777611 -0.628746 0.000000 +-0.842867 -0.538122 0.000000 +-0.899606 -0.436703 0.000000 +-0.899606 -0.436703 0.000000 +-0.842867 -0.538122 0.000000 +-0.899606 -0.436703 0.000000 +-0.842867 -0.538122 0.000000 +-0.899606 -0.436703 0.000000 +-0.944722 -0.327873 0.000000 +-0.944722 -0.327873 0.000000 +-0.899606 -0.436703 0.000000 +-0.944722 -0.327873 0.000000 +-0.899606 -0.436703 0.000000 +-0.944722 -0.327873 0.000000 +-0.976385 -0.216040 0.000000 +-0.976385 -0.216040 0.000000 +-0.944722 -0.327873 0.000000 +-0.976385 -0.216040 0.000000 +-0.944722 -0.327873 0.000000 +-0.976385 -0.216040 0.000000 +-0.994388 -0.105793 0.000000 +-0.994388 -0.105793 0.000000 +-0.976385 -0.216040 0.000000 +-0.994388 -0.105793 0.000000 +-0.976385 -0.216040 0.000000 +-0.994388 -0.105793 0.000000 +-1.000000 -0.000002 0.000000 +-1.000000 -0.000002 0.000000 +-0.994388 -0.105793 0.000000 +-1.000000 -0.000002 0.000000 +-0.994388 -0.105793 0.000000 +-1.000000 -0.000002 0.000000 +-0.994382 0.105847 0.000000 +-0.994382 0.105847 0.000000 +-1.000000 -0.000002 0.000000 +-0.994382 0.105847 0.000000 +-1.000000 -0.000002 0.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 -1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 +0.000000 0.000000 1.000000 + + + + + + + + + + + + +

0 0 1 1 65 2 0 3 65 4 64 5 1 6 2 7 66 8 1 9 66 10 65 11 2 12 3 13 67 14 2 15 67 16 66 17 3 18 4 19 68 20 3 21 68 22 67 23 4 24 5 25 69 26 4 27 69 28 68 29 5 30 6 31 70 32 5 33 70 34 69 35 6 36 7 37 71 38 6 39 71 40 70 41 7 42 8 43 72 44 7 45 72 46 71 47 8 48 9 49 73 50 8 51 73 52 72 53 9 54 10 55 74 56 9 57 74 58 73 59 10 60 11 61 75 62 10 63 75 64 74 65 11 66 12 67 76 68 11 69 76 70 75 71 12 72 13 73 77 74 12 75 77 76 76 77 13 78 14 79 78 80 13 81 78 82 77 83 14 84 15 85 79 86 14 87 79 88 78 89 15 90 16 91 80 92 15 93 80 94 79 95 16 96 17 97 81 98 16 99 81 100 80 101 17 102 18 103 82 104 17 105 82 106 81 107 18 108 19 109 83 110 18 111 83 112 82 113 19 114 20 115 84 116 19 117 84 118 83 119 20 120 21 121 85 122 20 123 85 124 84 125 21 126 22 127 86 128 21 129 86 130 85 131 22 132 23 133 87 134 22 135 87 136 86 137 23 138 24 139 88 140 23 141 88 142 87 143 24 144 25 145 89 146 24 147 89 148 88 149 25 150 26 151 90 152 25 153 90 154 89 155 26 156 27 157 91 158 26 159 91 160 90 161 27 162 28 163 92 164 27 165 92 166 91 167 28 168 29 169 93 170 28 171 93 172 92 173 29 174 30 175 94 176 29 177 94 178 93 179 30 180 31 181 95 182 30 183 95 184 94 185 31 186 32 187 96 188 31 189 96 190 95 191 32 192 33 193 97 194 32 195 97 196 96 197 33 198 34 199 98 200 33 201 98 202 97 203 34 204 35 205 99 206 34 207 99 208 98 209 35 210 36 211 100 212 35 213 100 214 99 215 36 216 37 217 101 218 36 219 101 220 100 221 37 222 38 223 102 224 37 225 102 226 101 227 38 228 39 229 103 230 38 231 103 232 102 233 39 234 40 235 104 236 39 237 104 238 103 239 40 240 41 241 105 242 40 243 105 244 104 245 41 246 42 247 106 248 41 249 106 250 105 251 42 252 43 253 107 254 42 255 107 256 106 257 43 258 44 259 108 260 43 261 108 262 107 263 44 264 45 265 109 266 44 267 109 268 108 269 45 270 46 271 110 272 45 273 110 274 109 275 46 276 47 277 111 278 46 279 111 280 110 281 47 282 48 283 112 284 47 285 112 286 111 287 48 288 49 289 113 290 48 291 113 292 112 293 49 294 50 295 114 296 49 297 114 298 113 299 50 300 51 301 115 302 50 303 115 304 114 305 51 306 52 307 116 308 51 309 116 310 115 311 52 312 53 313 117 314 52 315 117 316 116 317 53 318 54 319 118 320 53 321 118 322 117 323 54 324 55 325 119 326 54 327 119 328 118 329 55 330 56 331 120 332 55 333 120 334 119 335 56 336 57 337 121 338 56 339 121 340 120 341 57 342 58 343 122 344 57 345 122 346 121 347 58 348 59 349 123 350 58 351 123 352 122 353 59 354 60 355 124 356 59 357 124 358 123 359 60 360 61 361 125 362 60 363 125 364 124 365 61 366 62 367 126 368 61 369 126 370 125 371 62 372 63 373 127 374 62 375 127 376 126 377 63 378 0 379 64 380 63 381 64 382 127 383 128 384 129 385 185 386 128 387 185 388 184 389 129 390 130 391 186 392 129 393 186 394 185 395 130 396 131 397 187 398 130 399 187 400 186 401 131 402 132 403 188 404 131 405 188 406 187 407 132 408 133 409 189 410 132 411 189 412 188 413 133 414 134 415 190 416 133 417 190 418 189 419 134 420 135 421 191 422 134 423 191 424 190 425 135 426 136 427 192 428 135 429 192 430 191 431 136 432 137 433 193 434 136 435 193 436 192 437 137 438 138 439 194 440 137 441 194 442 193 443 138 444 139 445 195 446 138 447 195 448 194 449 139 450 140 451 196 452 139 453 196 454 195 455 140 456 141 457 197 458 140 459 197 460 196 461 141 462 142 463 198 464 141 465 198 466 197 467 142 468 143 469 199 470 142 471 199 472 198 473 143 474 144 475 200 476 143 477 200 478 199 479 144 480 145 481 201 482 144 483 201 484 200 485 145 486 146 487 202 488 145 489 202 490 201 491 146 492 147 493 203 494 146 495 203 496 202 497 147 498 148 499 204 500 147 501 204 502 203 503 148 504 149 505 205 506 148 507 205 508 204 509 149 510 150 511 206 512 149 513 206 514 205 515 150 516 151 517 207 518 150 519 207 520 206 521 151 522 152 523 208 524 151 525 208 526 207 527 152 528 153 529 209 530 152 531 209 532 208 533 153 534 154 535 210 536 153 537 210 538 209 539 154 540 155 541 211 542 154 543 211 544 210 545 155 546 156 547 212 548 155 549 212 550 211 551 156 552 157 553 213 554 156 555 213 556 212 557 157 558 158 559 214 560 157 561 214 562 213 563 158 564 159 565 215 566 158 567 215 568 214 569 159 570 160 571 216 572 159 573 216 574 215 575 160 576 161 577 217 578 160 579 217 580 216 581 161 582 162 583 218 584 161 585 218 586 217 587 162 588 163 589 219 590 162 591 219 592 218 593 163 594 164 595 220 596 163 597 220 598 219 599 164 600 165 601 221 602 164 603 221 604 220 605 165 606 166 607 222 608 165 609 222 610 221 611 166 612 167 613 223 614 166 615 223 616 222 617 167 618 168 619 224 620 167 621 224 622 223 623 168 624 169 625 225 626 168 627 225 628 224 629 169 630 170 631 226 632 169 633 226 634 225 635 170 636 171 637 227 638 170 639 227 640 226 641 171 642 172 643 228 644 171 645 228 646 227 647 172 648 173 649 229 650 172 651 229 652 228 653 173 654 174 655 230 656 173 657 230 658 229 659 174 660 175 661 231 662 174 663 231 664 230 665 175 666 176 667 232 668 175 669 232 670 231 671 176 672 177 673 233 674 176 675 233 676 232 677 177 678 178 679 234 680 177 681 234 682 233 683 178 684 179 685 235 686 178 687 235 688 234 689 179 690 180 691 236 692 179 693 236 694 235 695 180 696 181 697 237 698 180 699 237 700 236 701 181 702 182 703 238 704 181 705 238 706 237 707 182 708 183 709 239 710 182 711 239 712 238 713 183 714 128 715 184 716 183 717 184 718 239 719 62 720 0 721 63 722 61 723 0 724 62 725 61 726 176 727 0 728 60 729 176 730 61 731 60 732 177 733 176 734 60 735 178 736 177 737 59 738 178 739 60 740 59 741 179 742 178 743 58 744 179 745 59 746 58 747 180 748 179 749 57 750 180 751 58 752 56 753 180 754 57 755 56 756 181 757 180 758 55 759 181 760 56 761 55 762 182 763 181 764 54 765 182 766 55 767 53 768 182 769 54 770 53 771 183 772 182 773 52 774 183 775 53 776 52 777 128 778 183 779 51 780 128 781 52 782 51 783 129 784 128 785 50 786 129 787 51 788 50 789 130 790 129 791 49 792 130 793 50 794 49 795 131 796 130 797 48 798 131 799 49 800 48 801 132 802 131 803 47 804 132 805 48 806 47 807 133 808 132 809 46 810 133 811 47 812 46 813 134 814 133 815 45 816 134 817 46 818 45 819 135 820 134 821 44 822 135 823 45 824 44 825 136 826 135 827 43 828 136 829 44 830 43 831 137 832 136 833 42 834 137 835 43 836 42 837 138 838 137 839 41 840 138 841 42 842 41 843 139 844 138 845 40 846 139 847 41 848 40 849 140 850 139 851 39 852 140 853 40 854 39 855 141 856 140 857 39 858 142 859 141 860 38 861 142 862 39 863 38 864 143 865 142 866 37 867 143 868 38 869 37 870 144 871 143 872 36 873 144 874 37 875 35 876 144 877 36 878 35 879 145 880 144 881 34 882 145 883 35 884 34 885 146 886 145 887 33 888 146 889 34 890 33 891 147 892 146 893 32 894 147 895 33 896 32 897 148 898 147 899 31 900 148 901 32 902 31 903 149 904 148 905 30 906 149 907 31 908 30 909 150 910 149 911 29 912 150 913 30 914 29 915 151 916 150 917 28 918 151 919 29 920 28 921 152 922 151 923 27 924 152 925 28 926 27 927 153 928 152 929 26 930 153 931 27 932 26 933 154 934 153 935 25 936 154 937 26 938 25 939 155 940 154 941 24 942 155 943 25 944 24 945 156 946 155 947 23 948 156 949 24 950 23 951 157 952 156 953 22 954 157 955 23 956 22 957 158 958 157 959 21 960 158 961 22 962 21 963 159 964 158 965 20 966 159 967 21 968 20 969 160 970 159 971 19 972 160 973 20 974 19 975 161 976 160 977 18 978 161 979 19 980 18 981 162 982 161 983 17 984 162 985 18 986 17 987 163 988 162 989 16 990 163 991 17 992 16 993 164 994 163 995 15 996 164 997 16 998 15 999 165 1000 164 1001 14 1002 165 1003 15 1004 14 1005 166 1006 165 1007 13 1008 166 1009 14 1010 13 1011 167 1012 166 1013 12 1014 167 1015 13 1016 12 1017 168 1018 167 1019 11 1020 168 1021 12 1022 11 1023 169 1024 168 1025 10 1026 169 1027 11 1028 10 1029 170 1030 169 1031 9 1032 170 1033 10 1034 9 1035 171 1036 170 1037 8 1038 171 1039 9 1040 7 1041 171 1042 8 1043 7 1044 172 1045 171 1046 6 1047 172 1048 7 1049 6 1050 173 1051 172 1052 5 1053 173 1054 6 1055 4 1056 173 1057 5 1058 4 1059 174 1060 173 1061 1 1062 3 1063 2 1064 0 1065 3 1066 1 1067 176 1068 3 1069 0 1070 176 1071 4 1072 3 1073 175 1074 4 1075 176 1076 175 1077 174 1078 4 1079 126 1080 127 1081 64 1082 125 1083 126 1084 64 1085 125 1086 64 1087 232 1088 124 1089 125 1090 232 1091 124 1092 232 1093 233 1094 124 1095 233 1096 234 1097 123 1098 124 1099 234 1100 123 1101 234 1102 235 1103 122 1104 123 1105 235 1106 122 1107 235 1108 236 1109 121 1110 122 1111 236 1112 120 1113 121 1114 236 1115 120 1116 236 1117 237 1118 119 1119 120 1120 237 1121 119 1122 237 1123 238 1124 118 1125 119 1126 238 1127 117 1128 118 1129 238 1130 117 1131 238 1132 239 1133 116 1134 117 1135 239 1136 116 1137 239 1138 184 1139 115 1140 116 1141 184 1142 115 1143 184 1144 185 1145 114 1146 115 1147 185 1148 114 1149 185 1150 186 1151 113 1152 114 1153 186 1154 113 1155 186 1156 187 1157 112 1158 113 1159 187 1160 112 1161 187 1162 188 1163 111 1164 112 1165 188 1166 111 1167 188 1168 189 1169 110 1170 111 1171 189 1172 110 1173 189 1174 190 1175 109 1176 110 1177 190 1178 109 1179 190 1180 191 1181 108 1182 109 1183 191 1184 108 1185 191 1186 192 1187 107 1188 108 1189 192 1190 107 1191 192 1192 193 1193 106 1194 107 1195 193 1196 106 1197 193 1198 194 1199 105 1200 106 1201 194 1202 105 1203 194 1204 195 1205 104 1206 105 1207 195 1208 104 1209 195 1210 196 1211 103 1212 104 1213 196 1214 103 1215 196 1216 197 1217 103 1218 197 1219 198 1220 102 1221 103 1222 198 1223 102 1224 198 1225 199 1226 101 1227 102 1228 199 1229 101 1230 199 1231 200 1232 100 1233 101 1234 200 1235 99 1236 100 1237 200 1238 99 1239 200 1240 201 1241 98 1242 99 1243 201 1244 98 1245 201 1246 202 1247 97 1248 98 1249 202 1250 97 1251 202 1252 203 1253 96 1254 97 1255 203 1256 96 1257 203 1258 204 1259 95 1260 96 1261 204 1262 95 1263 204 1264 205 1265 94 1266 95 1267 205 1268 94 1269 205 1270 206 1271 93 1272 94 1273 206 1274 93 1275 206 1276 207 1277 92 1278 93 1279 207 1280 92 1281 207 1282 208 1283 91 1284 92 1285 208 1286 91 1287 208 1288 209 1289 90 1290 91 1291 209 1292 90 1293 209 1294 210 1295 89 1296 90 1297 210 1298 89 1299 210 1300 211 1301 88 1302 89 1303 211 1304 88 1305 211 1306 212 1307 87 1308 88 1309 212 1310 87 1311 212 1312 213 1313 86 1314 87 1315 213 1316 86 1317 213 1318 214 1319 85 1320 86 1321 214 1322 85 1323 214 1324 215 1325 84 1326 85 1327 215 1328 84 1329 215 1330 216 1331 83 1332 84 1333 216 1334 83 1335 216 1336 217 1337 82 1338 83 1339 217 1340 82 1341 217 1342 218 1343 81 1344 82 1345 218 1346 81 1347 218 1348 219 1349 80 1350 81 1351 219 1352 80 1353 219 1354 220 1355 79 1356 80 1357 220 1358 79 1359 220 1360 221 1361 78 1362 79 1363 221 1364 78 1365 221 1366 222 1367 77 1368 78 1369 222 1370 77 1371 222 1372 223 1373 76 1374 77 1375 223 1376 76 1377 223 1378 224 1379 75 1380 76 1381 224 1382 75 1383 224 1384 225 1385 74 1386 75 1387 225 1388 74 1389 225 1390 226 1391 73 1392 74 1393 226 1394 73 1395 226 1396 227 1397 72 1398 73 1399 227 1400 71 1401 72 1402 227 1403 71 1404 227 1405 228 1406 70 1407 71 1408 228 1409 70 1410 228 1411 229 1412 69 1413 70 1414 229 1415 68 1416 69 1417 229 1418 68 1419 229 1420 230 1421 65 1422 66 1423 67 1424 64 1425 65 1426 67 1427 232 1428 64 1429 67 1430 232 1431 67 1432 68 1433 231 1434 232 1435 68 1436 231 1437 68 1438 230 1439

 + + + + + + + + 1.000000 1.000000 1.000000 + + + + + + + 1.000000 0.000000 0.000000 0.992424 0.000000 1.000000 0.000000 -23.446840 0.000000 0.000000 1.000000 0.000000 0.000000 0.000000 0.000000 1.000000 + + + + + + + + + diff --git a/messages/visualization/spencer_tracking_rviz_plugin/package.xml b/messages/visualization/spencer_tracking_rviz_plugin/package.xml new file mode 100755 index 0000000..c455aa3 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/package.xml @@ -0,0 +1,36 @@ + + spencer_tracking_rviz_plugin + 1.2.1 + + + RViz plugin for visualizing tracked and detected persons and groups + + + Timm Linder + Timm Linder + + BSD; CC BY 3.0 (Fugue Icons by Yusuke Kamiyamane) + + catkin + + rviz + spencer_tracking_msgs + spencer_social_relation_msgs + spencer_human_attribute_msgs + roslib + qtbase5-dev + + rviz + spencer_tracking_msgs + spencer_social_relation_msgs + spencer_human_attribute_msgs + roslib + libqt5-core + libqt5-gui + libqt5-widgets + + + + + + diff --git a/messages/visualization/spencer_tracking_rviz_plugin/plugin_description.xml b/messages/visualization/spencer_tracking_rviz_plugin/plugin_description.xml new file mode 100755 index 0000000..36a7d14 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/plugin_description.xml @@ -0,0 +1,52 @@ + + + + + Displays detected persons from spencer_tracking_msgs/DetectedPersons messages. + + spencer_tracking_msgs/DetectedPersons + + + + + Displays target person from spencer_tracking_msgs/TargetPerson messages. + + spencer_tracking_msgs/TargetPerson + + + + + Displays tracked persons from spencer_tracking_msgs/TrackedPersons messages. + + spencer_tracking_msgs/TrackedPersons + + + + + Displays tracked groups from spencer_tracking_msgs/TrackedGroups messages. + + spencer_tracking_msgs/TrackedGroups + + + + + Displays social relations from spencer_social_relation_msgs/SocialRelations messages. + + spencer_social_relation_msgs/SocialRelations + + + + + Displays social activities from spencer_social_relation_msgs/SocialActivities messages. + + spencer_social_relation_msgs/SocialActivities + + + + + Displays human attributes from spencer_human_attribute_msgs/HumanAttributes messages. + + spencer_human_attribute_msgs/HumanAttributes + + + diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/detected_persons.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/detected_persons.png new file mode 100755 index 0000000..39e9cdc Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/detected_persons.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/human_attributes.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/human_attributes.png new file mode 100755 index 0000000..b8dc214 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/human_attributes.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/social_activities.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/social_activities.png new file mode 100755 index 0000000..778a1a7 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/social_activities.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/social_relations.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/social_relations.png new file mode 100755 index 0000000..1fa4151 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/social_relations.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_groups.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_groups.png new file mode 100755 index 0000000..6542297 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_groups.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_1.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_1.png new file mode 100755 index 0000000..332b00f Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_1.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_2.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_2.png new file mode 100755 index 0000000..865c845 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_2.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_3.png b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_3.png new file mode 100755 index 0000000..7e6b0a8 Binary files /dev/null and b/messages/visualization/spencer_tracking_rviz_plugin/screenshots/tracked_persons_3.png differ diff --git a/messages/visualization/spencer_tracking_rviz_plugin/scripts/send_test_msgs.py b/messages/visualization/spencer_tracking_rviz_plugin/scripts/send_test_msgs.py new file mode 100755 index 0000000..2da62e4 --- /dev/null +++ b/messages/visualization/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() \ No newline at end of file diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/additional_topic_subscriber.h b/messages/visualization/spencer_tracking_rviz_plugin/src/additional_topic_subscriber.h new file mode 100755 index 0000000..df8bee0 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/additional_topic_subscriber.h @@ -0,0 +1,223 @@ +/* +* 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 + +#if ROS_VERSION_MINIMUM(1, 14, 0) + #include +#else + #include +#endif + +#include +#include +#include +#include +#include +#endif + +#include +#include +#include +#include + + +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: 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 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 ATSClass; + + AdditionalTopicSubscriber(const QString& propertyName, Display* display, DisplayContext* context, ros::NodeHandle& update_nh, const boost::function)>& 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()); + 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(*m_context->getTF2BufferPtr(), "map", 10, m_updateNodeHandle); + #else + tf_filter = new tf::MessageFilter(*m_context->getTFClient(), "map", 10, m_updateNodeHandle); + #endif + + tf_filter->connectInput(m_subscriber); + tf_filter->registerCallback(boost::bind(&AdditionalTopicSubscriber::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* tf_filter; + #else + tf::MessageFilter* tf_filter; + #endif + +private: + std::string m_topic; + bool m_enabled; + Display* m_display; + DisplayContext* m_context; + ros::NodeHandle m_updateNodeHandle; + message_filters::Subscriber m_subscriber; + uint32_t m_messagesReceived; + + const boost::function)> m_messageCallback; +}; + +} // end namespace rviz + +#endif // ADDITIONAL_TOPIC_SUBSCRIBER_H diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/detected_persons_display.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/detected_persons_display.cpp new file mode 100755 index 0000000..766f4fa --- /dev/null +++ b/messages/visualization/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 +#include +#include "rviz/selection/selection_manager.h" + +#include "detected_persons_display.h" +#ifndef Q_MOC_RUN +#include +#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, 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, 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::const_iterator detectedPersonIt = msg->detections.begin(); detectedPersonIt != msg->detections.end(); ++detectedPersonIt) + { + boost::shared_ptr detectedPersonVisual; + + // Create a new visual representation of the detected person + detectedPersonVisual = boost::shared_ptr(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(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 = 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_EXPORT_CLASS(spencer_tracking_rviz_plugin::DetectedPersonsDisplay, rviz::Display) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/detected_persons_display.h b/messages/visualization/spencer_tracking_rviz_plugin/src/detected_persons_display.h new file mode 100755 index 0000000..d210b59 --- /dev/null +++ b/messages/visualization/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 +#include +#include +#include "person_display_common.h" +#endif + +namespace spencer_tracking_rviz_plugin +{ + /// The visual of a tracked person. + struct DetectedPersonVisual + { + boost::shared_ptr sceneNode; + + boost::shared_ptr personVisual; + boost::shared_ptr detectionIdText, confidenceText, modalityText; + boost::shared_ptr orientationArrow; + boost::shared_ptr 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 + { + 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 > 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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/human_attributes_display.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/human_attributes_display.cpp new file mode 100755 index 0000000..db71e15 --- /dev/null +++ b/messages/visualization/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 +#include + +#ifndef Q_MOC_RUN +#include "human_attributes_display.h" +#include +#include +#include +#include +#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, m_humanAttributeVisuals | boost::adaptors::map_values) { + updateVisualStyles(humanAttributeVisual); + } +} + +void HumanAttributesDisplay::updateVisualStyles(boost::shared_ptr& humanAttributeVisual) +{ + track_id trackId = humanAttributeVisual->trackId; + bool personHidden = isPersonHidden(trackId); + + boost::shared_ptr 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::createVisualIfNotExists(track_id trackId) +{ + if(m_humanAttributeVisuals.find(trackId) == m_humanAttributeVisuals.end()) { + boost::shared_ptr humanAttributeVisual = boost::shared_ptr(new HumanAttributeVisual); + + humanAttributeVisual->sceneNode = boost::shared_ptr(scene_node_->createChildSceneNode()); + + humanAttributeVisual->ageGroupText = boost::shared_ptr(new TextNode(context_->getSceneManager(), humanAttributeVisual->sceneNode.get())); + humanAttributeVisual->ageGroupText->showOnTop(); + humanAttributeVisual->ageGroupText->setCaption(" "); + + humanAttributeVisual->personHeightText = boost::shared_ptr(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 = 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(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 = 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 tracksWithUnknownPosition; + foreach(boost::shared_ptr humanAttributeVisual, m_humanAttributeVisuals | boost::adaptors::map_values) + { + boost::shared_ptr 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_EXPORT_CLASS(spencer_tracking_rviz_plugin::HumanAttributesDisplay, rviz::Display) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/human_attributes_display.h b/messages/visualization/spencer_tracking_rviz_plugin/src/human_attributes_display.h new file mode 100755 index 0000000..344afde --- /dev/null +++ b/messages/visualization/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 +#include +#include + +#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 + { + 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 sceneNode; + boost::shared_ptr genderMesh; + unsigned int trackId; + boost::shared_ptr ageGroupText; + boost::shared_ptr 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); + boost::shared_ptr 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 > 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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/person_display_common.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/person_display_common.cpp new file mode 100755 index 0000000..0333dea --- /dev/null +++ b/messages/visualization/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 +#include +#include +#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 separator(","); + tokenizer< char_separator > tokens(personIDString, separator); + foreach(const string& token, tokens) { + try { m_excludedPersonIDs.insert(lexical_cast(token)); } + catch(bad_lexical_cast &) {} + } + } + m_includedPersonIDs.clear(); + { + string personIDString = m_included_person_ids_property->getStdString(); + char_separator separator(","); + tokenizer< char_separator > tokens(personIDString, separator); + foreach(const string& token, tokens) { + try { m_includedPersonIDs.insert(lexical_cast(token)); } + catch(bad_lexical_cast &) {} + } + } + + // Relay change to other subscribers + m_stylesChangedSubscriber->stylesChanged(); +} + + +} // end namespace spencer_tracking_rviz_plugin diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/person_display_common.h b/messages/visualization/spencer_tracking_rviz_plugin/src/person_display_common.h new file mode 100755 index 0000000..9a49763 --- /dev/null +++ b/messages/visualization/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 +#include +#include +#include +#include +#include + +#include +#include +#include "visuals/person_visual.h" +#include "visuals/text_node.h" +#include "visuals/covariance_visual.h" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#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 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 + class PersonDisplayCommon: public rviz::MessageFilterDisplay, 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::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) + { + 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) { + if(!personVisual) return; + + // Set line width of wireframe visualization + HasLineWidth* hasLineWidth = dynamic_cast(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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/social_activities_display.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/social_activities_display.cpp new file mode 100755 index 0000000..17e7927 --- /dev/null +++ b/messages/visualization/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 +#include +#include "rviz/selection/selection_manager.h" + +#ifndef Q_MOC_RUN +#include "social_activities_display.h" + +#include +#include +#include +#include +#include +#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(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 separator(","); + tokenizer< char_separator > 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 separator(","); + tokenizer< char_separator > 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, 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) +{ + 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 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 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& 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 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 = boost::shared_ptr(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 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(); + } + 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 circle = boost::shared_ptr(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 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 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 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 = 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 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 seenTrackIds; + foreach(const TrackedPersonsCache::CachedTrackedPersonsMap::value_type& entry, m_trackedPersonsCache.getMap()) { + const track_id trackId = entry.first; + const boost::shared_ptr 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(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 = boost::dynamic_pointer_cast(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 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_EXPORT_CLASS(spencer_tracking_rviz_plugin::SocialActivitiesDisplay, rviz::Display) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/social_activities_display.h b/messages/visualization/spencer_tracking_rviz_plugin/src/social_activities_display.h new file mode 100755 index 0000000..9414e83 --- /dev/null +++ b/messages/visualization/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 +#include +#include +#include +#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 + { + 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 > socialActivityAssignmentCircles; + vector > connectionLines; + vector< boost::shared_ptr > typeTexts; + vector 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& groupVisual); + bool isActivityTypeHidden(activity_type activityType); + Ogre::ColourValue getActivityColor(activity_type activityType, float confidence); + + // Scene nodes + boost::shared_ptr 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; + boost::shared_ptr sceneNode; + track_id trackId; + }; + + vector > m_socialActivityVisuals; + map m_personVisualMap; // to keep person visuals alive across multiple frames, for walking animation + + map m_highestConfidenceActivityPerTrack; // only highest-confidence activity per person + map > m_allActivitiesPerTrack; // all activities that a track is involved in + + set 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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/social_relations_display.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/social_relations_display.cpp new file mode 100755 index 0000000..f38e44f --- /dev/null +++ b/messages/visualization/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 +#include +#include "rviz/selection/selection_manager.h" + +#ifndef Q_MOC_RUN +#include "social_relations_display.h" +#include +#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(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 personTrack1 = m_trackedPersonsCache.lookup(socialRelation.track1_id); + boost::shared_ptr 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 = boost::shared_ptr(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 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 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, m_relationVisuals) { + updateRelationVisualStyles(relationVisual); + } +} + +void SocialRelationsDisplay::updateRelationVisualStyles(boost::shared_ptr& 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_EXPORT_CLASS(spencer_tracking_rviz_plugin::SocialRelationsDisplay, rviz::Display) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/social_relations_display.h b/messages/visualization/spencer_tracking_rviz_plugin/src/social_relations_display.h new file mode 100755 index 0000000..48e53cc --- /dev/null +++ b/messages/visualization/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 +#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 + { + 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 relationLine; + boost::shared_ptr 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); + + // Scene node for group affiliation history visualization + boost::shared_ptr 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 > m_relationVisuals; + TrackedPersonsCache m_trackedPersonsCache; + + private Q_SLOTS: + virtual void stylesChanged(); + }; + +} // end namespace spencer_tracking_rviz_plugin + +#endif // SOCIAL_RELATIONS_DISPLAY_H diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/target_person_display.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/target_person_display.cpp new file mode 100755 index 0000000..39bd668 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/target_person_display.cpp @@ -0,0 +1,502 @@ +/* +* 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 +#include +#include "rviz/selection/selection_manager.h" +#include "target_person_display.h" +#include +#include +#include +#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 TargetPersonDisplay::onInitialize() +{ + PersonDisplayCommon::onInitialize(); + + 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_tracking_frame_property = new rviz::StringProperty( "Tracking frame", "odom", "Coordinate frame into which track history should be transformed. Usually the fixed frame of the tracker.", this, SLOT(stylesChanged())); + + 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(scene_node_->createChildSceneNode()); +} + +TargetPersonDisplay::~TargetPersonDisplay() +{ + m_cachedTracks.clear(); +} + +// Clear the visuals by deleting their objects. +void TargetPersonDisplay::reset() +{ + PersonDisplayCommon::reset(); + m_cachedTracks.clear(); +} + +void TargetPersonDisplay::update(float wall_dt, float ros_dt) +{ + // Move map scene node + Ogre::Vector3 mapFramePosition; Ogre::Quaternion mapFrameOrientation; + getContext()->getFrameManager()->getTransform(m_tracking_frame_property->getStdString(), 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 = 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 TargetPersonDisplay::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 = 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 = Ogre::ColourValue(0.0, 1.0, 0.0, 1.0); + Ogre::ColourValue trackColor = Ogre::ColourValue(0.0, 1.0, 0.0, 1.0); + 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 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 TargetPersonDisplay::personVisualTypeChanged() +{ + foreach(const track_map::value_type& entry, m_cachedTracks) + { + const boost::shared_ptr& trackedPersonVisual = entry.second; + trackedPersonVisual->personVisual.reset(); + createPersonVisualIfRequired(trackedPersonVisual->sceneNode.get(), trackedPersonVisual->personVisual); + } + stylesChanged(); +} + +// This is our callback to handle an incoming message. +void TargetPersonDisplay::processMessage(const spencer_tracking_msgs::TargetPerson::ConstPtr& target_msg) +{ + // Get transforms into fixed frame etc. + if(!preprocessMessage(target_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_tracking_frame_property->getStdString(), target_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 encounteredTrackIds; + + boost::shared_ptr trackedPersonVisual; + encounteredTrackIds.insert(target_msg->track_id); + + + // See if we have cached a track with this ID + if (m_cachedTracks.find(target_msg->track_id) != m_cachedTracks.end()) { + trackedPersonVisual = m_cachedTracks[target_msg->track_id]; + } + else { + // Create a new visual representation of the tracked person + trackedPersonVisual = boost::shared_ptr(new TrackedPersonVisual); + m_cachedTracks[target_msg->track_id] = trackedPersonVisual; + + // This scene node is the parent of all visualization elements for the tracked person + trackedPersonVisual->sceneNode = boost::shared_ptr(scene_node_->createChildSceneNode()); + trackedPersonVisual->historySceneNode = boost::shared_ptr(m_trackHistorySceneNode->createChildSceneNode()); + trackedPersonVisual->historyLineSceneNode = boost::shared_ptr(m_trackHistorySceneNode->createChildSceneNode()); + } + + // These values need to be remembered for later use in stylesChanged() + if(target_msg->is_occluded && !target_msg->is_matched){ + trackedPersonVisual->isOccluded = true; + trackedPersonVisual->isMissed = false; + } + else if(!target_msg->is_occluded && !target_msg->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 = 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(target_msg->pose); + setPoseOrientation(currentSceneNode, target_msg->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 newHistoryEntry(new TrackedPersonHistoryEntry); + newHistoryEntry->trackId = target_msg->track_id; + newHistoryEntry->position = newHistoryEntryPosition; // used by history lines (below) even if no shape is set + newHistoryEntry->wasOccluded = target_msg->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& 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(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 " << target_msg->detection_id; + trackedPersonVisual->detectionIdText->setCaption(ss.str()); + + // Track state + ss.str(""); + + if(target_msg->is_occluded && !target_msg->is_matched) + ss << "OCCLUDED"; + else if (!target_msg->is_occluded && !target_msg->is_matched) + ss << "MISSED"; + else + ss << "Target"; + + trackedPersonVisual->stateText->setCaption(ss.str()); + + // Track ID + ss.str(""); + ss << "0"; + 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(target_msg->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!", target_msg->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 = boost::dynamic_pointer_cast(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); + } + + + + // 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 trackIdsToDelete; + for (map >::const_iterator cachedTrackIt = m_cachedTracks.begin(); cachedTrackIt != m_cachedTracks.end(); ++cachedTrackIt) { + if (encounteredTrackIds.end() == encounteredTrackIds.find(cachedTrackIt->first)) { + const boost::shared_ptr& 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::const_iterator setIt = trackIdsToDelete.begin(); setIt != trackIdsToDelete.end(); ++setIt) { + m_cachedTracks.erase(*setIt); + } + + // + // Update status (shown in property pane) + // + ss.str(""); + ss << "target 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_EXPORT_CLASS(spencer_tracking_rviz_plugin::TargetPersonDisplay, rviz::Display) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/target_person_display.h b/messages/visualization/spencer_tracking_rviz_plugin/src/target_person_display.h new file mode 100755 index 0000000..41b0246 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/target_person_display.h @@ -0,0 +1,157 @@ +/* +* 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 TARGET_PERSON_DISPLAY_H +#define TARGET_PERSON_DISPLAY_H + +#ifndef Q_MOC_RUN +#include +#include +#include +#include +#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 shape; + bool wasOccluded; + track_id trackId; + }; + + /// History of a tracked person. + typedef circular_buffer > TrackedPersonHistory; + + /// The visual of a tracked person. + struct TrackedPersonVisual + { + TrackedPersonHistory history; + boost::shared_ptr historyLine; + Ogre::Vector3 positionOfLastHistoryEntry; + + boost::shared_ptr sceneNode, historySceneNode, historyLineSceneNode; + + boost::shared_ptr personVisual; + boost::shared_ptr idText, detectionIdText, stateText; + boost::shared_ptr velocityArrow; + boost::shared_ptr covarianceVisual; + + Ogre::Matrix4 lastObservedPose; + + bool isOccluded, isDeleted, isMissed, hasZeroVelocity; + int numCyclesNotSeen; + }; + + // The TargetPersonDisplay class itself just implements a circular buffer, + // editable parameters, and Display subclass machinery. + class TargetPersonDisplay: public PersonDisplayCommon + { + Q_OBJECT + public: + // Constructor. pluginlib::ClassLoader creates instances by calling + // the default constructor, so make sure you have one. + TargetPersonDisplay() {}; + virtual ~TargetPersonDisplay(); + + // 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::TargetPerson::ConstPtr& msg); + + // All currently active tracks, with unique track ID as map key + typedef map > track_map; + track_map m_cachedTracks; + + // Scene node for track history visualization + boost::shared_ptr m_trackHistorySceneNode; + std::string m_realFixedFrame; + + // User-editable property variables. + rviz::FloatProperty* m_occlusion_alpha_property; + rviz::FloatProperty* m_missed_alpha_property; + rviz::StringProperty* m_tracking_frame_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 // TARGET_PERSON_DISPLAY_H diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_groups_display.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_groups_display.cpp new file mode 100755 index 0000000..160c54f --- /dev/null +++ b/messages/visualization/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 +#include +#include "rviz/selection/selection_manager.h" + +#include "tracked_groups_display.h" +#ifndef Q_MOC_RUN +#include +#include +#include +#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(scene_node_->createChildSceneNode()); + m_groupsSceneNode = boost::shared_ptr(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 separator(","); + tokenizer< char_separator > tokens(groupIDString, separator); + foreach(const string& token, tokens) { + try { m_excludedGroupIDs.insert(lexical_cast(token)); } + catch(bad_lexical_cast &) {} + } + } + m_includedGroupIDs.clear(); + { + string groupIDString = m_included_group_ids_property->getStdString(); + char_separator separator(","); + tokenizer< char_separator > tokens(groupIDString, separator); + foreach(const string& token, tokens) { + try { m_includedGroupIDs.insert(lexical_cast(token)); } + catch(bad_lexical_cast &) {} + } + } + + foreach(boost::shared_ptr 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, m_groupVisuals) { + foreach(boost::shared_ptr& personVisual, groupVisual->personVisuals) { + Ogre::SceneNode* parentSceneNode = personVisual->getParentSceneNode(); + personVisual.reset(); + createPersonVisualIfRequired(parentSceneNode, personVisual); + } + } + stylesChanged(); +} + +void TrackedGroupsDisplay::updateGroupVisualStyles(boost::shared_ptr& 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 groupAssignmentCircle, groupVisual->groupAssignmentCircles) { + groupAssignmentCircle->setColor(groupColor.r, groupColor.g, groupColor.b, groupColor.a); + } + + foreach(boost::shared_ptr& 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 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, 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 = boost::shared_ptr(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 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 groupAssignmentCircle = boost::shared_ptr(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 sceneNode = boost::shared_ptr(scene_node_->createChildSceneNode()); + groupVisual->personVisualSceneNodes.push_back(sceneNode); + + // Create new visual for the person itself, if needed + boost::shared_ptr 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 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 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 newHistoryEntry(new GroupAffiliationHistoryEntry); + newHistoryEntry->shape = boost::shared_ptr(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 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_EXPORT_CLASS(spencer_tracking_rviz_plugin::TrackedGroupsDisplay, rviz::Display) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_groups_display.h b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_groups_display.h new file mode 100755 index 0000000..88c606d --- /dev/null +++ b/messages/visualization/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 +#include +#endif +#include + +#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 shape; + bool wasOccluded, wasSinglePersonGroup; + }; + + /// History of a tracked person. + typedef circular_buffer > GroupAffiliationHistory; + + /// The display which can be added in RViz to display tracked groups. + class TrackedGroupsDisplay: public PersonDisplayCommon + { + 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 > groupAssignmentCircles; + vector > personVisuals; + vector > personVisualSceneNodes; + vector > connectionLines; + boost::shared_ptr 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); + void updateHistoryStyles(); + bool isGroupHidden(group_id groupId); + + // Scene node for group affiliation history visualization + boost::shared_ptr 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 > m_groupVisuals; + + map m_groupAffiliations; + GroupAffiliationHistory m_groupAffiliationHistory; + set 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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_cache.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_cache.cpp new file mode 100755 index 0000000..df8d734 --- /dev/null +++ b/messages/visualization/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 +#include +#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("Tracked persons topic", display, context, update_nh, + boost::bind(&TrackedPersonsCache::processTrackedPersonsMessage, this, _1)); +} + +void TrackedPersonsCache::reset() +{ + m_cachedTrackedPersons.clear(); +} + +const boost::shared_ptr TrackedPersonsCache::lookup(track_id trackId) +{ + CachedTrackedPersonsMap::const_iterator entry = m_cachedTrackedPersons.find(trackId); + if(entry == m_cachedTrackedPersons.end()) return boost::shared_ptr(); + 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(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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_cache.h b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_cache.h new file mode 100755 index 0000000..55f641c --- /dev/null +++ b/messages/visualization/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 +#include +#include +#include +#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 > 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 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* m_tracked_person_subscriber; + rviz::Display* m_display; + rviz::DisplayContext* m_context; + + // Our TrackedPerson memory + CachedTrackedPersonsMap m_cachedTrackedPersons; + }; + + +} + +#endif // TRACKED_PERSONS_CACHE_H diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_display.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_display.cpp new file mode 100755 index 0000000..88fcc9e --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_display.cpp @@ -0,0 +1,509 @@ +/* +* 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 +#include +#include "rviz/selection/selection_manager.h" +#include "tracked_persons_display.h" +#include +#include +#include +#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(); + + 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_tracking_frame_property = new rviz::StringProperty( "Tracking frame", "odom", "Coordinate frame into which track history should be transformed. Usually the fixed frame of the tracker.", this, SLOT(stylesChanged())); + + 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(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_tracking_frame_property->getStdString(), 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 = 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 = 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 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 = 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_tracking_frame_property->getStdString(), 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 encounteredTrackIds; + for (vector::const_iterator trackedPersonIt = msg->tracks.begin(); trackedPersonIt != msg->tracks.end(); ++trackedPersonIt) + { + boost::shared_ptr 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(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(scene_node_->createChildSceneNode()); + trackedPersonVisual->historySceneNode = boost::shared_ptr(m_trackHistorySceneNode->createChildSceneNode()); + trackedPersonVisual->historyLineSceneNode = boost::shared_ptr(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 = 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 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& 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(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 << 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 = boost::dynamic_pointer_cast(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 trackIdsToDelete; + for (map >::const_iterator cachedTrackIt = m_cachedTracks.begin(); cachedTrackIt != m_cachedTracks.end(); ++cachedTrackIt) { + if (encounteredTrackIds.end() == encounteredTrackIds.find(cachedTrackIt->first)) { + const boost::shared_ptr& 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::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_EXPORT_CLASS(spencer_tracking_rviz_plugin::TrackedPersonsDisplay, rviz::Display) diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_display.h b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_display.h new file mode 100755 index 0000000..9e80883 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/tracked_persons_display.h @@ -0,0 +1,156 @@ +/* +* 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 +#include +#include +#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 shape; + bool wasOccluded; + track_id trackId; + }; + + /// History of a tracked person. + typedef circular_buffer > TrackedPersonHistory; + + /// The visual of a tracked person. + struct TrackedPersonVisual + { + TrackedPersonHistory history; + boost::shared_ptr historyLine; + Ogre::Vector3 positionOfLastHistoryEntry; + + boost::shared_ptr sceneNode, historySceneNode, historyLineSceneNode; + + boost::shared_ptr personVisual; + boost::shared_ptr idText, detectionIdText, stateText; + boost::shared_ptr velocityArrow; + boost::shared_ptr 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 + { + 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_map; + track_map m_cachedTracks; + + // Scene node for track history visualization + boost::shared_ptr m_trackHistorySceneNode; + std::string m_realFixedFrame; + + // User-editable property variables. + rviz::FloatProperty* m_occlusion_alpha_property; + rviz::FloatProperty* m_missed_alpha_property; + rviz::StringProperty* m_tracking_frame_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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/covariance_visual.h b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/covariance_visual.h new file mode 100755 index 0000000..b0a7c43 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/covariance_visual.h @@ -0,0 +1,213 @@ +/* +* 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 +#include +#include + + +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 { + 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); + } + }; + +} + +#endif // COVARIANCE_VISUAL_H + diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/mesh_node.h b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/mesh_node.h new file mode 100755 index 0000000..87e5a55 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/mesh_node.h @@ -0,0 +1,190 @@ +/* +* 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 +#ifndef Q_MOC_RUN +#include +#endif +#include +#include // hack to get camera position + +#include +#include +#include +#include +#include +#include +#include + + +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" ); + 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::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::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(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 m_materials; + std::string m_meshResource; + bool m_cameraFacing; + }; + +} + +#endif // MESH_NODE_H diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/person_visual.cpp b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/person_visual.cpp new file mode 100755 index 0000000..db3bdc3 --- /dev/null +++ b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/person_visual.cpp @@ -0,0 +1,229 @@ +/* +* 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 +#ifndef Q_MOC_RUN +#include +#include +#include + +#include +#include +#endif +#include +#include +#include +#include +#include +#include + +#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"; + + /// 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" ); + 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::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::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 diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/person_visual.h b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/person_visual.h new file mode 100755 index 0000000..f57fe67 --- /dev/null +++ b/messages/visualization/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 +#include + +#include +#include +#include +#include + + +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 materials_; + float m_walkingSpeed; + }; + +} + +#endif // PERSON_VISUAL_H diff --git a/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/text_node.h b/messages/visualization/spencer_tracking_rviz_plugin/src/visuals/text_node.h new file mode 100755 index 0000000..1389a33 --- /dev/null +++ b/messages/visualization/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 +#include + +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 diff --git a/messages/visualization/srl_tracking_exporter/CHANGELOG.rst b/messages/visualization/srl_tracking_exporter/CHANGELOG.rst new file mode 100755 index 0000000..727ca1c --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/CHANGELOG.rst @@ -0,0 +1,62 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package srl_tracking_exporter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.2.1 (2020-08-26) +------------------ + +1.2.0 (2020-08-26) +------------------ +* Merge branch 'master' into melodic +* Contributors: Timm Linder + +1.0.11 (2020-08-26) +------------------- +* Merge pull request `#65 `_ from dandedrick/fix-logerror + job_monitor: replace logerror with logerr +* job_monitor: replace logerror with logerr + rospy doesn't export a logerror symbol only logerr. If this was ever hit it + would have caused an exception instead of logging. +* Contributors: Dan Dedrick, Timm Linder + +1.0.10 (2018-09-22) +------------------- +* Merge pull request `#47 `_ from LCAS/master + 1.0.8 +* Contributors: Timm Linder + +1.0.9 (2018-01-17) +------------------ + +1.0.8 (2017-09-22) +------------------ +* Merge pull request `#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 +* Updating lots of utility packages to latest version from SPENCER repo. Licenses updated. +* Adding SVG exporter +* Contributors: Marc Hanheide, Timm Linder diff --git a/messages/visualization/srl_tracking_exporter/CMakeLists.txt b/messages/visualization/srl_tracking_exporter/CMakeLists.txt new file mode 100755 index 0000000..6daf1d8 --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/CMakeLists.txt @@ -0,0 +1,46 @@ +# 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. + +cmake_minimum_required(VERSION 2.8.3) +project(srl_tracking_exporter) +find_package(catkin REQUIRED COMPONENTS rospy tf spencer_tracking_msgs eigen_conversions tf_conversions message_generation) + +add_service_files( + FILES + JobFinished.srv +) + +generate_messages( + DEPENDENCIES + std_msgs +) + +catkin_package() + +install(PROGRAMS scripts/tracks_to_svg.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) \ No newline at end of file diff --git a/messages/visualization/srl_tracking_exporter/LICENSE b/messages/visualization/srl_tracking_exporter/LICENSE new file mode 100755 index 0000000..325ac7a --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/LICENSE @@ -0,0 +1,29 @@ +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. diff --git a/messages/visualization/srl_tracking_exporter/package.xml b/messages/visualization/srl_tracking_exporter/package.xml new file mode 100755 index 0000000..47f348f --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/package.xml @@ -0,0 +1,28 @@ + + srl_tracking_exporter + 1.2.1 + + Various exporters e.g. for SVG renderings + + Timm Linder + + BSD + + Timm Linder + + catkin + + rospy + tf + spencer_tracking_msgs + eigen_conversions + tf_conversions + message_generation + + rospy + tf + spencer_tracking_msgs + eigen_conversions + tf_conversions + message_generation +roslibroslib diff --git a/messages/visualization/srl_tracking_exporter/screenshots/groups_to_svg.png b/messages/visualization/srl_tracking_exporter/screenshots/groups_to_svg.png new file mode 100755 index 0000000..5f49dc3 Binary files /dev/null and b/messages/visualization/srl_tracking_exporter/screenshots/groups_to_svg.png differ diff --git a/messages/visualization/srl_tracking_exporter/screenshots/tracks_to_svg.png b/messages/visualization/srl_tracking_exporter/screenshots/tracks_to_svg.png new file mode 100755 index 0000000..992f3ea Binary files /dev/null and b/messages/visualization/srl_tracking_exporter/screenshots/tracks_to_svg.png differ diff --git a/messages/visualization/srl_tracking_exporter/screenshots/tracks_to_svg_animated.png b/messages/visualization/srl_tracking_exporter/screenshots/tracks_to_svg_animated.png new file mode 100755 index 0000000..cea96b4 Binary files /dev/null and b/messages/visualization/srl_tracking_exporter/screenshots/tracks_to_svg_animated.png differ diff --git a/messages/visualization/srl_tracking_exporter/scripts/__init__.py b/messages/visualization/srl_tracking_exporter/scripts/__init__.py new file mode 100755 index 0000000..e69de29 diff --git a/messages/visualization/srl_tracking_exporter/scripts/groups_to_svg.py b/messages/visualization/srl_tracking_exporter/scripts/groups_to_svg.py new file mode 100755 index 0000000..a278bdd --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/scripts/groups_to_svg.py @@ -0,0 +1,117 @@ +#!/usr/bin/env python + +# 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. + +""" +Renders groups (spencer_tracking_msg/TrackedGroups) and their tracks to an SVG file, (C)2014 Timm Linder +Based upon tracks_to_svg.py, using inheritance +Parameters in addition to those of tracks_to_svg.py: + None +""" + +import os, sys, math, time +import svgwrite + +import roslib, rospy; roslib.load_manifest('srl_tracking_exporter') +from spencer_tracking_msgs.msg import TrackedGroups, TrackedGroup +from tracks_to_svg import * + +### Database for storing information ### +class GroupDatabase(TrackDatabase): + def __init__(self): + super(GroupDatabase, self).__init__() + self.groups = dict() + +### Group ### +class Group(object): + def __init__(self): + pass + +### Group receiver ### +class GroupReceiver(TrackReceiver): + def __init__(self): + super(GroupReceiver, self).__init__() + self.firstGroupsReceived = False + self.groupSubscriber = rospy.Subscriber("/spencer/perception/tracked_groups", TrackedGroups, self.newGroupsReceived) + self.groupCycle = -1 + + def stop(self): + super(GroupReceiver, self).stop() + self.groupSubscriber.unregister() + + def storeGroupId(self, track, step, group_id): + if not hasattr(track, 'group_ids'): + track.group_ids = dict() + track.group_ids[step] = group_id + + def newGroupsReceived(self, trackedGroups): + with self.mutex: + if not self.firstGroupsReceived: + self.firstGroupsReceived = True + rospy.loginfo("First groups(s) received at %d secs" % int(trackedGroups.header.stamp.secs)) + + self.groupCycle += 1 + for trackedGroup in trackedGroups.groups: + for track_id in trackedGroup.track_ids: + track = self.getTrack(track_id) + self.storeGroupId(track, self.groupCycle, trackedGroup.group_id) + +### Group renderer ### +class GroupRenderer(TrackRenderer): + def getTrackColor(self, track_id, trackCycle = 0): + group_id = None + try: + track = database.tracks[track_id] + if hasattr(track, 'group_ids'): + group_id = track.group_ids[trackCycle] + except (IndexError, KeyError) as e: + pass + + if group_id == None or trackCycle == 0: + return '#888' + + return super(GroupRenderer, self).getTrackColor(group_id) # use group id instead of track id for coloring + +### Main method ### +def main(argv=None): + initNode('groups_to_svg') + + database = GroupDatabase() + + groupReceiver = GroupReceiver() + groupRenderer = GroupRenderer() + + groupReceiver.spin() + groupRenderer.generateSVG() + + sys.exit(0) + +### Globals ### +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/messages/visualization/srl_tracking_exporter/scripts/job_monitor.py b/messages/visualization/srl_tracking_exporter/scripts/job_monitor.py new file mode 100755 index 0000000..b7048a6 --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/scripts/job_monitor.py @@ -0,0 +1,77 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# 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. + +""" +Waits for one or multiple nodes to finish processing (which is reported by calling a service method), +then this node kills itself. This is useful in a ROS launch file to quit the entire launch script if +required processing is finished (by setting required=true on node_monitor). + +Parameters: + _count (int): Number of jobs to wait for. +""" + +import os, sys, time +import roslib, rospy, tf; roslib.load_manifest('srl_tracking_exporter') +from srl_tracking_exporter.srv import JobFinished + +### Callback when a job is finished ### +def jobFinished(request): + global numFinishedJobs + numFinishedJobs += 1 + rospy.loginfo("Job has finished: %s, %d job(s) remaining!" % (request.job_name, numTotalJobs - numFinishedJobs)) + +### Main method ### +def main(argv=None): + rospy.init_node("job_monitor") + + global numTotalJobs + numTotalJobs = rospy.get_param('~count', 0) + if 0 == numTotalJobs: + rospy.logerr("Job count is 0, not monitoring any jobs, terminating!") + + else: + rospy.loginfo("Will run until %d jobs have been completed." % numTotalJobs) + + jobFinishedService = rospy.Service('job_monitor', JobFinished, jobFinished) + while numFinishedJobs < numTotalJobs and not rospy.is_shutdown(): + time.sleep(0.1) + + if numFinishedJobs < numTotalJobs: + rospy.logwarn("Job monitor has been requested to terminate while %d job(s) were still active." % (numTotalJobs - numFinishedJobs)) + else: + rospy.loginfo("All jobs have finished! Terminating job monitor.") + sys.exit(0) + +### Globals ### +numTotalJobs = 0 +numFinishedJobs = 0 +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/messages/visualization/srl_tracking_exporter/scripts/svg_frame.html b/messages/visualization/srl_tracking_exporter/scripts/svg_frame.html new file mode 100755 index 0000000..fec2ab9 --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/scripts/svg_frame.html @@ -0,0 +1,217 @@ + + + + + + + + + + + + +
+ SVG playback controls + + + + + + + + +    + Playback rate: + + + +    + Current position: + +  secs + +    + Zoom: + + + + +    + Options: + Repeat + + + + + (—) + +
+ +
+ + + + +
+ + + + \ No newline at end of file diff --git a/messages/visualization/srl_tracking_exporter/scripts/tracks_to_svg.py b/messages/visualization/srl_tracking_exporter/scripts/tracks_to_svg.py new file mode 100755 index 0000000..797b068 --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/scripts/tracks_to_svg.py @@ -0,0 +1,618 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +# 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. + +""" +Renders tracks (spencer_tracking_msg/TrackedPersons) and odometry to an SVG file, (C)2014 Timm Linder +Parameters: + _odom (bool): render odometry published at /odom?, default: True + _detections (bool): render detections as rectangles?, default: True + _grid (bool): render metric grid?, default: True + _style (string): rendering style for tracks, either "lines" or "points", default: "points" + _colormap (string): colormap to use for tracks, either "rainbow", "srl" or "srlalternative", default: "rainbow" + _occlusions (bool): if true, occlusions will be rendered as circles without fill color (only for style "points"), default: True + _labels (bool): render track IDs? default: True + _timestamps (bool): render track deletion/creation timestamps? default: True + _velocities (bool): render track and odometry velocities (as < shape), default: True + _start (float): time index at which to start recording, default: 0 + _end (float): time index at which to stop recording, default: infinity + _prefix (string): prefix for filename of rendered SVGs, default: "Tracks " + _autoquit (bool): if true, will quit automatically if no data is received for 10 seconds, default: True + _animate (bool): add animation, default: False + _speed (float): animation speed, default: 1.0 + _history (bool): show track history in animation, default: True + _cycles (bool): show cycle number on track (lots of text elements!), default: False +""" + +import os, sys, math, time, socket, xmlrpclib, signal, codecs +import svgwrite +from multiprocessing import Lock + +import roslib, rospy, tf; roslib.load_manifest('srl_tracking_exporter') +import geometry_msgs.msg +from spencer_tracking_msgs.msg import TrackedPersons, TrackedPerson, DetectedPersons, DetectedPerson +from nav_msgs.msg import Odometry +from srl_tracking_exporter.srv import JobFinished + +### Database for storing information ### +class TrackDatabase(object): + def __init__(self): + self.tracks = dict() + self.trackStamps = [] + self.detections = [] + self.detectionStamps = [] + self.odom = [] + self.odomStamps = [] + self.minx = self.miny = float("+inf") + self.maxx = self.maxy = float("-inf") + self.firstTracksReceivedAt = None + self.lastTracksReceivedAt = None + self.firstDataReceivedAt = None + self.lastDataReceivedAt = None + +### Detection ### +class Detection(object): + def __init__(self): + self.position = None + self.cycle = None + +### Track ### +class Track(object): + def __init__(self): + self.positions = [] + self.velocities = [] + self.occluded = [] + self.cycles = [] + self.createdAt = None + self.deletedAt = None + +### Track receiver ### +class TrackReceiver(object): + def __init__(self): + self.mutex = Lock() + self.firstTracksReceived = False + self.trackCycle = -1 + self.detectionCycle = -1 + self.targetFrame = rospy.get_param('~frame', "odom") + + self.lastDataAt = None + self.startLimit = rospy.get_param('~start', 0.0) + self.endLimit = rospy.get_param('~end', float("inf")) + self.autoQuit = rospy.get_param('~auto_quit', True) + + self.transformListener = tf.TransformListener() + self.numDetectionTfErrors = 0 + self.numTrackTfErrors = 0 + self.numOdometryTfErrors = 0 + + self.detectionSubscriber = rospy.Subscriber("/spencer/perception/detected_persons", DetectedPersons, self.newDetectionsReceived) + self.trackSubscriber = rospy.Subscriber("/spencer/perception/tracked_persons", TrackedPersons, self.newTracksReceived) + self.odomSubscriber = rospy.Subscriber("/odom", Odometry, self.odometryReceived) + + rospy.loginfo("Will start processing tracks at time index %f and end at %f" % (self.startLimit, self.endLimit)) + if self.autoQuit: + rospy.loginfo("Will end processing if no data is received for more than 10 seconds after last data.") + + rospy.loginfo("Listening for tracks... press CTRL+C to stop and generate SVG output.") + + def stop(self): + self.detectionSubscriber.unregister() + self.trackSubscriber.unregister() + self.odomSubscriber.unregister() + + def dataReceivedAt(self, secs): + database.lastDataReceivedAt = secs + if database.firstDataReceivedAt is None: + database.firstDataReceivedAt = secs + + def positionPassesSanityCheck(self, pos): + if math.isnan(pos.x) or math.isnan(pos.y): + return False + + MAX_REASONABLE_DISTANCE = 1000.0 # in meters + if pos.x > MAX_REASONABLE_DISTANCE or pos.y > MAX_REASONABLE_DISTANCE: + return False + + return True + + def updateCoordinateLimits(self, pos): + database.minx = min(database.minx, pos.x) + database.miny = min(database.miny, pos.y) + database.maxx = max(database.maxx, pos.x) + database.maxy = max(database.maxy, pos.y) + + self.lastDataAt = time.time() + + def transformIntoTargetFrame(self, poseWithCovariance, header): + poseStamped = geometry_msgs.msg.PoseStamped() + poseStamped.header = header + poseStamped.pose = poseWithCovariance.pose + transformedPose = self.transformListener.transformPose(self.targetFrame, poseStamped) + if math.isnan(transformedPose.pose.position.x) or math.isnan(transformedPose.pose.position.y): + rospy.logwarn("Encountered NaN value in position, raising a TF exception...") + raise tf.Exception + return transformedPose + + def getTrack(self, track_id): + if track_id in database.tracks: + track = database.tracks[track_id] + else: + track = Track() + database.tracks[track_id] = track + return track + + def newDetectionsReceived(self, detectedPersons): + time = rospy.get_time() + if time < self.startLimit or time > self.endLimit: + return + + with self.mutex: + self.detectionCycle += 1 + secs = detectedPersons.header.stamp.to_sec() + database.detectionStamps.append(secs) + self.dataReceivedAt(secs) + + for detectedPerson in detectedPersons.detections: + detection = Detection() + + try: + pos = self.transformIntoTargetFrame(detectedPerson.pose, detectedPersons.header).pose.position + except tf.Exception: + self.numDetectionTfErrors += 1 + if self.numDetectionTfErrors < 2: + rospy.logwarn("TF exception encountered while transforming detection position into target frame!") + return + + pos.y = -pos.y # due to SVG conventions + detection.position = (pos.x, pos.y) + detection.cycle = self.detectionCycle + + if self.positionPassesSanityCheck(pos): + self.updateCoordinateLimits(pos) + database.detections.append(detection) + else: + rospy.logwarn("Ignoring DetectedPerson because its position does not pass heuristic sanity check:\n" + str(detectedPerson)) + + def newTracksReceived(self, trackedPersons): + time = rospy.get_time() + if time < self.startLimit or time > self.endLimit: + return + + with self.mutex: + secs = trackedPersons.header.stamp.to_sec() + database.lastTracksReceivedAt = secs + self.dataReceivedAt(secs) + self.trackCycle += 1 + + if not self.firstTracksReceived: + self.firstTracksReceived = True + database.firstTracksReceivedAt = secs + rospy.loginfo("First track(s) received at %f secs" % secs) + + for trackedPerson in trackedPersons.tracks: + track = self.getTrack(trackedPerson.track_id) + + try: + pos = self.transformIntoTargetFrame(trackedPerson.pose, trackedPersons.header).pose.position + except tf.Exception: + self.numTrackTfErrors += 1 + if self.numTrackTfErrors < 2: + rospy.logwarn("TF exception encountered while transforming track position into target frame!") + return + + pos.y = -pos.y # due to SVG conventions + + if self.positionPassesSanityCheck(pos): + track.positions.append( (pos.x, pos.y) ) + track.cycles.append(self.trackCycle) + self.updateCoordinateLimits(pos) + + vel = trackedPerson.twist.twist.linear # FIXME: Rotate into target frame! + vel.y = -vel.y # due to SVG conventions + track.velocities.append( (vel.x, vel.y) ) + + track.occluded.append( trackedPerson.is_occluded ) + + if track.createdAt is None: + track.createdAt = secs + track.deletedAt = secs # will be updated as long as track exists + else: + rospy.logwarn("Ignoring TrackedPerson because its position does not pass heuristic sanity check:\n" + str(trackedPerson)) + + database.trackStamps.append(secs) + + def odometryReceived(self, odom): + try: + posInTargetFrame = self.transformIntoTargetFrame(odom.pose, odom.header).pose.position + posInTargetFrame.y = -posInTargetFrame.y + database.odom.append( (posInTargetFrame.x, posInTargetFrame.y) ) + + secs = odom.header.stamp.to_sec() + database.odomStamps.append(secs) + self.dataReceivedAt(secs) + + self.updateCoordinateLimits(posInTargetFrame) + except tf.Exception: + self.numOdometryTfErrors += 1 + if self.numOdometryTfErrors < 2: + rospy.logwarn("TF exception encountered while transforming odometry into target frame!") + pass + + def spin(self): + while not (quitRequested or rospy.is_shutdown() or rospy.get_time() > self.endLimit): + time.sleep(0.1) + if self.autoQuit and self.firstTracksReceived and not self.lastDataAt is None and time.time() - self.lastDataAt > 10.0: + rospy.loginfo("No data received for more than 10 seconds, quitting.") + break + self.stop() + +### Track renderer ### +class TrackRenderer(object): + def __init__(self): + self.colormap = rospy.get_param('~colormap', 'rainbow') + self.style = rospy.get_param('~style', 'points') + self.showOcclusions = rospy.get_param('~occlusions', True) + self.filenamePrefix = rospy.get_param('~prefix', 'Tracks ') + self.gridEnabled = rospy.get_param('~grid', True) + self.odomEnabled = rospy.get_param('~odom', True) + self.detectionsEnabled = rospy.get_param('~detections', True) + self.velocitiesEnabled = rospy.get_param('~velocities', True) + self.labelsEnabled = rospy.get_param('~labels', True) + self.timestampsEnabled = rospy.get_param('~timestamps', True) + self.animationEnabled = rospy.get_param('~animate', False) + self.animationSpeed = rospy.get_param('~speed', 1.0) + self.historyEnabled = rospy.get_param('~history', True) + self.cyclesEnabled = rospy.get_param('~cycles', False) + + def getTrackColor(self, track_id, trackCycle = 0): + index = track_id + + if self.colormap == 'srl' or self.colormap == 'srlalternative': + palette = [ 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 + + if self.colormap == 'srlalternative': # swap rows and columns + index = index % len(palette) + index = (index % 6)*4 + (index / 6) + else: + palette = [0xaf1f90, 0x000846, 0x00468a, 0x00953d, 0xb2c908, 0xfcd22a, 0xffa800, 0xff4500, 0xe0000b, 0xb22222] # rainbow color map + + return '#%06x' % palette[index % len(palette)] + + def animateElement(self, element, currentCycle, cycleStamps, showHistory = True): + if not self.animationEnabled: + return + + firstStamp = database.firstDataReceivedAt + lastStamp = database.lastDataReceivedAt + duration = lastStamp - firstStamp + keyTimes = "0;" + opacityValues = [0.0, 0.0, 1.0] + + if currentCycle == 0: + keyTimes += "0" + else: + keyTimes += str((cycleStamps[currentCycle - 1] - firstStamp) / duration) + + keyTimes += ";" + str((cycleStamps[currentCycle] - firstStamp) / duration) + + if not showHistory: + if currentCycle < len(cycleStamps) - 1: + keyTimes += ";" + str((cycleStamps[currentCycle + 1] - firstStamp) / duration) + ";1" + opacityValues.append(0.0) + opacityValues.append(0.0) + else: + keyTimes +=";1" + opacityValues.append(1.0) + + animation = svg.animate(attributeName='opacity', values=opacityValues, keyTimes=keyTimes, calcMode='discrete', dur="%fs" % (duration / self.animationSpeed), repeatCount='indefinite', fill='freeze') + element.add(animation) + + def renderGrid(self, left, top, width, height): + pattern = svg.defs.add(svg.pattern(id="gridPattern", size=(1, 1), patternUnits="userSpaceOnUse")) + pattern.add(svg.path(d="M 1 0 L 0 0 0 1", fill="none", stroke="#CCC", stroke_width=0.01)) + content.add(svg.rect(id='background', insert=(left, top), size=(width, height), fill="white")) + content.add(svg.rect(id='grid', insert=(left, top), size=(width, height), fill="url(#gridPattern)")) + + def renderOdometry(self): + if len(database.odom) > 0: + content.add(svg.polyline(database.odom, id='odom', stroke='#999', stroke_width=0.02, stroke_linecap='round', fill='none')) + + if self.velocitiesEnabled or self.animationEnabled: + odomVelocityGroup = content.add(svg.g(id='odomVelocities')) + lookback = 3 + for step in range(lookback, len(database.odom), lookback): + pos = database.odom[step] + previousPos = database.odom[step-lookback] + avgPos = database.odom[step-lookback//2] + deltatime = database.odomStamps[step] - database.odomStamps[step-lookback] + vel = ((pos[0] - previousPos[0]) / deltatime, (pos[1] - previousPos[1]) / deltatime) + theta = math.degrees(math.atan2(vel[1], vel[0]) + math.pi) + + sx = 0.2 * math.hypot(vel[0], vel[1]) + sy = 0.2 + arrowCoords = [(sx,-sy), (0,0), (sx,sy)] + + arrow = svg.polyline(arrowCoords, id='arrow', stroke_width=0.015, stroke_opacity=0.35, stroke='#999', fill='none') + arrow.translate(avgPos) + arrow.rotate(theta) + + self.animateElement(arrow, step-lookback, database.odomStamps) + odomVelocityGroup.add(arrow) + + def renderVelocities(self): + velocityGroup = content.add(svg.g(id='velocities')) + + # Track velocities + for track_id, track in database.tracks.iteritems(): + trackVelocityGroup = velocityGroup.add(svg.g(id='velocities%d' % track_id)) + + for step in range(0, len(track.positions)): + vel = track.velocities[step] + pos = track.positions[step] + theta = math.degrees(math.atan2(vel[1], vel[0]) + math.pi) + + sx = 0.2 * math.hypot(vel[0], vel[1]) + sy = 0.2 + arrowCoords = [(sx,-sy), (0,0), (sx,sy)] + + arrow = svg.polyline(arrowCoords, id='arrow', stroke_width=0.015, stroke_opacity=0.35, stroke=self.getTrackColor(track_id), fill='none') + arrow.translate(pos) + arrow.rotate(theta) + + self.animateElement(arrow, track.cycles[step], database.trackStamps) + trackVelocityGroup.add(arrow) + + def renderTrack(self, track_id, track): + trackGroup = content.add(svg.g(id='track%d' % track_id)) + + #if self.style == 'lines': + if len(track.positions) > 0: + trackGroup.add(svg.polyline(track.positions, stroke=self.getTrackColor(track_id), stroke_width=0.01, stroke_linecap='round', fill='none')) + + if self.style == 'points': + for step in range(0, len(track.positions)): + #alpha = 0.5 if track.occluded[step] and showOcclusions else 1.0 + alpha = 1.0 + trackCycle = track.cycles[step] + if not track.occluded[step] or not self.showOcclusions: + trackVisual = svg.circle(track.positions[step], r=0.1, fill=self.getTrackColor(track_id, trackCycle)) + + else: + trackVisual = svg.circle(track.positions[step], r=0.09, fill='none', stroke=self.getTrackColor(track_id, trackCycle), stroke_width=0.01) + + self.animateElement(trackVisual, trackCycle, database.trackStamps, self.historyEnabled) + trackGroup.add(trackVisual) + + def renderDetections(self): + detectionsGroup = content.add(svg.g(id='detections')) + for detection in database.detections: + size = 0.075 + rect = svg.rect(size=(size, size), fill='none', stroke='#999', stroke_width=0.005) + rect.translate((detection.position[0] - 0.5*size, detection.position[1] - 0.5*size)) + rect.rotate(45) + + self.animateElement(rect, detection.cycle, database.detectionStamps, self.historyEnabled) + detectionsGroup.add(rect) + + def renderTrackLabels(self): + labelGroup = content.add(svg.g(id='labels')) + for track_id, track in database.tracks.iteritems(): + trackColor = self.getTrackColor(track_id) + if len(track.positions) > 0: + pos = track.positions[-1] + pos = (pos[0], pos[1] - 0.15) + fontSize = 0.25 + labelGroup.add(svg.text(str(track_id), insert=pos, text_anchor="middle", fill=trackColor, stroke='white', stroke_width=0.02, font_size=fontSize)) + labelGroup.add(svg.text(str(track_id), insert=pos, text_anchor="middle", fill=trackColor, font_size=fontSize)) + + def renderTimestamps(self): + timestampGroup = content.add(svg.g(id='timestamps')) + for track_id, track in database.tracks.iteritems(): + if len(track.positions) == 0: + continue + + trackColor = self.getTrackColor(track_id) + fontSize = 0.06 + + # Creation + pos = track.positions[0] + pos = (pos[0], pos[1] + 0.17) + timestamp = "+%.1f" % (track.createdAt - database.firstDataReceivedAt) + creationTimestampGroup = content.add(svg.g()) + creationTimestampGroup.add(svg.text(timestamp, insert=pos, text_anchor="middle", fill=trackColor, stroke='white', stroke_width=0.02, font_size=fontSize)) + creationTimestampGroup.add(svg.text(timestamp, insert=pos, text_anchor="middle", fill=trackColor, font_size=fontSize)) + + #self.animateElement(creationTimestampGroup, 1, [track.createdAt - 0.01, track.createdAt]) + timestampGroup.add(creationTimestampGroup) + + # Deletion + pos = track.positions[-1] + pos = (pos[0], pos[1] + 0.17) + timestamp = "-%.1f" % (track.deletedAt - database.firstDataReceivedAt) + deletionTimestampGroup = content.add(svg.g()) + deletionTimestampGroup.add(svg.text(timestamp, insert=pos, text_anchor="middle", fill=trackColor, stroke='white', stroke_width=0.02, font_size=fontSize)) + deletionTimestampGroup.add(svg.text(timestamp, insert=pos, text_anchor="middle", fill=trackColor, font_size=fontSize)) + + timestampGroup.add(deletionTimestampGroup) + + def renderCycles(self): + cyclesGroup = content.add(svg.g(id='cycles')) + for track_id, track in database.tracks.iteritems(): + trackColor = self.getTrackColor(track_id) + fontSize = 0.06 + + for i in xrange(0, len(track.positions), 2): + pos = track.positions[i] + pos = (pos[0], pos[1]) + cycleText = str(track.cycles[i]) + cyclesGroup.add(svg.text(cycleText, insert=pos, text_anchor="middle", fill=trackColor, stroke='white', stroke_width=0.02, font_size=fontSize)) + cyclesGroup.add(svg.text(cycleText, insert=pos, text_anchor="middle", fill=trackColor, font_size=fontSize)) + + def generateSVG(self): + rospy.loginfo("Generating SVG...") + + if database.minx == float("+inf") or database.miny == float("+inf"): + rospy.logwarn("Empty data, aborting!") + return + + # Calculate dimensions + left = database.minx - 0.5 + top = database.miny - 0.5 + width = database.maxx - database.minx + 1 + height = database.maxy - database.miny + 1 + + # Create SVG + global svg, content + filename = '%s%s.svg' % (self.filenamePrefix, time.strftime("%Y-%m-%d %H:%M:%S")) + svg = svgwrite.Drawing(filename, profile='full', size=('%f' % (150*width), '%f' % (150*height)), viewBox=('%f %f %f %f' % (left, top, width, height))) + content = svg.g(id='content') + svg.add(content) + + # Metric grid + if self.gridEnabled: + self.renderGrid(left, top, width, height) + + # Odometry + if self.odomEnabled: + self.renderOdometry() + + # Velocities + if self.velocitiesEnabled: + self.renderVelocities() + + # Tracks + rospy.loginfo("Writing %d tracks..." % len(database.tracks)) + for track_id, track in database.tracks.iteritems(): + self.renderTrack(track_id, track) + + # Detections + if self.detectionsEnabled: + self.renderDetections() + + # Track labels + if self.labelsEnabled: + self.renderTrackLabels() + + # Cycle numbers on tracks + if self.cyclesEnabled: + self.renderCycles() + + # Creation/deletion timestamps + if self.timestampsEnabled: + self.renderTimestamps() + + # Write SVG + rospy.loginfo("Starting to write SVG...") + if not self.animationEnabled: + svg.save() + else: + htmlTemplateFilename = roslib.packages.get_pkg_dir('srl_tracking_exporter') + "/scripts/svg_frame.html" + + with open (htmlTemplateFilename, "r") as htmlTemplate: + htmlContent = htmlTemplate.read() + + svgXml = svg.tostring() + htmlOutput = htmlContent.replace("", svgXml) + + filename = filename + ".html" + with codecs.open (filename, "w", "utf-8") as outputFile: + outputFile.write(htmlOutput) + + rospy.loginfo("SVG generation finished! Output filename: " + filename) + + # Notify job monitor, if running + try: + jobFinished = rospy.ServiceProxy('job_monitor', JobFinished) + jobFinished(rospy.get_name()) + except rospy.ServiceException, e: + rospy.loginfo("Failed to notify job monitor (maybe not running?): %s" % e) + + +### Signal handler for CTRL+C ### +def __signal_handler__(signal, frame): + global quitRequested + quitRequested = True + +### Handle CTRL+C ### +def __installSignalHandler__(): + signal.signal(signal.SIGINT, __signal_handler__) + signal.signal(signal.SIGTERM, __signal_handler__) + +### Makes sure that ROS master is running ### +def __ensureMasterRunning__(): + caller_id = '/script' + m = xmlrpclib.ServerProxy(os.environ['ROS_MASTER_URI']) + + messageShown = False + while not quitRequested: + try: + code, msg, val = m.getSystemState(caller_id) + return + except socket.error: + if not messageShown: + messageShown = True + print("Waiting for ROS master... please make sure that roscore is running!") + time.sleep(0.1) + sys.exit(0) + +### Node initializer, also installs signal handler and waits for ROS master ### +def initNode(nodeName): + # Ensure that ROS master is running, else wait + __installSignalHandler__() + __ensureMasterRunning__() + + # Initialize ROS node + rospy.init_node(nodeName, disable_signals=True) + +### Main method ### +def main(argv=None): + initNode('tracks_to_svg') + + # Create receiver and renderer and load parameters (do this at startup so script doesn't crash if rosmaster is terminated) + trackReceiver = TrackReceiver() + trackRenderer = TrackRenderer() + + trackReceiver.spin() + trackRenderer.generateSVG() + + sys.exit(0) + +### Globals ### +quitRequested = False +svg = None +content = None +database = TrackDatabase() +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/messages/visualization/srl_tracking_exporter/srv/JobFinished.srv b/messages/visualization/srl_tracking_exporter/srv/JobFinished.srv new file mode 100755 index 0000000..8b51c32 --- /dev/null +++ b/messages/visualization/srl_tracking_exporter/srv/JobFinished.srv @@ -0,0 +1,2 @@ +string job_name +--- \ No newline at end of file diff --git a/mono_following/scripts/mono_following/identifiers/base_identifier.py b/mono_following/scripts/mono_following/identifiers/base_identifier.py new file mode 100644 index 0000000..21b0a10 --- /dev/null +++ b/mono_following/scripts/mono_following/identifiers/base_identifier.py @@ -0,0 +1,44 @@ +import os +import numpy as np +from abc import ABCMeta, abstractmethod + + +class BaseIdentifier(metaclass=ABCMeta): + """Base class for target person identifier.""" + + def __init__(self): + super(BaseIdentifier, self).__init__() + self.target_id = None + self.visdom = None + self.params = None + self.frame_id = 0 + self.newest_loss = -1 + + @abstractmethod + def identify(self, *args, **kwargs): + """Identify the target person.""" + pass + + @abstractmethod + def extract_features(self, frame_id, image, tracks: dict): + """extract image patches features based on the tracks information + frame_id (int): frame id + image (array): with shape (3, width, height) + tracks (dict): {id(int):bbox[tl_x, tl_y, br_x, br_y]} + """ + pass + + @abstractmethod + def update_memory(self, tracklets: dict, target_id: int): + pass + + @abstractmethod + def update_classifier(self): + """ + return loss + """ + pass + + @abstractmethod + def predict(self, tracklets: dict, state): + pass \ No newline at end of file diff --git a/mono_following/scripts/mono_following/identifiers/part_identifier.py b/mono_following/scripts/mono_following/identifiers/part_identifier.py new file mode 100644 index 0000000..ae39bc8 --- /dev/null +++ b/mono_following/scripts/mono_following/identifiers/part_identifier.py @@ -0,0 +1,37 @@ +from .base_identifier import BaseIdentifier +import torch +import time +from ..utils import * + + + + +class PartIdentifier(BaseIdentifier): + def __init__(self, params): + self.target_id = -1 + self.target_conf = -1 + + self.params = params + self.state = None + self.classifier = None + + ### Counters ### + self.feature_extraction_times = AverageMeter() + self.identification_times = AverageMeter() + + ### set hyperparameter for reid model ### + losses_weights = { + GLOBAL: {'id': 1., 'tr': 0.}, + FOREGROUND: {'id': 0., 'tr': 0.}, + CONCAT_PARTS: {'id': 1., 'tr': 0.}, # hanjing + PARTS: {'id': 0., 'tr': 1.} + } + + ### initialize the reid model ### + + + ### initialize the classifier ### + rpf_model.reid.head.GiLt = GiLtLoss(losses_weights=losses_weights, use_visibility_scores=True, triplet_margin=self.params.triplet_margin, ce_smooth=self.params.ce_smooth,loss_name=self.params.triplet_loss, use_gpu=True) + self.classifier = name_match.classifiers[self.params.agent](params=self.params, reid_model=rpf_model.reid) + self.state = InitialState(self.params) + diff --git a/mono_following/scripts/mono_following/mono_following_node.py b/mono_following/scripts/mono_following/mono_following_node.py index 0e90d85..2c9bf9f 100755 --- a/mono_following/scripts/mono_following/mono_following_node.py +++ b/mono_following/scripts/mono_following/mono_following_node.py @@ -1,5 +1,6 @@ #! /usr/bin/env python3 import numpy as np +import torch import ros_numpy import cv2 from tqdm import main @@ -7,7 +8,7 @@ import os.path as osp # My class -from tracklet import Tracklet +from track_center.part_tracklet import PartTracklet from states.initial_state import InitialState from descriminator import Descriminator # standard ROS message @@ -23,11 +24,28 @@ # from mono_following.msg import Box from sensor_msgs.msg import Joy - +from .utils import AverageMeter, maybe_cuda, mini_batch_deep_part_features class MonoFollowing: def __init__(self): - ### Some parameters for debug and testing ### + self._init_identifier() + + self._init_subscriber() + + self._init_publisher() + + ### Node is already set up ### + rospy.loginfo("Mono Following Node is Ready!") + rospy.spin() + + def _init_identifier(self): + ### Our parameters ### + self.state = InitialState() + self.identifier = Descriminator() + + + + def _init_subscriber(self): self.evaluate = rospy.get_param("~evaluate") if self.evaluate: self.STORE_DIR = rospy.get_param("~track_store_dir") @@ -36,14 +54,6 @@ def __init__(self): self.result_fp = open(self.result_fname, "w") self.num = 0 - ### Our parameters ### - self.state = InitialState() - self.descriminator = Descriminator() - - ### Our necessary topics ### - # listen to the transform tree - # self.tf_listener = tf.TransformListener() - # subscribe joystick to dynamically choose the target self.reselect_target = False self.joystick = rospy.Subscriber('/bluetooth_teleop/joy', Joy, self.joystick_callback) @@ -52,29 +62,24 @@ def __init__(self): self.imageSubscriber = message_filters.Subscriber(imageTopic, Image) trackedPersonsTopic = "/mono_tracking/tracks" self.tracks_sub = message_filters.Subscriber(trackedPersonsTopic, TrackedPersons) - tss = message_filters.ApproximateTimeSynchronizer([self.tracks_sub, self.imageSubscriber], queue_size=20, slop=0.8) - tss.registerCallback(self.callback) - + self.tss = message_filters.ApproximateTimeSynchronizer([self.tracks_sub, self.imageSubscriber], queue_size=20, slop=0.8) + self.tss.registerCallback(self.callback) + + def _init_publisher(self): # publish image for visualization self.image_pub = rospy.Publisher("/mono_following/vis_image", Image, queue_size=1) # publish image patches for visualization self.patches_pub = rospy.Publisher("/mono_following/patches", Image, queue_size=1) # publish target information self.target_pub = rospy.Publisher("/mono_following/target", TargetPerson, queue_size=1) - - - - ### Node is already set up ### - rospy.loginfo("Mono Following Node is Ready!") - rospy.spin() def callback(self, tracks_msg, image_msg): # print("CALLBACK") # get messages if self.reselect_target: - self.init_model() - self.reselect_target = False - + self._re_init_model() + + ### Create tracklets ### self.tracks = {} if image_msg.encoding == "rgb8": image = ros_numpy.numpify(image_msg) @@ -84,18 +89,18 @@ def callback(self, tracks_msg, image_msg): for track in tracks_msg.tracks: if track.pose.pose.position.x == 0 or track.is_matched == False: continue - self.tracks[track.track_id] = Tracklet(tracks_msg.header, track, image) + self.tracks[track.track_id] = PartTracklet(self.params, track, image) - # extract features + # Extract Features ### self.descriminator.extractFeatures(self.tracks) - # update the state + ### Update States ### # print(self.state.state_name()) next_state = self.state.update(self.descriminator, self.tracks) if next_state is not self.state: self.state = next_state - # publish target information + ### Publish Target Information ### target = TargetPerson() target.header = tracks_msg.header target_id = self.state.target() @@ -175,9 +180,29 @@ def joystick_callback(self, joy_msg): print("\nRe-init target\n") self.reselect_target = True - def init_model(self): + def _re_init_model(self): self.state = InitialState() self.descriminator = Descriminator() + self.reselect_target = False + + def extract_features(self, model, tracklets: dict): + idx = list(tracklets.keys())[0] + img_size = tracklets[idx].image_patch.size() + vis_map_size = tracklets[idx].visibility_map.size() + img_patches = torch.empty((len(tracklets.keys()), *img_size[1:]), dtype=torch.float32) + vis_maps = torch.empty((len(tracklets.keys()), *vis_map_size), dtype=torch.float32) + for i, idx in enumerate(sorted(tracklets.keys())): + img_patches[i, :] = tracklets[idx].image_patch + vis_maps[i, :] = tracklets[idx].visibility_map + + # compute deep features with mini-batches + num = len(tracklets.keys()) + total_x = maybe_cuda(img_patches) + total_vis_map = maybe_cuda(vis_maps) + deep_features_, att_maps_ = mini_batch_deep_part_features(model, total_x, num, total_vis_map, True) + for i, idx in enumerate(sorted(tracklets.keys())): + tracklets[idx].deep_feature = deep_features_[i] # (5,512) + tracklets[idx].att_map = att_maps_[i] # (8,4) if __name__ == "__main__": rospy.init_node('mono_following', anonymous=True) diff --git a/mono_following/scripts/mono_following/reid/descriptor/deep/model.py b/mono_following/scripts/mono_following/reid/descriptor/deep/model.py index 97e8754..209a64b 100755 --- a/mono_following/scripts/mono_following/reid/descriptor/deep/model.py +++ b/mono_following/scripts/mono_following/reid/descriptor/deep/model.py @@ -46,6 +46,8 @@ def make_layers(c_in,c_out,repeat_times, is_downsample=False): return nn.Sequential(*blocks) class Net(nn.Module): + """Similar to ResNet18 + """ def __init__(self, num_classes=751 ,reid=False): super(Net,self).__init__() # 3 128 64 diff --git a/mono_following/scripts/mono_following/track_center/__pycache__/parsing_tracklet.cpython-37.pyc b/mono_following/scripts/mono_following/track_center/__pycache__/parsing_tracklet.cpython-37.pyc new file mode 100644 index 0000000..7eb8827 Binary files /dev/null and b/mono_following/scripts/mono_following/track_center/__pycache__/parsing_tracklet.cpython-37.pyc differ diff --git a/mono_following/scripts/mono_following/track_center/__pycache__/part_tracklet.cpython-37.pyc b/mono_following/scripts/mono_following/track_center/__pycache__/part_tracklet.cpython-37.pyc new file mode 100644 index 0000000..a1db2d0 Binary files /dev/null and b/mono_following/scripts/mono_following/track_center/__pycache__/part_tracklet.cpython-37.pyc differ diff --git a/mono_following/scripts/mono_following/track_center/__pycache__/tracklet.cpython-37.pyc b/mono_following/scripts/mono_following/track_center/__pycache__/tracklet.cpython-37.pyc new file mode 100644 index 0000000..584e64f Binary files /dev/null and b/mono_following/scripts/mono_following/track_center/__pycache__/tracklet.cpython-37.pyc differ diff --git a/mono_following/scripts/mono_following/track_center/data_loader.py b/mono_following/scripts/mono_following/track_center/data_loader.py new file mode 100644 index 0000000..9a53e01 --- /dev/null +++ b/mono_following/scripts/mono_following/track_center/data_loader.py @@ -0,0 +1,13 @@ +"""for deep feature extraction +aggregate all single tracklets -> batch +batch -> every single tracklet +""" + +# import torch +# from torch.utils.data import DataLoader, Dataset + +# class DataLoader(Dataset): +# def __init__(self): +# pass + +# def \ No newline at end of file diff --git a/mono_following/scripts/mono_following/track_center/parsing_tracklet.py b/mono_following/scripts/mono_following/track_center/parsing_tracklet.py new file mode 100644 index 0000000..7b94e51 --- /dev/null +++ b/mono_following/scripts/mono_following/track_center/parsing_tracklet.py @@ -0,0 +1,236 @@ +import cv2 +import torch +import torch.nn.functional as F +import numpy as np + +from scipy.special import softmax +from mmtrack.models.identifier.utils.utils import maybe_cuda +""" +Tracklet +1) use multi threads to accelerate single->batch and batch->single procedure (for deep-learning-based feature extractor) +""" +class ParsingTracklet: + def __init__(self, img, img_metas, params, observation, rescale=False): + """ + image (numpy.array): original image with (H,W,3) + bbox (list(int)): [tl_x, tl_y, br_x, br_y] + """ + self.params = params + self.part_nums = params.part_nums + self.img_metas = img_metas + self.vis_map_size = params.vis_map_size + self.vis_map_nums = self.vis_map_size[0] + self.vis_map_res = self.vis_map_size[1:] + self.part_nums = self.vis_map_nums+1 if not params.use_ori else 2*(self.vis_map_nums+1) + ### prior parameters ### + self.img_scale = (256, 128) + self.conf_thr = 0.3 + self.temp = 0.1 + # self.down_width = 64 + # self.down_height = 128 + + ### information ### + # self.track_id = track[0] + self.bbox = observation[:4] # (4) + self.bbox_score = observation[4] # (1) + self.kpts = observation[5] # (23,3) + self.ori = observation[6] # int 1 + self.parsing = observation[7] # (4,64,48) + self.image_patch = self.crop_imgs(img=img, + img_metas=img_metas, + bbox=self.bbox, + rescale=rescale) # Tensor + self.bbox_feature = self.get_bbox_feature(img_metas=img_metas, bbox=self.bbox) + self.binary_ori = self.get_binary_ori(self.ori) + self.visibility_indicator, self.visibility_map = self.get_vis_part(self.parsing, self.binary_ori) + + ### Test soft weighted ### + self.visibility_map = maybe_cuda(torch.Tensor(self.parsing)) + ### Test soft weighted ### + + ### feature ### + self.deep_feature = None # (5,512) + self.joints_feature = None + self.att_map = None # (H,W)-(8,4) + self.target_confidence = None + self.part_target_confidence = [None for _ in range(self.part_nums)] + + def get_vis_part(self, human_parsing:np.array, binary_ori): + """ + human_parsing: list (5,64,48) -- (whole-body, upper-body, lower-body ,knee-feet, background) + human_parsing: list (5,64,48) -- (head, torso, legs, feet, background) + binary_ori: int 1 with 0/1 + """ + human_parsing = np.array(human_parsing) + visible_indicator = torch.zeros(self.part_nums) # front-parts, back-parts + visible_part_indicator = torch.zeros(self.vis_map_nums) # parts + visible_part_map = torch.zeros(self.vis_map_size, dtype=torch.int32) # upper, lower and whole or head, torso, legs, feet and whole + + human_parsing = np.transpose(human_parsing, (2, 1, 0)) # (48, 64, 5) / for resize + human_parsing = cv2.resize(human_parsing, (self.vis_map_res[1], self.vis_map_res[0])) # to (4, 8, 5) + # human_parsing[human_parsing>0.5] = 1 + human_parsing[human_parsing>0.5] = 1 # for new 4 parts + human_parsing[human_parsing!=1] = 0 + human_parsing = np.transpose(human_parsing, (2, 0, 1)) # (5, 4, 8) + + human_parsing = torch.Tensor(human_parsing) + # visible_part_map[0] = visible_part_map[0][human_parsing[0]>0.1] + if self.vis_map_nums == 2: + visible_part_map[0][human_parsing[1]==1] = 1 # upper + visible_part_map[1][human_parsing[2]==1] = 1 # lower + if torch.sum(human_parsing[1]) > 0: + visible_part_indicator[0] = 1 + if torch.sum(human_parsing[2]) > 0: + visible_part_indicator[1] = 1 + # human parsing: head, upper, lower, feet + elif self.vis_map_nums == 4: + for i in range(self.vis_map_nums): + visible_part_map[i][human_parsing[i]==1] = 1 # upper + if torch.sum(human_parsing[i]) > 0: + visible_part_indicator[i] = 1 + + + if self.params.use_ori: + if binary_ori == 0: + visible_indicator[:self.part_nums//2-1] = visible_part_indicator + visible_indicator[self.part_nums//2-1] = 1 + else: + visible_indicator[self.part_nums//2:self.part_nums-1] = visible_part_indicator + visible_indicator[self.part_nums-1] = 1 + else: + visible_indicator[:self.part_nums-1] = visible_part_indicator + visible_indicator[self.part_nums-1] = 1 + + return maybe_cuda(visible_indicator.bool()), maybe_cuda(visible_part_map.bool()) + + def get_binary_ori(self, ori): + """Segment the orientation + Output: + 0 means the person not faces to the camera (Front) + 1 means the person faces to the camera (Back) + """ + # Front + if ori > 90 and ori < 270: + return 0 + # Back + else: + return 1 + + + def get_bbox_feature(self, img_metas, bbox): + """get box information + Input: + box: List of (4) with x1, y1, x2, y2 + Output: + box's scaled height, box's scaled width + """ + h, w, _ = img_metas[0]['img_shape'] + x1, y1, x2, y2 = map(int, bbox) + bbox_width = abs(x1-x2) + bbox_height = abs(y1-y2) + return bbox_height/h, bbox_width/w + + def get_joints_feature(self, img_metas, kpts: np.array): + """get joints information + Input: + kpts: [Nose, LShoulder, RShoulder, LElbow, RElbow, LWrist, RWrist, LHip, RHip, LKnee, Rknee, LAnkle, RAnkle, Neck] + Output: + Scaled coordinates of joints + """ + h, w, _ = img_metas[0]['img_shape'] + scaled_kpts = np.zeros((kpts.shape[0], 2)) + scaled_kpts[:, 0] = kpts[:, 0] / w + scaled_kpts[:, 1] = kpts[:, 1] / h + mask = kpts[:,2] > self.conf_thr + # print(scaled_kpts) + # print(mask) + scaled_kpts = scaled_kpts * np.expand_dims(mask, axis=1) + # print(scaled_kpts) + scaled_kpts = scaled_kpts.flatten() + scaled_kpts = softmax(scaled_kpts/self.temp) + return maybe_cuda(torch.Tensor(scaled_kpts)) + + def down_scale(self, image, bbox): + x1,y1,x2,y2 = bbox + # print(x1,y1,x2,y2) + image = image[y1:y2, x1:x2, :] + image = cv2.resize(image, (self.down_width, self.down_height), interpolation=cv2.INTER_LINEAR) + return image + + def crop_imgs(self, img, img_metas, bbox, rescale=False): + """Crop the images according to some bounding boxes. Typically for re- + identification sub-module. + + Args: + img (Tensor): of shape (N, C, H, W) encoding input images. + Typically these should be mean centered and std scaled. + img_metas (list[dict]): list of image info dict where each dict + has: 'img_shape', 'scale_factor', 'flip', and may also contain + 'filename', 'ori_shape', 'pad_shape', and 'img_norm_cfg'. + bboxes (Tensor): of shape (N, 4) or (N, 5). + rescale (bool, optional): If True, the bounding boxes should be + rescaled to fit the scale of the image. Defaults to False. + + Returns: + Tensor: Image tensor of shape (N, C, H, W). + """ + h, w, _ = img_metas[0]['img_shape'] + img = img[:, :, :h, :w] + + ### TODO: remove in the future### + if not isinstance(bbox, torch.Tensor): + bbox = torch.Tensor(bbox).to(img.device) + + if rescale: + bbox[:4] *= torch.tensor(img_metas[0]['scale_factor']).to( + bbox.device) + bbox[0::2] = torch.clamp(bbox[0::2], min=0, max=w) + bbox[1::2] = torch.clamp(bbox[1::2], min=0, max=h) + + # crop_imgs = [] + x1, y1, x2, y2 = map(int, bbox) + if x2 == x1: + x2 = x1 + 1 + if y2 == y1: + y2 = y1 + 1 + crop_img = img[:, :, y1:y2, x1:x2] + if self.img_scale is not None: + crop_img = F.interpolate( + crop_img, + size=self.img_scale, + mode='bilinear', + align_corners=False) + return crop_img + +class PartTracklet_for_predict: + def __init__(self, img_metas, image_patch, deep_feature, kpts, visibility_indicator, binary_ori): + self.img_metas = img_metas + self.img_patch = image_patch + self.deep_feature = deep_feature + self.kpts = kpts.cpu().numpy() + self.visibility_indicator = visibility_indicator + self.binary_ori = binary_ori + self.joints_feature = self.get_joints_feature(self.img_metas, self.kpts) + self.target_confidence = None + self.part_target_confidence = [None for _ in range(len(visibility_indicator))] + + + def get_joints_feature(self, img_metas, kpts: np.array, conf_thr=0.3, temp=0.1): + """get joints information + Input: + kpts: [Nose, LShoulder, RShoulder, LElbow, RElbow, LWrist, RWrist, LHip, RHip, LKnee, Rknee, LAnkle, RAnkle, Neck] + Output: + Scaled coordinates of joints + """ + h, w, _ = img_metas[0]['img_shape'] + scaled_kpts = np.zeros((kpts.shape[0], 2)) + scaled_kpts[:, 0] = kpts[:, 0] / w + scaled_kpts[:, 1] = kpts[:, 1] / h + mask = kpts[:,2] > conf_thr + # print(scaled_kpts) + # print(mask) + scaled_kpts = scaled_kpts * np.expand_dims(mask, axis=1) + # print(scaled_kpts) + scaled_kpts = scaled_kpts.flatten() + scaled_kpts = softmax(scaled_kpts/temp) + return maybe_cuda(torch.Tensor(scaled_kpts)) \ No newline at end of file diff --git a/mono_following/scripts/mono_following/track_center/part_tracklet.py b/mono_following/scripts/mono_following/track_center/part_tracklet.py new file mode 100644 index 0000000..fd74e22 --- /dev/null +++ b/mono_following/scripts/mono_following/track_center/part_tracklet.py @@ -0,0 +1,144 @@ +import cv2 +import torch +import torch.nn.functional as F +import numpy as np + +from scipy.special import softmax +from ..utils import maybe_cuda +""" +Tracklet +1) use multi threads to accelerate single->batch and batch->single procedure (for deep-learning-based feature extractor) +""" +class PartTracklet: + def __init__(self, params, img, observation): + """ + image (numpy.array): original image with (H,W,3) + bbox (list(int)): [tl_x, tl_y, br_x, br_y] + """ + ### parameters ### + self.params = params + self.vis_map_size = params.vis_map_size + self.vis_map_nums = self.vis_map_size[0] + self.vis_map_res = self.vis_map_size[1:] + self.part_nums = self.vis_map_nums+1 if not params.use_ori else 2*(self.vis_map_nums+1) + + ### prior parameters ### + self.img_scale = (256, 128) + self.conf_thr = 0.3 + self.temp = 0.1 + + ### information ### + self.bbox = observation[:4] # Tensor + self.image_patch = self.crop_imgs(img=img, bbox=self.bbox) # Tensor + + self.bbox_score = observation[4] + self.kpts = observation[5] + # [Nose, LShoulder, RShoulder, LElbow, RElbow, LWrist, RWrist, LHip, RHip, LKnee, Rknee, LAnkle, RAnkle, Neck] + self.kpts = np.concatenate((self.kpts, np.expand_dims((np.array(self.kpts)[1, :] + np.array(self.kpts)[2, :]) / 2, 0)), axis=0) + # if self.params.use_ori: + self.ori = observation[-1] + self.binary_ori = self.get_ori(observation[-1]) + self.visibility_indicator, self.visibility_map = self.get_vis_part(self.bbox, self.kpts, observation[-1]) # (4,4,8) + + ### feature ### + self.deep_feature = None # (5,512) + self.att_map = None # (H,W)-(8,4) + self.target_confidence = None + self.part_target_confidence = [None for _ in range(len(self.visibility_indicator))] + + def get_vis_part(self, bbox, kpts, ori): + """Generate visible parts of person + Input: + bbox: [tl_x, tl_y, br_x, br_y] + kpts: [Nose, LShoulder, RShoulder, LElbow, RElbow, LWrist, RWrist, LHip, RHip, LKnee, Rknee, LAnkle, RAnkle, Neck] + ori(degree): orientation that 0-front, 180-back + Output: + visible parts(4,4,8;Bool Map): with [head, torso, legs, feet], W, H + """ + # part_ids = {"head": [0], "torso": [1,2,13], "legs": [7,8,9,10], "feet":[11,12]} + tl_x, tl_y, br_x, br_y = bbox + visible_indicator = torch.zeros(self.part_nums) # front-parts, back-parts + visible_part_indicator = torch.zeros(self.vis_map_nums) # parts + visible_part_map = torch.zeros(self.vis_map_size, dtype=torch.int32) + parts_ids = [[0], [1,2,13], [7,8,9,10], [11,12]] # 4 parts + kpts = torch.Tensor(kpts) + kpts[:, 0] = torch.clamp(kpts[:, 0], min=tl_x, max=br_x) + kpts[:, 1] = torch.clamp(kpts[:, 1], min=tl_y, max=br_y) + kpts = np.array(kpts) + # print("\nkpts: ", kpts) + for index, part_ids in enumerate(parts_ids): + # if all confidences of kpts are smaller than threshold, continue + if all(kpts[i, 2] < self.conf_thr for i in part_ids): + continue + # head + if index == 0: + min_part_y = tl_y + else: + part_kpts = kpts[part_ids][kpts[part_ids, 2]>self.conf_thr] + min_part_y = np.amin(part_kpts[:, 1]) + # feet + if index == len(parts_ids)-1: + next_max_part_y = br_y + else: + if all(kpts[i, 2] < self.conf_thr for i in parts_ids[index+1]): + next_max_part_y = br_y + else: + next_part_kpts = kpts[parts_ids[index+1]][kpts[parts_ids[index+1], 2]>self.conf_thr] + # print("next: ", next_part_kpts[:, 1]) + next_max_part_y = np.amax(next_part_kpts[:, 1]) + + # resize to 4 x 8 (width x height) same as the feature map size + x1, y1, x2, y2 = 0, int((min_part_y-tl_y)/(br_y-tl_y)*self.vis_map_res[1]), self.vis_map_res[0], int((next_max_part_y-tl_y)/(br_y-tl_y)*self.vis_map_res[1]) + # print(x1, y1, x2, y2) + visible_part_map[index, x1:x2, y1:y2] = 1 + visible_part_indicator[index] = 1 + # Front + if self.params.use_ori: + if ori > 90 and ori < 270: + visible_indicator[:self.part_nums//2-1] = visible_part_indicator + visible_indicator[self.part_nums//2-1] = 1 + # Back + else: + visible_indicator[self.part_nums//2:self.part_nums-1] = visible_part_indicator + visible_indicator[self.part_nums-1] = 1 + else: + visible_indicator[:self.part_nums-1] = visible_part_indicator + visible_indicator[self.part_nums-1] = 1 + return maybe_cuda(visible_indicator.bool()), maybe_cuda(visible_part_map.bool()) + + def get_ori(self, ori): + """Segment the orientation + Output: + 0 means the person not faces to the camera (Front) + 1 means the person faces to the camera (Back) + """ + # Front + if ori > 90 and ori < 270: + return 0 + # Back + else: + return 1 + + def crop_imgs(self, img, bbox): + """Crop the images according to some bounding boxes. Typically for re- + identification sub-module. + Args: + img (Tensor): of shape (N, C, H, W) encoding input images. + Typically these should be mean centered and std scaled. + bboxes (Tensor): of shape (N, 4) or (N, 5). + Returns: + Tensor: Image tensor of shape (N, C, H, W). + """ + x1, y1, x2, y2 = map(int, bbox) + if x2 == x1: + x2 = x1 + 1 + if y2 == y1: + y2 = y1 + 1 + crop_img = img[:, :, y1:y2, x1:x2] + if self.img_scale is not None: + crop_img = F.interpolate( + crop_img, + size=self.img_scale, + mode='bilinear', + align_corners=False) + return crop_img \ No newline at end of file diff --git a/mono_following/scripts/mono_following/track_center/relation.py b/mono_following/scripts/mono_following/track_center/relation.py new file mode 100644 index 0000000..1504d7c --- /dev/null +++ b/mono_following/scripts/mono_following/track_center/relation.py @@ -0,0 +1,5 @@ +""" +This file is used to maintain the relationship between target tracklet and other tracklets +""" + + diff --git a/mono_following/scripts/mono_following/track_center/tracklet.py b/mono_following/scripts/mono_following/track_center/tracklet.py new file mode 100644 index 0000000..a8d3bca --- /dev/null +++ b/mono_following/scripts/mono_following/track_center/tracklet.py @@ -0,0 +1,161 @@ +import cv2 +import torch +import torch.nn.functional as F +import numpy as np + +from scipy.special import softmax +from mmtrack.models.identifier.utils.utils import maybe_cuda +""" +Tracklet +1) use multi threads to accelerate single->batch and batch->single procedure (for deep-learning-based feature extractor) +""" +class Tracklet: + def __init__(self, img, img_metas, observation, rescale=False): + """ + image (numpy.array): original image with (H,W,3) + bbox (list(int)): [tl_x, tl_y, br_x, br_y] + """ + ### prior parameters ### + self.img_scale = (256, 128) + self.conf_thr = 0.3 + self.temp = 0.1 + # self.down_width = 64 + # self.down_height = 128 + + ### information ### + # self.track_id = track[0] + self.bbox = observation[:4] # Tensor + self.image_patch = self.crop_imgs(img=img, + img_metas=img_metas, + bbox=self.bbox, + rescale=rescale) # Tensor + self.bbox_feature = self.get_bbox_feature(img_metas=img_metas, bbox=self.bbox) + + if len(observation) > 4: + self.bbox_score = observation[4] + self.kpts = observation[-1] + # [Nose, LShoulder, RShoulder, LElbow, RElbow, LWrist, RWrist, LHip, RHip, LKnee, Rknee, LAnkle, RAnkle, Neck] + self.kpts = np.concatenate((self.kpts, np.expand_dims((np.array(self.kpts)[1, :] + np.array(self.kpts)[2, :]) / 2, 0)), axis=0) + self.joints_feature = self.get_joints_feature(img_metas, self.kpts) + # print("\nkpts: {}".format(self.kpts)) + + ### feature ### + self.descriptor = None + self.deep_feature = None + self.target_confidence = None + + def get_bbox_feature(self, img_metas, bbox): + """get box information + Input: + box: List of (4) with x1, y1, x2, y2 + Output: + box's scaled height, box's scaled width + """ + h, w, _ = img_metas[0]['img_shape'] + x1, y1, x2, y2 = map(int, bbox) + bbox_width = abs(x1-x2) + bbox_height = abs(y1-y2) + return bbox_height/h, bbox_width/w + + def get_joints_feature(self, img_metas, kpts: np.array): + """get joints information + Input: + kpts: [Nose, LShoulder, RShoulder, LElbow, RElbow, LWrist, RWrist, LHip, RHip, LKnee, Rknee, LAnkle, RAnkle, Neck] + Output: + Scaled coordinates of joints + """ + h, w, _ = img_metas[0]['img_shape'] + scaled_kpts = np.zeros((kpts.shape[0], 2)) + scaled_kpts[:, 0] = kpts[:, 0] / w + scaled_kpts[:, 1] = kpts[:, 1] / h + mask = kpts[:,2] > self.conf_thr + # print(scaled_kpts) + # print(mask) + scaled_kpts = scaled_kpts * np.expand_dims(mask, axis=1) + # print(scaled_kpts) + scaled_kpts = scaled_kpts.flatten() + scaled_kpts = softmax(scaled_kpts/self.temp) + return maybe_cuda(torch.Tensor(scaled_kpts)) + + + def down_scale(self, image, bbox): + x1,y1,x2,y2 = bbox + # print(x1,y1,x2,y2) + image = image[y1:y2, x1:x2, :] + image = cv2.resize(image, (self.down_width, self.down_height), interpolation=cv2.INTER_LINEAR) + return image + + def crop_imgs(self, img, img_metas, bbox, rescale=False): + """Crop the images according to some bounding boxes. Typically for re- + identification sub-module. + + Args: + img (Tensor): of shape (N, C, H, W) encoding input images. + Typically these should be mean centered and std scaled. + img_metas (list[dict]): list of image info dict where each dict + has: 'img_shape', 'scale_factor', 'flip', and may also contain + 'filename', 'ori_shape', 'pad_shape', and 'img_norm_cfg'. + bboxes (Tensor): of shape (N, 4) or (N, 5). + rescale (bool, optional): If True, the bounding boxes should be + rescaled to fit the scale of the image. Defaults to False. + + Returns: + Tensor: Image tensor of shape (N, C, H, W). + """ + h, w, _ = img_metas[0]['img_shape'] + img = img[:, :, :h, :w] + + ### TODO: remove in the future### + if not isinstance(bbox, torch.Tensor): + bbox = torch.Tensor(bbox).to(img.device) + + if rescale: + bbox[:4] *= torch.tensor(img_metas[0]['scale_factor']).to( + bbox.device) + bbox[0::2] = torch.clamp(bbox[0::2], min=0, max=w) + bbox[1::2] = torch.clamp(bbox[1::2], min=0, max=h) + + # crop_imgs = [] + x1, y1, x2, y2 = map(int, bbox) + if x2 == x1: + x2 = x1 + 1 + if y2 == y1: + y2 = y1 + 1 + crop_img = img[:, :, y1:y2, x1:x2] + if self.img_scale is not None: + crop_img = F.interpolate( + crop_img, + size=self.img_scale, + mode='bilinear', + align_corners=False) + return crop_img + +class Tracklet_for_predict: + def __init__(self, img_metas, image_patch, deep_feature, kpts): + self.img_metas = img_metas + self.img_patch = image_patch + self.deep_feature = deep_feature + self.kpts = kpts.cpu().numpy() + self.joints_feature = self.get_joints_feature(self.img_metas, self.kpts) + self.target_confidence = None + + def get_joints_feature(self, img_metas, kpts: np.array, conf_thr=0.3, temp=0.1): + """get joints information + Input: + kpts: [Nose, LShoulder, RShoulder, LElbow, RElbow, LWrist, RWrist, LHip, RHip, LKnee, Rknee, LAnkle, RAnkle, Neck] + Output: + Scaled coordinates of joints + """ + h, w, _ = img_metas[0]['img_shape'] + scaled_kpts = np.zeros((kpts.shape[0], 2)) + scaled_kpts[:, 0] = kpts[:, 0] / w + scaled_kpts[:, 1] = kpts[:, 1] / h + mask = kpts[:,2] > conf_thr + # print(scaled_kpts) + # print(mask) + scaled_kpts = scaled_kpts * np.expand_dims(mask, axis=1) + # print(scaled_kpts) + scaled_kpts = scaled_kpts.flatten() + scaled_kpts = softmax(scaled_kpts/temp) + return maybe_cuda(torch.Tensor(scaled_kpts)) + \ No newline at end of file diff --git a/mono_following/scripts/mono_following/utils.py b/mono_following/scripts/mono_following/utils.py new file mode 100644 index 0000000..4e28209 --- /dev/null +++ b/mono_following/scripts/mono_following/utils.py @@ -0,0 +1,93 @@ +from __future__ import absolute_import +import torch + +GLOBAL = 'globl' +FOREGROUND = 'foreg' +BACKGROUND = 'backg' +CONCAT_PARTS = 'conct' +PARTS = 'parts' +BN_GLOBAL = 'bn_globl' +BN_FOREGROUND = 'bn_foreg' +BN_BACKGROUND = 'bn_backg' +BN_CONCAT_PARTS = 'bn_conct' +BN_PARTS = 'bn_parts' +PIXELS = 'pixls' + +class AverageMeter(object): + """Computes and stores the average and current value""" + + def __init__(self): + self.val = 0 + self.avg = 0 + self.sum = 0 + self.count = 0 + + def reset(self): + self.val = 0 + self.avg = 0 + self.sum = 0 + self.count = 0 + + def update(self, val, n=1): + self.val = val + self.sum += val * n + self.count += n + self.avg = self.sum / self.count + +def maybe_cuda(what, use_cuda=True, **kw): + """ + Moves `what` to CUDA and returns it, if `use_cuda` and it's available. + Args: + what (object): any object to move to eventually gpu + use_cuda (bool): if we want to use gpu or cpu. + Returns + object: the same object but eventually moved to gpu. + """ + + if use_cuda is not False and torch.cuda.is_available(): + what = what.cuda() + return what + +def mini_batch_deep_part_features(model, total_x, num, total_vis_map, is_vis_att_map=False, batch_size=512): + """ + Compute deep features with mini-batches. + Args: + model (object): neural network. + total_x (tensor): data tensor. + num (int): number of data. + vis_map (tensor): vis part map with shape (N,4,W,H) + Returns + deep_features (tensor): deep feature representation of data tensor. + """ + is_train = False + if model.training: + is_train = True + model.eval() + + with torch.no_grad(): + bs = batch_size + num_itr = num // bs + int(num % bs > 0) + sid = 0 + deep_features_list = [] + att_map_list = [] + for i in range(num_itr): + eid = sid + bs if i != num_itr - 1 else num + batch_x = total_x[sid: eid] + vis_map = total_vis_map[sid: eid] + + if is_vis_att_map: + batch_deep_features_, att_map = model.extract_features(batch_x, vis_map, True) + att_map_list.append(att_map) + else: + batch_deep_features_ = model.extract_features(batch_x, vis_map, False) + + deep_features_list.append(batch_deep_features_) + sid = eid + if is_vis_att_map: + att_maps_ = att_map_list[0] if num_itr == 1 else torch.cat(att_map_list, 0) + deep_features_ = deep_features_list[0] if num_itr == 1 else torch.cat(deep_features_list, 0) + if is_train: + model.train() + if is_vis_att_map: + return deep_features_, att_maps_ + return deep_features_ \ No newline at end of file