├── .github └── workflows │ └── config.yml ├── .gitignore ├── .travis.yml ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── launch ├── generator.launch └── rqt_plot_trajectory_pva.launch ├── package.xml ├── plugin.xml ├── resource ├── JointTrajectoryPlot.ui ├── rqt_joint_trajectory_plot_PVA.png └── screenshot.png ├── rqt_perspectives └── 3x_joint_trajectory_plot.perspective ├── script ├── joint_trajectory_generator.py └── rqt_joint_trajectory_plot ├── setup.py ├── src └── rqt_joint_trajectory_plot │ ├── __init__.py │ ├── main_widget.py │ ├── plot_widget.py │ └── plugin.py └── test └── plot.test /.github/workflows/config.yml: -------------------------------------------------------------------------------- 1 | on: [push, pull_request] 2 | 3 | jobs: 4 | build: 5 | strategy: 6 | fail-fast: false 7 | matrix: 8 | include: 9 | - OS: ubuntu:18.04 10 | ROS_DISTRO: melodic 11 | - OS: ubuntu:20.04 12 | ROS_DISTRO: noetic 13 | 14 | runs-on: ubuntu-latest 15 | container: 16 | image: ${{ matrix.OS }} 17 | volumes: 18 | - /tmp/node20:/__e/node20 19 | steps: 20 | - name: Try to replace `node` with an glibc 2.17 21 | shell: bash 22 | run: | 23 | ls -lar /__e/node20 && 24 | apt update -y && apt install -y curl && 25 | curl -Lo /tmp/node.tar.gz https://unofficial-builds.nodejs.org/download/release/v20.17.0/node-v20.17.0-linux-x64-glibc-217.tar.gz && 26 | cd /__e/node20 && 27 | tar -x --strip-components=1 -f /tmp/node.tar.gz && 28 | ls -lar /__e/node20/bin/ 29 | - name: Checkout 30 | uses: actions/checkout@v4 31 | - name: Setup ROS 32 | run: | 33 | apt update -y 34 | apt install -y curl gnupg lsb-release 35 | apt install -y xvfb 36 | echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" | tee /etc/apt/sources.list.d/ros-latest.list 37 | curl -s https://raw.githubusercontent.com/ros/rosdistro/master/ros.asc | apt-key add - 38 | # 39 | apt update -y 40 | apt install -y python-catkin-tools python-rosdep || apt install -y python3-catkin-tools python3-rosdep 41 | apt install -y ros-${{ matrix.ROS_DISTRO }}-catkin 42 | rosdep init 43 | rosdep update --include-eol-distros 44 | rosdep install --from-path $GITHUB_WORKSPACE --ignore-src --rosdistro ${{ matrix.ROS_DISTRO }} -r -y 45 | - name: Run catkin build 46 | shell: bash 47 | run: | 48 | source /opt/ros/${{ matrix.ROS_DISTRO }}/setup.bash 49 | mkdir -p ~/ws/src/ 50 | cp -r $GITHUB_WORKSPACE ~/ws/src/ 51 | cd ~/ws/; catkin build -vis --no-status 52 | - name: Run catkin run_test 53 | shell: bash 54 | run: | 55 | source /opt/ros/${{ matrix.ROS_DISTRO }}/setup.bash 56 | cd ~/ws/; xvfb-run --auto-servernum catkin run_tests 57 | - name: Run catkin test results 58 | shell: bash 59 | run: | 60 | source /opt/ros/${{ matrix.ROS_DISTRO }}/setup.bash 61 | cd ~/ws/; catkin_test_results --all build 62 | 63 | check_python2: 64 | runs-on: ubuntu-latest 65 | container: 66 | image: ubuntu:20.04 67 | volumes: 68 | - /tmp/node20:/__e/node20 69 | name: check_python2 70 | 71 | steps: 72 | - name: Try to replace `node` with an glibc 2.17 73 | shell: bash 74 | run: | 75 | ls -lar /__e/node20 && 76 | apt update -y && apt install -y curl && 77 | curl -Lo /tmp/node.tar.gz https://unofficial-builds.nodejs.org/download/release/v20.17.0/node-v20.17.0-linux-x64-glibc-217.tar.gz && 78 | cd /__e/node20 && 79 | tar -x --strip-components=1 -f /tmp/node.tar.gz && 80 | ls -lar /__e/node20/bin/ 81 | - name: Set up a Git safe directory 82 | run: | 83 | apt update -q && apt install -y -q git 84 | git config --global --add safe.directory "${GITHUB_WORKSPACE}" 85 | - name: Chcekout 86 | uses: actions/checkout@v4 87 | - name: Check Python2 88 | run: | 89 | apt update -q && apt install -y -q python2 90 | python2 -m compileall . 91 | 92 | check_python3: 93 | runs-on: ubuntu-latest 94 | container: 95 | image: ubuntu:20.04 96 | volumes: 97 | - /tmp/node20:/__e/node20 98 | name: check_python3 99 | 100 | steps: 101 | - name: Try to replace `node` with an glibc 2.17 102 | shell: bash 103 | run: | 104 | ls -lar /__e/node20 && 105 | apt update -y && apt install -y curl && 106 | curl -Lo /tmp/node.tar.gz https://unofficial-builds.nodejs.org/download/release/v20.17.0/node-v20.17.0-linux-x64-glibc-217.tar.gz && 107 | cd /__e/node20 && 108 | tar -x --strip-components=1 -f /tmp/node.tar.gz && 109 | ls -lar /__e/node20/bin/ 110 | - name: Set up a Git safe directory 111 | run: | 112 | apt update -q && apt install -y -q git 113 | git config --global --add safe.directory "${GITHUB_WORKSPACE}" 114 | - name: Chcekout 115 | uses: actions/checkout@v4 116 | - name: Check Python3 117 | run: | 118 | apt update -q && apt install -y -q python3 git 2to3 119 | bash -c "ret=0; trap 'ret=1' ERR; python3 -m compileall .; 2to3 -w -f except -f execfile -f has_key -f import -f raw_input -f zip .; git diff --exit-code . > /dev/null; echo Exitting with \$ret; exit \$ret" 120 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | bin/ 3 | lib/ 4 | msg_gen/ 5 | srv_gen/ 6 | msg/*Action.msg 7 | msg/*ActionFeedback.msg 8 | msg/*ActionGoal.msg 9 | msg/*ActionResult.msg 10 | msg/*Feedback.msg 11 | msg/*Goal.msg 12 | msg/*Result.msg 13 | msg/_*.py 14 | 15 | # Generated by dynamic reconfigure 16 | *.cfgc 17 | /cfg/cpp/ 18 | /cfg/*.py 19 | 20 | # Ignore generated docs 21 | *.dox 22 | *.wikidoc 23 | 24 | # eclipse stuff 25 | .project 26 | .cproject 27 | 28 | # qcreator stuff 29 | CMakeLists.txt.user 30 | 31 | srv/_*.py 32 | *.pcd 33 | *.pyc 34 | qtcreator-* 35 | *.user 36 | 37 | /planning/cfg 38 | /planning/docs 39 | /planning/src 40 | 41 | *~ 42 | 43 | # Emacs 44 | .#* 45 | 46 | # Catkin custom files 47 | CATKIN_IGNORE 48 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # https://github.com/ros-infrastructure/ros_buildfarm/blob/master/doc/jobs/devel_jobs.rst 2 | # https://github.com/ros-infrastructure/ros_buildfarm/blob/master/doc/jobs/prerelease_jobs.rst 3 | # while this doesn't require sudo we don't want to run within a Docker container 4 | sudo: true 5 | dist: trusty 6 | language: python 7 | python: 8 | - "3.6" 9 | env: 10 | global: 11 | - JOB_PATH=/tmp/devel_job 12 | - ABORT_ON_TEST_FAILURE=1 13 | matrix: 14 | - ROS_DISTRO_NAME=kinetic OS_NAME=ubuntu OS_CODE_NAME=xenial ARCH=amd64 15 | - ROS_DISTRO_NAME=melodic OS_NAME=ubuntu OS_CODE_NAME=bionic ARCH=amd64 16 | install: 17 | # either install the latest released version of ros_buildfarm 18 | - pip install ros_buildfarm 19 | # or checkout a specific branch 20 | #- git clone -b master https://github.com/ros-infrastructure/ros_buildfarm /tmp/ros_buildfarm 21 | #- pip install /tmp/ros_buildfarm 22 | # checkout catkin for catkin_test_results script 23 | - git clone https://github.com/ros/catkin /tmp/catkin 24 | # run devel job for a ROS repository with the same name as this repo 25 | - export REPOSITORY_NAME=`basename $TRAVIS_BUILD_DIR` 26 | # use the code already checked out by Travis 27 | - mkdir -p $JOB_PATH/ws/src 28 | - cp -R $TRAVIS_BUILD_DIR $JOB_PATH/ws/src/ 29 | # generate the script to run a pre-release job for that target and repo 30 | - generate_prerelease_script.py https://raw.githubusercontent.com/ros-infrastructure/ros_buildfarm_config/production/index.yaml $ROS_DISTRO_NAME default $OS_NAME $OS_CODE_NAME $ARCH --output-dir $JOB_PATH 31 | # run the actual job which involves Docker 32 | - cd $JOB_PATH; sh ./prerelease.sh -y 33 | script: 34 | # get summary of test results 35 | - /tmp/catkin/bin/catkin_test_results $JOB_PATH/ws/test_results --all 36 | notifications: 37 | email: false 38 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package rqt_joint_trajectory_plot 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.0.5 (2020-03-07) 6 | ------------------ 7 | * Add melodic release(`#13 `_) 8 | - Set CI python version to 3.6 9 | - Merge pull request 10 | * added ability to plot planned_path from moveit and made rqt launch file(`#10 `_) 11 | - updated package.xml to include moveit_msgs for plotting display_trajectory messages 12 | * fixed example PVA image size 13 | * added empty display trajectory check and updated readme with instrucitons to launch PVA perspective 14 | * added ability to plot planned_path from moveit and made rqt launch file with custom perspective 15 | * Fix workspace name for CI (`#11 `_) 16 | - Fix workspace name as https://github.com/tork-a/jog_control/pull/37/files 17 | * added empty display trajectory check and updated readme with instrucitons to launch PVA perspective 18 | * added ability to plot planned_path from moveit and made rqt launch file with custom perspective 19 | * Add travis.yml 20 | * Contributors: Ryosuke Tajima, marqrazz 21 | 22 | 0.0.4 (2018-06-14) 23 | ------------------ 24 | * Fix lint problem 25 | * Contributors: Ryosuke Tajima 26 | 27 | 0.0.3 (2018-06-14) 28 | ------------------ 29 | * Check the qt version in plot_widget.py for indigo release(`#9 `_) 30 | * Contributors: Ryosuke Tajima 31 | 32 | 0.0.2 (2018-06-08) 33 | ------------------ 34 | * Add script/rqt_joint_trajectory_plot(`#7 `_) 35 | * Update to visualize FollowJointTrajectoryActionGoal(`#6 `_) 36 | * Contributors: Kei Okada, Ryosuke Tajima 37 | 38 | 0.0.1 (2018-01-20) 39 | ------------------ 40 | * Organize and format package files 41 | - roslint check 42 | - autopep8 for python scripts 43 | * Port to PyQt5 as used in kinetic(`#1 `_) 44 | * Port to PyQt5 45 | no toolbar so far, apparently this is not yet(?!) available in matplotlibs Qt5Agg backend. 46 | * add screenshot to README.md 47 | * add a screenshot file. 48 | * Initial commit 49 | * Contributors: Ryosuke Tajima, Simon Schmeisser 50 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rqt_joint_trajectory_plot) 3 | find_package(catkin REQUIRED) 4 | catkin_package() 5 | catkin_python_setup() 6 | 7 | install( 8 | FILES plugin.xml 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) 11 | 12 | install( 13 | DIRECTORY resource rqt_perspectives 14 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 15 | FILES_MATCHING 16 | PATTERN "*.ui" 17 | PATTERN "*.perspective" 18 | ) 19 | 20 | catkin_install_python( 21 | PROGRAMS script/joint_trajectory_generator.py script/rqt_joint_trajectory_plot 22 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 23 | ) 24 | 25 | install( 26 | DIRECTORY launch 27 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 28 | FILES_MATCHING 29 | PATTERN "*.launch" 30 | ) 31 | 32 | if(CATKIN_ENABLE_TESTING) 33 | find_package(roslint REQUIRED) 34 | set(ROSLINT_PYTHON_OPTS "--max-line-length=180") 35 | roslint_python(src/rqt_joint_trajectory_plot/plot_widget.py) 36 | add_dependencies(tests roslint) 37 | 38 | find_package(roslaunch REQUIRED) 39 | find_package(rostest REQUIRED) 40 | file(GLOB LAUNCH_FILES launch/*.launch) 41 | foreach(LAUNCH_FILE ${LAUNCH_FILES}) 42 | message(STATUS "Testing ${LAUNCH_FILE}") 43 | roslaunch_add_file_check(${LAUNCH_FILE}) 44 | endforeach() 45 | 46 | add_rostest(test/plot.test) 47 | endif() 48 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2016 Ron Tajima 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 | ## rqt_joint_trajectory_plot 2 | 3 | ![Screenshot](resource/screenshot.png) 4 | 5 | ### To start up 6 | 7 | Run rqt as: 8 | ``` 9 | $ rqt 10 | ``` 11 | Choose the 'Plugins'->'Visualization'->'JointTrajectoryPlot' 12 | 13 | If you don't have any node to publish JointTrajectory, start dummy 14 | publisher as: 15 | 16 | ``` 17 | $ roslaunch rqt_joint_trajectory_plot generator.launch 18 | ``` 19 | 20 | To launch a pre-configured version of RQT with 3 plot tools to show Position, Velocity and Acceleration run as (the user is currently required to manually select pos, vel or accl for each plot): 21 | ``` 22 | $ roslaunch rqt_joint_trajectory_plot rqt_plot_trajectory_pva.launch 23 | ``` 24 | ![Screenshot](resource/rqt_joint_trajectory_plot_PVA.png) 25 | ### To select topic 26 | 27 | Chooese the topic name from the pulldown menu. The menu has the topic 28 | of trajectory_msgs/JointTrajectory type. If you don't see target 29 | topic, push the reload button to refresh the topic list. 30 | 31 | When you choose the topic, the plugin subscribe the target topic and ready to draw the curve in the plot area. 32 | 33 | ### To select curve to plot 34 | 35 | Click the checkbox of the target joint name. You can choose the curve 36 | of poistion, velocity, acceleration, and effort. 37 | 38 | Beware the plugin redraws the graph each time when the new message arrived. If you want to stop the data update, push pause button. 39 | -------------------------------------------------------------------------------- /launch/generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/rqt_plot_trajectory_pva.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rqt_joint_trajectory_plot 4 | 5 | 0.0.5 6 | 7 | The rqt_joint_trajectory_plot package 8 | 9 | Tokyo Opensource Robotics Kyokai (TORK) Developer Team 10 | 11 | Ryosuke Tajima 12 | 13 | MIT 14 | 15 | 16 | catkin 17 | python-setuptools 18 | python3-setuptools 19 | g++-static 20 | 21 | rospy 22 | rqt_gui 23 | rqt_gui_py 24 | roslaunch 25 | 26 | control_msgs 27 | rospy 28 | rqt_gui 29 | rqt_gui_py 30 | rqt_py_common 31 | trajectory_msgs 32 | moveit_msgs 33 | python-matplotlib 34 | python3-matplotlib 35 | 36 | rostest 37 | roslint 38 | rosservice 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A plugin visualizing the JointTrajectory message in 2D plot. 5 | 6 | 7 | 8 | 9 | folder 10 | Plugins related to visualization. 11 | 12 | 13 | utilities-system-monitor 14 | 15 | A plugin visualizing the JointTrajectory message in 2D plot. 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /resource/JointTrajectoryPlot.ui: -------------------------------------------------------------------------------- 1 | 2 | 3 | main_widget 4 | 5 | 6 | 7 | 0 8 | 0 9 | 519 10 | 300 11 | 12 | 13 | 14 | JointTrajectoryPlot 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 0 26 | 0 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 0 36 | 0 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 0 51 | 0 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | true 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 0 74 | 0 75 | 76 | 77 | 78 | 0 79 | 80 | 81 | Qt::ScrollBarAsNeeded 82 | 83 | 84 | 85 | JointTrajectory 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | -------------------------------------------------------------------------------- /resource/rqt_joint_trajectory_plot_PVA.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tork-a/rqt_joint_trajectory_plot/508ce2423d658f726f5ca0287e9cecde8dc893bf/resource/rqt_joint_trajectory_plot_PVA.png -------------------------------------------------------------------------------- /resource/screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tork-a/rqt_joint_trajectory_plot/508ce2423d658f726f5ca0287e9cecde8dc893bf/resource/screenshot.png -------------------------------------------------------------------------------- /rqt_perspectives/3x_joint_trajectory_plot.perspective: -------------------------------------------------------------------------------- 1 | { 2 | "keys": {}, 3 | "groups": { 4 | "pluginmanager": { 5 | "keys": { 6 | "running-plugins": { 7 | "type": "repr", 8 | "repr": "{u'rqt_joint_trajectory_plot/JointTrajectoryPlot': [3, 2, 1]}" 9 | } 10 | }, 11 | "groups": { 12 | "plugin__rqt_joint_trajectory_plot__JointTrajectoryPlot__1": { 13 | "keys": {}, 14 | "groups": { 15 | "dock_widget__main_widget": { 16 | "keys": { 17 | "dockable": { 18 | "type": "repr", 19 | "repr": "True" 20 | }, 21 | "parent": { 22 | "type": "repr", 23 | "repr": "None" 24 | }, 25 | "dock_widget_title": { 26 | "type": "repr", 27 | "repr": "u'JointTrajectoryPlot'" 28 | } 29 | }, 30 | "groups": {} 31 | } 32 | } 33 | }, 34 | "plugin__rqt_joint_trajectory_plot__JointTrajectoryPlot__2": { 35 | "keys": {}, 36 | "groups": { 37 | "dock_widget__main_widget": { 38 | "keys": { 39 | "dockable": { 40 | "type": "repr", 41 | "repr": "True" 42 | }, 43 | "parent": { 44 | "type": "repr", 45 | "repr": "None" 46 | }, 47 | "dock_widget_title": { 48 | "type": "repr", 49 | "repr": "u'JointTrajectoryPlot'" 50 | } 51 | }, 52 | "groups": {} 53 | } 54 | } 55 | }, 56 | "plugin__rqt_joint_trajectory_plot__JointTrajectoryPlot__3": { 57 | "keys": {}, 58 | "groups": { 59 | "dock_widget__main_widget": { 60 | "keys": { 61 | "dockable": { 62 | "type": "repr", 63 | "repr": "True" 64 | }, 65 | "parent": { 66 | "type": "repr", 67 | "repr": "None" 68 | }, 69 | "dock_widget_title": { 70 | "type": "repr", 71 | "repr": "u'JointTrajectoryPlot'" 72 | } 73 | }, 74 | "groups": {} 75 | } 76 | } 77 | } 78 | } 79 | }, 80 | "mainwindow": { 81 | "keys": { 82 | "geometry": { 83 | "type": "repr(QByteArray.hex)", 84 | "repr(QByteArray.hex)": "QtCore.QByteArray(b'01d9d0cb0002000000000064000000cb000006cf0000043700000064000000f0000006cf0000043700000002000000000a00')", 85 | "pretty-print": " d 7 d 7 " 86 | }, 87 | "state": { 88 | "type": "repr(QByteArray.hex)", 89 | "repr(QByteArray.hex)": "QtCore.QByteArray(b'000000ff00000000fd00000001000000030000066c0000031cfc0100000001fc000000000000066c0000010c00fffffffc0200000003fb0000007c007200710074005f006a006f0069006e0074005f007400720061006a006500630074006f00720079005f0070006c006f0074005f005f004a006f0069006e0074005400720061006a006500630074006f007200790050006c006f0074005f005f0031005f005f006d00610069006e005f00770069006400670065007401000000160000010e0000008c00fffffffb0000007c007200710074005f006a006f0069006e0074005f007400720061006a006500630074006f00720079005f0070006c006f0074005f005f004a006f0069006e0074005400720061006a006500630074006f007200790050006c006f0074005f005f0032005f005f006d00610069006e005f007700690064006700650074010000012a0000014d0000008c00fffffffb0000007c007200710074005f006a006f0069006e0074005f007400720061006a006500630074006f00720079005f0070006c006f0074005f005f004a006f0069006e0074005400720061006a006500630074006f007200790050006c006f0074005f005f0033005f005f006d00610069006e005f007700690064006700650074010000027d000000b50000008c00ffffff0000066c0000000000000004000000040000000800000008fc00000001000000030000000100000036004d0069006e0069006d0069007a006500640044006f0063006b00570069006400670065007400730054006f006f006c0062006100720000000000ffffffff0000000000000000')", 90 | "pretty-print": " l * M } l " 91 | } 92 | }, 93 | "groups": { 94 | "toolbar_areas": { 95 | "keys": { 96 | "MinimizedDockWidgetsToolbar": { 97 | "type": "repr", 98 | "repr": "8" 99 | } 100 | }, 101 | "groups": {} 102 | } 103 | } 104 | } 105 | } 106 | } -------------------------------------------------------------------------------- /script/joint_trajectory_generator.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import actionlib 5 | import argparse 6 | import numpy as np 7 | import rospy 8 | from trajectory_msgs.msg import JointTrajectory,JointTrajectoryPoint 9 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal 10 | 11 | class JointTrajectoryGenerator: 12 | def __init__(self, time=1.0, step=0.01, freq=1.0, joint_num=1, topic='/joint_trajectory', action='/joint_trajectroy_action'): 13 | self.time = time 14 | self.step = step 15 | self.freq = freq 16 | self.joint_names = ['joint_'+str(n) for n in range(joint_num)] 17 | 18 | self.pub = rospy.Publisher(topic, JointTrajectory, queue_size=10) 19 | self.client = actionlib.SimpleActionClient( 20 | action, FollowJointTrajectoryAction) 21 | 22 | def update(self): 23 | points = [] 24 | tick = 0 25 | self.phase = np.random.uniform(np.pi) 26 | while tick <= self.time: 27 | x = 2*np.pi*self.freq*tick + self.phase 28 | positions = [np.sin(x)]*len(self.joint_names) 29 | velocities = [np.cos(x)]*len(self.joint_names) 30 | accelerations = [-np.sin(x)]*len(self.joint_names) 31 | points.append(JointTrajectoryPoint(positions=positions, 32 | velocities=velocities, 33 | accelerations=accelerations, 34 | time_from_start=rospy.Time.from_sec(tick))) 35 | tick += self.step 36 | msg = JointTrajectory(joint_names=self.joint_names, points=points) 37 | msg.header.stamp = rospy.Time.now() 38 | self.pub.publish(msg) 39 | 40 | # joint_trajectroy_action 41 | goal = FollowJointTrajectoryGoal() 42 | goal.trajectory = msg 43 | self.client.send_goal(goal) 44 | 45 | if __name__ == "__main__": 46 | rospy.init_node('joint_trajectory_generator') 47 | 48 | parser = argparse.ArgumentParser() 49 | parser.add_argument('--time-step', type=float, help='Time step between each trajectory point') 50 | parser.add_argument('--topic', type=str, default="/joint_trajectory", help='Topic name to be published') 51 | parser.add_argument('--action', type=str, default="/joint_trajectory_action", help='Action name to be called') 52 | parser.add_argument('--joint-num', type=int, default=1, help='Number of joints') 53 | sys.argv = rospy.myargv() 54 | args = parser.parse_args() 55 | 56 | generator = JointTrajectoryGenerator(joint_num=args.joint_num, 57 | topic=args.topic, 58 | action=args.action) 59 | 60 | rate = rospy.Rate(1) 61 | while not rospy.is_shutdown(): 62 | generator.update() 63 | rate.sleep() 64 | -------------------------------------------------------------------------------- /script/rqt_joint_trajectory_plot: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | 5 | from rqt_gui.main import Main 6 | 7 | main = Main() 8 | sys.exit(main.main(sys.argv, standalone='rqt_joint_trajectory_plot.plugin.JointTrajectoryPlot')) 9 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | 2 | #!/usr/bin/env python 3 | 4 | from setuptools import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | d = generate_distutils_setup( 8 | packages=['rqt_joint_trajectory_plot'], 9 | package_dir={'': 'src'}, 10 | ) 11 | 12 | setup(**d) -------------------------------------------------------------------------------- /src/rqt_joint_trajectory_plot/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/tork-a/rqt_joint_trajectory_plot/508ce2423d658f726f5ca0287e9cecde8dc893bf/src/rqt_joint_trajectory_plot/__init__.py -------------------------------------------------------------------------------- /src/rqt_joint_trajectory_plot/main_widget.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from python_qt_binding import loadUi 4 | from python_qt_binding.QtCore import Qt, QTimer, qWarning, Signal, Slot 5 | from python_qt_binding.QtGui import QIcon 6 | from python_qt_binding.QtWidgets import QAction, QMenu, QWidget, QTreeWidgetItem 7 | import rospy 8 | import rospkg 9 | from roslib.message import get_message_class 10 | from rqt_py_common import topic_helpers 11 | from trajectory_msgs.msg import JointTrajectory 12 | from control_msgs.msg import FollowJointTrajectoryActionGoal 13 | from moveit_msgs.msg import DisplayTrajectory 14 | import numpy as np 15 | from .plot_widget import PlotWidget 16 | 17 | 18 | class MainWidget(QWidget): 19 | draw_curves = Signal(object, object) 20 | 21 | def __init__(self): 22 | super(MainWidget, self).__init__() 23 | self.setObjectName('MainWidget') 24 | 25 | rospack = rospkg.RosPack() 26 | ui_file = rospack.get_path( 27 | 'rqt_joint_trajectory_plot') + '/resource/JointTrajectoryPlot.ui' 28 | loadUi(ui_file, self) 29 | 30 | self.refresh_button.setIcon(QIcon.fromTheme('view-refresh')) 31 | self.pause_button.setIcon(QIcon.fromTheme('media-playback-pause')) 32 | 33 | self.handler = None 34 | self.joint_names = [] 35 | self.topic_name_class_map = {} 36 | self.timer = QTimer(self) 37 | self.timer.timeout.connect(self.update) 38 | self.plot_widget = PlotWidget(self) 39 | self.plot_layout.addWidget(self.plot_widget) 40 | self.draw_curves.connect(self.plot_widget.draw_curves) 41 | 42 | self.time = None 43 | (self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {}) 44 | 45 | # refresh topics list in the combobox 46 | self.refresh_topics() 47 | self.change_topic() 48 | 49 | self.refresh_button.clicked.connect(self.refresh_topics) 50 | self.topic_combox.currentIndexChanged.connect(self.change_topic) 51 | self.select_tree.itemChanged.connect(self.update_checkbox) 52 | 53 | def refresh_topics(self): 54 | ''' 55 | Refresh topic list in the combobox 56 | ''' 57 | topic_list = rospy.get_published_topics() 58 | if topic_list is None: 59 | return 60 | self.topic_combox.clear() 61 | self.topic_name_class_map = {} 62 | for (name, type) in topic_list: 63 | if type in [ 'trajectory_msgs/JointTrajectory', 64 | 'control_msgs/FollowJointTrajectoryActionGoal', 65 | 'moveit_msgs/DisplayTrajectory']: 66 | self.topic_name_class_map[name] = get_message_class(type) 67 | self.topic_combox.addItem(name) 68 | 69 | def change_topic(self): 70 | topic_name = self.topic_combox.currentText() 71 | if not topic_name: 72 | return 73 | if self.handler: 74 | self.handler.unregister() 75 | self.joint_names = [] 76 | self.handler = rospy.Subscriber( 77 | topic_name, rospy.AnyMsg, self.callback, topic_name) 78 | 79 | def close(self): 80 | if self.handler: 81 | self.handler.unregister() 82 | self.handler = None 83 | 84 | def refresh_tree(self): 85 | self.select_tree.itemChanged.disconnect() 86 | self.select_tree.clear() 87 | for traj_name in ['position', 'velocity', 'acceleration', 'effort']: 88 | item = QTreeWidgetItem(self.select_tree) 89 | item.setText(0, traj_name) 90 | item.setCheckState(0, Qt.Unchecked) 91 | item.setFlags(Qt.ItemIsUserCheckable | Qt.ItemIsEnabled) 92 | for joint_name in self.joint_names: 93 | sub_item = QTreeWidgetItem(item) 94 | sub_item.setText(0, joint_name) 95 | sub_item.setCheckState(0, Qt.Unchecked) 96 | sub_item.setFlags(Qt.ItemIsUserCheckable | Qt.ItemIsEnabled) 97 | self.select_tree.itemChanged.connect(self.update_checkbox) 98 | 99 | def callback(self, anymsg, topic_name): 100 | if self.pause_button.isChecked(): 101 | return 102 | # In case of control_msgs/FollowJointTrajectoryActionGoal 103 | # set trajectory_msgs/JointTrajectory to 'msg' 104 | # Convert AnyMsg to trajectory_msgs/JointTrajectory 105 | msg_class = self.topic_name_class_map[topic_name] 106 | if msg_class == JointTrajectory: 107 | msg = JointTrajectory().deserialize(anymsg._buff) 108 | elif msg_class == FollowJointTrajectoryActionGoal: 109 | msg = FollowJointTrajectoryActionGoal().deserialize(anymsg._buff).goal.trajectory 110 | elif msg_class == DisplayTrajectory: 111 | if DisplayTrajectory().deserialize(anymsg._buff).trajectory.__len__() > 0: 112 | msg = DisplayTrajectory().deserialize(anymsg._buff).trajectory.pop().joint_trajectory 113 | else: 114 | rospy.logwarn("Received planned trajectory has no waypoints in it. Nothing to plot!") 115 | return 116 | else: 117 | rospy.logerr('Wrong message type %s'%msg_class) 118 | return 119 | self.time = np.array([0.0] * len(msg.points)) 120 | (self.dis, self.vel, self.acc, self.eff) = ({}, {}, {}, {}) 121 | for joint_name in msg.joint_names: 122 | self.dis[joint_name] = np.array([0.0] * len(msg.points)) 123 | self.vel[joint_name] = np.array([0.0] * len(msg.points)) 124 | self.acc[joint_name] = np.array([0.0] * len(msg.points)) 125 | self.eff[joint_name] = np.array([0.0] * len(msg.points)) 126 | for i in range(len(msg.points)): 127 | point = msg.points[i] 128 | self.time[i] = point.time_from_start.to_sec() 129 | for j in range(len(msg.joint_names)): 130 | joint_name = msg.joint_names[j] 131 | if point.positions: 132 | self.dis[joint_name][i] = point.positions[j] 133 | if point.velocities: 134 | self.vel[joint_name][i] = point.velocities[j] 135 | if point.accelerations: 136 | self.acc[joint_name][i] = point.accelerations[j] 137 | if point.effort: 138 | self.eff[joint_name][i] = point.effort[j] 139 | if self.joint_names != msg.joint_names: 140 | self.joint_names = msg.joint_names 141 | self.refresh_tree() 142 | self.joint_names = msg.joint_names 143 | self.plot_graph() 144 | 145 | def plot_graph(self): 146 | ''' 147 | Emit changed signal to call plot_widget.draw_curves() 148 | ''' 149 | curve_names = [] 150 | data = {} 151 | data_list = [self.dis, self.vel, self.acc, self.eff] 152 | traj_names = ['position', 'velocity', 'acceleration', 'effort'] 153 | # Create curve name and data from checked items 154 | for i in range(self.select_tree.topLevelItemCount()): 155 | traj_item = self.select_tree.topLevelItem(i) 156 | for n in range(traj_item.childCount()): 157 | item = traj_item.child(n) 158 | if item.checkState(0): 159 | joint_name = item.text(0) 160 | curve_name = joint_name + ' ' + traj_item.text(0) 161 | curve_names.append(curve_name) 162 | data[curve_name] = (self.time, data_list[i][joint_name]) 163 | self.draw_curves.emit(curve_names, data) 164 | 165 | def update_checkbox(self, item, column): 166 | self.recursive_check(item) 167 | self.plot_graph() 168 | 169 | def recursive_check(self, item): 170 | check_state = item.checkState(0) 171 | for i in range(item.childCount()): 172 | item.child(i).setCheckState(0, check_state) 173 | self.recursive_check(item.child(i)) 174 | -------------------------------------------------------------------------------- /src/rqt_joint_trajectory_plot/plot_widget.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from python_qt_binding.QtCore import Slot, Qt, QTimer, qWarning, Signal 3 | from python_qt_binding.QtGui import QColor 4 | from python_qt_binding.QtWidgets import QWidget, QVBoxLayout, QSizePolicy 5 | from python_qt_binding import QT_BINDING_VERSION 6 | from distutils.version import LooseVersion 7 | from matplotlib.figure import Figure 8 | import operator 9 | import numpy as np 10 | import copy 11 | import threading 12 | import rospy 13 | from trajectory_msgs.msg import JointTrajectory 14 | if LooseVersion(QT_BINDING_VERSION) >= LooseVersion('5.0.0'): 15 | from matplotlib.backends.backend_qt5agg import FigureCanvasQTAgg as FigureCanvas 16 | else: 17 | from matplotlib.backends.backend_qt4agg import FigureCanvasQTAgg as FigureCanvas 18 | 19 | 20 | class PlotCanvas(FigureCanvas): 21 | def __init__(self): 22 | super(PlotCanvas, self).__init__(Figure()) 23 | self.axes = self.figure.add_subplot(111) 24 | self.axes.grid(True, color='gray') 25 | self.figure.tight_layout() 26 | self.setSizePolicy(QSizePolicy.Expanding, QSizePolicy.Expanding) 27 | self.updateGeometry() 28 | 29 | 30 | class PlotWidget(QWidget): 31 | def __init__(self, parent=None): 32 | super(PlotWidget, self).__init__(parent) 33 | # create widgets 34 | self.canvas = PlotCanvas() 35 | vbox = QVBoxLayout() 36 | vbox.addWidget(self.canvas) 37 | self.setLayout(vbox) 38 | 39 | def draw_curves(self, curve_names, data): 40 | self.canvas.axes.clear() 41 | self.canvas.axes.grid(True, color='gray') 42 | for name in curve_names: 43 | xdata, ydata = data[name] 44 | self.canvas.axes.plot(xdata, ydata, 'o-', label=name)[0] 45 | self.update_legend() 46 | self.canvas.draw() 47 | 48 | def update_legend(self): 49 | handles, labels = self.canvas.axes.get_legend_handles_labels() 50 | self.canvas.axes.legend(handles, labels, loc='upper left') 51 | -------------------------------------------------------------------------------- /src/rqt_joint_trajectory_plot/plugin.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from rqt_gui_py.plugin import Plugin 3 | from .main_widget import MainWidget 4 | 5 | 6 | class JointTrajectoryPlot(Plugin): 7 | def __init__(self, context): 8 | super(JointTrajectoryPlot, self).__init__(context) 9 | self.context = context 10 | self.setObjectName('JointTrajectoryPlot') 11 | # Create a MainWidget 12 | self.main_widget = MainWidget() 13 | context.add_widget(self.main_widget) 14 | 15 | def save_settings(self, plugin_settings, instance_settings): 16 | pass 17 | 18 | def restore_settings(self, plugin_settings, instance_settings): 19 | pass 20 | 21 | def trigger_configuration(self): 22 | pass 23 | 24 | def shutdown_plugin(self): 25 | self.main_widget.close() 26 | -------------------------------------------------------------------------------- /test/plot.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 9 | 10 | 11 | 12 | 13 | services: 14 | - name: /rqt_plot_traj/set_logger_level 15 | timeout: 2. 16 | 17 | 18 | 19 | --------------------------------------------------------------------------------