├── CMakeLists.txt ├── launch ├── localization.launch └── turtlebot_IMU_integration.launch ├── msg └── Eulers.msg ├── package.xml ├── param ├── global_costmap_params.yaml ├── global_costmap_params.yaml~ └── move_base_params.yaml └── src ├── command.py ├── odomsync.py ├── publisheuler.py └── trajectory.py /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(mypack) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES mypack 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(mypack 115 | # src/${PROJECT_NAME}/mypack.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(mypack ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(mypack_node src/mypack_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(mypack_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(mypack_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS mypack mypack_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_mypack.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /launch/localization.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 9 | 10 | 11 | 12 | 13 | 17 | 18 | 19 | 23 | 24 | 25 | 29 | 30 | 31 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 61 | 62 | 63 | 67 | 68 | 69 | 70 | 71 | 78 | [true, true, false, 79 | false, false, true, 80 | true, true, false, 81 | false, false, true, 82 | false, false, false] 83 | 84 | 85 | [false, false, false, 86 | false, false, true, 87 | false, false, false, 88 | false, false, false, 89 | true, false, false] 90 | 91 | 107 | 108 | 109 | 110 | 114 | 115 | 116 | 117 | 119 | 120 | 121 | 123 | 124 | 125 | 126 | 127 | 130 | 131 | 133 | 134 | 135 | 136 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 147 | 148 | 149 | 150 | 151 | 154 | [0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 155 | 0, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 156 | 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 157 | 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 158 | 0, 0, 0, 0, 0.03, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 159 | 0, 0, 0, 0, 0, 0.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 160 | 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 0, 161 | 0, 0, 0, 0, 0, 0, 0, 0.025, 0, 0, 0, 0, 0, 0, 0, 162 | 0, 0, 0, 0, 0, 0, 0, 0, 0.04, 0, 0, 0, 0, 0, 0, 163 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 0, 164 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 0, 0, 165 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.02, 0, 0, 0, 166 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 0, 167 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.01, 0, 168 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.015] 169 | 170 | 174 | [1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 175 | 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 176 | 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 177 | 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 178 | 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 179 | 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 0, 180 | 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 0, 181 | 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 0, 182 | 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 0, 183 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 0, 184 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 0, 185 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 0, 186 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 0, 187 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9, 0, 188 | 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1e-9] 189 | 190 | 191 | 194 | 195 | 196 | 197 | 198 | -------------------------------------------------------------------------------- /launch/turtlebot_IMU_integration.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /msg/Eulers.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 roll 3 | float64 pitch 4 | float64 yaw 5 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turtlebot_IMU_integration 4 | 0.0.0 5 | The turtlebot_IMU_integration package 6 | 7 | 8 | 9 | 10 | therrma2 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 | message_generation 36 | 37 | 38 | 39 | message_runtime 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /odom 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: false 7 | transform_tolerance: 0.5 8 | rolling_window: true 9 | width: 10.0 10 | height: 10.0 11 | resolution: 0.05 12 | plugins: 13 | 14 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 15 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 16 | 17 | -------------------------------------------------------------------------------- /param/global_costmap_params.yaml~: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: /map 3 | robot_base_frame: /base_footprint 4 | update_frequency: 1.0 5 | publish_frequency: 0.5 6 | static_map: true 7 | transform_tolerance: 0.5 8 | plugins: 9 | - {name: static_layer, type: "costmap_2d::StaticLayer"} 10 | - {name: obstacle_layer, type: "costmap_2d::VoxelLayer"} 11 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 12 | 13 | -------------------------------------------------------------------------------- /param/move_base_params.yaml: -------------------------------------------------------------------------------- 1 | # Move base node parameters. For full documentation of the parameters in this file, please see 2 | # 3 | # http://www.ros.org/wiki/move_base 4 | # 5 | shutdown_costmaps: false 6 | 7 | controller_frequency: 5.0 8 | controller_patience: 3.0 9 | 10 | 11 | planner_frequency: 1.0 12 | planner_patience: 5.0 13 | 14 | oscillation_timeout: 10.0 15 | oscillation_distance: 0.2 16 | 17 | # local planner - default is trajectory rollout 18 | base_local_planner: "dwa_local_planner/DWAPlannerROS" 19 | 20 | base_global_planner: "navfn/NavfnROS" #alternatives: global_planner/GlobalPlanner, carrot_planner/CarrotPlanner 21 | 22 | 23 | #We plan to integrate recovery behaviors for turtlebot but currently those belong to gopher and still have to be adapted. 24 | ## recovery behaviors; we avoid spinning, but we need a fall-back replanning 25 | #recovery_behavior_enabled: true 26 | 27 | #recovery_behaviors: 28 | #- name: 'super_conservative_reset1' 29 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 30 | #- name: 'conservative_reset1' 31 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 32 | #- name: 'aggressive_reset1' 33 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 34 | #- name: 'clearing_rotation1' 35 | #type: 'rotate_recovery/RotateRecovery' 36 | #- name: 'super_conservative_reset2' 37 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 38 | #- name: 'conservative_reset2' 39 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 40 | #- name: 'aggressive_reset2' 41 | #type: 'clear_costmap_recovery/ClearCostmapRecovery' 42 | #- name: 'clearing_rotation2' 43 | #type: 'rotate_recovery/RotateRecovery' 44 | 45 | #super_conservative_reset1: 46 | #reset_distance: 3.0 47 | #conservative_reset1: 48 | #reset_distance: 1.5 49 | #aggressive_reset1: 50 | #reset_distance: 0.0 51 | #super_conservative_reset2: 52 | #reset_distance: 3.0 53 | #conservative_reset2: 54 | #reset_distance: 1.5 55 | #aggressive_reset2: 56 | #reset_distance: 0.0 57 | -------------------------------------------------------------------------------- /src/command.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import Twist,PoseStamped,Quaternion 6 | from geometry_msgs.msg import PoseWithCovarianceStamped 7 | from nav_msgs.msg import Odometry 8 | from std_msgs.msg import Empty,Char,UInt8 9 | from sensor_msgs.msg import Imu 10 | 11 | def main(): 12 | 13 | rospy.init_node('command') 14 | pub = rospy.Publisher('command_input',UInt8,queue_size = 10) 15 | while(1): 16 | command = input('Enter command: ') 17 | print command 18 | pub.publish(command) 19 | 20 | 21 | 22 | if __name__ == '__main__': 23 | 24 | 25 | main() 26 | -------------------------------------------------------------------------------- /src/odomsync.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import roslib 4 | import rospy 5 | import tf 6 | import math 7 | from geometry_msgs.msg import Twist,PoseStamped 8 | from geometry_msgs.msg import PoseWithCovarianceStamped 9 | from std_msgs.msg import Empty,UInt8 10 | from nav_msgs.msg import Odometry 11 | from overhead_mobile_tracker.srv import * 12 | 13 | 14 | def callback(data,args): 15 | 16 | pubodom,pubfiltered,listener,trans,rot,serv = args 17 | 18 | if data.data == 0: 19 | print "returned 0" 20 | #### Send empty message to reset_odometry topic to zero /odom 21 | empty = Empty() 22 | pubodom.publish(empty) 23 | 24 | #### Send 0 pose to /set_pose topic to /zero odometry/filtered 25 | newodom = PoseWithCovarianceStamped() 26 | newodom.header.frame_id = 'odom' 27 | newodom.header.stamp = rospy.Time.now() 28 | newodom.pose.pose.position.x = 0.0 29 | newodom.pose.pose.position.y = 0.0 30 | newodom.pose.pose.position.z = 0.0 31 | newodom.pose.pose.orientation.x = 0.0 32 | newodom.pose.pose.orientation.y = 0.0 33 | newodom.pose.pose.orientation.z = 0.0 34 | newodom.pose.pose.orientation.w = 1.0 35 | pubfiltered.publish(newodom) 36 | 37 | 38 | #### get current transform from base_meas to odom_meas 39 | 40 | 41 | try: 42 | (trans,rot) = listener.lookupTransform('/odom_meas','/base_meas',rospy.Time(0)) 43 | print trans 44 | print rot 45 | except: 46 | print "transform not found" 47 | 48 | offset = Odometry() 49 | xsave = offset.pose.pose.position.x = -trans[0] 50 | ysave = offset.pose.pose.position.y = -trans[1] 51 | zsave = offset.pose.pose.position.z = -trans[2] 52 | 53 | x2 = offset.pose.pose.orientation.x = 0.0 54 | y2 = offset.pose.pose.orientation.y = 0.0 55 | z2 = offset.pose.pose.orientation.z = 0.0 56 | w2 = offset.pose.pose.orientation.w = 0.0 57 | (roll,pitch,yaw) = tf.transformations.euler_from_quaternion((x2,y2,z2,w2)) 58 | 59 | #### use current transform to set odom_offset 60 | bool = serv(offset) 61 | print bool 62 | print "---------------------------------" 63 | 64 | try: 65 | (trans,rot) = listener.lookupTransform('/base_meas','/odom_meas',rospy.Time(0)) 66 | print trans 67 | print rot 68 | except: 69 | print "transform not found" 70 | 71 | offset2 = Odometry() 72 | 73 | offset2.pose.pose.position.x = xsave 74 | offset2.pose.pose.position.y = ysave 75 | offset2.pose.pose.position.z = zsave 76 | 77 | x2 = offset2.pose.pose.orientation.x = rot[0] 78 | y2 = offset2.pose.pose.orientation.y = rot[1] 79 | z2 = offset2.pose.pose.orientation.z = rot[2] 80 | w2 = offset2.pose.pose.orientation.w = rot[3] 81 | 82 | bool = serv(offset2) 83 | print bool, "step 1" 84 | 85 | 86 | if data.data == 99: 87 | print "returned 99" 88 | empty = Empty() 89 | pubodom.publish(empty) 90 | 91 | #### Send 0 pose to /set_pose topic to /zero odometry/filtered 92 | newodom = PoseWithCovarianceStamped() 93 | newodom.header.frame_id = 'odom' 94 | newodom.header.stamp = rospy.Time.now() 95 | newodom.pose.pose.position.x = 0.0 96 | newodom.pose.pose.position.y = 0.0 97 | newodom.pose.pose.position.z = 0.0 98 | newodom.pose.pose.orientation.x = 0.0 99 | newodom.pose.pose.orientation.y = 0.0 100 | newodom.pose.pose.orientation.z = 0.0 101 | newodom.pose.pose.orientation.w = 1.0 102 | pubfiltered.publish(newodom) 103 | 104 | #### get current transform from base_meas to odom_meas 105 | 106 | 107 | try: 108 | (trans,rot) = listener.lookupTransform('/odom_meas','/base_meas',rospy.Time(0)) 109 | print trans 110 | print rot 111 | except: 112 | print "transform not found" 113 | offset = Odometry() 114 | xsave = offset.pose.pose.position.x = -trans[0] 115 | ysave = offset.pose.pose.position.y = -trans[1] 116 | zsave = offset.pose.pose.position.z = -trans[2] 117 | 118 | x2 = offset.pose.pose.orientation.x = 0.0 119 | y2 = offset.pose.pose.orientation.y = 0.0 120 | z2 = offset.pose.pose.orientation.z = 0.0 121 | w2 = offset.pose.pose.orientation.w = 0.0 122 | (roll,pitch,yaw) = tf.transformations.euler_from_quaternion((x2,y2,z2,w2)) 123 | 124 | #### use current transform to set odom_offset 125 | try: 126 | (trans,rot) = listener.lookupTransform('/base_meas','/odom_meas',rospy.Time(0)) 127 | print trans 128 | print rot 129 | except: 130 | print "transform not found" 131 | 132 | offset2 = Odometry() 133 | 134 | offset2.pose.pose.position.x = xsave 135 | offset2.pose.pose.position.y = ysave 136 | offset2.pose.pose.position.z = zsave 137 | 138 | x2 = offset2.pose.pose.orientation.x = 0 139 | y2 = offset2.pose.pose.orientation.y = 0 140 | z2 = offset2.pose.pose.orientation.z = 0 141 | w2 = offset2.pose.pose.orientation.w = 0 142 | 143 | bool = serv(offset2) 144 | print bool, "step 1" 145 | 146 | 147 | 148 | 149 | if data.data == 10: 150 | print "returned 10" 151 | zero = Odometry() 152 | zero.pose.pose.position.x = 0.0 153 | zero.pose.pose.position.y = 0.0 154 | zero.pose.pose.position.z = 0.0 155 | 156 | x2 = zero.pose.pose.orientation.x = 0.0 157 | y2 = zero.pose.pose.orientation.y = 0.0 158 | z2 = zero.pose.pose.orientation.z = 0.0 159 | w2 = zero.pose.pose.orientation.w = 1.0 160 | 161 | (roll,pitch,yaw) = tf.transformations.euler_from_quaternion((x2,y2,z2,w2)) 162 | #print zero 163 | bool = serv(zero) 164 | 165 | print bool 166 | print "----------------------------" 167 | 168 | 169 | def main(): 170 | trans= 0 171 | rot = 0 172 | rospy.init_node('odom_sync') 173 | listener = tf.TransformListener() 174 | pubfiltered = rospy.Publisher('set_pose',PoseWithCovarianceStamped,queue_size = 10) 175 | pubodom = rospy.Publisher('mobile_base/commands/reset_odometry', Empty, queue_size = 10) 176 | serv = rospy.ServiceProxy("set_offset",SetOdomOffset) 177 | rospy.Subscriber('/command_input',UInt8,callback,callback_args=(pubodom,pubfiltered,listener,trans,rot,serv)) 178 | rospy.sleep(1) 179 | print "done sleeping" 180 | 181 | while not rospy.is_shutdown(): 182 | try: 183 | (trans,rot) = listener.lookupTransform('/base_meas', '/odom_meas',rospy.Time(0)) 184 | 185 | except: continue 186 | 187 | 188 | rospy.spin() 189 | 190 | 191 | 192 | 193 | if __name__ == '__main__': 194 | 195 | 196 | main() 197 | -------------------------------------------------------------------------------- /src/publisheuler.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import Twist,PoseStamped,Quaternion 6 | from geometry_msgs.msg import PoseWithCovarianceStamped 7 | from nav_msgs.msg import Odometry 8 | from std_msgs.msg import Empty 9 | from sensor_msgs.msg import Imu 10 | 11 | 12 | def callbackimu(data,pub): 13 | pose = PoseStamped() 14 | pose.header = data.header 15 | quaternion = ( 16 | data.orientation.x, 17 | data.orientation.y, 18 | data.orientation.z, 19 | data.orientation.w) 20 | 21 | euler = tf.transformations.euler_from_quaternion(quaternion) 22 | 23 | pose.pose.orientation.x = euler[0] 24 | pose.pose.orientation.y = euler[1] 25 | pose.pose.orientation.z = euler[2] 26 | pub.publish(pose) 27 | 28 | 29 | def callbackodom(data,pub): 30 | pose = PoseStamped() 31 | pose.header = data.header 32 | quaternion = ( 33 | data.pose.pose.orientation.x, 34 | data.pose.pose.orientation.y, 35 | data.pose.pose.orientation.z, 36 | data.pose.pose.orientation.w) 37 | 38 | euler = tf.transformations.euler_from_quaternion(quaternion) 39 | 40 | pose.pose.orientation.x = euler[0] 41 | pose.pose.orientation.y = euler[1] 42 | pose.pose.orientation.z = euler[2] 43 | pub.publish(pose) 44 | 45 | 46 | def callbackfiltered(data,pub): 47 | pose = PoseStamped() 48 | pose.header = data.header 49 | quaternion = ( 50 | data.pose.pose.orientation.x, 51 | data.pose.pose.orientation.y, 52 | data.pose.pose.orientation.z, 53 | data.pose.pose.orientation.w) 54 | 55 | euler = tf.transformations.euler_from_quaternion(quaternion) 56 | 57 | pose.pose.orientation.x = euler[0] 58 | pose.pose.orientation.y = euler[1] 59 | pose.pose.orientation.z = euler[2] 60 | pub.publish(pose) 61 | 62 | 63 | def callbackmeas(data,pub): 64 | pose = PoseStamped() 65 | pose.header = data.header 66 | quaternion= ( 67 | data.pose.pose.orientation.x, 68 | data.pose.pose.orientation.y, 69 | data.pose.pose.orientation.z, 70 | data.pose.pose.orientation.w) 71 | 72 | euler = tf.transformations.euler_from_quaternion(quaternion) 73 | 74 | pose.pose.orientation.x = euler[0] 75 | pose.pose.orientation.y = euler[1] 76 | pose.pose.orientation.z = euler[2] 77 | pub.publish(pose) 78 | 79 | def pubeuler(): 80 | 81 | rospy.init_node('pub_euler') 82 | pubimu = rospy.Publisher('euler_imu', PoseStamped,queue_size = 10) 83 | pubodom = rospy.Publisher('euler_odom', PoseStamped,queue_size = 10) 84 | pubfiltered = rospy.Publisher('euler_filtered', PoseStamped,queue_size = 10) 85 | pubmeas = rospy.Publisher('euler_meas',PoseStamped,queue_size = 10) 86 | 87 | rospy.Subscriber('/imu',Imu,callbackimu, callback_args=pubimu) 88 | rospy.Subscriber('/odom',Odometry,callbackodom, callback_args=pubodom) 89 | rospy.Subscriber('/odometry/filtered',Odometry,callbackfiltered, callback_args=pubfiltered) 90 | rospy.Subscriber('/meas_pose',Odometry,callbackmeas,callback_args=pubmeas) 91 | 92 | 93 | rospy.spin() 94 | 95 | 96 | if __name__ == '__main__': 97 | 98 | 99 | pubeuler() 100 | 101 | -------------------------------------------------------------------------------- /src/trajectory.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | from geometry_msgs.msg import Twist,PoseStamped,Quaternion 6 | from geometry_msgs.msg import PoseWithCovarianceStamped 7 | from nav_msgs.msg import Odometry 8 | from std_msgs.msg import Empty,UInt8 9 | from sensor_msgs.msg import Imu 10 | 11 | 12 | def callback(data,pub): 13 | t = Twist() 14 | 15 | if data.data == 1: 16 | t.linear.x = .25 17 | print t 18 | if data.data == 2: 19 | t.angular.z = 2 20 | if data.data == 3: 21 | t.linear.x = .25 22 | t.angular.z = 1 23 | 24 | time = rospy.get_time() 25 | while rospy.get_time()-time < 6: 26 | pub.publish (t) 27 | rospy.sleep(.005) 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | def main(): 37 | rospy.init_node('trajectory') 38 | pub = rospy.Publisher('cmd_vel_mux/input/navi',Twist,queue_size = 10) 39 | 40 | rospy.Subscriber('/command_input',UInt8,callback,callback_args=pub) 41 | 42 | rospy.spin() 43 | 44 | 45 | 46 | if __name__ == '__main__': 47 | 48 | 49 | main() 50 | --------------------------------------------------------------------------------