├── .github └── workflows │ └── manual.yml ├── CMakeLists.txt ├── CODEOWNERS ├── Images └── Output.png ├── LICENSE ├── README.md ├── launch └── create_trajectory.launch ├── package.xml └── scripts ├── path_ekf_plotter.py └── path_odom_plotter.py /.github/workflows/manual.yml: -------------------------------------------------------------------------------- 1 | # Workflow to ensure whenever a Github PR is submitted, 2 | # a JIRA ticket gets created automatically. 3 | name: Manual Workflow 4 | 5 | # Controls when the action will run. 6 | on: 7 | # Triggers the workflow on pull request events but only for the master branch 8 | pull_request_target: 9 | types: [opened, reopened] 10 | 11 | # Allows you to run this workflow manually from the Actions tab 12 | workflow_dispatch: 13 | 14 | jobs: 15 | test-transition-issue: 16 | name: Convert Github Issue to Jira Issue 17 | runs-on: ubuntu-latest 18 | steps: 19 | - name: Checkout 20 | uses: actions/checkout@master 21 | 22 | - name: Login 23 | uses: atlassian/gajira-login@master 24 | env: 25 | JIRA_BASE_URL: ${{ secrets.JIRA_BASE_URL }} 26 | JIRA_USER_EMAIL: ${{ secrets.JIRA_USER_EMAIL }} 27 | JIRA_API_TOKEN: ${{ secrets.JIRA_API_TOKEN }} 28 | 29 | - name: Create NEW JIRA ticket 30 | id: create 31 | uses: atlassian/gajira-create@master 32 | with: 33 | project: CONUPDATE 34 | issuetype: Task 35 | summary: | 36 | Github PR [Assign the ND component] | Repo: ${{ github.repository }} | PR# ${{github.event.number}} 37 | description: | 38 | Repo link: https://github.com/${{ github.repository }} 39 | PR no. ${{ github.event.pull_request.number }} 40 | PR title: ${{ github.event.pull_request.title }} 41 | PR description: ${{ github.event.pull_request.description }} 42 | In addition, please resolve other issues, if any. 43 | fields: '{"components": [{"name":"Github PR"}], "customfield_16449":"https://classroom.udacity.com/", "customfield_16450":"Resolve the PR", "labels": ["github"], "priority":{"id": "4"}}' 44 | 45 | - name: Log created issue 46 | run: echo "Issue ${{ steps.create.outputs.issue }} was created" 47 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(odom_to_trajectory) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | rospy 13 | std_msgs 14 | ) 15 | 16 | ## System dependencies are found with CMake's conventions 17 | # find_package(Boost REQUIRED COMPONENTS system) 18 | 19 | 20 | ## Uncomment this if the package has a setup.py. This macro ensures 21 | ## modules and global scripts declared therein get installed 22 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 23 | # catkin_python_setup() 24 | 25 | ################################################ 26 | ## Declare ROS messages, services and actions ## 27 | ################################################ 28 | 29 | ## To declare and build messages, services or actions from within this 30 | ## package, follow these steps: 31 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 32 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 33 | ## * In the file package.xml: 34 | ## * add a build_depend tag for "message_generation" 35 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 36 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 37 | ## but can be declared for certainty nonetheless: 38 | ## * add a run_depend tag for "message_runtime" 39 | ## * In this file (CMakeLists.txt): 40 | ## * add "message_generation" and every package in MSG_DEP_SET to 41 | ## find_package(catkin REQUIRED COMPONENTS ...) 42 | ## * add "message_runtime" and every package in MSG_DEP_SET to 43 | ## catkin_package(CATKIN_DEPENDS ...) 44 | ## * uncomment the add_*_files sections below as needed 45 | ## and list every .msg/.srv/.action file to be processed 46 | ## * uncomment the generate_messages entry below 47 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 48 | 49 | ## Generate messages in the 'msg' folder 50 | # add_message_files( 51 | # FILES 52 | # Message1.msg 53 | # Message2.msg 54 | # ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | # generate_messages( 72 | # DEPENDENCIES 73 | # std_msgs 74 | # ) 75 | 76 | ################################################ 77 | ## Declare ROS dynamic reconfigure parameters ## 78 | ################################################ 79 | 80 | ## To declare and build dynamic reconfigure parameters within this 81 | ## package, follow these steps: 82 | ## * In the file package.xml: 83 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 84 | ## * In this file (CMakeLists.txt): 85 | ## * add "dynamic_reconfigure" to 86 | ## find_package(catkin REQUIRED COMPONENTS ...) 87 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 88 | ## and list every .cfg file to be processed 89 | 90 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 91 | # generate_dynamic_reconfigure_options( 92 | # cfg/DynReconf1.cfg 93 | # cfg/DynReconf2.cfg 94 | # ) 95 | 96 | ################################### 97 | ## catkin specific configuration ## 98 | ################################### 99 | ## The catkin_package macro generates cmake config files for your package 100 | ## Declare things to be passed to dependent projects 101 | ## INCLUDE_DIRS: uncomment this if your package contains header files 102 | ## LIBRARIES: libraries you create in this project that dependent projects also need 103 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 104 | ## DEPENDS: system dependencies of this project that dependent projects also need 105 | catkin_package( 106 | # INCLUDE_DIRS include 107 | # LIBRARIES odom_to_trajectory 108 | # CATKIN_DEPENDS roscpp rospy std_msgs 109 | # DEPENDS system_lib 110 | ) 111 | 112 | ########### 113 | ## Build ## 114 | ########### 115 | 116 | ## Specify additional locations of header files 117 | ## Your package locations should be listed before other locations 118 | include_directories( 119 | # include 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/odom_to_trajectory.cpp 126 | # ) 127 | 128 | ## Add cmake target dependencies of the library 129 | ## as an example, code may need to be generated before libraries 130 | ## either from message generation or dynamic reconfigure 131 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 132 | 133 | ## Declare a C++ executable 134 | ## With catkin_make all packages are built within a single CMake context 135 | ## The recommended prefix ensures that target names across packages don't collide 136 | # add_executable(${PROJECT_NAME}_node src/odom_to_trajectory_node.cpp) 137 | 138 | ## Rename C++ executable without prefix 139 | ## The above recommended prefix causes long target names, the following renames the 140 | ## target back to the shorter version for ease of user use 141 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 142 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 143 | 144 | ## Add cmake target dependencies of the executable 145 | ## same as for the library above 146 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 147 | 148 | ## Specify libraries to link a library or executable target against 149 | # target_link_libraries(${PROJECT_NAME}_node 150 | # ${catkin_LIBRARIES} 151 | # ) 152 | 153 | ############# 154 | ## Install ## 155 | ############# 156 | 157 | # all install targets should use catkin DESTINATION variables 158 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 159 | 160 | ## Mark executable scripts (Python etc.) for installation 161 | ## in contrast to setup.py, you can choose the destination 162 | # install(PROGRAMS 163 | # scripts/my_python_script 164 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 165 | # ) 166 | 167 | ## Mark executables and/or libraries for installation 168 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 169 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 170 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark cpp header files for installation 175 | # install(DIRECTORY include/${PROJECT_NAME}/ 176 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 177 | # FILES_MATCHING PATTERN "*.h" 178 | # PATTERN ".svn" EXCLUDE 179 | # ) 180 | 181 | ## Mark other files for installation (e.g. launch and bag files, etc.) 182 | # install(FILES 183 | # # myfile1 184 | # # myfile2 185 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 186 | # ) 187 | 188 | ############# 189 | ## Testing ## 190 | ############# 191 | 192 | ## Add gtest based cpp test target and link libraries 193 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_odom_to_trajectory.cpp) 194 | # if(TARGET ${PROJECT_NAME}-test) 195 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 196 | # endif() 197 | 198 | ## Add folders to be run by python nosetests 199 | # catkin_add_nosetests(test) 200 | 201 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @udacity/active-public-content -------------------------------------------------------------------------------- /Images/Output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/udacity/odom_to_trajectory/e5c1ba32f3c852c7bc79a2c38f0e1a3351557cd7/Images/Output.png -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Udacity 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 | [![Udacity - Robotics NanoDegree Program](https://s3-us-west-1.amazonaws.com/udacity-robotics/Extra+Images/RoboND_flag.png)](https://www.udacity.com/robotics) 2 | 3 | # odom_to_trajectory 4 | A ROS Package that subscribes to odometry values and publishes trajectories. This package will append the odometry values generated over time into vector of values. 5 | 6 | ### Nodes 7 | The package contains two nodes 8 | 1. **path_odom_plotter**: Subscribes to 2D unfiltered odometry, appends the robot last 1000 poses, and publishes the robot unfiltered trajectory. 9 | * Script File: path_odom_plotter.py 10 | * Subscriber: "/odom" 11 | * Publisher: "/odompath" 12 | 2. **path_ekf_plotter**: Subscribes to 3D filtered odometry, appends the robot last 1000 poses, and publishes the robot filtered trajectory. 13 | * Script File: path_ekf_plotter.py 14 | * Subscriber: "/robot_pose_ekf/odom_combined" 15 | * Publisher: "/ekfpath" 16 | 17 | ![alt text](Images/Output.png) 18 | 19 | ### Steps to launch the nodes 20 | #### Step1: Install the package 21 | ```sh 22 | $ cd /home/workspace/catkin_ws/src 23 | $ git clone https://github.com/udacity/odom_to_trajectory 24 | ``` 25 | #### Step2: Build the package 26 | ```sh 27 | $ cd /home/workspace/catkin_ws 28 | $ catkin_make 29 | $ source devel/setup.bash 30 | ``` 31 | #### Step3: Launch the nodes 32 | ```sh 33 | $ roslaunch odom_to_trajectory create_trajectory.launch 34 | ``` 35 | -------------------------------------------------------------------------------- /launch/create_trajectory.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | odom_to_trajectory 4 | 0.0.0 5 | The odom_to_trajectory package 6 | 7 | 8 | 9 | 10 | robond 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | roscpp 53 | rospy 54 | std_msgs 55 | roscpp 56 | rospy 57 | std_msgs 58 | roscpp 59 | rospy 60 | std_msgs 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /scripts/path_ekf_plotter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | import rospy 4 | from tf.transformations import quaternion_from_euler 5 | from std_msgs.msg import String 6 | from nav_msgs.msg import Odometry, Path 7 | from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped 8 | from sensor_msgs.msg import Joy 9 | 10 | import sys 11 | import json 12 | from math import sqrt 13 | from collections import deque 14 | 15 | import time 16 | 17 | 18 | def callback(data): 19 | global xAnt 20 | global yAnt 21 | global cont 22 | 23 | #Is created the pose msg, its necessary do it each time because Python manages objects by reference, 24 | #and does not make deep copies unless explicitly asked to do so. 25 | pose = PoseStamped() 26 | 27 | #Set a atributes of the msg 28 | pose.header.frame_id = "odom" 29 | pose.pose.position.x = float(data.pose.pose.position.x) 30 | pose.pose.position.y = float(data.pose.pose.position.y) 31 | pose.pose.orientation.x = float(data.pose.pose.orientation.x) 32 | pose.pose.orientation.y = float(data.pose.pose.orientation.y) 33 | pose.pose.orientation.z = float(data.pose.pose.orientation.z) 34 | pose.pose.orientation.w = float(data.pose.pose.orientation.w) 35 | 36 | #To avoid repeating the values, it is found that the received values are differents 37 | if (xAnt != pose.pose.position.x and yAnt != pose.pose.position.y): 38 | #Set a atributes of the msg 39 | pose.header.seq = path.header.seq + 1 40 | path.header.frame_id="odom" 41 | path.header.stamp=rospy.Time.now() 42 | pose.header.stamp = path.header.stamp 43 | path.poses.append(pose) 44 | #Published the msg 45 | 46 | cont=cont+1 47 | 48 | rospy.loginfo("Valor del contador: %i" % cont) 49 | if cont>max_append: 50 | path.poses.pop(0) 51 | 52 | pub.publish(path) 53 | 54 | #Save the last position 55 | xAnt=pose.pose.orientation.x 56 | yAnt=pose.pose.position.y 57 | return path 58 | 59 | 60 | 61 | 62 | if __name__ == '__main__': 63 | #Variable initialization 64 | global xAnt 65 | global yAnt 66 | global cont 67 | xAnt=0.0 68 | yAnt=0.0 69 | cont=0 70 | 71 | 72 | 73 | #Node and msg initialization 74 | rospy.init_node('path_ekf_plotter') 75 | 76 | 77 | #Rosparams that are set in the launch 78 | #max size of array pose msg from the path 79 | if not rospy.has_param("~max_list_append"): 80 | rospy.logwarn('The parameter max_list_append dont exists') 81 | max_append = rospy.set_param("~max_list_append",1000) 82 | max_append = 1000 83 | if not (max_append > 0): 84 | rospy.logwarn('The parameter max_list_append not is correct') 85 | sys.exit() 86 | pub = rospy.Publisher('/ekfpath', Path, queue_size=1) 87 | 88 | 89 | path = Path() #creamos el mensaje path de tipo path 90 | msg = Odometry() 91 | 92 | #Subscription to the topic 93 | msg = rospy.Subscriber('/robot_pose_ekf/odom_combined', PoseWithCovarianceStamped, callback) 94 | 95 | rate = rospy.Rate(30) # 30hz 96 | 97 | try: 98 | while not rospy.is_shutdown(): 99 | #rospy.spin() 100 | rate.sleep() 101 | except rospy.ROSInterruptException: 102 | pass 103 | -------------------------------------------------------------------------------- /scripts/path_odom_plotter.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | import rospy 4 | from tf.transformations import quaternion_from_euler 5 | from std_msgs.msg import String 6 | from nav_msgs.msg import Odometry, Path 7 | from geometry_msgs.msg import PoseWithCovarianceStamped, PoseStamped 8 | from sensor_msgs.msg import Joy 9 | 10 | import sys 11 | import json 12 | from math import sqrt 13 | from collections import deque 14 | 15 | import time 16 | 17 | 18 | def callback(data): 19 | global xAnt 20 | global yAnt 21 | global cont 22 | 23 | #Is created the pose msg, its necessary do it each time because Python manages objects by reference, 24 | #and does not make deep copies unless explicitly asked to do so. 25 | pose = PoseStamped() 26 | 27 | #Set a atributes of the msg 28 | pose.header.frame_id = "odom" 29 | pose.pose.position.x = float(data.pose.pose.position.x) 30 | pose.pose.position.y = float(data.pose.pose.position.y) 31 | pose.pose.orientation.x = float(data.pose.pose.orientation.x) 32 | pose.pose.orientation.y = float(data.pose.pose.orientation.y) 33 | pose.pose.orientation.z = float(data.pose.pose.orientation.z) 34 | pose.pose.orientation.w = float(data.pose.pose.orientation.w) 35 | 36 | #To avoid repeating the values, it is found that the received values are differents 37 | if (xAnt != pose.pose.position.x and yAnt != pose.pose.position.y): 38 | #Set a atributes of the msg 39 | pose.header.seq = path.header.seq + 1 40 | path.header.frame_id="odom" 41 | path.header.stamp=rospy.Time.now() 42 | pose.header.stamp = path.header.stamp 43 | path.poses.append(pose) 44 | #Published the msg 45 | 46 | cont=cont+1 47 | 48 | rospy.loginfo("Valor del contador: %i" % cont) 49 | if cont>max_append: 50 | path.poses.pop(0) 51 | 52 | pub.publish(path) 53 | 54 | #Save the last position 55 | xAnt=pose.pose.orientation.x 56 | yAnt=pose.pose.position.y 57 | return path 58 | 59 | 60 | 61 | 62 | if __name__ == '__main__': 63 | #Variable initialization 64 | global xAnt 65 | global yAnt 66 | global cont 67 | xAnt=0.0 68 | yAnt=0.0 69 | cont=0 70 | 71 | 72 | 73 | #Node and msg initialization 74 | rospy.init_node('path_odom_plotter') 75 | 76 | 77 | #Rosparams that are set in the launch 78 | #max size of array pose msg from the path 79 | if not rospy.has_param("~max_list_append"): 80 | rospy.logwarn('The parameter max_list_append dont exists') 81 | max_append = rospy.set_param("~max_list_append",1000) 82 | max_append = 1000 83 | if not (max_append > 0): 84 | rospy.logwarn('The parameter max_list_append not is correct') 85 | sys.exit() 86 | pub = rospy.Publisher('/odompath', Path, queue_size=1) 87 | 88 | 89 | path = Path() #creamos el mensaje path de tipo path 90 | msg = Odometry() 91 | 92 | #Subscription to the topic 93 | msg = rospy.Subscriber('/odom', Odometry, callback) 94 | 95 | rate = rospy.Rate(30) # 30hz 96 | 97 | try: 98 | while not rospy.is_shutdown(): 99 | #rospy.spin() 100 | rate.sleep() 101 | except rospy.ROSInterruptException: 102 | pass 103 | --------------------------------------------------------------------------------