├── CMakeLists.txt ├── LICENSE ├── README.md ├── images └── models_screenshot.png ├── launch ├── dynamic_models_test.launch ├── elevator_doors.launch └── empty_world.launch ├── media └── meshes │ └── elevator.dae ├── models ├── elev_slide_left.sdf ├── elev_slide_right.sdf ├── elevator.sdf ├── flip_door_left.sdf ├── flip_door_right.sdf ├── slide_left.sdf └── slide_right.sdf ├── msg └── ControlGroup.msg ├── package.xml ├── src ├── controllers │ ├── control_group.h │ ├── dynamics_manager.cpp │ └── keyboard_op.cpp └── plugins │ ├── auto_elev_door_plugin.cc │ ├── door_plugin.cc │ └── elevator_plugin.cc ├── srv ├── AddGroup.srv ├── DeleteGroup.srv ├── ListGroups.srv ├── OpenCloseDoors.srv ├── OpenCloseElevDoors.srv ├── SetElevProps.srv ├── SetVelDoors.srv └── TargetFloorElev.srv └── worlds └── empty.world /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dynamic_gazebo_models) 3 | 4 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") 5 | 6 | ## Find catkin macros and libraries 7 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 8 | ## is used, also find other catkin packages 9 | include(FindProtobuf) 10 | find_package(catkin REQUIRED COMPONENTS roscpp nodelet tf gazebo_plugins gazebo_ros message_generation geometry_msgs) 11 | find_package(Boost 1.40 COMPONENTS program_options REQUIRED) 12 | find_package(Protobuf REQUIRED) 13 | 14 | #add services: 15 | add_service_files(DIRECTORY srv FILES AddGroup.srv DeleteGroup.srv OpenCloseDoors.srv SetVelDoors.srv TargetFloorElev.srv SetElevProps.srv OpenCloseElevDoors.srv ListGroups.srv) 16 | add_message_files(DIRECTORY msg FILES ControlGroup.msg) 17 | 18 | generate_messages(DEPENDENCIES std_msgs geometry_msgs) 19 | 20 | #add catkin sourced packages: 21 | catkin_package( 22 | LIBRARIES ${PROJECT_NAME} 23 | CATKIN_DEPENDS roscpp nodelet std_msgs geometry_msgs tf gazebo_plugins gazebo_ros message_runtime 24 | ) 25 | 26 | #find and add gazebo 27 | include (FindPkgConfig) 28 | if (PKG_CONFIG_FOUND) 29 | pkg_check_modules(GAZEBO gazebo) 30 | endif() 31 | 32 | include_directories(${GAZEBO_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIR} ${PROTOBUF_INCLUDE_DIR}) 33 | 34 | #Controller Executables: 35 | add_executable(dynamics_manager src/controllers/dynamics_manager.cpp src/controllers/control_group.h) 36 | add_dependencies(dynamics_manager ${PROJECT_NAME}_generate_messages_cpp) 37 | target_link_libraries(dynamics_manager ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${PROTOBUF_LIBRARY}) 38 | 39 | add_executable(keyboard_op src/controllers/keyboard_op.cpp) 40 | add_dependencies(keyboard_op ${PROJECT_NAME}_generate_messages_cpp) 41 | target_link_libraries(keyboard_op ${catkin_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY}) 42 | 43 | #Plugin Libraries: 44 | add_library(door_plugin src/plugins/door_plugin.cc) 45 | target_link_libraries(door_plugin ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${PROTOBUF_LIBRARY}) 46 | 47 | add_library(elevator src/plugins/elevator_plugin.cc) 48 | target_link_libraries(elevator ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${PROTOBUF_LIBRARY}) 49 | 50 | add_library(auto_door src/plugins/auto_elev_door_plugin.cc) 51 | target_link_libraries(auto_door ${GAZEBO_LIBRARIES} ${catkin_LIBRARIES} ${Boost_FILESYSTEM_LIBRARY} ${Boost_SYSTEM_LIBRARY} ${PROTOBUF_LIBRARY}) 52 | 53 | install(TARGETS dynamics_manager keyboard_op door_plugin elevator auto_door 54 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 57 | 58 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2014 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Dynamic Gazebo Models 2 | ============== 3 | 4 | Gazebo models for simulating doors/elevators. Currently available models: flip-open doors, slide-open doors, elevators with automatic slide-open doors (more coming soon). Plus, the bundle also comes with a generic dynamics-manager to control model groups via ROS service-calls or keyboard-op. 5 | 6 | ## Dependencies & Prerequisites 7 | [ROS Hydro](http://wiki.ros.org/hydro), [Gazebo 3.0+](http://gazebosim.org/), 8 | [Catkin](http://wiki.ros.org/catkin): see [package.xml](package.xml) 9 | 10 | ## Installation 11 | Clone Repo: 12 | ```bash 13 | $ cd /src 14 | $ git clone https://github.com/MohitShridhar/dynamic_gazebo_models.git 15 | ``` 16 | Resolve dependencies (in Ubuntu) & Compile: 17 | ```bash 18 | $ cd 19 | $ rosdep install --from-paths src --ignore-src --rosdistro hydro -y 20 | $ catkin_make --pkg dynamic_gazebo_models 21 | ``` 22 | Launch sample: 23 | ```bash 24 | $ roslaunch dynamic_gazebo_models dynamic_models_test.launch 25 | ``` 26 | There are a lot of models to spawn, so be patient. You should see a bunch of doors and elevators: 27 | ![Flip-open, slide-open, elevators & auto-doors](images/models_screenshot.png) 28 | 29 | ## Control 30 | 31 | ### Manual 32 | ```bash 33 | $ rosrun dynamic_gazebo_models keyboard_op 34 | ``` 35 | Follow the instructions to control a group of doors | elevators. 36 | 37 | ## Guide 38 | 39 | See the [wiki](https://github.com/MohitShridhar/dynamic_gazebo_models/wiki/User-Guide) for more details. 40 | 41 | ## More Examples 42 | 43 | [multi_map_navigation](https://github.com/MohitShridhar/multi_map_navigation), [oculus_gazebo_navigator](https://github.com/MohitShridhar/oculus_gazebo_navigator) 44 | 45 | -------------------------------------------------------------------------------- /images/models_screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/MohitShridhar/dynamic_gazebo_models/936362a817662216a854764192ce64a3db2eb353/images/models_screenshot.png -------------------------------------------------------------------------------- /launch/dynamic_models_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 74 | 75 | 86 | 87 | 98 | 99 | 100 | 111 | 112 | 123 | 124 | 125 | -------------------------------------------------------------------------------- /launch/elevator_doors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | -------------------------------------------------------------------------------- /launch/empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /media/meshes/elevator.dae: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Blender User 6 | Blender 2.70.0 commit date:2014-04-10, commit time:11:49, hash:f93bc76 7 | 8 | 2014-06-18T10:03:21 9 | 2014-06-18T10:03:21 10 | 11 | Z_UP 12 | 13 | 14 | 15 | 16 | 17 | 18 | 49.13434 19 | 1.777778 20 | 0.1 21 | 100 22 | 23 | 24 | 25 | 26 | 27 | 0 28 | 0 29 | 0 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | -0.09430396 -0.09430396 -0.09430396 -0.09430396 -0.07553315 -0.07777857 -0.09430396 0.07781541 -0.07777857 -0.09430396 0.07781541 0.0755701 -0.09430396 0.09430396 0.09430408 -0.09430396 0.09430396 -0.09430396 -0.09430396 -0.09430396 0.09430408 -0.09430396 -0.07553315 0.0755701 0.09430408 -0.09430396 0.09430408 0.09430408 -0.09430396 -0.09430396 0.09430408 0.09430396 0.09430408 0.09430408 0.09430396 -0.09430396 0.09430408 -0.07553315 -0.07777857 0.09430408 -0.07553315 0.0755701 0.09430408 0.07781541 0.0755701 0.09430408 0.07781541 -0.07777857 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | -1 0 0 -1 0 0 0 -1 0 0 0 1 0 1 0 0 0 -1 1 0 0 0 -1 0 0 -1 0 0 0 -1 0 0 1 0 1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0 -1 0 -1 0 0 0 0 1 0 1 0 0 0 -1 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0 0 -1 0 0 1 0 1 0 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 3 65 |

6 0 7 0 0 0 7 1 1 1 0 1 8 2 6 2 0 2 8 3 10 3 4 3 4 4 10 4 11 4 0 5 5 5 11 5 13 6 10 6 8 6 14 7 3 7 15 7 3 8 2 8 15 8 14 9 13 9 7 9 2 10 1 10 12 10 13 11 12 11 1 11 5 12 0 12 1 12 4 13 5 13 3 13 5 14 1 14 2 14 5 15 2 15 3 15 3 16 7 16 4 16 9 17 8 17 0 17 7 18 6 18 4 18 6 19 8 19 4 19 5 20 4 20 11 20 9 21 0 21 11 21 12 22 13 22 9 22 13 23 14 23 10 23 11 24 10 24 14 24 13 25 8 25 9 25 9 26 11 26 12 26 11 27 14 27 15 27 11 28 15 28 12 28 3 29 14 29 7 29 15 30 2 30 12 30 7 31 13 31 1 31

