├── .devcontainer └── devcontainer.json ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── 1099_iris_rtabmap ├── joy_config.yaml ├── px4_config.yaml ├── rcS └── rviz.rviz ├── doc └── example.jpg ├── docker └── Dockerfile ├── launch ├── gazebo.launch ├── rviz.launch ├── slam.launch └── spawn_drone.launch ├── models ├── apt │ ├── meshes │ │ ├── apt.dae │ │ └── texture.jpg │ ├── model.config │ └── model.sdf └── room │ ├── meshes │ ├── room.dae │ └── room.jpg │ ├── model.config │ └── model.sdf ├── package.xml ├── param ├── base_local_planner_params.yaml ├── costmap_common_params.yaml ├── global_costmap_params.yaml └── local_costmap_params.yaml ├── scripts └── rotor_tf.py ├── src └── offboard_node.cpp ├── urdf ├── drone.gazebo ├── drone.urdf.xacro └── realsense-RS200.macro.xacro └── worlds ├── apt.world └── room.world /.devcontainer/devcontainer.json: -------------------------------------------------------------------------------- 1 | { 2 | "build": { 3 | "dockerfile": "../docker/Dockerfile", 4 | "context": ".." 5 | }, 6 | 7 | "customizations": { 8 | "vscode": { 9 | "extensions": ["ms-vscode.cpptools-themes", "ms-vscode.cmake-tools"] 10 | } 11 | }, 12 | "workspaceMount": "source=${localWorkspaceFolder},target=/catkin_ws/src/rtabmap_drone_example,type=bind", 13 | "workspaceFolder": "/catkin_ws", 14 | "postAttachCommand": "echo 'Initialize catkin: source /ros_entrypoint.sh && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make'", 15 | "runArgs": ["--privileged"], 16 | "mounts": ["source=/dev/input,target=/dev/input,type=bind,consistency=cached"] 17 | 18 | 19 | } 20 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(rtabmap_drone_example) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | mavros 12 | move_base 13 | roscpp 14 | rospy 15 | rtabmap_ros 16 | std_msgs 17 | geometry_msgs 18 | tf 19 | ) 20 | 21 | ## System dependencies are found with CMake's conventions 22 | # find_package(Boost REQUIRED COMPONENTS system) 23 | 24 | 25 | ## Uncomment this if the package has a setup.py. This macro ensures 26 | ## modules and global scripts declared therein get installed 27 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 28 | # catkin_python_setup() 29 | 30 | ################################################ 31 | ## Declare ROS messages, services and actions ## 32 | ################################################ 33 | 34 | ## To declare and build messages, services or actions from within this 35 | ## package, follow these steps: 36 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 37 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 38 | ## * In the file package.xml: 39 | ## * add a build_depend tag for "message_generation" 40 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 41 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 42 | ## but can be declared for certainty nonetheless: 43 | ## * add a exec_depend tag for "message_runtime" 44 | ## * In this file (CMakeLists.txt): 45 | ## * add "message_generation" and every package in MSG_DEP_SET to 46 | ## find_package(catkin REQUIRED COMPONENTS ...) 47 | ## * add "message_runtime" and every package in MSG_DEP_SET to 48 | ## catkin_package(CATKIN_DEPENDS ...) 49 | ## * uncomment the add_*_files sections below as needed 50 | ## and list every .msg/.srv/.action file to be processed 51 | ## * uncomment the generate_messages entry below 52 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 53 | 54 | ## Generate messages in the 'msg' folder 55 | # add_message_files( 56 | # FILES 57 | # Message1.msg 58 | # Message2.msg 59 | # ) 60 | 61 | ## Generate services in the 'srv' folder 62 | #add_service_files( 63 | # FILES 64 | # SetGoal.srv 65 | #) 66 | 67 | ## Generate actions in the 'action' folder 68 | # add_action_files( 69 | # FILES 70 | # Action1.action 71 | # Action2.action 72 | # ) 73 | 74 | ## Generate added messages and services with any dependencies listed here 75 | #generate_messages( 76 | # DEPENDENCIES 77 | # geometry_msgs 78 | #) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES rtabmap_drone_example 112 | # CATKIN_DEPENDS mavros move_base roscpp rospy rtabmap_ros std_msgs geometry_msgs 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | # include 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/rtabmap_drone_example.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/rtabmap_drone_example_node.cpp) 141 | 142 | add_executable(rtabmap_drone_example_offboard src/offboard_node.cpp) 143 | #add_dependencies(rtabmap_drone_example_offboard ${${PROJECT_NAME}_EXPORTED_TARGETS}) 144 | target_link_libraries(rtabmap_drone_example_offboard ${catkin_LIBRARIES}) 145 | set_target_properties(rtabmap_drone_example_offboard PROPERTIES OUTPUT_NAME "offboard") 146 | 147 | ## Rename C++ executable without prefix 148 | ## The above recommended prefix causes long target names, the following renames the 149 | ## target back to the shorter version for ease of user use 150 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 151 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 152 | 153 | ## Add cmake target dependencies of the executable 154 | ## same as for the library above 155 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 156 | 157 | ## Specify libraries to link a library or executable target against 158 | # target_link_libraries(${PROJECT_NAME}_node 159 | # ${catkin_LIBRARIES} 160 | # ) 161 | 162 | ############# 163 | ## Install ## 164 | ############# 165 | 166 | # all install targets should use catkin DESTINATION variables 167 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 168 | 169 | ## Mark executable scripts (Python etc.) for installation 170 | ## in contrast to setup.py, you can choose the destination 171 | # install(PROGRAMS 172 | # scripts/my_python_script 173 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 174 | # ) 175 | 176 | ## Mark executables for installation 177 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 178 | # install(TARGETS ${PROJECT_NAME}_node 179 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark libraries for installation 183 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 184 | install(TARGETS rtabmap_drone_example_offboard 185 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 186 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 187 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 188 | ) 189 | 190 | ## Mark cpp header files for installation 191 | # install(DIRECTORY include/${PROJECT_NAME}/ 192 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 193 | # FILES_MATCHING PATTERN "*.h" 194 | # PATTERN ".svn" EXCLUDE 195 | # ) 196 | 197 | install(DIRECTORY 198 | launch 199 | config 200 | param 201 | worlds 202 | urdf 203 | models 204 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 205 | ) 206 | 207 | ## Mark other files for installation (e.g. launch and bag files, etc.) 208 | # install(FILES 209 | # # myfile1 210 | # # myfile2 211 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 212 | # ) 213 | 214 | catkin_install_python(PROGRAMS scripts/rotor_tf.py 215 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 216 | 217 | ############# 218 | ## Testing ## 219 | ############# 220 | 221 | ## Add gtest based cpp test target and link libraries 222 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_rtabmap_drone_example.cpp) 223 | # if(TARGET ${PROJECT_NAME}-test) 224 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 225 | # endif() 226 | 227 | ## Add folders to be run by python nosetests 228 | # catkin_add_nosetests(test) 229 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2020, matlabbe 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rtabmap_drone_example 2 | 2D navigation example of a drone using [move_base](http://wiki.ros.org/move_base) with [mavros](http://wiki.ros.org/mavros)/[px4](https://github.com/PX4/PX4-Autopilot) and [rtabmap](wiki.ros.org/rtabmap_ros) visual SLAM. 3 | 4 | Overview video ([click](https://youtu.be/A487ybS7E4E) to watch on Youtube): 5 | 6 | [![Youtube](https://i.imgur.com/UKLtD7L.gif)](https://youtu.be/A487ybS7E4E) 7 | 8 | ## Install 9 | 10 | ### Docker (recommended) 11 | To make it simple, create the following docker image (nvidia GPU required): 12 | ```bash 13 | git clone https://github.com/matlabbe/rtabmap_drone_example.git 14 | cd rtabmap_drone_example 15 | docker build -t rtabmap_drone_example -f docker/Dockerfile . 16 | ``` 17 | 18 | ### Dev Container 19 | Open project in VSCode and click "Reopen in container". The image will be automatically built. 20 | 21 | ### Host 22 | Follow instructions from [docker/Dockerfile](https://github.com/matlabbe/rtabmap_drone_example/blob/master/docker/Dockerfile) to install dependencies. 23 | 24 | ## Usage 25 | 26 | ### Docker (recommended) 27 | 28 | Launch the simulator: 29 | ```bash 30 | XAUTH=/tmp/.docker.xauth 31 | touch $XAUTH 32 | xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - 33 | 34 | docker run -it --rm \ 35 | --privileged \ 36 | --network=host \ 37 | --env="DISPLAY=$DISPLAY" \ 38 | --env="QT_X11_NO_MITSHM=1" \ 39 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 40 | --env="XAUTHORITY=$XAUTH" \ 41 | --volume="$XAUTH:$XAUTH" \ 42 | --runtime=nvidia \ 43 | rtabmap_drone_example \ 44 | roslaunch rtabmap_drone_example gazebo.launch 45 | ``` 46 | 47 | Launch VSLAM: 48 | ```bash 49 | XAUTH=/tmp/.docker.xauth 50 | touch $XAUTH 51 | xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - 52 | 53 | docker run -it --rm \ 54 | --privileged \ 55 | --network=host \ 56 | --env="DISPLAY=$DISPLAY" \ 57 | --env="QT_X11_NO_MITSHM=1" \ 58 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 59 | --env="XAUTHORITY=$XAUTH" \ 60 | --volume="$XAUTH:$XAUTH" \ 61 | --runtime=nvidia \ 62 | rtabmap_drone_example \ 63 | roslaunch rtabmap_drone_example slam.launch 64 | ``` 65 | 66 | Launch rviz: 67 | ```bash 68 | XAUTH=/tmp/.docker.xauth 69 | touch $XAUTH 70 | xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - 71 | 72 | docker run -it --rm \ 73 | --privileged \ 74 | --network=host \ 75 | --env="DISPLAY=$DISPLAY" \ 76 | --env="QT_X11_NO_MITSHM=1" \ 77 | --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ 78 | --env="XAUTHORITY=$XAUTH" \ 79 | --volume="$XAUTH:$XAUTH" \ 80 | --runtime=nvidia \ 81 | rtabmap_drone_example \ 82 | roslaunch rtabmap_drone_example rviz.launch 83 | ``` 84 | 85 | Arm and take off: 86 | ```bash 87 | docker run -it --rm \ 88 | --privileged \ 89 | --network=host \ 90 | rtabmap_drone_example \ 91 | rosrun rtabmap_drone_example offboard 92 | ``` 93 | 94 | ### Dev Container 95 | Open 4 terminals: 96 | ```bash 97 | /entrypoint.sh roslaunch rtabmap_drone_example gazebo.launch 98 | /entrypoint.sh roslaunch rtabmap_drone_example slam.launch 99 | /entrypoint.sh roslaunch rtabmap_drone_example rviz.launch 100 | 101 | /entrypoint.sh rosrun rtabmap_drone_example offboard 102 | ``` 103 | To edit and use mounted code, init catkin workspace with: 104 | ``` 105 | source /ros_entrypoint.sh && cd /catkin_ws/src && catkin_init_workspace && cd /catkin_ws && catkin_make 106 | ``` 107 | then you will have to manually do what `/entrypoint.sh` does, but sourcing `/catkin_ws/devel/setup.bash` before setting the env variables: 108 | ``` 109 | source /opt/ros/noetic/setup.bash 110 | source /catkin_ws/devel/setup.bash 111 | source /usr/local/px4/Tools/setup_gazebo.bash /usr/local/px4 /usr/local/px4/build/px4_sitl_default 112 | export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/usr/local/px4:/usr/local/px4/Tools/sitl_gazebo 113 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/catkin_ws/src/rtabmap_drone_example/models 114 | export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/noetic/share/realsense_gazebo_plugin/models 115 | 116 | roslaunch ... 117 | ``` 118 | 119 | ### Host 120 | 121 | ```bash 122 | roslaunch rtabmap_drone_example gazebo.launch 123 | roslaunch rtabmap_drone_example slam.launch 124 | roslaunch rtabmap_drone_example rviz.launch 125 | 126 | rosrun rtabmap_drone_example offboard 127 | ``` 128 | 129 | 130 | ## Control 131 | * Autonomous control: use "2D Nav Goal" button in RVIZ to set a goal to reach 132 | * Manual control: If a joystick is plugged, you can send twists by holding L1 and moving the joysticks. Hold L1+L2 with left joystick down to land (be gentle to land smoothly), then hold left joystick in bottom-right position to disarm after the drone is on the ground. 133 | 134 | 135 | ![](https://raw.githubusercontent.com/matlabbe/rtabmap_drone_example/master/doc/example.jpg) 136 | 137 | -------------------------------------------------------------------------------- /config/1099_iris_rtabmap: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | # 3 | # @name 3DR Iris Quadrotor SITL (Vision) 4 | # 5 | # @type Quadrotor Wide 6 | # 7 | 8 | . ${R}etc/init.d-posix/airframes/10016_iris 9 | 10 | # EKF2: Vision position and heading 11 | param set-default EKF2_AID_MASK 24 12 | param set-default EKF2_EV_DELAY 200 13 | 14 | param set-default EKF2_HGT_MODE 3 15 | param set-default EKF2_MAG_TYPE 5 16 | param set-default EKF2_ASP_DELAY 50 17 | param set-default EKF2_OF_DELAY 0 18 | param set-default EKF2_MIN_OBS_DT 20 19 | param set-default EKF2_EV_POS_X 0 20 | param set-default EKF2_EV_POS_Y 0 21 | param set-default EKF2_EV_POS_Z 0 22 | param set-default EKF2_EVP_NOISE 0.1 23 | param set-default EKF2_EVA_NOISE 0.05 24 | 25 | # LPE: Vision + baro 26 | param set-default LPE_FUSION 132 27 | 28 | # AEQ: External heading set to use vision input 29 | param set-default ATT_EXT_HDG_M 1 30 | -------------------------------------------------------------------------------- /config/joy_config.yaml: -------------------------------------------------------------------------------- 1 | # ps4 2 | teleop_node: 3 | axis_linear: # Forward/Back 4 | x: 4 5 | y: 3 6 | z: 1 7 | scale_linear: 8 | x: 1 9 | y: 0.5 10 | z: 1 11 | 12 | axis_angular: # Turn Left/Right 13 | yaw: 0 14 | scale_angular: 15 | yaw: 1 16 | 17 | enable_button: 4 # Trigger 18 | -------------------------------------------------------------------------------- /config/px4_config.yaml: -------------------------------------------------------------------------------- 1 | # Common configuration for PX4 autopilot 2 | # 3 | # node: 4 | startup_px4_usb_quirk: true 5 | 6 | # --- system plugins --- 7 | 8 | # sys_status & sys_time connection options 9 | conn: 10 | heartbeat_rate: 1.0 # send hertbeat rate in Hertz 11 | timeout: 10.0 # hertbeat timeout in seconds 12 | timesync_rate: 10.0 # TIMESYNC rate in Hertz (feature disabled if 0.0) 13 | system_time_rate: 1.0 # send system time to FCU rate in Hertz (disabled if 0.0) 14 | 15 | # sys_status 16 | sys: 17 | min_voltage: 10.0 # diagnostics min voltage 18 | disable_diag: false # disable all sys_status diagnostics, except heartbeat 19 | 20 | # sys_time 21 | time: 22 | time_ref_source: "fcu" # time_reference source 23 | timesync_mode: MAVLINK 24 | timesync_avg_alpha: 0.6 # timesync averaging factor 25 | 26 | # --- mavros plugins (alphabetical order) --- 27 | 28 | # 3dr_radio 29 | tdr_radio: 30 | low_rssi: 40 # raw rssi lower level for diagnostics 31 | 32 | # actuator_control 33 | # None 34 | 35 | # command 36 | cmd: 37 | use_comp_id_system_control: false # quirk for some old FCUs 38 | 39 | # dummy 40 | # None 41 | 42 | # ftp 43 | # None 44 | 45 | # global_position 46 | global_position: 47 | frame_id: "map" # origin frame 48 | child_frame_id: "base_link" # body-fixed frame 49 | rot_covariance: 99999.0 # covariance for attitude? 50 | gps_uere: 1.0 # User Equivalent Range Error (UERE) of GPS sensor (m) 51 | use_relative_alt: true # use relative altitude for local coordinates 52 | tf: 53 | send: false # send TF? 54 | frame_id: "map" # TF frame_id 55 | global_frame_id: "earth" # TF earth frame_id 56 | child_frame_id: "base_link" # TF child_frame_id 57 | 58 | # imu_pub 59 | imu: 60 | frame_id: "base_link" 61 | # need find actual values 62 | linear_acceleration_stdev: 0.0003 63 | angular_velocity_stdev: !degrees 0.02 64 | orientation_stdev: 1.0 65 | magnetic_stdev: 0.0 66 | 67 | # local_position 68 | local_position: 69 | frame_id: "map" 70 | tf: 71 | send: false 72 | frame_id: "map" 73 | child_frame_id: "base_link" 74 | send_fcu: false 75 | 76 | # param 77 | # None, used for FCU params 78 | 79 | # rc_io 80 | # None 81 | 82 | # safety_area 83 | safety_area: 84 | p1: {x: 1.0, y: 1.0, z: 1.0} 85 | p2: {x: -1.0, y: -1.0, z: -1.0} 86 | 87 | # setpoint_accel 88 | setpoint_accel: 89 | send_force: false 90 | 91 | # setpoint_attitude 92 | setpoint_attitude: 93 | reverse_thrust: false # allow reversed thrust 94 | use_quaternion: false # enable PoseStamped topic subscriber 95 | tf: 96 | listen: false # enable tf listener (disable topic subscribers) 97 | frame_id: "map" 98 | child_frame_id: "target_attitude" 99 | rate_limit: 50.0 100 | 101 | setpoint_raw: 102 | thrust_scaling: 1.0 # used in setpoint_raw attitude callback. 103 | # Note: PX4 expects normalized thrust values between 0 and 1, which means that 104 | # the scaling needs to be unitary and the inputs should be 0..1 as well. 105 | 106 | # setpoint_position 107 | setpoint_position: 108 | tf: 109 | listen: false # enable tf listener (disable topic subscribers) 110 | frame_id: "map" 111 | child_frame_id: "target_position" 112 | rate_limit: 50.0 113 | mav_frame: LOCAL_NED 114 | 115 | # setpoint_velocity 116 | setpoint_velocity: 117 | mav_frame: LOCAL_NED 118 | 119 | # vfr_hud 120 | # None 121 | 122 | # waypoint 123 | mission: 124 | pull_after_gcs: true # update mission if gcs updates 125 | 126 | # --- mavros extras plugins (same order) --- 127 | 128 | # adsb 129 | # None 130 | 131 | # debug_value 132 | # None 133 | 134 | # distance_sensor 135 | ## Currently available orientations: 136 | # Check http://wiki.ros.org/mavros/Enumerations 137 | ## 138 | distance_sensor: 139 | hrlv_ez4_pub: 140 | id: 0 141 | frame_id: "hrlv_ez4_sonar" 142 | orientation: PITCH_270 # RPY:{0.0, 270.0, 0.0} - downward-facing 143 | field_of_view: 0.0 # XXX TODO 144 | send_tf: true 145 | sensor_position: {x: 0.0, y: 0.0, z: -0.1} 146 | lidarlite_pub: 147 | id: 1 148 | frame_id: "lidarlite_laser" 149 | orientation: PITCH_270 150 | field_of_view: 0.0 # XXX TODO 151 | send_tf: true 152 | sensor_position: {x: 0.0, y: 0.0, z: -0.1} 153 | sonar_1_sub: 154 | subscriber: true 155 | id: 2 156 | orientation: PITCH_270 157 | laser_1_sub: 158 | subscriber: true 159 | id: 3 160 | orientation: PITCH_270 161 | 162 | # image_pub 163 | image: 164 | frame_id: "px4flow" 165 | 166 | # fake_gps 167 | fake_gps: 168 | # select data source 169 | use_mocap: true # ~mocap/pose 170 | mocap_transform: true # ~mocap/tf instead of pose 171 | use_vision: false # ~vision (pose) 172 | # origin (default: Zürich) 173 | geo_origin: 174 | lat: 47.3667 # latitude [degrees] 175 | lon: 8.5500 # longitude [degrees] 176 | alt: 408.0 # altitude (height over the WGS-84 ellipsoid) [meters] 177 | eph: 2.0 178 | epv: 2.0 179 | satellites_visible: 5 # virtual number of visible satellites 180 | fix_type: 3 # type of GPS fix (default: 3D) 181 | tf: 182 | listen: false 183 | send: false # send TF? 184 | frame_id: "map" # TF frame_id 185 | child_frame_id: "fix" # TF child_frame_id 186 | rate_limit: 10.0 # TF rate 187 | gps_rate: 5.0 # GPS data publishing rate 188 | 189 | # mocap_pose_estimate 190 | mocap: 191 | # select mocap source 192 | use_tf: false # ~mocap/tf 193 | use_pose: true # ~mocap/pose 194 | 195 | # odom 196 | odometry: 197 | in: 198 | frame_id: "odom" 199 | child_frame_id: "base_link" 200 | frame_tf: 201 | local_frame: "local_origin_ned" 202 | body_frame_orientation: "flu" 203 | out: 204 | frame_tf: 205 | # available: check MAV_FRAME odometry local frames in 206 | # https://mavlink.io/en/messages/common.html 207 | local_frame: "vision_ned" 208 | # available: ned, frd or flu (though only the tf to frd is supported) 209 | body_frame_orientation: "frd" 210 | 211 | # px4flow 212 | px4flow: 213 | frame_id: "px4flow" 214 | ranger_fov: !degrees 6.8 # 6.8 degreens at 5 meters, 31 degrees at 1 meter 215 | ranger_min_range: 0.3 # meters 216 | ranger_max_range: 5.0 # meters 217 | 218 | # vision_pose_estimate 219 | vision_pose: 220 | tf: 221 | listen: false # enable tf listener (disable topic subscribers) 222 | frame_id: "map" 223 | child_frame_id: "vision_estimate" 224 | rate_limit: 10.0 225 | 226 | # vision_speed_estimate 227 | vision_speed: 228 | listen_twist: true # enable listen to twist topic, else listen to vec3d topic 229 | twist_cov: true # enable listen to twist with covariance topic 230 | 231 | # vibration 232 | vibration: 233 | frame_id: "base_link" 234 | 235 | # wheel_odometry 236 | wheel_odometry: 237 | count: 2 # number of wheels to compute odometry 238 | use_rpm: false # use wheel's RPM instead of cumulative distance to compute odometry 239 | wheel0: {x: 0.0, y: -0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) 240 | wheel1: {x: 0.0, y: 0.15, radius: 0.05} # x-, y-offset (m,NED) and radius (m) 241 | send_raw: true # send wheel's RPM and cumulative distance (~/wheel_odometry/rpm, ~/wheel_odometry/distance) 242 | send_twist: false # send geometry_msgs/TwistWithCovarianceStamped instead of nav_msgs/Odometry 243 | frame_id: "map" # origin frame 244 | child_frame_id: "base_link" # body-fixed frame 245 | vel_error: 0.1 # wheel velocity measurement error 1-std (m/s) 246 | tf: 247 | send: true 248 | frame_id: "map" 249 | child_frame_id: "base_link" 250 | 251 | # vim:set ts=2 sw=2 et: 252 | -------------------------------------------------------------------------------- /config/rcS: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | # PX4 commands need the 'px4-' prefix in bash. 4 | # (px4-alias.sh is expected to be in the PATH) 5 | # shellcheck disable=SC1091 6 | . px4-alias.sh 7 | 8 | #search path for sourcing px4-rc.* 9 | PATH="$PATH:${R}etc/init.d-posix" 10 | 11 | THISPATH="$(CDPATH='' cd -- "$(dirname -- "$0")" && pwd)" 12 | 13 | # 14 | # Main SITL startup script 15 | # 16 | 17 | # check for ekf2 replay 18 | # shellcheck disable=SC2154 19 | if [ "$replay_mode" = "ekf2" ] 20 | then 21 | . ${R}etc/init.d-posix/rc.replay 22 | exit 0 23 | fi 24 | 25 | # initialize script variables 26 | set AUX_MODE none 27 | set AUX_BANK2 none 28 | set IO_PRESENT no 29 | set MAV_TYPE none 30 | set MIXER none 31 | set MIXER_AUX none 32 | set MIXER_FILE none 33 | set OUTPUT_MODE sim 34 | set EXTRA_MIXER_MODE none 35 | set PWM_OUT none 36 | set SDCARD_MIXERS_PATH etc/mixers 37 | set USE_IO no 38 | set VEHICLE_TYPE none 39 | set LOGGER_ARGS "" 40 | set LOGGER_BUF 1000 41 | 42 | set RUN_MINIMAL_SHELL no 43 | 44 | # Use the variable set by sitl_run.sh to choose the model settings. 45 | if [ "$PX4_SIM_MODEL" = "shell" ]; then 46 | set RUN_MINIMAL_SHELL yes 47 | else 48 | # Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL}) 49 | # TODO: unify with rc.autostart generation 50 | # shellcheck disable=SC2012 51 | REQUESTED_AUTOSTART=$(ls "${THISPATH}" | sed -n 's/^\([0-9][0-9]*\)_'${PX4_SIM_MODEL}'$/\1/p') 52 | if [ -z "$REQUESTED_AUTOSTART" ]; then 53 | echo "Error: Unknown model $PX4_SIM_MODEL (not found by name on ${THISPATH})" 54 | exit 1 55 | else 56 | echo "Info: found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART" 57 | fi 58 | fi 59 | 60 | # Load parameters 61 | set PARAM_FILE eeprom/parameters_"$REQUESTED_AUTOSTART" 62 | param select $PARAM_FILE 63 | 64 | if [ -f $PARAM_FILE ] 65 | then 66 | if param load 67 | then 68 | echo "[param] Loaded: $PARAM_FILE" 69 | else 70 | echo "[param] FAILED loading $PARAM_FILE" 71 | fi 72 | else 73 | echo "[param] parameter file not found, creating $PARAM_FILE" 74 | fi 75 | 76 | # exit early when the minimal shell is requested 77 | [ $RUN_MINIMAL_SHELL = yes ] && exit 0 78 | 79 | 80 | # Use environment variable PX4_ESTIMATOR to choose estimator. 81 | if [ "$PX4_ESTIMATOR" = "q" ]; then 82 | param set SYS_MC_EST_GROUP 3 83 | elif [ "$PX4_ESTIMATOR" = "ekf2" ]; then 84 | param set SYS_MC_EST_GROUP 2 85 | elif [ "$PX4_ESTIMATOR" = "lpe" ]; then 86 | param set SYS_MC_EST_GROUP 1 87 | elif [ "$PX4_ESTIMATOR" = "inav" ]; then 88 | param set SYS_MC_EST_GROUP 0 89 | fi 90 | 91 | if param compare SYS_AUTOSTART $REQUESTED_AUTOSTART 92 | then 93 | set AUTOCNF no 94 | else 95 | set AUTOCNF yes 96 | param set SYS_AUTOCONFIG 1 97 | fi 98 | 99 | if param compare SYS_AUTOCONFIG 1 100 | then 101 | set AUTOCNF yes 102 | 103 | # Wipe out params except RC*, flight modes, total flight time, accel cal, gyro cal, next flight UUID 104 | param reset_all SYS_AUTO* RC* COM_FLTMODE* LND_FLIGHT* TC_* CAL_ACC* CAL_GYRO* COM_FLIGHT* 105 | fi 106 | 107 | # multi-instance setup 108 | # shellcheck disable=SC2154 109 | param set MAV_SYS_ID $((px4_instance+1)) 110 | 111 | if [ $AUTOCNF = yes ] 112 | then 113 | param set SYS_AUTOSTART $REQUESTED_AUTOSTART 114 | 115 | param set CAL_ACC0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION 116 | param set CAL_GYRO0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION 117 | param set CAL_ACC1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION 118 | param set CAL_GYRO1_ID 1310996 # 1310996: DRV_IMU_DEVTYPE_SIM, BUS: 2, ADDR: 1, TYPE: SIMULATION 119 | param set CAL_ACC2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION 120 | param set CAL_GYRO2_ID 1311004 # 1311004: DRV_IMU_DEVTYPE_SIM, BUS: 3, ADDR: 1, TYPE: SIMULATION 121 | 122 | param set CAL_MAG0_ID 197388 123 | param set CAL_MAG1_ID 197644 124 | 125 | param set SENS_BOARD_X_OFF 0.000001 126 | param set SENS_DPRES_OFF 0.001 127 | 128 | param set SYS_RESTART_TYPE 2 129 | fi 130 | 131 | param set-default BAT_N_CELLS 4 132 | 133 | param set-default CBRK_AIRSPD_CHK 0 134 | param set-default CBRK_SUPPLY_CHK 894281 135 | 136 | # disable check, no CPU load reported on posix yet 137 | param set-default COM_CPU_MAX -1 138 | 139 | # Don't require RC calibration and configuration 140 | param set-default COM_RC_IN_MODE 1 141 | 142 | # Speedup SITL startup 143 | param set-default EKF2_REQ_GPS_H 0.5 144 | 145 | # Multi-EKF 146 | param set-default EKF2_MULTI_IMU 3 147 | param set-default SENS_IMU_MODE 0 148 | param set-default EKF2_MULTI_MAG 2 149 | param set-default SENS_MAG_MODE 0 150 | 151 | param set-default IMU_GYRO_FFT_EN 1 152 | 153 | # By default log from boot until first disarm. 154 | param set-default SDLOG_MODE 1 155 | # enable default, estimator replay and vision/avoidance logging profiles 156 | param set-default SDLOG_PROFILE 131 157 | param set-default SDLOG_DIRS_MAX 7 158 | 159 | param set-default TRIG_INTERFACE 3 160 | 161 | # Adapt timeout parameters if simulation runs faster or slower than realtime. 162 | if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then 163 | COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc) 164 | echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER" 165 | param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER 166 | 167 | COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc) 168 | echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER" 169 | param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER 170 | 171 | COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 0.5" | bc) 172 | echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER" 173 | param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER 174 | 175 | COM_OBC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 5.0" | bc) 176 | echo "COM_OBC_LOSS_T set to $COM_OBC_LOSS_T_LONGER" 177 | param set COM_OBC_LOSS_T $COM_OBC_LOSS_T_LONGER 178 | fi 179 | 180 | # Autostart ID 181 | autostart_file='' 182 | # shellcheck disable=SC2231 183 | for f in ${THISPATH}/"$(param show -q SYS_AUTOSTART)"_* 184 | do 185 | filename=$(basename "$f") 186 | case "$filename" in 187 | *\.*) 188 | # ignore files that contain a dot (e.g. .post) 189 | ;; 190 | *) 191 | autostart_file="$f" 192 | ;; 193 | esac 194 | done 195 | if [ ! -e "$autostart_file" ]; then 196 | echo "Error: no autostart file found ($autostart_file)" 197 | exit 1 198 | fi 199 | 200 | . "$autostart_file" 201 | 202 | # 203 | # If autoconfig parameter was set, reset it and save parameters. 204 | # 205 | if [ $AUTOCNF = yes ] 206 | then 207 | param set SYS_AUTOCONFIG 0 208 | fi 209 | 210 | # Simulator IMU data provided at 250 Hz 211 | param set IMU_INTEG_RATE 250 212 | 213 | #user defined params for instances can be in PATH 214 | . px4-rc.params 215 | 216 | dataman start 217 | # only start the simulator if not in replay mode, as both control the lockstep time 218 | if ! replay tryapplyparams 219 | then 220 | . px4-rc.simulator 221 | fi 222 | load_mon start 223 | battery_simulator start 224 | tone_alarm start 225 | rc_update start 226 | sensors start 227 | commander start 228 | navigator start 229 | 230 | # Try to start the micrortps_client with UDP transport if module exists 231 | if px4-micrortps_client status > /dev/null 2>&1 232 | then 233 | # shellcheck disable=SC2154 234 | micrortps_client start -t UDP $((2019+2*px4_instance)) -s $((2020+2*px4_instance)) 235 | fi 236 | 237 | if param greater -s MNT_MODE_IN -1 238 | then 239 | vmount start 240 | fi 241 | 242 | if param greater -s TRIG_MODE 0 243 | then 244 | camera_trigger start 245 | camera_feedback start 246 | fi 247 | 248 | if param compare -s IMU_GYRO_FFT_EN 1 249 | then 250 | gyro_fft start 251 | fi 252 | 253 | if param compare -s IMU_GYRO_CAL_EN 1 254 | then 255 | gyro_calibration start 256 | fi 257 | 258 | # Configure vehicle type specific parameters. 259 | # Note: rc.vehicle_setup is the entry point for rc.interface, 260 | # rc.fw_apps, rc.mc_apps, rc.rover_apps, and rc.vtol_apps. 261 | # 262 | . ${R}etc/init.d/rc.vehicle_setup 263 | 264 | #user defined mavlink streams for instances can be in PATH 265 | . px4-rc.mavlink 266 | 267 | # execute autostart post script if any 268 | [ -e "$autostart_file".post ] && . "$autostart_file".post 269 | 270 | # Run script to start logging 271 | if param compare SYS_MC_EST_GROUP 2 272 | then 273 | set LOGGER_ARGS "-p ekf2_timestamps" 274 | else 275 | set LOGGER_ARGS "-p vehicle_attitude" 276 | fi 277 | . ${R}etc/init.d/rc.logging 278 | 279 | mavlink boot_complete 280 | replay trystart 281 | -------------------------------------------------------------------------------- /config/rviz.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /TF1/Frames1 8 | Splitter Ratio: 0.515418529510498 9 | Tree Height: 341 10 | - Class: rviz/Selection 11 | Name: Selection 12 | - Class: rviz/Tool Properties 13 | Expanded: 14 | - /2D Pose Estimate1 15 | - /2D Nav Goal1 16 | - /Publish Point1 17 | Name: Tool Properties 18 | Splitter Ratio: 0.5886790156364441 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | - Class: rviz/Time 25 | Experimental: false 26 | Name: Time 27 | SyncMode: 0 28 | SyncSource: DWA cost 29 | Preferences: 30 | PromptSaveOnExit: true 31 | Toolbars: 32 | toolButtonStyle: 2 33 | Visualization Manager: 34 | Class: "" 35 | Displays: 36 | - Alpha: 0.5 37 | Cell Size: 1 38 | Class: rviz/Grid 39 | Color: 160; 160; 164 40 | Enabled: true 41 | Line Style: 42 | Line Width: 0.029999999329447746 43 | Value: Lines 44 | Name: Grid 45 | Normal Cell Count: 0 46 | Offset: 47 | X: 0 48 | Y: 0 49 | Z: 0 50 | Plane: XY 51 | Plane Cell Count: 10 52 | Reference Frame: 53 | Value: true 54 | - Class: rviz/TF 55 | Enabled: true 56 | Frame Timeout: 15 57 | Frames: 58 | All Enabled: false 59 | base_camera: 60 | Value: false 61 | base_link: 62 | Value: true 63 | base_link_frd: 64 | Value: true 65 | base_link_stabilized: 66 | Value: true 67 | color: 68 | Value: false 69 | depth: 70 | Value: false 71 | imu_body_link: 72 | Value: false 73 | imu_link: 74 | Value: false 75 | ired1: 76 | Value: false 77 | ired2: 78 | Value: false 79 | map: 80 | Value: false 81 | map_ned: 82 | Value: true 83 | odom: 84 | Value: false 85 | odom_ned: 86 | Value: true 87 | rotor_0: 88 | Value: false 89 | rotor_1: 90 | Value: false 91 | rotor_2: 92 | Value: false 93 | rotor_3: 94 | Value: false 95 | rs200_camera: 96 | Value: false 97 | Marker Alpha: 1 98 | Marker Scale: 1 99 | Name: TF 100 | Show Arrows: true 101 | Show Axes: true 102 | Show Names: true 103 | Tree: 104 | map: 105 | map_ned: 106 | {} 107 | odom: 108 | base_link_stabilized: 109 | base_link: 110 | base_camera: 111 | rs200_camera: 112 | color: 113 | {} 114 | depth: 115 | {} 116 | ired1: 117 | {} 118 | ired2: 119 | {} 120 | base_link_frd: 121 | {} 122 | imu_body_link: 123 | {} 124 | imu_link: 125 | {} 126 | rotor_0: 127 | {} 128 | rotor_1: 129 | {} 130 | rotor_2: 131 | {} 132 | rotor_3: 133 | {} 134 | odom_ned: 135 | {} 136 | Update Interval: 0 137 | Value: true 138 | - Alpha: 1 139 | Class: rtabmap_rviz_plugins/MapGraph 140 | Enabled: true 141 | Global loop closure: 255; 0; 0 142 | Landmark: 0; 128; 0 143 | Local loop closure: 255; 255; 0 144 | Merged neighbor: 255; 170; 0 145 | Name: MapGraph 146 | Neighbor: 0; 0; 255 147 | Queue Size: 10 148 | Topic: /rtabmap/mapGraph 149 | Unreliable: false 150 | User: 255; 0; 0 151 | Value: true 152 | Virtual: 255; 0; 255 153 | - Class: rviz/Marker 154 | Enabled: true 155 | Marker Topic: /voxels_marked 156 | Name: Obstacles 157 | Namespaces: 158 | /move_base/local_costmap/voxel_layer/voxel_grid: true 159 | Queue Size: 100 160 | Value: true 161 | - Alpha: 0.699999988079071 162 | Class: rviz/Map 163 | Color Scheme: map 164 | Draw Behind: false 165 | Enabled: true 166 | Name: Occupancy Grid 167 | Topic: /rtabmap/grid_map 168 | Unreliable: false 169 | Use Timestamp: false 170 | Value: true 171 | - Alpha: 1 172 | Buffer Length: 1 173 | Class: rviz/Path 174 | Color: 255; 255; 0 175 | Enabled: true 176 | Head Diameter: 0.30000001192092896 177 | Head Length: 0.20000000298023224 178 | Length: 0.30000001192092896 179 | Line Style: Lines 180 | Line Width: 0.029999999329447746 181 | Name: DWA local plan 182 | Offset: 183 | X: 0 184 | Y: 0 185 | Z: 0 186 | Pose Color: 255; 85; 255 187 | Pose Style: None 188 | Queue Size: 10 189 | Radius: 0.029999999329447746 190 | Shaft Diameter: 0.10000000149011612 191 | Shaft Length: 0.10000000149011612 192 | Topic: /move_base/DWAPlannerROS/local_plan 193 | Unreliable: false 194 | Value: true 195 | - Alpha: 0.699999988079071 196 | Class: rviz/Map 197 | Color Scheme: costmap 198 | Draw Behind: false 199 | Enabled: false 200 | Name: Local CostMap 201 | Topic: /move_base/local_costmap/costmap 202 | Unreliable: false 203 | Use Timestamp: false 204 | Value: false 205 | - Alpha: 0.699999988079071 206 | Class: rviz/Map 207 | Color Scheme: costmap 208 | Draw Behind: false 209 | Enabled: true 210 | Name: Global CostMap 211 | Topic: /move_base/global_costmap/costmap 212 | Unreliable: false 213 | Use Timestamp: false 214 | Value: true 215 | - Alpha: 1 216 | Buffer Length: 1 217 | Class: rviz/Path 218 | Color: 25; 255; 0 219 | Enabled: true 220 | Head Diameter: 0.30000001192092896 221 | Head Length: 0.20000000298023224 222 | Length: 0.30000001192092896 223 | Line Style: Lines 224 | Line Width: 0.029999999329447746 225 | Name: NavFn global plan 226 | Offset: 227 | X: 0 228 | Y: 0 229 | Z: 0 230 | Pose Color: 255; 85; 255 231 | Pose Style: None 232 | Queue Size: 10 233 | Radius: 0.029999999329447746 234 | Shaft Diameter: 0.10000000149011612 235 | Shaft Length: 0.10000000149011612 236 | Topic: /move_base/NavfnROS/plan 237 | Unreliable: false 238 | Value: true 239 | - Alpha: 1 240 | Buffer Length: 1 241 | Class: rviz/Path 242 | Color: 193; 125; 17 243 | Enabled: true 244 | Head Diameter: 0.30000001192092896 245 | Head Length: 0.20000000298023224 246 | Length: 0.30000001192092896 247 | Line Style: Lines 248 | Line Width: 0.029999999329447746 249 | Name: RTAB global plan 250 | Offset: 251 | X: 0 252 | Y: 0 253 | Z: 0 254 | Pose Color: 255; 85; 255 255 | Pose Style: None 256 | Queue Size: 10 257 | Radius: 0.029999999329447746 258 | Shaft Diameter: 0.10000000149011612 259 | Shaft Length: 0.10000000149011612 260 | Topic: /rtabmap/global_path 261 | Unreliable: false 262 | Value: true 263 | - Alpha: 1 264 | Buffer Length: 1 265 | Class: rviz/Path 266 | Color: 83; 232; 252 267 | Enabled: true 268 | Head Diameter: 0.30000001192092896 269 | Head Length: 0.20000000298023224 270 | Length: 0.30000001192092896 271 | Line Style: Lines 272 | Line Width: 0.029999999329447746 273 | Name: RTAB local plan 274 | Offset: 275 | X: 0 276 | Y: 0 277 | Z: 0 278 | Pose Color: 255; 85; 255 279 | Pose Style: None 280 | Queue Size: 10 281 | Radius: 0.029999999329447746 282 | Shaft Diameter: 0.10000000149011612 283 | Shaft Length: 0.10000000149011612 284 | Topic: /rtabmap/local_path 285 | Unreliable: false 286 | Value: true 287 | - Class: rviz/MarkerArray 288 | Enabled: true 289 | Marker Topic: /rtabmap/labels 290 | Name: Labels 291 | Namespaces: 292 | ids: true 293 | labels: true 294 | Queue Size: 100 295 | Value: true 296 | - Alpha: 1 297 | Class: rviz/RobotModel 298 | Collision Enabled: false 299 | Enabled: true 300 | Links: 301 | All Links Enabled: true 302 | Expand Joint Details: false 303 | Expand Link Details: false 304 | Expand Tree: false 305 | Link Tree Style: Links in Alphabetic Order 306 | base_camera: 307 | Alpha: 1 308 | Show Axes: false 309 | Show Trail: false 310 | base_link: 311 | Alpha: 1 312 | Show Axes: false 313 | Show Trail: false 314 | Value: true 315 | color: 316 | Alpha: 1 317 | Show Axes: false 318 | Show Trail: false 319 | depth: 320 | Alpha: 1 321 | Show Axes: false 322 | Show Trail: false 323 | imu_body_link: 324 | Alpha: 1 325 | Show Axes: false 326 | Show Trail: false 327 | imu_link: 328 | Alpha: 1 329 | Show Axes: false 330 | Show Trail: false 331 | ired1: 332 | Alpha: 1 333 | Show Axes: false 334 | Show Trail: false 335 | ired2: 336 | Alpha: 1 337 | Show Axes: false 338 | Show Trail: false 339 | rotor_0: 340 | Alpha: 1 341 | Show Axes: false 342 | Show Trail: false 343 | Value: true 344 | rotor_1: 345 | Alpha: 1 346 | Show Axes: false 347 | Show Trail: false 348 | Value: true 349 | rotor_2: 350 | Alpha: 1 351 | Show Axes: false 352 | Show Trail: false 353 | Value: true 354 | rotor_3: 355 | Alpha: 1 356 | Show Axes: false 357 | Show Trail: false 358 | Value: true 359 | rs200_camera: 360 | Alpha: 1 361 | Show Axes: false 362 | Show Trail: false 363 | Value: true 364 | Name: RobotModel 365 | Robot Description: robot_description 366 | TF Prefix: "" 367 | Update Interval: 0 368 | Value: true 369 | Visual Enabled: true 370 | - Alpha: 1 371 | Autocompute Intensity Bounds: true 372 | Autocompute Value Bounds: 373 | Max Value: 10 374 | Min Value: -10 375 | Value: true 376 | Axis: Z 377 | Channel Name: intensity 378 | Class: rtabmap_rviz_plugins/MapCloud 379 | Cloud decimation: 4 380 | Cloud from scan: false 381 | Cloud max depth (m): 4 382 | Cloud min depth (m): 0 383 | Cloud voxel size (m): 0.009999999776482582 384 | Color: 255; 255; 255 385 | Color Transformer: RGB8 386 | Download graph: false 387 | Download map: false 388 | Download namespace: rtabmap 389 | Enabled: false 390 | Filter ceiling (m): 1.7000000476837158 391 | Filter floor (m): 0 392 | Invert Rainbow: false 393 | Max Color: 255; 255; 255 394 | Min Color: 0; 0; 0 395 | Name: MapCloud 396 | Node filtering angle (degrees): 30 397 | Node filtering radius (m): 0 398 | Position Transformer: XYZ 399 | Queue Size: 10 400 | Size (Pixels): 3 401 | Size (m): 0.009999999776482582 402 | Style: Points 403 | Topic: /rtabmap/mapData 404 | Unreliable: false 405 | Use Fixed Frame: true 406 | Use rainbow: true 407 | Value: false 408 | - Alpha: 1 409 | Autocompute Intensity Bounds: true 410 | Autocompute Value Bounds: 411 | Max Value: 0.17141246795654297 412 | Min Value: 0.02858993411064148 413 | Value: true 414 | Axis: Z 415 | Channel Name: total_cost 416 | Class: rviz/PointCloud2 417 | Color: 255; 255; 255 418 | Color Transformer: Intensity 419 | Decay Time: 0 420 | Enabled: true 421 | Invert Rainbow: false 422 | Max Color: 255; 255; 255 423 | Min Color: 0; 0; 0 424 | Name: DWA cost 425 | Position Transformer: XYZ 426 | Queue Size: 10 427 | Selectable: true 428 | Size (Pixels): 5 429 | Size (m): 0.009999999776482582 430 | Style: Points 431 | Topic: /move_base/DWAPlannerROS/cost_cloud 432 | Unreliable: false 433 | Use Fixed Frame: true 434 | Use rainbow: true 435 | Value: true 436 | - Alpha: 1 437 | Autocompute Intensity Bounds: true 438 | Autocompute Value Bounds: 439 | Max Value: 0.23110231757164001 440 | Min Value: 0.20249906182289124 441 | Value: true 442 | Axis: Z 443 | Channel Name: cost 444 | Class: rviz/PointCloud2 445 | Color: 255; 255; 255 446 | Color Transformer: Intensity 447 | Decay Time: 0 448 | Enabled: false 449 | Invert Rainbow: false 450 | Max Color: 255; 255; 255 451 | Min Color: 0; 0; 0 452 | Name: DWA traj 453 | Position Transformer: XYZ 454 | Queue Size: 10 455 | Selectable: true 456 | Size (Pixels): 3 457 | Size (m): 0.009999999776482582 458 | Style: Points 459 | Topic: /move_base/DWAPlannerROS/trajectory_cloud 460 | Unreliable: false 461 | Use Fixed Frame: true 462 | Use rainbow: true 463 | Value: false 464 | - Alpha: 1 465 | Axes Length: 1 466 | Axes Radius: 0.10000000149011612 467 | Class: rviz/Pose 468 | Color: 255; 25; 0 469 | Enabled: true 470 | Head Length: 0.30000001192092896 471 | Head Radius: 0.10000000149011612 472 | Name: Goal 473 | Queue Size: 10 474 | Shaft Length: 1 475 | Shaft Radius: 0.05000000074505806 476 | Shape: Arrow 477 | Topic: /rtabmap/goal_out 478 | Unreliable: false 479 | Value: true 480 | - Alpha: 1 481 | Class: rviz/Polygon 482 | Color: 25; 255; 0 483 | Enabled: true 484 | Name: Polygon 485 | Queue Size: 10 486 | Topic: /move_base/local_costmap/footprint 487 | Unreliable: false 488 | Value: true 489 | Enabled: true 490 | Global Options: 491 | Background Color: 48; 48; 48 492 | Default Light: true 493 | Fixed Frame: map 494 | Frame Rate: 30 495 | Name: root 496 | Tools: 497 | - Class: rviz/Interact 498 | Hide Inactive Objects: true 499 | - Class: rviz/MoveCamera 500 | - Class: rviz/Select 501 | - Class: rviz/FocusCamera 502 | - Class: rviz/Measure 503 | - Class: rviz/SetInitialPose 504 | Theta std deviation: 0.2617993950843811 505 | Topic: /initialpose 506 | X std deviation: 0.5 507 | Y std deviation: 0.5 508 | - Class: rviz/SetGoal 509 | Topic: /rtabmap/goal 510 | - Class: rviz/PublishPoint 511 | Single click: true 512 | Topic: /clicked_point 513 | Value: true 514 | Views: 515 | Current: 516 | Angle: 0 517 | Class: rviz/TopDownOrtho 518 | Enable Stereo Rendering: 519 | Stereo Eye Separation: 0.05999999865889549 520 | Stereo Focal Distance: 1 521 | Swap Stereo Eyes: false 522 | Value: false 523 | Invert Z Axis: false 524 | Name: Current View 525 | Near Clip Distance: 0.009999999776482582 526 | Scale: 65.64213562011719 527 | Target Frame: base_link 528 | X: 0 529 | Y: 0 530 | Saved: ~ 531 | Window Geometry: 532 | Displays: 533 | collapsed: true 534 | Height: 717 535 | Hide Left Dock: true 536 | Hide Right Dock: true 537 | QMainWindow State: 000000ff00000000fd00000004000000000000015600000273fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d00000192000000c900fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007300000001d5000000db0000005c00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d006100670065000000023e0000009c0000000000000000fb0000000a0049006d00610067006500000002c0000001240000000000000000fb0000000c00430061006d0065007200610000000319000000cb0000000000000000000000010000010f00000273fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000273000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004170000003efc0100000002fb0000000800540069006d0065000000000000000417000004f300fffffffb0000000800540069006d006501000000000000045000000000000000000000032e0000027300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 538 | Selection: 539 | collapsed: false 540 | Time: 541 | collapsed: false 542 | Tool Properties: 543 | collapsed: true 544 | Views: 545 | collapsed: true 546 | Width: 814 547 | X: 1106 548 | Y: 27 549 | -------------------------------------------------------------------------------- /doc/example.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matlabbe/rtabmap_drone_example/8cd48184ae3481628a939659f69f2c57f4e01d26/doc/example.jpg -------------------------------------------------------------------------------- /docker/Dockerfile: -------------------------------------------------------------------------------- 1 | # cd rtabmap_drone_example 2 | # docker build -t rtabmap_drone_example -f docker/Dockerfile . 3 | 4 | FROM ros:noetic-perception 5 | 6 | RUN apt update && apt install \ 7 | ros-noetic-gazebo-dev \ 8 | ros-noetic-joy \ 9 | ros-noetic-teleop-twist-joy \ 10 | ros-noetic-geographic-msgs \ 11 | ros-noetic-dwa-local-planner \ 12 | ros-noetic-rtabmap-ros \ 13 | ros-noetic-mavlink \ 14 | ros-noetic-gazebo-ros \ 15 | ros-noetic-control-toolbox \ 16 | ros-noetic-move-base \ 17 | ros-noetic-xacro \ 18 | ros-noetic-robot-state-publisher \ 19 | libgeographic-dev \ 20 | geographiclib-tools \ 21 | libgstreamer1.0-dev \ 22 | libgstreamer-plugins-base1.0-dev \ 23 | python3-pip \ 24 | git -y 25 | 26 | RUN pip3 install numpy toml packaging jinja2 empy pyros-genmsg 27 | 28 | RUN rm /bin/sh && ln -s /bin/bash /bin/sh 29 | 30 | WORKDIR /root/ 31 | 32 | RUN git clone https://github.com/PX4/PX4-Autopilot.git &&\ 33 | cd PX4-Autopilot && \ 34 | git checkout v1.12.3 && \ 35 | git submodule update --init --recursive && \ 36 | HEADLESS=1 make -j8 px4_sitl_default gazebo && \ 37 | make px4_sitl_default install && \ 38 | cd && \ 39 | rm -rf PX4-Autopilot 40 | 41 | # To work with PX4/Firmware 1.12.3, mavros 1.8.0 or 1.9.0 releases should be used 42 | # (With mavros master branch there are a lot of "Detected jump back in time" TF errors) 43 | RUN source /ros_entrypoint.sh && \ 44 | mkdir -p ~/catkin_ws/src && \ 45 | cd ~/catkin_ws/src && \ 46 | catkin_init_workspace && \ 47 | git clone https://github.com/mavlink/mavros.git && \ 48 | git clone https://github.com/SyrianSpock/realsense_gazebo_plugin.git && \ 49 | echo "install(DIRECTORY models DESTINATION \${CATKIN_PACKAGE_SHARE_DESTINATION})" >> realsense_gazebo_plugin/CMakeLists.txt && \ 50 | cd mavros && \ 51 | git checkout 1.9.0 && \ 52 | ./mavros/scripts/install_geographiclib_datasets.sh && \ 53 | cd ~/catkin_ws && \ 54 | catkin_make -j8 -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \ 55 | cd && \ 56 | rm -rf ~/catkin_ws/devel ~/catkin_ws/build ~/catkin_ws/src/mavros ~/catkin_ws/src/realsense_gazebo_plugin 57 | 58 | COPY . /root/catkin_ws/src/rtabmap_drone_example 59 | 60 | RUN source /ros_entrypoint.sh && \ 61 | cd ~/catkin_ws && \ 62 | apt update && \ 63 | rosdep update && \ 64 | rosdep install -y -i --from-paths src/rtabmap_drone_example && \ 65 | catkin_make -j8 -DCMAKE_INSTALL_PREFIX=/opt/ros/noetic install && \ 66 | cd && \ 67 | rm -rf ~/catkin_ws 68 | 69 | # nvidia-container-runtime 70 | ENV NVIDIA_VISIBLE_DEVICES \ 71 | ${NVIDIA_VISIBLE_DEVICES:-all} 72 | ENV NVIDIA_DRIVER_CAPABILITIES \ 73 | ${NVIDIA_DRIVER_CAPABILITIES:+$NVIDIA_DRIVER_CAPABILITIES,}graphics 74 | 75 | RUN echo "#!/bin/bash" >> /entrypoint.sh && \ 76 | echo "set -e" >> /entrypoint.sh && \ 77 | echo "source /opt/ros/noetic/setup.bash" >> /entrypoint.sh && \ 78 | echo "source /usr/local/px4/Tools/setup_gazebo.bash /usr/local/px4 /usr/local/px4/build/px4_sitl_default" >> /entrypoint.sh && \ 79 | echo "export ROS_PACKAGE_PATH=\$ROS_PACKAGE_PATH:/usr/local/px4:/usr/local/px4/Tools/sitl_gazebo" >> /entrypoint.sh && \ 80 | echo "export GAZEBO_MODEL_PATH=\$GAZEBO_MODEL_PATH:/opt/ros/noetic/share/rtabmap_drone_example/models" >> /entrypoint.sh && \ 81 | echo "export GAZEBO_MODEL_PATH=\$GAZEBO_MODEL_PATH:/opt/ros/noetic/share/realsense_gazebo_plugin/models" >> /entrypoint.sh && \ 82 | echo "exec \"\$@\"" >> /entrypoint.sh && \ 83 | chmod +x /entrypoint.sh 84 | 85 | ENTRYPOINT ["/entrypoint.sh"] 86 | CMD ["bash"] 87 | -------------------------------------------------------------------------------- /launch/gazebo.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 | -------------------------------------------------------------------------------- /launch/rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /launch/slam.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 | -------------------------------------------------------------------------------- /launch/spawn_drone.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 | 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 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /models/apt/meshes/texture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matlabbe/rtabmap_drone_example/8cd48184ae3481628a939659f69f2c57f4e01d26/models/apt/meshes/texture.jpg -------------------------------------------------------------------------------- /models/apt/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | apt 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Mathieu Labbe 10 | mathieu.m.labbe@usherbrooke.ca 11 | 12 | 13 | 14 | apt 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/apt/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 0 0 0 5 | true 6 | 7 | 8 | 9 | model://apt/meshes/apt.dae 10 | 11 | 12 | 13 | 14 | 15 | model://apt/meshes/apt.dae 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /models/room/meshes/room.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/matlabbe/rtabmap_drone_example/8cd48184ae3481628a939659f69f2c57f4e01d26/models/room/meshes/room.jpg -------------------------------------------------------------------------------- /models/room/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | room 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Mathieu Labbe 10 | mathieu.m.labbe@usherbrooke.ca 11 | 12 | 13 | 14 | Room 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/room/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 0 0 0 0 0 0 5 | true 6 | 7 | 8 | 9 | model://room/meshes/room.dae 10 | 11 | 12 | 13 | 14 | 15 | model://room/meshes/room.dae 16 | 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | rtabmap_drone_example 4 | 0.0.0 5 | The rtabmap_drone_example package 6 | 7 | 8 | 9 | 10 | matlabbe 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 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | mavros 53 | move_base 54 | roscpp 55 | rospy 56 | rtabmap_ros 57 | std_msgs 58 | geometry_msgs 59 | tf 60 | 61 | mavros 62 | move_base 63 | roscpp 64 | rospy 65 | rtabmap_ros 66 | std_msgs 67 | geometry_msgs 68 | tf 69 | 70 | gazebo_plugins 71 | gazebo_ros 72 | gazebo_ros_control 73 | xacro 74 | 75 | 76 | 77 | 78 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /param/base_local_planner_params.yaml: -------------------------------------------------------------------------------- 1 | DWAPlannerROS: 2 | 3 | global_frame_id: odom 4 | 5 | # Robot Configuration Parameters 6 | max_vel_x: 0.2 7 | min_vel_x: -0.01 8 | 9 | max_vel_y: 0.1 10 | min_vel_y: -0.1 11 | 12 | # The velocity when robot is moving in a straight line 13 | max_vel_trans: 0.2 14 | min_vel_trans: 0.01 15 | 16 | max_vel_theta: 1 17 | min_vel_theta: 0 18 | 19 | acc_lim_x: 1.5 20 | acc_lim_y: 1.5 21 | acc_lim_theta: 3 22 | acc_lim_trans: 1.5 23 | 24 | # Goal Tolerance Parametes 25 | xy_goal_tolerance: 0.3 26 | yaw_goal_tolerance: 0.5 27 | latch_xy_goal_tolerance: false 28 | 29 | trans_stopped_vel: 0.01 30 | theta_stopped_vel: 0.01 31 | 32 | # Forward Simulation Parameters 33 | sim_time: 4 34 | vx_samples: 6 35 | vy_samples: 6 36 | vth_samples: 20 37 | 38 | # Trajectory Scoring Parameters 39 | path_distance_bias: 64 40 | goal_distance_bias: 10 41 | occdist_scale: 0.08 42 | forward_point_distance: 0.1 43 | stop_time_buffer: 0.2 44 | scaling_speed: 0.25 45 | max_scaling_factor: 0.2 46 | 47 | # Oscillation Prevention Parameters 48 | oscillation_reset_dist: 0.05 49 | 50 | # Debugging 51 | publish_traj_pc : true 52 | publish_cost_grid_pc: true 53 | 54 | controller_frequency: 10.0 55 | planner_frequency: 1 56 | 57 | NavfnROS: 58 | allow_unknown: true 59 | -------------------------------------------------------------------------------- /param/costmap_common_params.yaml: -------------------------------------------------------------------------------- 1 | obstacle_range: 3.0 2 | raytrace_range: 3.5 3 | footprint: [[0.16,0.16], [-0.16,0.16], [-0.16,-0.16], [0.16,-0.16]] 4 | robot_radius: 0.32 5 | cost_scaling_factor: 1.5 6 | inflation_radius: 1.5 7 | transform_tolerance: 1 8 | 9 | controller_patience: 2.0 10 | -------------------------------------------------------------------------------- /param/global_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | global_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | update_frequency: 1 5 | publish_frequency: 1 6 | static_map: true 7 | footprint_padding: 0.05 8 | 9 | inflation_layer: 10 | inflate_unkown: true 11 | 12 | plugins: 13 | - {name: static_layer, type: "rtabmap_costmap_plugins::StaticLayer"} 14 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 15 | -------------------------------------------------------------------------------- /param/local_costmap_params.yaml: -------------------------------------------------------------------------------- 1 | local_costmap: 2 | global_frame: map 3 | robot_base_frame: base_link 4 | 5 | update_frequency: 5.0 6 | publish_frequency: 2.0 7 | 8 | static_map: false 9 | rolling_window: true 10 | 11 | width: 4 12 | height: 4 13 | resolution: 0.05 14 | transform_tolerance: 0.5 15 | 16 | footprint_padding: 0.05 17 | 18 | plugins: 19 | - {name: voxel_layer, type: "rtabmap_costmap_plugins::VoxelLayer"} 20 | - {name: inflation_layer, type: "costmap_2d::InflationLayer"} 21 | 22 | voxel_layer: 23 | enabled: true 24 | publish_voxel_map: true 25 | observation_sources: 3d_cloud 26 | 3d_cloud: 27 | data_type: PointCloud2 28 | topic: /camera_cloud 29 | marking: true 30 | clearing: true 31 | min_obstacle_height: -1000 32 | max_obstacle_height: 1000 33 | z_voxels: 9 34 | z_resolution: 0.05 35 | footprint_clearing_enabled: false 36 | -------------------------------------------------------------------------------- /scripts/rotor_tf.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import tf 4 | from mavros_msgs.msg import RCOut 5 | 6 | def callback(msg): 7 | global br 8 | global scale 9 | global angle 10 | global s13 11 | global s2 12 | global s22 13 | global s023 14 | br.sendTransform( 15 | (s13, -s2, s023), 16 | tf.transformations.quaternion_from_euler(0,0,angle), 17 | msg.header.stamp, 18 | "rotor_0", 19 | "base_link") 20 | br.sendTransform( 21 | (-s13, s22, s023), 22 | tf.transformations.quaternion_from_euler(0,0,angle), 23 | msg.header.stamp, 24 | "rotor_1", 25 | "base_link") 26 | br.sendTransform( 27 | (s13, s22, s023), 28 | tf.transformations.quaternion_from_euler(0,0,angle), 29 | msg.header.stamp, 30 | "rotor_2", 31 | "base_link") 32 | br.sendTransform( 33 | (-s13, -s2, s023), 34 | tf.transformations.quaternion_from_euler(0,0,angle), 35 | msg.header.stamp, 36 | "rotor_3", 37 | "base_link") 38 | if msg.channels[0] > 910: 39 | angle = angle + msg.channels[0]/1000 40 | 41 | if __name__ == "__main__": 42 | 43 | rospy.init_node("point_to_tf", anonymous=True) 44 | 45 | scale = rospy.get_param('~scale', 1.0) 46 | 47 | s13 = 0.13*scale 48 | s2 = 0.2*scale 49 | s22 = 0.22*scale 50 | s023 = 0.023*scale 51 | angle = 0 52 | 53 | br = tf.TransformBroadcaster() 54 | rospy.Subscriber("/mavros/rc/out", RCOut, callback, queue_size=1) 55 | rospy.spin() 56 | -------------------------------------------------------------------------------- /src/offboard_node.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * @file offb_node.cpp 3 | * @brief Offboard control example node, written with MAVROS version 0.19.x, PX4 Pro Flight 4 | * Stack and tested in Gazebo SITL 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | /* 19 | uint16 IGNORE_PX=1 20 | uint16 IGNORE_PY=2 21 | uint16 IGNORE_PZ=4 22 | uint16 IGNORE_VX=8 23 | uint16 IGNORE_VY=16 24 | uint16 IGNORE_VZ=32 25 | uint16 IGNORE_AFX=64 26 | uint16 IGNORE_AFY=128 27 | uint16 IGNORE_AFZ=256 28 | uint16 FORCE=512 29 | uint16 IGNORE_YAW=1024 30 | uint16 IGNORE_YAW_RATE=2048 31 | */ 32 | #define VELOCITY2D_CONTROL 0b011111000011 33 | #define VELOCITY_CONTROL 0b011111000111 34 | #define POSITION_CONTROL 0b101111111000 35 | unsigned short velocity_mask = VELOCITY2D_CONTROL; 36 | 37 | mavros_msgs::PositionTarget current_goal; 38 | ros::Time lastTwistReceived; 39 | 40 | mavros_msgs::State current_state; 41 | void state_cb(const mavros_msgs::State::ConstPtr& msg){ 42 | current_state = *msg; 43 | } 44 | 45 | void twist_cb(const geometry_msgs::Twist::ConstPtr& msg){ 46 | if(current_goal.type_mask == POSITION_CONTROL) 47 | { 48 | ROS_INFO("Switch to velocity control"); 49 | } 50 | current_goal.coordinate_frame = mavros_msgs::PositionTarget::FRAME_BODY_NED; 51 | current_goal.type_mask = velocity_mask; 52 | current_goal.velocity.x = msg->linear.x; 53 | current_goal.velocity.y = msg->linear.y; 54 | current_goal.velocity.z = velocity_mask == VELOCITY2D_CONTROL?0:msg->linear.z; 55 | current_goal.position.z = 1.5; 56 | current_goal.yaw_rate = msg->angular.z; 57 | current_goal.yaw_rate = msg->angular.z; 58 | lastTwistReceived = ros::Time::now(); 59 | } 60 | 61 | void joy_cb(const sensor_msgs::Joy::ConstPtr& msg){ 62 | if(msg->buttons[5] == 1) 63 | { 64 | // When holding right trigger, accept velocity in Z 65 | velocity_mask = VELOCITY_CONTROL; 66 | } 67 | else 68 | { 69 | velocity_mask = VELOCITY2D_CONTROL; 70 | } 71 | } 72 | 73 | int main(int argc, char **argv) 74 | { 75 | ros::init(argc, argv, "offboard_node"); 76 | ros::NodeHandle nh; 77 | 78 | ros::Subscriber state_sub = nh.subscribe 79 | ("mavros/state", 10, state_cb); 80 | ros::Publisher local_pos_pub = nh.advertise 81 | ("mavros/setpoint_raw/local", 1); 82 | ros::Publisher vision_pos_pub = nh.advertise 83 | ("mavros/vision_pose/pose", 1); 84 | ros::ServiceClient arming_client = nh.serviceClient 85 | ("mavros/cmd/arming"); 86 | ros::ServiceClient command_client = nh.serviceClient 87 | ("mavros/cmd/command"); 88 | ros::ServiceClient set_mode_client = nh.serviceClient 89 | ("mavros/set_mode"); 90 | ros::Subscriber twist_sub = nh.subscribe 91 | ("/cmd_vel", 1, twist_cb); 92 | ros::Subscriber joy_sub = nh.subscribe 93 | ("/joy", 1, joy_cb); 94 | 95 | //the setpoint publishing rate MUST be faster than 2Hz 96 | ros::Rate rate(50.0); 97 | 98 | // wait for FCU connection 99 | while(ros::ok() && !current_state.connected){ 100 | ros::spinOnce(); 101 | rate.sleep(); 102 | } 103 | 104 | mavros_msgs::SetMode offb_set_mode; 105 | offb_set_mode.request.custom_mode = "OFFBOARD"; 106 | 107 | mavros_msgs::CommandBool arm_cmd; 108 | arm_cmd.request.value = true; 109 | 110 | mavros_msgs::CommandLong disarm_cmd; 111 | disarm_cmd.request.broadcast = false; 112 | disarm_cmd.request.command = 400; 113 | //disarm_cmd.request.param2 = 21196; // Kill no check landed 114 | 115 | ros::Time last_request = ros::Time::now(); 116 | lastTwistReceived = ros::Time::now(); 117 | 118 | tf::TransformListener listener; 119 | 120 | ROS_INFO("Setting offboard mode... (5 seconds)"); 121 | ros::spinOnce(); 122 | if(!listener.waitForTransform("/map", "/base_link", ros::Time(0), ros::Duration(5))) 123 | { 124 | ROS_ERROR("Cannot get current position between /map and /base_link"); 125 | return -1; 126 | } 127 | 128 | try{ 129 | tf::StampedTransform visionPoseTf; 130 | listener.lookupTransform("/map", "/base_link", ros::Time(0), visionPoseTf); 131 | 132 | //update currentPose 133 | current_goal.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED; 134 | current_goal.type_mask = POSITION_CONTROL; 135 | current_goal.position.x = visionPoseTf.getOrigin().x(); 136 | current_goal.position.y = visionPoseTf.getOrigin().y(); 137 | current_goal.position.z = 1.5; 138 | current_goal.yaw = tf::getYaw(visionPoseTf.getRotation()); 139 | current_goal.velocity.x = 0; 140 | current_goal.velocity.y = 0; 141 | current_goal.velocity.z = 0; 142 | current_goal.yaw_rate = 0; 143 | current_goal.acceleration_or_force.x = 0; 144 | current_goal.acceleration_or_force.y = 0; 145 | current_goal.acceleration_or_force.z = 0; 146 | ROS_INFO("Initial position=(%f,%f,%f) yaw=%f", 147 | current_goal.position.x, 148 | current_goal.position.y, 149 | visionPoseTf.getOrigin().z(), 150 | current_goal.yaw); 151 | } 152 | catch (tf::TransformException & ex){ 153 | ROS_ERROR("%s",ex.what()); 154 | return -1; 155 | } 156 | //send a few setpoints before starting 157 | for(int i = 100; ros::ok() && i > 0; --i){ 158 | local_pos_pub.publish(current_goal); 159 | ros::spinOnce(); 160 | rate.sleep(); 161 | } 162 | 163 | geometry_msgs::PoseStamped current_pose; 164 | current_pose.header.frame_id = "map"; 165 | 166 | while(ros::ok()){ 167 | tf::StampedTransform visionPoseTf; 168 | try{ 169 | listener.lookupTransform("/map", "/base_link", ros::Time(0), visionPoseTf); 170 | 171 | //update currentPose 172 | current_pose.pose.position.x = visionPoseTf.getOrigin().x(); 173 | current_pose.pose.position.y = visionPoseTf.getOrigin().y(); 174 | current_pose.pose.position.z = visionPoseTf.getOrigin().z(); 175 | current_pose.pose.orientation.x = visionPoseTf.getRotation().x(); 176 | current_pose.pose.orientation.y = visionPoseTf.getRotation().y(); 177 | current_pose.pose.orientation.z = visionPoseTf.getRotation().z(); 178 | current_pose.pose.orientation.w = visionPoseTf.getRotation().w(); 179 | } 180 | catch (tf::TransformException & ex){ 181 | ROS_ERROR("%s",ex.what()); 182 | } 183 | 184 | if( current_state.mode != "OFFBOARD" && 185 | (ros::Time::now() - last_request > ros::Duration(5.0))){ 186 | if( set_mode_client.call(offb_set_mode) && 187 | offb_set_mode.response.mode_sent){ 188 | ROS_INFO("Offboard enabled"); 189 | ROS_INFO("Vehicle arming... (5 seconds)"); 190 | } 191 | last_request = ros::Time::now(); 192 | } else { 193 | if( !current_state.armed && 194 | !(current_goal.velocity.z < -0.4 && current_goal.yaw_rate < -0.4) && // left joystick down-right 195 | (ros::Time::now() - last_request > ros::Duration(5.0))){ 196 | if( arming_client.call(arm_cmd) && 197 | arm_cmd.response.success){ 198 | ROS_INFO("Vehicle armed"); 199 | ROS_INFO("Take off at 1.5 meter... to position=(%f,%f,%f) yaw=%f", 200 | current_goal.position.x, 201 | current_goal.position.y, 202 | current_goal.position.z, 203 | current_goal.yaw); 204 | } 205 | last_request = ros::Time::now(); 206 | } 207 | else if(current_goal.velocity.z < -0.4 && current_goal.yaw_rate < -0.4 && // left joystick down-right 208 | (ros::Time::now() - last_request > ros::Duration(5.0))){ 209 | if( command_client.call(disarm_cmd) && 210 | disarm_cmd.response.success){ 211 | ROS_INFO("Vehicle disarmed"); 212 | ros::shutdown(); 213 | } 214 | else 215 | { 216 | ROS_INFO("Disarming failed! Still in flight?"); 217 | } 218 | last_request = ros::Time::now(); 219 | } 220 | } 221 | 222 | current_goal.header.stamp = ros::Time::now(); 223 | 224 | if(current_goal.header.stamp.toSec() - lastTwistReceived.toSec() > 1 and current_goal.type_mask != POSITION_CONTROL) 225 | { 226 | //switch to position mode with last position 227 | 228 | current_goal.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED; 229 | current_goal.type_mask = POSITION_CONTROL; 230 | current_goal.position.x = current_pose.pose.position.x; 231 | current_goal.position.y = current_pose.pose.position.y; 232 | current_goal.position.z = 1.5; 233 | tfScalar yaw, pitch, roll; 234 | tf::Matrix3x3 mat(tf::Quaternion(current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w)); 235 | mat.getEulerYPR(yaw, pitch, roll); 236 | current_goal.yaw = yaw; 237 | ROS_INFO("Switch to position control (x=%f, y=%f, z=%f, yaw=%f)", 238 | current_goal.position.x, current_goal.position.y, current_goal.position.z, current_goal.yaw); 239 | } 240 | 241 | current_pose.header.stamp = current_goal.header.stamp; 242 | local_pos_pub.publish(current_goal); 243 | 244 | // Vision pose should be published at a steady 245 | // frame rate so that EKF from px4 stays stable 246 | vision_pos_pub.publish(current_pose); 247 | 248 | ros::spinOnce(); 249 | rate.sleep(); 250 | } 251 | 252 | return 0; 253 | } 254 | 255 | -------------------------------------------------------------------------------- /urdf/drone.gazebo: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | base_link 7 | 10 8 | 9 | 10 | 11 | rotor_0_joint 12 | rotor_0 13 | ccw 14 | 0.0125 15 | 0.025 16 | 1100 17 | 8.54858e-06 18 | 0.06 19 | /gazebo/command/motor_speed 20 | 0 21 | 0.000175 22 | 1e-06 23 | /motor_speed/0 24 | 10 25 | 26 | 27 | 28 | rotor_1_joint 29 | rotor_1 30 | ccw 31 | 0.0125 32 | 0.025 33 | 1100 34 | 8.54858e-06 35 | 0.06 36 | /gazebo/command/motor_speed 37 | 1 38 | 0.000175 39 | 1e-06 40 | /motor_speed/1 41 | 10 42 | 43 | 44 | 45 | rotor_2_joint 46 | rotor_2 47 | cw 48 | 0.0125 49 | 0.025 50 | 1100 51 | 8.54858e-06 52 | 0.06 53 | /gazebo/command/motor_speed 54 | 2 55 | 0.000175 56 | 1e-06 57 | /motor_speed/2 58 | 10 59 | 60 | 61 | 62 | rotor_3_joint 63 | rotor_3 64 | cw 65 | 0.0125 66 | 0.025 67 | 1100 68 | 8.54858e-06 69 | 0.06 70 | /gazebo/command/motor_speed 71 | 3 72 | 0.000175 73 | 1e-06 74 | /motor_speed/3 75 | 10 76 | 77 | 78 | 79 | 80 | 81 | 82 | 100 83 | 0.0004 84 | 6.4e-06 85 | 600 86 | /mag 87 | 88 | 89 | 90 | 50 91 | /baro 92 | 0 93 | 94 | 95 | 96 | /imu 97 | /mag 98 | /baro 99 | INADDR_ANY 100 | 14560 101 | 4560 102 | 0 103 | /dev/ttyACM0 104 | 921600 105 | INADDR_ANY 106 | 14550 107 | INADDR_ANY 108 | 14540 109 | 0 110 | 0 111 | 0 112 | 1 113 | 0 114 | 1 115 | 1 116 | /gazebo/command/motor_speed 117 | 118 | 119 | 0 120 | 0 121 | 1000 122 | 0 123 | 100 124 | velocity 125 | 126 | 127 | 1 128 | 0 129 | 1000 130 | 0 131 | 100 132 | velocity 133 | 134 | 135 | 2 136 | 0 137 | 1000 138 | 0 139 | 100 140 | velocity 141 | 142 | 143 | 3 144 | 0 145 | 1000 146 | 0 147 | 100 148 | velocity 149 | 150 | 151 | 4 152 | 1 153 | 324.6 154 | 0 155 | 0 156 | velocity 157 | 158 |

