├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── files ├── realtime_visualization.rviz ├── rviz.png ├── skeleton_joints.yml └── visualization.rviz ├── launch ├── realtime_visualization.launch └── visualization.launch ├── package.xml └── scripts ├── kinect_to_yml.py ├── realtime_visualization.py └── visualization.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | env/ 12 | build/ 13 | develop-eggs/ 14 | dist/ 15 | downloads/ 16 | eggs/ 17 | .eggs/ 18 | lib/ 19 | lib64/ 20 | parts/ 21 | sdist/ 22 | var/ 23 | wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | 28 | # PyInstaller 29 | # Usually these files are written by a python script from a template 30 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 31 | *.manifest 32 | *.spec 33 | 34 | # Installer logs 35 | pip-log.txt 36 | pip-delete-this-directory.txt 37 | 38 | # Unit test / coverage reports 39 | htmlcov/ 40 | .tox/ 41 | .coverage 42 | .coverage.* 43 | .cache 44 | nosetests.xml 45 | coverage.xml 46 | *.cover 47 | .hypothesis/ 48 | 49 | # Translations 50 | *.mo 51 | *.pot 52 | 53 | # Django stuff: 54 | *.log 55 | local_settings.py 56 | 57 | # Flask stuff: 58 | instance/ 59 | .webassets-cache 60 | 61 | # Scrapy stuff: 62 | .scrapy 63 | 64 | # Sphinx documentation 65 | docs/_build/ 66 | 67 | # PyBuilder 68 | target/ 69 | 70 | # Jupyter Notebook 71 | .ipynb_checkpoints 72 | 73 | # pyenv 74 | .python-version 75 | 76 | # celery beat schedule file 77 | celerybeat-schedule 78 | 79 | # SageMath parsed files 80 | *.sage.py 81 | 82 | # dotenv 83 | .env 84 | 85 | # virtualenv 86 | .venv 87 | venv/ 88 | ENV/ 89 | 90 | # Spyder project settings 91 | .spyderproject 92 | .spyproject 93 | 94 | # Rope project settings 95 | .ropeproject 96 | 97 | # mkdocs documentation 98 | /site 99 | 100 | # mypy 101 | .mypy_cache/ 102 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rviz_skeleton_visualization) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | ) 7 | 8 | catkin_package() 9 | 10 | include_directories( 11 | ${catkin_INCLUDE_DIRS} 12 | ) 13 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2017 Ravi Prakash Joshi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rviz_skeleton_visualization 2 | Demonstration of Skeleton visualization in RVIZ 3 | 4 | ![RViz Screenshot](files/rviz.png) 5 | 6 | ## Package Dependencies 7 | 1. [rviz](http://wiki.ros.org/rviz): It is used for visualization 8 | 1. [kinect_anywhere](https://github.com/ravijo/kinect_anywhere): It is used to obtain Kinect data for real time visualization. No need to install if you don't want to run `realtime_visualization.launch` 9 | 10 | ## Steps to compile 11 | 1. Make sure to download complete repository. Use `git clone` or download directory as per convenience 12 | 1. Invoke `catkin` tool while inside ros workspace i.e., `catkin_make` 13 | 1. Make the scripts executable by running following commands- 14 | 1. [visualization.py](scripts/visualization.py): `chmod +x ~/ros_ws/src/rviz_skeleton_visualization/scripts/visualization.py` 15 | 1. [realtime_visualization.py](scripts/realtime_visualization.py): `chmod +x ~/ros_ws/src/rviz_skeleton_visualization/scripts/realtime_visualization.py` 16 | 17 | In above commands, please note that ROS workspace is located inside` $HOME`. You may have to change the above command accordingly. 18 | 19 | ## Steps to run 20 | There are following two examples to run in this package. 21 | 1. **Recorded Skeletion Visualization**: Simply invoke `visualization.launch` i.e., `roslaunch rviz_skeleton_visualization visualization.launch` 22 | 1. **Realtime Skeletion Visualization**: Please follow below steps- 23 | 1. Make sure package dependency [kinect_anywhere](https://github.com/ravijo/kinect_anywhere) is configured properly and running 24 | 1. Invoke `realtime_visualization.launch` i.e., `roslaunch rviz_skeleton_visualization realtime_visualization.launch ` 25 | 26 | ## Issues 27 | Please check [here](https://github.com/ravijo/rviz_skeleton_visualization/issues) and create issues. 28 | -------------------------------------------------------------------------------- /files/realtime_visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | Splitter Ratio: 0.5 9 | Tree Height: 565 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.588679 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: "" 29 | Visualization Manager: 30 | Class: "" 31 | Displays: 32 | - Class: rviz/MarkerArray 33 | Enabled: true 34 | Marker Topic: /realtime_visualization 35 | Name: Skeleton 36 | Namespaces: 37 | realtime_visualization: true 38 | Queue Size: 100 39 | Value: true 40 | Enabled: true 41 | Global Options: 42 | Background Color: 48; 48; 48 43 | Fixed Frame: base 44 | Frame Rate: 30 45 | Name: root 46 | Tools: 47 | - Class: rviz/Interact 48 | Hide Inactive Objects: true 49 | - Class: rviz/MoveCamera 50 | - Class: rviz/Select 51 | - Class: rviz/FocusCamera 52 | - Class: rviz/Measure 53 | - Class: rviz/SetInitialPose 54 | Topic: /initialpose 55 | - Class: rviz/SetGoal 56 | Topic: /move_base_simple/goal 57 | - Class: rviz/PublishPoint 58 | Single click: true 59 | Topic: /clicked_point 60 | Value: true 61 | Views: 62 | Current: 63 | Class: rviz/Orbit 64 | Distance: 2.28107 65 | Enable Stereo Rendering: 66 | Stereo Eye Separation: 0.06 67 | Stereo Focal Distance: 1 68 | Swap Stereo Eyes: false 69 | Value: false 70 | Focal Point: 71 | X: 0.262362 72 | Y: 0.341997 73 | Z: 0.748207 74 | Name: Current View 75 | Near Clip Distance: 0.01 76 | Pitch: -1.5098 77 | Target Frame: 78 | Value: Orbit (rviz) 79 | Yaw: 1.8254 80 | Saved: ~ 81 | Window Geometry: 82 | Displays: 83 | collapsed: false 84 | Height: 846 85 | Hide Left Dock: false 86 | Hide Right Dock: false 87 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002f600fffffffb0000000800540069006d006501000000000000045000000000000000000000022b000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 88 | Selection: 89 | collapsed: false 90 | Time: 91 | collapsed: false 92 | Tool Properties: 93 | collapsed: false 94 | Views: 95 | collapsed: false 96 | Width: 1200 97 | X: 371 98 | Y: 169 99 | -------------------------------------------------------------------------------- /files/rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ravijo/rviz_skeleton_visualization/1ea2b513a65be32834f9fccf20a5307dc95b0228/files/rviz.png -------------------------------------------------------------------------------- /files/skeleton_joints.yml: -------------------------------------------------------------------------------- 1 | # the skeleton joints are defined by Kinect v2 SDK 2 | # https://docs.microsoft.com/en-us/previous-versions/windows/kinect/dn758662%28v%3dieb.10%29 3 | AnkleLeft: [0.9521, -0.2096, -0.6592] 4 | AnkleRight: [0.9928, 0.2173, -0.7290] 5 | ElbowLeft: [1.1012, -0.2656, 0.1086] 6 | ElbowRight: [1.1495, 0.2204, 0.1081] 7 | FootLeft: [0.8373, -0.1785, -0.6794] 8 | FootRight: [0.8781, 0.2387, -0.7494] 9 | HandLeft: [0.7493, -0.2458, 0.0926] 10 | HandRight: [0.8847, 0.2567, 0.1220] 11 | HandTipLeft: [0.6644, -0.2331, 0.0931] 12 | HandTipRight: [0.7893, 0.2589, 0.1223] 13 | Head: [1.3798, 0.0740, 0.3884] 14 | HipLeft: [1.3124, -0.0870, -0.3340] 15 | HipRight: [1.3555, 0.0595, -0.3415] 16 | KneeLeft: [0.9972, -0.1952, -0.3109] 17 | KneeRight: [1.0667, 0.2201, -0.3475] 18 | Neck: [1.3604, 0.0316, 0.2580] 19 | ShoulderLeft: [1.2747, -0.1331, 0.1839] 20 | ShoulderRight: [1.3556, 0.1615, 0.1530] 21 | SpineBase: [1.3608, -0.0131, -0.3593] 22 | SpineMid: [1.3691, 0.0100, -0.0498] 23 | SpineShoulder: [1.3654, 0.0265, 0.1816] 24 | ThumbLeft: [0.7284, -0.2205, 0.1385] 25 | ThumbRight: [0.8900, 0.2730, 0.0722] 26 | WristLeft: [0.8740, -0.2766, 0.0896] 27 | WristRight: [0.9461, 0.2612, 0.1213] 28 | -------------------------------------------------------------------------------- /files/visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1/Offset1 10 | Splitter Ratio: 0.5 11 | Tree Height: 883 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.588679 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Visualization Manager: 32 | Class: "" 33 | Displays: 34 | - Alpha: 0.5 35 | Cell Size: 1 36 | Class: rviz/Grid 37 | Color: 160; 160; 164 38 | Enabled: true 39 | Line Style: 40 | Line Width: 0.03 41 | Value: Lines 42 | Name: Grid 43 | Normal Cell Count: 0 44 | Offset: 45 | X: 0 46 | Y: 0 47 | Z: -1 48 | Plane: XY 49 | Plane Cell Count: 10 50 | Reference Frame: 51 | Value: true 52 | - Class: rviz/MarkerArray 53 | Enabled: true 54 | Marker Topic: /visualization 55 | Name: MarkerArray 56 | Namespaces: 57 | visualization: true 58 | Queue Size: 100 59 | Value: true 60 | Enabled: true 61 | Global Options: 62 | Background Color: 48; 48; 48 63 | Fixed Frame: base 64 | Frame Rate: 30 65 | Name: root 66 | Tools: 67 | - Class: rviz/Interact 68 | Hide Inactive Objects: true 69 | - Class: rviz/MoveCamera 70 | - Class: rviz/Select 71 | - Class: rviz/FocusCamera 72 | - Class: rviz/Measure 73 | - Class: rviz/SetInitialPose 74 | Topic: /initialpose 75 | - Class: rviz/SetGoal 76 | Topic: /move_base_simple/goal 77 | - Class: rviz/PublishPoint 78 | Single click: true 79 | Topic: /clicked_point 80 | Value: true 81 | Views: 82 | Current: 83 | Class: rviz/Orbit 84 | Distance: 2.50957 85 | Enable Stereo Rendering: 86 | Stereo Eye Separation: 0.06 87 | Stereo Focal Distance: 1 88 | Swap Stereo Eyes: false 89 | Value: false 90 | Focal Point: 91 | X: 0.910973 92 | Y: 0.158545 93 | Z: -0.292264 94 | Name: Current View 95 | Near Clip Distance: 0.01 96 | Pitch: 0.565203 97 | Target Frame: 98 | Value: Orbit (rviz) 99 | Yaw: 1.68537 100 | Saved: ~ 101 | Window Geometry: 102 | Displays: 103 | collapsed: false 104 | Height: 1056 105 | Hide Left Dock: false 106 | Hide Right Dock: false 107 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000402fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000000000000402000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000402fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000000000000402000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000046e0000004cfc0100000002fb0000000800540069006d006500000000000000046e000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000004ba0000040200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 108 | Selection: 109 | collapsed: false 110 | Time: 111 | collapsed: false 112 | Tool Properties: 113 | collapsed: false 114 | Views: 115 | collapsed: false 116 | Width: 1855 117 | X: 1985 118 | Y: 24 119 | -------------------------------------------------------------------------------- /launch/realtime_visualization.launch: -------------------------------------------------------------------------------- 1 | 2 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /launch/visualization.launch: -------------------------------------------------------------------------------- 1 | 2 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rviz_skeleton_visualization 4 | 0.0.0 5 | The rviz_skeleton_visualization package 6 | 7 | ravi 8 | MIT 9 | catkin 10 | rospy 11 | rospy 12 | rospy 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /scripts/kinect_to_yml.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # kinect_to_yml.py: store kinect tracking data into yml file 5 | # Author: Ravi Joshi 6 | # Date: 2018/11/30 7 | 8 | # import modules 9 | import yaml 10 | import rospy 11 | import rospkg 12 | import argparse 13 | from os.path import join 14 | from kinect_anywhere.msg import BodyFrame 15 | 16 | parser = argparse.ArgumentParser() 17 | parser.add_argument('--count', type=int, default=0, 18 | help='counter to create dynamic file name [default: 0]') 19 | parser.add_argument('--topic', default='/irex/kinect2/body_frame/bodies', 20 | help='name of the rostopic [default: /irex/kinect2/body_frame/bodies]') 21 | args = parser.parse_args() 22 | 23 | topic_name = args.topic 24 | count = args.count 25 | 26 | # joints info is taken from kinect website as mentioned below 27 | # https://docs.microsoft.com/en-us/previous-versions/windows/kinect/dn758662%28v%3dieb.10%29 28 | joints_name_id = {'AnkleLeft': 14, 29 | 'AnkleRight': 18, 30 | 'ElbowLeft': 5, 31 | 'ElbowRight': 9, 32 | 'FootLeft': 15, 33 | 'FootRight': 19, 34 | 'HandLeft': 7, 35 | 'HandRight': 11, 36 | 'HandTipLeft': 21, 37 | 'HandTipRight': 23, 38 | 'Head': 3, 39 | 'HipLeft': 12, 40 | 'HipRight': 16, 41 | 'KneeLeft': 13, 42 | 'KneeRight': 17, 43 | 'Neck': 2, 44 | 'ShoulderLeft': 4, 45 | 'ShoulderRight': 8, 46 | 'SpineBase': 0, 47 | 'SpineMid': 1, 48 | 'SpineShoulder': 20, 49 | 'ThumbLeft': 22, 50 | 'ThumbRight': 24, 51 | 'WristLeft': 6, 52 | 'WristRight': 10} 53 | 54 | # create a dictionry where key is joint id and value is joint name 55 | joints_id_name = {joints_name_id[name]: name for name in joints_name_id} 56 | 57 | 58 | def callback(data): 59 | # considering only one person 60 | body = data.bodies[0] 61 | # create a dictionry where key is joint name and value is joint position 62 | joint_info = {joints_id_name[joint.jointType]: 63 | [joint.position.x, joint.position.y, joint.position.z] 64 | for joint in body.jointPositions} 65 | 66 | # write every 10th frame to a yaml file 67 | global count 68 | if count % 10 == 0: 69 | file_name = join(project_location, 'files', 'skeleton_%d.yml' % count) 70 | with open(file_name, 'w') as yml_file: 71 | yaml.dump(joint_info, yml_file) 72 | 73 | # increment the counter 74 | count += 1 75 | 76 | 77 | if __name__ == '__main__': 78 | rospy.init_node('kinect_to_yml', anonymous=True) 79 | project_location = rospkg.RosPack().get_path('rviz_skeleton_visualization') 80 | rospy.Subscriber(topic_name, BodyFrame, callback) 81 | rospy.spin() 82 | -------------------------------------------------------------------------------- /scripts/realtime_visualization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # realtime_visualization.py: rviz visualization 5 | # Author: Ravi Joshi 6 | # Date: 2018/02/07 7 | 8 | # import modules 9 | import rospy 10 | from std_msgs.msg import ColorRGBA 11 | from geometry_msgs.msg import Vector3 12 | from kinect_anywhere.msg import BodyFrame 13 | from visualization_msgs.msg import Marker, MarkerArray 14 | 15 | 16 | class RealtimeVisualization(): 17 | def __init__(self, ns, body_topic, skeleton_frame, body_id_text_size, skeleton_line_width): 18 | self.ns = ns 19 | self.skeleton_frame = skeleton_frame 20 | 21 | self.body_id_text_size = float(body_id_text_size) 22 | self.skeleton_line_width = float(skeleton_line_width) 23 | 24 | self.skeleton_pub = rospy.Publisher(self.ns, MarkerArray, queue_size=2) 25 | 26 | # define the colors 27 | self.colors = [ColorRGBA(0.98, 0.30, 0.30, 1.00), 28 | ColorRGBA(0.12, 0.63, 0.42, 1.00), 29 | ColorRGBA(0.26, 0.09, 0.91, 1.00), 30 | ColorRGBA(0.77, 0.44, 0.14, 1.00), 31 | ColorRGBA(0.92, 0.73, 0.14, 1.00), 32 | ColorRGBA(0.00, 0.61, 0.88, 1.00), 33 | ColorRGBA(1.00, 0.65, 0.60, 1.00), 34 | ColorRGBA(0.59, 0.00, 0.56, 1.00)] 35 | 36 | self.body_id_color = ColorRGBA(0.62, 0.93, 0.14, 1.00) 37 | 38 | ''' 39 | The skeleton is considered as a combination of line strips. 40 | Hence, the skeleton is decomposed into three LINE_STRIP as following: 41 | 1) upper_body : from head to spine base 42 | 2) hands : from left-hand tip to right-hand tip 43 | 3) legs : from left foot to right foot 44 | 45 | See the link below to get the id of each joint as defined in Kinect v2 46 | source: https://msdn.microsoft.com/en-us/library/microsoft.kinect.jointtype.aspx 47 | 48 | upper_body: 49 | head 3, neck 2, spine shoulder 20, 50 | spine mid 1, spine base 0 51 | 52 | hands: 53 | hand tip left 21, hand left 7, wrist left 6, elbow left 5 54 | shoulder left 4, shoulder right 8, elbow right 9 55 | wrist right 10, hand right 11, hand tip right 23 56 | 57 | legs: 58 | foot left 15, ankle left 14, knee left 13 59 | hip left 12, spine base 0, hip right 16 60 | knee right 17, ankle right 18, foot right 19 61 | ''' 62 | self.upper_body_ids = [3, 2, 20, 1, 0] 63 | self.hands_ids = [21, 7, 6, 5, 4, 20, 8, 9, 10, 11, 23] 64 | self.legs_ids = [15, 14, 13, 12, 0, 16, 17, 18, 19] 65 | 66 | # define other joint ids 67 | self.head_id = 3 68 | 69 | # define a subscriber to retrive tracked bodies 70 | rospy.Subscriber(body_topic, BodyFrame, self.receive_skeleton_callback) 71 | rospy.spin() 72 | 73 | def create_marker(self, index, color, marker_type, size, time, frame_id): 74 | marker = Marker() 75 | marker.id = index 76 | marker.ns = self.ns 77 | marker.color = color 78 | marker.action = Marker.ADD 79 | marker.type = marker_type 80 | marker.scale = Vector3(size, size, size) 81 | marker.header.stamp = time 82 | marker.header.frame_id = frame_id 83 | marker.lifetime = rospy.Duration(1) # 1 second 84 | return marker 85 | 86 | def receive_skeleton_callback(self, data): 87 | marker_index = 0 88 | person_index = 1 89 | marker_array = MarkerArray() 90 | 91 | if not data.bodies: 92 | rospy.logdebug('No body tracked') 93 | else: 94 | for body in data.bodies: 95 | now = rospy.Time.now() 96 | 97 | marker_index += 1 98 | upper_body = self.create_marker( 99 | marker_index, 100 | self.colors[person_index], 101 | Marker.LINE_STRIP, 102 | self.skeleton_line_width, 103 | now, 104 | self.skeleton_frame) 105 | 106 | marker_index += 1 107 | hands = self.create_marker( 108 | marker_index, 109 | self.colors[person_index], 110 | Marker.LINE_STRIP, 111 | self.skeleton_line_width, 112 | now, 113 | self.skeleton_frame) 114 | 115 | marker_index += 1 116 | legs = self.create_marker( 117 | marker_index, 118 | self.colors[person_index], 119 | Marker.LINE_STRIP, 120 | self.skeleton_line_width, 121 | now, 122 | self.skeleton_frame) 123 | 124 | upper_body.points = [ 125 | body.jointPositions[id].position for id in self.upper_body_ids] 126 | hands.points = [ 127 | body.jointPositions[id].position for id in self.hands_ids] 128 | legs.points = [ 129 | body.jointPositions[id].position for id in self.legs_ids] 130 | 131 | marker_index += 1 132 | head_id_marker = self.create_marker( 133 | marker_index, 134 | self.body_id_color, 135 | Marker.TEXT_VIEW_FACING, 136 | self.body_id_text_size, 137 | now, 138 | self.skeleton_frame) 139 | head_id_marker.text = str(person_index) 140 | head_id_marker.pose.position = body.jointPositions[self.head_id].position 141 | 142 | marker_array.markers.append(head_id_marker) 143 | marker_array.markers.append(upper_body) 144 | marker_array.markers.append(hands) 145 | marker_array.markers.append(legs) 146 | 147 | person_index += 1 148 | self.skeleton_pub.publish(marker_array) 149 | 150 | def __del__(self): 151 | self.skeleton_pub.unregister() 152 | 153 | 154 | if __name__ == '__main__': 155 | # define some constants 156 | ns = 'realtime_visualization' 157 | 158 | # initialize ros node 159 | rospy.init_node('rviz_visualization', anonymous=True) 160 | body_topic = rospy.get_param('~body_topic') 161 | skeleton_frame = rospy.get_param('~skeleton_frame') 162 | body_id_text_size = rospy.get_param('~body_id_text_size') 163 | skeleton_line_width = rospy.get_param('~skeleton_line_width') 164 | 165 | RealtimeVisualization(ns, body_topic, skeleton_frame, 166 | body_id_text_size, skeleton_line_width) 167 | -------------------------------------------------------------------------------- /scripts/visualization.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | # visualization.py: code for skeleton visualizatio. 5 | # skeleton info is taken from yaml file. 6 | # Author: Ravi Joshi 7 | # Date: 2017/12/21 8 | # Modified on: 2018/11/27 9 | 10 | # import modules 11 | import yaml 12 | import rospy 13 | from std_msgs.msg import ColorRGBA 14 | from geometry_msgs.msg import Vector3, Point 15 | from visualization_msgs.msg import Marker, MarkerArray 16 | 17 | ''' 18 | The skeleton is considered as a combination of line strips. 19 | Hence, the skeleton is decomposed into three LINE_STRIP as following: 20 | 1) upper_body : [In Red Color] from head to spine base 21 | 2) hands : [In Green Color] from left-hand tip to right-hand tip 22 | 3) legs : [In Blue Color] from left foot to right foot 23 | 24 | See the link below to get the id of each joint as defined in Kinect v2 25 | source: https://msdn.microsoft.com/en-us/library/microsoft.kinect.jointtype.aspx 26 | new https://docs.microsoft.com/en-us/previous-versions/windows/kinect/dn758662%28v%3dieb.10%29 27 | 28 | 29 | upper_body: 30 | head 3, neck 2, spine shoulder 20, 31 | spine mid 1, spine base 0 32 | 33 | hands: 34 | hand tip left 21, hand left 7, wrist left 6, elbow left 5 35 | shoulder left 4, shoulder right 8, elbow right 9 36 | wrist right 10, hand right 11, hand tip right 23 37 | 38 | legs: 39 | foot left 15, ankle left 14, knee left 13 40 | hip left 12, spine base 0, hip right 16 41 | knee right 17, ankle right 18, foot right 19 42 | ''' 43 | 44 | 45 | def read_skeleton_file(file_name): 46 | # joints info is taken from kinect website as mentioned below 47 | # https://docs.microsoft.com/en-us/previous-versions/windows/kinect/dn758662%28v%3dieb.10%29 48 | joints_info = {'AnkleLeft': 14, 49 | 'AnkleRight': 18, 50 | 'ElbowLeft': 5, 51 | 'ElbowRight': 9, 52 | 'FootLeft': 15, 53 | 'FootRight': 19, 54 | 'HandLeft': 7, 55 | 'HandRight': 11, 56 | 'HandTipLeft': 21, 57 | 'HandTipRight': 23, 58 | 'Head': 3, 59 | 'HipLeft': 12, 60 | 'HipRight': 16, 61 | 'KneeLeft': 13, 62 | 'KneeRight': 17, 63 | 'Neck': 2, 64 | 'ShoulderLeft': 4, 65 | 'ShoulderRight': 8, 66 | 'SpineBase': 0, 67 | 'SpineMid': 1, 68 | 'SpineShoulder': 20, 69 | 'ThumbLeft': 22, 70 | 'ThumbRight': 24, 71 | 'WristLeft': 6, 72 | 'WristRight': 10} 73 | 74 | # read yaml file as a dictionary 75 | with open(file_name) as yaml_file: 76 | joints = yaml.load(yaml_file) 77 | 78 | # sort joints based on joint id 79 | joint_list = [None] * len(joints_info) 80 | for joint_name in joints: 81 | joint_list[joints_info[joint_name]] = joints[joint_name] 82 | 83 | return joint_list 84 | 85 | 86 | class Visualization(): 87 | def __init__(self, ns, skeleton_frame, body_id_text_size, skeleton_line_width, file_name): 88 | self.ns = ns 89 | skeleton_pub = rospy.Publisher(self.ns, MarkerArray, queue_size=2) 90 | 91 | # define the colors 92 | colors = [ColorRGBA(0.98, 0.30, 0.30, 1.00), 93 | ColorRGBA(0.12, 0.63, 0.42, 1.00), 94 | ColorRGBA(0.26, 0.09, 0.91, 1.00), 95 | ColorRGBA(0.77, 0.44, 0.14, 1.00), 96 | ColorRGBA(0.92, 0.73, 0.14, 1.00), 97 | ColorRGBA(0.00, 0.61, 0.88, 1.00), 98 | ColorRGBA(1.00, 0.65, 0.60, 1.00), 99 | ColorRGBA(0.59, 0.00, 0.56, 1.00)] 100 | 101 | body_id_color = ColorRGBA(0.62, 0.93, 0.14, 1.00) 102 | 103 | upper_body_ids = [3, 2, 20, 1, 0] 104 | hands_ids = [21, 7, 6, 5, 4, 20, 8, 9, 10, 11, 23] 105 | legs_ids = [15, 14, 13, 12, 0, 16, 17, 18, 19] 106 | 107 | # define other joint ids 108 | head_id = 3 109 | 110 | # for the demonstration, I am just using one person recorded data 111 | # however, it can be supplied here in real-time accquired using Kinect etc 112 | # see 'realtime_visualization.py' for more info 113 | skeleton_joints = read_skeleton_file(file_name) 114 | bodies = [skeleton_joints] 115 | 116 | rate = rospy.Rate(10) # 10hz 117 | while not rospy.is_shutdown(): 118 | marker_index = 0 119 | person_index = 1 120 | marker_array = MarkerArray() 121 | 122 | for body in bodies: 123 | now = rospy.Time.now() 124 | 125 | marker_index += 1 126 | upper_body = self.create_marker( 127 | marker_index, 128 | colors[person_index], 129 | Marker.LINE_STRIP, 130 | skeleton_line_width, 131 | now, 132 | skeleton_frame) 133 | 134 | marker_index += 1 135 | hands = self.create_marker( 136 | marker_index, 137 | colors[person_index], 138 | Marker.LINE_STRIP, 139 | skeleton_line_width, 140 | now, 141 | skeleton_frame) 142 | 143 | marker_index += 1 144 | legs = self.create_marker( 145 | marker_index, 146 | colors[person_index], 147 | Marker.LINE_STRIP, 148 | skeleton_line_width, 149 | now, 150 | skeleton_frame) 151 | 152 | all_joints = body 153 | upper_body.points = [Point( 154 | all_joints[id][0], 155 | all_joints[id][1], 156 | all_joints[id][2]) for id in upper_body_ids] 157 | hands.points = [Point( 158 | all_joints[id][0], 159 | all_joints[id][1], 160 | all_joints[id][2]) for id in hands_ids] 161 | legs.points = [Point( 162 | all_joints[id][0], 163 | all_joints[id][1], 164 | all_joints[id][2]) for id in legs_ids] 165 | 166 | marker_index += 1 167 | head_id_marker = self.create_marker( 168 | marker_index, 169 | body_id_color, 170 | Marker.TEXT_VIEW_FACING, 171 | body_id_text_size, 172 | now, 173 | skeleton_frame) 174 | head_id_marker.text = str(person_index) 175 | head_id_marker.pose.position = Point( 176 | all_joints[head_id][0], 177 | all_joints[head_id][1], 178 | all_joints[head_id][2]) 179 | 180 | marker_array.markers.append(head_id_marker) 181 | marker_array.markers.append(upper_body) 182 | marker_array.markers.append(hands) 183 | marker_array.markers.append(legs) 184 | 185 | person_index += 1 186 | 187 | skeleton_pub.publish(marker_array) 188 | rate.sleep() 189 | 190 | def create_marker(self, index, color, marker_type, size, time, frame_id): 191 | marker = Marker() 192 | marker.id = index 193 | marker.ns = self.ns 194 | marker.color = color 195 | marker.action = Marker.ADD 196 | marker.type = marker_type 197 | marker.scale = Vector3(size, size, size) 198 | marker.header.stamp = time 199 | marker.header.frame_id = frame_id 200 | marker.lifetime = rospy.Duration(1) # 1 second 201 | return marker 202 | 203 | 204 | if __name__ == '__main__': 205 | # define some constants 206 | ns = 'visualization' 207 | body_id_text_size = 0.2 208 | skeleton_line_width = 0.02 209 | 210 | # initialize ros node 211 | rospy.init_node(ns, anonymous=True) 212 | file_name = rospy.get_param('~file') 213 | skeleton_frame = rospy.get_param('~skeleton_frame') 214 | 215 | Visualization(ns, skeleton_frame, body_id_text_size, 216 | skeleton_line_width, file_name) 217 | --------------------------------------------------------------------------------