66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 0.6858805 -0.3173701 0.6548619 7.481132 0.7276338 0.3124686 -0.6106656 -6.50764 -0.01081678 0.8953432 0.4452454 5.343665 0 0 0 1 75 | 76 | 77 | 78 | 1.354642 0 0 -0.01117456 0 0.8829969 0 -0.03046834 0 0 1.409306 0.1828508 0 0 0 1 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /models/elev_slide_left.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10.615 6.25 1.06 0 -0 1.57 5 | 6 | 0 0 0 0 -0 0 7 | 10 8 | 9 | 1 10 | 0 11 | 0 12 | 0.1 13 | 0 14 | 0.1 15 | 16 | 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | 0.8 0.06 2.47 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 0 0 0 0 -0 0 35 | 36 | 37 | 0.8 0.06 2.47 38 | 39 | 40 | 41 | 45 | 46 | 47 | 0 48 | 49 | 0 50 | 0 51 | 52 | 0 53 | 54 | 55 | door 56 | world 57 | 58 | -0.000796327 -1 0 59 | 60 | 0 61 | -0.05 62 | 10 63 | 0.5 64 | 65 | 66 | 1 67 | 68 | 69 | 70 | 71 | 72 | auto_door_ 73 | elevator_1 74 | left 75 | 0.711305 76 | 1.0 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /models/elev_slide_right.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10.615 6.25 1.06 0 -0 1.57 5 | 6 | 0 0 0 0 -0 0 7 | 10 8 | 9 | 1 10 | 0 11 | 0 12 | 0.1 13 | 0 14 | 0.1 15 | 16 | 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | 0.8 0.06 2.47 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 0 0 0 0 -0 0 35 | 36 | 37 | 0.8 0.06 2.47 38 | 39 | 40 | 41 | 45 | 46 | 47 | 0 48 | 49 | 0 50 | 0 51 | 2 52 | 0 53 | 54 | 55 | door 56 | world 57 | 58 | -0.000796327 1 0 59 | 60 | 0 61 | -0.05 62 | 10 63 | 0.5 64 | 65 | 66 | 1 67 | 68 | 69 | 70 | 71 | 72 | auto_door_ 73 | elevator_1 74 | right 75 | 0.711305 76 | 1.0 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /models/elevator.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 -0.5 0 0 0 5 | 6 | 0 0 0 0 0 0 7 | 8 | -0.05 0 0 0 0 0 9 | 7.5 10 | 11 | 1 12 | 0 13 | 0 14 | 1 15 | 0 16 | 1 17 | 18 | 19 | 20 | 21 | 22 | file://elevator.dae 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | file://elevator.dae 38 | 39 | 40 | 41 | 45 | 46 | 47 | 0 48 | 49 | 0 50 | 0 51 | 52 | 1 53 | 54 | 55 | 56 | body 57 | world 58 | 59 | 0 0 -1 60 | 61 | -40 62 | 40 63 | 0 64 | 0 65 | 66 | 67 | 68 | 69 | 70 | 71 | elevator_ 72 | 0.84108, 3.65461, 6.85066, 10.0470, 13.24549, 16.45915, 19.65369 73 | 1.5 74 | 100 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /models/flip_door_left.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0 0 0 0 6 | 7 | -0.05 0 0 0 0 0 8 | 0.5 9 | 10 | 1 11 | 0 12 | 0 13 | 100 14 | 0 15 | 1 16 | 17 | 18 | 19 | 0 20 | 0 21 | 22 | 23 | 24 | 25 | 0.05 0.2 0.35 0 0 -1.57 26 | 27 | 0.05 0.2 0.35 0 0 -1.57 28 | 5 29 | 30 | 1 31 | 0 32 | 0 33 | 100 34 | 0 35 | 1 36 | 37 | 38 | 39 | 0.05 0.2 0.35 0 0 -1.57 40 | 41 | 42 | 0.927 0.1 1.96 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 0.05 0.2 0.35 0 0 -1.57 56 | 57 | 58 | 0.927 0.1 1.96 59 | 60 | 61 | 62 | 66 | 67 | 68 | 1 69 | 70 | 0 71 | 0 72 | 73 | 0 74 | 75 | 76 | 77 | door 78 | hinge 79 | 0.08 0 0 0 0 0 80 | 81 | 0 0 1 82 | 83 | 0 84 | 1.57 85 | 0 86 | 10 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | hinge 95 | world 96 | 97 | 0 0 1 98 | 99 | 0 100 | 0 101 | 0 102 | 0 103 | 104 | 105 | 106 | 107 | 108 | 109 | flip 110 | door_ 111 | counter_clockwise 112 | 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /models/flip_door_right.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 0 0 0 0 0 0 6 | 7 | -0.05 0 0 0 0 0 8 | 0.5 9 | 10 | 1 11 | 0 12 | 0 13 | 100 14 | 0 15 | 1 16 | 17 | 18 | 19 | 20 | 0 21 | 0 22 | 23 | 24 | 25 | 26 | 0.05 -0.2 0.35 0 0 -1.57 27 | 28 | 0.05 -0.2 0.35 0 0 -1.57 29 | 5 30 | 31 | 1 32 | 0 33 | 0 34 | 100 35 | 0 36 | 1 37 | 38 | 39 | 40 | 0.05 -0.2 0.35 0 0 -1.57 41 | 42 | 43 | 0.927 0.1 1.96 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 0.05 -0.2 0.35 0 0 -1.57 57 | 58 | 59 | 0.927 0.1 1.96 60 | 61 | 62 | 63 | 67 | 68 | 69 | 1 70 | 71 | 0 72 | 0 73 | 74 | 0 75 | 76 | 77 | 78 | door 79 | hinge 80 | 0.08 0 0 0 0 0 81 | 82 | 0 0 1 83 | 84 | -1.57 85 | 0 86 | 0 87 | 10 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | hinge 96 | world 97 | 98 | 0 0 1 99 | 100 | 0 101 | 0 102 | 0 103 | 0 104 | 105 | 106 | 107 | 108 | 109 | 110 | flip 111 | door_ 112 | clockwise 113 | 114 | 115 | 116 | 117 | -------------------------------------------------------------------------------- /models/slide_left.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10.615 6.25 1.06 0 -0 1.57 5 | 6 | 0 0 0 0 -0 0 7 | 10 8 | 9 | 1 10 | 0 11 | 0 12 | 0.1 13 | 0 14 | 0.1 15 | 16 | 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | 0.8 0.06 2.47 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 0 0 0 0 -0 0 35 | 36 | 37 | 0.8 0.06 2.47 38 | 39 | 40 | 41 | 45 | 46 | 47 | 0 48 | 49 | 0 50 | 0 51 | 52 | 0 53 | 54 | 55 | door 56 | world 57 | 58 | -0.000796327 -1 0 59 | 60 | 0 61 | -0.05 62 | 10 63 | 0.5 64 | 65 | 66 | 1 67 | 68 | 69 | 70 | 71 | 72 | slide 73 | door_ 74 | left 75 | 0.711305 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /models/slide_right.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 10.615 6.25 1.06 0 -0 1.57 5 | 6 | 0 0 0 0 -0 0 7 | 10 8 | 9 | 1 10 | 0 11 | 0 12 | 0.1 13 | 0 14 | 0.1 15 | 16 | 17 | 18 | 0 0 0 0 -0 0 19 | 20 | 21 | 0.8 0.06 2.47 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 0 0 0 0 -0 0 35 | 36 | 37 | 0.8 0.06 2.47 38 | 39 | 40 | 41 | 45 | 46 | 47 | 0 48 | 49 | 0 50 | 0 51 | 52 | 0 53 | 54 | 55 | door 56 | world 57 | 58 | -0.000796327 1 0 59 | 60 | 0 61 | -0.05 62 | 10 63 | 0.5 64 | 65 | 66 | 1 67 | 68 | 69 | 70 | 71 | 72 | slide 73 | door_ 74 | right 75 | 0.711305 76 | 77 | 78 | 79 | 80 | 81 | -------------------------------------------------------------------------------- /msg/ControlGroup.msg: -------------------------------------------------------------------------------- 1 | # Properties of a control group 2 | 3 | string group_name 4 | string type 5 | uint32[] active_units -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dynamic_gazebo_models 4 | 0.0.0 5 | Dynamic models for Gazebo 6 | 7 | MohitSridhar 8 | MohitSridhar 9 | 10 | MIT 11 | 12 | catkin 13 | 14 | roscpp 15 | nodelet 16 | std_msgs 17 | geometry_msgs 18 | gazebo_ros 19 | tf 20 | gazebo_plugins 21 | message_generation 22 | 23 | roscpp 24 | nodelet 25 | std_msgs 26 | geometry_msgs 27 | gazebo_ros 28 | tf 29 | gazebo_plugins 30 | message_runtime 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /src/controllers/control_group.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014 Mohit Shridhar, David Lee 2 | 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | 21 | enum GroupType {DOOR, ELEVATOR, INVALID}; 22 | 23 | class ControlGroup 24 | { 25 | private: 26 | 27 | std::string group_name; 28 | GroupType type; 29 | std::vector active_units; 30 | 31 | public: 32 | 33 | ControlGroup(std::string group_name, GroupType type, std::vector active_units) 34 | { 35 | this->group_name = group_name; 36 | this->type = type; 37 | this->active_units = active_units; 38 | } 39 | 40 | std::string getGroupName() 41 | { 42 | return this->group_name; 43 | } 44 | 45 | void setGroupName(std::string group_name) 46 | { 47 | this->group_name = group_name; 48 | } 49 | 50 | GroupType getType() 51 | { 52 | return this->type; 53 | } 54 | 55 | void setGroupType(GroupType type) 56 | { 57 | this->type = type; 58 | } 59 | 60 | std::vector getActiveUnits() 61 | { 62 | return this->active_units; 63 | } 64 | 65 | void setActiveUnits(std::vector active_units) 66 | { 67 | this->active_units = active_units; 68 | } 69 | }; -------------------------------------------------------------------------------- /src/controllers/dynamics_manager.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014 Mohit Shridhar, David Lee 2 | 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | #include "control_group.h" 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #define TYPE_DOOR_STR "door" 44 | #define TYPE_ELEVATOR_STR "elevator" 45 | 46 | #define DEFAULT_SLIDE_SPEED 1 // in m/s 47 | #define DEFAULT_FLIP_SPEED 1.57 // in rad/s 48 | 49 | #define STATE_OPEN true 50 | #define STATE_CLOSE false 51 | 52 | #define ELEV_DOOR_STATE_OPEN 1 53 | #define ELEV_DOOR_STATE_CLOSE 0 54 | #define ELEV_DOOR_STATE_FREE 2 55 | 56 | #define INDEX_NOT_FOUND -1 57 | 58 | /* 59 | 60 | Limitations: 61 | Sometimes service calls are dropped without notice. Solution: Implement 'wait for call' 62 | */ 63 | 64 | class DynamicsController 65 | { 66 | private: 67 | 68 | ros::NodeHandle rosNode; 69 | ros::ServiceServer add_group_server, delete_group_server, list_groups_server; 70 | ros::ServiceServer open_close_doors_server, set_vel_doors_server, target_floor_elev_server, set_elev_props_server, open_close_elev_doors_server; 71 | 72 | ros::Publisher door_cmd_vel_pub, door_active_pub; 73 | ros::Publisher elev_target_pub, elev_active_pub, elev_param_pub, elev_door_pub; 74 | 75 | std::vector groups; 76 | 77 | public: 78 | 79 | DynamicsController(ros::NodeHandle &nh) 80 | { 81 | rosNode = nh; 82 | nh = ros::NodeHandle(""); 83 | 84 | setupControlTopics(); 85 | setupManagerServices(); 86 | } 87 | 88 | void setupManagerServices() 89 | { 90 | add_group_server = rosNode.advertiseService("model_dynamics_manager/add_control_group", &DynamicsController::add_control_group_cb, this); 91 | delete_group_server = rosNode.advertiseService("model_dynamics_manager/delete_control_group", &DynamicsController::delete_control_group_cb, this); 92 | list_groups_server = rosNode.advertiseService("model_dynamics_manager/list_groups", &DynamicsController::list_groups_cb, this); 93 | 94 | open_close_doors_server = rosNode.advertiseService("model_dynamics_manager/doors/open_close", &DynamicsController::open_close_doors_cb, this); 95 | set_vel_doors_server = rosNode.advertiseService("model_dynamics_manager/doors/set_vel", &DynamicsController::set_vel_doors_cb, this); 96 | 97 | target_floor_elev_server = rosNode.advertiseService("model_dynamics_manager/elevators/target_floor", &DynamicsController::target_floor_elev_cb, this); 98 | set_elev_props_server = rosNode.advertiseService("model_dynamics_manager/elevators/set_props", &DynamicsController::set_elev_props_cb, this); 99 | open_close_elev_doors_server = rosNode.advertiseService("model_dynamics_manager/elevators/open_close_elev", &DynamicsController::open_close_elev_cb, this); 100 | } 101 | 102 | bool open_close_doors_cb(dynamic_gazebo_models::OpenCloseDoors::Request &req, dynamic_gazebo_models::OpenCloseDoors::Response &res) 103 | { 104 | if (!activateDoors(req.group_name)) { 105 | return false; 106 | } 107 | 108 | geometry_msgs::Twist cmd_vel; 109 | 110 | if (req.state == STATE_OPEN) { 111 | cmd_vel.linear.x = -DEFAULT_SLIDE_SPEED; 112 | cmd_vel.linear.y = -DEFAULT_SLIDE_SPEED; 113 | cmd_vel.angular.z = -DEFAULT_FLIP_SPEED; 114 | } else { 115 | cmd_vel.linear.x = DEFAULT_SLIDE_SPEED; 116 | cmd_vel.linear.y = DEFAULT_SLIDE_SPEED; 117 | cmd_vel.angular.z = DEFAULT_FLIP_SPEED; 118 | } 119 | 120 | door_cmd_vel_pub.publish(cmd_vel); 121 | 122 | return true; 123 | } 124 | 125 | bool set_vel_doors_cb(dynamic_gazebo_models::SetVelDoors::Request &req, dynamic_gazebo_models::SetVelDoors::Response &res) 126 | { 127 | if (!activateDoors(req.group_name)) { 128 | return false; 129 | } 130 | 131 | geometry_msgs::Twist cmd_vel; 132 | 133 | cmd_vel.linear.x = req.lin_x; 134 | cmd_vel.linear.y = req.lin_y; 135 | cmd_vel.angular.z = req.ang_z; 136 | 137 | door_cmd_vel_pub.publish(cmd_vel); 138 | 139 | return true; 140 | } 141 | 142 | bool target_floor_elev_cb(dynamic_gazebo_models::TargetFloorElev::Request &req, dynamic_gazebo_models::TargetFloorElev::Response &res) 143 | { 144 | if (!activateElevators(req.group_name)) { 145 | return false; 146 | } 147 | 148 | std_msgs::UInt8 elev_door_state; 149 | elev_door_state.data = ELEV_DOOR_STATE_FREE; 150 | 151 | std_msgs::Int32 target_floor; 152 | target_floor.data = req.target_floor; 153 | 154 | elev_door_pub.publish(elev_door_state); 155 | elev_target_pub.publish(target_floor); 156 | 157 | return true; 158 | } 159 | 160 | bool set_elev_props_cb(dynamic_gazebo_models::SetElevProps::Request &req, dynamic_gazebo_models::SetElevProps::Response &res) 161 | { 162 | if (!activateElevators(req.group_name)) { 163 | return false; 164 | } 165 | 166 | std_msgs::Float32MultiArray elev_params; 167 | elev_params.data.push_back(req.velocity); 168 | elev_params.data.push_back(req.force); 169 | 170 | elev_param_pub.publish(elev_params); 171 | 172 | return true; 173 | } 174 | 175 | bool open_close_elev_cb(dynamic_gazebo_models::OpenCloseElevDoors::Request &req, dynamic_gazebo_models::OpenCloseElevDoors::Response &res) 176 | { 177 | if (!activateElevators(req.group_name)) { 178 | return false; 179 | } 180 | 181 | std_msgs::UInt8 elev_door_state; 182 | 183 | if (req.state == STATE_OPEN) { 184 | elev_door_state.data = ELEV_DOOR_STATE_OPEN; 185 | } else { 186 | elev_door_state.data = ELEV_DOOR_STATE_CLOSE; 187 | } 188 | 189 | elev_door_pub.publish(elev_door_state); 190 | 191 | return true; 192 | } 193 | 194 | bool activateDoors(std::string group_name) 195 | { 196 | int groupIndex = getGroupIndex(group_name); 197 | 198 | if (groupIndex == INDEX_NOT_FOUND) { 199 | ROS_ERROR("Door Service Failed: The specified group does not exist"); 200 | return false; 201 | } 202 | 203 | ControlGroup currGroup = groups.at(groupIndex); 204 | 205 | if (currGroup.getType() != DOOR) { 206 | ROS_ERROR("Door Service Failed: This group type doesn't support this call"); 207 | return false; 208 | } 209 | 210 | // Publish the IDs of the active doors in the group 211 | std_msgs::UInt32MultiArray active_doors = uintVectorToStdMsgs(currGroup.getActiveUnits()); 212 | door_active_pub.publish(active_doors); 213 | 214 | return true; 215 | } 216 | 217 | bool activateElevators(std::string group_name) 218 | { 219 | int groupIndex = getGroupIndex(group_name); 220 | 221 | if (groupIndex == INDEX_NOT_FOUND) { 222 | ROS_ERROR("Elevator Service Failed: The specified group does not exist"); 223 | return false; 224 | } 225 | 226 | ControlGroup currGroup = groups.at(groupIndex); 227 | 228 | if (currGroup.getType() != ELEVATOR) { 229 | ROS_ERROR("Elevato Service Failed: This group type doesn't support this call"); 230 | return false; 231 | } 232 | 233 | std_msgs::UInt32MultiArray active_elevs = uintVectorToStdMsgs(currGroup.getActiveUnits()); 234 | elev_active_pub.publish(active_elevs); 235 | 236 | return true; 237 | } 238 | 239 | void setupControlTopics() 240 | { 241 | door_cmd_vel_pub = rosNode.advertise("/door_controller/command", 100); 242 | door_active_pub = rosNode.advertise("/door_controller/active", 1000); 243 | 244 | elev_target_pub = rosNode.advertise("/elevator_controller/target_floor", 100); 245 | elev_active_pub = rosNode.advertise("elevator_controller/active", 1000); 246 | elev_param_pub = rosNode.advertise("elevator_controller/param", 1000); 247 | elev_door_pub = rosNode.advertise("/elevator_controller/door", 100); 248 | } 249 | 250 | std_msgs::UInt32MultiArray uintVectorToStdMsgs(std::vector active_units) 251 | { 252 | std_msgs::UInt32MultiArray active_list; 253 | 254 | for (int i=0; i 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #define CONTROL_GROUP_NAME "keyboard_op_control_group" 37 | 38 | #define DEFAULT_ELEV_SPEED 1.5 39 | #define DEFAULT_ELEV_FORCE 100 40 | 41 | #define DEFAULT_SLIDE_SPEED 1.0 42 | #define DEFAULT_FLIP_SPEED 1.57 43 | 44 | enum ControlType {DOOR, ELEVATOR}; 45 | 46 | class KeyboardOp 47 | { 48 | private: 49 | ros::NodeHandle rosNode; 50 | ros::ServiceClient add_group_client, delete_group_client, list_groups_client; 51 | ros::ServiceClient open_close_doors_client, set_vel_doors_client, target_floor_elev_client, set_elev_props_client, open_close_elev_doors_client; 52 | 53 | ControlType type; 54 | std::string groupName; 55 | bool isGroupInitialized; 56 | 57 | dynamic_gazebo_models::OpenCloseDoors openDoorsCall; 58 | dynamic_gazebo_models::OpenCloseDoors closeDoorsCall; 59 | dynamic_gazebo_models::SetVelDoors setVelDoorsCall; 60 | dynamic_gazebo_models::TargetFloorElev targetFloorCall; 61 | dynamic_gazebo_models::OpenCloseElevDoors openElevDoorsCall; 62 | dynamic_gazebo_models::OpenCloseElevDoors closeElevDoorsCall; 63 | dynamic_gazebo_models::SetElevProps setElevPropsCall; 64 | 65 | public: 66 | KeyboardOp(ros::NodeHandle &nh) 67 | { 68 | rosNode = nh; 69 | rosNode = ros::NodeHandle(""); 70 | 71 | setupClientServices(); 72 | initVars(); 73 | } 74 | 75 | void initVars() 76 | { 77 | type = DOOR; 78 | } 79 | 80 | void setupClientServices() 81 | { 82 | add_group_client = rosNode.serviceClient("model_dynamics_manager/add_control_group"); 83 | delete_group_client = rosNode.serviceClient("model_dynamics_manager/delete_control_group"); 84 | list_groups_client = rosNode.serviceClient("model_dynamics_manager/list_groups"); 85 | 86 | open_close_doors_client = rosNode.serviceClient("model_dynamics_manager/doors/open_close"); 87 | set_vel_doors_client = rosNode.serviceClient("model_dynamics_manager/doors/set_vel"); 88 | 89 | target_floor_elev_client = rosNode.serviceClient("model_dynamics_manager/elevators/target_floor"); 90 | set_elev_props_client = rosNode.serviceClient("model_dynamics_manager/elevators/set_props"); 91 | open_close_elev_doors_client = rosNode.serviceClient("model_dynamics_manager/elevators/open_close_elev"); 92 | } 93 | 94 | bool setControlType(char input[]) 95 | { 96 | std::string inputStr(input); 97 | 98 | if (boost::iequals(inputStr, "door")) { 99 | std::cout << "Control type set to 'door'" << std::endl; 100 | type = DOOR; 101 | setActiveUnits(); 102 | return true; 103 | } else if (boost::iequals(inputStr, "elevator")){ 104 | std::cout << "Control type set to 'elevator'" << std::endl; 105 | type = ELEVATOR; 106 | setActiveUnits(); 107 | return true; 108 | } 109 | 110 | return false; 111 | } 112 | 113 | void setActiveUnits() 114 | { 115 | char input[30]; 116 | std::cout << "Enter the reference numbers of the units you want to control. Eg: 1, 3, 4 for units one, three & four" << std::endl; 117 | 118 | readLineInput(input); 119 | std::vector activeList = parseActiveList(input); 120 | 121 | // Delete previous group if already initialized. Note: IGNORE the warning produced during initialization about delete service failing 122 | dynamic_gazebo_models::DeleteGroup deleteSrv; 123 | deleteSrv.request.group_name = CONTROL_GROUP_NAME; 124 | delete_group_client.call(deleteSrv); 125 | 126 | // Add new group with the desired units 127 | dynamic_gazebo_models::AddGroup addSrv; 128 | addSrv.request.group.group_name = CONTROL_GROUP_NAME; 129 | addSrv.request.group.type = type == DOOR ? "door" : "elevator"; 130 | addSrv.request.group.active_units = activeList; 131 | add_group_client.call(addSrv); 132 | 133 | type == DOOR ? printDoorControls() : printElevatorControls(); 134 | 135 | isGroupInitialized = true; 136 | } 137 | 138 | void printDoorControls() 139 | { 140 | std::cout << "\n-----------------\nDoor Controls:\nPress 'Enter' after each input.\n'q' to quit.\n'o' to open doors\n'c' to close doors"; 141 | std::cout << "\n'l' and 'value' to specify the linear velocity of a sliding door in m/s (eg 'l 10' or 'l-3.1')\n'a' and 'value' to specify angular velocity of a revolving door in radians (eg: 'a -1.57' or 'a3.14')"; 142 | std::cout << "\n'0' to stop movement\n-----------------\n" << std::endl; 143 | } 144 | 145 | void printElevatorControls() 146 | { 147 | std::cout << "\n-----------------\nElevator Controls:"; 148 | std::cout << "\nPress 'Enter' after each input.\n'q' to quit.\nEg: '4' goes to the fourth floor"; 149 | std::cout << "\n's##' to set the lift speed (eg: s4.2 for 4.2 m/s). Default: 1.5m/s\n'f##' to set the lift force (eg: f150 for 150N). Default: 150N\n'o' to force open the doors on the current floor\n'c' to force close the doors on the current floor\nDefault floor: 'F0'\n-----------------\n" << std::endl; 150 | } 151 | 152 | std::vector parseActiveList(char input[]) 153 | { 154 | std::string active_list_str(input); 155 | std::vector active_list; 156 | 157 | // parse csv-style input (also remove whitespace): 158 | std::string::iterator end_pos = std::remove(active_list_str.begin(), active_list_str.end(), ' '); 159 | active_list_str.erase(end_pos, active_list_str.end()); 160 | 161 | std::istringstream ss(active_list_str); 162 | std::string token; 163 | 164 | while (std::getline(ss, token, ',')) { 165 | try { 166 | active_list.push_back(atoi(token.c_str())); 167 | } catch (...) { 168 | std::cout << "Invalid active list. Exiting.." << std::endl; 169 | std::exit(EXIT_SUCCESS); 170 | } 171 | } 172 | 173 | return active_list; 174 | } 175 | 176 | void initialize() 177 | { 178 | char input[30]; 179 | isGroupInitialized = false; 180 | 181 | std::cout << "Enter the |type| of models you want to control: 'door' or 'elevator'" << std::endl; 182 | 183 | readLineInput(input); 184 | while (!setControlType(input)) { 185 | std::cout << "Invalid type. Options: 'door' or 'elevator'" << std::endl; 186 | readLineInput(input); 187 | } 188 | 189 | setupCallTemplates(); 190 | } 191 | 192 | void setupCallTemplates() 193 | { 194 | // DOOR based services: 195 | openDoorsCall.request.group_name = CONTROL_GROUP_NAME; 196 | openDoorsCall.request.state = true; 197 | 198 | closeDoorsCall.request.group_name = CONTROL_GROUP_NAME; 199 | closeDoorsCall.request.state = false; 200 | 201 | setVelDoorsCall.request.group_name = CONTROL_GROUP_NAME; 202 | setVelDoorsCall.request.lin_x = DEFAULT_SLIDE_SPEED; 203 | setVelDoorsCall.request.lin_y = DEFAULT_SLIDE_SPEED; 204 | setVelDoorsCall.request.ang_z = DEFAULT_FLIP_SPEED; 205 | 206 | // ELEVATOR based services: 207 | targetFloorCall.request.group_name = CONTROL_GROUP_NAME; 208 | targetFloorCall.request.target_floor = 0; 209 | 210 | openElevDoorsCall.request.group_name = CONTROL_GROUP_NAME; 211 | openElevDoorsCall.request.state = true; 212 | 213 | closeElevDoorsCall.request.group_name = CONTROL_GROUP_NAME; 214 | closeElevDoorsCall.request.state = false; 215 | 216 | setElevPropsCall.request.group_name = CONTROL_GROUP_NAME; 217 | setElevPropsCall.request.velocity = DEFAULT_ELEV_SPEED; 218 | setElevPropsCall.request.force = DEFAULT_ELEV_FORCE; 219 | } 220 | 221 | void readLineInput(char input[30]) 222 | { 223 | std::cin.getline(input, 30); 224 | 225 | std::string inputStr(input); 226 | if (boost::iequals(inputStr, "q")) { 227 | rosNode.shutdown(); 228 | std::exit(EXIT_SUCCESS); 229 | } 230 | } 231 | 232 | void start() 233 | { 234 | initialize(); 235 | char input[30]; 236 | 237 | while (rosNode.ok()) 238 | { 239 | readLineInput(input); 240 | 241 | // check if the type was toggled (between 'door' & 'elevator') 242 | if (setControlType(input)) { 243 | continue; 244 | } 245 | 246 | callServices(input); 247 | } 248 | 249 | } 250 | 251 | void callServices(char input[]) 252 | { 253 | ROS_ASSERT(type == DOOR || type == ELEVATOR); 254 | 255 | if (type == DOOR) { 256 | executeDoorServices(input); 257 | } else if (type == ELEVATOR) { 258 | executeElevatorServices(input); 259 | } 260 | } 261 | 262 | void executeElevatorServices(char input[]) 263 | { 264 | std::string inputStr(input); 265 | 266 | switch(input[0]) { 267 | case 'o': 268 | open_close_elev_doors_client.call(openElevDoorsCall); 269 | break; 270 | case 'c': 271 | open_close_elev_doors_client.call(closeElevDoorsCall); 272 | break; 273 | case 's': 274 | setElevPropsCall.request.velocity = parseFloat(inputStr.substr(1)); 275 | set_elev_props_client.call(setElevPropsCall); 276 | break; 277 | case 'f': 278 | setElevPropsCall.request.force = parseFloat(inputStr.substr(1)); 279 | set_elev_props_client.call(setElevPropsCall); 280 | break; 281 | default: 282 | try { 283 | targetFloorCall.request.target_floor = std::stoi(inputStr); 284 | target_floor_elev_client.call(targetFloorCall); 285 | } catch(std::exception const & e) { 286 | std::cout << "Unknown command" << std::endl; 287 | } 288 | }; 289 | } 290 | 291 | void executeDoorServices(char input[]) 292 | { 293 | std::string inputStr(input); 294 | 295 | switch(input[0]) { 296 | 297 | case 'o': 298 | open_close_doors_client.call(openDoorsCall); 299 | break; 300 | case 'c': 301 | open_close_doors_client.call(closeDoorsCall); 302 | break; 303 | case 'l': 304 | setVelDoorsCall.request.lin_x = setVelDoorsCall.request.lin_y = parseFloat(inputStr.substr(1)); 305 | set_vel_doors_client.call(setVelDoorsCall); 306 | break; 307 | case 'a': 308 | setVelDoorsCall.request.ang_z = parseFloat(inputStr.substr(1)); 309 | set_vel_doors_client.call(setVelDoorsCall); 310 | break; 311 | default: 312 | std::cout << "Unknown command" << std::endl; 313 | }; 314 | } 315 | 316 | float parseFloat(std::string input) 317 | { 318 | std::string::iterator end_pos = std::remove(input.begin(), input.end(), ' '); 319 | input.erase(end_pos, input.end()); 320 | 321 | return atof(input.c_str()); 322 | } 323 | }; 324 | 325 | 326 | int main(int argc, char** argv) 327 | { 328 | 329 | ros::init(argc, argv, "keyboard_op_model_dynamics_control"); 330 | ros::NodeHandle nh; 331 | 332 | KeyboardOp controller(nh); 333 | controller.start(); 334 | } -------------------------------------------------------------------------------- /src/plugins/auto_elev_door_plugin.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014 Mohit Shridhar, David Lee 2 | 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #define DEFAULT_SLIDE_DISTANCE 0.711305 35 | #define DEFAULT_SLIDE_SPEED 1 // in m/s 36 | 37 | #define HEIGHT_LEVEL_TOLERANCE 1.5 38 | 39 | #define ELEV_DOOR_STATE_OPEN 1 40 | #define ELEV_DOOR_STATE_CLOSE 0 41 | #define ELEV_DOOR_STATE_FREE 2 42 | 43 | /* 44 | 45 | Limitations: 46 | The door must be facing either the x axis or the y axis; not skewed in any sense 47 | 48 | */ 49 | 50 | enum DoorDirection {LEFT, RIGHT}; 51 | 52 | namespace gazebo 53 | { 54 | class AutoElevDoorPlugin : public ModelPlugin 55 | { 56 | private: 57 | ros::NodeHandle *rosNode; 58 | event::ConnectionPtr updateConnection; 59 | ros::Subscriber target_floor_sub, est_floor_sub, open_close_sub, active_elevs_sub; 60 | 61 | physics::ModelPtr model, elevatorModel; 62 | physics::LinkPtr doorLink; 63 | 64 | std::string model_domain_space, elevator_ref_name, elevator_domain_space; 65 | int elevator_ref_num, targetFloor, estCurrFloor; 66 | DoorDirection direction; 67 | uint doorState; 68 | 69 | float openVel, closeVel, slide_speed; 70 | float max_trans_dist, maxPosX, maxPosY, minPosX, minPosY; 71 | bool isActive; 72 | 73 | public: 74 | 75 | AutoElevDoorPlugin() 76 | { 77 | std::string name = "auto_elevator_door_plugin"; 78 | int argc = 0; 79 | ros::init(argc, NULL, name); 80 | } 81 | 82 | ~AutoElevDoorPlugin() 83 | { 84 | delete rosNode; 85 | } 86 | 87 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 88 | { 89 | determineDomainSpace(_sdf); 90 | determineCorresElev(_sdf); 91 | determineDoorDirection(_sdf); 92 | determineConstraints(_sdf); 93 | establishLinks(_parent); 94 | initVars(); 95 | } 96 | 97 | private: 98 | void OnUpdate() 99 | { 100 | ros::spinOnce(); 101 | activateDoors(); 102 | checkSlideConstraints(); 103 | } 104 | 105 | void determineDomainSpace(sdf::ElementPtr _sdf) 106 | { 107 | if (!_sdf->HasElement("model_domain_space")) { 108 | ROS_WARN("Model Domain Space not specified in the plugin reference. Defaulting to 'auto_door_'"); 109 | model_domain_space = "auto_door_"; 110 | } else { 111 | model_domain_space = _sdf->GetElement("model_domain_space")->Get(); 112 | } 113 | 114 | rosNode = new ros::NodeHandle(""); 115 | 116 | if (!rosNode->hasParam("/model_dynamics_manager/elevator_domain_space")) { 117 | ROS_ERROR("The parameter 'elevator_domain_space' does not exist. Check that the elevator plugin sets this param"); 118 | std::exit(EXIT_FAILURE); 119 | } else { 120 | rosNode->getParam("/model_dynamics_manager/elevator_domain_space", elevator_domain_space); 121 | } 122 | } 123 | 124 | void determineCorresElev(sdf::ElementPtr _sdf) 125 | { 126 | if (!_sdf->HasElement("elevator_name")) { 127 | ROS_ERROR("Elevator name not specified in the plugin reference. An auto door can exist only if there is a corresponding elevator."); 128 | std::exit(EXIT_FAILURE); 129 | } else { 130 | elevator_ref_name = _sdf->GetElement("elevator_name")->Get(); 131 | } 132 | } 133 | 134 | void determineDoorDirection(sdf::ElementPtr _sdf) 135 | { 136 | if (!_sdf->HasElement("door_direction")) { 137 | ROS_WARN("Door direction not specified in the plugin reference. Defaulting to 'left'"); 138 | direction = LEFT; 139 | } else { 140 | std::string direction_str = _sdf->GetElement("door_direction")->Get(); 141 | direction = direction_str.compare("right") == 0 ? RIGHT : LEFT; 142 | } 143 | } 144 | 145 | void determineConstraints(sdf::ElementPtr _sdf) 146 | { 147 | if (!_sdf->HasElement("max_trans_dist")) { 148 | ROS_WARN("Maximum translation distance not specified in the plugin reference. Defaulting to '0.711305'"); 149 | max_trans_dist = DEFAULT_SLIDE_DISTANCE; 150 | } else { 151 | max_trans_dist = _sdf->GetElement("max_trans_dist")->Get(); 152 | } 153 | 154 | if (!_sdf->HasElement("speed")) { 155 | ROS_WARN("Sliding speed not specified in the plugin reference. Defaulting to '1.0 m/s'"); 156 | slide_speed = DEFAULT_SLIDE_SPEED; 157 | } else { 158 | slide_speed = _sdf->GetElement("speed")->Get(); 159 | } 160 | } 161 | 162 | void establishLinks(physics::ModelPtr _parent) 163 | { 164 | model = _parent; 165 | doorLink = model->GetLink("door"); 166 | 167 | target_floor_sub = rosNode->subscribe("/elevator_controller/target_floor", 50, &AutoElevDoorPlugin::target_floor_cb, this); 168 | est_floor_sub = rosNode->subscribe("/elevator_controller/" + elevator_ref_name + "/estimated_current_floor", 50, &AutoElevDoorPlugin::est_floor_cb, this); 169 | open_close_sub = rosNode->subscribe("/elevator_controller/door", 50, &AutoElevDoorPlugin::open_close_cb, this); 170 | active_elevs_sub = rosNode->subscribe("/elevator_controller/active", 50, &AutoElevDoorPlugin::active_elevs_cb, this); 171 | 172 | updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&AutoElevDoorPlugin::OnUpdate, this)); 173 | } 174 | 175 | void initVars() 176 | { 177 | // parse elevator reference number: 178 | std::string elevator_ref_num_str = elevator_ref_name; 179 | replaceSubstring(elevator_ref_num_str, elevator_domain_space, ""); 180 | elevator_ref_num = atoi(elevator_ref_num_str.c_str()); 181 | 182 | ROS_ASSERT(direction == LEFT || direction == RIGHT); 183 | 184 | // compute open-close velocities 185 | openVel = direction == RIGHT ? -slide_speed : slide_speed; 186 | closeVel = direction == RIGHT ? slide_speed : -slide_speed; 187 | 188 | // compute slide constraints 189 | float spawnPosX = model->GetWorldPose().pos.x; 190 | minPosX = direction == RIGHT ? spawnPosX - max_trans_dist : spawnPosX; 191 | maxPosX = direction == RIGHT ? spawnPosX : spawnPosX + max_trans_dist; 192 | 193 | float spawnPosY = model->GetWorldPose().pos.y; 194 | minPosY = direction == RIGHT ? spawnPosY - max_trans_dist : spawnPosY; 195 | maxPosY = direction == RIGHT ? spawnPosY : spawnPosY + max_trans_dist; 196 | 197 | elevatorModel = model->GetWorld()->GetModel(elevator_ref_name); 198 | } 199 | 200 | void activateDoors() 201 | { 202 | if (!isActive) { 203 | return; 204 | } 205 | 206 | float currElevHeight = elevatorModel->GetWorldPose().pos.z; 207 | float currDoorHeight = model->GetWorldPose().pos.z; 208 | float doorElevHeightDiff = fabs(currElevHeight - currDoorHeight); 209 | 210 | // Primary condition: the elevator is behind the doors 211 | if (doorElevHeightDiff > HEIGHT_LEVEL_TOLERANCE || estCurrFloor != targetFloor) { 212 | setDoorSlideVel(closeVel); 213 | return; 214 | } 215 | 216 | // Secondary condition: check if the door has to be forced open/closed [OVERIDE auto open-close] 217 | if (doorState == ELEV_DOOR_STATE_OPEN) { 218 | setDoorSlideVel(openVel); 219 | return; 220 | } else if (doorState == ELEV_DOOR_STATE_CLOSE) { 221 | setDoorSlideVel(closeVel); 222 | return; 223 | } 224 | 225 | // Else: open/close doors based on target floor reference 226 | setDoorSlideVel(openVel); 227 | } 228 | 229 | void setDoorSlideVel(float vel) 230 | { 231 | doorLink->SetLinearVel(math::Vector3(vel, vel, 0)); // we set the vel for both x & y directions since we don't know which direction the door is facing 232 | } 233 | 234 | void checkSlideConstraints() 235 | { 236 | float currDoorPosX = model->GetWorldPose().pos.x; 237 | float currDoorPosY = model->GetWorldPose().pos.y; 238 | 239 | math::Pose constrainedPose; 240 | 241 | if (currDoorPosX > maxPosX) { 242 | constrainedPose.pos.x = maxPosX; 243 | } else if (currDoorPosX < minPosX) { 244 | constrainedPose.pos.x = minPosX; 245 | } else { 246 | constrainedPose.pos.x = currDoorPosX; 247 | } 248 | 249 | if (currDoorPosY > maxPosY) { 250 | constrainedPose.pos.y = maxPosY; 251 | } else if (currDoorPosY < minPosY) { 252 | constrainedPose.pos.y = minPosY; 253 | } else { 254 | constrainedPose.pos.y = currDoorPosY; 255 | } 256 | 257 | constrainedPose.pos.z = model->GetWorldPose().pos.z; 258 | constrainedPose.rot.x = model->GetWorldPose().rot.x; 259 | constrainedPose.rot.y = model->GetWorldPose().rot.y; 260 | constrainedPose.rot.z = model->GetWorldPose().rot.z; 261 | 262 | model->SetWorldPose(constrainedPose); 263 | } 264 | 265 | void target_floor_cb(const std_msgs::Int32::ConstPtr& msg) 266 | { 267 | targetFloor = msg->data; 268 | } 269 | 270 | void est_floor_cb(const std_msgs::Int32::ConstPtr& msg) 271 | { 272 | estCurrFloor = msg->data; 273 | } 274 | 275 | void open_close_cb(const std_msgs::UInt8::ConstPtr& msg) 276 | { 277 | doorState = msg->data; 278 | } 279 | 280 | void active_elevs_cb(const std_msgs::UInt32MultiArray::ConstPtr& array) 281 | { 282 | isActive = false; 283 | 284 | for (std::vector::const_iterator it = array->data.begin(); it != array->data.end(); ++it) { 285 | if (*it == elevator_ref_num) { 286 | isActive = true; 287 | } 288 | } 289 | } 290 | 291 | std::string replaceSubstring(std::string &s, std::string toReplace, std::string replaceWith) 292 | { 293 | return(s.replace(s.find(toReplace), toReplace.length(), replaceWith)); 294 | } 295 | }; 296 | 297 | GZ_REGISTER_MODEL_PLUGIN(AutoElevDoorPlugin); 298 | } -------------------------------------------------------------------------------- /src/plugins/door_plugin.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014 Mohit Shridhar, David Lee 2 | 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #define DEFAULT_OPEN_VEL -1.57 43 | #define DEFAULT_CLOSE_VEL 1.57 44 | #define DEFAULT_SLIDE_DISTANCE 0.711305 45 | 46 | #define TYPE_FLIP_OPEN "flip" 47 | #define TYPE_SLIDE_OPEN "slide" 48 | 49 | #define DIRECTION_FLIP_CLOCKWISE "clockwise" 50 | #define DIRECTION_FLIP_COUNTER_CLOCKWISE "counter_clockwise" 51 | #define DIRECTION_SLIDE_LEFT "left" 52 | #define DIRECTION_SLIDE_RIGHT "right" 53 | 54 | #define CONTEXT_SPACE_X_RANGE 2.0 // in m 55 | #define CONTEXT_SPACE_Y_RANGE 2.0 56 | #define CONTEXT_SPACE_Z_RANGE 2.0 57 | 58 | 59 | namespace gazebo 60 | { 61 | enum DoorType {FLIP, SLIDE}; 62 | 63 | class DoorPlugin : public ModelPlugin 64 | { 65 | 66 | private: 67 | physics::ModelPtr model; 68 | physics::LinkPtr doorLink; 69 | math::Pose currPose, currFpvPose; 70 | 71 | math::Vector3 cmd_vel; 72 | 73 | bool isActive; 74 | int activeDoors[100]; 75 | DoorType type; 76 | 77 | int door_ref_num; 78 | std::string door_type, door_model_name, door_direction, model_domain_space; 79 | float max_trans_dist, maxPosX, maxPosY, minPosX, minPosY; 80 | 81 | ros::NodeHandle* rosNode; 82 | transport::NodePtr gazeboNode; 83 | event::ConnectionPtr updateConnection; 84 | 85 | transport::SubscriberPtr subFpvPose, subGzRequest; 86 | ros::Subscriber sub, sub_active; 87 | 88 | public: 89 | DoorPlugin() 90 | { 91 | std::string name = "door_plugin_node"; 92 | int argc = 0; 93 | ros::init(argc, NULL, name); 94 | 95 | } 96 | ~DoorPlugin() 97 | { 98 | delete rosNode; 99 | } 100 | 101 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 102 | { 103 | establishLinks(_parent); 104 | determineDoorType(_sdf); 105 | determineDoorDirection(_sdf); 106 | determineModelDomain(_sdf); 107 | determineConstraints(_sdf); 108 | initVars(); 109 | } 110 | 111 | void OnUpdate() 112 | { 113 | ros::spinOnce(); 114 | updateLinkVel(); 115 | applyConstraints(); 116 | } 117 | 118 | private: 119 | void determineDoorType(sdf::ElementPtr _sdf) 120 | { 121 | if (!_sdf->HasElement("door_type")) { 122 | ROS_WARN("Door Type not specified. Defaulting to 'flip'"); 123 | door_type = "flip"; 124 | } else { 125 | door_type = _sdf->GetElement("door_type")->Get(); 126 | } 127 | 128 | if (door_type.compare(TYPE_SLIDE_OPEN) == 0) { 129 | type = SLIDE; 130 | } else { 131 | type = FLIP; 132 | } 133 | } 134 | 135 | void determineDoorDirection(sdf::ElementPtr _sdf) 136 | { 137 | if (!_sdf->HasElement("door_direction")) { 138 | if (type == FLIP) { 139 | ROS_WARN("Door direction not specified in the plugin reference. Defaulting to 'clockwise'"); 140 | door_direction = DIRECTION_FLIP_CLOCKWISE; 141 | } else if (type == SLIDE) { 142 | ROS_WARN("Door direction not specified in the plugin reference. Defaulting to 'left'"); 143 | door_direction = DIRECTION_SLIDE_LEFT; 144 | } 145 | } else { 146 | door_direction = _sdf->GetElement("door_direction")->Get(); 147 | checkDirectionValidity(); 148 | } 149 | } 150 | 151 | void determineConstraints(sdf::ElementPtr _sdf) 152 | { 153 | if (type == SLIDE) { 154 | if (!_sdf->HasElement("max_trans_dist")) { 155 | ROS_WARN("Max Translation Distance for sliding door not specified in the plugin reference. Defaulting to '0.711305' m"); 156 | max_trans_dist = DEFAULT_SLIDE_DISTANCE; 157 | } else { 158 | max_trans_dist = _sdf->GetElement("max_trans_dist")->Get(); 159 | } 160 | } 161 | } 162 | 163 | void checkDirectionValidity() 164 | { 165 | if (type == FLIP) { 166 | if (door_direction.compare(DIRECTION_FLIP_CLOCKWISE) != 0 && door_direction.compare(DIRECTION_FLIP_COUNTER_CLOCKWISE) != 0) { 167 | ROS_WARN("Invalid door direction specified. Only two states possible: 'clockwise' OR 'counter_clockwise'. Defaulting to 'clockwise'"); 168 | door_direction = DIRECTION_FLIP_CLOCKWISE; 169 | } 170 | } else if (type == SLIDE) { 171 | if (door_direction.compare(DIRECTION_SLIDE_LEFT) != 0 && door_direction.compare(DIRECTION_SLIDE_RIGHT) != 0) { 172 | ROS_WARN("Invalid door direction specified. Only two states possible: 'left' OR 'right'. Defaulting to 'left'"); 173 | door_direction = DIRECTION_SLIDE_LEFT; 174 | } 175 | } 176 | } 177 | 178 | void determineModelDomain(sdf::ElementPtr _sdf) 179 | { 180 | if (!_sdf->HasElement("model_domain_space")) { 181 | ROS_WARN("Model Domain Space not specified in the plugin reference. Defaulting to 'door_'"); 182 | model_domain_space = "door_"; 183 | } else { 184 | model_domain_space = _sdf->GetElement("model_domain_space")->Get(); 185 | } 186 | 187 | ROS_INFO("Door '%s' initialized - Type: %s, Direction: %s, Domain Space: %s\n", door_type.c_str(), door_model_name.c_str(), door_direction.c_str(), model_domain_space.c_str()); 188 | } 189 | 190 | void initVars() 191 | { 192 | isActive = false; 193 | 194 | // find the elevator reference number 195 | std::string door_ref_num_str = door_model_name; 196 | replaceSubstring(door_ref_num_str, model_domain_space, ""); 197 | door_ref_num = atoi(door_ref_num_str.c_str()); 198 | 199 | if (type == SLIDE) { 200 | // compute slide constraints 201 | float spawnPosX = model->GetWorldPose().pos.x; 202 | minPosX = door_direction.compare(DIRECTION_SLIDE_RIGHT) == 0 ? spawnPosX - max_trans_dist : spawnPosX; 203 | maxPosX = door_direction.compare(DIRECTION_SLIDE_RIGHT) == 0 ? spawnPosX : spawnPosX + max_trans_dist; 204 | 205 | float spawnPosY = model->GetWorldPose().pos.y; 206 | minPosY = door_direction.compare(DIRECTION_SLIDE_RIGHT) == 0 ? spawnPosY - max_trans_dist : spawnPosY; 207 | maxPosY = door_direction.compare(DIRECTION_SLIDE_RIGHT) == 0 ? spawnPosY : spawnPosY + max_trans_dist; 208 | } 209 | } 210 | 211 | void establishLinks(physics::ModelPtr _parent) 212 | { 213 | model = _parent; 214 | doorLink = model->GetLink("door"); 215 | door_model_name = model->GetName(); 216 | 217 | rosNode = new ros::NodeHandle(""); 218 | 219 | sub = rosNode->subscribe("/door_controller/command", 1000, &DoorPlugin::cmd_ang_cb, this ); 220 | sub_active = rosNode->subscribe("/door_controller/active", 1000, &DoorPlugin::active_doors_cb, this); 221 | 222 | updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&DoorPlugin::OnUpdate, this)); 223 | } 224 | 225 | void cmd_ang_cb(const geometry_msgs::Twist::ConstPtr& msg) 226 | { 227 | if (isActive) { 228 | if (type == FLIP) { 229 | setAngularVel(msg->angular.z); 230 | ROS_INFO("Door '%s' - Angular z: [%f]", door_model_name.c_str(), msg->angular.z); 231 | } else if (type == SLIDE) { 232 | setLinearVel(msg->linear.x, msg->linear.y); 233 | ROS_INFO("Door '%s' - Linear x: [%f], y: [%f]", door_model_name.c_str(), msg->linear.x, msg->linear.y); 234 | } 235 | } 236 | } 237 | 238 | void updateLinkVel() 239 | { 240 | if (type == FLIP) { 241 | doorLink->SetAngularVel(cmd_vel); 242 | } else if (type == SLIDE) { 243 | doorLink->SetLinearVel(cmd_vel); 244 | } 245 | } 246 | 247 | void applyConstraints() 248 | { 249 | if (type == SLIDE) { 250 | float currDoorPosX = model->GetWorldPose().pos.x; 251 | float currDoorPosY = model->GetWorldPose().pos.y; 252 | 253 | math::Pose constrainedPose; 254 | 255 | if (currDoorPosX > maxPosX) { 256 | constrainedPose.pos.x = maxPosX; 257 | } else if (currDoorPosX < minPosX) { 258 | constrainedPose.pos.x = minPosX; 259 | } else { 260 | constrainedPose.pos.x = currDoorPosX; 261 | } 262 | 263 | if (currDoorPosY > maxPosY) { 264 | constrainedPose.pos.y = maxPosY; 265 | } else if (currDoorPosY < minPosY) { 266 | constrainedPose.pos.y = minPosY; 267 | } else { 268 | constrainedPose.pos.y = currDoorPosY; 269 | } 270 | 271 | constrainedPose.pos.z = model->GetWorldPose().pos.z; 272 | constrainedPose.rot.x = model->GetWorldPose().rot.x; 273 | constrainedPose.rot.y = model->GetWorldPose().rot.y; 274 | constrainedPose.rot.z = model->GetWorldPose().rot.z; 275 | 276 | model->SetWorldPose(constrainedPose); 277 | } 278 | } 279 | 280 | void setAngularVel(float rot_z) 281 | { 282 | cmd_vel = math::Vector3(); 283 | 284 | if (door_direction.compare(DIRECTION_FLIP_CLOCKWISE) == 0) { 285 | cmd_vel.z = rot_z; 286 | } else { 287 | cmd_vel.z = -rot_z; 288 | } 289 | } 290 | 291 | void setLinearVel(float lin_x, float lin_y) 292 | { 293 | cmd_vel = math::Vector3(); 294 | 295 | if (door_direction.compare(DIRECTION_SLIDE_LEFT) == 0) { 296 | cmd_vel.x = -lin_x; 297 | cmd_vel.y = -lin_y; 298 | } else { 299 | cmd_vel.x = lin_x; 300 | cmd_vel.y = lin_y; 301 | } 302 | } 303 | 304 | void active_doors_cb(const std_msgs::UInt32MultiArray::ConstPtr& array) 305 | { 306 | isActive = false; 307 | 308 | int i=0; 309 | 310 | for (std::vector::const_iterator it = array->data.begin(); it != array->data.end(); ++it) { 311 | activeDoors[i] = *it; 312 | i++; 313 | 314 | if (*it == door_ref_num) { 315 | isActive = true; 316 | } 317 | } 318 | } 319 | 320 | std::string replaceSubstring(std::string &s, std::string toReplace, std::string replaceWith) 321 | { 322 | return(s.replace(s.find(toReplace), toReplace.length(), replaceWith)); 323 | } 324 | 325 | // Deprecated function: 326 | std::vector parseTopicStr(std::string bot_pose_topics_str) 327 | { 328 | std::vector bot_pose_topic_list; 329 | 330 | // parse csv-style input (also remove whitespace): 331 | std::string::iterator end_pos = std::remove(bot_pose_topics_str.begin(), bot_pose_topics_str.end(), ' '); 332 | bot_pose_topics_str.erase(end_pos, bot_pose_topics_str.end()); 333 | 334 | std::istringstream ss(bot_pose_topics_str); 335 | std::string token; 336 | 337 | while (std::getline(ss, token, ',')) { 338 | bot_pose_topic_list.push_back(token.c_str()); 339 | } 340 | 341 | return bot_pose_topic_list; 342 | } 343 | 344 | }; 345 | 346 | GZ_REGISTER_MODEL_PLUGIN(DoorPlugin) 347 | } -------------------------------------------------------------------------------- /src/plugins/elevator_plugin.cc: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2014 Mohit Shridhar, David Lee 2 | 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | 10 | // The above copyright notice and this permission notice shall be included in all 11 | // copies or substantial portions of the Software. 12 | 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 16 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 19 | // SOFTWARE. 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #define DEFAULT_LIFT_SPEED 1.5 39 | #define DEFAULT_LIFT_FORCE 100 40 | 41 | #define UNKNOWN_FLOOR -100 42 | #define HEIGHT_LEVEL_TOLERANCE 0.01 43 | 44 | namespace gazebo 45 | { 46 | class ElevatorPlugin : public ModelPlugin 47 | { 48 | 49 | private: 50 | 51 | ros::NodeHandle *rosNode; 52 | event::ConnectionPtr updateConnection; 53 | 54 | physics::ModelPtr model; 55 | physics::LinkPtr bodyLink; 56 | std::string modelName; 57 | 58 | ros::Subscriber target_floor_sub, active_elevs_sub, set_param_sub; 59 | ros::Publisher estimated_floor_pub; 60 | 61 | std::string model_domain_space, floor_heights_str; 62 | uint numFloors; 63 | 64 | std::map floorHeightMap; 65 | std::map floorIndexMap; 66 | 67 | bool isActive; 68 | int targetFloor, elev_ref_num; 69 | float elevSpeed, elevForce, spawnPosX, spawnPosY; 70 | 71 | public: 72 | 73 | ElevatorPlugin() 74 | { 75 | std::string name = "elevator_plugin"; 76 | int argc = 0; 77 | ros::init(argc, NULL, name); 78 | } 79 | 80 | ~ElevatorPlugin() 81 | { 82 | delete rosNode; 83 | } 84 | 85 | void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 86 | { 87 | establishLinks(_parent); 88 | detemineModelDomain(_sdf); 89 | loadFloorHeights(_sdf); 90 | loadSpeedForce(_sdf); 91 | initVars(); 92 | } 93 | 94 | private: 95 | 96 | void OnUpdate() 97 | { 98 | ros::spinOnce(); 99 | directElevator(); 100 | constrainHorizontalMovement(); 101 | publishEstimatedPos(); 102 | } 103 | 104 | void detemineModelDomain(sdf::ElementPtr _sdf) 105 | { 106 | if (!_sdf->HasElement("model_domain_space")) { 107 | ROS_WARN("Model Domain Space not specified in the plugin reference. Defaulting to 'elevator_'"); 108 | model_domain_space = "elevator_"; 109 | } else { 110 | model_domain_space = _sdf->GetElement("model_domain_space")->Get(); 111 | } 112 | 113 | rosNode->setParam("/model_dynamics_manager/elevator_domain_space", model_domain_space); 114 | } 115 | 116 | void loadFloorHeights(sdf::ElementPtr _sdf) 117 | { 118 | if (!_sdf->HasElement("floor_heights")) { 119 | ROS_ERROR("Floor heights not specified in the plugin reference. The elevator model cannot function without known floor heights"); 120 | std::exit(EXIT_FAILURE); 121 | } else { 122 | floor_heights_str = _sdf->GetElement("floor_heights")->Get(); 123 | } 124 | 125 | parseFloorHeights(floor_heights_str); 126 | 127 | } 128 | 129 | void loadSpeedForce(sdf::ElementPtr _sdf) 130 | { 131 | if (!_sdf->HasElement("speed")) { 132 | ROS_WARN("Elevator Speed not specified in the plugin reference. Defaulting to 1.5 m/s"); 133 | elevSpeed = DEFAULT_LIFT_SPEED; 134 | } else { 135 | elevSpeed = _sdf->GetElement("speed")->Get(); 136 | } 137 | 138 | if (!_sdf->HasElement("force")) { 139 | ROS_WARN("Elevator Speed not specified in the plugin reference. Defaulting to 100 N"); 140 | elevForce = DEFAULT_LIFT_FORCE; 141 | } else { 142 | elevForce = _sdf->GetElement("force")->Get(); 143 | } 144 | } 145 | 146 | void establishLinks(physics::ModelPtr _parent) 147 | { 148 | model = _parent; 149 | bodyLink = model->GetLink("body"); 150 | modelName = model->GetName(); 151 | 152 | rosNode = new ros::NodeHandle(""); 153 | 154 | target_floor_sub = rosNode->subscribe("/elevator_controller/target_floor", 100, &ElevatorPlugin::target_floor_cb, this); 155 | active_elevs_sub = rosNode->subscribe("/elevator_controller/active", 100, &ElevatorPlugin::active_elevs_cb, this); 156 | set_param_sub = rosNode->subscribe("/elevator_controller/param", 100, &ElevatorPlugin::set_param_cb, this); 157 | estimated_floor_pub = rosNode->advertise("/elevator_controller/" + modelName + "/estimated_current_floor", 100); 158 | 159 | updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&ElevatorPlugin::OnUpdate, this)); 160 | } 161 | 162 | void target_floor_cb(const std_msgs::Int32::ConstPtr& floorRef) 163 | { 164 | if (isActive && targetFloor != floorRef->data) { 165 | if (floorHeightMap.count(floorRef->data) == 0) { 166 | ROS_ERROR("Elevator %d: Floor %d does not exist!", elev_ref_num, floorRef->data); 167 | return; 168 | } 169 | 170 | targetFloor = floorRef->data; 171 | ROS_INFO("Elevator %d: Target Floor - %d", elev_ref_num, targetFloor); 172 | } 173 | } 174 | 175 | void active_elevs_cb(const std_msgs::UInt32MultiArray::ConstPtr& activeList) 176 | { 177 | isActive = false; 178 | 179 | for (std::vector::const_iterator it = activeList->data.begin(); it != activeList->data.end(); ++it) 180 | { 181 | if (*it == elev_ref_num) { 182 | isActive = true; 183 | } 184 | } 185 | } 186 | 187 | void set_param_cb(const std_msgs::Float32MultiArray::ConstPtr& param) 188 | { 189 | if (isActive) { 190 | 191 | if (param->data[0] != elevSpeed) { 192 | ROS_INFO("Lift speed of '%s' set to: %f m/s\n", model->GetName().c_str(), param->data[0]); 193 | } 194 | 195 | if (param->data[1] != elevForce) { 196 | ROS_INFO("Lift force of '%s' set to: %f N\n", model->GetName().c_str(), param->data[1]); 197 | } 198 | 199 | elevSpeed = param->data[0]; 200 | elevForce = param->data[1]; 201 | } 202 | } 203 | 204 | void parseFloorHeights(std::string floor_heights_str) 205 | { 206 | std::vector floor_heights; 207 | 208 | // parse csv-style input (also remove whitespace): 209 | std::string::iterator end_pos = std::remove(floor_heights_str.begin(), floor_heights_str.end(), ' '); 210 | floor_heights_str.erase(end_pos, floor_heights_str.end()); 211 | 212 | std::istringstream ss(floor_heights_str); 213 | std::string token; 214 | 215 | 216 | float height; 217 | 218 | while (std::getline(ss, token, ',')) { 219 | 220 | try { 221 | height = std::stod(token); 222 | } catch (...) { 223 | ROS_ERROR("Invalid floor height %s", token.c_str()); 224 | std::exit(EXIT_FAILURE); 225 | } 226 | 227 | floor_heights.push_back(height); 228 | } 229 | 230 | genFloorMap(floor_heights); 231 | } 232 | 233 | void genFloorMap(std::vector floor_heights) 234 | { 235 | sort(floor_heights.begin(), floor_heights.end()); 236 | 237 | for (int floorIndex = 0; floorIndex < floor_heights.size(); floorIndex++) 238 | { 239 | floorHeightMap[floorIndex] = floor_heights.at(floorIndex); 240 | floorIndexMap[floor_heights.at(floorIndex)] = floorIndex; 241 | 242 | ROS_INFO("Mapped Floor%d to height: %f", floorIndex, floor_heights.at(floorIndex)); 243 | } 244 | 245 | numFloors = floor_heights.size(); 246 | ROS_DEBUG("Total number of floors initialized: %d", numFloors); 247 | } 248 | 249 | std::string replaceSubstring(std::string &s, std::string toReplace, std::string replaceWith) 250 | { 251 | return(s.replace(s.find(toReplace), toReplace.length(), replaceWith)); 252 | } 253 | void directElevator() 254 | { 255 | float targetHeight = floorHeightMap[targetFloor]; 256 | float currentHeight = bodyLink->GetWorldCoGPose().pos.z; 257 | float heightDiff = currentHeight - targetHeight; 258 | 259 | if (heightDiff > HEIGHT_LEVEL_TOLERANCE || heightDiff < HEIGHT_LEVEL_TOLERANCE) { 260 | if (heightDiff > 0.0) { 261 | moveDown(); 262 | } else { 263 | moveUp(); 264 | } 265 | } else { 266 | stopMotion(); 267 | } 268 | } 269 | 270 | void constrainHorizontalMovement() 271 | { 272 | math::Pose currPose = model->GetWorldPose(); 273 | float currHeight = currPose.pos.z; 274 | 275 | math::Pose stabilizedPose; 276 | stabilizedPose.rot.x = stabilizedPose.rot.y = stabilizedPose.rot.z = 0; 277 | 278 | stabilizedPose.pos.x = spawnPosX; 279 | stabilizedPose.pos.y = spawnPosY; 280 | stabilizedPose.pos.z = currHeight; 281 | 282 | model->SetWorldPose(stabilizedPose); 283 | } 284 | 285 | void publishEstimatedPos() 286 | { 287 | std_msgs::Int32 estimatedFloor; 288 | estimatedFloor.data = estimateCurrFloor(); 289 | estimated_floor_pub.publish(estimatedFloor); 290 | } 291 | 292 | int estimateCurrFloor() 293 | { 294 | float currHeight = bodyLink->GetWorldCoGPose().pos.z; 295 | 296 | for (int i=0; iSetForce(math::Vector3(0, 0, elevForce)); 308 | bodyLink->SetLinearVel(math::Vector3(0, 0, elevSpeed)); 309 | } 310 | 311 | void moveDown() 312 | { 313 | bodyLink->SetForce(math::Vector3(0, 0, -elevForce)); 314 | bodyLink->SetLinearVel(math::Vector3(0, 0, -elevSpeed)); 315 | } 316 | 317 | void stopMotion() 318 | { 319 | bodyLink->SetForce(math::Vector3(0, 0, 0)); 320 | bodyLink->SetLinearVel(math::Vector3(0, 0, 0)); 321 | } 322 | 323 | void initVars() 324 | { 325 | isActive = false; 326 | targetFloor = 0; 327 | 328 | std::string elev_ref_num_str = model->GetName(); 329 | replaceSubstring(elev_ref_num_str, model_domain_space, ""); 330 | elev_ref_num = atoi(elev_ref_num_str.c_str()); 331 | 332 | spawnPosX = bodyLink->GetWorldPose().pos.x; 333 | spawnPosY = bodyLink->GetWorldPose().pos.y; 334 | } 335 | 336 | }; 337 | 338 | GZ_REGISTER_MODEL_PLUGIN(ElevatorPlugin) 339 | } -------------------------------------------------------------------------------- /srv/AddGroup.srv: -------------------------------------------------------------------------------- 1 | # Add a control group to the manager 2 | 3 | ControlGroup group 4 | ---- -------------------------------------------------------------------------------- /srv/DeleteGroup.srv: -------------------------------------------------------------------------------- 1 | # Delete an existing group 2 | 3 | string group_name 4 | ---- 5 | string feedback -------------------------------------------------------------------------------- /srv/ListGroups.srv: -------------------------------------------------------------------------------- 1 | # List exiting control groups handled by the manager 2 | 3 | ---- 4 | ControlGroup[] groups -------------------------------------------------------------------------------- /srv/OpenCloseDoors.srv: -------------------------------------------------------------------------------- 1 | # Binary state control for doors 2 | 3 | string group_name 4 | bool state 5 | ---- -------------------------------------------------------------------------------- /srv/OpenCloseElevDoors.srv: -------------------------------------------------------------------------------- 1 | # Binary state control for elevator doors (current floor) 2 | 3 | string group_name 4 | bool state 5 | ---- -------------------------------------------------------------------------------- /srv/SetElevProps.srv: -------------------------------------------------------------------------------- 1 | # Set Elevator Properties - Speed & Force 2 | 3 | string group_name 4 | float32 force 5 | float32 velocity 6 | ---- -------------------------------------------------------------------------------- /srv/SetVelDoors.srv: -------------------------------------------------------------------------------- 1 | # Set linear or angular velocities of doors 2 | 3 | string group_name 4 | 5 | float32 lin_x 6 | float32 lin_y 7 | float32 ang_z 8 | ---- -------------------------------------------------------------------------------- /srv/TargetFloorElev.srv: -------------------------------------------------------------------------------- 1 | # Set the target floor for a group of elevators 2 | 3 | string group_name 4 | int32 target_floor 5 | ---- -------------------------------------------------------------------------------- /worlds/empty.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://ground_plane 6 | 7 | 8 | model://sun 9 | 10 | 11 | 12 | 0.4 0.4 0.4 1.0 13 | 0.0 191.0 255.0 1.0 14 | 0 15 | 16 | 17 | 18 | 19 | --------------------------------------------------------------------------------