Commit 02e77e73 authored by Stephen Rees's avatar Stephen Rees
Browse files

Update sitl_gazebo to upstream master

parents 85f0b13c 9353834b
Build/
.DS_Store
*~
models/iris/*.sdf
*.pyc
cmake_minimum_required(VERSION 2.8.8 FATAL_ERROR)
# Add search directory for CMake on OS X
list(APPEND CMAKE_MODULE_PATH /usr/local/share/cmake/Modules)
find_package( PkgConfig REQUIRED)
find_package(OpenCV REQUIRED)
find_package(gazebo REQUIRED)
pkg_check_modules( EIGEN3 REQUIRED eigen3)
if(${GAZEBO_VERSION} VERSION_LESS "6.0")
message(FATAL_ERROR "You need at least Gazebo 6.0. Your version: ${GAZEBO_VERSION}")
endif()
find_package(Eigen3 QUIET)
if(NOT EIGEN3_FOUND)
# Fallback to cmake_modules
find_package(Eigen QUIET)
if(NOT EIGEN_FOUND)
pkg_check_modules( EIGEN3 REQUIRED eigen3)
else()
set(EIGEN3_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS})
set(EIGEN3_LIBRARIES ${EIGEN_LIBRARIES})
endif()
else()
set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR})
endif()
pkg_check_modules( protobuf REQUIRED protobuf )
FIND_PACKAGE( Boost 1.40 COMPONENTS system thread REQUIRED )
pkg_check_modules(GAZEBO REQUIRED gazebo>=6.0)
pkg_check_modules(SDF REQUIRED sdformat)
add_definitions(-std=c++11)
set(GAZEBO_MSG_INCLUDE_DIRS)
......@@ -22,13 +40,16 @@ include_directories(
include
${GAZEBO_INCLUDE_DIRS}
${GAZEBO_MSG_INCLUDE_DIRS}
${SDF_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIRS}
${OpenCV_INCLUDE_DIRS}
${CMAKE_CURRENT_BINARY_DIR}/msgs
${Boost_INCLUDE_DIR}
# Hack to work around pkg_check_modules not setting the Eigen dir
# Workaround for Eigen3
${Boost_INCLUDE_DIR}/eigen3
${EIGEN3_INCLUDE_DIRS}/eigen3
# Workaround for OGRE include dirs on Mac OS
/usr/local/include/OGRE
/usr/local/include/OGRE/Paging
)
set(enable_mavlink_interface "true")
......@@ -58,31 +79,29 @@ target_link_libraries(rotors_gazebo_controller_interface ${GAZEBO_LIBRARIES} ${B
add_dependencies(rotors_gazebo_controller_interface mav_msgs)
add_library(rotors_gazebo_motor_model SHARED src/gazebo_motor_model.cpp)
target_link_libraries(rotors_gazebo_motor_model mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
target_link_libraries(rotors_gazebo_motor_model mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
add_dependencies(rotors_gazebo_motor_model mav_msgs)
add_library(rotors_gazebo_multirotor_base_plugin SHARED src/gazebo_multirotor_base_plugin.cpp)
target_link_libraries(rotors_gazebo_multirotor_base_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
target_link_libraries(rotors_gazebo_multirotor_base_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
add_dependencies(rotors_gazebo_multirotor_base_plugin mav_msgs)
add_library(rotors_gazebo_imu_plugin SHARED src/gazebo_imu_plugin.cpp)
target_link_libraries(rotors_gazebo_imu_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
target_link_libraries(rotors_gazebo_imu_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
add_dependencies(rotors_gazebo_imu_plugin mav_msgs)
add_library(gazebo_opticalFlow_plugin SHARED src/gazebo_opticalFlow_plugin.cpp)
target_link_libraries(gazebo_opticalFlow_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${OpenCV_LIBS})
target_link_libraries(gazebo_opticalFlow_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${OpenCV_LIBS})
add_dependencies(gazebo_opticalFlow_plugin mav_msgs)
add_library(gazebo_lidar_plugin SHARED src/gazebo_lidar_plugin.cpp)
target_link_libraries(gazebo_lidar_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
target_link_libraries(gazebo_lidar_plugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
add_dependencies(gazebo_lidar_plugin mav_msgs)
add_library(rotors_gazebo_mavlink_interface SHARED src/gazebo_mavlink_interface.cpp src/geo_mag_declination.cpp)
target_link_libraries(rotors_gazebo_mavlink_interface mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
target_link_libraries(rotors_gazebo_mavlink_interface mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
add_dependencies(rotors_gazebo_mavlink_interface mav_msgs)
if(${CMAKE_HOST_SYSTEM_NAME} STREQUAL "Darwin")
add_library(LiftDragPlugin SHARED src/liftdrag_plugin/liftdrag_plugin.cpp)
target_link_libraries(LiftDragPlugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${SDF_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
target_link_libraries(LiftDragPlugin mav_msgs ${Boost_LIBRARIES} ${GAZEBO_LIBRARIES} ${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE})
add_dependencies(LiftDragPlugin mav_msgs)
endif()
# Gazebo for PX4 SITL
# Gazebo for MAVLink SITL
This is a flight simulator for multirotors, VTOL and fixed wing.
NOTE: This repository is in the process of being re-integrated into RotorS: https://github.com/ethz-asl/rotors_simulator
## Install Gazebo Simulator
Follow instructions on the [official site](http://gazebosim.org/tutorials?cat=install) to install Gazebo. Mac OS users should install Gazebo 7, Linux users Gazebo 6. Failing to install the right version can render the simulation inoperational.
Follow instructions on the [official site](http://gazebosim.org/tutorials?cat=install) to install Gazebo. Mac OS users should install Gazebo 7, Linux users PGazebo 6. Failing to install the right version can render the simulation inoperational.
## Protobuf
......@@ -31,7 +33,7 @@ Clone the gazebo plugins repository to your computer. IMPORTANT: If you do not c
```bash
mkdir -p ~/src
cd src
git clone https://github.com/PX4/sitl_gazebo.git
git clone https://github.com/Dronecode/sitl_gazebo.git
```
Create a build folder in the top level of your repository:
......@@ -83,4 +85,4 @@ Gazebo will now launch when typing 'gazebo' on the shell:
gazebo
```
Start the PX4 SITL executable as documented [in the PX4 SITL documentation](http://dev.px4.io/simulation-gazebo.html). Then insert the IRIS model from the **insert** tab. This should trigger the communication with the PX4 SITL app.
Please refer to the documentation of the particular flight stack how to run it against this framework.
......@@ -163,6 +163,7 @@ class GazeboMavlinkInterface : public ModelPlugin {
common::Time last_time_;
common::Time last_gps_time_;
common::Time last_actuator_time_;
double gps_update_interval_;
double lat_rad;
double lon_rad;
......
......@@ -25,8 +25,8 @@
<camera>
<horizontal_fov>0.6</horizontal_fov>
<image>
<width>64</width>
<height>64</height>
<width>256</width>
<height>256</height>
</image>
<clip>
<near>0.1</near>
......@@ -35,12 +35,12 @@
<noise>
<type>gaussian</type>
<mean>0.0</mean>
<stddev>0.005</stddev>
<stddev>0.001</stddev>
</noise>
</camera>
<plugin name="CameraPlugin" filename="libgazebo_opticalFlow_plugin.so"/>
<always_on>1</always_on>
<update_rate>10</update_rate>
<update_rate>30</update_rate>
<visualize>false</visualize>
</sensor>
</link>
......
......@@ -34,8 +34,8 @@
</horizontal>
</scan>
<range>
<min>0.09</min>
<max>15</max>
<min>0.06</min>
<max>25</max>
<resolution>0.01</resolution>
</range>
<noise>
......@@ -46,7 +46,7 @@
</ray>
<plugin name="LaserPlugin" filename="libgazebo_lidar_plugin.so"/>
<always_on>1</always_on>
<update_rate>10</update_rate>
<update_rate>20</update_rate>
<visualize>false</visualize>
</sensor>
</link>
......
This diff is collapsed.
This source diff could not be displayed because it is too large. You can view the blob instead.
This source diff could not be displayed because it is too large. You can view the blob instead.
This diff is collapsed.
<?xml version="1.0" encoding="UTF-8"?>
<COLLADA xmlns="http://www.collada.org/2005/11/COLLADASchema" version="1.4.1">
<asset>
<contributor>
<author>VCGLab</author>
<authoring_tool>VCGLib | MeshLab</authoring_tool>
</contributor>
<up_axis>Y_UP</up_axis>
<created>Fr. Dez 19 16:04:46 2014</created>
<modified>Fr. Dez 19 16:04:46 2014</modified>
</asset>
<library_images/>
<library_materials/>
<library_effects/>
<library_geometries>
<geometry id="shape0-lib" name="shape0">
<mesh>
<source id="shape0-lib-positions" name="position">
<float_array id="shape0-lib-positions-array" count="300">-0.0835714 0.140031 -0.0736456 -0.0400765 0.163115 -0.0483281 -0.117913 0.305442 -0.144312 -0.0703607 0.798102 -0.0199993 -0.0560492 0.545571 -0.0512484 -0.00078476 0.975427 0.00767503 0.0160651 0.594631 -0.00720468 0.0298948 0.965824 0.00334253 0.00751962 0.685387 0.000359752 0.0540712 0.429634 -0.0101518 0.0151478 0.102216 -0.0131658 -0.0556911 -0.0661715 -0.00963317 -0.00174063 -0.0818349 -0.00868353 0.0559242 -0.0649661 -0.00957289 0.0703602 -0.798109 -0.0199979 0.0560498 -0.54557 -0.0512488 0.0750017 -0.975039 -0.00934454 0.000782845 -0.975427 0.00767481 -0.01485 -0.614361 -0.00403874 -0.0298926 -0.965824 0.00334368 -0.0548356 -0.290864 -0.0115746 0.0115154 -0.203636 -0.0264133 0.118578 -0.302999 -0.139409 -0.0121224 -0.120255 -0.0720243 0.0226319 -0.110745 -0.133017 -0.00674525 0.0938006 -0.100237 -0.0231875 -0.121853 -0.0140071 -0.0227039 -0.852975 0.00390995 0.0723746 -0.204958 -0.0813719 0.0406506 -0.549299 -0.0369143 -0.0549052 -0.803689 -0.00777506 -0.0303961 -0.366957 -0.0129591 0.00129643 -0.676714 -0.0029672 0.0229853 0.118236 -0.0121112 -0.0122597 0.134668 -0.0463299 -0.00497452 0.0848861 -0.010146 -0.026551 0.101783 -0.0245763 0.0142935 0.08864 -0.0122476 0.00830604 -0.0268358 -0.0117744 0.00449618 -0.0836288 -0.0193353 0.022756 -0.115931 -0.0296084 -0.00274884 0.0269521 -0.0451194 0.0193773 0.0148156 -0.0105058 -0.0177061 -0.00590916 -0.0110817 -0.00867444 -0.0773042 -0.00921494 -0.0148637 -0.0936643 -0.0989573 -0.00392095 0.206809 -0.0239818 -0.0750017 0.975039 -0.00934454 0.0227405 0.85053 0.00378915 0.0574457 0.791463 -0.00951397 0.000301326 0.377456 -0.019279 -0.025568 0.993204 0.00603233 0.0165912 0.104876 -0.0548642 0.0173898 0.0893329 -0.135902 -0.07848 0.839861 -0.0171015 -0.0410062 0.563133 -0.0362053 -0.104112 0.546217 -0.0949896 0.053552 0.950918 -0.00758994 0.00787323 0.603278 -0.00724069 -0.11474 0.256342 -0.121651 -0.118667 0.439386 -0.118333 0.0562149 0.618315 0.000361391 0.0297782 0.463137 -0.00252085 0.0303766 0.95587 0.0126425 0.0573177 0.951992 -4.06283e-05 0.0582776 0.0699707 -0.000511708 -0.0558582 0.066586 -0.00972389 -0.118004 0.364506 -0.138633 -0.0937259 0.247795 -0.109201 -0.0129714 -0.0875212 -0.000267106 -0.0142037 0.0889641 -0.00118289 -0.058478 0.0641806 -4.29309e-06 0.0787758 -0.813625 -0.0191612 0.118975 -0.372814 -0.139636 0.0368024 -0.992893 0.0026906 -0.0533484 -0.950983 -0.00752904 0.083117 -0.138354 -0.0731762 0.0818296 -0.808715 -0.0116048 -0.0573177 -0.951992 -4.06283e-05 -0.0298975 -0.956894 0.0126223 0.0582653 -0.0732486 -0.00292781 0.112117 -0.483966 -0.11112 -0.0583462 -0.188043 -0.00926188 -0.0578409 -0.0756444 -0.000201417 0.0103472 -0.4132 -0.014694 0.0408722 -1.00118 0.0125076 0.118766 -0.442389 -0.115879 0.0609817 -0.51919 -0.0445129 0.123134 -0.278077 -0.131944 0.0370371 -0.13461 -0.0302756 -0.0511726 -0.503887 -0.000242579 -0.00561221 -0.652544 0.00854968 0.00138781 -0.137464 -0.0204127 0.00564852 0.249971 -0.0118047 0.00393641 0.767754 0.0145574 -0.0408722 1.00118 0.0125076 -0.0603939 0.509636 -0.0446996 -0.0514677 0.26177 -0.0479069 -0.0805473 0.828811 -0.0103958 0.0161138 -0.103875 -0.0375713</float_array>
<technique_common>
<accessor count="100" source="#shape0-lib-positions-array" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<source id="shape0-lib-normals" name="normal">
<float_array id="shape0-lib-normals-array" count="597">-0.133297 0.0211742 -0.99085 0.574718 -0.217697 -0.788864 0.458684 0.116613 -0.880915 0.796763 0.0286666 -0.603612 0.637445 0.1301 -0.759433 0.428682 0.134839 -0.893336 0.274549 0.064973 -0.959376 0.0857926 0.0275753 -0.995931 -0.0247788 0.0121475 -0.999619 0.430487 0.115151 -0.895221 -0.25635 0.0307289 -0.966095 -0.201373 0.00360488 -0.979508 0.0843124 -0.000850521 -0.996439 0.549969 0.0165927 -0.83502 0.516535 0.00940306 -0.856215 0.322449 0.052078 -0.945153 -0.0335466 -0.0296519 -0.998997 0.607426 -0.378598 -0.698353 0.15074 -0.388236 -0.909148 0.000547457 -0.000682649 -1 -0.188206 0.602477 -0.775629 0.00115012 -0.0565694 -0.998398 -0.580676 0.05504 -0.812272 0.131633 -0.0264486 -0.990946 -0.639939 0.164155 -0.750687 -0.600964 -0.131612 -0.788366 -0.455317 -0.134708 -0.88008 -0.272125 -0.0649464 -0.960068 -0.155379 -0.0387502 -0.987095 -0.16364 -0.0431855 -0.985574 0.234548 -0.0304476 -0.971627 0.127333 -0.00736573 -0.991833 0.020128 0.00871479 -0.999759 -0.33958 0.0991923 -0.935332 -0.746178 -0.0489602 -0.663944 -0.547152 -0.0104714 -0.836968 -0.303996 -0.0583859 -0.950883 -0.411912 0.906427 -0.0933727 -0.383743 -0.922565 0.0401973 0.568828 0.682376 -0.459127 0.42064 -0.738002 -0.527651 -0.00175281 0.0309075 -0.999521 -0.536497 0.180328 -0.82441 0.701499 -0.711733 -0.0365385 0.0317392 0.0966715 -0.99481 -0.95107 0.308836 0.00929138 -0.669577 -0.0341059 -0.741959 -0.789851 -0.00640954 -0.613265 -0.399725 -0.118701 -0.908917 -0.277976 -0.051835 -0.959188 -0.0846766 -0.0273322 -0.996034 -0.493359 -0.0784218 -0.866284 0.319854 -0.0156218 -0.947338 0.422089 0.002947 -0.90655 0.242721 -0.0707989 -0.967509 0.127605 -0.0189321 -0.991644 -0.151454 -0.00323371 -0.988459 -0.191947 -0.0052902 -0.981391 0.315678 0.110243 0.94244 -0.296303 0.0100185 -0.955041 -0.287174 -0.0493665 -0.956605 0.959209 -0.281808 0.0224395 0.191154 -0.98156 -0.000713827 -0.556961 -0.820605 -0.128067 -0.030548 -0.025678 -0.999203 0.286646 -0.565131 -0.773603 0.173521 -0.502022 -0.847269 0.873465 0.486328 0.0233435 0.0581331 0.332251 -0.941398 -0.355835 0.533283 -0.767458 -0.528274 0.187115 -0.828199 0.0581217 0.0149521 -0.998198 0.142486 0.121223 -0.982346 -0.0126377 0.0120754 -0.999847 -0.03695 -0.000729376 -0.999317 0.00278682 -0.0257849 -0.999664 0.301704 0.934056 -0.191087 0.26447 0.94675 -0.183633 -0.930448 0.202214 -0.305576 0.441304 0.0138075 -0.897251 0.55055 -0.214323 -0.806821 0.28063 0.0477879 -0.958626 -0.33254 0.0169824 -0.942936 -0.41912 0.000720725 -0.907931 -0.268139 0.0590185 -0.961571 -0.273083 0.0603385 -0.960096 0.0389661 -0.00683833 -0.999217 0.720886 0.127695 -0.681188 0.689043 0.002923 -0.724715 0.648741 -0.135545 -0.748841 0.636136 0.0714873 -0.768258 0.491251 0.0299339 -0.870504 0.0722015 0.0401157 -0.996583 0.261315 0.0552144 -0.963673 -0.93967 -0.286378 -0.187102 0.320575 -0.946574 0.0350669 -0.56586 -0.771852 -0.289907 0.63497 0.705242 -0.315353 -0.935602 0.228627 -0.269033 -0.0473885 -0.994583 0.0925202 -0.0974954 0.977242 -0.188393 -0.596161 0.787386 -0.156894 -0.871918 0.0175052 -0.489339 -0.965453 -0.00139316 -0.260572 0.511024 0.229124 -0.828466 0.654563 0.139928 -0.742945 0.431224 0.120127 -0.894212 0.790089 0.00978929 -0.612914 0.669775 0.0702733 -0.739231 -0.547135 0.0147329 0.836915 -0.97102 0.0385212 -0.235874 0.00566892 0.637056 -0.770796 0.163542 0.0708451 -0.983989 -0.991119 0.0619576 0.117662 -0.147734 -0.0402535 -0.988208 0.170092 -0.000234405 -0.985428 0.973803 -0.00171444 -0.227389 -0.484979 0.870415 0.0846928 -0.377604 0.380711 0.844082 -0.815008 0.22749 -0.532926 0.468273 -0.123234 -0.874948 -0.959682 -0.162511 0.229347 0.416634 -0.24919 -0.874254 -0.998513 -0.0223246 0.0497307 -0.720235 -0.118933 0.683459 0.00420263 0.00958784 0.999945 0.40185 0.635479 0.659305 0.394973 0.861298 -0.319628 0.892965 0.0272271 -0.449302 0.425897 -0.000317519 0.904772 -0.0970638 -0.00194985 0.995276 -0.00807406 0.00452466 0.999957 0.999987 0.00109094 -0.00496858 0.368027 -9.60815e-05 0.929815 0.209665 -0.019518 0.977579 -0.9868 0.116086 -0.112916 0.522797 0.822842 0.222743 0.0406913 -0.0168571 0.99903 -0.00791449 0.00513354 0.999956 0.0236165 0.00535241 0.999707 0.00109216 -0.00140481 0.999998 0.571992 -0.490798 0.657223 0.949872 0.308613 0.0500148 0.945265 0.00542256 -0.326259 -0.543624 0.188326 -0.817928 -0.640792 -0.153441 -0.752224 0.947093 -0.171471 -0.271317 -0.646631 -0.143584 -0.749168 0.54514 -0.0109876 0.838273 0.935718 -0.0431707 -0.350098 0.13002 -0.730264 -0.670679 -0.183519 -0.069504 -0.980556 0.996045 -0.0637611 0.0618805 -0.132039 -0.000143242 -0.991244 0.671823 -0.115333 0.731678 0.697289 0.130325 0.704843 0.928302 -0.185352 -0.322336 0.00951954 0.0126202 0.999875 -0.409606 -0.657436 0.632456 -0.393671 -0.860025 -0.324624 0.0186685 0.000194589 0.999826 -0.998019 -0.000584304 0.0629117 -0.915486 -0.00608856 -0.402304 -0.418349 0.00614631 0.908266 0.807142 -0.192938 -0.55794 0.99471 -0.0358545 -0.096262 -0.634721 0.0901806 -0.767461 -0.475878 -0.759235 -0.443962 -0.88294 -0.010116 -0.469376 -0.993521 -0.00468689 0.113555 0.121164 -0.438756 0.8904 -0.189839 0.502055 -0.843743 0.075519 0.126612 -0.989074 -0.636105 -0.100472 -0.765034 -0.948654 0.135966 -0.285602 0.848919 0.0460372 -0.526514 -0.0104485 -0.0449987 0.998932 0.223937 0.0033933 0.974598 -0.11762 0.0226705 0.9928 0.312214 0.0523961 0.948566 0.441463 0.132655 0.887419 0.75261 0.044227 0.656979 0.780304 0.0254496 0.624882 0.51369 -0.00122638 0.857975 0.506904 -0.000351007 0.862003 0.100823 0.0895047 0.99087 0.570298 0.0416434 0.820382 -0.989057 0.115932 -0.0912446 0.145214 -0.0430987 0.988461 -0.317694 -0.0526722 0.946729 -0.770633 -0.028076 0.636661 -0.757154 -0.0357068 0.65226 -0.512754 0.119312 0.850205 -0.759325 0.0121701 0.650598 -0.538516 -0.0302892 0.84207 0.062979 -0.0505388 0.996734 -0.577001 -0.0434322 0.815588 -0.361324 -0.122166 0.924403 0.794431 -0.38983 -0.465738</float_array>
<technique_common>
<accessor count="199" source="#shape0-lib-normals-array" stride="3">
<param name="X" type="float"/>
<param name="Y" type="float"/>
<param name="Z" type="float"/>
</accessor>
</technique_common>
</source>
<vertices id="shape0-lib-vertices">
<input semantic="POSITION" source="#shape0-lib-positions"/>
</vertices>
<triangles count="199">
<input offset="0" semantic="VERTEX" source="#shape0-lib-vertices"/>
<input offset="1" semantic="NORMAL" source="#shape0-lib-normals"/>
<p>7 0 8 0 5 0 1 1 0 1 2 1 4 2 8 2 6 2 1 3 2 3 4 3 4 4 2 4 3 4 8 5 4 5 3 5 51 6 3 6 47 6 51 7 5 7 8 7 10 8 6 8 9 8 6 9 8 9 9 9 7 10 49 10 8 10 49 11 9 11 8 11 13 12 10 12 9 12 10 13 1 13 4 13 10 14 4 14 6 14 8 15 3 15 51 15 13 16 66 16 10 16 1 17 66 17 0 17 1 18 10 18 66 18 11 19 66 19 13 19 76 20 12 20 13 20 11 21 13 21 12 21 12 22 76 22 21 22 19 23 18 23 17 23 21 24 76 24 22 24 15 25 22 25 14 25 18 26 15 26 14 26 74 27 14 27 16 27 74 28 17 28 18 28 21 29 18 29 20 29 19 30 30 30 18 30 30 31 20 31 18 31 20 32 11 32 12 32 21 33 20 33 12 33 21 34 22 34 15 34 21 35 15 35 18 35 18 36 14 36 74 36 24 37 23 37 99 37 24 38 99 38 45 38 34 39 52 39 25 39 43 40 41 40 42 40 38 41 43 41 42 41 40 42 28 42 92 42 92 43 24 43 40 43 26 44 11 44 44 44 44 45 45 45 26 45 29 46 92 46 28 46 29 47 28 47 73 47 29 48 72 48 32 48 16 49 74 49 72 49 32 50 74 50 27 50 29 51 32 51 31 51 27 52 19 52 30 52 19 53 75 53 30 53 27 54 30 54 32 54 32 55 30 55 31 55 31 56 30 56 82 56 31 57 11 57 92 57 92 58 11 58 26 58 92 59 29 59 31 59 74 60 32 60 72 60 33 61 37 61 53 61 53 62 37 62 35 62 53 63 35 63 36 63 37 64 13 64 42 64 0 65 36 65 66 65 66 66 36 66 35 66 24 67 39 67 40 67 13 68 40 68 39 68 40 69 13 69 76 69 76 70 28 70 40 70 42 71 13 71 38 71 13 72 39 72 38 72 35 73 43 73 66 73 66 74 43 74 11 74 11 75 43 75 44 75 45 76 44 76 39 76 45 77 39 77 24 77 46 78 34 78 36 78 46 79 50 79 33 79 68 80 36 80 0 80 54 81 47 81 51 81 7 82 49 82 48 82 49 83 7 83 57 83 48 84 49 84 58 84 58 85 49 85 50 85 37 86 33 86 13 86 33 87 34 87 46 87 68 88 50 88 46 88 36 89 68 89 46 89 50 90 68 90 55 90 50 91 55 91 58 91 58 92 51 92 48 92 58 93 54 93 51 93 45 94 23 94 26 94 26 95 23 95 24 95 26 96 24 96 92 96 33 97 52 97 34 97 34 98 25 98 36 98 36 99 25 99 53 99 53 100 25 100 52 100 53 101 52 101 33 101 71 102 11 102 82 102 71 103 66 103 11 103 60 104 55 104 67 104 54 105 55 105 56 105 54 106 58 106 55 106 55 107 68 107 67 107 55 108 60 108 56 108 47 109 98 109 95 109 54 110 98 110 47 110 47 111 95 111 51 111 51 112 63 112 48 112 48 113 63 113 7 113 33 114 50 114 13 114 50 115 49 115 13 115 65 116 13 116 49 116 71 117 70 117 66 117 59 118 66 118 70 118 60 119 98 119 56 119 59 120 67 120 68 120 66 121 59 121 0 121 0 122 59 122 68 122 67 123 59 123 60 123 60 124 96 124 98 124 94 125 63 125 95 125 63 126 64 126 7 126 57 127 7 127 64 127 49 128 57 128 64 128 63 129 61 129 64 129 62 130 65 130 61 130 62 131 70 131 65 131 65 132 49 132 64 132 61 133 65 133 64 133 63 134 94 134 61 134 56 135 98 135 54 135 95 136 63 136 51 136 80 137 65 137 69 137 69 138 65 138 70 138 69 139 70 139 71 139 69 140 71 140 83 140 80 141 89 141 76 141 80 142 76 142 13 142 13 143 65 143 80 143 76 144 88 144 28 144 81 145 29 145 73 145 77 146 72 146 81 146 72 147 29 147 81 147 16 148 77 148 85 148 72 149 77 149 16 149 16 150 85 150 74 150 74 151 79 151 27 151 27 152 79 152 19 152 11 153 31 153 82 153 76 154 89 154 88 154 77 155 86 155 87 155 81 156 86 156 77 156 91 157 79 157 85 157 79 158 78 158 19 158 75 159 19 159 78 159 78 160 90 160 83 160 83 161 82 161 78 161 82 162 30 162 78 162 79 163 90 163 78 163 86 164 81 164 73 164 86 165 73 165 88 165 73 166 28 166 88 166 85 167 79 167 74 167 78 168 30 168 75 168 82 169 83 169 71 169 89 170 80 170 69 170 35 171 37 171 41 171 38 172 39 172 43 172 43 173 39 173 44 173 41 174 43 174 35 174 41 175 37 175 42 175 69 176 83 176 84 176 84 177 83 177 90 177 91 178 90 178 79 178 85 179 77 179 91 179 77 180 87 180 91 180 87 181 86 181 88 181 87 182 88 182 89 182 69 183 84 183 89 183 89 184 84 184 87 184 84 185 90 185 91 185 87 186 84 186 91 186 70 187 62 187 93 187 62 188 61 188 94 188 95 189 98 189 94 189 96 190 60 190 59 190 96 191 59 191 97 191 70 192 93 192 97 192 59 193 70 193 97 193 97 194 93 194 96 194 93 195 62 195 94 195 96 196 93 196 94 196 96 197 94 197 98 197 45 198 99 198 23 198</p>
</triangles>
</mesh>
</geometry>
</library_geometries>
<library_visual_scenes>
<visual_scene id="VisualSceneNode" name="VisualScene">
<node id="node" name="node">
<instance_geometry url="#shape0-lib">
<bind_material>
<technique_common/>
</bind_material>
</instance_geometry>
</node>
</visual_scene>
</library_visual_scenes>
<scene>
<instance_visual_scene url="#VisualSceneNode"/>
</scene>
</COLLADA>
<?xml version="1.0"?>
<model>
<name>Plane</name>
<version>1.0</version>
<sdf version='1.4'>plane.sdf</sdf>
<author>
<name>Roman Bapst</name>
<email>roman@px4.io</email>
</author>
<description>
This is a model of a standard plane.
</description>
</model>
......@@ -16,16 +16,16 @@
<xacro:property name="arm_length_front_y" value="0.22" /> <!-- [m] -->
<xacro:property name="arm_length_back_y" value="0.2" /> <!-- [m] -->
<xacro:property name="rotor_offset_top" value="0.023" /> <!-- [m] -->
<xacro:property name="radius_rotor" value="0.1" /> <!-- [m] -->
<xacro:property name="motor_constant" value="8.54858e-06" /> <!-- [kg.m/s^2] -->
<xacro:property name="moment_constant" value="0.16" /> <!-- [m] -->
<xacro:property name="radius_rotor" value="0.128" /> <!-- [m] -->
<xacro:property name="motor_constant" value="9.9865e-06" /> <!-- [kg.m/s^2] -->
<xacro:property name="moment_constant" value="0.016" /> <!-- [m] -->
<xacro:property name="time_constant_up" value="0.0125" /> <!-- [s] -->
<xacro:property name="time_constant_down" value="0.025" /> <!-- [s] -->
<xacro:property name="max_rot_velocity" value="838" /> <!-- [rad/s] -->
<xacro:property name="sin30" value="0.5" />
<xacro:property name="cos30" value="0.866025403784" />
<xacro:property name="sqrt2" value="1.4142135623730951" />
<xacro:property name="rotor_drag_coefficient" value="8.06428e-05" />
<xacro:property name="rotor_drag_coefficient" value="8.06428e-04" />
<xacro:property name="rolling_moment_coefficient" value="0.000001" />
<xacro:property name="color" value="DarkGrey" />
......
......@@ -32,6 +32,7 @@
#include <chrono>
#include <cmath>
#include <iostream>
#include <memory>
#include <stdio.h>
using namespace gazebo;
......@@ -47,7 +48,11 @@ RayPlugin::RayPlugin()
/////////////////////////////////////////////////
RayPlugin::~RayPlugin()
{
#if GAZEBO_MAJOR_VERSION >= 7
this->parentSensor->LaserShape()->DisconnectNewLaserScans(
#else
this->parentSensor->GetLaserShape()->DisconnectNewLaserScans(
#endif
this->newLaserScansConnection);
this->newLaserScansConnection.reset();
......@@ -60,15 +65,27 @@ void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
{
// Get then name of the parent sensor
this->parentSensor =
#if GAZEBO_MAJOR_VERSION >= 7
std::dynamic_pointer_cast<sensors::RaySensor>(_parent);
#else
boost::dynamic_pointer_cast<sensors::RaySensor>(_parent);
#endif
if (!this->parentSensor)
gzthrow("RayPlugin requires a Ray Sensor as its parent");
#if GAZEBO_MAJOR_VERSION >= 7
this->world = physics::get_world(this->parentSensor->WorldName());
#else
this->world = physics::get_world(this->parentSensor->GetWorldName());
#endif
this->newLaserScansConnection =
#if GAZEBO_MAJOR_VERSION >= 7
this->parentSensor->LaserShape()->ConnectNewLaserScans(
#else
this->parentSensor->GetLaserShape()->ConnectNewLaserScans(
#endif
boost::bind(&RayPlugin::OnNewLaserScans, this));
if (_sdf->HasElement("robotNamespace"))
......@@ -86,9 +103,15 @@ void RayPlugin::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
void RayPlugin::OnNewLaserScans()
{
lidar_message.set_time_msec(0);
#if GAZEBO_MAJOR_VERSION >= 7
lidar_message.set_min_distance(parentSensor->RangeMin());
lidar_message.set_max_distance(parentSensor->RangeMax());
lidar_message.set_current_distance(parentSensor->Range(0));
#else
lidar_message.set_min_distance(parentSensor->GetRangeMin());
lidar_message.set_max_distance(parentSensor->GetRangeMax());
lidar_message.set_current_distance(parentSensor->GetRange(0));
#endif
lidar_pub_->Publish(lidar_message);
}
......
......@@ -166,9 +166,12 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
mav_msgs::msgs::CommandMotorSpeed turning_velocities_msg;
for (int i = 0; i < input_reference_.size(); i++){
turning_velocities_msg.add_motor_speed(input_reference_[i]);
if (last_actuator_time_ == 0 || (now - last_actuator_time_).Double() > 0.2) {
turning_velocities_msg.add_motor_speed(0);
} else {
turning_velocities_msg.add_motor_speed(input_reference_[i]);
}
}
// TODO Add timestamp and Header
// turning_velocities_msg->header.stamp.sec = now.sec;
......@@ -191,34 +194,37 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
math::Vector3 velocity_current_W_xy = velocity_current_W;
velocity_current_W_xy.z = 0;
// Set default global reference point
// Set global reference point
// Zurich Irchel Park: 47.397742, 8.545594, 488m
const double lat_default = 47.397742 * M_PI / 180; // rad
const double lon_default = 8.545594 * M_PI / 180; // rad
const double alt_default = 488.0; // meters
const float radius_default = 6353000; // m
double lat_world = lat_default;
double lon_world = lon_default;
double alt_world = alt_default;
double radius_world = radius_default;
// Seattle downtown (15 deg declination): 47.592182, -122.316031, 86m
// Moscow downtown: 55.753395, 37.625427, 155m
// TODO: Remove GPS message from IMU plugin. Added gazebo GPS plugin. This is temp here.
// Zurich Irchel Park
const double lat_zurich = 47.397742 * M_PI / 180; // rad
const double lon_zurich = 8.545594 * M_PI / 180; // rad
const double alt_zurich = 488.0; // meters
// Seattle downtown (15 deg declination): 47.592182, -122.316031
// const double lat_zurich = 47.592182 * M_PI / 180; // rad
// const double lon_zurich = -122.316031 * M_PI / 180; // rad
// const double alt_zurich = 86.0; // meters
const float earth_radius = 6353000; // m
//Patched by Stephen Rees based on https://github.com/PX4/sitl_gazebo/pull/18/commits/c2f4fbca9d90a747535ffe7bc22190db86ed71c7
double lat_world = lat_zurich;
double lon_world = lon_zurich;
double alt_world = alt_zurich;
common::SphericalCoordinatesPtr spherical_coordinates = world_->GetSphericalCoordinates();
if (spherical_coordinates) {
if(spherical_coordinates){
lat_world = spherical_coordinates->LatitudeReference().Radian(); // rad
lon_world = spherical_coordinates->LongitudeReference().Radian(); // rad
alt_world = spherical_coordinates->GetElevationReference(); // m
switch (spherical_coordinates->GetSurfaceType()) {
case (common::SphericalCoordinates::EARTH_WGS84): {
radius_world = 6353000; // m
break;
}
}
}
// reproject local position to gps coordinates
double x_rad = pos_W_I.x / radius_world;
double y_rad = -pos_W_I.y / radius_world;
double x_rad = pos_W_I.x / earth_radius;
double y_rad = -pos_W_I.y / earth_radius;
double c = sqrt(x_rad * x_rad + y_rad * y_rad);
double sin_c = sin(c);
double cos_c = cos(c);
......@@ -229,7 +235,7 @@ void GazeboMavlinkInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
lat_rad = lat_world;
lon_rad = lon_world;
}
if(current_time.Double() - last_gps_time_.Double() > gps_update_interval_){ // 5Hz
if(use_mavlink_udp){
......@@ -499,6 +505,8 @@ void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
// TODO XXX: this makes SITL work again
bool is_fixed_wing = false;
last_actuator_time_ = world_->GetSimTime();
input_reference_.resize(_rotor_count);
// set rotor speeds for all systems
......@@ -514,9 +522,15 @@ void GazeboMavlinkInterface::handle_message(mavlink_message_t *msg)
// set angles of control surface joints (this should go into a message for the correct plugin)
double roll = 0.5 * (inputs.control[4] + inputs.control[5]);
double pitch = 0.5 * (inputs.control[4] - inputs.control[5]);
#if GAZEBO_MAJOR_VERSION >= 6
left_elevon_joint_->SetPosition(0, roll);
right_elevon_joint_->SetPosition(0, -roll);
elevator_joint_->SetPosition(0, -pitch);
#else
left_elevon_joint_->SetAngle(0, roll);
right_elevon_joint_->SetAngle(0, -roll);
elevator_joint_->SetAngle(0, -pitch);
#endif
}
received_first_referenc_ = true;
......
......@@ -56,16 +56,28 @@ void CameraPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
gzerr << "Invalid sensor pointer.\n";
this->parentSensor =
#if GAZEBO_MAJOR_VERSION >= 7
std::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#else
boost::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
#endif
if (!this->parentSensor)
{
gzerr << "CameraPlugin requires a CameraSensor.\n";
#if GAZEBO_MAJOR_VERSION >= 7
if (std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#else
if (boost::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
#endif
gzmsg << "It is a depth camera sensor\n";
}
#if GAZEBO_MAJOR_VERSION >= 7
this->camera = this->parentSensor->Camera();
#else
this->camera = this->parentSensor->GetCamera();
#endif
if (!this->parentSensor)
{
......@@ -73,10 +85,17 @@ void CameraPlugin::Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
return;
}
#if GAZEBO_MAJOR_VERSION >= 7
this->width = this->camera->ImageWidth();
this->height = this->camera->ImageHeight();
this->depth = this->camera->ImageDepth();
this->format = this->camera->ImageFormat();
#else
this->width = this->camera->GetImageWidth();
this->height = this->camera->GetImageHeight();
this->depth = this->camera->GetImageDepth();
this->format = this->camera->GetImageFormat();
#endif
if (_sdf->HasElement("robotNamespace"))
......@@ -103,14 +122,22 @@ void CameraPlugin::OnNewFrame(const unsigned char * _image,
unsigned int _depth,
const std::string &_format)
{
#if GAZEBO_MAJOR_VERSION >= 7
_image = this->camera->ImageData(0);
#else
_image = this->camera->GetImageData(0);
#endif
//GetHFOV gives fucking gazebo::math::Angle which you can not cast...
const double Hfov = 0.6;
const double focal_length = (_width/2)/tan(Hfov/2);
double pixel_flow_x_integral = 0.0;
double pixel_flow_y_integral = 0.0;
#if GAZEBO_MAJOR_VERSION >= 7
double rate = this->camera->RenderRate();
#else
double rate = this->camera->GetRenderRate();
#endif
if (!isfinite(rate))
rate = 30.0;
double dt = 1.0 / rate;
......
<?xml version="1.0" ?>
<sdf version="1.5">
<world name="default">
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://cessna</uri>
</include>
</world>
</sdf>
\ No newline at end of file
......@@ -19,5 +19,29 @@
<include>
<uri>model://iris</uri>
</include>
<include>
<uri>model://asphalt_plane</uri>
</include>
<physics name='default_physics' default='0' type='ode'>
<gravity>0 0 -9.8066</gravity>
<ode>
<solver>
<type>quick</type>
<iters>10</iters>
<sor>1.3</sor>
<use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>100</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</ode>
<max_step_size>0.002</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>500</real_time_update_rate>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
</physics>
</world>
</sdf>
......@@ -15,5 +15,26 @@
<include>
<uri>model://iris_opt_flow</uri>
</include>
<physics name='default_physics' default='0' type='ode'>
<gravity>0 0 -9.8066</gravity>
<ode>
<solver>
<type>quick</type>
<iters>10</iters>
<sor>1.3</sor>
<use_dynamic_moi_rescaling>0</use_dynamic_moi_rescaling>
</solver>
<constraints>
<cfm>0</cfm>
<erp>0.2</erp>
<contact_max_correcting_vel>100</contact_max_correcting_vel>
<contact_surface_layer>0.001</contact_surface_layer>
</constraints>
</ode>
<max_step_size>0.002</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>500</real_time_update_rate>