├── .gitignore ├── LICENSE.md ├── README.md ├── turbot ├── CMakeLists.txt └── package.xml ├── turbot_apps ├── CMakeLists.txt ├── launch │ ├── dock.launch │ ├── dock_activate.launch │ ├── follower.launch │ └── panorama.launch └── package.xml ├── turbot_arm ├── CMakeLists.txt ├── package.xml └── scripts │ ├── arm.rules │ ├── create_udev_rules.sh │ └── delete_udev_rules.sh ├── turbot_bringup ├── CMakeLists.txt ├── launch │ ├── kinect.launch │ ├── minimal.launch │ └── start.launch └── package.xml ├── turbot_cali ├── CMakeLists.txt └── package.xml ├── turbot_code ├── CMakeLists.txt ├── package.xml └── scripts │ ├── calibrateAngular.py │ ├── calibrateLinear.py │ ├── drawSquare.py │ ├── followTheRoute.py │ ├── goBack.py │ ├── goButton.py │ ├── goForward.py │ ├── goForwardWithAvoidObstacle.py │ ├── goLeft.py │ ├── goRight.py │ ├── goRound.py │ ├── goToSpecificPointOnMap.py │ ├── go_to_specific_point_on_map.py │ ├── kobukiBattery.py │ ├── moveBaseSquare.py │ ├── navMultiPoints.py │ ├── navMultiPoints2.py │ ├── navMultiPoints3.py │ ├── navMultiPoints4.py │ ├── navMultiPoints5.py │ ├── netbookBattery.py │ ├── odomOutAndBack.py │ ├── odomSquare.py │ └── takePhoto.py ├── turbot_map ├── CMakeLists.txt ├── map │ ├── cartoghapher_kinect_v1.pgm │ ├── cartoghapher_kinect_v1.yaml │ ├── cartographer_rplidar_a2.pgm │ ├── cartographer_rplidar_a2.yaml │ ├── rplidar_a2.pgm │ ├── rplidar_a2.yaml │ ├── rplidar_a2_hector_noodom.pgm │ ├── rplidar_a2_hector_noodom.yaml │ ├── rplidar_a2_karto.pgm │ ├── rplidar_a2_karto.yaml │ ├── test.pgm │ └── test.yaml ├── package.xml └── script │ └── saver ├── turbot_rviz ├── CMakeLists.txt ├── launch │ ├── nav.launch │ ├── nav_cartographer.launch │ └── nav_cartographer_3d.launch ├── package.xml └── rviz │ └── cartographer_3d.rviz ├── turbot_slam ├── CMakeLists.txt ├── launch │ ├── amcl │ │ ├── amcl_demo.launch │ │ ├── laser_amcl_demo.launch │ │ └── laser_amcl_rplidar.launch │ ├── cartographer │ │ ├── assets │ │ │ └── turtlebot_assets_3d.launch │ │ ├── cartographer.launch │ │ ├── cartographer_demo_2d.launch │ │ ├── cartographer_demo_3d.launch │ │ ├── cartographer_laser.launch │ │ ├── cartographer_laser_3d.launch │ │ ├── laser_cartographer.launch │ │ ├── laser_cartographer_3d.launch │ │ ├── laser_cartographer_demo.launch │ │ ├── laser_cartographer_demo_3d.launch │ │ ├── laser_cartographer_demo_3d_offline.launch │ │ ├── lua │ │ │ ├── transform.lua │ │ │ ├── turtlebot_assets_rslidar_3d.lua │ │ │ └── turtlebot_rslidar_3d.lua │ │ ├── turtlebot_lida.launch │ │ └── urdf │ │ │ ├── kobuki_hexagons_kinect.urdf │ │ │ └── turtlebot_urdf_rslidar_3d.launch │ ├── gmapping │ │ ├── gmapping_demo.launch │ │ └── laser_gmapping_demo.launch │ ├── hector │ │ ├── laser_hector_demo.launch │ │ └── readme.txt │ ├── karto │ │ ├── karto_demo.launch │ │ └── laser_karto_demo.launch │ └── readme.txt └── package.xml ├── turbot_teleop ├── CMakeLists.txt ├── launch │ └── keyboard.launch └── package.xml ├── turbot_tools ├── CMakeLists.txt ├── launch │ ├── freenect.launch │ ├── rplidar.launch │ └── test_rplidar.launch ├── package.xml └── script │ ├── test_draw_a_square.py │ ├── test_goforward.py │ ├── test_goincircles.py │ ├── test_kinect_color │ ├── test_kinect_depth │ ├── test_kobuki │ ├── test_lidar │ └── test_move └── turbot_vslam ├── CMakeLists.txt ├── Makefile ├── launch └── rtabmap │ ├── rtabmap.launch │ └── rtabmap_rviz.launch ├── mainpage.dox └── manifest.xml /.gitignore: -------------------------------------------------------------------------------- 1 | bin 2 | build 3 | lib 4 | *.swp 5 | *.swo 6 | *.pyc 7 | -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017, AiZheTeng / ChuangKeZhiZao 2 | 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without modification, 6 | are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, this 12 | list of conditions and the following disclaimer in the documentation and/or 13 | other materials provided with the distribution. 14 | 15 | * Neither the name of the {organization} nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 20 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 21 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR 23 | ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 24 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON 26 | ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 27 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 28 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | 30 | Author: ncnynl 31 | website: http://ncnynl.com | http://imakething.com 32 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Turbot 2 | Turbot is Custom Edition of Turtlebot2 For SLAM and DL . 3 | 4 | 5 | Hardware: 6 | 7 | - Kobuki 8 | - TK1/TX1/TX2 9 | - Power cable and Adapter for TK1/TX1/TX2 10 | - Wireless Card (intel 7260) + antenna * 2 11 | - RPlidar A1 / RPlidar A2 / EAI F4 12 | - Kinect v1 13 | - 10400mah lithium battery 14 | - USB-HUB (4 ports) 15 | 16 | Software: 17 | 18 | - Dependent on Turtlebot / rplidar_ros / freenect and so on 19 | - SLAM: gmapping, hector_slam, cartographer SLAM, ORB-SLAM2, RGBD-SLAM-V2, AMCL 20 | - DeepLearning: Caffe, TensorFlow, Torch, Theano, Mxnet 21 | 22 | Doc: 23 | 24 | - http://www.ncnynl.com 25 | - For Turbot 1：http://www.ncnynl.com/category/turbot/ 26 | - For Turbot 2：http://www.ncnynl.com/category/turbot-medium/ 27 | - For SLAM： http://www.ncnynl.com/category/turbot-SLAM/ 28 | - For DL： http://www.ncnynl.com/category/turbot-DL/ 29 | 30 | 31 | Picture: 32 | 33 | - http://images.ncnynl.com/ros/2017/IMG_3611_1.jpg 34 | - http://images.ncnynl.com/ros/2017/IMG_3614_4.jpg 35 | - http://images.ncnynl.com/ros/2017/IMG_3615_5.jpg 36 | 37 | Buy: 38 | 39 | - http://ncnynl.taobao.com 40 | 41 | 42 | -------------------------------------------------------------------------------- /turbot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /turbot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | turbot 3 | 0.0.0 4 | The turbot package 5 | 6 | 7 | 8 | 9 | ubuntu 10 | 11 | 12 | 13 | 14 | 15 | TODO 16 | 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 | catkin 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /turbot_apps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_apps) 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 COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES turbot_apps 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(turbot_apps 122 | # src/${PROJECT_NAME}/turbot_apps.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(turbot_apps ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(turbot_apps_node src/turbot_apps_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(turbot_apps_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(turbot_apps_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS turbot_apps turbot_apps_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turbot_apps.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /turbot_apps/launch/dock.launch: -------------------------------------------------------------------------------- 1 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /turbot_apps/launch/dock_activate.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /turbot_apps/launch/follower.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /turbot_apps/launch/panorama.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /turbot_apps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_apps 4 | 0.0.0 5 | The turbot_apps package 6 | 7 | 8 | 9 | 10 | nc 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /turbot_arm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_arm) 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 COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES turbot_arm 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(turbot_arm 122 | # src/${PROJECT_NAME}/turbot_arm.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(turbot_arm ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(turbot_arm_node src/turbot_arm_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(turbot_arm_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(turbot_arm_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS turbot_arm turbot_arm_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turbot_arm.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /turbot_arm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_arm 4 | 0.0.0 5 | The turbot_arm package 6 | 7 | 8 | 9 | 10 | nc 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /turbot_arm/scripts/arm.rules: -------------------------------------------------------------------------------- 1 | # set the udev rule , make the device_port be fixed by rplidar 2 | # 3 | KERNEL=="ttyUSB*", ATTRS{idVendor}=="0403", ATTRS{idProduct}=="6001", MODE:="0777", SYMLINK+="arm" 4 | 5 | -------------------------------------------------------------------------------- /turbot_arm/scripts/create_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "remap the device serial port(ttyUSBX) to turbot_arm" 4 | echo "turbot_arm usb connection as /dev/arm , check it using the command : ls -l /dev|grep ttyUSB" 5 | echo "start copy arm.rules to /etc/udev/rules.d/" 6 | echo "`rospack find turbot_arm`/scripts/arm.rules" 7 | sudo cp `rospack find turbot_arm`/scripts/arm.rules /etc/udev/rules.d 8 | echo " " 9 | echo "Restarting udev" 10 | echo "" 11 | sudo service udev reload 12 | sudo service udev restart 13 | echo "finish " 14 | -------------------------------------------------------------------------------- /turbot_arm/scripts/delete_udev_rules.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "delete remap the device serial port(ttyUSBX) to turbot_arm" 4 | echo "sudo rm /etc/udev/rules.d/arm.rules" 5 | sudo rm /etc/udev/rules.d/arm.rules 6 | echo " " 7 | echo "Restarting udev" 8 | echo "" 9 | sudo service udev reload 10 | sudo service udev restart 11 | echo "finish delete" 12 | -------------------------------------------------------------------------------- /turbot_bringup/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_bringup) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-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 you 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 turbot_bringup 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(include) 119 | include_directories( 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/turbot_bringup.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/turbot_bringup_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_turbot_bringup.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 | -------------------------------------------------------------------------------- /turbot_bringup/launch/kinect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /turbot_bringup/launch/minimal.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /turbot_bringup/launch/start.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /turbot_bringup/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_bringup 4 | 0.0.0 5 | The turbot_bringup package 6 | 7 | 8 | 9 | 10 | ubuntu 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /turbot_cali/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_cali) 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 COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES turbot_cali 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(turbot_cali 122 | # src/${PROJECT_NAME}/turbot_cali.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(turbot_cali ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(turbot_cali_node src/turbot_cali_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(turbot_cali_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(turbot_cali_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS turbot_cali turbot_cali_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turbot_cali.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /turbot_cali/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_cali 4 | 0.0.0 5 | The turbot_cali package 6 | 7 | 8 | 9 | 10 | nc 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /turbot_code/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_code) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-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 you 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 turbot_code 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(include) 119 | include_directories( 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/turbot_code.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/turbot_code_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_turbot_code.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 | -------------------------------------------------------------------------------- /turbot_code/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_code 4 | 0.0.0 5 | The turbot_code package 6 | 7 | 8 | 9 | 10 | ubuntu 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /turbot_code/scripts/calibrateAngular.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import Twist, Quaternion 3 | from nav_msgs.msg import Odometry 4 | from dynamic_reconfigure.server import Server 5 | import dynamic_reconfigure.client 6 | from rbx1_nav.cfg import CalibrateAngularConfig 7 | import tf 8 | from math import radians, copysign 9 | from rbx1_nav.transform_utils import quat_to_angle, normalize_angle 10 | 11 | class CalibrateAngular(): 12 | def __init__(self): 13 | # Give the node a name 14 | rospy.init_node('calibrate_angular', anonymous=False) 15 | 16 | # Set rospy to execute a shutdown function when terminating the script 17 | rospy.on_shutdown(self.shutdown) 18 | 19 | # How fast will we check the odometry values? 20 | self.rate = rospy.get_param('~rate', 20) 21 | r = rospy.Rate(self.rate) 22 | 23 | # The test angle is 360 degrees 24 | self.test_angle = radians(rospy.get_param('~test_angle', 360.0)) 25 | 26 | self.speed = rospy.get_param('~speed', 0.5) # radians per second 27 | self.tolerance = radians(rospy.get_param('tolerance', 1)) # degrees converted to radians 28 | self.odom_angular_scale_correction = rospy.get_param('~odom_angular_scale_correction', 1.0) 29 | self.start_test = rospy.get_param('~start_test', True) 30 | 31 | # Publisher to control the robot's speed 32 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 33 | 34 | # Fire up the dynamic_reconfigure server 35 | dyn_server = Server(CalibrateAngularConfig, self.dynamic_reconfigure_callback) 36 | 37 | # Connect to the dynamic_reconfigure server 38 | dyn_client = dynamic_reconfigure.client.Client("calibrate_angular", timeout=60) 39 | 40 | # The base frame is usually base_link or base_footprint 41 | self.base_frame = rospy.get_param('~base_frame', '/base_link') 42 | 43 | # The odom frame is usually just /odom 44 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 45 | 46 | # Initialize the tf listener 47 | self.tf_listener = tf.TransformListener() 48 | 49 | # Give tf some time to fill its buffer 50 | rospy.sleep(2) 51 | 52 | # Make sure we see the odom and base frames 53 | self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 54 | 55 | rospy.loginfo("Bring up rqt_reconfigure to control the test.") 56 | 57 | reverse = 1 58 | 59 | while not rospy.is_shutdown(): 60 | if self.start_test: 61 | # Get the current rotation angle from tf 62 | self.odom_angle = self.get_odom_angle() 63 | 64 | last_angle = self.odom_angle 65 | turn_angle = 0 66 | self.test_angle *= reverse 67 | error = self.test_angle - turn_angle 68 | 69 | # Alternate directions between tests 70 | reverse = -reverse 71 | 72 | while abs(error) > self.tolerance and self.start_test: 73 | if rospy.is_shutdown(): 74 | return 75 | 76 | # Rotate the robot to reduce the error 77 | move_cmd = Twist() 78 | move_cmd.angular.z = copysign(self.speed, error) 79 | self.cmd_vel.publish(move_cmd) 80 | r.sleep() 81 | 82 | # Get the current rotation angle from tf 83 | self.odom_angle = self.get_odom_angle() 84 | 85 | # Compute how far we have gone since the last measurement 86 | delta_angle = self.odom_angular_scale_correction * normalize_angle(self.odom_angle - last_angle) 87 | 88 | # Add to our total angle so far 89 | turn_angle += delta_angle 90 | 91 | # Compute the new error 92 | error = self.test_angle - turn_angle 93 | 94 | # Store the current angle for the next comparison 95 | last_angle = self.odom_angle 96 | 97 | # Stop the robot 98 | self.cmd_vel.publish(Twist()) 99 | 100 | # Update the status flag 101 | self.start_test = False 102 | params = {'start_test': False} 103 | dyn_client.update_configuration(params) 104 | 105 | rospy.sleep(0.5) 106 | 107 | # Stop the robot 108 | self.cmd_vel.publish(Twist()) 109 | 110 | def get_odom_angle(self): 111 | # Get the current transform between the odom and base frames 112 | try: 113 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 114 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 115 | rospy.loginfo("TF Exception") 116 | return 117 | 118 | # Convert the rotation from a quaternion to an Euler angle 119 | return quat_to_angle(Quaternion(*rot)) 120 | 121 | def dynamic_reconfigure_callback(self, config, level): 122 | self.test_angle = radians(config['test_angle']) 123 | self.speed = config['speed'] 124 | self.tolerance = radians(config['tolerance']) 125 | self.odom_angular_scale_correction = config['odom_angular_scale_correction'] 126 | self.start_test = config['start_test'] 127 | 128 | return config 129 | 130 | def shutdown(self): 131 | # Always stop the robot when shutting down the node 132 | rospy.loginfo("Stopping the robot...") 133 | self.cmd_vel.publish(Twist()) 134 | rospy.sleep(1) 135 | 136 | if __name__ == '__main__': 137 | try: 138 | CalibrateAngular() 139 | except: 140 | rospy.loginfo("Calibration terminated.") 141 | -------------------------------------------------------------------------------- /turbot_code/scripts/calibrateLinear.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from geometry_msgs.msg import Twist, Point 3 | from math import copysign, sqrt, pow 4 | from dynamic_reconfigure.server import Server 5 | import dynamic_reconfigure.client 6 | from rbx1_nav.cfg import CalibrateLinearConfig 7 | import tf 8 | 9 | class CalibrateLinear(): 10 | def __init__(self): 11 | # Give the node a name 12 | rospy.init_node('calibrate_linear', anonymous=False) 13 | 14 | # Set rospy to execute a shutdown function when terminating the script 15 | rospy.on_shutdown(self.shutdown) 16 | 17 | # How fast will we check the odometry values? 18 | self.rate = rospy.get_param('~rate', 20) 19 | r = rospy.Rate(self.rate) 20 | 21 | # Set the distance to travel 22 | self.test_distance = rospy.get_param('~test_distance', 1.0) # meters 23 | self.speed = rospy.get_param('~speed', 0.15) # meters per second 24 | self.tolerance = rospy.get_param('~tolerance', 0.01) # meters 25 | self.odom_linear_scale_correction = rospy.get_param('~odom_linear_scale_correction', 1.0) 26 | self.start_test = rospy.get_param('~start_test', True) 27 | 28 | # Publisher to control the robot's speed 29 | self.cmd_vel = rospy.Publisher('/cmd_vel', Twist, queue_size=5) 30 | 31 | # Fire up the dynamic_reconfigure server 32 | dyn_server = Server(CalibrateLinearConfig, self.dynamic_reconfigure_callback) 33 | 34 | # Connect to the dynamic_reconfigure server 35 | dyn_client = dynamic_reconfigure.client.Client("calibrate_linear", timeout=60) 36 | 37 | # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot 38 | self.base_frame = rospy.get_param('~base_frame', '/base_link') 39 | 40 | # The odom frame is usually just /odom 41 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 42 | 43 | # Initialize the tf listener 44 | self.tf_listener = tf.TransformListener() 45 | 46 | # Give tf some time to fill its buffer 47 | rospy.sleep(2) 48 | 49 | # Make sure we see the odom and base frames 50 | self.tf_listener.waitForTransform(self.odom_frame, self.base_frame, rospy.Time(), rospy.Duration(60.0)) 51 | 52 | rospy.loginfo("Bring up rqt_reconfigure to control the test.") 53 | 54 | self.position = Point() 55 | 56 | # Get the starting position from the tf transform between the odom and base frames 57 | self.position = self.get_position() 58 | 59 | x_start = self.position.x 60 | y_start = self.position.y 61 | 62 | move_cmd = Twist() 63 | 64 | while not rospy.is_shutdown(): 65 | # Stop the robot by default 66 | move_cmd = Twist() 67 | 68 | if self.start_test: 69 | # Get the current position from the tf transform between the odom and base frames 70 | self.position = self.get_position() 71 | 72 | # Compute the Euclidean distance from the target point 73 | distance = sqrt(pow((self.position.x - x_start), 2) + 74 | pow((self.position.y - y_start), 2)) 75 | 76 | # Correct the estimated distance by the correction factor 77 | distance *= self.odom_linear_scale_correction 78 | 79 | # How close are we? 80 | error = distance - self.test_distance 81 | 82 | # Are we close enough? 83 | if not self.start_test or abs(error) < self.tolerance: 84 | self.start_test = False 85 | params = {'start_test': False} 86 | rospy.loginfo(params) 87 | dyn_client.update_configuration(params) 88 | else: 89 | # If not, move in the appropriate direction 90 | move_cmd.linear.x = copysign(self.speed, -1 * error) 91 | else: 92 | self.position = self.get_position() 93 | x_start = self.position.x 94 | y_start = self.position.y 95 | 96 | self.cmd_vel.publish(move_cmd) 97 | r.sleep() 98 | 99 | # Stop the robot 100 | self.cmd_vel.publish(Twist()) 101 | 102 | def dynamic_reconfigure_callback(self, config, level): 103 | self.test_distance = config['test_distance'] 104 | self.speed = config['speed'] 105 | self.tolerance = config['tolerance'] 106 | self.odom_linear_scale_correction = config['odom_linear_scale_correction'] 107 | self.start_test = config['start_test'] 108 | 109 | return config 110 | 111 | def get_position(self): 112 | # Get the current transform between the odom and base frames 113 | try: 114 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 115 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 116 | rospy.loginfo("TF Exception") 117 | return 118 | 119 | return Point(*trans) 120 | 121 | def shutdown(self): 122 | # Always stop the robot when shutting down the node 123 | rospy.loginfo("Stopping the robot...") 124 | self.cmd_vel.publish(Twist()) 125 | rospy.sleep(1) 126 | 127 | if __name__ == '__main__': 128 | try: 129 | CalibrateLinear() 130 | rospy.spin() 131 | except: 132 | rospy.loginfo("Calibration terminated.") 133 | -------------------------------------------------------------------------------- /turbot_code/scripts/drawSquare.py: -------------------------------------------------------------------------------- 1 | # An example of TurtleBot 2 drawing a 0.4 meter square. 2 | # Written for indigo 3 | # drawSquare.py 4 | 5 | import rospy 6 | from geometry_msgs.msg import Twist 7 | from math import radians 8 | 9 | class DrawASquare(): 10 | def __init__(self): 11 | # initiliaze 12 | rospy.init_node('drawasquare', anonymous=False) 13 | 14 | # What to do you ctrl + c 15 | rospy.on_shutdown(self.shutdown) 16 | 17 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 18 | 19 | # 5 HZ 20 | r = rospy.Rate(5); 21 | 22 | # create two different Twist() variables. One for moving forward. One for turning 45 degrees. 23 | 24 | # let's go forward at 0.2 m/s 25 | move_cmd = Twist() 26 | move_cmd.linear.x = 0.2 27 | # by default angular.z is 0 so setting this isn't required 28 | 29 | #let's turn at 45 deg/s 30 | turn_cmd = Twist() 31 | turn_cmd.linear.x = 0 32 | turn_cmd.angular.z = radians(45); #45 deg/s in radians/s 33 | 34 | #two keep drawing squares. Go forward for 2 seconds (10 x 5 HZ) then turn for 2 second 35 | count = 0 36 | while not rospy.is_shutdown(): 37 | # go forward 0.4 m (2 seconds * 0.2 m / seconds) 38 | rospy.loginfo("Going Straight") 39 | for x in range(0,10): 40 | self.cmd_vel.publish(move_cmd) 41 | r.sleep() 42 | # turn 90 degrees 43 | rospy.loginfo("Turning") 44 | for x in range(0,10): 45 | self.cmd_vel.publish(turn_cmd) 46 | r.sleep() 47 | count = count + 1 48 | if(count == 4): 49 | count = 0 50 | if(count == 0): 51 | rospy.loginfo("TurtleBot should be close to the original starting position (but it's probably way off)") 52 | 53 | def shutdown(self): 54 | # stop turtlebot 55 | rospy.loginfo("Stop Drawing Squares") 56 | self.cmd_vel.publish(Twist()) 57 | rospy.sleep(1) 58 | 59 | if __name__ == '__main__': 60 | try: 61 | DrawASquare() 62 | except: 63 | rospy.loginfo("node terminated.") 64 | -------------------------------------------------------------------------------- /turbot_code/scripts/followTheRoute.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import yaml 3 | from take_photo import TakePhoto 4 | from go_to_specific_point_on_map import GoToPose 5 | 6 | if __name__ == '__main__': 7 | 8 | # Read information from yaml file 9 | with open("route.yaml", 'r') as stream: 10 | dataMap = yaml.load(stream) 11 | 12 | try: 13 | # Initialize 14 | rospy.init_node('follow_route', anonymous=False) 15 | navigator = GoToPose() 16 | camera = TakePhoto() 17 | 18 | for obj in dataMap: 19 | 20 | if rospy.is_shutdown(): 21 | break 22 | 23 | name = obj['filename'] 24 | 25 | # Navigation 26 | rospy.loginfo("Go to %s pose", name[:-4]) 27 | success = navigator.goto(obj['position'], obj['quaternion']) 28 | if not success: 29 | rospy.loginfo("Failed to reach %s pose", name[:-4]) 30 | continue 31 | rospy.loginfo("Reached %s pose", name[:-4]) 32 | 33 | # Take a photo 34 | if camera.take_picture(name): 35 | rospy.loginfo("Saved image " + name) 36 | else: 37 | rospy.loginfo("No images received") 38 | 39 | rospy.sleep(1) 40 | 41 | except rospy.ROSInterruptException: 42 | rospy.loginfo("Ctrl-C caught. Quitting") 43 | -------------------------------------------------------------------------------- /turbot_code/scripts/goBack.py: -------------------------------------------------------------------------------- 1 | # A very basic TurtleBot script that moves TurtleBot forward indefinitely. Press CTRL + C to stop. To run: 2 | # On TurtleBot: 3 | # roslaunch turtlebot_bringup minimal.launch 4 | # On work station: 5 | # python goBack.py 6 | 7 | import rospy 8 | from geometry_msgs.msg import Twist 9 | 10 | class GoForward(): 11 | def __init__(self): 12 | # initiliaze 13 | rospy.init_node('GoForward', anonymous=False) 14 | 15 | # tell user how to stop TurtleBot 16 | rospy.loginfo("To stop TurtleBot CTRL + C") 17 | 18 | # What function to call when you ctrl + c 19 | rospy.on_shutdown(self.shutdown) 20 | 21 | # Create a publisher which can "talk" to TurtleBot and tell it to move 22 | # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 23 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 24 | 25 | #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ 26 | r = rospy.Rate(10); 27 | 28 | # Twist is a datatype for velocity 29 | move_cmd = Twist() 30 | # let's go forward at 0.2 m/s 31 | move_cmd.linear.x = -0.2 32 | # let's turn at 0 radians/s 33 | move_cmd.angular.z = 0 34 | 35 | # as long as you haven't ctrl + c keeping doing... 36 | while not rospy.is_shutdown(): 37 | # publish the velocity 38 | self.cmd_vel.publish(move_cmd) 39 | # wait for 0.1 seconds (10 HZ) and publish again 40 | r.sleep() 41 | 42 | 43 | def shutdown(self): 44 | # stop turtlebot 45 | rospy.loginfo("Stop TurtleBot") 46 | # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot 47 | self.cmd_vel.publish(Twist()) 48 | # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script 49 | rospy.sleep(1) 50 | 51 | if __name__ == '__main__': 52 | try: 53 | GoForward() 54 | except: 55 | rospy.loginfo("GoForward node terminated.") 56 | -------------------------------------------------------------------------------- /turbot_code/scripts/goButton.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Monitor the kobuki's button status 3 | 4 | import roslib 5 | import rospy 6 | from kobuki_msgs.msg import ButtonEvent 7 | import actionlib 8 | from actionlib_msgs.msg import * 9 | from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist 10 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 11 | from math import pow, sqrt 12 | 13 | class kobuki_button(): 14 | 15 | def __init__(self): 16 | rospy.init_node("kobuki_button") 17 | 18 | # Goal state return values 19 | self.goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 20 | 'SUCCEEDED', 'ABORTED', 'REJECTED', 21 | 'PREEMPTING', 'RECALLING', 'RECALLED', 22 | 'LOST'] 23 | 24 | #monitor kobuki's button events 25 | rospy.Subscriber("/mobile_base/events/button",ButtonEvent,self.ButtonEventCallback) 26 | 27 | # Publisher to manually control the robot (e.g. to stop it, queue_size=5) 28 | self.cmd_vel_pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=5) 29 | 30 | # Subscribe to the move_base action server 31 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 32 | 33 | rospy.loginfo("Waiting for move_base action server...") 34 | 35 | # Wait 60 seconds for the action server to become available 36 | self.move_base.wait_for_server(rospy.Duration(60)) 37 | 38 | rospy.loginfo("Connected to move base server") 39 | 40 | self.locations = dict() 41 | 42 | self.locations['A'] = Pose(Point(-1.79, -1.23, 0.000), Quaternion(0.000, 0.000, 0.223, 0.975)) 43 | self.locations['B'] = Pose(Point(-1.31, -0.131, 0.000), Quaternion(0.000, 0.000, -0.670, 0.743)) 44 | self.locations['C'] = Pose(Point(-2.62, -0.286, 0.000), Quaternion(0.000, 0.000, 0.733, 0.680)) 45 | 46 | self.goal = MoveBaseGoal() 47 | 48 | #rospy.spin() tells the program to not exit until you press ctrl + c. If this wasn't there... it'd subscribe and then immediatly exit (therefore stop "listening" to the thread). 49 | rospy.spin(); 50 | 51 | def go(self, location): 52 | # Set up the next goal location 53 | 54 | self.goal.target_pose.pose = self.locations[location] 55 | self.goal.target_pose.header.frame_id = 'map' 56 | self.goal.target_pose.header.stamp = rospy.Time.now() 57 | 58 | # Let the user know where the robot is going next 59 | rospy.loginfo("Going to: " + str(location)) 60 | 61 | # Start the robot toward the next location 62 | self.move_base.send_goal(self.goal) 63 | 64 | # Allow 5 minutes to get there 65 | finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 66 | 67 | # Check for success or failure 68 | if not finished_within_time: 69 | self.move_base.cancel_goal() 70 | rospy.loginfo("Timed out achieving goal") 71 | else: 72 | state = self.move_base.get_state() 73 | if state == GoalStatus.SUCCEEDED: 74 | rospy.loginfo("Goal succeeded!") 75 | rospy.loginfo("State:" + str(state)) 76 | else: 77 | rospy.loginfo("Goal failed with error code: " + str(self.goal_states[state])) 78 | 79 | 80 | def ButtonEventCallback(self,data): 81 | if ( data.state == ButtonEvent.RELEASED ) : 82 | ButtonState = "released" 83 | else: 84 | ButtonState = "pressed" 85 | if ( data.button == ButtonEvent.Button0 ) : 86 | button = "A" 87 | elif ( data.button == ButtonEvent.Button1 ) : 88 | button = "B" 89 | else: 90 | button = "C" 91 | rospy.loginfo("Button %s was %s."%(button, ButtonState)) 92 | 93 | self.go(button) 94 | 95 | 96 | if __name__ == '__main__': 97 | try: 98 | kobuki_button() 99 | except rospy.ROSInterruptException: 100 | rospy.loginfo("exception") 101 | -------------------------------------------------------------------------------- /turbot_code/scripts/goForward.py: -------------------------------------------------------------------------------- 1 | # A very basic TurtleBot script that moves TurtleBot forward indefinitely. Press CTRL + C to stop. To run: 2 | # On TurtleBot: 3 | # roslaunch turtlebot_bringup minimal.launch 4 | # On work station: 5 | # python goForward.py 6 | 7 | import rospy 8 | from geometry_msgs.msg import Twist 9 | 10 | class GoForward(): 11 | def __init__(self): 12 | # initiliaze 13 | rospy.init_node('GoForward', anonymous=False) 14 | 15 | # tell user how to stop TurtleBot 16 | rospy.loginfo("To stop TurtleBot CTRL + C") 17 | 18 | # What function to call when you ctrl + c 19 | rospy.on_shutdown(self.shutdown) 20 | 21 | # Create a publisher which can "talk" to TurtleBot and tell it to move 22 | # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 23 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 24 | 25 | #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ 26 | r = rospy.Rate(10); 27 | 28 | # Twist is a datatype for velocity 29 | move_cmd = Twist() 30 | # let's go forward at 0.2 m/s 31 | move_cmd.linear.x = 0.2 32 | # let's turn at 0 radians/s 33 | move_cmd.angular.z = 0 34 | 35 | # as long as you haven't ctrl + c keeping doing... 36 | while not rospy.is_shutdown(): 37 | # publish the velocity 38 | self.cmd_vel.publish(move_cmd) 39 | # wait for 0.1 seconds (10 HZ) and publish again 40 | r.sleep() 41 | 42 | 43 | def shutdown(self): 44 | # stop turtlebot 45 | rospy.loginfo("Stop TurtleBot") 46 | # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot 47 | self.cmd_vel.publish(Twist()) 48 | # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script 49 | rospy.sleep(1) 50 | 51 | if __name__ == '__main__': 52 | try: 53 | GoForward() 54 | except: 55 | rospy.loginfo("GoForward node terminated.") 56 | -------------------------------------------------------------------------------- /turbot_code/scripts/goForwardWithAvoidObstacle.py: -------------------------------------------------------------------------------- 1 | #Code is inspired by http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals (written in C++). 2 | #TurtleBot must have minimal.launch & amcl_demo.launch running prior to starting this script. 3 | #goForwardWithAvoidObstacle.py 4 | 5 | import rospy 6 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 7 | import actionlib 8 | from actionlib_msgs.msg import * 9 | 10 | class GoForwardAvoid(): 11 | def __init__(self): 12 | rospy.init_node('nav_test', anonymous=False) 13 | 14 | #what to do if shut down (e.g. ctrl + C or failure) 15 | rospy.on_shutdown(self.shutdown) 16 | 17 | 18 | #tell the action client that we want to spin a thread by default 19 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 20 | rospy.loginfo("wait for the action server to come up") 21 | #allow up to 5 seconds for the action server to come up 22 | self.move_base.wait_for_server(rospy.Duration(5)) 23 | 24 | #we'll send a goal to the robot to move 3 meters forward 25 | goal = MoveBaseGoal() 26 | goal.target_pose.header.frame_id = 'base_link' 27 | goal.target_pose.header.stamp = rospy.Time.now() 28 | goal.target_pose.pose.position.x = 3.0 #3 meters 29 | goal.target_pose.pose.orientation.w = 1.0 #go forward 30 | 31 | #start moving 32 | self.move_base.send_goal(goal) 33 | 34 | #allow TurtleBot up to 60 seconds to complete task 35 | success = self.move_base.wait_for_result(rospy.Duration(60)) 36 | 37 | 38 | if not success: 39 | self.move_base.cancel_goal() 40 | rospy.loginfo("The base failed to move forward 3 meters for some reason") 41 | else: 42 | # We made it! 43 | state = self.move_base.get_state() 44 | if state == GoalStatus.SUCCEEDED: 45 | rospy.loginfo("Hooray, the base moved 3 meters forward") 46 | 47 | 48 | 49 | def shutdown(self): 50 | rospy.loginfo("Stop") 51 | 52 | 53 | if __name__ == '__main__': 54 | try: 55 | GoForwardAvoid() 56 | except rospy.ROSInterruptException: 57 | rospy.loginfo("Exception thrown") 58 | -------------------------------------------------------------------------------- /turbot_code/scripts/goLeft.py: -------------------------------------------------------------------------------- 1 | # A very basic TurtleBot script that moves TurtleBot forward indefinitely. Press CTRL + C to stop. To run: 2 | # On TurtleBot: 3 | # roslaunch turtlebot_bringup minimal.launch 4 | # On work station: 5 | # python goLeft.py 6 | 7 | import rospy 8 | from geometry_msgs.msg import Twist 9 | from math import randians 10 | 11 | class GoForward(): 12 | def __init__(self): 13 | # initiliaze 14 | rospy.init_node('GoForward', anonymous=False) 15 | 16 | # tell user how to stop TurtleBot 17 | rospy.loginfo("To stop TurtleBot CTRL + C") 18 | 19 | # What function to call when you ctrl + c 20 | rospy.on_shutdown(self.shutdown) 21 | 22 | # Create a publisher which can "talk" to TurtleBot and tell it to move 23 | # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 24 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 25 | 26 | #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ 27 | r = rospy.Rate(10); 28 | 29 | # Twist is a datatype for velocity 30 | move_cmd = Twist() 31 | # let's go forward at 0.2 m/s 32 | move_cmd.linear.x = 0.1 33 | # let's turn at 0 radians/s 34 | move_cmd.angular.z = -radians(45); #45 deg/s in radians/s 35 | 36 | # as long as you haven't ctrl + c keeping doing... 37 | while not rospy.is_shutdown(): 38 | # publish the velocity 39 | self.cmd_vel.publish(move_cmd) 40 | # wait for 0.1 seconds (10 HZ) and publish again 41 | r.sleep() 42 | 43 | 44 | def shutdown(self): 45 | # stop turtlebot 46 | rospy.loginfo("Stop TurtleBot") 47 | # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot 48 | self.cmd_vel.publish(Twist()) 49 | # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script 50 | rospy.sleep(1) 51 | 52 | if __name__ == '__main__': 53 | try: 54 | GoForward() 55 | except: 56 | rospy.loginfo("GoForward node terminated.") 57 | -------------------------------------------------------------------------------- /turbot_code/scripts/goRight.py: -------------------------------------------------------------------------------- 1 | # A very basic TurtleBot script that moves TurtleBot forward indefinitely. Press CTRL + C to stop. To run: 2 | # On TurtleBot: 3 | # roslaunch turtlebot_bringup minimal.launch 4 | # On work station: 5 | # python goRight.py 6 | 7 | import rospy 8 | from geometry_msgs.msg import Twist 9 | from math import radians 10 | 11 | class GoForward(): 12 | def __init__(self): 13 | # initiliaze 14 | rospy.init_node('GoForward', anonymous=False) 15 | 16 | # tell user how to stop TurtleBot 17 | rospy.loginfo("To stop TurtleBot CTRL + C") 18 | 19 | # What function to call when you ctrl + c 20 | rospy.on_shutdown(self.shutdown) 21 | 22 | # Create a publisher which can "talk" to TurtleBot and tell it to move 23 | # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 24 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 25 | 26 | #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ 27 | r = rospy.Rate(10); 28 | 29 | # Twist is a datatype for velocity 30 | move_cmd = Twist() 31 | # let's go forward at 0.2 m/s 32 | move_cmd.linear.x = 0.1 33 | # let's turn at 0 radians/s 34 | move_cmd.angular.z = radians(45); #45 deg/s in radians/s 35 | 36 | # as long as you haven't ctrl + c keeping doing... 37 | while not rospy.is_shutdown(): 38 | # publish the velocity 39 | self.cmd_vel.publish(move_cmd) 40 | # wait for 0.1 seconds (10 HZ) and publish again 41 | r.sleep() 42 | 43 | 44 | def shutdown(self): 45 | # stop turtlebot 46 | rospy.loginfo("Stop TurtleBot") 47 | # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot 48 | self.cmd_vel.publish(Twist()) 49 | # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script 50 | rospy.sleep(1) 51 | 52 | if __name__ == '__main__': 53 | try: 54 | GoForward() 55 | except: 56 | rospy.loginfo("GoForward node terminated.") 57 | -------------------------------------------------------------------------------- /turbot_code/scripts/goRound.py: -------------------------------------------------------------------------------- 1 | # A very basic TurtleBot script that moves TurtleBot forward indefinitely. Press CTRL + C to stop. To run: 2 | # On TurtleBot: 3 | # roslaunch turtlebot_bringup minimal.launch 4 | # On work station: 5 | # python goRound.py 6 | 7 | import rospy 8 | from geometry_msgs.msg import Twist 9 | from math import radians 10 | 11 | class GoForward(): 12 | def __init__(self): 13 | # initiliaze 14 | rospy.init_node('GoForward', anonymous=False) 15 | 16 | # tell user how to stop TurtleBot 17 | rospy.loginfo("To stop TurtleBot CTRL + C") 18 | 19 | # What function to call when you ctrl + c 20 | rospy.on_shutdown(self.shutdown) 21 | 22 | # Create a publisher which can "talk" to TurtleBot and tell it to move 23 | # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 24 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 25 | 26 | #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ 27 | r = rospy.Rate(10); 28 | 29 | # Twist is a datatype for velocity 30 | move_cmd = Twist() 31 | # let's go forward at 0.2 m/s 32 | move_cmd.linear.x = 0 33 | # let's turn at 45 deg/s in radians/s 34 | move_cmd.angular.z = -radians(45) 35 | 36 | # as long as you haven't ctrl + c keeping doing... 37 | while not rospy.is_shutdown(): 38 | # publish the velocity 39 | self.cmd_vel.publish(move_cmd) 40 | # wait for 0.1 seconds (10 HZ) and publish again 41 | r.sleep() 42 | 43 | 44 | def shutdown(self): 45 | # stop turtlebot 46 | rospy.loginfo("Stop TurtleBot") 47 | # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot 48 | self.cmd_vel.publish(Twist()) 49 | # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script 50 | rospy.sleep(1) 51 | 52 | if __name__ == '__main__': 53 | try: 54 | GoForward() 55 | except: 56 | rospy.loginfo("GoForward node terminated.") 57 | -------------------------------------------------------------------------------- /turbot_code/scripts/goToSpecificPointOnMap.py: -------------------------------------------------------------------------------- 1 | # TurtleBot must have minimal.launch & amcl_demo.launch 2 | # running prior to starting this script 3 | # For simulation: launch gazebo world & amcl_demo prior to run this script 4 | # goToSpecificPointOnMap.py 5 | 6 | import rospy 7 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 8 | import actionlib 9 | from actionlib_msgs.msg import * 10 | from geometry_msgs.msg import Pose, Point, Quaternion 11 | 12 | class GoToPose(): 13 | def __init__(self): 14 | 15 | self.goal_sent = False 16 | 17 | # What to do if shut down (e.g. Ctrl-C or failure) 18 | rospy.on_shutdown(self.shutdown) 19 | 20 | # Tell the action client that we want to spin a thread by default 21 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 22 | rospy.loginfo("Wait for the action server to come up") 23 | 24 | # Allow up to 5 seconds for the action server to come up 25 | self.move_base.wait_for_server(rospy.Duration(5)) 26 | 27 | def goto(self, pos, quat): 28 | 29 | # Send a goal 30 | self.goal_sent = True 31 | goal = MoveBaseGoal() 32 | goal.target_pose.header.frame_id = 'map' 33 | goal.target_pose.header.stamp = rospy.Time.now() 34 | goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000), 35 | Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4'])) 36 | 37 | # Start moving 38 | self.move_base.send_goal(goal) 39 | 40 | # Allow TurtleBot up to 60 seconds to complete task 41 | success = self.move_base.wait_for_result(rospy.Duration(60)) 42 | 43 | state = self.move_base.get_state() 44 | result = False 45 | 46 | if success and state == GoalStatus.SUCCEEDED: 47 | # We made it! 48 | result = True 49 | else: 50 | self.move_base.cancel_goal() 51 | 52 | self.goal_sent = False 53 | return result 54 | 55 | def shutdown(self): 56 | if self.goal_sent: 57 | self.move_base.cancel_goal() 58 | rospy.loginfo("Stop") 59 | rospy.sleep(1) 60 | 61 | if __name__ == '__main__': 62 | try: 63 | rospy.init_node('nav_test', anonymous=False) 64 | navigator = GoToPose() 65 | 66 | # Customize the following values so they are appropriate for your location 67 | position = {'x': 1.22, 'y' : 2.56} 68 | quaternion = {'r1' : 0.000, 'r2' : 0.000, 'r3' : 0.000, 'r4' : 1.000} 69 | 70 | rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y']) 71 | success = navigator.goto(position, quaternion) 72 | 73 | if success: 74 | rospy.loginfo("Hooray, reached the desired pose") 75 | else: 76 | rospy.loginfo("The base failed to reach the desired pose") 77 | 78 | # Sleep to give the last log messages time to be sent 79 | rospy.sleep(1) 80 | 81 | except rospy.ROSInterruptException: 82 | rospy.loginfo("Ctrl-C caught. Quitting") 83 | -------------------------------------------------------------------------------- /turbot_code/scripts/go_to_specific_point_on_map.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # TurtleBot must have minimal.launch & amcl_demo.launch 4 | # running prior to starting this script 5 | # For simulation: launch gazebo world & amcl_demo prior to run this script 6 | 7 | import rospy 8 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 9 | import actionlib 10 | from actionlib_msgs.msg import * 11 | from geometry_msgs.msg import Pose, Point, Quaternion 12 | import sys 13 | 14 | class GoToPose(): 15 | def __init__(self): 16 | 17 | self.goal_sent = False 18 | 19 | # What to do if shut down (e.g. Ctrl-C or failure) 20 | rospy.on_shutdown(self.shutdown) 21 | 22 | # Tell the action client that we want to spin a thread by default 23 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 24 | rospy.loginfo("Wait for the action server to come up") 25 | 26 | # Allow up to 5 seconds for the action server to come up 27 | self.move_base.wait_for_server(rospy.Duration(5)) 28 | 29 | def goto(self, pos, quat): 30 | 31 | # Send a goal 32 | self.goal_sent = True 33 | goal = MoveBaseGoal() 34 | goal.target_pose.header.frame_id = 'map' 35 | goal.target_pose.header.stamp = rospy.Time.now() 36 | goal.target_pose.pose = Pose(Point(pos['x'], pos['y'], 0.000), 37 | Quaternion(quat['r1'], quat['r2'], quat['r3'], quat['r4'])) 38 | 39 | # Start moving 40 | self.move_base.send_goal(goal) 41 | 42 | # Allow TurtleBot up to 60 seconds to complete task 43 | success = self.move_base.wait_for_result(rospy.Duration(60)) 44 | 45 | state = self.move_base.get_state() 46 | result = False 47 | 48 | if success and state == GoalStatus.SUCCEEDED: 49 | # We made it! 50 | result = True 51 | else: 52 | self.move_base.cancel_goal() 53 | 54 | self.goal_sent = False 55 | return result 56 | 57 | def shutdown(self): 58 | if self.goal_sent: 59 | self.move_base.cancel_goal() 60 | rospy.loginfo("Stop") 61 | rospy.sleep(1) 62 | 63 | if __name__ == '__main__': 64 | try: 65 | 66 | #if len(sys.argv)!=3: 67 | # sys.exit("Format is : rosrun turbot_code go_to_specific_point_on_map.py x y ") 68 | 69 | rospy.init_node('nav_test', anonymous=False) 70 | navigator = GoToPose() 71 | #px = sys.argv[1] 72 | #py = sys.argv[2] 73 | 74 | # Customize the following values so they are appropriate for your location 75 | position = {'x': 0.134, 'y' : 1.27} 76 | quaternion = {'r1' : 0.000, 'r2' : 0.000, 'r3' : 0.000, 'r4' : 1.000} 77 | 78 | rospy.loginfo("Go to (%s, %s) pose", position['x'], position['y']) 79 | success = navigator.goto(position, quaternion) 80 | 81 | if success: 82 | rospy.loginfo("Hooray, reached the desired pose") 83 | else: 84 | rospy.loginfo("The base failed to reach the desired pose") 85 | 86 | # Sleep to give the last log messages time to be sent 87 | rospy.sleep(1) 88 | 89 | except rospy.ROSInterruptException: 90 | rospy.loginfo("Ctrl-C caught. Quitting") 91 | -------------------------------------------------------------------------------- /turbot_code/scripts/kobukiBattery.py: -------------------------------------------------------------------------------- 1 | # Monitor the kobuki's battery level 2 | 3 | import roslib 4 | import rospy 5 | from kobuki_msgs.msg import SensorState 6 | 7 | class kobuki_battery(): 8 | 9 | kobuki_base_max_charge = 160 10 | 11 | def __init__(self): 12 | rospy.init_node("kobuki_battery") 13 | 14 | #monitor Kobuki's power and charging status. If an event occurs (low battery, charging, not charging etc) call function SensorPowerEventCallback 15 | rospy.Subscriber("/mobile_base/sensors/core",SensorState,self.SensorPowerEventCallback) 16 | 17 | #rospy.spin() tells the program to not exit until you press ctrl + c. If this wasn't there... it'd subscribe and then immediatly exit (therefore stop "listening" to the thread). 18 | rospy.spin(); 19 | 20 | 21 | def SensorPowerEventCallback(self,data): 22 | rospy.loginfo("Kobuki's battery is now: " + str(round(float(data.battery) / float(self.kobuki_base_max_charge) * 100)) + "%") 23 | if(int(data.charger) == 0) : 24 | rospy.loginfo("Not charging at docking station") 25 | else: 26 | rospy.loginfo("Charging at docking station") 27 | 28 | 29 | if __name__ == '__main__': 30 | try: 31 | kobuki_battery() 32 | except rospy.ROSInterruptException: 33 | rospy.loginfo("exception") 34 | -------------------------------------------------------------------------------- /turbot_code/scripts/moveBaseSquare.py: -------------------------------------------------------------------------------- 1 | # moveBaseSquare.py 2 | import rospy 3 | import actionlib 4 | from actionlib_msgs.msg import * 5 | from geometry_msgs.msg import Pose, Point, Quaternion, Twist 6 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 7 | from tf.transformations import quaternion_from_euler 8 | from visualization_msgs.msg import Marker 9 | from math import radians, pi 10 | 11 | class MoveBaseSquare(): 12 | def __init__(self): 13 | rospy.init_node('nav_test', anonymous=False) 14 | 15 | rospy.on_shutdown(self.shutdown) 16 | 17 | # How big is the square we want the robot to navigate? 18 | square_size = rospy.get_param("~square_size", 1.0) # meters 19 | 20 | # Create a list to hold the target quaternions (orientations) 21 | quaternions = list() 22 | 23 | # First define the corner orientations as Euler angles 24 | euler_angles = (pi/2, pi, 3*pi/2, 0) 25 | 26 | # Then convert the angles to quaternions 27 | for angle in euler_angles: 28 | q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz') 29 | q = Quaternion(*q_angle) 30 | quaternions.append(q) 31 | 32 | # Create a list to hold the waypoint poses 33 | waypoints = list() 34 | 35 | # Append each of the four waypoints to the list. Each waypoint 36 | # is a pose consisting of a position and orientation in the map frame. 37 | waypoints.append(Pose(Point(square_size, 0.0, 0.0), quaternions[0])) 38 | waypoints.append(Pose(Point(square_size, square_size, 0.0), quaternions[1])) 39 | waypoints.append(Pose(Point(0.0, square_size, 0.0), quaternions[2])) 40 | waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3])) 41 | 42 | # Initialize the visualization markers for RViz 43 | self.init_markers() 44 | 45 | # Set a visualization marker at each waypoint 46 | for waypoint in waypoints: 47 | p = Point() 48 | p = waypoint.position 49 | self.markers.points.append(p) 50 | 51 | # Publisher to manually control the robot (e.g. to stop it, queue_size=5) 52 | self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5) 53 | 54 | # Subscribe to the move_base action server 55 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 56 | 57 | rospy.loginfo("Waiting for move_base action server...") 58 | 59 | # Wait 60 seconds for the action server to become available 60 | self.move_base.wait_for_server(rospy.Duration(60)) 61 | 62 | rospy.loginfo("Connected to move base server") 63 | rospy.loginfo("Starting navigation test") 64 | 65 | # Initialize a counter to track waypoints 66 | i = 0 67 | 68 | # Cycle through the four waypoints 69 | while i < 4 and not rospy.is_shutdown(): 70 | # Update the marker display 71 | self.marker_pub.publish(self.markers) 72 | 73 | # Intialize the waypoint goal 74 | goal = MoveBaseGoal() 75 | 76 | # Use the map frame to define goal poses 77 | goal.target_pose.header.frame_id = 'map' 78 | 79 | # Set the time stamp to "now" 80 | goal.target_pose.header.stamp = rospy.Time.now() 81 | 82 | # Set the goal pose to the i-th waypoint 83 | goal.target_pose.pose = waypoints[i] 84 | 85 | # Start the robot moving toward the goal 86 | self.move(goal) 87 | 88 | i += 1 89 | 90 | def move(self, goal): 91 | # Send the goal pose to the MoveBaseAction server 92 | self.move_base.send_goal(goal) 93 | 94 | # Allow 1 minute to get there 95 | finished_within_time = self.move_base.wait_for_result(rospy.Duration(60)) 96 | 97 | # If we don't get there in time, abort the goal 98 | if not finished_within_time: 99 | self.move_base.cancel_goal() 100 | rospy.loginfo("Timed out achieving goal") 101 | else: 102 | # We made it! 103 | state = self.move_base.get_state() 104 | if state == GoalStatus.SUCCEEDED: 105 | rospy.loginfo("Goal succeeded!") 106 | 107 | def init_markers(self): 108 | # Set up our waypoint markers 109 | marker_scale = 0.2 110 | marker_lifetime = 0 # 0 is forever 111 | marker_ns = 'waypoints' 112 | marker_id = 0 113 | marker_color = {'r': 1.0, 'g': 0.7, 'b': 1.0, 'a': 1.0} 114 | 115 | # Define a marker publisher. 116 | self.marker_pub = rospy.Publisher('waypoint_markers', Marker, queue_size=5) 117 | 118 | # Initialize the marker points list. 119 | self.markers = Marker() 120 | self.markers.ns = marker_ns 121 | self.markers.id = marker_id 122 | self.markers.type = Marker.CUBE_LIST 123 | self.markers.action = Marker.ADD 124 | self.markers.lifetime = rospy.Duration(marker_lifetime) 125 | self.markers.scale.x = marker_scale 126 | self.markers.scale.y = marker_scale 127 | self.markers.color.r = marker_color['r'] 128 | self.markers.color.g = marker_color['g'] 129 | self.markers.color.b = marker_color['b'] 130 | self.markers.color.a = marker_color['a'] 131 | 132 | self.markers.header.frame_id = 'odom' 133 | self.markers.header.stamp = rospy.Time.now() 134 | self.markers.points = list() 135 | 136 | def shutdown(self): 137 | rospy.loginfo("Stopping the robot...") 138 | # Cancel any active goals 139 | self.move_base.cancel_goal() 140 | rospy.sleep(2) 141 | # Stop the robot 142 | self.cmd_vel_pub.publish(Twist()) 143 | rospy.sleep(1) 144 | 145 | if __name__ == '__main__': 146 | try: 147 | MoveBaseSquare() 148 | except rospy.ROSInterruptException: 149 | rospy.loginfo("Navigation test finished.") 150 | -------------------------------------------------------------------------------- /turbot_code/scripts/navMultiPoints2.py: -------------------------------------------------------------------------------- 1 | # navMultiPoints.py 2 | import rospy 3 | import actionlib 4 | from actionlib_msgs.msg import * 5 | from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist 6 | from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal 7 | from random import sample 8 | from math import pow, sqrt 9 | 10 | class NavTest(): 11 | def __init__(self): 12 | rospy.init_node('nav_test', anonymous=True) 13 | 14 | rospy.on_shutdown(self.shutdown) 15 | 16 | # How long in seconds should the robot pause at each location? 17 | self.rest_time = rospy.get_param("~rest_time", 1) 18 | 19 | # Are we running in the fake simulator? 20 | self.fake_test = rospy.get_param("~fake_test", False) 21 | 22 | # Goal state return values 23 | goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED', 24 | 'SUCCEEDED', 'ABORTED', 'REJECTED', 25 | 'PREEMPTING', 'RECALLING', 'RECALLED', 26 | 'LOST'] 27 | 28 | # Set up the goal locations. Poses are defined in the map frame. 29 | # An easy way to find the pose coordinates is to point-and-click 30 | # Nav Goals in RViz when running in the simulator. 31 | # Pose coordinates are then displayed in the terminal 32 | # that was used to launch RViz. 33 | locations = dict() 34 | 35 | locations['A']=Pose(Point(7.41, 1.98, 0.000), Quaternion(0.000, 0.000, 0.000, 0.975)) 36 | locations['B']=Pose(Point(3.03, -0.17, 0.000), Quaternion(0.000, 0.000, 0.000, 0.743)) 37 | 38 | # Publisher to manually control the robot (e.g. to stop it, queue_size=5) 39 | self.cmd_vel_pub = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=5) 40 | 41 | # Subscribe to the move_base action server 42 | self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) 43 | 44 | rospy.loginfo("Waiting for move_base action server...") 45 | 46 | # Wait 60 seconds for the action server to become available 47 | self.move_base.wait_for_server(rospy.Duration(60)) 48 | 49 | rospy.loginfo("Connected to move base server") 50 | 51 | # A variable to hold the initial pose of the robot to be set by 52 | # the user in RViz 53 | initial_pose = PoseWithCovarianceStamped() 54 | 55 | # Variables to keep track of success rate, running time, 56 | # and distance traveled 57 | n_locations = len(locations) 58 | n_goals = 0 59 | n_successes = 0 60 | i = n_locations 61 | distance_traveled = 0 62 | start_time = rospy.Time.now() 63 | running_time = 0 64 | location = "" 65 | last_location = "" 66 | 67 | # Get the initial pose from the user 68 | rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...") 69 | rospy.wait_for_message('initialpose', PoseWithCovarianceStamped) 70 | self.last_location = Pose() 71 | rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose) 72 | sequence = {0:'A', 1:'B'} 73 | # Make sure we have the initial pose 74 | while initial_pose.header.stamp == "": 75 | rospy.sleep(1) 76 | 77 | rospy.loginfo("Starting navigation test") 78 | 79 | # Begin the main loop and run through a sequence of locations 80 | while not rospy.is_shutdown(): 81 | # If we've gone through the current sequence, 82 | # start with a new random sequence 83 | if i == n_locations: 84 | i = 0 85 | #sequence = sample(locations, n_locations) 86 | # Skip over first location if it is the same as 87 | # the last location 88 | if sequence[0] == last_location: 89 | i = 1 90 | 91 | # Get the next location in the current sequence 92 | location = sequence[i] 93 | 94 | # Keep track of the distance traveled. 95 | # Use updated initial pose if available. 96 | if initial_pose.header.stamp == "": 97 | distance = sqrt(pow(locations[location].position.x - 98 | locations[last_location].position.x, 2) + 99 | pow(locations[location].position.y - 100 | locations[last_location].position.y, 2)) 101 | else: 102 | rospy.loginfo("Updating current pose.") 103 | distance = sqrt(pow(locations[location].position.x - 104 | initial_pose.pose.pose.position.x, 2) + 105 | pow(locations[location].position.y - 106 | initial_pose.pose.pose.position.y, 2)) 107 | initial_pose.header.stamp = "" 108 | 109 | # Store the last location for distance calculations 110 | last_location = location 111 | 112 | # Increment the counters 113 | i += 1 114 | n_goals += 1 115 | 116 | # Set up the next goal location 117 | self.goal = MoveBaseGoal() 118 | self.goal.target_pose.pose = locations[location] 119 | self.goal.target_pose.header.frame_id = 'map' 120 | self.goal.target_pose.header.stamp = rospy.Time.now() 121 | 122 | # Let the user know where the robot is going next 123 | rospy.loginfo("Going to: " + str(location)) 124 | 125 | # Start the robot toward the next location 126 | self.move_base.send_goal(self.goal) 127 | 128 | # Allow 5 minutes to get there 129 | finished_within_time = self.move_base.wait_for_result(rospy.Duration(300)) 130 | 131 | # Check for success or failure 132 | if not finished_within_time: 133 | self.move_base.cancel_goal() 134 | rospy.loginfo("Timed out achieving goal") 135 | else: 136 | state = self.move_base.get_state() 137 | if state == GoalStatus.SUCCEEDED: 138 | rospy.loginfo("Goal succeeded!") 139 | n_successes += 1 140 | distance_traveled += distance 141 | rospy.loginfo("State:" + str(state)) 142 | else: 143 | rospy.loginfo("Goal failed with error code: " + str(goal_states[state])) 144 | 145 | # How long have we been running? 146 | running_time = rospy.Time.now() - start_time 147 | running_time = running_time.secs / 60.0 148 | 149 | # Print a summary success/failure, distance traveled and time elapsed 150 | rospy.loginfo("Success so far: " + str(n_successes) + "/" + 151 | str(n_goals) + " = " + 152 | str(100 * n_successes/n_goals) + "%") 153 | rospy.loginfo("Running time: " + str(trunc(running_time, 1)) + 154 | " min Distance: " + str(trunc(distance_traveled, 1)) + " m") 155 | rospy.sleep(self.rest_time) 156 | 157 | def update_initial_pose(self, initial_pose): 158 | self.initial_pose = initial_pose 159 | 160 | def shutdown(self): 161 | rospy.loginfo("Stopping the robot...") 162 | self.move_base.cancel_goal() 163 | rospy.sleep(2) 164 | self.cmd_vel_pub.publish(Twist()) 165 | rospy.sleep(1) 166 | 167 | def trunc(f, n): 168 | # Truncates/pads a float f to n decimal places without rounding 169 | slen = len('%.*f' % (n, f)) 170 | return float(str(f)[:slen]) 171 | 172 | if __name__ == '__main__': 173 | try: 174 | NavTest() 175 | rospy.spin() 176 | except rospy.ROSInterruptException: 177 | rospy.loginfo("AMCL navigation test finished.") 178 | -------------------------------------------------------------------------------- /turbot_code/scripts/netbookBattery.py: -------------------------------------------------------------------------------- 1 | # Monitor the netbook's battery level 2 | 3 | import roslib 4 | import rospy 5 | from smart_battery_msgs.msg import SmartBatteryStatus #for netbook battery 6 | 7 | class netbook_battery(): 8 | 9 | def __init__(self): 10 | rospy.init_node("netbook_battery") 11 | 12 | #monitor netbook's battery status. Everytime anything changes call the call back function self.NetbookPowerEventCallback and pass the data regarding the current battery status 13 | rospy.Subscriber("/laptop_charge/",SmartBatteryStatus,self.NetbookPowerEventCallback) 14 | 15 | #rospy.spin() tells the program to not exit until you press ctrl + c. If this wasn't there... it'd subscribe to /laptop_charge/ then immediatly exit (therefore stop "listening" to the thread). 16 | rospy.spin(); 17 | 18 | 19 | def NetbookPowerEventCallback(self,data): 20 | print("Percent: " + str(data.percentage)) 21 | print("Charge: " + str(data.charge)) 22 | if(int(data.charge_state) == 1): 23 | print("Currently charging") 24 | else: 25 | print("Not charging") 26 | print("-----") 27 | #Tip: try print(data) for a complete list of information available in the /laptop_charge/ thread 28 | 29 | if __name__ == '__main__': 30 | try: 31 | netbook_battery() 32 | except rospy.ROSInterruptException: 33 | rospy.loginfo("exception") 34 | -------------------------------------------------------------------------------- /turbot_code/scripts/odomOutAndBack.py: -------------------------------------------------------------------------------- 1 | # A basic demo of using the /odom topic to move a robot a given distance 2 | # or rotate through a given angle. 3 | # odomOutAndBack.py 4 | 5 | import rospy 6 | from geometry_msgs.msg import Twist, Point, Quaternion 7 | import tf 8 | from rbx1_nav.transform_utils import quat_to_angle, normalize_angle 9 | from math import radians, copysign, sqrt, pow, pi 10 | 11 | class OutAndBack(): 12 | def __init__(self): 13 | # Give the node a name 14 | rospy.init_node('out_and_back', anonymous=False) 15 | 16 | # Set rospy to execute a shutdown function when exiting 17 | rospy.on_shutdown(self.shutdown) 18 | 19 | # Publisher to control the robot's speed 20 | self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=5) 21 | 22 | # How fast will we update the robot's movement? 23 | rate = 20 24 | 25 | # Set the equivalent ROS rate variable 26 | r = rospy.Rate(rate) 27 | 28 | # Set the forward linear speed to 0.15 meters per second 29 | linear_speed = 0.15 30 | 31 | # Set the travel distance in meters 32 | goal_distance = 1.0 33 | 34 | # Set the rotation speed in radians per second 35 | angular_speed = 0.5 36 | 37 | # Set the angular tolerance in degrees converted to radians 38 | angular_tolerance = radians(1.0) 39 | 40 | # Set the rotation angle to Pi radians (180 degrees) 41 | goal_angle = pi 42 | 43 | # Initialize the tf listener 44 | self.tf_listener = tf.TransformListener() 45 | 46 | # Give tf some time to fill its buffer 47 | rospy.sleep(2) 48 | 49 | # Set the odom frame 50 | self.odom_frame = '/odom' 51 | 52 | # Find out if the robot uses /base_link or /base_footprint 53 | try: 54 | self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) 55 | self.base_frame = '/base_footprint' 56 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 57 | try: 58 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) 59 | self.base_frame = '/base_link' 60 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 61 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 62 | rospy.signal_shutdown("tf Exception") 63 | 64 | # Initialize the position variable as a Point type 65 | position = Point() 66 | 67 | # Loop once for each leg of the trip 68 | for i in range(2): 69 | # Initialize the movement command 70 | move_cmd = Twist() 71 | 72 | # Set the movement command to forward motion 73 | move_cmd.linear.x = linear_speed 74 | 75 | # Get the starting position values 76 | (position, rotation) = self.get_odom() 77 | 78 | x_start = position.x 79 | y_start = position.y 80 | 81 | # Keep track of the distance traveled 82 | distance = 0 83 | 84 | # Enter the loop to move along a side 85 | while distance < goal_distance and not rospy.is_shutdown(): 86 | # Publish the Twist message and sleep 1 cycle 87 | self.cmd_vel.publish(move_cmd) 88 | 89 | r.sleep() 90 | 91 | # Get the current position 92 | (position, rotation) = self.get_odom() 93 | 94 | # Compute the Euclidean distance from the start 95 | distance = sqrt(pow((position.x - x_start), 2) + 96 | pow((position.y - y_start), 2)) 97 | 98 | # Stop the robot before the rotation 99 | move_cmd = Twist() 100 | self.cmd_vel.publish(move_cmd) 101 | rospy.sleep(1) 102 | 103 | # Set the movement command to a rotation 104 | move_cmd.angular.z = angular_speed 105 | 106 | # Track the last angle measured 107 | last_angle = rotation 108 | 109 | # Track how far we have turned 110 | turn_angle = 0 111 | 112 | while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): 113 | # Publish the Twist message and sleep 1 cycle 114 | self.cmd_vel.publish(move_cmd) 115 | r.sleep() 116 | 117 | # Get the current rotation 118 | (position, rotation) = self.get_odom() 119 | 120 | # Compute the amount of rotation since the last loop 121 | delta_angle = normalize_angle(rotation - last_angle) 122 | 123 | # Add to the running total 124 | turn_angle += delta_angle 125 | last_angle = rotation 126 | 127 | # Stop the robot before the next leg 128 | move_cmd = Twist() 129 | self.cmd_vel.publish(move_cmd) 130 | rospy.sleep(1) 131 | 132 | # Stop the robot for good 133 | self.cmd_vel.publish(Twist()) 134 | 135 | def get_odom(self): 136 | # Get the current transform between the odom and base frames 137 | try: 138 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 139 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 140 | rospy.loginfo("TF Exception") 141 | return 142 | 143 | return (Point(*trans), quat_to_angle(Quaternion(*rot))) 144 | 145 | def shutdown(self): 146 | # Always stop the robot when shutting down the node. 147 | rospy.loginfo("Stopping the robot...") 148 | self.cmd_vel.publish(Twist()) 149 | rospy.sleep(1) 150 | 151 | if __name__ == '__main__': 152 | try: 153 | OutAndBack() 154 | except: 155 | rospy.loginfo("Out-and-Back node terminated.") 156 | -------------------------------------------------------------------------------- /turbot_code/scripts/odomSquare.py: -------------------------------------------------------------------------------- 1 | #odomSquare.py 2 | import rospy 3 | from geometry_msgs.msg import Twist, Point, Quaternion 4 | import tf 5 | from rbx1_nav.transform_utils import quat_to_angle, normalize_angle 6 | from math import radians, copysign, sqrt, pow, pi 7 | 8 | class NavSquare(): 9 | def __init__(self): 10 | # Give the node a name 11 | rospy.init_node('nav_square', anonymous=False) 12 | 13 | # Set rospy to execute a shutdown function when terminating the script 14 | rospy.on_shutdown(self.shutdown) 15 | 16 | # How fast will we check the odometry values? 17 | rate = 20 18 | 19 | # Set the equivalent ROS rate variable 20 | r = rospy.Rate(rate) 21 | 22 | # Set the parameters for the target square 23 | goal_distance = rospy.get_param("~goal_distance", 1.0) # meters 24 | goal_angle = radians(rospy.get_param("~goal_angle", 90)) # degrees converted to radians 25 | linear_speed = rospy.get_param("~linear_speed", 0.2) # meters per second 26 | angular_speed = rospy.get_param("~angular_speed", 0.7) # radians per second 27 | angular_tolerance = radians(rospy.get_param("~angular_tolerance", 2)) # degrees to radians 28 | 29 | # Publisher to control the robot's speed 30 | self.cmd_vel = rospy.Publisher('/cmd_vel_mux/input/navi', Twist, queue_size=5) 31 | 32 | # The base frame is base_footprint for the TurtleBot but base_link for Pi Robot 33 | self.base_frame = rospy.get_param('~base_frame', '/base_link') 34 | 35 | # The odom frame is usually just /odom 36 | self.odom_frame = rospy.get_param('~odom_frame', '/odom') 37 | 38 | # Initialize the tf listener 39 | self.tf_listener = tf.TransformListener() 40 | 41 | # Give tf some time to fill its buffer 42 | rospy.sleep(2) 43 | 44 | # Set the odom frame 45 | self.odom_frame = '/odom' 46 | 47 | # Find out if the robot uses /base_link or /base_footprint 48 | try: 49 | self.tf_listener.waitForTransform(self.odom_frame, '/base_footprint', rospy.Time(), rospy.Duration(1.0)) 50 | self.base_frame = '/base_footprint' 51 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 52 | try: 53 | self.tf_listener.waitForTransform(self.odom_frame, '/base_link', rospy.Time(), rospy.Duration(1.0)) 54 | self.base_frame = '/base_link' 55 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 56 | rospy.loginfo("Cannot find transform between /odom and /base_link or /base_footprint") 57 | rospy.signal_shutdown("tf Exception") 58 | 59 | # Initialize the position variable as a Point type 60 | position = Point() 61 | 62 | # Cycle through the four sides of the square 63 | for i in range(4): 64 | # Initialize the movement command 65 | move_cmd = Twist() 66 | 67 | # Set the movement command to forward motion 68 | move_cmd.linear.x = linear_speed 69 | 70 | # Get the starting position values 71 | (position, rotation) = self.get_odom() 72 | 73 | x_start = position.x 74 | y_start = position.y 75 | 76 | # Keep track of the distance traveled 77 | distance = 0 78 | 79 | # Enter the loop to move along a side 80 | while distance < goal_distance and not rospy.is_shutdown(): 81 | # Publish the Twist message and sleep 1 cycle 82 | self.cmd_vel.publish(move_cmd) 83 | 84 | r.sleep() 85 | 86 | # Get the current position 87 | (position, rotation) = self.get_odom() 88 | 89 | # Compute the Euclidean distance from the start 90 | distance = sqrt(pow((position.x - x_start), 2) + 91 | pow((position.y - y_start), 2)) 92 | 93 | # Stop the robot before rotating 94 | move_cmd = Twist() 95 | self.cmd_vel.publish(move_cmd) 96 | rospy.sleep(1.0) 97 | 98 | # Set the movement command to a rotation 99 | move_cmd.angular.z = angular_speed 100 | 101 | # Track the last angle measured 102 | last_angle = rotation 103 | 104 | # Track how far we have turned 105 | turn_angle = 0 106 | 107 | # Begin the rotation 108 | while abs(turn_angle + angular_tolerance) < abs(goal_angle) and not rospy.is_shutdown(): 109 | # Publish the Twist message and sleep 1 cycle 110 | self.cmd_vel.publish(move_cmd) 111 | 112 | r.sleep() 113 | 114 | # Get the current rotation 115 | (position, rotation) = self.get_odom() 116 | 117 | # Compute the amount of rotation since the last lopp 118 | delta_angle = normalize_angle(rotation - last_angle) 119 | 120 | turn_angle += delta_angle 121 | last_angle = rotation 122 | 123 | move_cmd = Twist() 124 | self.cmd_vel.publish(move_cmd) 125 | rospy.sleep(1.0) 126 | 127 | # Stop the robot when we are done 128 | self.cmd_vel.publish(Twist()) 129 | 130 | def get_odom(self): 131 | # Get the current transform between the odom and base frames 132 | try: 133 | (trans, rot) = self.tf_listener.lookupTransform(self.odom_frame, self.base_frame, rospy.Time(0)) 134 | except (tf.Exception, tf.ConnectivityException, tf.LookupException): 135 | rospy.loginfo("TF Exception") 136 | return 137 | 138 | return (Point(*trans), quat_to_angle(Quaternion(*rot))) 139 | 140 | def shutdown(self): 141 | # Always stop the robot when shutting down the node 142 | rospy.loginfo("Stopping the robot...") 143 | self.cmd_vel.publish(Twist()) 144 | rospy.sleep(1) 145 | 146 | if __name__ == '__main__': 147 | try: 148 | NavSquare() 149 | except rospy.ROSInterruptException: 150 | rospy.loginfo("Navigation terminated.") 151 | -------------------------------------------------------------------------------- /turbot_code/scripts/takePhoto.py: -------------------------------------------------------------------------------- 1 | # Script for simulation 2 | # Launch gazebo world prior to run this script 3 | 4 | from __future__ import print_function 5 | import sys 6 | import rospy 7 | import cv2 8 | from std_msgs.msg import String 9 | from sensor_msgs.msg import Image 10 | from cv_bridge import CvBridge, CvBridgeError 11 | 12 | class TakePhoto: 13 | def __init__(self): 14 | 15 | self.bridge = CvBridge() 16 | self.image_received = False 17 | 18 | # Connect image topic 19 | img_topic = "/camera/rgb/image_raw" 20 | self.image_sub = rospy.Subscriber(img_topic, Image, self.callback) 21 | 22 | # Allow up to one second to connection 23 | rospy.sleep(1) 24 | 25 | def callback(self, data): 26 | 27 | # Convert image to OpenCV format 28 | try: 29 | cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8") 30 | except CvBridgeError as e: 31 | print(e) 32 | 33 | self.image_received = True 34 | self.image = cv_image 35 | 36 | def take_picture(self, img_title): 37 | if self.image_received: 38 | # Save an image 39 | cv2.imwrite(img_title, self.image) 40 | return True 41 | else: 42 | return False 43 | 44 | if __name__ == '__main__': 45 | 46 | # Initialize 47 | rospy.init_node('take_photo', anonymous=False) 48 | camera = TakePhoto() 49 | 50 | # Take a photo 51 | 52 | # Use '_image_title' parameter from command line 53 | # Default value is 'photo.jpg' 54 | img_title = rospy.get_param('~image_title', 'photo.jpg') 55 | 56 | if camera.take_picture(img_title): 57 | rospy.loginfo("Saved image " + img_title) 58 | else: 59 | rospy.loginfo("No images received") 60 | 61 | # Sleep to give the last log messages time to be sent 62 | rospy.sleep(1) 63 | -------------------------------------------------------------------------------- /turbot_map/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_map) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | # add_definitions(-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 you 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 turbot_map 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(include) 119 | include_directories( 120 | ${catkin_INCLUDE_DIRS} 121 | ) 122 | 123 | ## Declare a C++ library 124 | # add_library(${PROJECT_NAME} 125 | # src/${PROJECT_NAME}/turbot_map.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/turbot_map_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_turbot_map.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 | -------------------------------------------------------------------------------- /turbot_map/map/cartoghapher_kinect_v1.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ncnynl/turbot/a77152d95606ab6493f791d8d87c57f159d5d7d0/turbot_map/map/cartoghapher_kinect_v1.pgm -------------------------------------------------------------------------------- /turbot_map/map/cartoghapher_kinect_v1.yaml: -------------------------------------------------------------------------------- 1 | image: /home/ubuntu/turtlebot_ws/src/turbot/turbot_map/map/cartoghapher_kinect_v1.pgm 2 | resolution: 0.035000 3 | origin: [-1.920950, -6.862728, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /turbot_map/map/cartographer_rplidar_a2.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ncnynl/turbot/a77152d95606ab6493f791d8d87c57f159d5d7d0/turbot_map/map/cartographer_rplidar_a2.pgm -------------------------------------------------------------------------------- /turbot_map/map/cartographer_rplidar_a2.yaml: -------------------------------------------------------------------------------- 1 | image: /home/ubuntu/turtlebot_ws/src/turbot/turbot_map/map/cartographer_rplidar_a2.pgm 2 | resolution: 0.050000 3 | origin: [-3.328566, -4.555147, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /turbot_map/map/rplidar_a2.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ncnynl/turbot/a77152d95606ab6493f791d8d87c57f159d5d7d0/turbot_map/map/rplidar_a2.pgm -------------------------------------------------------------------------------- /turbot_map/map/rplidar_a2.yaml: -------------------------------------------------------------------------------- 1 | image: /home/ubuntu/turtlebot_ws/src/turbot/turbot_map/map/rplidar_a2.pgm 2 | resolution: 0.050000 3 | origin: [-13.800000, -15.400000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /turbot_map/map/rplidar_a2_hector_noodom.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ncnynl/turbot/a77152d95606ab6493f791d8d87c57f159d5d7d0/turbot_map/map/rplidar_a2_hector_noodom.pgm -------------------------------------------------------------------------------- /turbot_map/map/rplidar_a2_hector_noodom.yaml: -------------------------------------------------------------------------------- 1 | image: /home/ubuntu/turtlebot_ws/src/turbot/turbot_map/map/rplidar_a2_hector_noodom.pgm 2 | resolution: 0.050000 3 | origin: [-20.024998, -20.024998, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /turbot_map/map/rplidar_a2_karto.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ncnynl/turbot/a77152d95606ab6493f791d8d87c57f159d5d7d0/turbot_map/map/rplidar_a2_karto.pgm -------------------------------------------------------------------------------- /turbot_map/map/rplidar_a2_karto.yaml: -------------------------------------------------------------------------------- 1 | image: /home/ubuntu/turtlebot_ws/src/turbot/turbot_map/map/rplidar_a2_karto.pgm 2 | resolution: 0.025000 3 | origin: [-0.909572, -9.662303, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /turbot_map/map/test.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ncnynl/turbot/a77152d95606ab6493f791d8d87c57f159d5d7d0/turbot_map/map/test.pgm -------------------------------------------------------------------------------- /turbot_map/map/test.yaml: -------------------------------------------------------------------------------- 1 | image: /home/ubuntu/turtlebot_ws/src/turbot/turbot_map/map/test.pgm 2 | resolution: 0.050000 3 | origin: [-13.800000, -12.200000, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /turbot_map/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_map 4 | 0.0.0 5 | The turbot_map package 6 | 7 | 8 | 9 | 10 | ubuntu 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /turbot_map/script/saver: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | #author: ncnynl 3 | #website:ncnynl.com 4 | 5 | if [ ! -n "$1" ] ;then 6 | echo "Please input file name" 7 | else 8 | myPath="$HOME/map" 9 | 10 | if [ ! -d "$myPath" ] ;then 11 | mkdir "$myPath" 12 | fi 13 | 14 | echo "Your filename is $1, Paht is $myPath/$1 " 15 | rosrun map_server map_saver -f "$myPath"/$1 16 | fi 17 | 18 | -------------------------------------------------------------------------------- /turbot_rviz/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_rviz) 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 COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES turbot_rviz 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(turbot_rviz 122 | # src/${PROJECT_NAME}/turbot_rviz.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(turbot_rviz ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(turbot_rviz_node src/turbot_rviz_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(turbot_rviz_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(turbot_rviz_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS turbot_rviz turbot_rviz_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turbot_rviz.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /turbot_rviz/launch/nav.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /turbot_rviz/launch/nav_cartographer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /turbot_rviz/launch/nav_cartographer_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /turbot_rviz/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_rviz 4 | 0.0.0 5 | The turbot_rviz package 6 | 7 | 8 | 9 | 10 | nc 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /turbot_slam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_slam) 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 COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES turbot_slam 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(turbot_slam 122 | # src/${PROJECT_NAME}/turbot_slam.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(turbot_slam ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(turbot_slam_node src/turbot_slam_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(turbot_slam_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(turbot_slam_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS turbot_slam turbot_slam_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turbot_slam.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /turbot_slam/launch/amcl/amcl_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /turbot_slam/launch/amcl/laser_amcl_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /turbot_slam/launch/amcl/laser_amcl_rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/assets/turtlebot_assets_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/cartographer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/cartographer_demo_2d.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 21 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/cartographer_demo_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/cartographer_laser.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/cartographer_laser_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 10 | 11 | 12 | 13 | 14 | 16 | 17 | 18 | 19 | 20 | 21 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/laser_cartographer.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/laser_cartographer_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/laser_cartographer_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/laser_cartographer_demo_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/laser_cartographer_demo_3d_offline.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/lua/transform.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2017 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | XY_TRANSFORM = { 16 | translation = { 0., 0., 0. }, 17 | rotation = { 0., -math.pi / 2., 0., }, 18 | } 19 | 20 | XZ_TRANSFORM = { 21 | translation = { 0., 0., 0. }, 22 | rotation = { 0. , 0., -math.pi / 2, }, 23 | } 24 | 25 | YZ_TRANSFORM = { 26 | translation = { 0., 0., 0. }, 27 | rotation = { 0. , 0., math.pi, }, 28 | } 29 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/lua/turtlebot_assets_rslidar_3d.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | VOXEL_SIZE = 5e-2 16 | 17 | include "transform.lua" 18 | 19 | options = { 20 | tracking_frame = "base_link", 21 | pipeline = { 22 | { 23 | action = "min_max_range_filter", 24 | min_range = 1., 25 | max_range = 20., 26 | }, 27 | { 28 | action = "dump_num_points", 29 | }, 30 | { 31 | action = "fixed_ratio_sampler", 32 | sampling_ratio = 0.01, 33 | }, 34 | 35 | { 36 | action = "write_ply", 37 | filename = "points.ply" 38 | }, 39 | 40 | -- Gray X-Rays. These only use geometry to color pixels. 41 | { 42 | action = "write_xray_image", 43 | voxel_size = VOXEL_SIZE, 44 | filename = "xray_yz_all", 45 | transform = YZ_TRANSFORM, 46 | }, 47 | { 48 | action = "write_xray_image", 49 | voxel_size = VOXEL_SIZE, 50 | filename = "xray_xy_all", 51 | transform = XY_TRANSFORM, 52 | }, 53 | { 54 | action = "write_xray_image", 55 | voxel_size = VOXEL_SIZE, 56 | filename = "xray_xz_all", 57 | transform = XZ_TRANSFORM, 58 | }, 59 | 60 | -- Now we recolor our points by frame and write another batch of X-Rays. It 61 | -- is visible in them what was seen by the horizontal and the vertical 62 | -- laser. 63 | { 64 | action = "color_points", 65 | frame_id = "rslidar", 66 | color = { 255., 0., 0. }, 67 | }, 68 | 69 | { 70 | action = "write_xray_image", 71 | voxel_size = VOXEL_SIZE, 72 | filename = "xray_yz_all_color", 73 | transform = YZ_TRANSFORM, 74 | }, 75 | { 76 | action = "write_xray_image", 77 | voxel_size = VOXEL_SIZE, 78 | filename = "xray_xy_all_color", 79 | transform = XY_TRANSFORM, 80 | }, 81 | { 82 | action = "write_xray_image", 83 | voxel_size = VOXEL_SIZE, 84 | filename = "xray_xz_all_color", 85 | transform = XZ_TRANSFORM, 86 | }, 87 | } 88 | } 89 | 90 | return options 91 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/lua/turtlebot_rslidar_3d.lua: -------------------------------------------------------------------------------- 1 | -- Copyright 2016 The Cartographer Authors 2 | -- 3 | -- Licensed under the Apache License, Version 2.0 (the "License"); 4 | -- you may not use this file except in compliance with the License. 5 | -- You may obtain a copy of the License at 6 | -- 7 | -- http://www.apache.org/licenses/LICENSE-2.0 8 | -- 9 | -- Unless required by applicable law or agreed to in writing, software 10 | -- distributed under the License is distributed on an "AS IS" BASIS, 11 | -- WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | -- See the License for the specific language governing permissions and 13 | -- limitations under the License. 14 | 15 | include "map_builder.lua" 16 | include "trajectory_builder.lua" 17 | 18 | options = { 19 | map_builder = MAP_BUILDER, 20 | trajectory_builder = TRAJECTORY_BUILDER, 21 | map_frame = "map", 22 | tracking_frame = "gyro_link", 23 | published_frame = "odom", 24 | odom_frame = "odom", 25 | provide_odom_frame = false, 26 | publish_frame_projected_to_2d = false, 27 | use_odometry = true, 28 | use_nav_sat = false, 29 | use_landmarks = false, 30 | num_laser_scans = 0, 31 | num_multi_echo_laser_scans = 0, 32 | num_subdivisions_per_laser_scan = 1, 33 | num_point_clouds = 1, 34 | lookup_transform_timeout_sec = 0.2, 35 | submap_publish_period_sec = 0.3, 36 | pose_publish_period_sec = 5e-3, 37 | trajectory_publish_period_sec = 30e-3, 38 | rangefinder_sampling_ratio = 1., 39 | odometry_sampling_ratio = 1., 40 | fixed_frame_pose_sampling_ratio = 1., 41 | imu_sampling_ratio = 1., 42 | landmarks_sampling_ratio = 1., 43 | } 44 | 45 | TRAJECTORY_BUILDER_3D.num_accumulated_range_data = 1 46 | TRAJECTORY_BUILDER_3D.min_range = 0.2 47 | TRAJECTORY_BUILDER_3D.max_range = 20. 48 | TRAJECTORY_BUILDER_2D.min_z = 0.1 49 | TRAJECTORY_BUILDER_2D.max_z = 1.0 50 | TRAJECTORY_BUILDER_3D.use_online_correlative_scan_matching = false 51 | 52 | MAP_BUILDER.use_trajectory_builder_3d = true 53 | MAP_BUILDER.num_background_threads = 4 54 | POSE_GRAPH.optimization_problem.huber_scale = 5e2 55 | POSE_GRAPH.optimize_every_n_nodes = 40 56 | POSE_GRAPH.constraint_builder.sampling_ratio = 0.03 57 | POSE_GRAPH.optimization_problem.ceres_solver_options.max_num_iterations = 20 58 | POSE_GRAPH.constraint_builder.min_score = 0.5 59 | POSE_GRAPH.constraint_builder.global_localization_min_score = 0.55 60 | 61 | 62 | POSE_GRAPH.optimization_problem.odometry_translation_weight = 1e3 63 | POSE_GRAPH.optimization_problem.odometry_rotation_weight = 1e3 64 | 65 | return options 66 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/turtlebot_lida.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /turbot_slam/launch/cartographer/urdf/turtlebot_urdf_rslidar_3d.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /turbot_slam/launch/gmapping/gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /turbot_slam/launch/gmapping/laser_gmapping_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /turbot_slam/launch/hector/laser_hector_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /turbot_slam/launch/hector/readme.txt: -------------------------------------------------------------------------------- 1 | if laser hector , it will shake when start mapping. 2 | It is ok , just continue to mapping. it will stop shake after you move robot. 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /turbot_slam/launch/karto/karto_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /turbot_slam/launch/karto/laser_karto_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /turbot_slam/launch/readme.txt: -------------------------------------------------------------------------------- 1 | #depth camera 2 | #TURTLEBOT_3D_SENSOR 3 | #kinect,kinect_v2, 4 | 5 | #TURTLEBOT_LASER_SENSOR 6 | #rplidar_a1,rplidar_a2,eai_f4 7 | 8 | -------------------------------------------------------------------------------- /turbot_slam/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_slam 4 | 0.0.0 5 | The turbot_slam package 6 | 7 | 8 | 9 | 10 | nc 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /turbot_teleop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_teleop) 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 COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES turbot_teleop 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(turbot_teleop 122 | # src/${PROJECT_NAME}/turbot_teleop.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(turbot_teleop ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(turbot_teleop_node src/turbot_teleop_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(turbot_teleop_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(turbot_teleop_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS turbot_teleop turbot_teleop_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turbot_teleop.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /turbot_teleop/launch/keyboard.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /turbot_teleop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_teleop 4 | 0.0.0 5 | The turbot_teleop package 6 | 7 | 8 | 9 | 10 | nc 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /turbot_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turbot_tools) 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 COMPONENTS 8 | roscpp 9 | rospy 10 | std_msgs 11 | ) 12 | 13 | ## System dependencies are found with CMake's conventions 14 | # find_package(Boost REQUIRED COMPONENTS system) 15 | 16 | 17 | ## Uncomment this if the package has a setup.py. This macro ensures 18 | ## modules and global scripts declared therein get installed 19 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 20 | # catkin_python_setup() 21 | 22 | ################################################ 23 | ## Declare ROS messages, services and actions ## 24 | ################################################ 25 | 26 | ## To declare and build messages, services or actions from within this 27 | ## package, follow these steps: 28 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 29 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 30 | ## * In the file package.xml: 31 | ## * add a build_depend tag for "message_generation" 32 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 33 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 34 | ## but can be declared for certainty nonetheless: 35 | ## * add a run_depend tag for "message_runtime" 36 | ## * In this file (CMakeLists.txt): 37 | ## * add "message_generation" and every package in MSG_DEP_SET to 38 | ## find_package(catkin REQUIRED COMPONENTS ...) 39 | ## * add "message_runtime" and every package in MSG_DEP_SET to 40 | ## catkin_package(CATKIN_DEPENDS ...) 41 | ## * uncomment the add_*_files sections below as needed 42 | ## and list every .msg/.srv/.action file to be processed 43 | ## * uncomment the generate_messages entry below 44 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 45 | 46 | ## Generate messages in the 'msg' folder 47 | # add_message_files( 48 | # FILES 49 | # Message1.msg 50 | # Message2.msg 51 | # ) 52 | 53 | ## Generate services in the 'srv' folder 54 | # add_service_files( 55 | # FILES 56 | # Service1.srv 57 | # Service2.srv 58 | # ) 59 | 60 | ## Generate actions in the 'action' folder 61 | # add_action_files( 62 | # FILES 63 | # Action1.action 64 | # Action2.action 65 | # ) 66 | 67 | ## Generate added messages and services with any dependencies listed here 68 | # generate_messages( 69 | # DEPENDENCIES 70 | # std_msgs 71 | # ) 72 | 73 | ################################################ 74 | ## Declare ROS dynamic reconfigure parameters ## 75 | ################################################ 76 | 77 | ## To declare and build dynamic reconfigure parameters within this 78 | ## package, follow these steps: 79 | ## * In the file package.xml: 80 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 81 | ## * In this file (CMakeLists.txt): 82 | ## * add "dynamic_reconfigure" to 83 | ## find_package(catkin REQUIRED COMPONENTS ...) 84 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 85 | ## and list every .cfg file to be processed 86 | 87 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 88 | # generate_dynamic_reconfigure_options( 89 | # cfg/DynReconf1.cfg 90 | # cfg/DynReconf2.cfg 91 | # ) 92 | 93 | ################################### 94 | ## catkin specific configuration ## 95 | ################################### 96 | ## The catkin_package macro generates cmake config files for your package 97 | ## Declare things to be passed to dependent projects 98 | ## INCLUDE_DIRS: uncomment this if you package contains header files 99 | ## LIBRARIES: libraries you create in this project that dependent projects also need 100 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 101 | ## DEPENDS: system dependencies of this project that dependent projects also need 102 | catkin_package( 103 | # INCLUDE_DIRS include 104 | # LIBRARIES turbot_tools 105 | # CATKIN_DEPENDS roscpp rospy std_msgs 106 | # DEPENDS system_lib 107 | ) 108 | 109 | ########### 110 | ## Build ## 111 | ########### 112 | 113 | ## Specify additional locations of header files 114 | ## Your package locations should be listed before other locations 115 | # include_directories(include) 116 | include_directories( 117 | ${catkin_INCLUDE_DIRS} 118 | ) 119 | 120 | ## Declare a C++ library 121 | # add_library(turbot_tools 122 | # src/${PROJECT_NAME}/turbot_tools.cpp 123 | # ) 124 | 125 | ## Add cmake target dependencies of the library 126 | ## as an example, code may need to be generated before libraries 127 | ## either from message generation or dynamic reconfigure 128 | # add_dependencies(turbot_tools ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Declare a C++ executable 131 | # add_executable(turbot_tools_node src/turbot_tools_node.cpp) 132 | 133 | ## Add cmake target dependencies of the executable 134 | ## same as for the library above 135 | # add_dependencies(turbot_tools_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Specify libraries to link a library or executable target against 138 | # target_link_libraries(turbot_tools_node 139 | # ${catkin_LIBRARIES} 140 | # ) 141 | 142 | ############# 143 | ## Install ## 144 | ############# 145 | 146 | # all install targets should use catkin DESTINATION variables 147 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 148 | 149 | ## Mark executable scripts (Python etc.) for installation 150 | ## in contrast to setup.py, you can choose the destination 151 | # install(PROGRAMS 152 | # scripts/my_python_script 153 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark executables and/or libraries for installation 157 | # install(TARGETS turbot_tools turbot_tools_node 158 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 161 | # ) 162 | 163 | ## Mark cpp header files for installation 164 | # install(DIRECTORY include/${PROJECT_NAME}/ 165 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 166 | # FILES_MATCHING PATTERN "*.h" 167 | # PATTERN ".svn" EXCLUDE 168 | # ) 169 | 170 | ## Mark other files for installation (e.g. launch and bag files, etc.) 171 | # install(FILES 172 | # # myfile1 173 | # # myfile2 174 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 175 | # ) 176 | 177 | ############# 178 | ## Testing ## 179 | ############# 180 | 181 | ## Add gtest based cpp test target and link libraries 182 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_turbot_tools.cpp) 183 | # if(TARGET ${PROJECT_NAME}-test) 184 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 185 | # endif() 186 | 187 | ## Add folders to be run by python nosetests 188 | # catkin_add_nosetests(test) 189 | -------------------------------------------------------------------------------- /turbot_tools/launch/freenect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /turbot_tools/launch/rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /turbot_tools/launch/test_rplidar.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /turbot_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turbot_tools 4 | 0.0.0 5 | The turbot_tools package 6 | 7 | 8 | 9 | 10 | nc 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 | catkin 43 | roscpp 44 | rospy 45 | std_msgs 46 | roscpp 47 | rospy 48 | std_msgs 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /turbot_tools/script/test_draw_a_square.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | Copyright (c) 2015, Mark Silliman 5 | All rights reserved. 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 11 | ''' 12 | 13 | # An example of TurtleBot 2 drawing a 0.4 meter square. 14 | # Written for indigo 15 | 16 | import rospy 17 | from geometry_msgs.msg import Twist 18 | from math import radians 19 | 20 | class DrawASquare(): 21 | def __init__(self): 22 | # initiliaze 23 | rospy.init_node('drawasquare', anonymous=False) 24 | # What to do you ctrl + c 25 | rospy.on_shutdown(self.shutdown) 26 | 27 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 28 | 29 | # 5 HZ 30 | r = rospy.Rate(5); 31 | 32 | # create two different Twist() variables. One for moving forward. One for turning 45 degrees. 33 | 34 | # let's go forward at 0.2 m/s 35 | move_cmd = Twist() 36 | move_cmd.linear.x = 0 37 | # by default angular.z is 0 so setting this isn't required 38 | 39 | #let's turn at 45 deg/s 40 | turn_cmd = Twist() 41 | turn_cmd.linear.x = 0 42 | turn_cmd.angular.z = radians(45); #45 deg/s in radians/s 43 | 44 | #two keep drawing squares. Go forward for 2 seconds (10 x 5 HZ) then turn for 2 second 45 | count = 0 46 | while not rospy.is_shutdown(): 47 | # go forward 0.4 m (2 seconds * 0.2 m / seconds) 48 | rospy.loginfo("Going Straight") 49 | for x in range(0,10): 50 | self.cmd_vel.publish(move_cmd) 51 | r.sleep() 52 | # turn 90 degrees 53 | rospy.loginfo("Turning") 54 | for x in range(0,10): 55 | self.cmd_vel.publish(turn_cmd) 56 | r.sleep() 57 | count = count + 1 58 | if(count == 4): 59 | count = 0 60 | if(count == 0): 61 | rospy.loginfo("TurtleBot should be close to the original starting position (but it's probably way off)") 62 | 63 | def shutdown(self): 64 | # stop turtlebot 65 | rospy.loginfo("Stop Drawing Squares") 66 | self.cmd_vel.publish(Twist()) 67 | rospy.sleep(1) 68 | 69 | if __name__ == '__main__': 70 | try: 71 | DrawASquare() 72 | except: 73 | rospy.loginfo("node terminated.") 74 | 75 | -------------------------------------------------------------------------------- /turbot_tools/script/test_goforward.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | Copyright (c) 2015, Mark Silliman 5 | All rights reserved. 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 11 | ''' 12 | 13 | # A very basic TurtleBot script that moves TurtleBot forward indefinitely. Press CTRL + C to stop. To run: 14 | # On TurtleBot: 15 | # roslaunch turtlebot_bringup minimal.launch 16 | # On work station: 17 | # python goforward.py 18 | 19 | import rospy 20 | from geometry_msgs.msg import Twist 21 | 22 | class GoForward(): 23 | def __init__(self): 24 | # initiliaze 25 | rospy.init_node('GoForward', anonymous=False) 26 | 27 | # tell user how to stop TurtleBot 28 | rospy.loginfo("To stop TurtleBot CTRL + C") 29 | 30 | # What function to call when you ctrl + c 31 | rospy.on_shutdown(self.shutdown) 32 | 33 | # Create a publisher which can "talk" to TurtleBot and tell it to move 34 | # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 35 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 36 | 37 | #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ 38 | r = rospy.Rate(10); 39 | 40 | # Twist is a datatype for velocity 41 | move_cmd = Twist() 42 | # let's go forward at 0.2 m/s 43 | move_cmd.linear.x = 0.2 44 | # let's turn at 0 radians/s 45 | move_cmd.angular.z = 0 46 | 47 | # as long as you haven't ctrl + c keeping doing... 48 | while not rospy.is_shutdown(): 49 | # publish the velocity 50 | self.cmd_vel.publish(move_cmd) 51 | # wait for 0.1 seconds (10 HZ) and publish again 52 | r.sleep() 53 | 54 | 55 | def shutdown(self): 56 | # stop turtlebot 57 | rospy.loginfo("Stop TurtleBot") 58 | # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot 59 | self.cmd_vel.publish(Twist()) 60 | # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script 61 | rospy.sleep(1) 62 | 63 | if __name__ == '__main__': 64 | try: 65 | GoForward() 66 | except: 67 | rospy.loginfo("GoForward node terminated.") 68 | -------------------------------------------------------------------------------- /turbot_tools/script/test_goincircles.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | ''' 4 | Copyright (c) 2015, Mark Silliman 5 | All rights reserved. 6 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 7 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. 10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 11 | ''' 12 | 13 | # A very basic TurtleBot script that moves TurtleBot forward indefinitely. Press CTRL + C to stop. To run: 14 | # On TurtleBot: 15 | # roslaunch turtlebot_bringup minimal.launch 16 | # On work station: 17 | # python goforward.py 18 | 19 | import rospy 20 | from geometry_msgs.msg import Twist 21 | 22 | class GoForward(): 23 | def __init__(self): 24 | # initiliaze 25 | rospy.init_node('GoForward', anonymous=False) 26 | 27 | # tell user how to stop TurtleBot 28 | rospy.loginfo("To stop TurtleBot CTRL + C") 29 | 30 | # What function to call when you ctrl + c 31 | rospy.on_shutdown(self.shutdown) 32 | 33 | # Create a publisher which can "talk" to TurtleBot and tell it to move 34 | # Tip: You may need to change cmd_vel_mux/input/navi to /cmd_vel if you're not using TurtleBot2 35 | self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10) 36 | 37 | #TurtleBot will stop if we don't keep telling it to move. How often should we tell it to move? 10 HZ 38 | r = rospy.Rate(10); 39 | 40 | # Twist is a datatype for velocity 41 | move_cmd = Twist() 42 | # let's go forward at 0.2 m/s 43 | move_cmd.linear.x = 0 44 | # let's turn at 0 radians/s 45 | move_cmd.angular.z = 0.5 46 | 47 | # as long as you haven't ctrl + c keeping doing... 48 | while not rospy.is_shutdown(): 49 | # publish the velocity 50 | self.cmd_vel.publish(move_cmd) 51 | # wait for 0.1 seconds (10 HZ) and publish again 52 | r.sleep() 53 | 54 | 55 | def shutdown(self): 56 | # stop turtlebot 57 | rospy.loginfo("Stop TurtleBot") 58 | # a default Twist has linear.x of 0 and angular.z of 0. So it'll stop TurtleBot 59 | self.cmd_vel.publish(Twist()) 60 | # sleep just makes sure TurtleBot receives the stop command prior to shutting down the script 61 | rospy.sleep(1) 62 | 63 | if __name__ == '__main__': 64 | try: 65 | GoForward() 66 | except: 67 | rospy.loginfo("GoForward node terminated.") 68 | -------------------------------------------------------------------------------- /turbot_tools/script/test_kinect_color: -------------------------------------------------------------------------------- 1 | rosrun image_view image_view image:=/camera/rgb/image_color 2 | -------------------------------------------------------------------------------- /turbot_tools/script/test_kinect_depth: -------------------------------------------------------------------------------- 1 | rosrun image_view image_view image:=/camera/depth_registered/image_raw 2 | -------------------------------------------------------------------------------- /turbot_tools/script/test_kobuki: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rqt -s kobuki_dashboard 4 | 5 | -------------------------------------------------------------------------------- /turbot_tools/script/test_lidar: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | roslaunch turbot_tools test_rplidar.launch 4 | 5 | -------------------------------------------------------------------------------- /turbot_tools/script/test_move: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | rostopic pub -r 10 /cmd_vel_mux/input/navi geometry_msgs/Twist '{linear: {x: 0.1, y: 0.0, z: 0.0}, angular: {x: 0.0,y: 0.0,z: 0.0}}' 4 | -------------------------------------------------------------------------------- /turbot_vslam/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.4.6) 2 | include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake) 3 | 4 | # Set the build type. Options are: 5 | # Coverage : w/ debug symbols, w/o optimization, w/ code-coverage 6 | # Debug : w/ debug symbols, w/o optimization 7 | # Release : w/o debug symbols, w/ optimization 8 | # RelWithDebInfo : w/ debug symbols, w/ optimization 9 | # MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries 10 | #set(ROS_BUILD_TYPE RelWithDebInfo) 11 | 12 | rosbuild_init() 13 | 14 | #set the default path for built executables to the "bin" directory 15 | set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin) 16 | #set the default path for built libraries to the "lib" directory 17 | set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) 18 | 19 | #uncomment if you have defined messages 20 | #rosbuild_genmsg() 21 | #uncomment if you have defined services 22 | #rosbuild_gensrv() 23 | 24 | #common commands for building c++ executables and libraries 25 | #rosbuild_add_library(${PROJECT_NAME} src/example.cpp) 26 | #target_link_libraries(${PROJECT_NAME} another_library) 27 | #rosbuild_add_boost_directories() 28 | #rosbuild_link_boost(${PROJECT_NAME} thread) 29 | #rosbuild_add_executable(example examples/example.cpp) 30 | #target_link_libraries(example ${PROJECT_NAME}) 31 | -------------------------------------------------------------------------------- /turbot_vslam/Makefile: -------------------------------------------------------------------------------- 1 | include $(shell rospack find mk)/cmake.mk -------------------------------------------------------------------------------- /turbot_vslam/launch/rtabmap/rtabmap.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /turbot_vslam/launch/rtabmap/rtabmap_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /turbot_vslam/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b turbot_vslam 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /turbot_vslam/manifest.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | turbot_vslam 5 | 6 | 7 | ubuntu 8 | BSD 9 | 10 | http://ros.org/wiki/turbot_vslam 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | --------------------------------------------------------------------------------