0.1

159 | 0 160 | 0 161 | 0.0 162 | 0.0 163 | 2 164 | -2 165 | 166 | zephyr_delta_wing::propeller_joint 167 | 168 | 169 | 5 170 | 0 171 | 0.524 172 | 0 173 | 0 174 | position 175 | zephyr_delta_wing::flap_left_joint 176 | 177 |

10.0

178 | 0 179 | 0 180 | 0 181 | 0 182 | 20 183 | -20 184 | 185 | 186 | 187 | 6 188 | 0 189 | 0.524 190 | 0 191 | 0 192 | position 193 | zephyr_delta_wing::flap_right_joint 194 | 195 |

10.0

196 | 0 197 | 0 198 | 0 199 | 0 200 | 20 201 | -20 202 | 203 | 204 | 205 | 7 206 | 0 207 | 0.524 208 | 0 209 | 0 210 | position 211 | 212 | 213 | 214 | 0 215 | 216 | 217 | imu_link 218 | /imu 219 | 0.00018665 220 | 3.8785e-05 221 | 1000.0 222 | 0.0087 223 | 0.00186 224 | 0.006 225 | 300.0 226 | 0.196 227 | 228 | 229 | 230 | -------------------------------------------------------------------------------- /urdf/drone.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 47 | 48 | 1 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 103 | 104 | 1 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 148 | 149 | 1 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 193 | 194 | 1 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 238 | 239 | 1 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 263 | 264 | 265 | 266 | 267 | 268 | body_imu/data_raw 269 | imu_body_link 270 | 100 271 | 0.01 272 | 0 0 0 273 | 0 0 0 274 | imu_body_link 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | -------------------------------------------------------------------------------- /urdf/realsense-RS200.macro.xacro: -------------------------------------------------------------------------------- 1 | 2 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 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 | 0 93 | 0 94 | 0 95 | 1 96 | 97 | 1 98 | 0 0 0 99 | 101 | 1e+13 102 | 1 103 | 105 | 106 | 107 | 0 -0.046 0.004 0 0 0 108 | 109 | 2 110 | 111 | 640 112 | 480 113 | RGB_INT8 114 | 115 | 116 | 0.1 117 | 100 118 | 119 | 120 | gaussian 121 | 0.0 122 | 0.007 123 | 124 | 125 | 1 126 | 60 127 | 1 128 | 129 | 130 | 0 -0.06 0.004 0 0 0 131 | 132 | 2 133 | 134 | 640 135 | 480 136 | L_INT8 137 | 138 | 139 | 0.1 140 | 100 141 | 142 | 143 | gaussian 144 | 0.0 145 | 0.05 146 | 147 | 148 | 1 149 | 60 150 | 0 151 | 152 | 153 | 0 0.01 0.004 0 0 0 154 | 155 | 2 156 | 157 | 640 158 | 480 159 | L_INT8 160 | 161 | 162 | 0.1 163 | 100 164 | 165 | 166 | gaussian 167 | 0.0 168 | 0.05 169 | 170 | 171 | 1 172 | 60 173 | 0 174 | 175 | 176 | 0 -0.03 0.004 0 0 0 177 | 178 | 2 179 | 180 | 640 181 | 480 182 | 183 | 184 | 0.1 185 | 100 186 | 187 | 188 | gaussian 189 | 0.0 190 | 0.100 191 | 192 | 193 | 1 194 | 60 195 | 0 196 | 197 | 198 | 199 | 200 | 201 | -------------------------------------------------------------------------------- /worlds/apt.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1 1 1 1 6 | 0 0 0 1 7 | false 8 | false 9 | 10 | 11 | model://sun 12 | 13 | 14 | model://apt 15 | -0.8 0 1.12 0 0 0 16 | 17 | 18 | 19 | true 20 | 21 | 22 | 0 0 -0.025 0 0 0 23 | 24 | 25 | 1 1 0.05 26 | 27 | 28 | 29 | 30 | 31 | 100 32 | 50 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /worlds/room.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 1 1 1 1 6 | 0 0 0 1 7 | false 8 | false 9 | 10 | 11 | model://sun 12 | 13 | 14 | model://room 15 | 0.5 -2.5 1.52 0 0 1.15 16 | 17 | 18 | 19 | true 20 | 21 | 22 | 0 0 -0.025 0 0 0 23 | 24 | 25 | 1 1 0.05 26 | 27 | 28 | 29 | 30 | 31 | 100 32 | 50 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | --------------------------------------------------------------------------------