├── 1.png ├── CMakeLists.txt ├── CMakeLists.txt~ ├── README.md ├── all.png ├── ex.png ├── ex1.png ├── launch ├── config │ ├── list.txt │ ├── planner_parameters.yaml │ ├── planner_parameters_return.yaml │ └── robot.rviz ├── driver.launch ├── start_turtlebot_mapping_planning_control.launch ├── start_turtlebot_modules.launch ├── test_planning.launch └── view_robot.launch ├── package.xml ├── package.xml~ ├── src ├── MapProcessing.py ├── MapProcessing.pyc ├── controller_turtlebot.py ├── driver.py ├── explore.py ├── laserscan_to_pointcloud.cpp ├── node.py ├── offline_planner_R2.cpp ├── offline_planner_R2_2.cpp ├── state_validity_checker_octomap_R2.cpp ├── state_validity_checker_octomap_R2.hpp ├── turtlebot_drive.py ├── turtlebot_drive.pyc ├── turtlebot_return .py ├── turtlebot_return.py └── turtlebot_return.pyc ├── srv ├── FindPathToGoal.srv └── GotoWaypoint.srv └── worlds ├── willow.world └── willowgarage.world /1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bigmb/Autonomous-Navigation-and-Exploration/465b86c8587209f6d963bb9263846db51e292e45/1.png -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autonomous_explore_map_plan) 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 | geometry_msgs 15 | message_generation 16 | laser_geometry 17 | tf 18 | roslib 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | find_package(Boost REQUIRED COMPONENTS system) 23 | find_package(PCL REQUIRED) 24 | find_package(Octomap) 25 | find_package(OMPL REQUIRED) 26 | 27 | 28 | ## Uncomment this if the package has a setup.py. This macro ensures 29 | ## modules and global scripts declared therein get installed 30 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 31 | # catkin_python_setup() 32 | 33 | ################################################ 34 | ## Declare ROS messages, services and actions ## 35 | ################################################ 36 | 37 | ## To declare and build messages, services or actions from within this 38 | ## package, follow these steps: 39 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 40 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 41 | ## * In the file package.xml: 42 | ## * add a build_depend tag for "message_generation" 43 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 44 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 45 | ## but can be declared for certainty nonetheless: 46 | ## * add a run_depend tag for "message_runtime" 47 | ## * In this file (CMakeLists.txt): 48 | ## * add "message_generation" and every package in MSG_DEP_SET to 49 | ## find_package(catkin REQUIRED COMPONENTS ...) 50 | ## * add "message_runtime" and every package in MSG_DEP_SET to 51 | ## catkin_package(CATKIN_DEPENDS ...) 52 | ## * uncomment the add_*_files sections below as needed 53 | ## and list every .msg/.srv/.action file to be processed 54 | ## * uncomment the generate_messages entry below 55 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 56 | 57 | ## Generate messages in the 'msg' folder 58 | # add_message_files( 59 | # FILES 60 | # Message1.msg 61 | # Message2.msg 62 | # ) 63 | 64 | ## Generate services in the 'srv' folder 65 | # add_service_files( 66 | # FILES 67 | # Service1.srv 68 | # Service2.srv 69 | # ) 70 | 71 | add_service_files( 72 | FILES 73 | GotoWaypoint.srv 74 | FindPathToGoal.srv 75 | ) 76 | 77 | ## Generate actions in the 'action' folder 78 | # add_action_files( 79 | # FILES 80 | # Action1.action 81 | # Action2.action 82 | # ) 83 | 84 | ## Generate added messages and services with any dependencies listed here 85 | # generate_messages( 86 | # DEPENDENCIES 87 | # std_msgs 88 | # ) 89 | generate_messages( 90 | DEPENDENCIES 91 | std_msgs geometry_msgs 92 | ) 93 | 94 | ################################################ 95 | ## Declare ROS dynamic reconfigure parameters ## 96 | ################################################ 97 | 98 | ## To declare and build dynamic reconfigure parameters within this 99 | ## package, follow these steps: 100 | ## * In the file package.xml: 101 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 102 | ## * In this file (CMakeLists.txt): 103 | ## * add "dynamic_reconfigure" to 104 | ## find_package(catkin REQUIRED COMPONENTS ...) 105 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 106 | ## and list every .cfg file to be processed 107 | 108 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 109 | # generate_dynamic_reconfigure_options( 110 | # cfg/DynReconf1.cfg 111 | # cfg/DynReconf2.cfg 112 | # ) 113 | 114 | ################################### 115 | ## catkin specific configuration ## 116 | ################################### 117 | ## The catkin_package macro generates cmake config files for your package 118 | ## Declare things to be passed to dependent projects 119 | ## INCLUDE_DIRS: uncomment this if you package contains header files 120 | ## LIBRARIES: libraries you create in this project that dependent projects also need 121 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 122 | ## DEPENDS: system dependencies of this project that dependent projects also need 123 | catkin_package( 124 | # INCLUDE_DIRS include 125 | # LIBRARIES autonomous_explore_map_plan 126 | CATKIN_DEPENDS roscpp rospy std_msgs 127 | # DEPENDS system_lib 128 | DEPENDS Eigen 129 | ) 130 | 131 | ########### 132 | ## Build ## 133 | ########### 134 | 135 | ## Specify additional locations of header files 136 | ## Your package locations should be listed before other locations 137 | # include_directories(include) 138 | include_directories( 139 | ${catkin_INCLUDE_DIRS} 140 | ${Eigen_INCLUDE_DIRS} 141 | ${OMPL_INCLUDE_DIRS} 142 | ${PCL_INCLUDE_DIRS} 143 | ) 144 | 145 | add_definitions(${EIGEN_DEFINITIONS}) 146 | 147 | ## Declare a C++ library 148 | # add_library(${PROJECT_NAME} 149 | # src/${PROJECT_NAME}/autonomous_explore_map_plan.cpp 150 | # ) 151 | 152 | ## Add cmake target dependencies of the library 153 | ## as an example, code may need to be generated before libraries 154 | ## either from message generation or dynamic reconfigure 155 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Declare a C++ executable 158 | ## With catkin_make all packages are built within a single CMake context 159 | ## The recommended prefix ensures that target names across packages don't collide 160 | # add_executable(${PROJECT_NAME}_node src/autonomous_explore_map_plan_node.cpp) 161 | 162 | add_executable(laserscan_2_pointcloud src/laserscan_to_pointcloud.cpp) 163 | target_link_libraries(laserscan_2_pointcloud ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ) 164 | 165 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 166 | 167 | add_executable(offline_planner_R2 168 | src/offline_planner_R2.cpp 169 | src/state_validity_checker_octomap_R2.cpp 170 | ) 171 | target_link_libraries(offline_planner_R2 ${catkin_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${OMPL_LIBRARIES}) 172 | add_dependencies(offline_planner_R2 auv_msgs_generate_messages_cpp) 173 | 174 | add_executable(offline_planner_R2_2 175 | src/offline_planner_R2_2.cpp 176 | src/state_validity_checker_octomap_R2.cpp 177 | ) 178 | target_link_libraries(offline_planner_R2_2 ${catkin_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${OMPL_LIBRARIES}) 179 | add_dependencies(offline_planner_R2_2 auv_msgs_generate_messages_cpp) 180 | 181 | ## Rename C++ executable without prefix 182 | ## The above recommended prefix causes long target names, the following renames the 183 | ## target back to the shorter version for ease of user use 184 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 185 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 186 | 187 | ## Add cmake target dependencies of the executable 188 | ## same as for the library above 189 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 190 | 191 | ## Specify libraries to link a library or executable target against 192 | # target_link_libraries(${PROJECT_NAME}_node 193 | # ${catkin_LIBRARIES} 194 | # ) 195 | 196 | ############# 197 | ## Install ## 198 | ############# 199 | 200 | # all install targets should use catkin DESTINATION variables 201 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 202 | 203 | ## Mark executable scripts (Python etc.) for installation 204 | ## in contrast to setup.py, you can choose the destination 205 | # install(PROGRAMS 206 | # scripts/my_python_script 207 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 208 | # ) 209 | 210 | ## Mark executables and/or libraries for installation 211 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 212 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 213 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 214 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 215 | # ) 216 | 217 | ## Mark cpp header files for installation 218 | # install(DIRECTORY include/${PROJECT_NAME}/ 219 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 220 | # FILES_MATCHING PATTERN "*.h" 221 | # PATTERN ".svn" EXCLUDE 222 | # ) 223 | 224 | ## Mark other files for installation (e.g. launch and bag files, etc.) 225 | # install(FILES 226 | # # myfile1 227 | # # myfile2 228 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 229 | # ) 230 | 231 | ############# 232 | ## Testing ## 233 | ############# 234 | 235 | ## Add gtest based cpp test target and link libraries 236 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_autonomous_explore_map_plan.cpp) 237 | # if(TARGET ${PROJECT_NAME}-test) 238 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 239 | # endif() 240 | 241 | ## Add folders to be run by python nosetests 242 | # catkin_add_nosetests(test) 243 | -------------------------------------------------------------------------------- /CMakeLists.txt~: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autonomous_explore_map_plan) 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 | geometry_msgs 15 | message_generation 16 | laser_geometry 17 | tf 18 | roslib 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | find_package(Boost REQUIRED COMPONENTS system) 23 | find_package(PCL REQUIRED) 24 | find_package(Octomap) 25 | find_package(OMPL REQUIRED) 26 | 27 | 28 | ## Uncomment this if the package has a setup.py. This macro ensures 29 | ## modules and global scripts declared therein get installed 30 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 31 | # catkin_python_setup() 32 | 33 | ################################################ 34 | ## Declare ROS messages, services and actions ## 35 | ################################################ 36 | 37 | ## To declare and build messages, services or actions from within this 38 | ## package, follow these steps: 39 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 40 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 41 | ## * In the file package.xml: 42 | ## * add a build_depend tag for "message_generation" 43 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 44 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 45 | ## but can be declared for certainty nonetheless: 46 | ## * add a run_depend tag for "message_runtime" 47 | ## * In this file (CMakeLists.txt): 48 | ## * add "message_generation" and every package in MSG_DEP_SET to 49 | ## find_package(catkin REQUIRED COMPONENTS ...) 50 | ## * add "message_runtime" and every package in MSG_DEP_SET to 51 | ## catkin_package(CATKIN_DEPENDS ...) 52 | ## * uncomment the add_*_files sections below as needed 53 | ## and list every .msg/.srv/.action file to be processed 54 | ## * uncomment the generate_messages entry below 55 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 56 | 57 | ## Generate messages in the 'msg' folder 58 | # add_message_files( 59 | # FILES 60 | # Message1.msg 61 | # Message2.msg 62 | # ) 63 | 64 | ## Generate services in the 'srv' folder 65 | # add_service_files( 66 | # FILES 67 | # Service1.srv 68 | # Service2.srv 69 | # ) 70 | 71 | add_service_files( 72 | FILES 73 | GotoWaypoint.srv 74 | FindPathToGoal.srv 75 | ) 76 | 77 | ## Generate actions in the 'action' folder 78 | # add_action_files( 79 | # FILES 80 | # Action1.action 81 | # Action2.action 82 | # ) 83 | 84 | ## Generate added messages and services with any dependencies listed here 85 | # generate_messages( 86 | # DEPENDENCIES 87 | # std_msgs 88 | # ) 89 | generate_messages( 90 | DEPENDENCIES 91 | std_msgs geometry_msgs 92 | ) 93 | 94 | ################################################ 95 | ## Declare ROS dynamic reconfigure parameters ## 96 | ################################################ 97 | 98 | ## To declare and build dynamic reconfigure parameters within this 99 | ## package, follow these steps: 100 | ## * In the file package.xml: 101 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 102 | ## * In this file (CMakeLists.txt): 103 | ## * add "dynamic_reconfigure" to 104 | ## find_package(catkin REQUIRED COMPONENTS ...) 105 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 106 | ## and list every .cfg file to be processed 107 | 108 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 109 | # generate_dynamic_reconfigure_options( 110 | # cfg/DynReconf1.cfg 111 | # cfg/DynReconf2.cfg 112 | # ) 113 | 114 | ################################### 115 | ## catkin specific configuration ## 116 | ################################### 117 | ## The catkin_package macro generates cmake config files for your package 118 | ## Declare things to be passed to dependent projects 119 | ## INCLUDE_DIRS: uncomment this if you package contains header files 120 | ## LIBRARIES: libraries you create in this project that dependent projects also need 121 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 122 | ## DEPENDS: system dependencies of this project that dependent projects also need 123 | catkin_package( 124 | # INCLUDE_DIRS include 125 | # LIBRARIES autonomous_explore_map_plan 126 | CATKIN_DEPENDS roscpp rospy std_msgs 127 | # DEPENDS system_lib 128 | DEPENDS Eigen 129 | ) 130 | 131 | ########### 132 | ## Build ## 133 | ########### 134 | 135 | ## Specify additional locations of header files 136 | ## Your package locations should be listed before other locations 137 | # include_directories(include) 138 | include_directories( 139 | ${catkin_INCLUDE_DIRS} 140 | ${Eigen_INCLUDE_DIRS} 141 | ${OMPL_INCLUDE_DIRS} 142 | ${PCL_INCLUDE_DIRS} 143 | ) 144 | 145 | add_definitions(${EIGEN_DEFINITIONS}) 146 | 147 | ## Declare a C++ library 148 | # add_library(${PROJECT_NAME} 149 | # src/${PROJECT_NAME}/autonomous_explore_map_plan.cpp 150 | # ) 151 | 152 | ## Add cmake target dependencies of the library 153 | ## as an example, code may need to be generated before libraries 154 | ## either from message generation or dynamic reconfigure 155 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Declare a C++ executable 158 | ## With catkin_make all packages are built within a single CMake context 159 | ## The recommended prefix ensures that target names across packages don't collide 160 | # add_executable(${PROJECT_NAME}_node src/autonomous_explore_map_plan_node.cpp) 161 | add_executable(laserscan_to_pointcloud src/laserscan_to_pointcloud.cpp) 162 | target_link_libraries(laserscan_to_pointcloud ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ) 163 | 164 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11") 165 | 166 | add_executable(offline_planner_R2 167 | src/offline_planner_R2.cpp 168 | src/state_validity_checker_octomap_R2.cpp 169 | ) 170 | target_link_libraries(offline_planner_R2 ${catkin_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${CCD_LIBRARIES} ${OCTOMAP_LIBRARIES} ${OMPL_LIBRARIES}) 171 | add_dependencies(offline_planner_R2 auv_msgs_generate_messages_cpp) 172 | 173 | ## Rename C++ executable without prefix 174 | ## The above recommended prefix causes long target names, the following renames the 175 | ## target back to the shorter version for ease of user use 176 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 177 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 178 | 179 | ## Add cmake target dependencies of the executable 180 | ## same as for the library above 181 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 182 | 183 | ## Specify libraries to link a library or executable target against 184 | # target_link_libraries(${PROJECT_NAME}_node 185 | # ${catkin_LIBRARIES} 186 | # ) 187 | 188 | ############# 189 | ## Install ## 190 | ############# 191 | 192 | # all install targets should use catkin DESTINATION variables 193 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 194 | 195 | ## Mark executable scripts (Python etc.) for installation 196 | ## in contrast to setup.py, you can choose the destination 197 | # install(PROGRAMS 198 | # scripts/my_python_script 199 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 200 | # ) 201 | 202 | ## Mark executables and/or libraries for installation 203 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 204 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 205 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 206 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 207 | # ) 208 | 209 | ## Mark cpp header files for installation 210 | # install(DIRECTORY include/${PROJECT_NAME}/ 211 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 212 | # FILES_MATCHING PATTERN "*.h" 213 | # PATTERN ".svn" EXCLUDE 214 | # ) 215 | 216 | ## Mark other files for installation (e.g. launch and bag files, etc.) 217 | # install(FILES 218 | # # myfile1 219 | # # myfile2 220 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 221 | # ) 222 | 223 | ############# 224 | ## Testing ## 225 | ############# 226 | 227 | ## Add gtest based cpp test target and link libraries 228 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_autonomous_explore_map_plan.cpp) 229 | # if(TARGET ${PROJECT_NAME}-test) 230 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 231 | # endif() 232 | 233 | ## Add folders to be run by python nosetests 234 | # catkin_add_nosetests(test) 235 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Autonomous-Navigation-and-Exploration using Turtlebot 2 | 3 | This project report deals with work done for implementing and testing different highlevel con-trollers for path planning algorithms in a simulated en-vironment. The work is done in a simulated environmentand the task of autonomous navigation in an unknown environment is projected. The project is divided intotwo parts , the first step is to navigate in an unknown environment for certain time to create a map of theplaced system. The second step is to return back to the home position from any point the exploration is stopped. 4 | 5 | # Project flow 6 | 7 | Exploration is carried out as shown in the figure below. Evidence grid is created to store the probability of the corresponding region in any space. It can fuse information from different types of sensors. Cells are initialized at a prior probability withrough estimate of overall probability at any given location. Once evidence grid there, for each cell occupancy probability is compared with the initialprobability and they are classified. 8 | 9 | 10 | 11 | # Explored Regions 12 | 13 | This is exploration carried out for around 50 min in willow garage map. 14 | 15 | 16 | 17 | For returning home, this is the path taken. 18 | 19 | 20 | 21 | 22 | # Outcome 23 | 24 | The robot was able to successfully explore the environment. 25 | 26 | 27 | -------------------------------------------------------------------------------- /all.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bigmb/Autonomous-Navigation-and-Exploration/465b86c8587209f6d963bb9263846db51e292e45/all.png -------------------------------------------------------------------------------- /ex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bigmb/Autonomous-Navigation-and-Exploration/465b86c8587209f6d963bb9263846db51e292e45/ex.png -------------------------------------------------------------------------------- /ex1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bigmb/Autonomous-Navigation-and-Exploration/465b86c8587209f6d963bb9263846db51e292e45/ex1.png -------------------------------------------------------------------------------- /launch/config/list.txt: -------------------------------------------------------------------------------- 1 | 1,0,0 2 | 2,-0.5,1 3 | 3,-1,-0.2 4 | -------------------------------------------------------------------------------- /launch/config/planner_parameters.yaml: -------------------------------------------------------------------------------- 1 | planner_name : "RRT" # available planners (RRT, RRTstar, RRTConnect), (EST), (PRM, PRMstar) 2 | solving_time : 5.0 # available computation time to attempt to solve the query 3 | 4 | #Configuration Space (C-Space) boundaries 5 | planning_bounds_x : [-35, 35] 6 | planning_bounds_y : [-35, 35] 7 | 8 | # Range: Represents the maximum length of a motion to be added to the tree of motions. Posible values- From 0 to 10000 in 1 step 9 | range: 0.5 10 | 11 | -------------------------------------------------------------------------------- /launch/config/planner_parameters_return.yaml: -------------------------------------------------------------------------------- 1 | planner_name : "RRTstar" # available planners (RRT, RRTstar, RRTConnect), (EST), (PRM, PRMstar) 2 | solving_time : 20.0 # available computation time to attempt to solve the query 3 | 4 | #Configuration Space (C-Space) boundaries 5 | planning_bounds_x : [-35, 35] 6 | planning_bounds_y : [-35, 35] 7 | 8 | # Range: Represents the maximum length of a motion to be added to the tree of motions. Posible values- From 0 to 10000 in 1 step 9 | range: 5.5 10 | 11 | -------------------------------------------------------------------------------- /launch/config/robot.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 81 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Tree1 9 | - /TF2/Frames1 10 | Splitter Ratio: 0.5 11 | Tree Height: 528 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.03 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: odom 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base_footprint: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | base_link: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | Value: true 70 | camera_depth_frame: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | camera_depth_optical_frame: 75 | Alpha: 1 76 | Show Axes: false 77 | Show Trail: false 78 | camera_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | camera_rgb_frame: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | camera_rgb_optical_frame: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | caster_back_link: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | caster_front_link: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | cliff_sensor_front_link: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | cliff_sensor_left_link: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | cliff_sensor_right_link: 110 | Alpha: 1 111 | Show Axes: false 112 | Show Trail: false 113 | gyro_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | mount_asus_xtion_pro_link: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | Value: true 122 | plate_bottom_link: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | Value: true 127 | plate_middle_link: 128 | Alpha: 1 129 | Show Axes: false 130 | Show Trail: false 131 | Value: true 132 | plate_top_link: 133 | Alpha: 1 134 | Show Axes: false 135 | Show Trail: false 136 | Value: true 137 | pole_bottom_0_link: 138 | Alpha: 1 139 | Show Axes: false 140 | Show Trail: false 141 | Value: true 142 | pole_bottom_1_link: 143 | Alpha: 1 144 | Show Axes: false 145 | Show Trail: false 146 | Value: true 147 | pole_bottom_2_link: 148 | Alpha: 1 149 | Show Axes: false 150 | Show Trail: false 151 | Value: true 152 | pole_bottom_3_link: 153 | Alpha: 1 154 | Show Axes: false 155 | Show Trail: false 156 | Value: true 157 | pole_bottom_4_link: 158 | Alpha: 1 159 | Show Axes: false 160 | Show Trail: false 161 | Value: true 162 | pole_bottom_5_link: 163 | Alpha: 1 164 | Show Axes: false 165 | Show Trail: false 166 | Value: true 167 | pole_kinect_0_link: 168 | Alpha: 1 169 | Show Axes: false 170 | Show Trail: false 171 | Value: true 172 | pole_kinect_1_link: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | Value: true 177 | pole_middle_0_link: 178 | Alpha: 1 179 | Show Axes: false 180 | Show Trail: false 181 | Value: true 182 | pole_middle_1_link: 183 | Alpha: 1 184 | Show Axes: false 185 | Show Trail: false 186 | Value: true 187 | pole_middle_2_link: 188 | Alpha: 1 189 | Show Axes: false 190 | Show Trail: false 191 | Value: true 192 | pole_middle_3_link: 193 | Alpha: 1 194 | Show Axes: false 195 | Show Trail: false 196 | Value: true 197 | pole_top_0_link: 198 | Alpha: 1 199 | Show Axes: false 200 | Show Trail: false 201 | Value: true 202 | pole_top_1_link: 203 | Alpha: 1 204 | Show Axes: false 205 | Show Trail: false 206 | Value: true 207 | pole_top_2_link: 208 | Alpha: 1 209 | Show Axes: false 210 | Show Trail: false 211 | Value: true 212 | pole_top_3_link: 213 | Alpha: 1 214 | Show Axes: false 215 | Show Trail: false 216 | Value: true 217 | wheel_left_link: 218 | Alpha: 1 219 | Show Axes: false 220 | Show Trail: false 221 | Value: true 222 | wheel_right_link: 223 | Alpha: 1 224 | Show Axes: false 225 | Show Trail: false 226 | Value: true 227 | Name: RobotModel 228 | Robot Description: robot_description 229 | TF Prefix: "" 230 | Update Interval: 0 231 | Value: true 232 | Visual Enabled: true 233 | - Class: rviz/TF 234 | Enabled: false 235 | Frame Timeout: 15 236 | Frames: 237 | All Enabled: true 238 | Marker Scale: 1 239 | Name: TF 240 | Show Arrows: true 241 | Show Axes: true 242 | Show Names: true 243 | Tree: 244 | {} 245 | Update Interval: 0 246 | Value: false 247 | - Alpha: 1 248 | Autocompute Intensity Bounds: true 249 | Autocompute Value Bounds: 250 | Max Value: 0.2972 251 | Min Value: 0.2972 252 | Value: true 253 | Axis: Z 254 | Channel Name: intensity 255 | Class: rviz/LaserScan 256 | Color: 255; 255; 255 257 | Color Transformer: AxisColor 258 | Decay Time: 0 259 | Enabled: false 260 | Invert Rainbow: false 261 | Max Color: 255; 255; 255 262 | Max Intensity: 4096 263 | Min Color: 0; 0; 0 264 | Min Intensity: 0 265 | Name: LaserScan 266 | Position Transformer: XYZ 267 | Queue Size: 10 268 | Selectable: true 269 | Size (Pixels): 3 270 | Size (m): 0.01 271 | Style: Squares 272 | Topic: /scan 273 | Unreliable: false 274 | Use Fixed Frame: true 275 | Use rainbow: true 276 | Value: false 277 | - Alpha: 1 278 | Auto Size: 279 | Auto Size Factor: 1 280 | Value: true 281 | Autocompute Intensity Bounds: true 282 | Autocompute Value Bounds: 283 | Max Value: 3.286 284 | Min Value: 0.683 285 | Value: true 286 | Axis: X 287 | Channel Name: intensity 288 | Class: rviz/DepthCloud 289 | Color: 255; 255; 255 290 | Color Image Topic: "" 291 | Color Transformer: AxisColor 292 | Color Transport Hint: raw 293 | Decay Time: 0 294 | Depth Map Topic: /camera/depth/image_raw 295 | Depth Map Transport Hint: raw 296 | Enabled: false 297 | Invert Rainbow: false 298 | Max Color: 255; 255; 255 299 | Max Intensity: 4096 300 | Min Color: 0; 0; 0 301 | Min Intensity: 0 302 | Name: DepthCloud 303 | Occlusion Compensation: 304 | Occlusion Time-Out: 30 305 | Value: false 306 | Position Transformer: XYZ 307 | Queue Size: 5 308 | Selectable: true 309 | Size (Pixels): 3 310 | Style: Points 311 | Topic Filter: true 312 | Use Fixed Frame: true 313 | Use rainbow: true 314 | Value: false 315 | - Alpha: 1 316 | Auto Size: 317 | Auto Size Factor: 1 318 | Value: true 319 | Autocompute Intensity Bounds: true 320 | Autocompute Value Bounds: 321 | Max Value: 10 322 | Min Value: -10 323 | Value: true 324 | Axis: Z 325 | Channel Name: intensity 326 | Class: rviz/DepthCloud 327 | Color: 255; 255; 255 328 | Color Image Topic: /camera/rgb/image_color 329 | Color Transformer: RGB8 330 | Color Transport Hint: raw 331 | Decay Time: 0 332 | Depth Map Topic: /camera/depth_registered/image_raw 333 | Depth Map Transport Hint: raw 334 | Enabled: false 335 | Invert Rainbow: false 336 | Max Color: 255; 255; 255 337 | Max Intensity: 4096 338 | Min Color: 0; 0; 0 339 | Min Intensity: 0 340 | Name: Registered DepthCloud 341 | Occlusion Compensation: 342 | Occlusion Time-Out: 30 343 | Value: false 344 | Position Transformer: XYZ 345 | Queue Size: 5 346 | Selectable: true 347 | Size (Pixels): 3 348 | Style: Points 349 | Topic Filter: true 350 | Use Fixed Frame: true 351 | Use rainbow: true 352 | Value: false 353 | - Class: rviz/Image 354 | Enabled: false 355 | Image Topic: /camera/rgb/image_color 356 | Max Value: 1 357 | Median window: 5 358 | Min Value: 0 359 | Name: Image 360 | Normalize Range: true 361 | Queue Size: 2 362 | Transport Hint: raw 363 | Unreliable: false 364 | Value: false 365 | - Alpha: 1 366 | Autocompute Intensity Bounds: true 367 | Autocompute Value Bounds: 368 | Max Value: 3.286 369 | Min Value: 0.681 370 | Value: true 371 | Axis: X 372 | Channel Name: intensity 373 | Class: rviz/PointCloud2 374 | Color: 255; 255; 255 375 | Color Transformer: AxisColor 376 | Decay Time: 0 377 | Enabled: false 378 | Invert Rainbow: false 379 | Max Color: 255; 255; 255 380 | Max Intensity: 4096 381 | Min Color: 0; 0; 0 382 | Min Intensity: 0 383 | Name: PointCloud 384 | Position Transformer: XYZ 385 | Queue Size: 10 386 | Selectable: true 387 | Size (Pixels): 3 388 | Size (m): 0.01 389 | Style: Points 390 | Topic: /camera/depth/points 391 | Unreliable: false 392 | Use Fixed Frame: true 393 | Use rainbow: true 394 | Value: false 395 | - Alpha: 1 396 | Autocompute Intensity Bounds: true 397 | Autocompute Value Bounds: 398 | Max Value: 5.632 399 | Min Value: 0.65 400 | Value: true 401 | Axis: X 402 | Channel Name: intensity 403 | Class: rviz/PointCloud2 404 | Color: 255; 255; 255 405 | Color Transformer: RGB8 406 | Decay Time: 0 407 | Enabled: false 408 | Invert Rainbow: false 409 | Max Color: 255; 255; 255 410 | Max Intensity: 4096 411 | Min Color: 0; 0; 0 412 | Min Intensity: 0 413 | Name: Registered PointCloud 414 | Position Transformer: XYZ 415 | Queue Size: 10 416 | Selectable: true 417 | Size (Pixels): 3 418 | Size (m): 0.01 419 | Style: Points 420 | Topic: /camera/depth_registered/points 421 | Unreliable: false 422 | Use Fixed Frame: true 423 | Use rainbow: true 424 | Value: false 425 | - Class: rviz/InteractiveMarkers 426 | Enable Transparency: true 427 | Enabled: false 428 | Name: InteractiveMarkers 429 | Show Axes: false 430 | Show Descriptions: true 431 | Show Visual Aids: false 432 | Update Topic: /turtlebot_marker_server/update 433 | Value: false 434 | - Class: rviz/MarkerArray 435 | Enabled: true 436 | Marker Topic: /occupied_cells_vis_array 437 | Name: Octomap 438 | Namespaces: 439 | map: true 440 | Queue Size: 100 441 | Value: true 442 | - Class: rviz/Marker 443 | Enabled: true 444 | Marker Topic: /controller_turtlebot/solution_path 445 | Name: Planning Solution 446 | Namespaces: 447 | online_planner_result_path: true 448 | online_planner_rrt: true 449 | Queue Size: 100 450 | Value: true 451 | - Class: rviz/TF 452 | Enabled: false 453 | Frame Timeout: 15 454 | Frames: 455 | All Enabled: false 456 | Marker Scale: 1 457 | Name: TF 458 | Show Arrows: true 459 | Show Axes: true 460 | Show Names: true 461 | Tree: 462 | {} 463 | Update Interval: 0 464 | Value: false 465 | - Alpha: 0.7 466 | Class: rviz/Map 467 | Color Scheme: map 468 | Draw Behind: false 469 | Enabled: true 470 | Name: Map 471 | Topic: /projected_map 472 | Unreliable: false 473 | Value: true 474 | Enabled: true 475 | Global Options: 476 | Background Color: 48; 48; 48 477 | Fixed Frame: odom 478 | Frame Rate: 30 479 | Name: root 480 | Tools: 481 | - Class: rviz/MoveCamera 482 | - Class: rviz/Interact 483 | Hide Inactive Objects: true 484 | - Class: rviz/Select 485 | - Class: rviz/SetInitialPose 486 | Topic: /initialpose 487 | - Class: rviz/SetGoal 488 | Topic: /move_base_simple/goal 489 | Value: true 490 | Views: 491 | Current: 492 | Class: rviz/Orbit 493 | Distance: 12.0523 494 | Enable Stereo Rendering: 495 | Stereo Eye Separation: 0.06 496 | Stereo Focal Distance: 1 497 | Swap Stereo Eyes: false 498 | Value: false 499 | Focal Point: 500 | X: -0.102786 501 | Y: -0.0889391 502 | Z: 0.112978 503 | Name: Current View 504 | Near Clip Distance: 0.01 505 | Pitch: 0.624798 506 | Target Frame: 507 | Value: Orbit (rviz) 508 | Yaw: 1.1954 509 | Saved: ~ 510 | Window Geometry: 511 | Displays: 512 | collapsed: false 513 | Height: 744 514 | Hide Left Dock: false 515 | Hide Right Dock: false 516 | Image: 517 | collapsed: false 518 | QMainWindow State: 000000ff00000000fd000000040000000000000195000002a2fc0200000006fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002a2000000dd00fffffffb0000000a0049006d006100670065000000028d000000c60000001600ffffff000000010000010b00000294fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003300000294000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650000000000000004a0000002f600fffffffb0000000800540069006d00650100000000000004500000000000000000000003bb000002a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 519 | Selection: 520 | collapsed: false 521 | Time: 522 | collapsed: false 523 | Tool Properties: 524 | collapsed: false 525 | Views: 526 | collapsed: false 527 | Width: 1366 528 | X: 0 529 | Y: 24 530 | -------------------------------------------------------------------------------- /launch/driver.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 | -------------------------------------------------------------------------------- /launch/start_turtlebot_mapping_planning_control.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 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /launch/start_turtlebot_modules.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 | -------------------------------------------------------------------------------- /launch/test_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /launch/view_robot.launch: -------------------------------------------------------------------------------- 1 | 6 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autonomous_explore_map_plan 4 | 0.0.0 5 | The autonomous_explore_map_plan package 6 | 7 | 8 | 9 | 10 | abdelrahman 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 | gazebo_msgs 47 | roscpp 48 | rospy 49 | std_msgs 50 | gazebo_msgs 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /package.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | autonomous_explore_map_plan 4 | 0.0.0 5 | The autonomous_explore_map_plan package 6 | 7 | 8 | 9 | 10 | abdelrahman 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 | -------------------------------------------------------------------------------- /src/MapProcessing.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | import matplotlib.image as mpimg 5 | import scipy.io as sio 6 | import scipy.ndimage as ndimage 7 | import rospy 8 | import time 9 | import math 10 | from nav_msgs.msg import Odometry 11 | from nav_msgs.msg import OccupancyGrid 12 | from autonomous_explore_map_plan.srv import GotoWaypoint, GotoWaypointRequest, GotoWaypointResponse 13 | np.set_printoptions(threshold='nan') 14 | 15 | 16 | class ProcessMap(object): 17 | def __init__(self): 18 | self.odometry_sub_ = rospy.Subscriber("/odom",Odometry, self.OdometryCallback) 19 | self.current_position = np.zeros(2) 20 | 21 | # get the map >> call ProcessProjectedMap() 22 | self.map_sub_ = rospy.Subscriber("/projected_map", OccupancyGrid, self.OccupancyGridCallback, queue_size = 1) 23 | 24 | # Use RRT* to go to best point 25 | rospy.wait_for_service('/turtlebot_drive/goto') 26 | try: 27 | self.goto_serv_ = rospy.ServiceProxy('/turtlebot_drive/goto', GotoWaypoint) 28 | except rospy.ServiceException, e: 29 | print "Goto Service call failed: %s"%e 30 | return 31 | 32 | 33 | def OccupancyGridCallback(self, msg): 34 | self.dat = msg.data 35 | self.wid = msg.info.width 36 | self.heigh = msg.info.height 37 | self.res = msg.info.resolution 38 | self.xorg = msg.info.origin.position.x 39 | self.yorg = msg.info.origin.position.y 40 | 41 | def OdometryCallback(self, msg): 42 | self.current_position[0] = msg.pose.pose.position.x 43 | self.current_position[1] = msg.pose.pose.position.y 44 | 45 | 46 | def FindBestPoint(self, BrushMap): 47 | rows = BrushMap.shape[0] 48 | cols = BrushMap.shape[1] 49 | print(rows) 50 | print(cols) 51 | 52 | NumUnknownElems = np.count_nonzero(BrushMap == -1) 53 | NumWallElems = np.count_nonzero(BrushMap == 100) 54 | #best = 109 55 | #NumbestElems = np.count_nonzero(BrushMap == best) 56 | 57 | 58 | if (NumWallElems > 1): 59 | BestGazeboArray = self.WallsFreeUnknown(BrushMap) 60 | 61 | #return bestPoint, BestGazebo 62 | return BestGazeboArray 63 | 64 | 65 | def WallsFreeUnknown(self, BrushMap): 66 | rows = BrushMap.shape[0] 67 | cols = BrushMap.shape[1] 68 | # this gives you the highest value 69 | #best=np.amax(BrushMap) 70 | #print(best) 71 | best = 106 72 | 73 | if best > np.amax(BrushMap): 74 | best = np.amax(BrushMap) 75 | 76 | window = 10 77 | index=[] 78 | # finding indeces of maximum values 79 | for k in range(window, rows-window+1): 80 | for j in range(window, cols-window+1): 81 | if BrushMap[k][j]==best: 82 | inr=[k,j] 83 | index.append(inr) 84 | 85 | index = np.asarray(index) 86 | 87 | N = [] 88 | for i in range(index.shape[0]): 89 | n = 0 90 | for k in range(-window, window): 91 | for j in range(-window, window): 92 | if BrushMap[index[i][0]+k][index[i][1]+j] == -1: 93 | n=n+1 94 | N.append(n) 95 | 96 | N = np.asarray(N) 97 | #max_value = max(N) 98 | #max_index = N.index(max_value) 99 | 100 | num = 20 101 | if index.shape[0] < num: 102 | num = index.shape[0] 103 | 104 | # getting indeces of best 5 points 105 | 106 | Nlargest = N.argsort()[-num:] 107 | Nlargest = np.asarray(Nlargest) 108 | 109 | SmallestDist = [] 110 | for m in range(Nlargest.shape[0]): 111 | #print('Brushmap indeces: ') 112 | #print( np.asarray([index[Nlargest[m]][0], index[Nlargest[m]][1]]) ) 113 | dist = self.dist_to_bestpoint_xy(np.asarray([index[Nlargest[m]][0], index[Nlargest[m]][1]]) ) 114 | SmallestDist.append(dist) 115 | 116 | # sort the array by the smallest dist to robot 117 | SmallestDist = np.asarray(SmallestDist) 118 | DistSorted = SmallestDist.argsort() 119 | 120 | print(SmallestDist) 121 | print(DistSorted) 122 | 123 | BestGazeboArray = np.zeros((num,2))*1.0 124 | 125 | for z in range(DistSorted.shape[0]): 126 | max_index = Nlargest[DistSorted[z]] 127 | #max_value = max(N) 128 | #max_index = N.index(max_value) 129 | bestPoint=[[index[max_index][0],index[max_index][1]]] 130 | BestGazebo=[[float(index[max_index][0]*self.res+self.xorg),float(index[max_index][1]*self.res+self.yorg)]] 131 | BestGazeboArray[z][0] = BestGazebo[0][0] 132 | BestGazeboArray[z][1] = BestGazebo[0][1] 133 | 134 | 135 | #print('Best point is ' ) 136 | #print(bestPoint) 137 | print('best points in Gazebo is') 138 | print(BestGazeboArray) 139 | #bestPoint = np.array(bestPoint) 140 | BestGazebo = np.array(BestGazebo) 141 | 142 | return BestGazeboArray 143 | #return bestPoint, BestGazebo 144 | 145 | def dist_to_bestpoint_xy(self, desired_position ): 146 | # computes the distance in x and y direction to the given goal 147 | 148 | return math.sqrt(pow(self.current_position[0]-desired_position[0],2)+pow(self.current_position[1]-desired_position[1],2)) 149 | 150 | 151 | 152 | 153 | 154 | def brushfire(self, map1): 155 | #print(map1) 156 | 157 | rows = map1.shape[0] 158 | cols = map1.shape[1] 159 | num_zeros = (map1 == 0).sum() 160 | print('size of map start brushfire: ') 161 | print(map1.shape) 162 | 163 | k=100; #Obstacle value 164 | while num_zeros >0 : 165 | for i in range(rows): 166 | for j in range(cols): 167 | if map1[i][j]==k: 168 | if (i-1)>=0 and (i+1)=0 and (j+1)=0 and j-1>=0: 194 | if map1[i-1][j-1]==0: 195 | map1[i-1][j-1]=k+1 196 | num_zeros=num_zeros-1 197 | elif i-1>=0: 198 | if map1[i-1][j]==0: 199 | map1[i-1][j]=k+1 200 | num_zeros=num_zeros-1 201 | elif i-1>=0 and j+1=0: 206 | if map1[i][j-1]==0: 207 | map1[i][j-1]=k+1 208 | num_zeros=num_zeros-1 209 | elif j+1=0: 214 | if map1[i+1][j-1]==0: 215 | map1[i+1][j-1]=k+1 216 | num_zeros=num_zeros-1 217 | elif i+10: 284 | for i in range(rows): 285 | for j in range(cols): 286 | if map2[i][j]==k: 287 | 288 | if i-1>=0 and i+1=0 and j+1=0 and j-1>=0: 315 | if map2[i-1][j-1]==0: 316 | map2[i-1][j-1]=k+1 317 | num_zeros=num_zeros-1 318 | elif i-1>=0: 319 | if map2[i-1][j]==0: 320 | map2[i-1][j]=k+1 321 | num_zeros=num_zeros-1 322 | elif i-1>=0 and j+1=0: 327 | if map2[i][j-1]==0: 328 | map2[i][j-1]=k+1 329 | num_zeros=num_zeros-1 330 | elif j+1=0: 335 | if map2[i+1][j-1]==0: 336 | map2[i+1][j-1]=k+1 337 | num_zeros=num_zeros-1 338 | elif i+11: 362 | cr=cr-1 363 | point=[cr,cc] 364 | traject.append(point) 365 | elif map2[cr+1][cc]1: 366 | cr=cr+1 367 | point=[cr,cc] 368 | traject.append(point) 369 | elif map2[cr][cc-1]1: 370 | cc=cc-1 371 | point=[cr,cc] 372 | traject.append(point) 373 | elif map2[cr][cc+1]1: 374 | cc=cc+1 375 | point=[cr,cc] 376 | traject.append(point) 377 | elif map2[cr-1][cc-1]1: 378 | cr=cr-1 379 | cc=cc-1 380 | point=[cr,cc] 381 | traject.append(point) 382 | elif map2[cr-1][cc+1]1: 383 | cr=cr-1 384 | cc=cc-1 385 | point=[cr,cc] 386 | traject.append(point) 387 | elif map2[cr+1][cc-1]1: 388 | cr=cr+1 389 | cc=cc-1 390 | point=[cr,cc] 391 | traject.append(point) 392 | elif map2[cr+1][cc+1]1: 393 | cr=cr+1 394 | cc=cc-1 395 | point=[cr,cc] 396 | traject.append(point) 397 | point=[goal_row,goal_col] 398 | traject.append(point) 399 | 400 | print('You Finished') 401 | 402 | traject=np.asarray(traject) 403 | print(traject) 404 | 405 | return map2,traject 406 | 407 | 408 | def ProcessProjectedMap(self): 409 | # get map 410 | # reshape the map to the given dimensions 411 | rospy.sleep(1) 412 | data1 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 413 | data2 = np.asarray(data1) 414 | print('sahpe of map is:') 415 | print(data2.shape) 416 | 417 | StartPointWF=np.array([[int(math.floor((self.current_position[0]-self.xorg)/self.res)),int(math.floor((self.current_position[1]-self.yorg)/self.res))]]) 418 | 419 | 420 | # Dilation 421 | data3 = ndimage.grey_dilation(data2, footprint=np.ones((9,9))) 422 | #data3 = data2 423 | #print(type(data2)) 424 | #print(data2.shape 425 | 426 | 427 | # Call brushfire 428 | #map1 = self.brushfire(data3) 429 | print('size of map before brushfire: ') 430 | print(data3.shape) 431 | print('start point is:') 432 | print(StartPointWF[0][0]) 433 | print(StartPointWF[0][1]) 434 | 435 | WS = 20 436 | if (StartPointWF[0][0] <= WS or StartPointWF[0][1] <= WS): 437 | map1 = self.brushfire(data3) 438 | else: 439 | map1 = self.brushfire(data3) 440 | 441 | #map1 = self.brushfire(data3[StartPointWF[0]-WS : StartPointWF[0]+WS][StartPointWF[1]-WS : StartPointWF[1]+WS]) 442 | print('size of map after brushfire: ') 443 | print(map1.shape) 444 | #[value_m, BestGazebo, map1] = self.brushfire(data3[StartPointWF[0]-100:StartPointWF[0]+100][StartPointWF[1]-100:StartPointWF[1]+100]) 445 | 446 | #[value_m, BestGazebo] = self.FindBestPoint(map1) 447 | BestGazeboArray = self.FindBestPoint(map1) 448 | 449 | #GoalPoint = BestGazebo 450 | 451 | ''' 452 | print(value_m.shape) 453 | data4 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 454 | data5 = np.asarray(data4) 455 | data6 = ndimage.grey_dilation(data5, footprint=np.ones((15,15))) 456 | GoalPoint = np.array([BestGazebo]) 457 | [map2,traject]=self.wavefront(data6,StartPointWF,value_m) 458 | print('traject shape') 459 | print(traject.shape) 460 | traject=np.asarray(traject) 461 | rows_traj = traject.shape[0] 462 | cols_traj = traject.shape[1] 463 | map2[traject[0][0]][traject[0][1]]=10 464 | for i in range (1,rows_traj-2): 465 | map2[traject[i][0]][traject[i][1]]=120 466 | map2[traject[rows_traj-1][0]][traject[rows_traj-1][1]]=130 467 | plt.imshow(map2) 468 | plt.show() 469 | 470 | trajectoryGazebo=np.zeros((rows_traj,2)) 471 | 472 | for i in range (rows_traj): 473 | trajectoryGazebo[i][0]=float(traject[i][0]**self.res+self.xorg) 474 | trajectoryGazebo[i][1]=float(traject[i][1]**self.res+self.yorg) 475 | print(trajectoryGazebo) 476 | #print(value_m) 477 | #print(GoalPoint.shape) 478 | ''' 479 | 480 | # Send request to /goto service 481 | 482 | for i in range(BestGazeboArray.shape[0]): 483 | print('got the point ... finding a path to point : ') 484 | print(BestGazeboArray[i][0]) 485 | print(BestGazeboArray[i][1]) 486 | goto_request = GotoWaypointRequest() 487 | goto_request.goal_state_x = round(BestGazeboArray[i][0], 1) 488 | goto_request.goal_state_y = round(BestGazeboArray[i][1], 1) 489 | 490 | goto_response = self.goto_serv_(goto_request) 491 | 492 | if(goto_response == 1): 493 | break 494 | 495 | 496 | 497 | 498 | #data1 = np.reshape(data,(155,120), order="F") 499 | #data2 = np.asarray(data1) 500 | #data3 = ndimage.grey_dilation(data2, footprint=np.ones((7,7))) 501 | 502 | # wavefront algorithm 503 | #[value_map, traject] = self.wavefront(data3, np.array([self.current_position]), GoalPoint) 504 | #traject=np.asarray(traject) 505 | #print(traject) 506 | #rows_traj=traject.shape[0] 507 | #cols_traj=traject.shape[1] 508 | #value_map[traject[0][0]][traject[0][1]]=110 509 | 510 | #for i in range (1,rows_traj-2): 511 | # value_map[traject[i][0]][traject[i][1]]=120 512 | 513 | #value_map[traject[rows_traj-1][0]][traject[rows_traj-1][1]]=130 514 | #plt.imshow(value_map) 515 | #plt.show() 516 | #rospy.spin() 517 | 518 | 519 | #if __name__ == "__main__": 520 | # pm = ProcessMap() 521 | # pm.ProcessProjectedMap() 522 | #main() 523 | 524 | -------------------------------------------------------------------------------- /src/MapProcessing.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bigmb/Autonomous-Navigation-and-Exploration/465b86c8587209f6d963bb9263846db51e292e45/src/MapProcessing.pyc -------------------------------------------------------------------------------- /src/controller_turtlebot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib; roslib.load_manifest('autonomous_explore_map_plan') 5 | import rospy 6 | import tf 7 | import math 8 | 9 | #ROS messages 10 | from geometry_msgs.msg import Twist 11 | from nav_msgs.msg import Odometry 12 | from autonomous_explore_map_plan.srv import GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest 13 | 14 | #Numpy 15 | import numpy as np 16 | 17 | class Controller(object): 18 | 19 | def __init__(self): 20 | self.odometry_sub_ = rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size = 1) 21 | self.control_input_pub_ = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size = 1) 22 | 23 | self.serv_ = rospy.Service('/controller_turtlebot/goto', 24 | GotoWaypoint, 25 | self.calculateControlInput) 26 | 27 | self.current_position_ = np.zeros(2) 28 | self.current_orientation_ = 0.0 29 | 30 | self.desired_position_ = np.zeros(2) 31 | self.desired_orientation_ = 0.0 32 | 33 | 34 | rospy.wait_for_service('/controller_turtlebot/find_path_to_goal') 35 | try: 36 | self.find_path_to_goal_serv_ = rospy.ServiceProxy('/controller_turtlebot/find_path_to_goal', FindPathToGoal) 37 | except rospy.ServiceException, e: 38 | print "Service call failed: %s"%e 39 | 40 | return 41 | 42 | def calculateControlInput(self, req): 43 | # define a request for FindPathToGoal service 44 | planner_request = FindPathToGoalRequest() 45 | planner_request.goal_state_x = req.goal_state_x 46 | planner_request.goal_state_y = req.goal_state_y 47 | 48 | planner_response = self.find_path_to_goal_serv_(planner_request) 49 | 50 | # Driver for the robot 51 | for pose in planner_response.poses: 52 | #print pose 53 | control_input = Twist() 54 | self.desired_position_[0] = pose.x 55 | self.desired_position_[1] = pose.y 56 | 57 | loop_rate = rospy.Rate(100) # 10Hz 58 | orientation_approach = False 59 | while not rospy.is_shutdown(): 60 | inc_x = self.desired_position_[0] - self.current_position_[0] 61 | inc_y = self.desired_position_[1] - self.current_position_[1] 62 | 63 | self.desired_orientation_ = wrapAngle(math.atan2(inc_y, inc_x)) 64 | yaw_error = wrapAngle(self.desired_orientation_ - self.current_orientation_) 65 | distance_to_goal = math.sqrt(math.pow(inc_x, 2.0) + math.pow(inc_y, 2.0)) 66 | 67 | if abs(yaw_error) > 0.04 and not orientation_approach: 68 | control_input.angular.x = 0.0 69 | control_input.angular.y = 0.0 70 | control_input.angular.z = yaw_error * 1.0 71 | 72 | control_input.linear.x = 0.08 73 | control_input.linear.y = 0.0 74 | control_input.linear.z = 0.0 75 | else: 76 | control_input.angular.x = 0.0 77 | control_input.angular.y = 0.0 78 | control_input.angular.z = 0.0 79 | 80 | orientation_approach = True 81 | liner_speed = abs(distance_to_goal) * 0.5 82 | 83 | if liner_speed < 0.1: 84 | control_input.linear.x = 0.1 85 | elif liner_speed > 0.2: 86 | control_input.linear.x = 0.2 87 | else: 88 | control_input.linear.x = liner_speed 89 | 90 | control_input.linear.y = 0.0 91 | control_input.linear.z = 0.0 92 | 93 | self.control_input_pub_.publish(control_input) 94 | 95 | rospy.logdebug("%s: current position: [%f, %f]\n", rospy.get_name(), self.current_position_[0], self.current_position_[1]) 96 | rospy.logdebug("%s: desired position: [%f, %f]\n", rospy.get_name(), self.desired_position_[0], self.desired_position_[1]) 97 | rospy.logdebug("%s: yaw_error: %f\n", rospy.get_name(), yaw_error) 98 | rospy.logdebug("%s: current orientation: %f\n", rospy.get_name(), self.current_orientation_) 99 | rospy.logdebug("%s: desired orientation: %f\n", rospy.get_name(), self.desired_orientation_) 100 | rospy.logdebug("%s: distance_to_goal: %f\n", rospy.get_name(), distance_to_goal) 101 | rospy.logdebug("%s: control_input.linear.x %f\n", rospy.get_name(), control_input.linear.x) 102 | rospy.logdebug("%s: control_input.angular.z %f\n", rospy.get_name(), control_input.angular.z) 103 | 104 | if distance_to_goal <= 0.4: 105 | break 106 | loop_rate.sleep() 107 | 108 | return GotoWaypointResponse() 109 | 110 | def odomCallback(self, odometry_msg): 111 | self.current_position_[0] = odometry_msg.pose.pose.position.x 112 | self.current_position_[1] = odometry_msg.pose.pose.position.y 113 | (r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w]) 114 | self.current_orientation_ = wrapAngle(y) 115 | return 116 | 117 | def wrapAngle(angle): 118 | """wrapAngle 119 | 120 | Calculates angles values between 0 and 2pi""" 121 | return (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) ) 122 | 123 | if __name__ == '__main__': 124 | rospy.init_node('control_turtlebot', log_level=rospy.INFO) 125 | rospy.loginfo("%s: starting turtlebot controller", rospy.get_name()) 126 | 127 | controller = Controller() 128 | rospy.spin() 129 | -------------------------------------------------------------------------------- /src/driver.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS general imports 4 | import roslib 5 | roslib.load_manifest('autonomous_explore_map_plan') 6 | import rospy 7 | 8 | # Other imoprts 9 | import math #math library 10 | import numpy as np #numpy library 11 | from probabilistic_lib.functions import angle_wrap #Normalize angles between -pi and pi 12 | 13 | #import the library to read csv 14 | import csv 15 | 16 | #import the library to compute transformations 17 | from tf.transformations import euler_from_quaternion 18 | 19 | #ROS messages 20 | from gazebo_msgs.msg import ModelStates 21 | from geometry_msgs.msg import Twist 22 | 23 | 24 | class driver(object): 25 | 26 | def __init__(self): 27 | ''' 28 | constructor 29 | ''' 30 | #Initialize ros node 31 | rospy.init_node('turtlebot_driver') 32 | 33 | #Initialize goals 34 | self.x = np.array([]) 35 | self.y = np.array([]) 36 | self.theta = np.array([]) 37 | 38 | #Threshold for distance to goal 39 | self.goal_th_xy = rospy.get_param('goal_thershold_xy',0.1) #Position threshold 40 | self.goal_th_ang = rospy.get_param('goal_threshold_ang',0.01) #Orientation threshold 41 | 42 | #Point to the first goal 43 | self.active_goal = 0 44 | 45 | #Define subscriber 46 | self.sub = rospy.Subscriber('/gazebo/model_states', ModelStates, self.callback) 47 | 48 | #Define publisher either /mobile_base/commands/velocity or /cmd_vel_mux/input/teleop 49 | self.pub = rospy.Publisher('/mobile_base/commands/velocity', Twist, queue_size=10) 50 | 51 | #Define the velocity message 52 | self.vmsg = Twist() 53 | 54 | #Has the goal been loaded? 55 | self.params_loaded = False 56 | 57 | def print_goals(self): 58 | ''' 59 | print_goals prints the list of goals 60 | ''' 61 | rospy.loginfo("List of goals:") 62 | rospy.loginfo("X:\t " + str(self.x)) 63 | rospy.loginfo("Y:\t " + str(self.y)) 64 | rospy.loginfo("Theta:\t " + str(self.theta)) 65 | 66 | def print_goal(self): 67 | ''' 68 | pritn_goal prints the next goal 69 | ''' 70 | rospy.loginfo( "Goal: (" + str(self.x[self.active_goal]) + " , " + str(self.y[self.active_goal]) + " , " + str(self.theta[self.active_goal]) + ")") 71 | 72 | def print_pose(self): 73 | ''' 74 | print_pose pints the robot's actuall position 75 | ''' 76 | rospy.loginfo( "Pose: (" + str(self.position_x) + " , " + str(self.position_y) + " , " + str(self.position_theta) + " )") 77 | 78 | def print_goal_and_pose(self): 79 | ''' 80 | pritn_goal_and_pose prints both goal and pose 81 | ''' 82 | rospy.loginfo("\tPose\t\tGoal") 83 | rospy.loginfo("X:\t%f\t%f",self.position_x,self.x[self.active_goal]) 84 | rospy.loginfo("Y:\t%f\t%f",self.position_y,self.y[self.active_goal]) 85 | rospy.loginfo("A:\t%f\t%f",self.position_theta,self.theta[self.active_goal]) 86 | 87 | def dist_to_goal_xy(self): 88 | ''' 89 | dist_to_goal_xy computes the distance in x and y direction to the 90 | active goal 91 | ''' 92 | return math.sqrt(pow(self.position_x-self.x[self.active_goal],2)+pow(self.position_y-self.y[self.active_goal],2)) 93 | 94 | def dist_to_goal_ang(self): 95 | ''' 96 | dist_to_goal_ang computes the orientation distance to the active 97 | goal 98 | ''' 99 | return angle_wrap(self.theta[self.active_goal]-self.position_theta) 100 | 101 | def has_arrived_xy(self): 102 | ''' 103 | has_arrived_xy returns true if the xy distance to the ative goal is 104 | smaller than the position threshold 105 | ''' 106 | return self.dist_to_goal_xy()= len(self.x)): 183 | print('Final goal reached!') 184 | rospy.signal_shutdown('Final goal reached!') 185 | else: 186 | self.print_goal() 187 | 188 | def read_position(self,msg): 189 | ''' 190 | read_position copy the position received in the message (msg) to the 191 | internal class variables self.position_x, self.position_y and 192 | self.position_theta 193 | 194 | Tip: in order to convert from quaternion to euler angles give a look 195 | at tf.transformations 196 | ''' 197 | 198 | #Find index of mobile_base 199 | mobile_base_idx = -1 200 | for i in range(len(msg.name)): 201 | if msg.name[i] == 'mobile_base': 202 | mobile_base_idx = i 203 | break 204 | 205 | #Update position_x and position_y 206 | self.position_x = msg.pose[mobile_base_idx].position.x 207 | self.position_y = msg.pose[mobile_base_idx].position.y 208 | 209 | #Get euler angles from quaternion 210 | (ang_x, ang_y, ang_z) = euler_from_quaternion([msg.pose[mobile_base_idx].orientation.x, 211 | msg.pose[mobile_base_idx].orientation.y, 212 | msg.pose[mobile_base_idx].orientation.z, 213 | msg.pose[mobile_base_idx].orientation.w]) 214 | #Update theta with euler_z 215 | self.position_theta = angle_wrap(ang_z) 216 | 217 | 218 | def compute_velocity(self): 219 | ''' 220 | compute_velocity computes the velocity which will be published. 221 | 222 | TODO implement! 223 | ''' 224 | self.vmsg=Twist() 225 | 226 | if self.dist_to_goal_xy() > self.goal_th_xy: 227 | # Compute delta_angle 228 | angle_to_goal = math.atan2(self.y[self.active_goal] - self.position_y, self.x[self.active_goal]-self.position_x) 229 | delta_angle_to_goal = angle_wrap(self.position_theta - angle_to_goal) 230 | 231 | # Assign angular velocity as 2*delta_angle 232 | self.vmsg.angular.z = -delta_angle_to_goal 233 | 234 | # If delta_angle small (facing obstacle, we can move) 235 | if np.abs(delta_angle_to_goal) < 0.05: 236 | self.vmsg.linear.x = min(0.5, 2*self.dist_to_goal_xy()) 237 | else: 238 | self.vmsg.angular.z = -self.dist_to_goal_ang() 239 | 240 | 241 | -------------------------------------------------------------------------------- /src/explore.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib; roslib.load_manifest('autonomous_explore_map_plan') 5 | import rospy 6 | import tf 7 | import math 8 | 9 | #ROS messages 10 | from geometry_msgs.msg import Twist 11 | from nav_msgs.msg import Odometry 12 | from autonomous_explore_map_plan.srv import GotoWaypoint, GotoWaypointResponse, GotoWaypointRequest, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest 13 | 14 | #Numpy 15 | import numpy as np 16 | 17 | from MapProcessing import ProcessMap 18 | from turtlebot_drive import Controller 19 | from turtlebot_return import Returner 20 | 21 | 22 | global initial_position_ 23 | initial_position_ = np.zeros(2) 24 | 25 | 26 | def odomCallback(odometry_msg): 27 | global initial_position_ 28 | initial_position_[0] = odometry_msg.pose.pose.position.x 29 | initial_position_[1] = odometry_msg.pose.pose.position.y 30 | return 31 | 32 | 33 | if __name__ == '__main__': 34 | rospy.init_node('explore_node', log_level=rospy.INFO) 35 | rospy.loginfo("%s: starting turtlebot exploration", rospy.get_name()) 36 | 37 | #controller = Controller() 38 | #print (str(controller.current_orientation_)) 39 | #while not rospy.is_shutdown(): 40 | #controller.rotateOnce() 41 | 42 | # get map from controller through service call 43 | #self.map_sub_ = rospy.Subscriber("/projected_map", OccupancyGrid, self.occupCallback, queue_size = 1) 44 | 45 | # pass the map and its resolution to ProcessProjectedMap() 46 | global initial_position_ 47 | odometry_sub_ = rospy.Subscriber("/odom", Odometry, odomCallback, queue_size = 1) 48 | rospy.sleep(10) 49 | 50 | initial_position_local = np.zeros(2) 51 | initial_position_local[0] = initial_position_[0] 52 | initial_position_local[1] = initial_position_[1] 53 | 54 | 55 | print ('before return service') 56 | rospy.wait_for_service('/turtlebot_return/goto') 57 | try: 58 | goto_serv_ = rospy.ServiceProxy('/turtlebot_return/goto', GotoWaypoint) 59 | except rospy.ServiceException, e: 60 | print "Goto Service call failed: %s"%e 61 | 62 | 63 | print ('after return service') 64 | rospy.sleep(10) 65 | start_time = rospy.Time.now() 66 | 67 | three_min_timeout = rospy.Duration(240) 68 | #while rospy.Time.now() - start_time < three_min_timeout: 69 | #print (str(rospy.Time.now() - start_time)) 70 | 71 | while rospy.Time.now() - start_time < three_min_timeout: 72 | 73 | pm = ProcessMap() 74 | pm.ProcessProjectedMap() 75 | print ('enter loop again') 76 | #ProcessMap().ProcessProjectedMap() 77 | 78 | print ('exit out explore, begin to return') 79 | 80 | 81 | 82 | while True: 83 | print('in explorer node') 84 | print('returning to ' + 'x:'+str(initial_position_local[0])) 85 | print('returning to ' + 'y:'+str(initial_position_local[1])) 86 | return_request = GotoWaypointRequest() 87 | return_request.goal_state_x = initial_position_local[0] 88 | return_request.goal_state_y = initial_position_local[1] 89 | 90 | return_response = goto_serv_(return_request) 91 | 92 | if return_response.FindPathFlag == 1: 93 | break 94 | 95 | print ('returning again') 96 | 97 | print ('finish return') 98 | rospy.spin() 99 | -------------------------------------------------------------------------------- /src/laserscan_to_pointcloud.cpp: -------------------------------------------------------------------------------- 1 | // ROS 2 | #include 3 | 4 | // ROS LaserScan tools 5 | #include 6 | 7 | // ROS messages 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | // ROS services 18 | #include 19 | 20 | // ROS tf 21 | #include 22 | #include 23 | 24 | // Octomap 25 | #include 26 | #include 27 | //#include 28 | #include 29 | typedef octomap_msgs::GetOctomap OctomapSrv; 30 | //#include 31 | //typedef laser_octomap::BoundingBoxQuery BBXSrv; 32 | #include 33 | 34 | #include 35 | 36 | 37 | void stopNode(int sig) 38 | { 39 | ros::shutdown(); 40 | exit(0); 41 | } 42 | 43 | /// CLASS DEFINITION =========================================================== 44 | class LaserScanToPointCloud 45 | { 46 | public: 47 | // Constructor and destructor 48 | LaserScanToPointCloud(); 49 | // virtual ~LaserScanToPointCloud(); 50 | 51 | // Callbacks 52 | void laserScanCallback(const sensor_msgs::LaserScanConstPtr& scan); 53 | 54 | private: 55 | 56 | // ROS 57 | ros::NodeHandle node_hand_; 58 | ros::Subscriber laser_scan_sub_; 59 | ros::Publisher pc_pub_; 60 | 61 | //TF 62 | tf::TransformListener listener_; 63 | 64 | //PC 65 | laser_geometry::LaserProjection projector_; 66 | }; 67 | 68 | /// Constructor and destructor ================================================= 69 | LaserScanToPointCloud::LaserScanToPointCloud(){ 70 | //======================================================================= 71 | // Subscribers 72 | //======================================================================= 73 | //Mission Flag (feedback) 74 | laser_scan_sub_ = node_hand_.subscribe("/scan", 2, &LaserScanToPointCloud::laserScanCallback, this); 75 | 76 | //======================================================================= 77 | // Publishers 78 | //======================================================================= 79 | pc_pub_ = node_hand_.advertise("pc_from_scan", 2, true); 80 | // // Waiting for mission flag 81 | // ros::Rate loop_rate(10); 82 | // if(!mission_flag_available_) 83 | // ROS_WARN("Waiting for mission_flag"); 84 | // while (ros::ok() && !mission_flag_available_) 85 | // { 86 | // ros::spinOnce(); 87 | // loop_rate.sleep(); 88 | // } 89 | // //Navigation data (feedback) 90 | // odomSub_ = node_hand_.subscribe("/pose_ekf_slam/odometry", 1, &LaserScanToPointCloud::odomCallback, this); 91 | // nav_sts_available_ = false; 92 | // while (ros::ok() && !nav_sts_available_) 93 | // { 94 | // ros::spinOnce(); 95 | // loop_rate.sleep(); 96 | // } 97 | } 98 | 99 | void LaserScanToPointCloud::laserScanCallback(const sensor_msgs::LaserScanConstPtr &scan){ 100 | if(!listener_.waitForTransform( 101 | scan->header.frame_id, 102 | "/odom", 103 | scan->header.stamp + ros::Duration().fromSec(scan->ranges.size()*scan->time_increment), 104 | ros::Duration(1.0))){ 105 | return; 106 | } 107 | 108 | sensor_msgs::PointCloud2 cloud; 109 | projector_.transformLaserScanToPointCloud("/base_link",*scan, 110 | cloud,listener_); 111 | 112 | // Do something with cloud. 113 | pc_pub_.publish(cloud); 114 | } 115 | 116 | /// MAIN NODE FUNCTION ========================================================= 117 | int main(int argc, char** argv){ 118 | 119 | //======================================================================= 120 | // Override SIGINT handler 121 | //======================================================================= 122 | signal(SIGINT, stopNode); 123 | 124 | // Init ROS node 125 | ros::init(argc, argv, "laserscan_to_pointcloud"); 126 | ros::NodeHandle private_nh("~"); 127 | 128 | // Constructor 129 | LaserScanToPointCloud laserscan_to_pc; 130 | 131 | // Spin 132 | ros::spin(); 133 | 134 | // Exit main function without errors 135 | return 0; 136 | } 137 | -------------------------------------------------------------------------------- /src/node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # TODO is it necessary here? 4 | import roslib; roslib.load_manifest('lab1_turtlebot') 5 | import rospy 6 | 7 | from driver import driver 8 | 9 | if __name__ == '__main__': 10 | try: 11 | 12 | pilot = driver() 13 | pilot.load_goals() 14 | pilot.drive() 15 | 16 | 17 | except rospy.ROSInterruptException: 18 | pass 19 | -------------------------------------------------------------------------------- /src/offline_planner_R2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * offline_planner_R2_main.cpp 3 | * 4 | * Created on: Feb 18, 2015 5 | * Author: juandhv (Juan David Hernandez Vega, juandhv@eia.udg.edu) 6 | * 7 | * Offline path planning (without differential constraints) using OMPL. 8 | * Octomaps are used to represent workspace and to validate if configurations are collision-free. 9 | */ 10 | 11 | // *** OMPL *** 12 | //#include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // *** ROS *** 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | // *** IO *** 38 | #include 39 | #include 40 | 41 | // *** state validity checker, collision checking using Octomaps*** 42 | #include "state_validity_checker_octomap_R2.hpp" 43 | 44 | namespace ob = ompl::base; 45 | 46 | //! OfflinePlannerR2 class. 47 | /*! 48 | * Offline Planner. 49 | * Setup a sampling-based planner for computing a collision-free path. 50 | * C-Space: R2 51 | * Workspace is represented with Octomaps 52 | */ 53 | class OfflinePlannerR2 54 | { 55 | public: 56 | //! Constructor 57 | OfflinePlannerR2(); 58 | //! Planner setup 59 | void planWithSimpleSetup(std::vector start_state, std::vector goal_state, autonomous_explore_map_plan::FindPathToGoal::Response& response); 60 | //! Procedure to visualize the resulting path 61 | void visualizeRRT(og::PathGeometric geopath); 62 | //! Procedure to create a mission file using the resulting path 63 | void createMissionFile(og::PathGeometric geopath); 64 | //! Callback for getting current vehicle position 65 | void odomCallback(const nav_msgs::OdometryPtr &odom_msg); 66 | //! Callback for setting the query goal to specified values 67 | bool findPathToGoal(autonomous_explore_map_plan::FindPathToGoal::Request& request, autonomous_explore_map_plan::FindPathToGoal::Response& response); 68 | private: 69 | // *** ROS *** 70 | ros::NodeHandle node_handler_; 71 | ros::Publisher online_traj_pub_; 72 | ros::Subscriber odom_sub_; 73 | ros::ServiceServer service_; 74 | // *** OMPL *** 75 | og::SimpleSetupPtr simple_setup_; 76 | // *** Planner parameters *** 77 | std::vector planning_bounds_x_, planning_bounds_y_, current_position_; 78 | double planning_depth_, solving_time_; 79 | std::string planner_name_; 80 | }; 81 | 82 | //! Constructor. 83 | /*! 84 | * Load planner parameters from configuration file. 85 | * Publishers to visualize the resulting path. 86 | */ 87 | OfflinePlannerR2::OfflinePlannerR2() 88 | { 89 | //======================================================================= 90 | // Subscribers. 91 | //======================================================================= 92 | //Navigation data 93 | odom_sub_ = node_handler_.subscribe("/odom", 1, &OfflinePlannerR2::odomCallback, this); 94 | 95 | //======================================================================= 96 | // Publishers (to visualize the resulting path). 97 | //======================================================================= 98 | online_traj_pub_ = node_handler_.advertise ("/controller_turtlebot/solution_path", 1, true); 99 | 100 | //======================================================================= 101 | // Missions are defined at a constant depth, thus C-Space dimension is R2. 102 | //======================================================================= 103 | planning_bounds_x_.resize(2); 104 | planning_bounds_y_.resize(2); 105 | current_position_.resize(2); 106 | 107 | //======================================================================= 108 | // Get parameters from configuration file. 109 | //======================================================================= 110 | node_handler_.getParam("planning_bounds_x", planning_bounds_x_); 111 | node_handler_.getParam("planning_bounds_y", planning_bounds_y_); 112 | node_handler_.getParam("solving_time", solving_time_); 113 | node_handler_.getParam("planner_name", planner_name_); 114 | planning_depth_ = 0.3; 115 | 116 | //======================================================================= 117 | // Service 118 | //======================================================================= 119 | service_ = node_handler_.advertiseService("/turtlebot_drive/find_path_to_goal", &OfflinePlannerR2::findPathToGoal, this); 120 | } 121 | 122 | //! Planner setup. 123 | /*! 124 | * Setup a sampling-based planner using OMPL. 125 | */ 126 | void OfflinePlannerR2::planWithSimpleSetup(std::vector start_state, std::vector goal_state, autonomous_explore_map_plan::FindPathToGoal::Response& response) 127 | { 128 | //======================================================================= 129 | // Instantiate the state space (R2, X-Y). Navigating at a constant depth. 130 | //======================================================================= 131 | ob::StateSpacePtr space(new ob::RealVectorStateSpace(2)); 132 | 133 | // Set the bounds for the state space 134 | ob::RealVectorBounds bounds(2); 135 | 136 | bounds.setLow(0, planning_bounds_x_[0]); 137 | bounds.setHigh(0, planning_bounds_x_[1]); 138 | bounds.setLow(1, planning_bounds_y_[0]); 139 | bounds.setHigh(1, planning_bounds_y_[1]); 140 | 141 | space->as()->setBounds(bounds); 142 | 143 | //======================================================================= 144 | // Define a simple setup class, which contains the planning problem. 145 | //======================================================================= 146 | simple_setup_ = og::SimpleSetupPtr( new og::SimpleSetup(space) ); 147 | ob::SpaceInformationPtr si = simple_setup_->getSpaceInformation(); 148 | 149 | //======================================================================= 150 | // Create a planner. OMPL is specialized in sampling-based methods 151 | //======================================================================= 152 | // (RRT, RRTstar, RRTConnect), (EST), (PRM, PRMstar) 153 | ob::PlannerPtr planner; 154 | if(planner_name_.compare("RRT")==0) 155 | planner = ob::PlannerPtr(new og::RRT(si)); 156 | else if(planner_name_.compare("RRTstar")==0) 157 | planner = ob::PlannerPtr(new og::RRTstar(si)); 158 | else if(planner_name_.compare("RRTConnect")==0) 159 | planner = ob::PlannerPtr(new og::RRTConnect(si)); 160 | else if(planner_name_.compare("EST")==0) 161 | planner = ob::PlannerPtr(new og::EST(si)); 162 | else if(planner_name_.compare("PRM")==0) 163 | planner = ob::PlannerPtr(new og::PRM(si)); 164 | else if(planner_name_.compare("PRMstar")==0) 165 | planner = ob::PlannerPtr(new og::PRMstar(si)); 166 | else 167 | planner = ob::PlannerPtr(new og::RRT(si)); 168 | 169 | //planner->as()->setRange(0.2); 170 | //======================================================================= 171 | // Set the setup planner 172 | //======================================================================= 173 | simple_setup_->setPlanner(planner); 174 | 175 | //======================================================================= 176 | // Set state validity checking for this space. 177 | //======================================================================= 178 | OmFclStateValidityCheckerR2 * om_stat_val_check = new OmFclStateValidityCheckerR2(si, planning_depth_, planning_bounds_x_, planning_bounds_y_); 179 | simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(om_stat_val_check)); 180 | 181 | //======================================================================= 182 | // Create a start and goal states 183 | //======================================================================= 184 | ob::ScopedState<> start(space); 185 | 186 | start[0] = double(start_state[0]); 187 | start[1] = double(start_state[1]); 188 | 189 | // create a goal state 190 | ob::ScopedState<> goal(space); 191 | 192 | goal[0] = double(goal_state[0]); 193 | goal[1] = double(goal_state[1]); 194 | 195 | //======================================================================= 196 | // Set the start and goal states 197 | //======================================================================= 198 | simple_setup_->setStartAndGoalStates(start, goal); 199 | 200 | //======================================================================= 201 | // Perform setup steps for the planner 202 | //======================================================================= 203 | simple_setup_->setup(); 204 | 205 | //======================================================================= 206 | // Print planner information 207 | //======================================================================= 208 | //planner->printProperties(std::cout);// print planner properties 209 | //si->printSettings(std::cout);// print the settings for this space 210 | 211 | //======================================================================= 212 | // Attempt to solve the query 213 | //======================================================================= 214 | ob::PlannerStatus solved = simple_setup_->solve( solving_time_ ); 215 | 216 | if (solved && simple_setup_->haveExactSolutionPath()) 217 | { 218 | // get the goal representation from the problem definition (not the same as the goal state) 219 | // and inquire about the found path 220 | // simple_setup_->simplifySolution(); 221 | og::PathGeometric path = simple_setup_->getSolutionPath(); 222 | //path.interpolate(int(path.length()/1.0)); 223 | visualizeRRT(path); 224 | 225 | std::vector< ob::State * > states = path.getStates(); 226 | const ob::RealVectorStateSpace::StateType *state; 227 | for (uint32_t i = 0; i < path.getStateCount(); ++i) 228 | { 229 | geometry_msgs::Pose2D pose; 230 | // extract the component of the state and cast it to what we expect 231 | state = states[i]->as(); 232 | 233 | pose.x = state->values[0]; 234 | pose.y = state->values[1]; 235 | 236 | response.poses.push_back(pose); 237 | } 238 | 239 | 240 | ROS_INFO("%s: path has been found with simple_setup", ros::this_node::getName().c_str()); 241 | } 242 | else 243 | ROS_INFO("%s: path has not been found", ros::this_node::getName().c_str()); 244 | } 245 | 246 | //! Resulting path visualization. 247 | /*! 248 | * Visualize resulting path. 249 | */ 250 | void OfflinePlannerR2::visualizeRRT(og::PathGeometric geopath) 251 | { 252 | // %Tag(MARKER_INIT)% 253 | visualization_msgs::Marker q_init_goal, visual_rrt, result_path, visual_result_path; 254 | visual_result_path.header.frame_id = result_path.header.frame_id = q_init_goal.header.frame_id = visual_rrt.header.frame_id = "/odom"; 255 | visual_result_path.header.stamp = result_path.header.stamp = q_init_goal.header.stamp = visual_rrt.header.stamp = ros::Time::now(); 256 | q_init_goal.ns = "online_planner_points"; 257 | visual_rrt.ns = "online_planner_rrt"; 258 | result_path.ns = "online_planner_result"; 259 | visual_result_path.ns = "online_planner_result_path"; 260 | visual_result_path.action = result_path.action = q_init_goal.action = visual_rrt.action = visualization_msgs::Marker::ADD; 261 | 262 | visual_result_path.pose.orientation.w = result_path.pose.orientation.w = q_init_goal.pose.orientation.w = visual_rrt.pose.orientation.w = 1.0; 263 | // %EndTag(MARKER_INIT)% 264 | 265 | // %Tag(ID)% 266 | q_init_goal.id = 0; 267 | visual_rrt.id = 1; 268 | result_path.id = 2; 269 | visual_result_path.id = 3; 270 | // %EndTag(ID)% 271 | 272 | // %Tag(TYPE)% 273 | result_path.type = q_init_goal.type = visualization_msgs::Marker::POINTS; 274 | visual_rrt.type = visual_result_path.type = visualization_msgs::Marker::LINE_LIST; 275 | // %EndTag(TYPE)% 276 | 277 | // %Tag(SCALE)% 278 | // POINTS markers use x and y scale for width/height respectively 279 | result_path.scale.x = q_init_goal.scale.x = 0.5; 280 | result_path.scale.y = q_init_goal.scale.y = 0.5; 281 | result_path.scale.z = q_init_goal.scale.z = 0.5; 282 | 283 | // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width 284 | visual_rrt.scale.x = 0.01; 285 | visual_result_path.scale.x = 0.02; 286 | // %EndTag(SCALE)% 287 | 288 | // %Tag(COLOR)% 289 | // Points are green 290 | visual_result_path.color.g = 1.0; 291 | result_path.color.g = q_init_goal.color.g = 1.0; 292 | visual_result_path.color.a = result_path.color.a = q_init_goal.color.a = 1.0; 293 | 294 | // Line strip is blue 295 | visual_rrt.color.b = 1.0; 296 | visual_rrt.color.a = 1.0; 297 | 298 | 299 | ob::PlannerData planner_data(simple_setup_->getSpaceInformation()); 300 | simple_setup_->getPlannerData(planner_data); 301 | std::vector< unsigned int > edgeList; 302 | int num_parents; 303 | const ob::SE2StateSpace::StateType *state_se2; 304 | const ob::RealVectorStateSpace::StateType *state_r2; 305 | 306 | const ob::RealVectorStateSpace::StateType *state; 307 | geometry_msgs::Point p; 308 | 309 | for (unsigned int i = 1 ; i < planner_data.numVertices() ; ++i) 310 | { 311 | if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0) 312 | { 313 | state_r2 = planner_data.getVertex(i).getState()->as(); 314 | p.x = state_r2->values[0]; 315 | p.y = state_r2->values[1]; 316 | p.z = planning_depth_; 317 | visual_rrt.points.push_back(p); 318 | 319 | state_r2 = planner_data.getVertex(edgeList[0]).getState()->as(); 320 | p.x = state_r2->values[0]; 321 | p.y = state_r2->values[1]; 322 | p.z = planning_depth_; 323 | visual_rrt.points.push_back(p); 324 | } 325 | } 326 | std::vector< ob::State * > states = geopath.getStates(); 327 | 328 | for (uint32_t i = 0; i < geopath.getStateCount(); ++i) 329 | { 330 | // extract the component of the state and cast it to what we expect 331 | state = states[i]->as(); 332 | 333 | p.x = state->values[0]; 334 | p.y = state->values[1]; 335 | p.z = planning_depth_;//pos->values[2]; 336 | 337 | result_path.points.push_back(p); 338 | 339 | if(i>0) 340 | { 341 | visual_result_path.points.push_back(p); 342 | state = states[i-1]->as(); 343 | 344 | p.x = state->values[0]; 345 | p.y = state->values[1]; 346 | p.z = planning_depth_; 347 | visual_result_path.points.push_back(p); 348 | } 349 | } 350 | 351 | //online_traj_pub_.publish(q_init_goal); 352 | online_traj_pub_.publish(visual_rrt); 353 | online_traj_pub_.publish(visual_result_path); 354 | //online_traj_pub_.publish(result_path); 355 | } 356 | 357 | 358 | //! odomCallback. 359 | /*! 360 | * Callback for getting updated vehicle position 361 | */ 362 | void OfflinePlannerR2::odomCallback(const nav_msgs::OdometryPtr &odom_msg) 363 | { 364 | current_position_[0] = odom_msg->pose.pose.position.x; 365 | current_position_[1] = odom_msg->pose.pose.position.y; 366 | } 367 | 368 | //! setGoalCallback. 369 | /*! 370 | * Service callback that sets the value of the query goal to the value of current position 371 | */ 372 | bool OfflinePlannerR2::findPathToGoal(autonomous_explore_map_plan::FindPathToGoal::Request& request, autonomous_explore_map_plan::FindPathToGoal::Response& response){ 373 | ROS_INFO("%s: finding a path from current position to a given goal", ros::this_node::getName().c_str()); 374 | vector goal_state; 375 | goal_state.resize(2); 376 | goal_state[0] = request.goal_state_x; 377 | goal_state[1] = request.goal_state_y; 378 | 379 | planWithSimpleSetup(current_position_, goal_state, response); 380 | return true; 381 | } 382 | 383 | 384 | int main(int argc, char **argv) 385 | { 386 | ros::init(argc, argv, "offline_planner_with_services_R2"); 387 | ros::NodeHandle node_handler; 388 | ROS_INFO("%s: offline planner (C++)", ros::this_node::getName().c_str()); 389 | ROS_INFO("%s: using OMPL version %s", ros::this_node::getName().c_str(), OMPL_VERSION); 390 | ompl::msg::setLogLevel(ompl::msg::LOG_NONE); 391 | 392 | OfflinePlannerR2 offline_planner; 393 | 394 | ros::Rate loop_rate(10); 395 | while (ros::ok()) 396 | { 397 | ros::spinOnce(); 398 | loop_rate.sleep(); 399 | } 400 | 401 | return 0; 402 | } 403 | -------------------------------------------------------------------------------- /src/offline_planner_R2_2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * offline_planner_R2_main.cpp 3 | * 4 | * Created on: Feb 18, 2015 5 | * Author: juandhv (Juan David Hernandez Vega, juandhv@eia.udg.edu) 6 | * 7 | * Offline path planning (without differential constraints) using OMPL. 8 | * Octomaps are used to represent workspace and to validate if configurations are collision-free. 9 | */ 10 | 11 | // *** OMPL *** 12 | //#include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | // *** ROS *** 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | // *** IO *** 38 | #include 39 | #include 40 | 41 | // *** state validity checker, collision checking using Octomaps*** 42 | #include "state_validity_checker_octomap_R2.hpp" 43 | 44 | namespace ob = ompl::base; 45 | 46 | //! OfflinePlannerR2 class. 47 | /*! 48 | * Offline Planner. 49 | * Setup a sampling-based planner for computing a collision-free path. 50 | * C-Space: R2 51 | * Workspace is represented with Octomaps 52 | */ 53 | class OfflinePlannerR2 54 | { 55 | public: 56 | //! Constructor 57 | OfflinePlannerR2(); 58 | //! Planner setup 59 | void planWithSimpleSetup(std::vector start_state, std::vector goal_state, autonomous_explore_map_plan::FindPathToGoal::Response& response); 60 | //! Procedure to visualize the resulting path 61 | void visualizeRRT(og::PathGeometric geopath); 62 | //! Procedure to create a mission file using the resulting path 63 | void createMissionFile(og::PathGeometric geopath); 64 | //! Callback for getting current vehicle position 65 | void odomCallback(const nav_msgs::OdometryPtr &odom_msg); 66 | //! Callback for setting the query goal to specified values 67 | bool findPathToGoal(autonomous_explore_map_plan::FindPathToGoal::Request& request, autonomous_explore_map_plan::FindPathToGoal::Response& response); 68 | private: 69 | // *** ROS *** 70 | ros::NodeHandle node_handler_; 71 | ros::Publisher online_traj_pub_; 72 | ros::Subscriber odom_sub_; 73 | ros::ServiceServer service_; 74 | // *** OMPL *** 75 | og::SimpleSetupPtr simple_setup_; 76 | // *** Planner parameters *** 77 | std::vector planning_bounds_x_, planning_bounds_y_, current_position_; 78 | double planning_depth_, solving_time_; 79 | std::string planner_name_; 80 | }; 81 | 82 | //! Constructor. 83 | /*! 84 | * Load planner parameters from configuration file. 85 | * Publishers to visualize the resulting path. 86 | */ 87 | OfflinePlannerR2::OfflinePlannerR2() 88 | { 89 | //======================================================================= 90 | // Subscribers. 91 | //======================================================================= 92 | //Navigation data 93 | odom_sub_ = node_handler_.subscribe("/odom", 1, &OfflinePlannerR2::odomCallback, this); 94 | 95 | //======================================================================= 96 | // Publishers (to visualize the resulting path). 97 | //======================================================================= 98 | online_traj_pub_ = node_handler_.advertise ("/controller_turtlebot/solution_path", 1, true); 99 | 100 | //======================================================================= 101 | // Missions are defined at a constant depth, thus C-Space dimension is R2. 102 | //======================================================================= 103 | planning_bounds_x_.resize(2); 104 | planning_bounds_y_.resize(2); 105 | current_position_.resize(2); 106 | 107 | //======================================================================= 108 | // Get parameters from configuration file. 109 | //======================================================================= 110 | node_handler_.getParam("planning_bounds_x", planning_bounds_x_); 111 | node_handler_.getParam("planning_bounds_y", planning_bounds_y_); 112 | //node_handler_.getParam("solving_time", solving_time_); 113 | //node_handler_.getParam("planner_name", planner_name_); 114 | planning_depth_ = 0.3; 115 | solving_time_ = 50; 116 | planner_name_ = "RRT"; 117 | //node_handler_.planner_name_ = "RRTstar"; 118 | //node_handler_.solving_time_ = 20; 119 | //======================================================================= 120 | // Service 121 | //======================================================================= 122 | service_ = node_handler_.advertiseService("/turtlebot_return/find_path_to_goal", &OfflinePlannerR2::findPathToGoal, this); 123 | } 124 | 125 | //! Planner setup. 126 | /*! 127 | * Setup a sampling-based planner using OMPL. 128 | */ 129 | void OfflinePlannerR2::planWithSimpleSetup(std::vector start_state, std::vector goal_state, autonomous_explore_map_plan::FindPathToGoal::Response& response) 130 | { 131 | //======================================================================= 132 | // Instantiate the state space (R2, X-Y). Navigating at a constant depth. 133 | //======================================================================= 134 | ob::StateSpacePtr space(new ob::RealVectorStateSpace(2)); 135 | 136 | // Set the bounds for the state space 137 | ob::RealVectorBounds bounds(2); 138 | 139 | bounds.setLow(0, planning_bounds_x_[0]); 140 | bounds.setHigh(0, planning_bounds_x_[1]); 141 | bounds.setLow(1, planning_bounds_y_[0]); 142 | bounds.setHigh(1, planning_bounds_y_[1]); 143 | 144 | space->as()->setBounds(bounds); 145 | 146 | //======================================================================= 147 | // Define a simple setup class, which contains the planning problem. 148 | //======================================================================= 149 | simple_setup_ = og::SimpleSetupPtr( new og::SimpleSetup(space) ); 150 | ob::SpaceInformationPtr si = simple_setup_->getSpaceInformation(); 151 | 152 | //======================================================================= 153 | // Create a planner. OMPL is specialized in sampling-based methods 154 | //======================================================================= 155 | // (RRT, RRTstar, RRTConnect), (EST), (PRM, PRMstar) 156 | ob::PlannerPtr planner; 157 | if(planner_name_.compare("RRT")==0) 158 | planner = ob::PlannerPtr(new og::RRT(si)); 159 | else if(planner_name_.compare("RRTstar")==0) 160 | planner = ob::PlannerPtr(new og::RRTstar(si)); 161 | else if(planner_name_.compare("RRTConnect")==0) 162 | planner = ob::PlannerPtr(new og::RRTConnect(si)); 163 | else if(planner_name_.compare("EST")==0) 164 | planner = ob::PlannerPtr(new og::EST(si)); 165 | else if(planner_name_.compare("PRM")==0) 166 | planner = ob::PlannerPtr(new og::PRM(si)); 167 | else if(planner_name_.compare("PRMstar")==0) 168 | planner = ob::PlannerPtr(new og::PRMstar(si)); 169 | else 170 | planner = ob::PlannerPtr(new og::RRT(si)); 171 | 172 | //planner->as()->setRange(0.2); 173 | //======================================================================= 174 | // Set the setup planner 175 | //======================================================================= 176 | simple_setup_->setPlanner(planner); 177 | 178 | //======================================================================= 179 | // Set state validity checking for this space. 180 | //======================================================================= 181 | OmFclStateValidityCheckerR2 * om_stat_val_check = new OmFclStateValidityCheckerR2(si, planning_depth_, planning_bounds_x_, planning_bounds_y_); 182 | simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(om_stat_val_check)); 183 | 184 | //======================================================================= 185 | // Create a start and goal states 186 | //======================================================================= 187 | ob::ScopedState<> start(space); 188 | 189 | start[0] = double(start_state[0]); 190 | start[1] = double(start_state[1]); 191 | 192 | // create a goal state 193 | ob::ScopedState<> goal(space); 194 | 195 | goal[0] = double(goal_state[0]); 196 | goal[1] = double(goal_state[1]); 197 | 198 | //======================================================================= 199 | // Set the start and goal states 200 | //======================================================================= 201 | simple_setup_->setStartAndGoalStates(start, goal); 202 | 203 | //======================================================================= 204 | // Perform setup steps for the planner 205 | //======================================================================= 206 | simple_setup_->setup(); 207 | 208 | //======================================================================= 209 | // Print planner information 210 | //======================================================================= 211 | //planner->printProperties(std::cout);// print planner properties 212 | //si->printSettings(std::cout);// print the settings for this space 213 | 214 | //======================================================================= 215 | // Attempt to solve the query 216 | //======================================================================= 217 | ob::PlannerStatus solved = simple_setup_->solve( solving_time_ ); 218 | 219 | if (solved && simple_setup_->haveExactSolutionPath()) 220 | { 221 | // get the goal representation from the problem definition (not the same as the goal state) 222 | // and inquire about the found path 223 | // simple_setup_->simplifySolution(); 224 | og::PathGeometric path = simple_setup_->getSolutionPath(); 225 | //path.interpolate(int(path.length()/1.0)); 226 | visualizeRRT(path); 227 | 228 | std::vector< ob::State * > states = path.getStates(); 229 | const ob::RealVectorStateSpace::StateType *state; 230 | for (uint32_t i = 0; i < path.getStateCount(); ++i) 231 | { 232 | geometry_msgs::Pose2D pose; 233 | // extract the component of the state and cast it to what we expect 234 | state = states[i]->as(); 235 | 236 | pose.x = state->values[0]; 237 | pose.y = state->values[1]; 238 | 239 | response.poses.push_back(pose); 240 | } 241 | 242 | 243 | ROS_INFO("%s: path has been found with simple_setup", ros::this_node::getName().c_str()); 244 | } 245 | else 246 | ROS_INFO("%s: path has not been found", ros::this_node::getName().c_str()); 247 | } 248 | 249 | //! Resulting path visualization. 250 | /*! 251 | * Visualize resulting path. 252 | */ 253 | void OfflinePlannerR2::visualizeRRT(og::PathGeometric geopath) 254 | { 255 | // %Tag(MARKER_INIT)% 256 | visualization_msgs::Marker q_init_goal, visual_rrt, result_path, visual_result_path; 257 | visual_result_path.header.frame_id = result_path.header.frame_id = q_init_goal.header.frame_id = visual_rrt.header.frame_id = "/odom"; 258 | visual_result_path.header.stamp = result_path.header.stamp = q_init_goal.header.stamp = visual_rrt.header.stamp = ros::Time::now(); 259 | q_init_goal.ns = "online_planner_points"; 260 | visual_rrt.ns = "online_planner_rrt"; 261 | result_path.ns = "online_planner_result"; 262 | visual_result_path.ns = "online_planner_result_path"; 263 | visual_result_path.action = result_path.action = q_init_goal.action = visual_rrt.action = visualization_msgs::Marker::ADD; 264 | 265 | visual_result_path.pose.orientation.w = result_path.pose.orientation.w = q_init_goal.pose.orientation.w = visual_rrt.pose.orientation.w = 1.0; 266 | // %EndTag(MARKER_INIT)% 267 | 268 | // %Tag(ID)% 269 | q_init_goal.id = 0; 270 | visual_rrt.id = 1; 271 | result_path.id = 2; 272 | visual_result_path.id = 3; 273 | // %EndTag(ID)% 274 | 275 | // %Tag(TYPE)% 276 | result_path.type = q_init_goal.type = visualization_msgs::Marker::POINTS; 277 | visual_rrt.type = visual_result_path.type = visualization_msgs::Marker::LINE_LIST; 278 | // %EndTag(TYPE)% 279 | 280 | // %Tag(SCALE)% 281 | // POINTS markers use x and y scale for width/height respectively 282 | result_path.scale.x = q_init_goal.scale.x = 0.5; 283 | result_path.scale.y = q_init_goal.scale.y = 0.5; 284 | result_path.scale.z = q_init_goal.scale.z = 0.5; 285 | 286 | // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width 287 | visual_rrt.scale.x = 0.01; 288 | visual_result_path.scale.x = 0.02; 289 | // %EndTag(SCALE)% 290 | 291 | // %Tag(COLOR)% 292 | // Points are green 293 | visual_result_path.color.g = 1.0; 294 | result_path.color.g = q_init_goal.color.g = 1.0; 295 | visual_result_path.color.a = result_path.color.a = q_init_goal.color.a = 1.0; 296 | 297 | // Line strip is blue 298 | visual_rrt.color.b = 1.0; 299 | visual_rrt.color.a = 1.0; 300 | 301 | 302 | ob::PlannerData planner_data(simple_setup_->getSpaceInformation()); 303 | simple_setup_->getPlannerData(planner_data); 304 | std::vector< unsigned int > edgeList; 305 | int num_parents; 306 | const ob::SE2StateSpace::StateType *state_se2; 307 | const ob::RealVectorStateSpace::StateType *state_r2; 308 | 309 | const ob::RealVectorStateSpace::StateType *state; 310 | geometry_msgs::Point p; 311 | 312 | for (unsigned int i = 1 ; i < planner_data.numVertices() ; ++i) 313 | { 314 | if (planner_data.getVertex(i).getState() && planner_data.getIncomingEdges(i,edgeList) > 0) 315 | { 316 | state_r2 = planner_data.getVertex(i).getState()->as(); 317 | p.x = state_r2->values[0]; 318 | p.y = state_r2->values[1]; 319 | p.z = planning_depth_; 320 | visual_rrt.points.push_back(p); 321 | 322 | state_r2 = planner_data.getVertex(edgeList[0]).getState()->as(); 323 | p.x = state_r2->values[0]; 324 | p.y = state_r2->values[1]; 325 | p.z = planning_depth_; 326 | visual_rrt.points.push_back(p); 327 | } 328 | } 329 | std::vector< ob::State * > states = geopath.getStates(); 330 | 331 | for (uint32_t i = 0; i < geopath.getStateCount(); ++i) 332 | { 333 | // extract the component of the state and cast it to what we expect 334 | state = states[i]->as(); 335 | 336 | p.x = state->values[0]; 337 | p.y = state->values[1]; 338 | p.z = planning_depth_;//pos->values[2]; 339 | 340 | result_path.points.push_back(p); 341 | 342 | if(i>0) 343 | { 344 | visual_result_path.points.push_back(p); 345 | state = states[i-1]->as(); 346 | 347 | p.x = state->values[0]; 348 | p.y = state->values[1]; 349 | p.z = planning_depth_; 350 | visual_result_path.points.push_back(p); 351 | } 352 | } 353 | 354 | //online_traj_pub_.publish(q_init_goal); 355 | online_traj_pub_.publish(visual_rrt); 356 | online_traj_pub_.publish(visual_result_path); 357 | //online_traj_pub_.publish(result_path); 358 | } 359 | 360 | 361 | //! odomCallback. 362 | /*! 363 | * Callback for getting updated vehicle position 364 | */ 365 | void OfflinePlannerR2::odomCallback(const nav_msgs::OdometryPtr &odom_msg) 366 | { 367 | current_position_[0] = odom_msg->pose.pose.position.x; 368 | current_position_[1] = odom_msg->pose.pose.position.y; 369 | } 370 | 371 | //! setGoalCallback. 372 | /*! 373 | * Service callback that sets the value of the query goal to the value of current position 374 | */ 375 | bool OfflinePlannerR2::findPathToGoal(autonomous_explore_map_plan::FindPathToGoal::Request& request, autonomous_explore_map_plan::FindPathToGoal::Response& response){ 376 | ROS_INFO("%s: finding a path from current position to a given goal", ros::this_node::getName().c_str()); 377 | vector goal_state; 378 | goal_state.resize(2); 379 | goal_state[0] = request.goal_state_x; 380 | goal_state[1] = request.goal_state_y; 381 | 382 | planWithSimpleSetup(current_position_, goal_state, response); 383 | return true; 384 | } 385 | 386 | 387 | int main(int argc, char **argv) 388 | { 389 | ros::init(argc, argv, "offline_planner_with_services_R2"); 390 | ros::NodeHandle node_handler; 391 | ROS_INFO("%s: offline planner (C++)", ros::this_node::getName().c_str()); 392 | ROS_INFO("%s: using OMPL version %s", ros::this_node::getName().c_str(), OMPL_VERSION); 393 | ompl::msg::setLogLevel(ompl::msg::LOG_NONE); 394 | 395 | OfflinePlannerR2 offline_planner; 396 | 397 | ros::Rate loop_rate(10); 398 | while (ros::ok()) 399 | { 400 | ros::spinOnce(); 401 | loop_rate.sleep(); 402 | } 403 | 404 | return 0; 405 | } 406 | -------------------------------------------------------------------------------- /src/state_validity_checker_octomap_R2.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * state_validity_checker_octomap_R2.cpp 3 | * 4 | * Created on: Mar 26, 2015 5 | * Author: juandhv (Juan David Hernandez Vega, juandhv@eia.udg.edu) 6 | * 7 | * State checker. Check is a given configuration (R2 state) is collision-free. 8 | * The workspace is represented by an octomap. 9 | */ 10 | 11 | #include "state_validity_checker_octomap_R2.hpp" 12 | 13 | OmFclStateValidityCheckerR2::OmFclStateValidityCheckerR2(const ob::SpaceInformationPtr &si, const double planning_depth, std::vector planning_bounds_x, std::vector planning_bounds_y) : 14 | ob::StateValidityChecker(si) 15 | { 16 | GetOctomap::Request req; 17 | GetOctomap::Response resp; 18 | std::string serv_name; 19 | 20 | planning_depth_ = planning_depth; 21 | planning_bounds_x_ = planning_bounds_x; 22 | planning_bounds_y_ = planning_bounds_y; 23 | 24 | serv_name = "/octomap_binary"; 25 | octree_ = NULL; 26 | 27 | ROS_DEBUG("%s: requesting the map to %s...", ros::this_node::getName().c_str(), node_hand_.resolveName(serv_name).c_str()); 28 | 29 | while((node_hand_.ok() && !ros::service::call(serv_name, req, resp)) || resp.map.data.size()==0) 30 | { 31 | ROS_WARN("Request to %s failed; trying again...", node_hand_.resolveName(serv_name).c_str()); 32 | usleep(1000000); 33 | } 34 | if (node_hand_.ok()){ // skip when CTRL-C 35 | abs_octree_ = octomap_msgs::msgToMap(resp.map); 36 | std::cout << std::endl; 37 | if (abs_octree_){ 38 | octree_ = dynamic_cast(abs_octree_); 39 | } 40 | 41 | octree_->getMetricMin(octree_min_x_, octree_min_y_, octree_min_z_); 42 | octree_->getMetricMax(octree_max_x_, octree_max_y_, octree_max_z_); 43 | octree_res_ = octree_->getResolution(); 44 | if (octree_){ 45 | ROS_INFO("%s: Octomap received (%zu nodes, %f m res)", ros::this_node::getName().c_str(), octree_->size(), octree_->getResolution()); 46 | } else{ 47 | ROS_ERROR("Error reading OcTree from stream"); 48 | } 49 | } 50 | } 51 | 52 | bool OmFclStateValidityCheckerR2::isValid(const ob::State *state) const 53 | { 54 | OcTreeNode* result; 55 | point3d query; 56 | bool collision(false); 57 | double node_occupancy; 58 | double square_length = 0.5; 59 | 60 | // extract the component of the state and cast it to what we expect 61 | const ob::RealVectorStateSpace::StateType *pos = state->as(); 62 | 63 | //pos->values[0] = 0.8; 64 | //pos->values[1] = -0.8; 65 | for(double xi = pos->values[0]-(square_length/2.0);xi <= pos->values[0]+(square_length/2.0);xi=xi+octree_res_) 66 | for(double yi = pos->values[1]-(square_length/2.0);yi <= pos->values[1]+(square_length/2.0);yi=yi+octree_res_) 67 | for(double zi = planning_depth_-(square_length/4.0);zi <= planning_depth_+(square_length/4.0);zi=zi+octree_res_){ 68 | query.x() = xi; 69 | query.y() = yi; 70 | query.z() = zi; 71 | result = octree_->search (query); 72 | 73 | 74 | if(result != NULL){ 75 | // collision = false; 76 | // } 77 | // else{ 78 | node_occupancy = result->getOccupancy(); 79 | //std::cout << "xi:" << xi << std::endl; 80 | //std::cout << " node_occupancy: " << node_occupancy << std::endl; 81 | //std::cout << " octree_res_: " << octree_res_ << std::endl; 82 | if (node_occupancy > 0.4) 83 | { 84 | collision = true; 85 | break; 86 | } 87 | } 88 | } 89 | return !collision; 90 | } 91 | 92 | OmFclStateValidityCheckerR2::~OmFclStateValidityCheckerR2() 93 | { 94 | delete octree_; 95 | } 96 | -------------------------------------------------------------------------------- /src/state_validity_checker_octomap_R2.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * state_validity_checker_octomap_R2.hpp 3 | * 4 | * Created on: Mar 26, 2015 5 | * Author: juandhv (Juan David Hernandez Vega, juandhv@eia.udg.edu) 6 | * 7 | * State checker. Check is a given configuration (R2 state) is collision-free. 8 | * The workspace is represented by an octomap. 9 | */ 10 | 11 | #ifndef OMPL_CONTRIB_STATE_VALIDITY_CHECKER_OCTOMAP_R2_ 12 | #define OMPL_CONTRIB_STATE_VALIDITY_CHECKER_OCTOMAP_R2_ 13 | 14 | //ROS 15 | #include 16 | //ROS markers rviz 17 | #include 18 | // ROS messages 19 | #include 20 | // ROS tf 21 | #include 22 | #include 23 | 24 | //Standard libraries 25 | #include 26 | #include 27 | #include 28 | 29 | //Octomap 30 | #include 31 | #include 32 | #include 33 | 34 | //OMPL 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | //Boost 42 | #include 43 | #include 44 | #include 45 | 46 | //Eigen 47 | #include 48 | 49 | #include 50 | 51 | //ROS-Octomap interface 52 | using octomap_msgs::GetOctomap; 53 | //Standard namespace 54 | using namespace std; 55 | //Octomap namespace 56 | using namespace octomap; 57 | //OMPL namespaces 58 | namespace ob = ompl::base; 59 | namespace og = ompl::geometric; 60 | 61 | 62 | //! OmFclStateValidityCheckerR2 class. 63 | /*! 64 | Octomap State Validity checker. 65 | Extension of an abstract class used to implement the state validity checker over an octomap using FCL. 66 | */ 67 | class OmFclStateValidityCheckerR2 : public ob::StateValidityChecker { 68 | 69 | public: 70 | //! OmFclStateValidityCheckerR2 constructor. 71 | /*! 72 | * Besides of initializing the private attributes, it loads the octomap. 73 | */ 74 | OmFclStateValidityCheckerR2(const ob::SpaceInformationPtr &si, const double planning_depth, std::vector planning_bounds_x, std::vector planning_bounds_y); 75 | 76 | //! OmFclStateValidityCheckerR2 destructor. 77 | /*! 78 | * Destroy the octomap. 79 | */ 80 | ~OmFclStateValidityCheckerR2(); 81 | 82 | //! State validator. 83 | /*! 84 | * Function that verifies if the given state is valid (i.e. is free of collision) using FCL 85 | */ 86 | virtual bool isValid(const ob::State *state) const; 87 | private: 88 | //ROS 89 | ros::NodeHandle node_hand_; 90 | 91 | //Octomap 92 | octomap::AbstractOcTree* abs_octree_; 93 | octomap::OcTree* octree_; 94 | double octree_min_x_, octree_min_y_, octree_min_z_; 95 | double octree_max_x_, octree_max_y_, octree_max_z_; 96 | std::vector planning_bounds_x_, planning_bounds_y_; 97 | 98 | double planning_depth_, octree_res_; 99 | }; 100 | 101 | #endif 102 | -------------------------------------------------------------------------------- /src/turtlebot_drive.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib; roslib.load_manifest('autonomous_explore_map_plan') 5 | import rospy 6 | import tf 7 | import math 8 | import time 9 | #import the library to compute transformations 10 | from tf.transformations import euler_from_quaternion 11 | from probabilistic_lib.functions import angle_wrap #Normalize angles between -pi and pi 12 | 13 | 14 | #ROS messages 15 | from geometry_msgs.msg import Twist 16 | from nav_msgs.msg import Odometry 17 | from nav_msgs.msg import OccupancyGrid 18 | from gazebo_msgs.msg import ModelStates 19 | from autonomous_explore_map_plan.srv import GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest 20 | 21 | #Numpy 22 | import numpy as np 23 | 24 | class Controller(object): 25 | 26 | def __init__(self): 27 | 28 | self.current_position_ = np.zeros(2) 29 | self.current_orientation_ = 0.0 30 | 31 | self.desired_position_ = np.zeros(2) 32 | self.desired_orientation_ = 0.0 33 | self.vmsg=Twist() 34 | self.goal_th_xy = 0.1 35 | self.goal_th_ang = 0.01 36 | 37 | rospy.sleep(1) 38 | 39 | #rospy.init_node('turtlebot_drive', log_level=rospy.INFO) 40 | #rospy.loginfo("%s: starting turtlebot controller", rospy.get_name()) 41 | self.odometry_sub_ = rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size = 1) 42 | #self.model_sub = rospy.Subscriber("/gazebo/model_states", ModelStates, self.modelcallback) 43 | self.map_sub_ = rospy.Subscriber("/projected_map", OccupancyGrid, self.OccupancyGridCallback, queue_size = 1) 44 | self.control_input_pub_ = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size = 10) 45 | 46 | self.serv_ = rospy.Service('/turtlebot_drive/goto', 47 | GotoWaypoint, 48 | self.calculateControlInput2) 49 | 50 | 51 | 52 | # rotate once at the beginning before exploring 53 | rospy.sleep(1) 54 | self.rotateOnce() 55 | 56 | #print ('before service wait') 57 | rospy.wait_for_service('/turtlebot_drive/find_path_to_goal') 58 | try: 59 | self.find_path_to_goal_serv_ = rospy.ServiceProxy('/turtlebot_drive/find_path_to_goal', FindPathToGoal) 60 | except rospy.ServiceException, e: 61 | print "Service call failed: %s"%e 62 | 63 | return 64 | #print ('after service wait') 65 | 66 | ''' 67 | def service(self): 68 | print ('in service function') 69 | self.serv_ = rospy.Service('/turtlebot_drive/goto', 70 | GotoWaypoint, 71 | self.calculateControlInput) 72 | rospy.wait_for_service('/turtlebot_drive/find_path_to_goal') 73 | try: 74 | self.find_path_to_goal_serv_ = rospy.ServiceProxy('/turtlebot_drive/find_path_to_goal', FindPathToGoal) 75 | except rospy.ServiceException, e: 76 | print "Service call failed: %s"%e 77 | ''' 78 | def calculateControlInput2(self, req): 79 | 80 | GotoResp = GotoWaypointResponse() 81 | print ('in turtlebot_drive') 82 | planner_request = FindPathToGoalRequest() 83 | planner_request.goal_state_x = req.goal_state_x 84 | planner_request.goal_state_y = req.goal_state_y 85 | planner_response = self.find_path_to_goal_serv_(planner_request) 86 | 87 | checker = 0 88 | for pose in planner_response.poses: 89 | self.desired_position_[0] = pose.x 90 | self.desired_position_[1] = pose.y 91 | 92 | check_skip = 0 93 | r = rospy.Rate(30) 94 | while True: 95 | self.compute_velocity() # and publish 96 | #rospy.sleep(0.5) 97 | 98 | check_skip = (check_skip+1) % 4 99 | if check_skip == 0: 100 | obstacle = self.check_unknown_obstacle() 101 | if obstacle is True: 102 | print ('obstacle in path') 103 | GotoResp.FindPathFlag = 0 104 | return GotoResp 105 | 106 | xy_reach = self.has_arrived_xy() 107 | if xy_reach: 108 | print ('goal reached') 109 | #GotoResp.FindPathFlag = 1 110 | checker += 1 111 | break 112 | 113 | r.sleep() 114 | 115 | #if checker == 2: 116 | #break 117 | #return GotoWaypointResponse() 118 | GotoResp.FindPathFlag = 1 119 | return GotoResp 120 | 121 | def check_unknown_obstacle(self): 122 | # 1. defined unit vector from current pos to goal pos (gazebo unit) 123 | # 2. define segments in gazebo unit to assess obstacle 124 | # 3. for i = all seg, convert each to cell unit and check value 125 | dist_to_goal_xy = self.dist_to_goal_xy() 126 | unit_distance = 0.1 127 | 128 | x_unit_vect = (self.desired_position_[0] - self.current_position_[0])*1.1 #/ dist_to_goal_xy 129 | y_unit_vect = (self.desired_position_[1] - self.current_position_[1])*1.1 #/ dist_to_goal_xy 130 | num_seg = int(round(dist_to_goal_xy / unit_distance)) 131 | obstacle = False 132 | 133 | data1 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 134 | data2 = np.asarray(data1) 135 | x_org = self.xorg 136 | y_org = self.yorg 137 | x = self.current_position_[0] 138 | y = self.current_position_[1] 139 | 140 | 141 | for i in range(1, num_seg+1): 142 | x_gazebo = x + x_unit_vect/num_seg * i 143 | y_gazebo = y + y_unit_vect/num_seg * i 144 | dist = math.sqrt(pow(x-x_gazebo,2)+pow(y-y_gazebo,2)) 145 | x_map, y_map = self.gazebo2map(x_gazebo, y_gazebo, x_org, y_org) 146 | 147 | #print ('x_map is '+str(x_map)) 148 | #print ('y_map is '+str(y_map)) 149 | 150 | #data1 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 151 | #data2 = np.asarray(data1) 152 | 153 | #print ('in the map:'+str(data2[x_map, y_map])) 154 | 155 | # window around (x_map, y_map) all will be checked 156 | win = np.zeros((5, 5)) 157 | win = data2[x_map-2:x_map+3, y_map-2:y_map+3] 158 | if np.sum([win]) >= 100 and dist < 2: 159 | #if data2[x_map, y_map] == 100: # obstacle 160 | obstacle = True 161 | return obstacle 162 | #break 163 | return obstacle 164 | 165 | def gazebo2map(self, x_gazebo, y_gazebo, x_org, y_org): 166 | # convert units from gazebo to map 167 | x_map = int(math.floor((x_gazebo - x_org) / self.res)) 168 | y_map = int(math.floor((y_gazebo - y_org) / self.res)) 169 | #print ('x_map is '+str(x_map)) 170 | #print ('y_map is '+str(y_map)) 171 | #print ('x_org is '+str(self.xorg)) 172 | #print ('y_org is '+str(self.yorg)) 173 | #print ('x_gazebo is '+str(x_gazebo)) 174 | #print ('y_gazebo is '+str(y_gazebo)) 175 | #print ('height is '+str(self.heigh)) 176 | #print ('width is '+str(self.wid)) 177 | 178 | if x_map < 0: 179 | x_map = 0 180 | elif x_map > self.wid - 2: 181 | x_map = self.wid -1 182 | 183 | if y_map < 0: 184 | y_map = 0 185 | elif y_map > self.heigh -1: 186 | y_map = self.heigh -1 187 | 188 | 189 | #print ('x_map is '+str(x_map)) 190 | #print ('y_map is '+str(y_map)) 191 | #print ('x_org is '+str(self.xorg)) 192 | #print ('y_org is '+str(self.yorg)) 193 | #print ('x_gazebo is '+str(x_gazebo)) 194 | #print ('y_gazebo is '+str(y_gazebo)) 195 | #print ('height is '+str(self.heigh)) 196 | #print ('width is '+str(self.wid)) 197 | 198 | return x_map, y_map 199 | 200 | def dist_to_goal_xy(self): 201 | 202 | #dist_to_goal_xy computes the distance in x and y direction to the 203 | #active goal 204 | 205 | return math.sqrt(pow(self.current_position_[0]-self.desired_position_[0],2)+pow(self.current_position_[1]-self.desired_position_[1],2)) 206 | 207 | 208 | def dist_to_goal_ang(self): 209 | 210 | #dist_to_goal_ang computes the orientation distance to the active 211 | #goal 212 | 213 | return wrapAngle(self.desired_orientation_-self.current_orientation_) 214 | 215 | 216 | def has_arrived_xy(self): 217 | 218 | #has_arrived_xy returns true if the xy distance to the ative goal is 219 | #smaller than the position threshold 220 | 221 | return self.dist_to_goal_xy() self.goal_th_xy: 234 | # Compute delta_angle 235 | angle_to_goal = math.atan2(self.goals_y[self.active_goal] - self.position_y, self.goals_x[self.active_goal]-self.position_x) 236 | delta_angle_to_goal = angle_wrap(self.position_theta - angle_to_goal) 237 | 238 | # Angular speed 239 | sign_angle = delta_angle_to_goal/np.abs(delta_angle_to_goal) 240 | 241 | temp_msg.angular.z = -sign_angle*min(self.max_ang_speed, # Max value 242 | 6*np.abs(delta_angle_to_goal), # Proportional component 243 | abs(self.vmsg.angular.z)*1.1 + 0.05) # Smoothing factor 244 | 245 | # Linear speed 246 | if np.abs(delta_angle_to_goal) < 0.1: 247 | temp_msg.linear.x = min(self.max_lin_speed, # Max value 248 | 2*self.dist_to_goal_xy() + 0.1, # Proportional component 249 | self.vmsg.linear.x*1.1 + 0.05) # Smoothing factor 250 | else: 251 | temp_msg.linear.x = self.vmsg.linear.x*0.75 #Keep some speed if not facing obstacle 252 | 253 | 254 | self.vmsg = temp_msg 255 | 256 | def compute_velocity(self): 257 | 258 | #compute_velocity computes the velocity which will be published. 259 | 260 | #TODO implement! 261 | 262 | # Build new message in temporal variable 263 | #self.vmsg = Twist() 264 | self.max_lin_speed = 0.8 265 | self.max_ang_speed = 0.8 266 | 267 | 268 | 269 | if self.dist_to_goal_xy() > self.goal_th_xy: 270 | # Compute delta_angle 271 | angle_to_goal = math.atan2(self.desired_position_[1] - self.current_position_[1], self.desired_position_[0]-self.current_position_[0]) 272 | #print ('angle to goal angle before wrap:'+ str(angle_to_goal)) 273 | delta_angle_to_goal = wrapAngle(self.current_orientation_ - angle_to_goal) #wrapAngle 274 | 275 | # Angular speed 276 | sign_angle = delta_angle_to_goal/np.abs(delta_angle_to_goal) 277 | 278 | self.vmsg.angular.z = -sign_angle*min(self.max_ang_speed, # Max value 279 | 6*np.abs(delta_angle_to_goal), # Proportional component 280 | abs(self.vmsg.angular.z)*1.1 + 0.05) # Smoothing factor 281 | #print ('delta angle:'+ str(delta_angle_to_goal)) 282 | #print ('current orientation angle:'+ str(self.current_orientation_)) 283 | #print ('angle to goal angle:'+ str(angle_to_goal)) 284 | # Linear speed 285 | if np.abs(delta_angle_to_goal) < 0.1: 286 | self.vmsg.linear.x = min(self.max_lin_speed, # Max value 287 | 2*self.dist_to_goal_xy() + 0.1, # Proportional component 288 | self.vmsg.linear.x*1.1+ 0.05) # Smoothing factor 289 | else: 290 | self.vmsg.linear.x = self.vmsg.linear.x*0.75 #Keep some speed if not facing obstacle 291 | #else: 292 | #self.vmsg.linear.x = self.vmsg.linear.x * 0.5 293 | 294 | #self.vmsg = temp_msg 295 | 296 | self.control_input_pub_.publish(self.vmsg) 297 | 298 | ''' 299 | if self.dist_to_goal_xy() >= self.goal_th_xy: 300 | # Compute delta_angle 301 | angle_to_goal = math.atan2(self.desired_position_[1] - self.current_position_[1], self.desired_position_[0]-self.current_position_[0]) 302 | delta_angle_to_goal = wrapAngle(self.current_orientation_ - angle_to_goal) 303 | print ('delta angle:'+ str(delta_angle_to_goal)) 304 | print ('current orientation angle:'+ str(self.current_orientation_)) 305 | print ('current orientation angle:'+ str(angle_to_goal)) 306 | #np.abs(delta_angle_to_goal) >= 0.05: 307 | # Assign angular velocity as 2*delta_angle 308 | self.vmsg.angular.z = -1.2*delta_angle_to_goal #-1.5 309 | 310 | # If delta_angle small (facing obstacle, we can move) 311 | if np.abs(delta_angle_to_goal) < 0.02: 312 | 313 | self.vmsg.linear.x = min(0.5, 2*self.dist_to_goal_xy()) 314 | ''' 315 | #else: 316 | #self.vmsg.angular.z = -1.2*self.dist_to_goal_ang()#-1.2 317 | 318 | 319 | 320 | 321 | 322 | def odomCallback(self, odometry_msg): 323 | self.current_position_[0] = odometry_msg.pose.pose.position.x 324 | self.current_position_[1] = odometry_msg.pose.pose.position.y 325 | (r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w]) 326 | self.current_orientation_ = wrapAngle(y) 327 | return 328 | 329 | def OccupancyGridCallback(self, msg): 330 | self.dat = msg.data 331 | self.wid = msg.info.width 332 | self.heigh = msg.info.height 333 | self.res = msg.info.resolution 334 | self.xorg = msg.info.origin.position.x 335 | self.yorg = msg.info.origin.position.y 336 | 337 | 338 | def modelcallback(self, msg): 339 | #a = model_msg.name[6] 340 | #mobile_base_idx = 6 341 | #print (str(model_msg.pose[mobile_base_idx].orientation.x)) 342 | #print (str(len(model_msg.name))) 343 | mobile_base_idx = -1 344 | for i in range(len(msg.name)): 345 | #print ('in loop') 346 | if msg.name[i] == 'mobile_base': 347 | mobile_base_idx = i 348 | #print (str(mobile_base_idx)) 349 | break 350 | #print (str(mobile_base_idx)) 351 | 352 | self.current_position_[0] = msg.pose[mobile_base_idx].position.x 353 | self.current_position_[1] = msg.pose[mobile_base_idx].position.y 354 | (ang_x, ang_y, ang_z) = euler_from_quaternion([msg.pose[mobile_base_idx].orientation.x, 355 | msg.pose[mobile_base_idx].orientation.y, 356 | msg.pose[mobile_base_idx].orientation.z, 357 | msg.pose[mobile_base_idx].orientation.w]) 358 | #Update theta with euler_z 359 | self.current_orientation_ = wrapAngle(ang_z) 360 | #return 361 | 362 | 363 | def rotateOnce(self): 364 | print ('current orientation' + str(self.current_orientation_)) 365 | control_input = Twist() 366 | control_input.angular.z = 0.6 367 | #rate = rospy.Rate(10) # 10hz 368 | while np.abs(self.current_orientation_) < 0.5: 369 | #control_input.angular.z = control_input.angular.z * 1.1 + 0.05 370 | self.control_input_pub_.publish(control_input) 371 | 372 | #rospy.sleep(1.0) 373 | rotate = 0 374 | while True: 375 | #control_input.angular.z = control_input.angular.z * 1.1 + 0.05 376 | self.control_input_pub_.publish(control_input) 377 | #print ('current orientation' + str(self.current_orientation_)) 378 | #rospy.sleep(0.1) 379 | if np.abs(self.current_orientation_) < 0.1: 380 | rotate += 1 381 | print ('rotate' + str(rotate)) 382 | if rotate == 1: 383 | break 384 | 385 | 386 | def angle_wrap(angle): #angle_wrap 387 | """wrapAngle 388 | 389 | Calculates angles values between 0 and 2pi""" 390 | return (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) ) 391 | 392 | 393 | def wrapAngle(ang): 394 | """ 395 | Return the angle normalized between [-pi, pi]. 396 | 397 | Works with numbers and numpy arrays. 398 | 399 | :param ang: the input angle/s. 400 | :type ang: float, numpy.ndarray 401 | :returns: angle normalized between [-pi, pi]. 402 | :rtype: float, numpy.ndarray 403 | """ 404 | ang = ang % (2 * np.pi) 405 | if (isinstance(ang, int) or isinstance(ang, float)) and (ang > np.pi): 406 | ang -= 2 * np.pi 407 | elif isinstance(ang, np.ndarray): 408 | ang[ang > np.pi] -= 2 * np.pi 409 | return ang 410 | 411 | if __name__ == '__main__': 412 | rospy.init_node('turtlebot_drive', log_level=rospy.INFO) 413 | rospy.loginfo("%s: starting turtlebot controller", rospy.get_name()) 414 | 415 | print ('before controller') 416 | controller = Controller() 417 | print ('after controller') 418 | print (str(controller.current_orientation_)) 419 | #while not rospy.is_shutdown(): 420 | #controller.rotateOnce() 421 | 422 | #controller.service() 423 | print ('exit out drive') 424 | rospy.spin() 425 | 426 | -------------------------------------------------------------------------------- /src/turtlebot_drive.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bigmb/Autonomous-Navigation-and-Exploration/465b86c8587209f6d963bb9263846db51e292e45/src/turtlebot_drive.pyc -------------------------------------------------------------------------------- /src/turtlebot_return .py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib; roslib.load_manifest('autonomous_explore_map_plan') 5 | import rospy 6 | import tf 7 | import math 8 | import time 9 | #import the library to compute transformations 10 | from tf.transformations import euler_from_quaternion 11 | from probabilistic_lib.functions import angle_wrap #Normalize angles between -pi and pi 12 | 13 | 14 | #ROS messages 15 | from geometry_msgs.msg import Twist 16 | from nav_msgs.msg import Odometry 17 | from nav_msgs.msg import OccupancyGrid 18 | from gazebo_msgs.msg import ModelStates 19 | from autonomous_explore_map_plan.srv import GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest 20 | 21 | #Numpy 22 | import numpy as np 23 | 24 | class Returner(object): 25 | 26 | def __init__(self): 27 | #rospy.init_node('turtlebot_drive', log_level=rospy.INFO) 28 | #rospy.loginfo("%s: starting turtlebot controller", rospy.get_name()) 29 | self.current_position_ = np.zeros(2) 30 | self.current_orientation_ = 0.0 31 | 32 | self.desired_position_ = np.zeros(2) 33 | self.desired_orientation_ = 0.0 34 | self.vmsg=Twist() 35 | self.goal_th_xy = 0.1 36 | self.goal_th_ang = 0.01 37 | 38 | 39 | 40 | self.odometry_sub_ = rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size = 1) 41 | #self.model_sub = rospy.Subscriber("/gazebo/model_states", ModelStates, self.modelcallback) 42 | self.map_sub_ = rospy.Subscriber("/projected_map", OccupancyGrid, self.OccupancyGridCallback, queue_size = 1) 43 | self.control_input_pub_ = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size = 10) 44 | 45 | self.serv_ = rospy.Service('/turtlebot_return/goto', 46 | GotoWaypoint, 47 | self.calculateControlInput2) 48 | 49 | 50 | 51 | 52 | # rotate once at the beginning before exploring 53 | #rospy.sleep(1) 54 | #self.rotateOnce() 55 | 56 | #print ('before service wait') 57 | rospy.wait_for_service('/turtlebot_return/find_path_to_goal') 58 | try: 59 | self.find_path_to_goal_serv_2 = rospy.ServiceProxy('/turtlebot_return/find_path_to_goal', FindPathToGoal) 60 | except rospy.ServiceException, e: 61 | print "Service call failed: %s"%e 62 | 63 | return 64 | #print ('after service wait') 65 | 66 | ''' 67 | def service(self): 68 | print ('in service function') 69 | self.serv_ = rospy.Service('/turtlebot_drive/goto', 70 | GotoWaypoint, 71 | self.calculateControlInput) 72 | rospy.wait_for_service('/turtlebot_drive/find_path_to_goal') 73 | try: 74 | self.find_path_to_goal_serv_ = rospy.ServiceProxy('/turtlebot_drive/find_path_to_goal', FindPathToGoal) 75 | except rospy.ServiceException, e: 76 | print "Service call failed: %s"%e 77 | ''' 78 | def calculateControlInput2(self, req): 79 | 80 | GotoResp = GotoWaypointResponse() 81 | #print ('in controlinput2') 82 | planner_request = FindPathToGoalRequest() 83 | planner_request.goal_state_x = req.goal_state_x 84 | planner_request.goal_state_y = req.goal_state_y 85 | 86 | print ('in turtlebot_return function') 87 | print ('currentx' + str(self.current_position_[0])) 88 | print ('currenty' + str(self.current_position_[1])) 89 | print ('currentx' + str(req.goal_state_x)) 90 | print ('currenty' + str(req.goal_state_y)) 91 | planner_response = self.find_path_to_goal_serv_2(planner_request) 92 | 93 | checker = 0 94 | 95 | #GotoResp.FindPathFlag = 0 96 | for pose in planner_response.poses: 97 | self.desired_position_[0] = pose.x 98 | self.desired_position_[1] = pose.y 99 | 100 | check_skip = 0 101 | r = rospy.Rate(30) 102 | while True: 103 | self.compute_velocity() # and publish 104 | #rospy.sleep(0.5) 105 | 106 | check_skip = (check_skip+1) % 4 107 | if check_skip == 0: 108 | obstacle = self.check_unknown_obstacle() 109 | if obstacle is True: 110 | print ('obstacle in path') 111 | GotoResp.FindPathFlag = 0 112 | return GotoResp 113 | 114 | xy_reach = self.has_arrived_xy() 115 | if xy_reach: 116 | print ('goal reached') 117 | #GotoResp.FindPathFlag = 1 118 | checker += 1 119 | break 120 | 121 | r.sleep() 122 | 123 | #if checker == 2: 124 | #break 125 | #return GotoWaypointResponse() 126 | GotoResp.FindPathFlag = 1 127 | 128 | return GotoResp 129 | 130 | def check_unknown_obstacle(self): 131 | # 1. defined unit vector from current pos to goal pos (gazebo unit) 132 | # 2. define segments in gazebo unit to assess obstacle 133 | # 3. for i = all seg, convert each to cell unit and check value 134 | dist_to_goal_xy = self.dist_to_goal_xy() 135 | unit_distance = 0.1 136 | 137 | x_unit_vect = (self.desired_position_[0] - self.current_position_[0])*1.2 #/ dist_to_goal_xy 138 | y_unit_vect = (self.desired_position_[1] - self.current_position_[1])*1.2 #/ dist_to_goal_xy 139 | num_seg = int(round(dist_to_goal_xy / unit_distance)) 140 | obstacle = False 141 | 142 | data1 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 143 | data2 = np.asarray(data1) 144 | x_org = self.xorg 145 | y_org = self.yorg 146 | x = self.current_position_[0] 147 | y = self.current_position_[1] 148 | 149 | 150 | for i in range(1, num_seg+1): 151 | x_gazebo = x + x_unit_vect/num_seg * i 152 | y_gazebo = y + y_unit_vect/num_seg * i 153 | 154 | x_map, y_map = self.gazebo2map(x_gazebo, y_gazebo, x_org, y_org) 155 | 156 | #print ('x_map is '+str(x_map)) 157 | #print ('y_map is '+str(y_map)) 158 | 159 | #data1 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 160 | #data2 = np.asarray(data1) 161 | 162 | #print ('in the map:'+str(data2[x_map, y_map])) 163 | 164 | # window around (x_map, y_map) all will be checked 165 | win = np.zeros((5, 5)) 166 | win = data2[x_map-2:x_map+3, y_map-2:y_map+3] 167 | if np.sum([win]) >= 100: 168 | #if data2[x_map, y_map] == 100: # obstacle 169 | obstacle = True 170 | return obstacle 171 | #break 172 | return obstacle 173 | 174 | def gazebo2map(self, x_gazebo, y_gazebo, x_org, y_org): 175 | # convert units from gazebo to map 176 | x_map = int(math.floor((x_gazebo - x_org) / self.res)) 177 | y_map = int(math.floor((y_gazebo - y_org) / self.res)) 178 | #print ('x_map is '+str(x_map)) 179 | #print ('y_map is '+str(y_map)) 180 | #print ('x_org is '+str(self.xorg)) 181 | #print ('y_org is '+str(self.yorg)) 182 | #print ('x_gazebo is '+str(x_gazebo)) 183 | #print ('y_gazebo is '+str(y_gazebo)) 184 | #print ('height is '+str(self.heigh)) 185 | #print ('width is '+str(self.wid)) 186 | 187 | if x_map < 0: 188 | x_map = 0 189 | elif x_map > self.wid - 2: 190 | x_map = self.wid -1 191 | 192 | if y_map < 0: 193 | y_map = 0 194 | elif y_map > self.heigh -1: 195 | y_map = self.heigh -1 196 | 197 | 198 | #print ('x_map is '+str(x_map)) 199 | #print ('y_map is '+str(y_map)) 200 | #print ('x_org is '+str(self.xorg)) 201 | #print ('y_org is '+str(self.yorg)) 202 | #print ('x_gazebo is '+str(x_gazebo)) 203 | #print ('y_gazebo is '+str(y_gazebo)) 204 | #print ('height is '+str(self.heigh)) 205 | #print ('width is '+str(self.wid)) 206 | 207 | return x_map, y_map 208 | 209 | def dist_to_goal_xy(self): 210 | 211 | #dist_to_goal_xy computes the distance in x and y direction to the 212 | #active goal 213 | 214 | return math.sqrt(pow(self.current_position_[0]-self.desired_position_[0],2)+pow(self.current_position_[1]-self.desired_position_[1],2)) 215 | 216 | 217 | def dist_to_goal_ang(self): 218 | 219 | #dist_to_goal_ang computes the orientation distance to the active 220 | #goal 221 | 222 | return wrapAngle(self.desired_orientation_-self.current_orientation_) 223 | 224 | 225 | def has_arrived_xy(self): 226 | 227 | #has_arrived_xy returns true if the xy distance to the ative goal is 228 | #smaller than the position threshold 229 | 230 | return self.dist_to_goal_xy() self.goal_th_xy: 243 | # Compute delta_angle 244 | angle_to_goal = math.atan2(self.goals_y[self.active_goal] - self.position_y, self.goals_x[self.active_goal]-self.position_x) 245 | delta_angle_to_goal = angle_wrap(self.position_theta - angle_to_goal) 246 | 247 | # Angular speed 248 | sign_angle = delta_angle_to_goal/np.abs(delta_angle_to_goal) 249 | 250 | temp_msg.angular.z = -sign_angle*min(self.max_ang_speed, # Max value 251 | 6*np.abs(delta_angle_to_goal), # Proportional component 252 | abs(self.vmsg.angular.z)*1.1 + 0.05) # Smoothing factor 253 | 254 | # Linear speed 255 | if np.abs(delta_angle_to_goal) < 0.1: 256 | temp_msg.linear.x = min(self.max_lin_speed, # Max value 257 | 2*self.dist_to_goal_xy() + 0.1, # Proportional component 258 | self.vmsg.linear.x*1.1 + 0.05) # Smoothing factor 259 | else: 260 | temp_msg.linear.x = self.vmsg.linear.x*0.75 #Keep some speed if not facing obstacle 261 | 262 | 263 | self.vmsg = temp_msg 264 | 265 | def compute_velocity(self): 266 | 267 | #compute_velocity computes the velocity which will be published. 268 | 269 | #TODO implement! 270 | 271 | # Build new message in temporal variable 272 | #self.vmsg = Twist() 273 | self.max_lin_speed = 0.8 274 | self.max_ang_speed = 0.8 275 | 276 | 277 | 278 | if self.dist_to_goal_xy() > self.goal_th_xy: 279 | # Compute delta_angle 280 | angle_to_goal = math.atan2(self.desired_position_[1] - self.current_position_[1], self.desired_position_[0]-self.current_position_[0]) 281 | #print ('angle to goal angle before wrap:'+ str(angle_to_goal)) 282 | delta_angle_to_goal = wrapAngle(self.current_orientation_ - angle_to_goal) #wrapAngle 283 | 284 | # Angular speed 285 | sign_angle = delta_angle_to_goal/np.abs(delta_angle_to_goal) 286 | 287 | self.vmsg.angular.z = -sign_angle*min(self.max_ang_speed, # Max value 288 | 6*np.abs(delta_angle_to_goal), # Proportional component 289 | abs(self.vmsg.angular.z)*1.1 + 0.05) # Smoothing factor 290 | print ('delta angle:'+ str(delta_angle_to_goal)) 291 | #print ('current orientation angle:'+ str(self.current_orientation_)) 292 | #print ('angle to goal angle:'+ str(angle_to_goal)) 293 | # Linear speed 294 | if np.abs(delta_angle_to_goal) < 0.1: 295 | self.vmsg.linear.x = min(self.max_lin_speed, # Max value 296 | 2*self.dist_to_goal_xy() + 0.1, # Proportional component 297 | self.vmsg.linear.x*1.1+ 0.05) # Smoothing factor 298 | else: 299 | self.vmsg.linear.x = self.vmsg.linear.x*0.75 #Keep some speed if not facing obstacle 300 | 301 | 302 | #self.vmsg = temp_msg 303 | 304 | self.control_input_pub_.publish(self.vmsg) 305 | 306 | ''' 307 | if self.dist_to_goal_xy() >= self.goal_th_xy: 308 | # Compute delta_angle 309 | angle_to_goal = math.atan2(self.desired_position_[1] - self.current_position_[1], self.desired_position_[0]-self.current_position_[0]) 310 | delta_angle_to_goal = wrapAngle(self.current_orientation_ - angle_to_goal) 311 | print ('delta angle:'+ str(delta_angle_to_goal)) 312 | print ('current orientation angle:'+ str(self.current_orientation_)) 313 | print ('current orientation angle:'+ str(angle_to_goal)) 314 | #np.abs(delta_angle_to_goal) >= 0.05: 315 | # Assign angular velocity as 2*delta_angle 316 | self.vmsg.angular.z = -1.2*delta_angle_to_goal #-1.5 317 | 318 | # If delta_angle small (facing obstacle, we can move) 319 | if np.abs(delta_angle_to_goal) < 0.02: 320 | 321 | self.vmsg.linear.x = min(0.5, 2*self.dist_to_goal_xy()) 322 | ''' 323 | #else: 324 | #self.vmsg.angular.z = -1.2*self.dist_to_goal_ang()#-1.2 325 | 326 | 327 | 328 | 329 | 330 | def odomCallback(self, odometry_msg): 331 | self.current_position_[0] = odometry_msg.pose.pose.position.x 332 | self.current_position_[1] = odometry_msg.pose.pose.position.y 333 | (r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w]) 334 | self.current_orientation_ = wrapAngle(y) 335 | return 336 | 337 | def OccupancyGridCallback(self, msg): 338 | self.dat = msg.data 339 | self.wid = msg.info.width 340 | self.heigh = msg.info.height 341 | self.res = msg.info.resolution 342 | self.xorg = msg.info.origin.position.x 343 | self.yorg = msg.info.origin.position.y 344 | 345 | 346 | def modelcallback(self, msg): 347 | #a = model_msg.name[6] 348 | #mobile_base_idx = 6 349 | #print (str(model_msg.pose[mobile_base_idx].orientation.x)) 350 | #print (str(len(model_msg.name))) 351 | mobile_base_idx = -1 352 | for i in range(len(msg.name)): 353 | #print ('in loop') 354 | if msg.name[i] == 'mobile_base': 355 | mobile_base_idx = i 356 | #print (str(mobile_base_idx)) 357 | break 358 | #print (str(mobile_base_idx)) 359 | 360 | self.current_position_[0] = msg.pose[mobile_base_idx].position.x 361 | self.current_position_[1] = msg.pose[mobile_base_idx].position.y 362 | (ang_x, ang_y, ang_z) = euler_from_quaternion([msg.pose[mobile_base_idx].orientation.x, 363 | msg.pose[mobile_base_idx].orientation.y, 364 | msg.pose[mobile_base_idx].orientation.z, 365 | msg.pose[mobile_base_idx].orientation.w]) 366 | #Update theta with euler_z 367 | self.current_orientation_ = wrapAngle(ang_z) 368 | #return 369 | 370 | 371 | def rotateOnce(self): 372 | print ('current orientation' + str(self.current_orientation_)) 373 | control_input = Twist() 374 | control_input.angular.z = 0.6 375 | #rate = rospy.Rate(10) # 10hz 376 | while np.abs(self.current_orientation_) < 0.5: 377 | #control_input.angular.z = control_input.angular.z * 1.1 + 0.05 378 | self.control_input_pub_.publish(control_input) 379 | 380 | #rospy.sleep(1.0) 381 | rotate = 0 382 | while True: 383 | #control_input.angular.z = control_input.angular.z * 1.1 + 0.05 384 | self.control_input_pub_.publish(control_input) 385 | #print ('current orientation' + str(self.current_orientation_)) 386 | #rospy.sleep(0.1) 387 | if np.abs(self.current_orientation_) < 0.1: 388 | rotate += 1 389 | print ('rotate' + str(rotate)) 390 | if rotate == 1: 391 | break 392 | 393 | 394 | def angle_wrap(angle): #angle_wrap 395 | """wrapAngle 396 | 397 | Calculates angles values between 0 and 2pi""" 398 | return (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) ) 399 | 400 | 401 | def wrapAngle(ang): 402 | """ 403 | Return the angle normalized between [-pi, pi]. 404 | 405 | Works with numbers and numpy arrays. 406 | 407 | :param ang: the input angle/s. 408 | :type ang: float, numpy.ndarray 409 | :returns: angle normalized between [-pi, pi]. 410 | :rtype: float, numpy.ndarray 411 | """ 412 | ang = ang % (2 * np.pi) 413 | if (isinstance(ang, int) or isinstance(ang, float)) and (ang > np.pi): 414 | ang -= 2 * np.pi 415 | elif isinstance(ang, np.ndarray): 416 | ang[ang > np.pi] -= 2 * np.pi 417 | return ang 418 | 419 | if __name__ == '__main__': 420 | rospy.init_node('turtlebot_return', log_level=rospy.INFO) 421 | rospy.loginfo("%s: starting turtlebot returner", rospy.get_name()) 422 | 423 | print ('before controller') 424 | returner = Returner() 425 | print ('after returner') 426 | #print (str(returner.current_orientation_)) 427 | #while not rospy.is_shutdown(): 428 | #controller.rotateOnce() 429 | 430 | #controller.service() 431 | #print ('exit out drive') 432 | rospy.spin() 433 | 434 | -------------------------------------------------------------------------------- /src/turtlebot_return.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib; roslib.load_manifest('autonomous_explore_map_plan') 5 | import rospy 6 | import tf 7 | import math 8 | import time 9 | #import the library to compute transformations 10 | from tf.transformations import euler_from_quaternion 11 | from probabilistic_lib.functions import angle_wrap #Normalize angles between -pi and pi 12 | 13 | 14 | #ROS messages 15 | from geometry_msgs.msg import Twist 16 | from nav_msgs.msg import Odometry 17 | from nav_msgs.msg import OccupancyGrid 18 | from gazebo_msgs.msg import ModelStates 19 | from autonomous_explore_map_plan.srv import GotoWaypoint, GotoWaypointResponse, FindPathToGoal, FindPathToGoalResponse, FindPathToGoalRequest 20 | 21 | #Numpy 22 | import numpy as np 23 | 24 | class Returner(object): 25 | 26 | def __init__(self): 27 | #rospy.init_node('turtlebot_drive', log_level=rospy.INFO) 28 | #rospy.loginfo("%s: starting turtlebot controller", rospy.get_name()) 29 | self.current_position_ = np.zeros(2) 30 | self.current_orientation_ = 0.0 31 | 32 | self.desired_position_ = np.zeros(2) 33 | self.desired_orientation_ = 0.0 34 | self.vmsg=Twist() 35 | self.goal_th_xy = 0.1 36 | self.goal_th_ang = 0.01 37 | 38 | rospy.sleep(1) 39 | 40 | self.odometry_sub_ = rospy.Subscriber("/odom", Odometry, self.odomCallback, queue_size = 1) 41 | #self.model_sub = rospy.Subscriber("/gazebo/model_states", ModelStates, self.modelcallback) 42 | self.map_sub_ = rospy.Subscriber("/projected_map", OccupancyGrid, self.OccupancyGridCallback, queue_size = 1) 43 | self.control_input_pub_ = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size = 10) 44 | 45 | self.serv_ = rospy.Service('/turtlebot_return/goto', 46 | GotoWaypoint, 47 | self.calculateControlInput2) 48 | 49 | 50 | 51 | 52 | # rotate once at the beginning before exploring 53 | #rospy.sleep(1) 54 | #self.rotateOnce() 55 | 56 | #print ('before service wait') 57 | rospy.wait_for_service('/turtlebot_return/find_path_to_goal') 58 | try: 59 | self.find_path_to_goal_serv_ = rospy.ServiceProxy('/turtlebot_return/find_path_to_goal', FindPathToGoal) 60 | except rospy.ServiceException, e: 61 | print "Service call failed: %s"%e 62 | 63 | return 64 | #print ('after service wait') 65 | 66 | ''' 67 | def service(self): 68 | print ('in service function') 69 | self.serv_ = rospy.Service('/turtlebot_drive/goto', 70 | GotoWaypoint, 71 | self.calculateControlInput) 72 | rospy.wait_for_service('/turtlebot_drive/find_path_to_goal') 73 | try: 74 | self.find_path_to_goal_serv_ = rospy.ServiceProxy('/turtlebot_drive/find_path_to_goal', FindPathToGoal) 75 | except rospy.ServiceException, e: 76 | print "Service call failed: %s"%e 77 | ''' 78 | def calculateControlInput2(self, req): 79 | 80 | GotoResp = GotoWaypointResponse() 81 | print ('in controlinput2') 82 | planner_request = FindPathToGoalRequest() 83 | planner_request.goal_state_x = req.goal_state_x 84 | planner_request.goal_state_y = req.goal_state_y 85 | planner_response = self.find_path_to_goal_serv_(planner_request) 86 | 87 | checker = 0 88 | for pose in planner_response.poses: 89 | self.desired_position_[0] = pose.x 90 | self.desired_position_[1] = pose.y 91 | 92 | check_skip = 0 93 | r = rospy.Rate(30) 94 | while True: 95 | self.compute_velocity() # and publish 96 | #rospy.sleep(0.5) 97 | 98 | check_skip = (check_skip+1) % 4 99 | if check_skip == 0: 100 | obstacle = self.check_unknown_obstacle() 101 | if obstacle is True: 102 | print ('obstacle in path') 103 | GotoResp.FindPathFlag = 0 104 | return GotoResp 105 | 106 | xy_reach = self.has_arrived_xy() 107 | if xy_reach: 108 | print ('goal reached') 109 | checker += 1 110 | break 111 | 112 | r.sleep() 113 | 114 | #if checker == 2: 115 | #break 116 | #return GotoWaypointResponse() 117 | GotoResp.FindPathFlag = 1 118 | return GotoResp 119 | 120 | def check_unknown_obstacle(self): 121 | # 1. defined unit vector from current pos to goal pos (gazebo unit) 122 | # 2. define segments in gazebo unit to assess obstacle 123 | # 3. for i = all seg, convert each to cell unit and check value 124 | dist_to_goal_xy = self.dist_to_goal_xy() 125 | unit_distance = 0.1 126 | 127 | x_unit_vect = (self.desired_position_[0] - self.current_position_[0])*1.2 #/ dist_to_goal_xy 128 | y_unit_vect = (self.desired_position_[1] - self.current_position_[1])*1.2 #/ dist_to_goal_xy 129 | num_seg = int(round(dist_to_goal_xy / unit_distance)) 130 | obstacle = False 131 | 132 | data1 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 133 | data2 = np.asarray(data1) 134 | x_org = self.xorg 135 | y_org = self.yorg 136 | x = self.current_position_[0] 137 | y = self.current_position_[1] 138 | 139 | 140 | for i in range(1, num_seg+1): 141 | x_gazebo = x + x_unit_vect/num_seg * i 142 | y_gazebo = y + y_unit_vect/num_seg * i 143 | 144 | x_map, y_map = self.gazebo2map(x_gazebo, y_gazebo, x_org, y_org) 145 | 146 | #print ('x_map is '+str(x_map)) 147 | #print ('y_map is '+str(y_map)) 148 | 149 | #data1 = np.reshape(self.dat, (self.wid ,self.heigh), order="F") 150 | #data2 = np.asarray(data1) 151 | 152 | #print ('in the map:'+str(data2[x_map, y_map])) 153 | 154 | # window around (x_map, y_map) all will be checked 155 | win = np.zeros((5, 5)) 156 | win = data2[x_map-2:x_map+3, y_map-2:y_map+3] 157 | if np.sum([win]) >= 100: 158 | #if data2[x_map, y_map] == 100: # obstacle 159 | obstacle = True 160 | return obstacle 161 | #break 162 | return obstacle 163 | 164 | def gazebo2map(self, x_gazebo, y_gazebo, x_org, y_org): 165 | # convert units from gazebo to map 166 | x_map = int(math.floor((x_gazebo - x_org) / self.res)) 167 | y_map = int(math.floor((y_gazebo - y_org) / self.res)) 168 | #print ('x_map is '+str(x_map)) 169 | #print ('y_map is '+str(y_map)) 170 | #print ('x_org is '+str(self.xorg)) 171 | #print ('y_org is '+str(self.yorg)) 172 | #print ('x_gazebo is '+str(x_gazebo)) 173 | #print ('y_gazebo is '+str(y_gazebo)) 174 | #print ('height is '+str(self.heigh)) 175 | #print ('width is '+str(self.wid)) 176 | 177 | if x_map < 0: 178 | x_map = 0 179 | elif x_map > self.wid - 2: 180 | x_map = self.wid -1 181 | 182 | if y_map < 0: 183 | y_map = 0 184 | elif y_map > self.heigh -1: 185 | y_map = self.heigh -1 186 | 187 | 188 | #print ('x_map is '+str(x_map)) 189 | #print ('y_map is '+str(y_map)) 190 | #print ('x_org is '+str(self.xorg)) 191 | #print ('y_org is '+str(self.yorg)) 192 | #print ('x_gazebo is '+str(x_gazebo)) 193 | #print ('y_gazebo is '+str(y_gazebo)) 194 | #print ('height is '+str(self.heigh)) 195 | #print ('width is '+str(self.wid)) 196 | 197 | return x_map, y_map 198 | 199 | def dist_to_goal_xy(self): 200 | 201 | #dist_to_goal_xy computes the distance in x and y direction to the 202 | #active goal 203 | 204 | return math.sqrt(pow(self.current_position_[0]-self.desired_position_[0],2)+pow(self.current_position_[1]-self.desired_position_[1],2)) 205 | 206 | 207 | def dist_to_goal_ang(self): 208 | 209 | #dist_to_goal_ang computes the orientation distance to the active 210 | #goal 211 | 212 | return wrapAngle(self.desired_orientation_-self.current_orientation_) 213 | 214 | 215 | def has_arrived_xy(self): 216 | 217 | #has_arrived_xy returns true if the xy distance to the ative goal is 218 | #smaller than the position threshold 219 | 220 | return self.dist_to_goal_xy() self.goal_th_xy: 233 | # Compute delta_angle 234 | angle_to_goal = math.atan2(self.goals_y[self.active_goal] - self.position_y, self.goals_x[self.active_goal]-self.position_x) 235 | delta_angle_to_goal = angle_wrap(self.position_theta - angle_to_goal) 236 | 237 | # Angular speed 238 | sign_angle = delta_angle_to_goal/np.abs(delta_angle_to_goal) 239 | 240 | temp_msg.angular.z = -sign_angle*min(self.max_ang_speed, # Max value 241 | 6*np.abs(delta_angle_to_goal), # Proportional component 242 | abs(self.vmsg.angular.z)*1.1 + 0.05) # Smoothing factor 243 | 244 | # Linear speed 245 | if np.abs(delta_angle_to_goal) < 0.1: 246 | temp_msg.linear.x = min(self.max_lin_speed, # Max value 247 | 2*self.dist_to_goal_xy() + 0.1, # Proportional component 248 | self.vmsg.linear.x*1.1 + 0.05) # Smoothing factor 249 | else: 250 | temp_msg.linear.x = self.vmsg.linear.x*0.75 #Keep some speed if not facing obstacle 251 | 252 | 253 | self.vmsg = temp_msg 254 | 255 | def compute_velocity(self): 256 | 257 | #compute_velocity computes the velocity which will be published. 258 | 259 | #TODO implement! 260 | 261 | # Build new message in temporal variable 262 | #self.vmsg = Twist() 263 | self.max_lin_speed = 0.8 264 | self.max_ang_speed = 0.8 265 | 266 | 267 | 268 | if self.dist_to_goal_xy() > self.goal_th_xy: 269 | # Compute delta_angle 270 | angle_to_goal = math.atan2(self.desired_position_[1] - self.current_position_[1], self.desired_position_[0]-self.current_position_[0]) 271 | #print ('angle to goal angle before wrap:'+ str(angle_to_goal)) 272 | delta_angle_to_goal = wrapAngle(self.current_orientation_ - angle_to_goal) #wrapAngle 273 | 274 | # Angular speed 275 | sign_angle = delta_angle_to_goal/np.abs(delta_angle_to_goal) 276 | 277 | self.vmsg.angular.z = -sign_angle*min(self.max_ang_speed, # Max value 278 | 6*np.abs(delta_angle_to_goal), # Proportional component 279 | abs(self.vmsg.angular.z)*1.1 + 0.05) # Smoothing factor 280 | print ('delta angle:'+ str(delta_angle_to_goal)) 281 | #print ('current orientation angle:'+ str(self.current_orientation_)) 282 | #print ('angle to goal angle:'+ str(angle_to_goal)) 283 | # Linear speed 284 | if np.abs(delta_angle_to_goal) < 0.1: 285 | self.vmsg.linear.x = min(self.max_lin_speed, # Max value 286 | 2*self.dist_to_goal_xy() + 0.1, # Proportional component 287 | self.vmsg.linear.x*1.1+ 0.05) # Smoothing factor 288 | else: 289 | self.vmsg.linear.x = self.vmsg.linear.x*0.75 #Keep some speed if not facing obstacle 290 | 291 | 292 | #self.vmsg = temp_msg 293 | 294 | self.control_input_pub_.publish(self.vmsg) 295 | 296 | ''' 297 | if self.dist_to_goal_xy() >= self.goal_th_xy: 298 | # Compute delta_angle 299 | angle_to_goal = math.atan2(self.desired_position_[1] - self.current_position_[1], self.desired_position_[0]-self.current_position_[0]) 300 | delta_angle_to_goal = wrapAngle(self.current_orientation_ - angle_to_goal) 301 | print ('delta angle:'+ str(delta_angle_to_goal)) 302 | print ('current orientation angle:'+ str(self.current_orientation_)) 303 | print ('current orientation angle:'+ str(angle_to_goal)) 304 | #np.abs(delta_angle_to_goal) >= 0.05: 305 | # Assign angular velocity as 2*delta_angle 306 | self.vmsg.angular.z = -1.2*delta_angle_to_goal #-1.5 307 | 308 | # If delta_angle small (facing obstacle, we can move) 309 | if np.abs(delta_angle_to_goal) < 0.02: 310 | 311 | self.vmsg.linear.x = min(0.5, 2*self.dist_to_goal_xy()) 312 | ''' 313 | #else: 314 | #self.vmsg.angular.z = -1.2*self.dist_to_goal_ang()#-1.2 315 | 316 | 317 | 318 | 319 | 320 | def odomCallback(self, odometry_msg): 321 | self.current_position_[0] = odometry_msg.pose.pose.position.x 322 | self.current_position_[1] = odometry_msg.pose.pose.position.y 323 | (r, p, y) = tf.transformations.euler_from_quaternion([odometry_msg.pose.pose.orientation.x, odometry_msg.pose.pose.orientation.y, odometry_msg.pose.pose.orientation.z, odometry_msg.pose.pose.orientation.w]) 324 | self.current_orientation_ = wrapAngle(y) 325 | return 326 | 327 | def OccupancyGridCallback(self, msg): 328 | self.dat = msg.data 329 | self.wid = msg.info.width 330 | self.heigh = msg.info.height 331 | self.res = msg.info.resolution 332 | self.xorg = msg.info.origin.position.x 333 | self.yorg = msg.info.origin.position.y 334 | 335 | 336 | def modelcallback(self, msg): 337 | #a = model_msg.name[6] 338 | #mobile_base_idx = 6 339 | #print (str(model_msg.pose[mobile_base_idx].orientation.x)) 340 | #print (str(len(model_msg.name))) 341 | mobile_base_idx = -1 342 | for i in range(len(msg.name)): 343 | #print ('in loop') 344 | if msg.name[i] == 'mobile_base': 345 | mobile_base_idx = i 346 | #print (str(mobile_base_idx)) 347 | break 348 | #print (str(mobile_base_idx)) 349 | 350 | self.current_position_[0] = msg.pose[mobile_base_idx].position.x 351 | self.current_position_[1] = msg.pose[mobile_base_idx].position.y 352 | (ang_x, ang_y, ang_z) = euler_from_quaternion([msg.pose[mobile_base_idx].orientation.x, 353 | msg.pose[mobile_base_idx].orientation.y, 354 | msg.pose[mobile_base_idx].orientation.z, 355 | msg.pose[mobile_base_idx].orientation.w]) 356 | #Update theta with euler_z 357 | self.current_orientation_ = wrapAngle(ang_z) 358 | #return 359 | 360 | 361 | def rotateOnce(self): 362 | print ('current orientation' + str(self.current_orientation_)) 363 | control_input = Twist() 364 | control_input.angular.z = 0.6 365 | #rate = rospy.Rate(10) # 10hz 366 | while np.abs(self.current_orientation_) < 0.5: 367 | #control_input.angular.z = control_input.angular.z * 1.1 + 0.05 368 | self.control_input_pub_.publish(control_input) 369 | 370 | #rospy.sleep(1.0) 371 | rotate = 0 372 | while True: 373 | #control_input.angular.z = control_input.angular.z * 1.1 + 0.05 374 | self.control_input_pub_.publish(control_input) 375 | #print ('current orientation' + str(self.current_orientation_)) 376 | #rospy.sleep(0.1) 377 | if np.abs(self.current_orientation_) < 0.1: 378 | rotate += 1 379 | print ('rotate' + str(rotate)) 380 | if rotate == 1: 381 | break 382 | 383 | 384 | def angle_wrap(angle): #angle_wrap 385 | """wrapAngle 386 | 387 | Calculates angles values between 0 and 2pi""" 388 | return (angle + ( 2.0 * math.pi * math.floor( ( math.pi - angle ) / ( 2.0 * math.pi ) ) ) ) 389 | 390 | 391 | def wrapAngle(ang): 392 | """ 393 | Return the angle normalized between [-pi, pi]. 394 | 395 | Works with numbers and numpy arrays. 396 | 397 | :param ang: the input angle/s. 398 | :type ang: float, numpy.ndarray 399 | :returns: angle normalized between [-pi, pi]. 400 | :rtype: float, numpy.ndarray 401 | """ 402 | ang = ang % (2 * np.pi) 403 | if (isinstance(ang, int) or isinstance(ang, float)) and (ang > np.pi): 404 | ang -= 2 * np.pi 405 | elif isinstance(ang, np.ndarray): 406 | ang[ang > np.pi] -= 2 * np.pi 407 | return ang 408 | 409 | if __name__ == '__main__': 410 | rospy.init_node('turtlebot_return', log_level=rospy.INFO) 411 | rospy.loginfo("%s: starting turtlebot returner", rospy.get_name()) 412 | 413 | print ('before controller') 414 | returner = Returner() 415 | print ('after returner') 416 | #print (str(returner.current_orientation_)) 417 | #while not rospy.is_shutdown(): 418 | #controller.rotateOnce() 419 | 420 | #controller.service() 421 | #print ('exit out drive') 422 | rospy.spin() 423 | 424 | -------------------------------------------------------------------------------- /src/turtlebot_return.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/bigmb/Autonomous-Navigation-and-Exploration/465b86c8587209f6d963bb9263846db51e292e45/src/turtlebot_return.pyc -------------------------------------------------------------------------------- /srv/FindPathToGoal.srv: -------------------------------------------------------------------------------- 1 | ## Service 2 | float32 goal_state_x 3 | float32 goal_state_y 4 | --- 5 | geometry_msgs/Pose2D[] poses -------------------------------------------------------------------------------- /srv/GotoWaypoint.srv: -------------------------------------------------------------------------------- 1 | ## Service 2 | float32 goal_state_x 3 | float32 goal_state_y 4 | --- 5 | int8 FindPathFlag --------------------------------------------------------------------------------