├── .gitignore ├── README.md ├── ap_msgs ├── CMakeLists.txt ├── msg │ └── NavStatus.msg ├── package.xml └── srv │ ├── GetEpisodeComplete.srv │ ├── GetEpisodeNames.srv │ ├── GetEpisodeSummary.srv │ ├── GetLatestEpisodeName.srv │ ├── PauseResumePatroller.srv │ ├── SaveMap.srv │ ├── SaveWaypoint.srv │ └── SaveWaypointFile.srv ├── autonomous_patrolling ├── CMakeLists.txt └── package.xml ├── doc ├── joypad-cheat-sheet.jpg ├── joypad-cheat-sheet.odg └── joypad-cheat-sheet.pdf ├── nav_monitor ├── CMakeLists.txt ├── package.xml └── scripts │ └── nav_monitor.py └── waypoint_patroller ├── CMakeLists.txt ├── cfg └── PatrollerTresholds.cfg ├── launch └── long_term_patroller.launch ├── package.xml ├── scripts ├── log_inspector.py ├── log_services.py ├── long_term_patroller.py └── web_publisher.py ├── setup.py └── src └── waypoint_patroller ├── __init__.py ├── charging.py ├── log_util.py ├── logger.py ├── monitor_states.py ├── navigation.py ├── patroller.py └── recover_states.py /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | devel/ 3 | lib/ 4 | bin/ 5 | *~ 6 | .* 7 | src/map_saver/maps/* 8 | src/waypoint_recorder/waypoints/* 9 | *.pyc 10 | 11 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # The autonomous patrolling package 2 | 3 | 4 | This package allows for the sequential or randomized visit of a set of pre-defined waypoints in the environment, along with a charging behaviour when the battery level drops below a given treshold. It also has two tools for creating and saving a map, and for creating and saving a set of waypoints. 5 | It assumes that either a [morse simulation](https://github.com/strands-project/strands_morse) or the [scitos_bringup](https://github.com/strands-project/scitos_robot) have been launched 6 | 7 | 8 | ## The map_saver 9 | 10 | ### Without rumblepad 11 | 12 | Prior to waypoint autonomous patrolling, a map of the area to be patrolled needs to be created. For that: 13 | 14 | 1. Have the robot docked (the docking station must be the origin of the odometry frame) 15 | 16 | 2. Run the `map_saver` package: 17 | 18 | $ roslaunch map_saver ap_map_saver.launch 19 | 20 | 21 | The `ap_map_saver` provides a `SaveMap` service, that takes as input a string of the form `"path to folder where to save the map"/map_name` (e.g., `/home/strands_user/map`) and creates the `map_name.yaml` and `map_name.pgm` files in the selected folder. 22 | 23 | 24 | ### With rumblepad 25 | 26 | The `ap_map_saver` can also be used in conjunction with the rumblepad, where the user can build a map by driving the robot around and then save it. To do this: 27 | 28 | * Have the robot docked (the docking station must be the origin of the odometry frame) 29 | 30 | * Before launching `ap_map_saver.launch`, launch the teleop_app: 31 | 32 | $ roslaunch scitos_teleop teleop_joystick.launch js:=/dev/input/"joystick name" 33 | 34 | * Launch the `ap_map_saver`, with an optional argument: 35 | 36 | $ roslaunch map_saver ap_map_saver.launch map:="path to folder where to save the map"/map_name 37 | 38 | * (Optional) Start rviz to visualize the map creation: 39 | 40 | $ rosrun rviz rviz 41 | 42 | * Drive the robot around using the rumblepad 43 | 44 | * To save the map, press the (A) button. 45 | 46 | * The optional `map` argument sets the path to save the map. This string has the same format as the one used for the `SaveMap` service. It's default value is `~/map`. 47 | 48 | 49 | 50 | ## The waypoint recorder 51 | 52 | 53 | ### Without rumblepad 54 | 55 | Prior to waypoints autonomous patrolling, a file with the waypoints to be visited needs to be created. For that, the `waypoint_recorder` package can be used: 56 | 57 | $ roslaunch waypoint_recorder waypoint_recorder map:="file path to the map's .yaml file" 58 | 59 | 60 | The `waypoint_recorder` provides two services: 61 | 62 | * A `SaveWaypoint` service, that saves the next pose published by amcl in a list. 63 | 64 | * A `SaveWaypointFile` service, that receives a string specifying the file path of the output file (e.g., `/home/strands_user/waypoints.csv`), and writes all the waypoints saved in the list to that file. 65 | 66 | 67 | 68 | 69 | 70 | ### With rumblepad 71 | 72 | The `waypoint_recorder` can also be used in conjunction with the rumblepad, where the user can drive the robot around, create waypoints, and then save them to a file. To do this: 73 | 74 | * Before launching `waypoint_recorder.launch`, launch the teleop_app: 75 | 76 | $ roslaunch scitos_teleop teleop_joystick.launch js:=/dev/input/"joystick name" 77 | 78 | * Launch the `waypoint_recorder`, with an optional argument: 79 | 80 | $ roslaunch waypoint_recorder waypoint_recorder.launch map:="file path to the map's .yaml file" waypoints:="file path to the file where waypoints shoud be saved" 81 | 82 | * (Optional) Start rviz to check if the robot is well localized, and give it a pose estimate if needed: 83 | 84 | $ rosrun rviz rviz 85 | 86 | * Drive the robot around using the rumblepad 87 | 88 | * To save waypoints to the list, press the (A) button. NOTE: The FIRST point you save should be the one in front of your charging station, so that the robot knows where to navigate when it needs to charge. 89 | 90 | * To save the list of waypoints to a file, press the (B) button. 91 | 92 | * The optional `waypoints` argument sets the file path to save the waypoints. This string has the same format as the one used for the `SaveWaypointFile` service. It's default value is `~/waypoints.csv`. 93 | 94 | 95 | 96 | ## The waypoint patroller 97 | 98 | Aunonomously visits a pre-defined list of points randomly or in sequence. Goes to charge when battery drops below a given treshold and after it is recharged, continues the patrolling. Assumes static map and waypoints set are given as input. 99 | 100 | * Launch the scitos 2d navigation: 101 | 102 | $ roslaunch scitos_2d_navigation scitos_2d_nav.launch map:="file path to the map's .yaml file" 103 | 104 | * The 2d navigation is kept separated from the autonomous patroller so one can kill the patrolling process without killing the navigation related nodes. 105 | 106 | 107 | * (Optional) Start rviz to check if the robot is well localized, and give it a pose estimate if needed: 108 | 109 | $ rosrun rviz rviz 110 | 111 | * Launch the mongodb_store: 112 | 113 | $ HOSTNAME=bob roslaunch mongodb_store datacentre.launch 114 | 115 | 116 | * If you haven't inserted waypoints in your datacentre, you can insert the waypoints from a waypoint log file (as created using the waypoint recorder) into your datacentre: 117 | 118 | $ rosrun waypoint_recorder insert_in_db.py waypoints.csv point_set_name map_name 119 | 120 | Currently map_name is unimportant (until the maps are also stored in db), and the first waypoint is assumed to be the pre-charging waypoint. 121 | 122 | 123 | 124 | 125 | 126 | 127 | * Run the autonomous docking service: 128 | 129 | $ roslaunch scitos_docking charging.launch 130 | 131 | 132 | * If this is your first run, or you changed the robot station's location, you need to run the docking calibration: 133 | 134 | $ rosrun actionlib axclient.py /chargingServer 135 | 136 | 137 | Then in the `Goal` textfield complete as follows: 138 | 139 | Command: calibrate 140 | Timeout: 1000 141 | 142 | 143 | Then press the `SEND GOAL` button. 144 | 145 | * Launch the marathon gui, where HOST_IP should be set to an external ip for your machine if you want this to be externally accessible, else can be left blank for 127.0.0.1. E.g.: 146 | 147 | $ HOST_IP=10.0.11.158 roslaunch marathon_touch_gui marathon_gui_dependencies.launch 148 | 149 | * WARNING: If you are running the marathon gui for the first time, before launching, follow the installation instuctions on https://github.com/strands-project/strands_ui 150 | 151 | * After launching, open a browser window and navigate to `HOST_IP:8090`, where HOST_IP is the one you just set above 152 | 153 | 154 | * Launch the patroller: 155 | 156 | $ roslaunch waypoint_patroller long_term_patroller.launch waypoints:=waypoint_set_name 157 | 158 | 159 | * The optional argument randomized can be true or false. Default is true. If false is given, then the points are visited sequentially 160 | * The optional argument n_it specifies how many complete iterations of all the points should be done before the patroller outputs succeeded. Default is -1, which means infinite iterations 161 | * NOTE: For the robot to speak and ask for help, the user that launches this file needs to be logged in the robot's computer. This is related to [this](https://github.com/strands-project/strands_hri/issues/7) 162 | * If you have the gamepad running, you can pause the patroller and assume control by pressing the dead-man switch. When the dead-man switch is released the patroller resumes patrolling the previous waypoint. 163 | * You can also pause execution and regain the possibility to control the robot via other sources (e.g. rviz) by running: 164 | 165 | $ rosservice call /pause_resume_patroller 166 | 167 | To resume, just send another service call as above 168 | 169 | 170 | -------------------------------------------------------------------------------- /ap_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ap_msgs) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED std_msgs message_generation) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | add_message_files( 24 | FILES 25 | NavStatus.msg 26 | ) 27 | 28 | ## Generate services in the 'srv' folder 29 | add_service_files( 30 | FILES 31 | SaveMap.srv 32 | SaveWaypoint.srv 33 | SaveWaypointFile.srv 34 | PauseResumePatroller.srv 35 | GetEpisodeNames.srv 36 | GetLatestEpisodeName.srv 37 | GetEpisodeSummary.srv 38 | GetEpisodeComplete.srv 39 | # replacing with std_srvs/Empty 40 | # BumperRecovered.srv 41 | # RobotMoved.srv 42 | # HelpOffered.srv 43 | ) 44 | 45 | ## Generate added messages and services with any dependencies listed here 46 | generate_messages( 47 | DEPENDENCIES 48 | std_msgs 49 | ) 50 | 51 | ################################### 52 | ## catkin specific configuration ## 53 | ################################### 54 | ## The catkin_package macro generates cmake config files for your package 55 | ## Declare things to be passed to dependent projects 56 | ## LIBRARIES: libraries you create in this project that dependent projects also need 57 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 58 | ## DEPENDS: system dependencies of this project that dependent projects also need 59 | catkin_package( 60 | # INCLUDE_DIRS include 61 | # LIBRARIES rumblepad_control 62 | # CATKIN_DEPENDS std_msgs message_runtime 63 | # DEPENDS system_lib 64 | ) 65 | 66 | ########### 67 | ## Build ## 68 | ########### 69 | 70 | ## Specify additional locations of header files 71 | ## Your package locations should be listed before other locations 72 | #include_directories(include 73 | # ${catkin_INCLUDE_DIRS}, 74 | # ${scitos_msgs_INCLUDE_DIRS} 75 | #) 76 | 77 | ## Declare a cpp library 78 | # add_library(rumblepad_control 79 | # src/${PROJECT_NAME}/rumblepad_control.cpp 80 | # ) 81 | 82 | ## Declare a cpp executable 83 | #add_executable(rumble_control src/rumble_control.cpp) 84 | 85 | ## Add cmake target dependencies of the executable/library 86 | ## as an example, message headers may need to be generated before nodes 87 | # add_dependencies(rumblepad_control_node rumblepad_control_generate_messages_cpp) 88 | 89 | ## Specify libraries to link a library or executable target against 90 | #target_link_libraries(rumble_control ${catkin_LIBRARIES} ${scitos_msgs_LIBRARIES}) 91 | 92 | ############# 93 | ## Install ## 94 | ############# 95 | 96 | # all install targets should use catkin DESTINATION variables 97 | # See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html 98 | 99 | ## Mark executable scripts (Python etc.) for installation 100 | ## in contrast to setup.py, you can choose the destination 101 | # install(PROGRAMS 102 | # scripts/my_python_script 103 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 104 | # ) 105 | 106 | ## Mark executables and/or libraries for installation 107 | # install(TARGETS rumblepad_control rumblepad_control_node 108 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 109 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 110 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 111 | # ) 112 | 113 | ## Mark cpp header files for installation 114 | # install(DIRECTORY include/${PROJECT_NAME}/ 115 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 116 | # FILES_MATCHING PATTERN "*.h" 117 | # PATTERN ".svn" EXCLUDE 118 | # ) 119 | 120 | ## Mark other files for installation (e.g. launch and bag files, etc.) 121 | # install(FILES 122 | # # myfile1 123 | # # myfile2 124 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 125 | # ) 126 | 127 | ############# 128 | ## Testing ## 129 | ############# 130 | 131 | ## Add gtest based cpp test target and link libraries 132 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rumblepad_control.cpp) 133 | # if(TARGET ${PROJECT_NAME}-test) 134 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 135 | # endif() 136 | 137 | ## Add folders to be run by python nosetests 138 | # catkin_add_nosetests(test) 139 | -------------------------------------------------------------------------------- /ap_msgs/msg/NavStatus.msg: -------------------------------------------------------------------------------- 1 | bool carpet_stuck 2 | bool is_paused 3 | -------------------------------------------------------------------------------- /ap_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ap_msgs 4 | 0.0.0 5 | The autonomous_patrolling messages package 6 | 7 | Christian Dondrup 8 | 9 | 10 | 11 | 12 | 13 | BSD 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 | catkin 40 | 41 | message_generation 42 | std_msgs 43 | 44 | std_msgs 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | -------------------------------------------------------------------------------- /ap_msgs/srv/GetEpisodeComplete.srv: -------------------------------------------------------------------------------- 1 | string episode_name 2 | --- 3 | string complete -------------------------------------------------------------------------------- /ap_msgs/srv/GetEpisodeNames.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | string[] episodes -------------------------------------------------------------------------------- /ap_msgs/srv/GetEpisodeSummary.srv: -------------------------------------------------------------------------------- 1 | string episode_name 2 | --- 3 | string summary -------------------------------------------------------------------------------- /ap_msgs/srv/GetLatestEpisodeName.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | string name -------------------------------------------------------------------------------- /ap_msgs/srv/PauseResumePatroller.srv: -------------------------------------------------------------------------------- 1 | 2 | --- 3 | string result -------------------------------------------------------------------------------- /ap_msgs/srv/SaveMap.srv: -------------------------------------------------------------------------------- 1 | string file_name 2 | --- -------------------------------------------------------------------------------- /ap_msgs/srv/SaveWaypoint.srv: -------------------------------------------------------------------------------- 1 | 2 | --- -------------------------------------------------------------------------------- /ap_msgs/srv/SaveWaypointFile.srv: -------------------------------------------------------------------------------- 1 | string file_name 2 | --- -------------------------------------------------------------------------------- /autonomous_patrolling/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autonomous_patrolling) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() -------------------------------------------------------------------------------- /autonomous_patrolling/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autonomous_patrolling 4 | 0.0.0 5 | The autonomous_patrolling package 6 | 7 | 8 | 9 | 10 | lacerdab 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 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /doc/joypad-cheat-sheet.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/autonomous_patrolling/fef1c81b42b4cdfe56b823f212bbb7dfb2504ee0/doc/joypad-cheat-sheet.jpg -------------------------------------------------------------------------------- /doc/joypad-cheat-sheet.odg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/autonomous_patrolling/fef1c81b42b4cdfe56b823f212bbb7dfb2504ee0/doc/joypad-cheat-sheet.odg -------------------------------------------------------------------------------- /doc/joypad-cheat-sheet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strands-project/autonomous_patrolling/fef1c81b42b4cdfe56b823f212bbb7dfb2504ee0/doc/joypad-cheat-sheet.pdf -------------------------------------------------------------------------------- /nav_monitor/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(nav_monitor) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS geometry_msgs rospy std_msgs nav_msgs sensor_msgs ap_msgs) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | ## Generate messages in the 'msg' folder 23 | # add_message_files( 24 | # FILES 25 | # Message1.msg 26 | # Message2.msg 27 | # ) 28 | 29 | ## Generate services in the 'srv' folder 30 | # add_service_files( 31 | # FILES 32 | # Service1.srv 33 | # Service2.srv 34 | # ) 35 | 36 | ## Generate added messages and services with any dependencies listed here 37 | # generate_messages( 38 | # DEPENDENCIES 39 | # geometry_msgs# std_msgs 40 | # ) 41 | 42 | ################################### 43 | ## catkin specific configuration ## 44 | ################################### 45 | ## The catkin_package macro generates cmake config files for your package 46 | ## Declare things to be passed to dependent projects 47 | ## INCLUDE_DIRS: uncomment this if you package contains header files 48 | ## LIBRARIES: libraries you create in this project that dependent projects also need 49 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 50 | ## DEPENDS: system dependencies of this project that dependent projects also need 51 | catkin_package( 52 | # INCLUDE_DIRS include 53 | # LIBRARIES nav_monitor 54 | # CATKIN_DEPENDS geometry_msgs roscpp rospy std_msgs 55 | # DEPENDS system_lib 56 | ) 57 | 58 | ########### 59 | ## Build ## 60 | ########### 61 | 62 | ## Specify additional locations of header files 63 | ## Your package locations should be listed before other locations 64 | # include_directories(include) 65 | include_directories( 66 | ${catkin_INCLUDE_DIRS} 67 | ) 68 | 69 | ## Declare a cpp library 70 | # add_library(nav_monitor 71 | # src/${PROJECT_NAME}/nav_monitor.cpp 72 | # ) 73 | 74 | ## Declare a cpp executable 75 | # add_executable(nav_monitor_node src/nav_monitor_node.cpp) 76 | 77 | ## Add cmake target dependencies of the executable/library 78 | ## as an example, message headers may need to be generated before nodes 79 | # add_dependencies(nav_monitor_node nav_monitor_generate_messages_cpp) 80 | 81 | ## Specify libraries to link a library or executable target against 82 | # target_link_libraries(nav_monitor_node 83 | # ${catkin_LIBRARIES} 84 | # ) 85 | 86 | ############# 87 | ## Install ## 88 | ############# 89 | 90 | # all install targets should use catkin DESTINATION variables 91 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 92 | 93 | ## Mark executable scripts (Python etc.) for installation 94 | ## in contrast to setup.py, you can choose the destination 95 | # install(PROGRAMS 96 | # scripts/my_python_script 97 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 98 | # ) 99 | 100 | ## Mark executables and/or libraries for installation 101 | # install(TARGETS nav_monitor nav_monitor_node 102 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 103 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 104 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 105 | # ) 106 | 107 | ## Mark cpp header files for installation 108 | # install(DIRECTORY include/${PROJECT_NAME}/ 109 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 110 | # FILES_MATCHING PATTERN "*.h" 111 | # PATTERN ".svn" EXCLUDE 112 | # ) 113 | 114 | ## Mark other files for installation (e.g. launch and bag files, etc.) 115 | # install(FILES 116 | # # myfile1 117 | # # myfile2 118 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 119 | # ) 120 | 121 | ############# 122 | ## Testing ## 123 | ############# 124 | 125 | ## Add gtest based cpp test target and link libraries 126 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_nav_monitor.cpp) 127 | # if(TARGET ${PROJECT_NAME}-test) 128 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 129 | # endif() 130 | 131 | ## Add folders to be run by python nosetests 132 | # catkin_add_nosetests(test) -------------------------------------------------------------------------------- /nav_monitor/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | nav_monitor 4 | 0.0.0 5 | The nav_monitor package 6 | 7 | 8 | 9 | 10 | bruno 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 | rospy 44 | message_generation 45 | geometry_msgs 46 | std_msgs 47 | nav_msgs 48 | sensor_msgs 49 | ap_msgs 50 | rospy 51 | message_runtime 52 | geometry_msgs 53 | std_msgs 54 | nav_msgs 55 | sensor_msgs 56 | ap_msgs 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /nav_monitor/scripts/nav_monitor.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from nav_msgs.msg import Odometry 6 | from sensor_msgs.msg import Joy 7 | 8 | from ap_msgs.msg import NavStatus 9 | from ap_msgs.srv import PauseResumePatroller 10 | 11 | 12 | 13 | 14 | class NavMonitor(object): 15 | def __init__(self): 16 | self.goal_z=0 17 | self.current_z=0 18 | 19 | self.n_fails=0 20 | self.MAX_FAILS=100 21 | 22 | 23 | rospy.init_node('nav_monitor') 24 | 25 | #stuck in carpet 26 | rospy.Subscriber("/cmd_vel", Twist, self.vel_callback) 27 | rospy.Subscriber("/odom", Odometry, self.odom_callback) 28 | 29 | #pause_nav 30 | self.pad_paused=False 31 | rospy.Subscriber("/teleop_joystick/joy",Joy,self.pad_callback) 32 | 33 | self.pub = rospy.Publisher('nav_status', NavStatus) 34 | self.pub_msg=NavStatus() 35 | 36 | #pause service 37 | self.service_pause=False 38 | self.pause_service = rospy.Service('pause_resume_patroller', PauseResumePatroller, self.pause_service_handler) 39 | 40 | 41 | 42 | def vel_callback(self,msg): 43 | self.goal_z=msg.angular.z 44 | if self.goal_z != 0 and self.current_z==0: 45 | self.n_fails=self.n_fails+1 46 | else: 47 | self.n_fails=0 48 | if self.n_fails>self.MAX_FAILS: 49 | self.pub_msg.carpet_stuck=True 50 | else: 51 | self.pub_msg.carpet_stuck=False 52 | 53 | 54 | def odom_callback(self,msg): 55 | self.current_z=msg.twist.twist.angular.z 56 | 57 | 58 | def pad_callback(self,msg): 59 | if msg.buttons[4]==0: 60 | self.pad_paused=False 61 | else: 62 | self.pad_paused=True 63 | 64 | def pause_service_handler(self, req): 65 | self.service_pause=not self.service_pause 66 | if self.service_pause: 67 | return 'paused' 68 | else: 69 | return 'resumed' 70 | 71 | 72 | 73 | def publisher(self): 74 | r = rospy.Rate(10) # 10hz 75 | while not rospy.is_shutdown(): 76 | self.pub_msg.is_paused=self.pad_paused or self.service_pause 77 | self.pub.publish(self.pub_msg) 78 | r.sleep() 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | if __name__ == '__main__': 87 | 88 | 89 | monitor=NavMonitor() 90 | 91 | monitor.publisher() 92 | 93 | 94 | rospy.spin() 95 | -------------------------------------------------------------------------------- /waypoint_patroller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_patroller) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS actionlib actionlib_msgs move_base_msgs rospy scitos_msgs scitos_docking std_msgs dynamic_reconfigure marathon_touch_gui) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | catkin_python_setup() 17 | 18 | ####################################### 19 | ## Declare ROS messages and services ## 20 | ####################################### 21 | 22 | # Add dynamic reconfigure api 23 | find_package(catkin REQUIRED dynamic_reconfigure) 24 | generate_dynamic_reconfigure_options( 25 | cfg/PatrollerTresholds.cfg 26 | ) 27 | 28 | ## Generate messages in the 'msg' folder 29 | # add_message_files( 30 | # FILES 31 | # Message1.msg 32 | # Message2.msg 33 | # ) 34 | 35 | ## Generate services in the 'srv' folder 36 | # add_service_files( 37 | # FILES 38 | # Service1.srv 39 | # Service2.srv 40 | # ) 41 | 42 | ## Generate added messages and services with any dependencies listed here 43 | # generate_messages( 44 | # DEPENDENCIES 45 | # actionlib_msgs# move_base_msgs 46 | # ) 47 | 48 | ################################### 49 | ## catkin specific configuration ## 50 | ################################### 51 | ## The catkin_package macro generates cmake config files for your package 52 | ## Declare things to be passed to dependent projects 53 | ## LIBRARIES: libraries you create in this project that dependent projects also need 54 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 55 | ## DEPENDS: system dependencies of this project that dependent projects also need 56 | catkin_package( 57 | # INCLUDE_DIRS include 58 | # LIBRARIES waypoint_patroller 59 | # CATKIN_DEPENDS actionlib actionlib_msgs move_base_msgs rospy 60 | # DEPENDS system_lib 61 | ) 62 | 63 | ########### 64 | ## Build ## 65 | ########### 66 | 67 | ## Specify additional locations of header files 68 | ## Your package locations should be listed before other locations 69 | # include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 70 | 71 | ## Declare a cpp library 72 | # add_library(waypoint_patroller 73 | # src/${PROJECT_NAME}/waypoint_patroller.cpp 74 | # ) 75 | 76 | ## Declare a cpp executable 77 | # add_executable(waypoint_patroller_node src/waypoint_patroller_node.cpp) 78 | 79 | ## Add cmake target dependencies of the executable/library 80 | ## as an example, message headers may need to be generated before nodes 81 | # add_dependencies(waypoint_patroller_node waypoint_patroller_generate_messages_cpp) 82 | 83 | ## Specify libraries to link a library or executable target against 84 | # target_link_libraries(waypoint_patroller_node 85 | # ${catkin_LIBRARIES} 86 | # ) 87 | 88 | 89 | ############# 90 | ## Install ## 91 | ############# 92 | 93 | # all install targets should use catkin DESTINATION variables 94 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 95 | 96 | ## Mark executable scripts (Python etc.) for installation 97 | ## in contrast to setup.py, you can choose the destination 98 | # install(PROGRAMS 99 | # scripts/my_python_script 100 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 101 | # ) 102 | 103 | ## Mark executables and/or libraries for installation 104 | # install(TARGETS waypoint_patroller waypoint_patroller_node 105 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 106 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 107 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 108 | # ) 109 | 110 | ## Mark cpp header files for installation 111 | # install(DIRECTORY include/${PROJECT_NAME}/ 112 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 113 | # FILES_MATCHING PATTERN "*.h" 114 | # PATTERN ".svn" EXCLUDE 115 | # ) 116 | 117 | ## Mark other files for installation (e.g. launch and bag files, etc.) 118 | # install(FILES 119 | # # myfile1 120 | # # myfile2 121 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 122 | # ) 123 | 124 | install(PROGRAMS scripts/long_term_patroller.py 125 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 126 | ) 127 | 128 | ############# 129 | ## Testing ## 130 | ############# 131 | 132 | ## Add gtest based cpp test target and link libraries 133 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_waypoint_patroller.cpp) 134 | # if(TARGET ${PROJECT_NAME}-test) 135 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 136 | # endif() 137 | 138 | ## Add folders to be run by python nosetests 139 | # catkin_add_nosetests(test) 140 | -------------------------------------------------------------------------------- /waypoint_patroller/cfg/PatrollerTresholds.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "waypoint_patroller" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("low_battery", int_t, 0, "low battery treshold", 35, 25, 95) 9 | gen.add("very_low_battery", int_t, 0, "panic treshold. robot should callfor help if battery drops below this level", 20, 10, 30) 10 | gen.add("charged_battery", int_t, 0, "undocking treshold", 98, 40, 100) 11 | gen.add("max_bumper_recovery_attempts", int_t, 0, "numer of bumper recovery attempts before calling for help by sending an email.", 10, 5, 20) 12 | gen.add("max_move_base_recovery_attempts", int_t, 0, "numer of navigation recovery attempts before giving up and tryingto go to another waypoint.", 5, 1, 20) 13 | 14 | 15 | 16 | 17 | exit(gen.generate(PACKAGE, "waypoint_patroller", "PatrollerTresholds")) -------------------------------------------------------------------------------- /waypoint_patroller/launch/long_term_patroller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /waypoint_patroller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waypoint_patroller 4 | 0.0.0 5 | The waypoint_patroller package 6 | 7 | 8 | 9 | 10 | lacerdab 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 | actionlib 44 | actionlib_msgs 45 | move_base_msgs 46 | rospy 47 | scitos_msgs 48 | scitos_docking 49 | std_msgs 50 | dynamic_reconfigure 51 | mongodb_store 52 | strands_diagnostics 53 | marathon_touch_gui 54 | std_srvs 55 | actionlib 56 | actionlib_msgs 57 | move_base_msgs 58 | rospy 59 | scitos_msgs 60 | scitos_docking 61 | std_msgs 62 | dynamic_reconfigure 63 | mongodb_store 64 | strands_diagnostics 65 | marathon_touch_gui 66 | std_srvs 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /waypoint_patroller/scripts/log_inspector.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from optparse import OptionParser 4 | import pymongo 5 | import datetime 6 | 7 | def create_stats(episode_name=None, dc_host="localhost", dc_port=62345, t_minus_s=1200): 8 | """ Returns a list of event tuples """ 9 | 10 | client = pymongo.MongoClient(dc_host, dc_port) 11 | 12 | db = client["autonomous_patrolling"]["episodes"] 13 | 14 | episodes = [ i for i in db.distinct("episode_name")] 15 | if episode_name is None: 16 | episode_name = episodes[-1] 17 | elif episode_name not in episodes: 18 | raise Exception("The suplied episode is not in the datacentre.") 19 | 20 | print "Episode Name: ", episode_name 21 | start = db.find({'episode_name':episode_name, 'event_type':'episode start'})[0] 22 | print "Episode Date: ", datetime.datetime.fromtimestamp(start['stamp']['secs']) 23 | number_waypoint_success = db.find({'episode_name':episode_name, 'event_type':'success waypoint'}).count() 24 | number_waypoint_fail = db.find({'episode_name':episode_name, 'event_type':'failed waypoint'}).count() 25 | print "Number of waypoints: ", number_waypoint_success, " / ", number_waypoint_fail+number_waypoint_success 26 | print "="*35 27 | print "The last ", t_minus_s, "seconds of events:" 28 | print 29 | 30 | now = datetime.datetime.now() 31 | last = None 32 | for i in db.find({'episode_name':episode_name}).sort('stamp.secs', pymongo.DESCENDING): 33 | event_type = i['event_type'] 34 | if last is None: 35 | last = datetime.datetime.fromtimestamp(i['stamp']['secs']) 36 | time = datetime.datetime.fromtimestamp(i['stamp']['secs']) 37 | if last - time < datetime.timedelta(seconds=t_minus_s): 38 | #print it 39 | print time, "\tbattery=",i['battery level'],"\tmileage=",i['mileage'],"\t", 40 | if event_type == "start waypoint": 41 | print "starting waypoint [",i['waypoint'],"]", 42 | elif event_type=="failed waypoint": 43 | print "waypoint failed.", 44 | elif event_type=="success waypoint": 45 | print "waypoint reached.", 46 | elif event_type=="helped": 47 | print "Helped, reason=", i['type'] , 48 | else: 49 | print event_type, 50 | print 51 | 52 | 53 | if __name__ == '__main__': 54 | parser = OptionParser() 55 | 56 | parser.add_option("-d","--datacentre-host",dest="datacentre", default="localhost", 57 | help="the machine that the datacentre(mongodb) is on") 58 | 59 | parser.add_option("-p","--datacentre-port",dest="mongodb_port", type="int", default="62345", 60 | help="the port that the datacentre(mongodb) is on") 61 | 62 | parser.add_option("-e", "--episode", dest="episode_name", default=None, 63 | help="The name of the episode to show stats for") 64 | 65 | parser.add_option("-t","--history",dest="t_minus", type="int", default="1200", 66 | help="How many seconds of history") 67 | 68 | (options,args) = parser.parse_args() 69 | 70 | create_stats(options.episode_name, options.datacentre, options.mongodb_port, options.t_minus) 71 | -------------------------------------------------------------------------------- /waypoint_patroller/scripts/log_services.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import roslib 5 | import rospy 6 | 7 | from ap_msgs.srv import * 8 | from std_msgs.msg import String 9 | 10 | import waypoint_patroller.log_util 11 | 12 | class RosInterface(object): 13 | def __init__(self): 14 | self._gen = waypoint_patroller.log_util.StatGenerator(rospy.get_param("mongodb_host", "localhost"), 15 | int(rospy.get_param("mongodb_port")) ) 16 | 17 | # set up services 18 | for s in dir(self): 19 | if s.endswith("_srv_cb"): 20 | rospy.Service("/ap_log_server/"+s[:-7], 21 | getattr(self,s).type, getattr(self,s)) 22 | 23 | 24 | def get_episode_names_srv_cb(self, req): 25 | r=GetEpisodeNamesResponse() 26 | r.episodes=self._gen.get_episode_names() 27 | return r 28 | get_episode_names_srv_cb.type=GetEpisodeNames 29 | 30 | def get_latest_episode_name_srv_cb(self, req): 31 | r=GetLatestEpisodeNameResponse() 32 | r.name = self._gen.get_latest_run_name() 33 | return r 34 | get_latest_episode_name_srv_cb.type=GetLatestEpisodeName 35 | 36 | def get_episode_summary_srv_cb(self, req): 37 | r=GetEpisodeSummaryResponse() 38 | r.summary = self._gen.get_episode(req.episode_name).get_json_summary() 39 | return r 40 | get_episode_summary_srv_cb.type=GetEpisodeSummary 41 | 42 | def get_episode_complete_srv_cb(self, req): 43 | r=GetEpisodeCompleteResponse() 44 | r.complete = self._gen.get_episode(req.episode_name).get_json_complete() 45 | return r 46 | get_episode_complete_srv_cb.type=GetEpisodeComplete 47 | 48 | 49 | 50 | if __name__ == '__main__': 51 | rospy.init_node("ap_log_server") 52 | dm = RosInterface() 53 | rospy.spin() 54 | -------------------------------------------------------------------------------- /waypoint_patroller/scripts/long_term_patroller.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import sys 4 | import rospy 5 | import csv 6 | 7 | import smach 8 | import smach_ros 9 | 10 | from waypoint_patroller.patroller import WaypointPatrollerWithPulse 11 | from waypoint_patroller.logger import PatrollLogger 12 | 13 | from dynamic_reconfigure.server import Server 14 | from waypoint_patroller.cfg import PatrollerTresholdsConfig 15 | 16 | import mongodb_store.util 17 | got_pymongo = mongodb_store.util.check_for_pymongo() 18 | if got_pymongo: 19 | import pymongo 20 | 21 | 22 | import marathon_touch_gui.client 23 | 24 | 25 | class LongTermPatroller(object): 26 | """ 27 | Constructor. 28 | :param waypoints_name: str, name of waypoints as appears in the datacentre 29 | :param is_random: bool, should the waypoints be visited in random order 30 | :param n_iterations: int, how many times to visit all the waypoints 31 | """ 32 | def __init__(self, waypoints_name, is_random, n_iterations): 33 | 34 | # Create the main state machine 35 | self.long_term_patrol_sm = WaypointPatrollerWithPulse(waypoints_name, 36 | is_random, 37 | n_iterations) 38 | 39 | # Create a logger 40 | logger = PatrollLogger("autonomous_patrolling") 41 | self.long_term_patrol_sm.set_logger(logger) 42 | 43 | # dynamic reconfiguration of battery tresholds 44 | self.srv = Server(PatrollerTresholdsConfig, self.reconfigure_callback) 45 | 46 | pass 47 | 48 | """ Dyanmic reconfigure callback for the battery tresholds """ 49 | def reconfigure_callback(self, config, level): 50 | self.long_term_patrol_sm.set_patroller_thresholds(config.very_low_battery, 51 | config.low_battery, 52 | config.charged_battery, 53 | config.max_bumper_recovery_attempts, 54 | config.max_move_base_recovery_attempts) 55 | #ParameterStore().CHARGED_BATTERY = config.charged_battery 56 | #ParameterStore().LOW_BATTERY = config.low_battery 57 | #ParameterStore().VERY_LOW_BATTERY = config.very_low_battery 58 | return config 59 | 60 | 61 | """ The Main start point for Long Term Patroller """ 62 | def main(self): 63 | # Execute SMACH plan 64 | sis = smach_ros.IntrospectionServer('server_name', 65 | self.long_term_patrol_sm, 66 | '/SM_ROOT') 67 | sis.start() 68 | outcome = self.long_term_patrol_sm.execute() 69 | 70 | rospy.spin() 71 | sis.stop() 72 | 73 | 74 | if __name__ == '__main__': 75 | rospy.init_node('patroller') 76 | 77 | # Check for connection to the datacentre 78 | got_mongo = mongodb_store.util.wait_for_mongo() 79 | if not got_pymongo: 80 | sys.exit(1) 81 | if not got_mongo: 82 | sys.exit(1) 83 | 84 | # Check if a waypoints file was given as argument 85 | if len(sys.argv) < 2: 86 | rospy.logerr("No waypoints dataset name given. Use rosrun " 87 | "waypoint_patroller patroller.py [dataset name]. If you " 88 | "are using a launch file, see launch/patroller.launch for " 89 | "an example.") 90 | sys.exit(1) 91 | 92 | # waypoints file is a csv file with goal poses. The first line of the file 93 | # has the position in front of the charging station 94 | waypoints_name = sys.argv[1] 95 | 96 | is_random = 1 97 | if len(sys.argv) > 2: 98 | if sys.argv[2] == "false": 99 | is_random = 0 100 | rospy.loginfo( 101 | "Executing waypoint_patroller with sequential point selection.") 102 | else: 103 | rospy.loginfo( 104 | "Executing waypoint_patroller with random point selection.") 105 | else: 106 | rospy.loginfo( 107 | "Executing waypoint_patroller with random point selection.") 108 | 109 | n_iterations = -1 # infinite iterations 110 | if len(sys.argv) > 3: 111 | if int(sys.argv[3]) > 0: 112 | n_iterations = int(sys.argv[3]) 113 | rospy.loginfo( 114 | "Executing waypoint_patroller for %d iterations.", n_iterations) 115 | else: 116 | rospy.loginfo( 117 | "Executing waypoint_patroller with infinite iterations") 118 | else: 119 | rospy.loginfo("Executing waypoint_patroller with infinite iterations") 120 | 121 | 122 | displayNo = rospy.get_param("~display", 0) 123 | marathon_touch_gui.client.init_marathon_gui() 124 | marathon_touch_gui.client.display_main_page(displayNo) 125 | 126 | l = LongTermPatroller(waypoints_name, is_random, n_iterations) 127 | l.main() 128 | -------------------------------------------------------------------------------- /waypoint_patroller/scripts/web_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import subprocess 5 | import sys 6 | from optparse import OptionParser 7 | from time import sleep 8 | import waypoint_patroller.log_util 9 | import datetime 10 | from dateutil import parser 11 | import json 12 | 13 | def create_sumary_file(mongodb_host, mongodb_port, jsonfile, start_time=None, end_time=None): 14 | """ create a json summary. if start_time and end_time are set, then all episodes in that 15 | window are sumarised, otherwise only the latest""" 16 | gen = waypoint_patroller.log_util.StatGenerator(mongodb_host, mongodb_port) 17 | if start_time is None: 18 | summary = gen.get_episode(gen.get_latest_run_name()).get_json_summary() 19 | with open(jsonfile, "w") as f: 20 | f.write(summary) 21 | else: 22 | episodes = gen.get_episode_names_since(start_time, end_time) 23 | summaries = [gen.get_episode(i).get_summary() for i in episodes] 24 | with open(jsonfile, "w") as f: 25 | f.write(json.dumps(summaries)) 26 | 27 | def upload_summary_scp(upload_path, server, user, password, jsonfile): 28 | call = subprocess.call(["sshpass", "-p", password, "scp", jsonfile, "%s@%s:%s"%(user,server,upload_path) ]) 29 | if call != 0: 30 | raise Exception("Failed to upload summary. Bad call to sshpass...scp") 31 | 32 | 33 | if __name__ == '__main__': 34 | parser = OptionParser() 35 | parser.add_option("-s","--server",dest="hostname", 36 | help="the ssh host to copy the file to") 37 | parser.add_option("-u","--username",dest="username", 38 | help="username for account on ssh server") 39 | parser.add_option("-p","--password",dest="password", default="nopass", 40 | help="the users password") 41 | parser.add_option("-j","--jsonfile",dest="jsonfile", default="/tmp/patrol_run.json", 42 | help="the path of the temporary json file") 43 | parser.add_option("-f","--filepath",dest="path", 44 | help="the location to place the file on the server.") 45 | parser.add_option("-t","--time-between",dest="timeout", type="int", default=300, 46 | help="the time in seconds to pause between uploads to server. default = 300 seconds") 47 | 48 | parser.add_option("-d","--datacentre-host",dest="datacentre", default="localhost", 49 | help="the machine that the datacentre(mongodb) is on") 50 | 51 | parser.add_option("-k","--datacentre-port",dest="mongodb_port", type="int", default="62345", 52 | help="the port that the datacentre(mongodb) is on") 53 | 54 | 55 | (options,args) = parser.parse_args() 56 | 57 | while True: 58 | try: 59 | create_sumary_file(options.datacentre, options.mongodb_port,options.jsonfile, 60 | start_time=datetime.datetime.strptime('Nov 25 2013 09:40AM', '%b %d %Y %I:%M%p')) 61 | 62 | if options.hostname != None: 63 | upload_summary_scp(options.path, options.hostname, options.username, options.password,options.jsonfile) 64 | print "File done." 65 | except Exception, e: 66 | print "Upload failed: ", e 67 | sleep(options.timeout) 68 | -------------------------------------------------------------------------------- /waypoint_patroller/setup.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | from distutils.core import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | d = generate_distutils_setup( 7 | # don't do this unless you want a globally visible script 8 | # scripts=['bin/myscript'], 9 | packages=['waypoint_patroller'], 10 | package_dir={'': 'src'} 11 | ) 12 | 13 | setup(**d) -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/__init__.py: -------------------------------------------------------------------------------- 1 | import charging 2 | import monitor_states 3 | import navigation 4 | import recover_states 5 | import patroller 6 | import logger 7 | 8 | __all__ = ['navigation', 'recover_states', 'monitor_states', 'charging', 9 | 'patroller', 'logger'] -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/charging.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | 3 | import smach 4 | import smach_ros 5 | 6 | from geometry_msgs.msg import Twist 7 | 8 | 9 | from scitos_docking.srv import Charging 10 | from scitos_docking.msg import ChargingAction, ChargingGoal 11 | 12 | 13 | from monitor_states import BumperMonitor, BatteryMonitor, NavPauseMonitor 14 | from recover_states import RecoverBumper 15 | from logger import Loggable 16 | 17 | """ 18 | A smach_ros SimpleActionState state that calls the robot docking behaviour 19 | """ 20 | class DockToChargingStation(smach_ros.SimpleActionState, Loggable): 21 | def __init__(self): 22 | dock_goal = ChargingGoal() 23 | dock_goal.Command = "charge" 24 | dock_goal.Timeout = 100 25 | 26 | smach_ros.SimpleActionState.__init__(self, 27 | 'chargingServer', 28 | ChargingAction, 29 | goal=dock_goal ) 30 | 31 | def execute(self, ud): 32 | self.get_logger().log_charging_start() 33 | outcome = smach_ros.SimpleActionState.execute(self, ud) 34 | 35 | return outcome 36 | 37 | """ 38 | Undocks the robot from the station. 39 | 40 | outcomes: succeeded 41 | failure 42 | preempted 43 | 44 | """ 45 | 46 | class UndockFromChargingStation(smach_ros.SimpleActionState, Loggable): 47 | def __init__(self): 48 | dock_goal = ChargingGoal() 49 | dock_goal.Command = "undock" 50 | dock_goal.Timeout = 100 51 | 52 | smach_ros.SimpleActionState.__init__(self, 53 | 'chargingServer', 54 | ChargingAction, 55 | goal=dock_goal ) 56 | 57 | def execute(self, ud): 58 | self.get_logger().log_charging_finish() 59 | outcome = smach_ros.SimpleActionState.execute(self, ud) 60 | 61 | return outcome 62 | 63 | 64 | #class UndockFromChargingStation(smach.State, Loggable): 65 | #def __init__(self): 66 | #smach.State.__init__(self, 67 | #outcomes=['succeeded', 68 | #'failure', 69 | #'preempted'], 70 | #) 71 | #self.vel_pub = rospy.Publisher('/cmd_vel', Twist) 72 | #self._vel_cmd = Twist() 73 | #self._vel_cmd.linear.x = -0.2 74 | 75 | #def execute(self, userdata): 76 | #self.get_logger().log_charging_finish() 77 | #for i in range(0, 20): 78 | #self.vel_pub.publish(self._vel_cmd) 79 | #if self.preempt_requested(): 80 | #self.service_preempt() 81 | #return 'preempted' 82 | #rospy.sleep(0.1) 83 | 84 | #return 'succeeded' 85 | 86 | """ 87 | A state machine that docks the robot to the charging station, waits until it is 88 | charged, then undocks the robot. 89 | """ 90 | class DockUndockBehaviour(smach.StateMachine, Loggable): 91 | def __init__(self): 92 | smach.StateMachine.__init__(self, outcomes=['succeeded', 93 | 'failure', 94 | 'preempted']) 95 | 96 | self._battery_monitor = BatteryMonitor(True) 97 | self._dock_to_charge = DockToChargingStation() 98 | self._undock_from_charge = UndockFromChargingStation() 99 | 100 | with self: 101 | smach.StateMachine.add('DOCK_TO_CHARGING_STATION', 102 | self._dock_to_charge, 103 | transitions={'succeeded': 'CHARGING', 104 | 'aborted': 'DOCK_TO_CHARGING_STATION', 105 | 'preempted':'preempted'}) 106 | 107 | smach.StateMachine.add('CHARGING', 108 | self._battery_monitor, 109 | transitions={ 110 | 'invalid': 'UNDOCK_FROM_CHARGING_STATION', 111 | 'valid': 'UNDOCK_FROM_CHARGING_STATION'}) 112 | 113 | smach.StateMachine.add('UNDOCK_FROM_CHARGING_STATION', 114 | self._undock_from_charge, 115 | transitions={'succeeded': 'succeeded', 116 | 'aborted': 'succeeded', 117 | 'preempted': 'preempted'}) 118 | 119 | """ 120 | Set the battery level thresholds. 121 | """ 122 | def set_patroller_thresholds(self, very_low_battery, low_battery, 123 | charged_battery): 124 | self._battery_monitor.set_patroller_thresholds(very_low_battery, 125 | low_battery, 126 | charged_battery) 127 | 128 | 129 | """ 130 | A bumper aware version of DockUndockBeahviour. 131 | """ 132 | class MonitoredDockUndockBehaviour(smach.Concurrence, Loggable): 133 | def __init__(self, to_base=False): 134 | smach.Concurrence.__init__(self, 135 | outcomes=['bumper_pressed', 136 | 'pause_requested', 137 | 'succeeded', 138 | 'failure'], 139 | default_outcome='failure', 140 | child_termination_cb=self.child_term_cb, 141 | outcome_cb=self.out_cb 142 | ) 143 | self._dock_undock_bahaviour = DockUndockBehaviour() 144 | self._bumper_monitor = BumperMonitor() 145 | self._nav_pause_monitor=NavPauseMonitor(False) 146 | with self: 147 | smach.Concurrence.add('UNMONITORED_DOCKING', self._dock_undock_bahaviour) 148 | smach.Concurrence.add('BUMPER_MONITOR', self._bumper_monitor) 149 | smach.Concurrence.add('NAV_PAUSE_MONITOR', self._nav_pause_monitor) 150 | 151 | def child_term_cb(self, outcome_map): 152 | # decide if this state is done when one or more concurrent inner states 153 | # stop 154 | return True 155 | 156 | def out_cb(self, outcome_map): 157 | # determine what the outcome of this machine is 158 | if outcome_map['BUMPER_MONITOR'] == 'invalid': 159 | return 'bumper_pressed' 160 | if outcome_map["NAV_PAUSE_MONITOR"] == "invalid": 161 | return "pause_requested" 162 | if outcome_map["UNMONITORED_DOCKING"] == "succeeded": 163 | return "succeeded" 164 | 165 | """ 166 | Set the battery level thresholds. 167 | """ 168 | def set_patroller_thresholds(self, very_low_battery, low_battery, 169 | charged_battery): 170 | self._dock_undock_bahaviour.set_patroller_thresholds(very_low_battery, 171 | low_battery, 172 | charged_battery) 173 | 174 | 175 | 176 | """ 177 | Highest level charging behaviour. This will dock the robot to the charging 178 | station, wait until the charge is sufficient, and then undock. The bumper is 179 | monitored through out, and if pressed then the the robot attempts to revocer 180 | using recover_states.RecoverBumper. 181 | 182 | outcomes: succeeded 183 | failure 184 | 185 | input_keys: going_to_charge 186 | """ 187 | class HighLevelDockUndockBehaviour(smach.StateMachine, Loggable): 188 | def __init__(self): 189 | smach.StateMachine.__init__(self, 190 | outcomes=['succeeded', 191 | 'failure']) 192 | 193 | self._bump_monitored_dk_undk = MonitoredDockUndockBehaviour() 194 | self._recover_bumper = RecoverBumper() 195 | self._nav_resume_monitor=NavPauseMonitor(True) 196 | with self: 197 | smach.StateMachine.add('MONITORED_DOCK', 198 | self._bump_monitored_dk_undk, 199 | transitions={'succeeded': 'succeeded', 200 | 'failure': 'failure', 201 | 'bumper_pressed': 'RECOVER_BUMPER', 202 | 'pause_requested':'NAV_RESUME_MONITOR' 203 | } 204 | ) 205 | smach.StateMachine.add('RECOVER_BUMPER', 206 | self._recover_bumper, 207 | transitions={'succeeded': 'MONITORED_DOCK' }) 208 | smach.StateMachine.add('NAV_RESUME_MONITOR', 209 | self._nav_resume_monitor, 210 | transitions={'invalid': 'MONITORED_DOCK', 211 | 'valid': 'MONITORED_DOCK', 212 | 'preempted':'MONITORED_DOCK'}) 213 | 214 | """ 215 | Set the patrolelr level thresholds. 216 | """ 217 | def set_patroller_thresholds(self, very_low_battery, low_battery, 218 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts): 219 | self._bump_monitored_dk_undk.set_patroller_thresholds(very_low_battery, 220 | low_battery, 221 | charged_battery) 222 | self._recover_bumper.set_patroller_thresholds(max_bumper_recovery_attempts) 223 | -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/log_util.py: -------------------------------------------------------------------------------- 1 | """ 2 | Provides utility functions for interpretting logged patrol runs 3 | """ 4 | import rospy 5 | import mongodb_store.util as dc_util 6 | 7 | got_pymongo = dc_util.check_for_pymongo() 8 | if not got_pymongo: 9 | raise Exception("Can't find pymongo library") 10 | import pymongo 11 | 12 | import datetime 13 | import json 14 | 15 | def stamp_to_datetime(stamp): 16 | """ 17 | Takes a datacentre timestamp (stamp.secs, stamp.nsecs) and returns a python datatime object 18 | """ 19 | return datetime.datetime.fromtimestamp(stamp['secs'] + stamp['nasecs']/1e9) 20 | 21 | 22 | class Episode(object): 23 | """ 24 | Information related to a single patrol run 25 | """ 26 | 27 | def __init__(self, name): 28 | self.name = name 29 | self._populated = False 30 | 31 | self.start_time = None 32 | self.latest_event_time = None 33 | self.start_mileage = 0 34 | self.bumper_hits=[] 35 | self.navigation_fails=[] 36 | self.finish_time = None 37 | self.start_mileage = 0 38 | self.finish_mileage = 0 39 | self.stamped_mileage=[] 40 | self.stamped_battery=[] 41 | self.stamped_charges=[] 42 | self.waypoints_stamps=[] 43 | self.active_point_name = "" 44 | self.bump_help_count = 0 45 | self.navigation_help_count = 0 46 | 47 | 48 | def fill_from_log(self, events): 49 | """ Fill this episode stat from a pymongo iterator of documents, all assumed to be 50 | event logs for this episode """ 51 | for event in events.sort('stamp.secs'): 52 | if not event['episode_name']==self.name: 53 | raise Exception("Trying to fill in an episode object with wrong events.") 54 | event_type = event['event_type'] 55 | 56 | if event_type == "episode start": 57 | self.start_time = stamp_to_datetime(event['stamp']) 58 | self.start_mileage = event['odom_mileage'] 59 | 60 | if self.start_time is None: 61 | # Problem - should never get to this as the first event should be a start event.. 62 | continue 63 | #raise () 64 | 65 | self.stamped_mileage.append([ event['odom_mileage'] - self.start_mileage, 66 | stamp_to_datetime(event['stamp']) - self.start_time ]) 67 | self.stamped_battery.append([ event['battery level'], 68 | stamp_to_datetime(event['stamp']) - self.start_time ]) 69 | self.latest_event_time = stamp_to_datetime(event['stamp']) 70 | 71 | if event_type == "heartbeat": 72 | continue 73 | 74 | if event_type == "bumper pressed": 75 | self.bumper_hits.append(stamp_to_datetime(event['stamp']) - self.start_time ) 76 | 77 | if event_type == "navigation recovery": 78 | self.navigation_fails.append(stamp_to_datetime(event['stamp']) - self.start_time ) 79 | 80 | if event_type == "charging finished": 81 | self.stamped_charges.append(stamp_to_datetime(event['stamp']) - self.start_time ) 82 | 83 | if event_type == "episode finish": 84 | self.finish_time = stamp_to_datetime(event['stamp']) 85 | self.finish_mileage = event['odom_mileage'] 86 | 87 | if event_type == "start waypoint": 88 | self.active_point_name = event["waypoint"] 89 | 90 | if event_type == "success waypoint": 91 | self.waypoints_stamps.append([ self.active_point_name, True, 92 | stamp_to_datetime(event['stamp']) - self.start_time ]) 93 | 94 | if event_type == "failed waypoint": 95 | self.waypoints_stamps.append([ self.active_point_name, False, 96 | stamp_to_datetime(event['stamp']) - self.start_time ]) 97 | 98 | if event_type == "helped": 99 | if event['type'] == "navigation": 100 | self.navigation_help_count += 1 101 | elif event['type'] == "bumper": 102 | self.bump_help_count += 1 103 | 104 | self._populated = True 105 | 106 | def __str__(self): 107 | """ 108 | For debug printing 109 | """ 110 | return """Episode Name: %s 111 | Date: %s 112 | Run duration: %s 113 | Distance covered: %s 114 | Bumper recoveries: %d 115 | Charge cycles: %d 116 | """ % (self.name, self.start_time,self.stamped_mileage[-1][1], 117 | self.stamped_mileage[-1][0],len(self.bumper_hits),len(self.stamped_charges)) 118 | 119 | def get_json_complete(self): 120 | complete = {} 121 | complete["start_time"] = str(self.start_time) 122 | complete["start_mileage"] = self.start_mileage 123 | complete["bumper_hits"]=[str(i) for i in self.bumper_hits] 124 | complete["navigation_fails"]=[str(i) for i in self.navigation_fails] 125 | complete["finish_time"] = str(self.finish_time) 126 | complete["start_mileage"] = self.start_mileage 127 | complete["finish_mileage"] = self.finish_mileage 128 | complete["stamped_mileage"]=[[i[0], str(i[1])] for i in self.stamped_mileage ] 129 | complete["stamped_battery"]=[[i[0], str(i[1])] for i in self.stamped_battery ] 130 | complete["stamped_charges"]=[[i[0], str(i[1])] for i in self.stamped_charges ] 131 | complete["stamped_waypoints"] = [[i[0], i[1], str(i[2])] for i in self.waypoints_stamps] 132 | complete["current_waypoint"]=self.active_point_name 133 | complete['last_event_time']=str(self.latest_event_time) 134 | complete['bump_help_count']= self.bump_help_count 135 | complete['navigation_help_count']=self.navigation_help_count 136 | 137 | return json.dumps(complete) 138 | 139 | def get_summary(self): 140 | summary = {} 141 | summary['episode_name']=self.name 142 | summary['date']=str(self.start_time) 143 | summary['run_duration']=self.stamped_mileage[-1][1].total_seconds() 144 | summary['distance']=self.stamped_mileage[-1][0] 145 | summary['bump_recoveries']=len(self.bumper_hits) 146 | summary['navigation_recoveries']=len(self.navigation_fails) 147 | summary['charge_cycles']=len(self.stamped_charges) 148 | summary['successful_waypoints']=sum([1 if i[1] else 0 for i in self.waypoints_stamps ]) 149 | summary['failed_waypoints']=sum([1 if not i[1] else 0 for i in self.waypoints_stamps ]) 150 | summary['active_waypoint']=self.active_point_name 151 | summary['last_event_time']=str(self.latest_event_time) 152 | summary['bump_help_count']= self.bump_help_count 153 | summary['navigation_help_count']=self.navigation_help_count 154 | return summary 155 | 156 | def get_json_summary(self): 157 | summary = self.get_summary() 158 | return json.dumps([summary]) 159 | 160 | 161 | 162 | 163 | class StatGenerator(object): 164 | """ 165 | Connects to MongoDB and pulls out statistics for runs 166 | """ 167 | 168 | def __init__(self, db_server='localhost', db_port=62345): 169 | """ 170 | Arguments: 171 | - `db_server`: 172 | - `db_port`: 173 | """ 174 | self._db_server = db_server 175 | self._db_port = db_port 176 | 177 | self._client = pymongo.MongoClient(self._db_server, self._db_port) 178 | self._collection = self._client.autonomous_patrolling.episodes 179 | self._episodes={} 180 | 181 | def _update_episodes(self): 182 | self._episodes={} 183 | for i in self._collection.find().distinct("episode_name"): 184 | self._episodes[i]=Episode(i) 185 | 186 | def _update_episode(self, episode_name): 187 | """ 188 | Calculate statistics for the given episode 189 | Arguments: 190 | - `episode_name`: 191 | """ 192 | episode = Episode(episode_name) 193 | episode.fill_from_log(self._collection.find({'episode_name':episode_name})) 194 | self._episodes[episode_name]=episode 195 | 196 | 197 | def get_episode_names(self): 198 | """ 199 | Returns a list of the episode names 200 | """ 201 | names = [ i for i in self._collection.find().distinct("episode_name")] 202 | return names 203 | 204 | def get_episode_names_since(self, start_date, end_date=None): 205 | """ 206 | Gets a list of episodes that started since the start_date, started before end_date 207 | """ 208 | candidates = self.get_episode_names() 209 | episodes=[] 210 | for i in candidates: 211 | start = self._collection.find_one({"episode_name":i,"event_type":"episode start"}) 212 | time = stamp_to_datetime(start["stamp"]) 213 | if time > start_date and ( end_date is None or time < end_date): 214 | episodes.append(i) 215 | return episodes 216 | 217 | 218 | def get_episode(self, episode_name): 219 | """ 220 | Return the Episode 221 | """ 222 | if episode_name not in self.get_episode_names(): 223 | raise Exception("Unknown episode name") 224 | refresh = (self.get_latest_run_name()==episode_name) 225 | if refresh or not self._episodes.has_key(episode_name): 226 | self._update_episode(episode_name) 227 | ep = self._episodes[episode_name] 228 | if not ep._populated: 229 | self._update_episode(episode_name) 230 | 231 | return self._episodes[episode_name] 232 | 233 | def get_latest_run_name(self): 234 | """ 235 | Return the name of the run that has the most recent time stamp 236 | """ 237 | name = self._collection.find().sort('stamp.secs').distinct("episode_name")[-1] 238 | return name 239 | 240 | 241 | 242 | 243 | -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/logger.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import mongodb_store.util as dc_util 3 | 4 | from std_msgs.msg import Float32 5 | from scitos_msgs.msg import BatteryState 6 | 7 | got_pymongo = dc_util.check_for_pymongo() 8 | if not got_pymongo: 9 | raise Exception("Can't find pymongo library") 10 | import pymongo 11 | 12 | """ 13 | A base class for all states that are going to have logging capabilities. It 14 | stores and a reference to the logger and shares among with children. 15 | """ 16 | class Loggable(object): 17 | def _ensure_logger(self): 18 | if not hasattr(self, "_logger"): 19 | self._logger=Logger() 20 | 21 | def set_logger(self,logger): 22 | assert isinstance(logger, Logger) 23 | self._logger=logger 24 | for a in dir(self): 25 | attr = getattr(self, a) 26 | if isinstance(attr, Loggable): 27 | rospy.logdebug("Passing on logger to " + a) 28 | attr.set_logger(self._logger) 29 | 30 | def get_logger(self): 31 | self._ensure_logger() 32 | return self._logger 33 | 34 | """ 35 | Base class for loggers, simply the non-logger: it silently logs nothing 36 | """ 37 | class Logger(object): 38 | def __getattr__(self, name): 39 | isinstance(name, str) 40 | if name.startswith("log_"): 41 | return lambda *args: False 42 | else: 43 | raise AttributeError("Logger has no attrribute %s"%name) 44 | 45 | """ 46 | Logging class for entering run data into the mongodb_store. 47 | This class should be created once and each state that wants to log should 48 | hold a reference to it. 49 | """ 50 | class PatrollLogger(Logger): 51 | def __init__(self, name="autonomous_patroller"): 52 | self._active_episode = False 53 | got_datacentre = dc_util.wait_for_mongo() 54 | if not got_datacentre: 55 | raise Exception("mongodb_stores does not appear to be running") 56 | self._mongo = pymongo.MongoClient(rospy.get_param("mongodb_host", 57 | "localhost"), 58 | int(rospy.get_param("mongodb_port"))) 59 | self._db = self._mongo[name].episodes 60 | 61 | self._mileage_sub = rospy.Subscriber('/odom_mileage', Float32, 62 | self._mileage_cb) 63 | self._odom_mileage_sub = rospy.Subscriber('/odom_mileage', Float32, 64 | self._odom_mileage_cb) 65 | self._batterystate_sub = rospy.Subscriber('/battery_state', BatteryState, 66 | self._batterystate_cb) 67 | self._mileage = 0 68 | self._odom_mileage = 0 69 | self._battery = 100 70 | 71 | 72 | def _mileage_cb(self, msg): 73 | self._mileage = msg.data 74 | 75 | def _odom_mileage_cb(self, msg): 76 | self._odom_mileage = msg.data 77 | 78 | def _batterystate_cb(self, msg): 79 | self._battery = msg.lifePercent 80 | 81 | """ put a ros timestamp on a document """ 82 | def _stamp(self, doc, now=None): 83 | if now is None: 84 | now = rospy.get_rostime() 85 | doc["stamp"] = {"secs": now.secs, "nasecs": now.nsecs,} 86 | return doc 87 | 88 | def _record(self, doc): 89 | if self._active_episode: 90 | doc['episode_name'] = self._episode_name 91 | doc['mileage'] = self._mileage 92 | doc['odom_mileage'] = self._odom_mileage 93 | doc['battery level'] = self._battery 94 | self._db.insert(self._stamp(doc)) 95 | 96 | 97 | def log_start_episode(self, pt_set_name): 98 | #rospy.logerr("[NotImp] Episode started!!!!!!") 99 | # create a name for the episode 100 | t = rospy.get_rostime() 101 | self._episode_name = '%d.%d' % (t.secs, t.nsecs) 102 | self._active_episode = True 103 | 104 | self._record({'event_type': 'episode start', 105 | 'waypoint_set': pt_set_name, }) 106 | 107 | def log_finish_episode(self): 108 | self._record({'event_type': 'episode finish', }) 109 | self._active_episode = False 110 | 111 | def log_waypoint_visit(self, point_name): 112 | self._record({'event_type': 'start waypoint', 113 | 'waypoint': point_name, }) 114 | 115 | def log_waypoint_success(self, waypoint): 116 | self._record({'event_type': 'success waypoint', }) 117 | 118 | def log_waypoint_fail(self, waypoint): 119 | self._record({'event_type': 'failed waypoint', }) 120 | 121 | def log_bump(self): 122 | self._record({'event_type': 'bumper pressed', }) 123 | 124 | def log_bump_count(self,number): 125 | self._record({'event_type': 'bump recovered', 126 | 'number of resets':number}) 127 | 128 | def log_intervention_alarm(self): 129 | pass 130 | 131 | def log_navigation_recovery_attempt(self, success, attempts): 132 | self._record({'event_type': 'navigation recovery', 133 | 'success': success, 134 | 'attempts': attempts,}) 135 | 136 | def log_mileage(self, mileage): 137 | pass 138 | 139 | def log_charging_start(self): 140 | self._record({'event_type': 'charging started', }) 141 | 142 | def log_charging_finish(self): 143 | self._record({'event_type': 'charging finished', }) 144 | 145 | 146 | def log_carpet_stuck(self): 147 | self._record({'event_type': 'stuck in carpet', }) 148 | 149 | 150 | def log_pulse(self): 151 | self._record({'event_type': 'heartbeat', }) 152 | 153 | 154 | def log_helped(self, help_type): 155 | """ Logs when a user has helped the robot """ 156 | self._record({'event_type': 'helped', 'type': help_type,}) 157 | 158 | 159 | -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/monitor_states.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | 3 | import smach 4 | import smach_ros 5 | from smach import * 6 | from smach_ros import * 7 | 8 | from scitos_msgs.msg import MotorStatus, BatteryState 9 | from ap_msgs.msg import NavStatus 10 | 11 | from logger import Loggable 12 | 13 | """ 14 | A smach_ros MonitorState that monitors the robot's battery. 15 | """ 16 | class BatteryMonitor(smach_ros.MonitorState): 17 | def __init__(self, going_to_charge, very_low_battery=15, low_battery=35, 18 | charged_battery=90): 19 | smach_ros.MonitorState.__init__(self, "/battery_state", 20 | BatteryState, 21 | self._callback) 22 | self.set_patroller_thresholds(very_low_battery, low_battery, 23 | charged_battery) 24 | self._going_to_charge=going_to_charge 25 | 26 | """ 27 | Set the battery level thresholds. 28 | """ 29 | def set_patroller_thresholds(self, very_low_battery, low_battery, 30 | charged_battery): 31 | if very_low_battery is not None: 32 | self.VERY_LOW_BATTERY = very_low_battery 33 | if low_battery is not None: 34 | self.LOW_BATTERY = low_battery 35 | if charged_battery is not None: 36 | self.CHARGED_BATTERY = charged_battery 37 | 38 | """ Test the message and decide exit or not """ 39 | def _callback(self, ud, msg): 40 | if msg.lifePercent < self.VERY_LOW_BATTERY: 41 | return False 42 | if self._going_to_charge: 43 | return msg.lifePercent < self.CHARGED_BATTERY 44 | else: 45 | return msg.lifePercent > self.LOW_BATTERY 46 | 47 | 48 | 49 | """ 50 | A smach_ros MonitorState that monitors the robot's bumper. If the bumper get 51 | pressed, this state to exit with outcome 'invalid'. 52 | """ 53 | class BumperMonitor(smach_ros.MonitorState, Loggable): 54 | def __init__(self,): 55 | smach_ros.MonitorState.__init__(self, "/motor_status", 56 | MotorStatus, 57 | self._callback) 58 | 59 | """ Test the message and decide exit or not """ 60 | def _callback(self, ud, msg): 61 | # using msg.bumper_pressed does not work properly because sometimes the 62 | # bumper is pressed but no change of state is published 63 | if msg.motor_stopped and not msg.free_run: 64 | self.get_logger().log_bump() 65 | return False 66 | else: 67 | return True 68 | 69 | 70 | class StuckOnCarpetMonitor(smach_ros.MonitorState, Loggable): 71 | def __init__(self): 72 | smach_ros.MonitorState.__init__(self, "/nav_status", 73 | NavStatus, 74 | self._callback) 75 | 76 | """ Test the message and decide exit or not """ 77 | def _callback(self, ud, msg): 78 | if msg.carpet_stuck: 79 | self.get_logger().log_carpet_stuck() 80 | return False 81 | else: 82 | return True 83 | 84 | 85 | 86 | class NavPauseMonitor(smach_ros.MonitorState, Loggable): 87 | def __init__(self,is_paused): 88 | self._is_paused=is_paused 89 | smach_ros.MonitorState.__init__(self, "/nav_status", 90 | NavStatus, 91 | self._callback, 92 | input_keys=['is_paused']) 93 | 94 | """ Test the message and decide exit or not """ 95 | def _callback(self, ud, msg): 96 | if self._is_paused: 97 | if msg.is_paused: 98 | return True 99 | else: 100 | #self.get_logger().log_carpet_stuck() 101 | return False 102 | else: 103 | if msg.is_paused: 104 | #self.get_logger().log_carpet_stuck() 105 | return False 106 | else: 107 | return True 108 | 109 | 110 | -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/navigation.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | 3 | import smach 4 | import smach_ros 5 | 6 | import actionlib 7 | from actionlib_msgs.msg import * 8 | from move_base_msgs.msg import * 9 | 10 | 11 | from monitor_states import BatteryMonitor, BumperMonitor, StuckOnCarpetMonitor, NavPauseMonitor 12 | from recover_states import RecoverMoveBase, RecoverBumper, RecoverStuckOnCarpet 13 | from logger import Loggable 14 | 15 | 16 | 17 | 18 | """ 19 | A SimpleActionState that sends a goal to the navigation stack using move_base 20 | 21 | input keys: goal_pose - geometry_msgs/Pose, goal pose in /map 22 | frame 23 | n_move_base_fails - number of failures so far, will be 24 | incremented and returned if fail 25 | 26 | output keys: n_move_base_fails - see input. 27 | """ 28 | class MoveBaseActionState(smach_ros.SimpleActionState, Loggable): 29 | def __init__(self): 30 | 31 | self.MOVE_BASE_EXEC_TIMEOUT = rospy.Duration(600.0) 32 | self.MOVE_BASE_PREEMPT_TIMEOUT = rospy.Duration(30.0) 33 | 34 | 35 | smach_ros.SimpleActionState.__init__(self, 36 | 'move_base', 37 | MoveBaseAction, 38 | goal_cb=self.move_base_goal_cb, 39 | result_cb=self.move_base_result_cb, 40 | input_keys=['goal_pose', 41 | 'n_move_base_fails'], 42 | output_keys=['n_move_base_fails'], 43 | exec_timeout=self.MOVE_BASE_EXEC_TIMEOUT, 44 | preempt_timeout=self.MOVE_BASE_PREEMPT_TIMEOUT 45 | ) 46 | 47 | """ callback that builds the move_base goal, from the input data """ 48 | def move_base_goal_cb(self, userdata, goal): 49 | next_goal = move_base_msgs.msg.MoveBaseGoal() 50 | next_goal.target_pose.header.frame_id = "/map" 51 | next_goal.target_pose.header.stamp = rospy.Time.now() 52 | next_goal.target_pose.pose = userdata.goal_pose 53 | 54 | return next_goal 55 | 56 | 57 | """ 58 | called after the move_base state terminates. Incre continue 59 | ases the number of 60 | move_base fails or resets it to 0 according to the move_base result 61 | """ 62 | def move_base_result_cb(self, userdata, status, result): 63 | if status == GoalStatus.ABORTED: 64 | userdata.n_move_base_fails = userdata.n_move_base_fails + 1 65 | elif status == GoalStatus.SUCCEEDED: 66 | userdata.n_move_base_fails = 0 67 | 68 | 69 | 70 | 71 | """ 72 | Move the robot to a goal location, with some basic recovery attempts on failure. 73 | Recovery is implemented in recover_states.RecoverMoveBase 74 | 75 | outcomes: succeeded 76 | failed 77 | preempted 78 | 79 | input keys: goal_pose 80 | """ 81 | class RecoverableMoveBase(smach.StateMachine, Loggable): 82 | def __init__(self): 83 | smach.StateMachine.__init__(self, 84 | outcomes=['succeeded', 85 | 'failure', 86 | 'preempted'], 87 | input_keys=['goal_pose']) 88 | 89 | self.userdata.n_move_base_fails = 0 90 | self._move_base_action = MoveBaseActionState() 91 | self._recover_move_base = RecoverMoveBase() 92 | with self: 93 | smach.StateMachine.add('MOVE_BASE', 94 | self._move_base_action, 95 | transitions={'succeeded': 'succeeded', 96 | 'aborted': 'RECOVER_MOVE_BASE', 97 | 'preempted': 'preempted'} 98 | ) 99 | smach.StateMachine.add('RECOVER_MOVE_BASE', 100 | self._recover_move_base, 101 | transitions={'succeeded': 'MOVE_BASE', 102 | 'failure': 'failure'} ) 103 | 104 | def execute(self, userdata=smach.UserData()): 105 | outcome = smach.StateMachine.execute(self, userdata) 106 | if outcome == 'succeeded': 107 | self.get_logger().log_waypoint_success(userdata.goal_pose) 108 | elif outcome == 'failure': 109 | self.get_logger().log_waypoint_fail(userdata.goal_pose) 110 | 111 | return outcome 112 | 113 | def set_patroller_thresholds(self, max_move_base_recovery_attempts): 114 | self._recover_move_base.set_patroller_thresholds(max_move_base_recovery_attempts) 115 | 116 | """ 117 | A monitored version of RecoverableMoveBase. Adds checking for battery level 118 | too low, and stoping move_base if bumper is pressed. 119 | 120 | outcomes: bumper_pressed 121 | battery_low 122 | succeeded 123 | failure 124 | 125 | input keys: goal_pose 126 | going_to_charge 127 | """ 128 | class MonitoredRecoverableMoveBase(smach.Concurrence, Loggable): 129 | def __init__(self,going_to_charge): 130 | smach.Concurrence.__init__(self, 131 | outcomes=['bumper_pressed', 132 | 'battery_low', 133 | 'succeeded', 134 | 'failure', 135 | 'stuck_on_carpet', 136 | 'pause_requested'], 137 | default_outcome='failure', 138 | child_termination_cb=self.child_term_cb, 139 | outcome_cb=self.out_cb, 140 | input_keys=['goal_pose'] 141 | ) 142 | self._battery_monitor = BatteryMonitor(going_to_charge) 143 | self._bumper_monitor = BumperMonitor() 144 | self._recoverable_move_base = RecoverableMoveBase() 145 | self._carpet_monitor = StuckOnCarpetMonitor() 146 | self._nav_pause_monitor=NavPauseMonitor(False) 147 | with self: 148 | smach.Concurrence.add('BATTERY_MONITOR', self._battery_monitor) 149 | smach.Concurrence.add('BUMPER_MONITOR', self._bumper_monitor) 150 | smach.Concurrence.add('STUCK_ON_CARPET_MONITOR', self._carpet_monitor) 151 | smach.Concurrence.add('NAV_PAUSE_MONITOR', self._nav_pause_monitor) 152 | smach.Concurrence.add('MOVE_BASE_SM', self._recoverable_move_base) 153 | 154 | def child_term_cb(self, outcome_map): 155 | # decide if this state is done when one or more concurrent inner states 156 | # stop 157 | if ( outcome_map['BUMPER_MONITOR'] == 'invalid' or 158 | outcome_map["BATTERY_MONITOR"] == "invalid" or 159 | outcome_map["STUCK_ON_CARPET_MONITOR"] == "invalid" or 160 | outcome_map["NAV_PAUSE_MONITOR"] == "invalid" or 161 | outcome_map["MOVE_BASE_SM"] == "succeeded" or 162 | outcome_map['MOVE_BASE_SM'] == "failure" ): 163 | return True 164 | return False 165 | 166 | def out_cb(self, outcome_map): 167 | # determine what the outcome of this machine is 168 | 169 | # rospy.sleep(0.1) without this sleep, sometimes the concurrence container 170 | # terminates before all its children terminate, and an error is printed. 171 | # However, that does not affect the evolution, and I think that with the 172 | # sleep sometimes the container blocks and never terminates 173 | if outcome_map['BUMPER_MONITOR'] == 'invalid': 174 | return 'bumper_pressed' 175 | if outcome_map["BATTERY_MONITOR"] == "invalid": 176 | return "battery_low" 177 | if outcome_map["STUCK_ON_CARPET_MONITOR"] == "invalid": 178 | return "stuck_on_carpet" 179 | if outcome_map["NAV_PAUSE_MONITOR"] == "invalid": 180 | return "pause_requested" 181 | if outcome_map["MOVE_BASE_SM"] == "succeeded": 182 | return "succeeded" 183 | if outcome_map["MOVE_BASE_SM"] == "failure": 184 | return "failure" 185 | 186 | 187 | """ 188 | Set the battery level thresholds. 189 | """ 190 | def set_patroller_thresholds(self, very_low_battery, low_battery, 191 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts): 192 | self._battery_monitor.set_patroller_thresholds(very_low_battery, 193 | low_battery, 194 | charged_battery) 195 | self._recoverable_move_base.set_patroller_thresholds(max_move_base_recovery_attempts) 196 | 197 | 198 | 199 | 200 | """ 201 | The highest level "goto position" state. This will use move_base to goto a goal 202 | position, mean while checking the battery and the bumper. If move_base fails, 203 | some recovary is attempted. If the bumper is pressed, but then released, it will 204 | resume. If the battery is low, move_base action is stopped and the state exits. 205 | 206 | outcomes: succeeded - got to position 207 | bumper_failure - bumper is in constant contact 208 | move_base_failure - move_base fails, can't recover 209 | battery_low - battery is low, goal cancelled 210 | 211 | input_keys: goal_pose - geometry_msgs/Pose, the target location in /map 212 | frame 213 | going_to_charge - bool, if going to charging station then don't 214 | cancel because of the battery monitor 215 | """ 216 | class HighLevelMoveBase(smach.StateMachine, Loggable): 217 | def __init__(self,going_to_charge): 218 | smach.StateMachine.__init__(self, outcomes=['succeeded', 219 | 'move_base_failure', 220 | 'battery_low'], 221 | input_keys=['goal_pose'] ) 222 | self._monitored_recoverable_move_base = MonitoredRecoverableMoveBase(going_to_charge) 223 | self._recover_bumper = RecoverBumper() 224 | self._recover_carpet = RecoverStuckOnCarpet() 225 | self._nav_resume_monitor=NavPauseMonitor(True) 226 | with self: 227 | smach.StateMachine.add('MONITORED_MOVE_BASE', 228 | self._monitored_recoverable_move_base, 229 | transitions={'bumper_pressed': 'RECOVER_BUMPER', 230 | 'battery_low': 'battery_low', 231 | 'succeeded': 'succeeded', 232 | 'failure': 'move_base_failure', 233 | 'stuck_on_carpet':'RECOVER_STUCK_ON_CARPET', 234 | 'pause_requested':'NAV_RESUME_MONITOR'}) 235 | smach.StateMachine.add('RECOVER_BUMPER', 236 | self._recover_bumper, 237 | transitions={'succeeded': 'MONITORED_MOVE_BASE'}) 238 | smach.StateMachine.add('RECOVER_STUCK_ON_CARPET', 239 | self._recover_carpet, 240 | transitions={'succeeded': 'MONITORED_MOVE_BASE', 241 | 'failure': 'RECOVER_STUCK_ON_CARPET'}) 242 | smach.StateMachine.add('NAV_RESUME_MONITOR', 243 | self._nav_resume_monitor, 244 | transitions={'invalid': 'MONITORED_MOVE_BASE', 245 | 'valid': 'MONITORED_MOVE_BASE', 246 | 'preempted':'MONITORED_MOVE_BASE'}) 247 | 248 | 249 | """ 250 | Set the battery level thresholds. 251 | """ 252 | def set_patroller_thresholds(self, very_low_battery, low_battery, 253 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts): 254 | self._monitored_recoverable_move_base.set_patroller_thresholds(very_low_battery, 255 | low_battery, 256 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts) 257 | self._recover_bumper.set_patroller_thresholds(max_bumper_recovery_attempts) 258 | 259 | -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/patroller.py: -------------------------------------------------------------------------------- 1 | import smach 2 | import smach_ros 3 | import rospy 4 | import time 5 | 6 | from random import shuffle 7 | from . import charging, navigation 8 | from logger import Loggable 9 | 10 | 11 | from scitos_msgs.msg import BatteryState 12 | 13 | from geometry_msgs.msg import Pose 14 | 15 | from mongodb_store.message_store import MessageStoreProxy 16 | 17 | 18 | class VeryLowBattery(smach.State, Loggable): 19 | def __init__(self): 20 | smach.State.__init__(self) 21 | 22 | def execute(self, userdata): 23 | while not rospy.is_shutdown(): 24 | rospy.sleep(1) 25 | 26 | 27 | 28 | """ 29 | The point chooser state selects which waypoint to visit next. It checks the 30 | battery life, and if it is greater than CHARGE_BATTERY_TRESHOLD, it selects 31 | a new patrol point. Otherwise, the robot is sent to the charging station 32 | point (the point in the dataset with the name "charging_point". The ordering 33 | of visitng the patrolling points is either sequential or random, the constructor 34 | parameter, as is the number of iterations the robot should do until the state 35 | machine terminates with success 36 | 37 | outcomes: 'patrol': a standard waypoint was selected 38 | 'go_charge': the charging station is the next point 39 | 'succeeded': if required no. of iterations achieved 40 | 41 | ouput keys: 'goal_pose': geometry_msgs/Pose that should drive to 42 | 43 | """ 44 | class PointChooser(smach.State, Loggable): 45 | """ 46 | Constructor. 47 | :param waypoints_name: str, name of waypoints as appears in the datacentre 48 | :param is_random: bool, should the waypoints be visited in random order 49 | :param n_iterations: int, how many times to visit all the waypoints 50 | """ 51 | def __init__(self, waypoints_name, is_random, n_iterations): 52 | smach.State.__init__(self, 53 | outcomes=['patrol', 54 | 'go_charge', 55 | 'succeeded'], 56 | output_keys=['goal_pose'] 57 | ) 58 | 59 | self.LOW_BATTERY = 35 60 | 61 | self.msg_store = MessageStoreProxy() 62 | 63 | 64 | if not self.waypoints_available(waypoints_name): 65 | error_str = "Desired pointset '"+waypoints_name+"' not in datacentre" 66 | rospy.logerr(error_str) 67 | raise Exception(error_str) 68 | 69 | self.points = self._get_points(waypoints_name) # [] 70 | self.point_set = waypoints_name 71 | self.charing_station_pos = [waypoints_name, "charging_point"] 72 | 73 | self.current_point = -1 74 | self.n_points = len(self.points) 75 | 76 | self.is_random = is_random 77 | self.n_iterations = n_iterations 78 | self.iterations_completed = 0 79 | 80 | # rearranges the list of points to visit randomly 81 | if self.is_random: 82 | shuffle(self.points) 83 | 84 | self.battery_life = 100 85 | self.battery_monitor = rospy.Subscriber( 86 | "/battery_state", BatteryState, self.bat_cb) 87 | 88 | """ Check whether waypoints are available in db """ 89 | def waypoints_available(self, point_set): 90 | query_meta = {} 91 | query_meta["pointset"] = point_set 92 | return len(self.msg_store.query(Pose._type, {}, query_meta)) > 0 93 | 94 | """ Get a list of points in the given set """ 95 | def _get_points(self, point_set): 96 | query_meta = {} 97 | query_meta["pointset"] = point_set 98 | message_list = self.msg_store.query(Pose._type, {}, query_meta) 99 | 100 | points = [] 101 | for [point, meta] in message_list: 102 | # print point 103 | if meta["name"] != "charging_point": 104 | points.append([point_set, meta["name"]]) 105 | return points 106 | 107 | """ Get a given waypoint pose """ 108 | def _get_point(self, point_name, point_set): 109 | query_meta = {} 110 | query_meta["name"] = point_name 111 | query_meta["pointset"] = point_set 112 | return self.msg_store.query(Pose._type, {}, query_meta, True)[0] 113 | 114 | 115 | """ /battery_state subscription callback""" 116 | def bat_cb(self, msg): 117 | self.battery_life = msg.lifePercent 118 | 119 | def execute(self, userdata): 120 | rospy.sleep(1) 121 | 122 | if self.battery_life > self.LOW_BATTERY + 5: 123 | self.current_point = self.current_point + 1 124 | if self.current_point == self.n_points: 125 | self.iterations_completed = self.iterations_completed + 1 126 | if self.iterations_completed == self.n_iterations: 127 | return 'succeeded' 128 | self.current_point = 0 129 | if self.is_random: 130 | shuffle(self.points) 131 | 132 | current_pt = self.points[self.current_point] 133 | userdata.goal_pose = self._get_point(current_pt[1], current_pt[0]) 134 | self.get_logger().log_waypoint_visit(current_pt[1]) 135 | return 'patrol' 136 | else: 137 | userdata.goal_pose = self._get_point("charging_point", 138 | self.point_set) 139 | self.get_logger().log_waypoint_visit("charging_point") 140 | return 'go_charge' 141 | 142 | """ 143 | Set the battery level thresholds. 144 | """ 145 | def set_patroller_thresholds(self, very_low_battery, low_battery, 146 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts): 147 | self.LOW_BATTERY = low_battery 148 | 149 | """ 150 | A top level state machine, that implements waypoint patrolling. Waypoints are 151 | chosen by a PointChooser state. Navigation to the point then occurs, and another 152 | point is selected. If the battery is too low charging is started, and once 153 | charged, patrolling continues. 154 | 155 | outcomes: 'succeeded': required no. iterations of patrol points achieved 156 | 'aborted': something went wrong 157 | """ 158 | class WaypointPatroller(smach.StateMachine, Loggable): 159 | """ 160 | Constructor. 161 | :param waypoints_name: str, name of waypoints as appears in the datacentre 162 | :param is_random: bool, should the waypoints be visited in random order 163 | :param n_iterations: int, how many times to visit all the waypoints 164 | """ 165 | def __init__(self, waypoints_name, is_random, n_iterations): 166 | smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted']) 167 | 168 | self._waypoints_name = waypoints_name 169 | 170 | self._point_chooser = PointChooser(waypoints_name, 171 | is_random, 172 | n_iterations) 173 | self._high_level_move_base_patrol = navigation.HighLevelMoveBase(False) 174 | self._high_level_move_base_charge = navigation.HighLevelMoveBase(True) 175 | self._dock_undock = charging.HighLevelDockUndockBehaviour() 176 | self._very_low_battery=VeryLowBattery() 177 | 178 | with self: 179 | smach.StateMachine.add('POINT_CHOOSER', 180 | self._point_chooser, 181 | transitions={'patrol': 'PATROL_POINT', 182 | 'go_charge': 'GO_TO_CHARGING_STATION', 183 | 'succeeded': 'succeeded'}) 184 | smach.StateMachine.add('PATROL_POINT', 185 | #navigation.HighLevelMoveBase(), 186 | self._high_level_move_base_patrol, 187 | transitions={'succeeded': 'POINT_CHOOSER', 188 | 'battery_low': 'POINT_CHOOSER', 189 | 'move_base_failure': 'POINT_CHOOSER'}) 190 | smach.StateMachine.add('GO_TO_CHARGING_STATION', 191 | #navigation.HighLevelMoveBase(), 192 | self._high_level_move_base_charge, 193 | transitions={'succeeded': 'DOCK_AND_CHARGE', 194 | 'battery_low': 'VERY_LOW_BATTERY', 195 | 'move_base_failure': 'POINT_CHOOSER'}) 196 | 197 | smach.StateMachine.add('DOCK_AND_CHARGE', 198 | self._dock_undock, 199 | transitions={'succeeded': 'POINT_CHOOSER', 200 | 'failure': 'POINT_CHOOSER'}) 201 | 202 | smach.StateMachine.add('VERY_LOW_BATTERY', 203 | self._very_low_battery) 204 | 205 | """ 206 | Set the patoller thresholds. 207 | """ 208 | def set_patroller_thresholds(self, very_low_battery, low_battery, 209 | charged_battery,max_bumper_recovery_attempts, 210 | max_move_base_recovery_attempts): 211 | self._point_chooser.set_patroller_thresholds(very_low_battery, 212 | low_battery, 213 | charged_battery, 214 | max_bumper_recovery_attempts, 215 | max_move_base_recovery_attempts) 216 | self._high_level_move_base_patrol.set_patroller_thresholds(very_low_battery, 217 | low_battery, 218 | charged_battery, 219 | max_bumper_recovery_attempts, 220 | max_move_base_recovery_attempts) 221 | self._high_level_move_base_charge.set_patroller_thresholds(very_low_battery, 222 | low_battery, 223 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts) 224 | self._dock_undock.set_patroller_thresholds(very_low_battery, 225 | low_battery, 226 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts) 227 | rospy.loginfo("Updating patroller thresholds::") 228 | rospy.loginfo("V.Low="+str(very_low_battery)) 229 | rospy.loginfo("Low="+str(low_battery)) 230 | rospy.loginfo("Charged="+str(charged_battery)) 231 | rospy.loginfo("Max bumper recovers before sending e-mail="+str(max_bumper_recovery_attempts)) 232 | rospy.loginfo("Max move_base recovers before going to a new point="+str(max_move_base_recovery_attempts)) 233 | 234 | 235 | def execute(self, userdata=None): 236 | self.get_logger().log_start_episode(self._waypoints_name) 237 | outcome = smach.StateMachine.execute(self) 238 | self.get_logger().log_finish_episode() 239 | return outcome 240 | 241 | 242 | 243 | class Pulse(smach.State, Loggable): 244 | def __init__(self): 245 | smach.State.__init__(self, 246 | outcomes = ['aborted']) 247 | 248 | def execute(self,userdata): 249 | while True: 250 | self.get_logger().log_pulse() 251 | time.sleep(300) # every 5 minutes 252 | 253 | 254 | 255 | class WaypointPatrollerWithPulse(smach.Concurrence, Loggable): 256 | def __init__(self,waypoints_name, is_random, n_iterations): 257 | smach.Concurrence.__init__(self, 258 | outcomes=['succeeded', 'aborted'], 259 | default_outcome='aborted', 260 | child_termination_cb=self.child_term_cb, 261 | outcome_cb=self.out_cb 262 | ) 263 | self._patroller = WaypointPatroller(waypoints_name, is_random, n_iterations) 264 | self._pulse = Pulse() 265 | with self: 266 | smach.Concurrence.add('PATROLLER', self._patroller) 267 | smach.Concurrence.add('PULSE', self._pulse) 268 | 269 | def child_term_cb(self, outcome_map): 270 | # decide if this state is done when one or more concurrent inner states 271 | # stop. Only done when the patroler is done 272 | if ( outcome_map['PATROLLER'] == 'succeeded' or 273 | outcome_map['PATROLLER'] == 'aborted'): 274 | return True 275 | return False 276 | 277 | def out_cb(self, outcome_map): 278 | # determine what the outcome of this machine is 279 | 280 | # rospy.sleep(0.1) without this sleep, sometimes the concurrence container 281 | # terminates before all its children terminate, and an error is printed. 282 | # However, that does not affect the evolution, and I think that with the 283 | # sleep sometimes the container blocks and never terminates 284 | return outcome_map['PATROLLER'] 285 | 286 | """ 287 | Set the battery level thresholds. 288 | """ 289 | def set_patroller_thresholds(self, very_low_battery, low_battery, 290 | charged_battery,max_bumper_recovery_attempts,max_move_base_recovery_attempts): 291 | self._patroller.set_patroller_thresholds(very_low_battery, 292 | low_battery, 293 | charged_battery, 294 | max_bumper_recovery_attempts, 295 | max_move_base_recovery_attempts) 296 | 297 | 298 | 299 | -------------------------------------------------------------------------------- /waypoint_patroller/src/waypoint_patroller/recover_states.py: -------------------------------------------------------------------------------- 1 | # TODO: Add speaking back in here 2 | import rospy 3 | 4 | import smach 5 | import smach_ros 6 | 7 | from scitos_msgs.srv import ResetMotorStop 8 | from scitos_msgs.srv import EnableMotors 9 | from std_srvs.srv import Empty 10 | 11 | from scitos_msgs.msg import MotorStatus 12 | from geometry_msgs.msg import Twist 13 | 14 | import actionlib 15 | #from actionlib_msgs.msg import * 16 | from mary_tts.msg import * 17 | 18 | from logger import Loggable 19 | 20 | import marathon_touch_gui.client 21 | 22 | 23 | # this file has the recovery states that will be used when some failures are 24 | # detected. There is a recovery behaviour for move_base and another for when the 25 | # bumper is pressed 26 | 27 | 28 | 29 | class RecoverMoveBase(smach.State, Loggable): 30 | def __init__(self,max_move_base_recovery_attempts=5): 31 | smach.State.__init__(self, 32 | # we need the number of move_base fails as 33 | # incoming data from the move_base action state, 34 | # because it is not possible for this recovery 35 | # behaviour to check if it was succeeded 36 | outcomes=['succeeded', 'failure', 'preempted'], 37 | input_keys=['n_move_base_fails'], 38 | output_keys=['n_move_base_fails'], 39 | ) 40 | #self.vel_pub = rospy.Publisher('/cmd_vel', Twist) 41 | #self._vel_cmd = Twist() 42 | #self._vel_cmd.linear.x = -0.1 43 | self.set_patroller_thresholds(max_move_base_recovery_attempts) 44 | 45 | 46 | self.enable_motors= rospy.ServiceProxy('enable_motors', 47 | EnableMotors) 48 | 49 | #self.clear_costmap 50 | 51 | self.speaker=actionlib.SimpleActionClient('/speak', maryttsAction) 52 | self.speak_goal= maryttsGoal() 53 | self.speaker.wait_for_server() 54 | 55 | self.being_helped=False 56 | self.help_finished=False 57 | 58 | 59 | 60 | def help_offered(self, req): 61 | self.being_helped=True 62 | return [] 63 | 64 | 65 | def nav_recovered(self,req): 66 | self.being_helped=False 67 | self.help_finished=True 68 | return [] 69 | 70 | 71 | def execute(self, userdata): 72 | # move slowly backwards a bit. A better option might be to save the 73 | # latest messages received in cmd_vel and reverse them 74 | #for i in range(0, 20): 75 | #self.vel_pub.publish(self._vel_cmd) 76 | #if self.preempt_requested(): 77 | #self.service_preempt() 78 | #return 'preempted' 79 | #rospy.sleep(0.2) 80 | 81 | self.isRecovered=False 82 | 83 | self.enable_motors(False) 84 | rospy.sleep(0.2) 85 | # since there is no way to check if the recovery behaviour was 86 | # successful, we always go back to the move_base action state with 87 | # 'succeeded' until the number of failures treshold is reached 88 | if userdata.n_move_base_fails < self.MAX_MOVE_BASE_RECOVERY_ATTEMPTS: 89 | self.get_logger().log_navigation_recovery_attempt(success=True, 90 | attempts=userdata.n_move_base_fails) 91 | 92 | 93 | help_offered_monitor=rospy.Service('/patroller/help_offered', Empty, self.help_offered) 94 | help_done_monitor=rospy.Service('/patroller/robot_moved', Empty, self.nav_recovered) 95 | 96 | displayNo = rospy.get_param("~display", 0) 97 | 98 | on_completion = 'help_offered' 99 | service_prefix = '/patroller' 100 | marathon_touch_gui.client.nav_fail(displayNo, service_prefix, on_completion) 101 | 102 | 103 | 104 | self.speak_goal.text='I am having problems moving. Please push me to a clear area.' 105 | self.speaker.send_goal(self.speak_goal) 106 | self.speaker.wait_for_result() 107 | 108 | for i in range(0,40): 109 | if self.being_helped: 110 | on_completion = 'robot_moved' 111 | service_prefix = '/patroller' 112 | marathon_touch_gui.client.being_helped(displayNo, service_prefix, on_completion) 113 | break 114 | rospy.sleep(1) 115 | 116 | if self.being_helped: 117 | self.being_helped=False 118 | for i in range(0,60): 119 | if self.help_finished: 120 | self.get_logger().log_helped("navigation") 121 | self.speak_goal.text='Thank you! I will be on my way.' 122 | self.speaker.send_goal(self.speak_goal) 123 | self.speaker.wait_for_result() 124 | self.help_finished=False 125 | break 126 | rospy.sleep(1) 127 | 128 | marathon_touch_gui.client.display_main_page(displayNo) 129 | help_offered_monitor.shutdown() 130 | help_done_monitor.shutdown() 131 | return 'succeeded' 132 | else: 133 | userdata.n_move_base_fails=0 134 | self.get_logger().log_navigation_recovery_attempt(success=False, 135 | attempts=userdata.n_move_base_fails) 136 | return 'failure' 137 | 138 | 139 | 140 | def set_patroller_thresholds(self, max_move_base_recovery_attempts): 141 | if max_move_base_recovery_attempts is not None: 142 | self.MAX_MOVE_BASE_RECOVERY_ATTEMPTS = max_move_base_recovery_attempts 143 | 144 | 145 | 146 | class RecoverBumper(smach.State, Loggable): 147 | def __init__(self,max_bumper_recovery_attempts=5): 148 | smach.State.__init__(self, 149 | outcomes=['succeeded'] 150 | ) 151 | self.reset_motorstop = rospy.ServiceProxy('reset_motorstop', 152 | ResetMotorStop) 153 | self.enable_motors= rospy.ServiceProxy('enable_motors', 154 | EnableMotors) 155 | self.being_helped = False 156 | self.help_finished=False 157 | self.motor_monitor = rospy.Subscriber("/motor_status", 158 | MotorStatus, 159 | self.bumper_monitor_cb) 160 | 161 | self.speaker=actionlib.SimpleActionClient('/speak', maryttsAction) 162 | self.speak_goal= maryttsGoal() 163 | self.speaker.wait_for_server() 164 | self.set_patroller_thresholds(max_bumper_recovery_attempts) 165 | 166 | 167 | 168 | 169 | def help_offered(self, req): 170 | self.being_helped=True 171 | return [] 172 | 173 | 174 | def bumper_recovered(self,req): 175 | self.being_helped=False 176 | self.help_finished=True 177 | return [] 178 | 179 | def bumper_monitor_cb(self, msg): 180 | self.isRecovered = not msg.bumper_pressed 181 | 182 | # restarts the motors and check to see of they really restarted. 183 | # Between each failure the waiting time to try and restart the motors 184 | # again increases. This state can check its own success 185 | def execute(self, userdata): 186 | help_offered_monitor=rospy.Service('/patroller/help_offered', Empty, self.help_offered) 187 | help_done_monitor=rospy.Service('/patroller/bumper_recovered', Empty, self.bumper_recovered) 188 | displayNo = rospy.get_param("~display", 0) 189 | n_tries=1 190 | while True: 191 | if self.being_helped: 192 | on_completion = 'bumper_recovered' 193 | service_prefix = '/patroller' 194 | marathon_touch_gui.client.being_helped(displayNo, service_prefix, on_completion) 195 | for i in range(0,60): 196 | if self.help_finished: 197 | break 198 | rospy.sleep(1) 199 | if not self.help_finished: 200 | self.being_helped=False 201 | on_completion = 'help_offered' 202 | service_prefix = '/patroller' 203 | marathon_touch_gui.client.bumper_stuck(displayNo, service_prefix, on_completion) 204 | elif self.help_finished: 205 | self.help_finished=False 206 | self.reset_motorstop() 207 | rospy.sleep(0.1) 208 | if self.isRecovered: 209 | self.get_logger().log_helped("bumper") 210 | self.speak_goal.text='Thank you! I will be on my way.' 211 | self.speaker.send_goal(self.speak_goal) 212 | self.speaker.wait_for_result() 213 | help_done_monitor.shutdown() 214 | help_offered_monitor.shutdown() 215 | self.get_logger().log_bump_count(n_tries) 216 | marathon_touch_gui.client.display_main_page(displayNo) 217 | return 'succeeded' 218 | else: 219 | self.speak_goal.text='Something is still wrong. Are you sure I am in a clear area?' 220 | self.speaker.send_goal(self.speak_goal) 221 | self.speaker.wait_for_result() 222 | on_completion = 'help_offered' 223 | service_prefix = '/patroller' 224 | marathon_touch_gui.client.bumper_stuck(displayNo, service_prefix, on_completion) 225 | else: 226 | for i in range(0,4*n_tries): 227 | if self.being_helped: 228 | break 229 | self.enable_motors(False) 230 | self.reset_motorstop() 231 | rospy.sleep(0.1) 232 | if self.isRecovered: 233 | help_done_monitor.shutdown() 234 | help_offered_monitor.shutdown() 235 | self.get_logger().log_bump_count(n_tries) 236 | marathon_touch_gui.client.display_main_page(displayNo) 237 | return 'succeeded' 238 | rospy.sleep(1) 239 | #if n_tries>self.MAX_BUMPER_RECOVERY_ATTEMPTS: 240 | #send email 241 | n_tries += 1 242 | 243 | if n_tries==2: 244 | on_completion = 'help_offered' 245 | service_prefix = '/patroller' 246 | marathon_touch_gui.client.bumper_stuck(displayNo, service_prefix, on_completion) 247 | 248 | if n_tries>1: 249 | self.speak_goal.text='My bumper is being pressed. Please release it so I can move on!' 250 | self.speaker.send_goal(self.speak_goal) 251 | self.speaker.wait_for_result() 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | def set_patroller_thresholds(self, max_bumper_recovery_attempts): 260 | if max_bumper_recovery_attempts is not None: 261 | self.MAX_BUMPER_RECOVERY_ATTEMPTS = max_bumper_recovery_attempts 262 | 263 | 264 | class RecoverStuckOnCarpet(smach.State, Loggable): 265 | def __init__(self): 266 | smach.State.__init__(self, 267 | outcomes = ['succeeded','failure']) 268 | self.vel_pub = rospy.Publisher('/cmd_vel', Twist) 269 | self._vel_cmd = Twist() 270 | 271 | 272 | 273 | 274 | def execute(self,userdata): 275 | #small forward vel to unstuck robot 276 | self._vel_cmd.linear.x=0.8 277 | self._vel_cmd.angular.z=0.4 278 | for i in range(0,4): 279 | self.vel_pub.publish(self._vel_cmd) 280 | self._vel_cmd.linear.x=self._vel_cmd.linear.x-0.2 281 | self._vel_cmd.angular.z=self._vel_cmd.angular.z-0.2 282 | # if self.preempt_requested(): 283 | # self.service_preempt() 284 | # return 'preempted' 285 | rospy.sleep(0.2) 286 | # os.system("rosservice call /ros_mary 'Recovering carpet stuck'") 287 | self._vel_cmd.linear.x=0.0 288 | self._vel_cmd.angular.z=0.0 289 | self.vel_pub.publish(self._vel_cmd) 290 | 291 | # now = rospy.get_rostime() 292 | # f = open('/home/bruno/log.txt','a') 293 | # f.write(str(rospy.get_time())) 294 | # f.write('\n') 295 | # f.close() 296 | 297 | #check if behaviour was successful 298 | if True: 299 | return 'succeeded' 300 | else: 301 | return 'failure' 302 | --------------------------------------------------------------------------------