diff --git a/.gitignore b/.gitignore index 663c7b0..a538daa 100755 --- a/.gitignore +++ b/.gitignore @@ -47,6 +47,10 @@ qtcreator-* # Covers JetBrains IDEs: IntelliJ, RubyMine, PhpStorm, AppCode, PyCharm, CLion, Android Studio and Webstorm # Reference: https://intellij-support.jetbrains.com/hc/en-us/articles/206544839 +# Catch everything +.idea/ +.cmake-build-* + # User-specific stuff: .idea/**/workspace.xml .idea/**/tasks.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index f345e30..409c657 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,170 +1,49 @@ -cmake_minimum_required(VERSION 2.8 FATAL_ERROR) - +cmake_minimum_required(VERSION 3.10.2 FATAL_ERROR) +#============================================================================ +# Initialize the project +#============================================================================ project(ardupilot_sitl_gazebo) +#------------------------------------------------------------------------ +# Compile as C++14 -####################### -## Find Dependencies ## -####################### - -find_package(gazebo REQUIRED) -#set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") - -if("${GAZEBO_VERSION}" VERSION_LESS "8.0") - message(FATAL_ERROR "You need at least Gazebo 8.0. Your version: ${GAZEBO_VERSION}") -else() - message("Gazebo version: ${GAZEBO_VERSION}") -endif() - -###### COMPILE RULES ###### -include(CheckCXXCompilerFlag) - -macro(filter_valid_compiler_flags) - foreach(flag ${ARGN}) - CHECK_CXX_COMPILER_FLAG(${flag} R${flag}) - if(${R${flag}}) - set(VALID_CXX_FLAGS "${VALID_CXX_FLAGS} ${flag}") - endif() - endforeach() -endmacro() - -set(CMAKE_CXX_EXTENSIONS off) # see gazebo CMakeList - -if (NOT CMAKE_BUILD_TYPE) - set (CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING - "Choose the type of build, options are: Debug Release RelWithDebInfo Profile Check" FORCE) -endif (NOT CMAKE_BUILD_TYPE) - -# Build type link flags -set (CMAKE_LINK_FLAGS_RELEASE " " CACHE INTERNAL "Link flags for release" FORCE) -set (CMAKE_LINK_FLAGS_RELWITHDEBINFO " " CACHE INTERNAL "Link flags for release with debug support" FORCE) -set (CMAKE_LINK_FLAGS_DEBUG " " CACHE INTERNAL "Link flags for debug" FORCE) -set (CMAKE_LINK_FLAGS_PROFILE " -pg" CACHE INTERNAL "Link flags for profile" FORCE) -set (CMAKE_LINK_FLAGS_COVERAGE " --coverage" CACHE INTERNAL "Link flags for static code coverage" FORCE) - -set (CMAKE_C_FLAGS_RELEASE "") -if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang" AND NOT MSVC) - # -s doesn't work with clang or Visual Studio, see alternative in link below: - # http://stackoverflow.com/questions/6085491/gcc-vs-clang-symbol-strippingu - set (CMAKE_C_FLAGS_RELEASE "-s") -endif() - -if (NOT MSVC) - set (CMAKE_C_FLAGS_RELEASE " ${CMAKE_C_FLAGS_RELEASE} -O3 -DNDEBUG ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release" FORCE) - set (CMAKE_CXX_FLAGS_RELEASE ${CMAKE_C_FLAGS_RELEASE}) - - set (CMAKE_C_FLAGS_RELWITHDEBINFO " -g -O2 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for release with debug support" FORCE) - set (CMAKE_CXX_FLAGS_RELWITHDEBINFO ${CMAKE_C_FLAGS_RELWITHDEBINFO}) - - set (CMAKE_C_FLAGS_DEBUG " -ggdb3 ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for debug" FORCE) - set (CMAKE_CXX_FLAGS_DEBUG ${CMAKE_C_FLAGS_DEBUG}) - - set (CMAKE_C_FLAGS_PROFILE " -fno-omit-frame-pointer -g -pg ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for profile" FORCE) - set (CMAKE_CXX_FLAGS_PROFILE ${CMAKE_C_FLAGS_PROFILE}) - - set (CMAKE_C_FLAGS_COVERAGE " -g -O0 -Wformat=2 --coverage -fno-inline ${CMAKE_C_FLAGS_ALL}" CACHE INTERNAL "C Flags for static code coverage" FORCE) - set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_C_FLAGS_COVERAGE}") - if (NOT "${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") - # -fno-default-inline -fno-implicit-inline-templates are unimplemented, cause errors in clang - # -fno-elide-constructors can cause seg-faults in clang 3.4 and earlier - # http://llvm.org/bugs/show_bug.cgi?id=12208 - set (CMAKE_CXX_FLAGS_COVERAGE "${CMAKE_CXX_FLAGS_COVERAGE} -fno-default-inline -fno-implicit-inline-templates -fno-elide-constructors") - endif() -endif() - -##################################### -# Set all the global build flags -set (CMAKE_C_FLAGS "${CMAKE_C_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") -set (CMAKE_EXE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") -set (CMAKE_SHARED_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") -set (CMAKE_MODULE_LINKER_FLAGS "${CMAKE_LINK_FLAGS_${CMAKE_BUILD_TYPE_UPPERCASE}}") - -# Compiler-specific C++11 activation. -if ("${CMAKE_CXX_COMPILER_ID}" MATCHES "GNU") - execute_process( - COMMAND ${CMAKE_CXX_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) - if (NOT (GCC_VERSION VERSION_GREATER 4.7)) - message(FATAL_ERROR "${PROJECT_NAME} requires g++ 4.8 or greater.") - endif () -elseif ("${CMAKE_CXX_COMPILER_ID}" MATCHES "Clang") - execute_process( - COMMAND ${CMAKE_CXX_COMPILER} -dumpversion OUTPUT_VARIABLE CLANG_VERSION) - if (NOT (CLANG_VERSION VERSION_GREATER 3.2)) - message(FATAL_ERROR "${PROJECT_NAME} requires clang 3.3 or greater.") - endif () - - if ("${CMAKE_SYSTEM_NAME}" MATCHES "Darwin") - set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -stdlib=libc++") - endif () -elseif ("${CMAKE_CXX_COMPILER_ID}" STREQUAL "MSVC") - if (MSVC_VERSION LESS 1800) - message(FATAL_ERROR "${PROJECT_NAME} requires VS 2013 or greater.") - endif () -else () - message(FATAL_ERROR "Your C++ compiler does not support C++11.") -endif () - -set(WARN_LEVEL "-Wall") -filter_valid_compiler_flags(${WARN_LEVEL} - -Wextra -Wno-long-long -Wno-unused-value -Wfloat-equal -Wshadow - -Wswitch-default -Wmissing-include-dirs -pedantic) -if (UNIX AND NOT APPLE) - filter_valid_compiler_flags(-fvisibility=hidden -fvisibility-inlines-hidden) -endif() - -set(UNFILTERED_FLAGS "-std=c++14") -set (CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${VALID_CXX_FLAGS} ${UNFILTERED_FLAGS}") - -########### -## Build ## -########### - -include_directories( - ${PROJECT_SOURCE_DIR} - include - ${GAZEBO_INCLUDE_DIRS} - ) - -link_libraries( - ${GAZEBO_LIBRARIES} - ) - -link_directories( - ${GAZEBO_LIBRARY_DIRS} - ) +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) -set (plugins_single_header - ArduPilotPlugin - ArduCopterIRLockPlugin - GimbalSmall2dPlugin - ) +#============================================================================ +# Find gz-cmake +#============================================================================ +find_package(gz-cmake3 3.0.0 REQUIRED) +set(GZ_CMAKE_VER ${gz-cmake3_VERSION_MAJOR}) -add_library(ArduCopterIRLockPlugin SHARED src/ArduCopterIRLockPlugin.cc) -target_link_libraries(ArduCopterIRLockPlugin ${GAZEBO_LIBRARIES}) +#============================================================================ +# Search for project-specific dependencies +#============================================================================ -add_library(ArduPilotPlugin SHARED src/ArduPilotPlugin.cc) -target_link_libraries(ArduPilotPlugin ${GAZEBO_LIBRARIES}) +#-------------------------------------- +# Find gz-sim +gz_find_package(gz-sim7 REQUIRED) +set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) -if("${GAZEBO_VERSION}" VERSION_LESS "8.0") - add_library(GimbalSmall2dPlugin SHARED src/GimbalSmall2dPlugin.cc) - target_link_libraries(GimbalSmall2dPlugin ${GAZEBO_LIBRARIES}) - install(TARGETS GimbalSmall2dPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) -endif() +#-------------------------------------- +# Find RapidJSON +find_package(RapidJSON REQUIRED) -install(TARGETS ArduCopterIRLockPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) -install(TARGETS ArduPilotPlugin DESTINATION ${GAZEBO_PLUGIN_PATH}) +#====================================== +# Build plugin -install(DIRECTORY models DESTINATION ${GAZEBO_MODEL_PATH}/..) -install(DIRECTORY worlds DESTINATION ${GAZEBO_MODEL_PATH}/..) +add_library(ArduPilotPlugin + SHARED + src/ArduPilotPlugin.cc + src/Socket.cpp +) -# uninstall target -if(NOT TARGET uninstall) - configure_file( - "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in" - "${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake" - IMMEDIATE @ONLY) +target_include_directories(ArduPilotPlugin PUBLIC + include + ${GZ-SIM_INCLUDE_DIRS} +) - add_custom_target(uninstall - COMMAND ${CMAKE_COMMAND} -P ${CMAKE_CURRENT_BINARY_DIR}/cmake_uninstall.cmake) -endif() +target_link_libraries(ArduPilotPlugin PUBLIC + ${GZ-SIM_LIBRARIES} +) diff --git a/README.md b/README.md index 973f198..d36a28f 100644 --- a/README.md +++ b/README.md @@ -1,135 +1,84 @@ -# Ardupilot Gazebo plugin +# ArduPilot Gazebo Simulator plugin +This is ArduPilot official plugin for Gazebo Simulator plugin +It replaces the old Gazebo plugin to bring support of the next gen Gazebo simulator. +It also brings support for more features : +- more flexible data exchange between SITL and Gazebo with JSON data sharing. +- more sensors supported. +- true Simulation lockstepping. It is now possible to use GDB to stop the Gazebo time for debugging. +- Better 3D rendering -## Requirements : -Native Ubuntu able to run full 3D graphics. -Gazebo version 8.x or greater -The dev branch will works on gazebo >= 8.x -For Gazebo 7 use branch gazebo7 +The project is composed of a Gazebo plugin to connect to ArduPilot SITL (Software In The Loop) and some example models and worlds. -## Disclamer : -This is a playground until I get some time to push the correct patch to gazebo master (I got hard time to work with mercurial..)! -So you can expect things to not be up-to-date. -This assume that your are using Ubuntu 16.04 or Ubuntu 18.04 +## Disclaimer : +The plugin is currently working, but we are working into bringing support for more feature and refine the API. -## Usage : -I assume you already have Gazebo installed with ROS (or without). -If you don't have it yet, install ROS with `sudo apt install ros-melodic-desktop-full` -(follow instruction here http://wiki.ros.org/melodic/Installation/Ubuntu). -Due to a bug in current gazebo release from ROS, please update gazebo with OSRF version from http://gazebosim.org/tutorials?tut=install_ubuntu +## Prerequisites : +Gazebo Sim Garden is supported on Ubuntu Focal and Jammy. If you are running Ubuntu as a virtual machine you will need at least Ubuntu 20.04 (Focal) in order to have the OpenGL support required for the `ogre2` render engine. -libgazeboX-dev must be installed, X be your gazebo version (9 on ROS melodic). +Follow the instructions for a [binary install of Gazebo Garden](https://gazebosim.org/docs/garden/install) and verify that Gazebo is running correctly. -For Gazebo X -```` -sudo apt-get install libgazeboX-dev -```` +Set up an [ArduPilot development environment](https://ardupilot.org/dev/index.html). In the following it is assumed that you are able to +run ArduPilot SITL using the [MAVProxy GCS](https://ardupilot.org/mavproxy/index.html). -```` -git clone https://github.com/khancyr/ardupilot_gazebo -cd ardupilot_gazebo -mkdir build -cd build -cmake .. -make -j4 -sudo make install -```` +## Installation : +Install Gazebo Sim Gazebo development libs and rapidjson: ```` -echo 'source /usr/share/gazebo/setup.sh' >> ~/.bashrc +sudo apt install rapidjson-dev libgz-sim7-dev ```` -Set Path of Gazebo Models (Adapt the path to where to clone the repo) -```` -echo 'export GAZEBO_MODEL_PATH=~/ardupilot_gazebo/models' >> ~/.bashrc +Clone the repo and build with: +````bash +git clone https://github.com/ArduPilot/ardupilot_gazebo -b ignition-garden +cd ardupilot_gazebo +mkdir build && cd build +cmake .. -DCMAKE_BUILD_TYPE=RelWithDebInfo +make -j4 ```` -Set Path of Gazebo Worlds (Adapt the path to where to clone the repo) -```` -echo 'export GAZEBO_RESOURCE_PATH=~/ardupilot_gazebo/worlds:${GAZEBO_RESOURCE_PATH}' >> ~/.bashrc -```` +## Running : -```` -source ~/.bashrc -```` +Set the Gazebo environment variables in your `.bashrc` or `.zshrc` or in the terminal used to run gazebo: -DONE ! +### In terminal +Assuming that you have clone the repository in `$HOME/ardupilot_gazebo`: +```bash +export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:$GZ_SIM_SYSTEM_PLUGIN_PATH +export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:$GZ_SIM_RESOURCE_PATH +``` -Now launch a world file with a copter/rover/plane and ardupilot plugin, and it should work! -(I will try to add some world file and model later) +### In .bashrc +Assuming that you have clone the repository in `$HOME/ardupilot_gazebo`: +```bash +echo 'export GZ_SIM_SYSTEM_PLUGIN_PATH=$HOME/ardupilot_gazebo/build:${GZ_SIM_SYSTEM_PLUGIN_PATH}' >> ~/.bashrc +echo 'export GZ_SIM_RESOURCE_PATH=$HOME/ardupilot_gazebo/models:$HOME/ardupilot_gazebo/worlds:${GZ_SIM_RESOURCE_PATH}' >> ~/.bashrc +``` -## HELP +Reload your terminal with source ~/.bashrc -How to Launch : -Launch Ardupilot Software In the Loop Simulation for each vehicle. -On new terminal, launch Gazebo with basic demo world. +### Run Gazebo -#####ROVER (no model provided for now) +```bash +gz sim -v 4 -r iris_arducopter_runway.world +``` -On 1st Terminal (Launch Ardupilot SITL) -```` -sim_vehicle.py -v APMrover2 -f gazebo-rover --map --console -```` +The `-v 4` parameter is not mandatory, it shows the debug informations. -On 2nd Terminal (Launch Gazebo with demo Rover model) -```` -gazebo --verbose worlds/ (Please Add if there is one.) -```` +### Run ArduPilot SITL +To run an ArduPilot simulation with Gazebo, the frame should have `gazebo-` in it and have `JSON` as model. Other commandline parameters are the same as usal on SITL. +```bash +sim_vehicle.py -v ArduCopter -f gazebo-iris --model JSON --map --console +``` -##### COPTER +### Arm and takeoff -On 1st Terminal (Launch Ardupilot SITL) -```` -sim_vehicle.py -v ArduCopter -f gazebo-iris --map --console -```` - -On 2nd Terminal (Launch Gazebo with demo 3DR Iris model) -```` -gazebo --verbose worlds/iris_arducopter_runway.world -```` - -##### PLANE - -On 1st Terminal (Launch Ardupilot SITL) -```` -sim_vehicle.py -v ArduPlane -f gazebo-zephyr --map --console -```` - -On 2nd Terminal (Launch Gazebo with demo Zephyr flying wing model) -```` -gazebo --verbose worlds/zephyr_ardupilot_demo.world -```` - -In addition, you can use any GCS of Ardupilot locally or remotely (will require connection setup). -If MAVProxy Developer GCS is uncomportable. Omit --map --console arguments out of SITL launch and use APMPlanner 2 or QGroundControl instead. -Local connection with APMPlanner2/QGroundControl is automatic, and recommended. +```bash +STABILIZE> mode guided +GUIDED> arm throttle +GUIDED> takeoff 5 +``` ## Troubleshooting -### Missing libArduPilotPlugin.so... etc - -In case you see this message when you launch gazebo with demo worlds, check you have no error after sudo make install. -If no error use "ls" on the install path given to see if the plugin is really here. -If this is correct, check with `cat /usr/share/gazebo/setup.sh` the variable `GAZEBO_PLUGIN_PATH`. It should be the same as the install path. If not use `cp` to copy the lib to right path. - -For Example - -```` -sudo cp -a /usr/lib/x86_64-linux-gnu/gazebo-7.0/plugins/ /usr/lib/x86_64-linux-gnu/gazebo-7/ -```` - -path mismatch is confirmed as ROS's glitch. It will be fixed. - -### Future(not activated yet) -To use Gazebo gps, you must offset the heading of +90° as gazebo gps is NWU and ardupilot is NED -(I don't use GPS altitude for now) -example : for SITL default location -```` - - EARTH_WGS84 - -35.363261 - 149.165230 - 584 - 87 - -```` -Rangefinder +### Gazebo issues +Gazebo documentation is already providing some help on most frequents issues https://gazebosim.org/docs/garden/troubleshooting#ubuntu diff --git a/include/ArduPilotPlugin.hh b/include/ArduPilotPlugin.hh index b449cac..c21e735 100755 --- a/include/ArduPilotPlugin.hh +++ b/include/ArduPilotPlugin.hh @@ -17,12 +17,21 @@ #ifndef GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ #define GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ +#include #include -#include -#include -namespace gazebo +namespace gz { +namespace sim { +namespace systems { + // The servo packet received from ArduPilot SITL. Defined in SIM_JSON.h. + struct servo_packet { + uint16_t magic; // 18458 expected magic value + uint16_t frame_rate; + uint32_t frame_count; + uint16_t pwm[16]; + }; + // Forward declare private data class class ArduPilotSocketPrivate; class ArduPilotPluginPrivate; @@ -33,10 +42,14 @@ namespace gazebo /// The plugin requires the following parameters: /// control description block /// - /// channel attribute, ardupilot control channel - /// multiplier command multiplier + /// "channel" attribute, ardupilot control channel + /// command multiplier + /// command offset + /// upper limit for PWM input + /// lower limit for PWM input /// - /// type type of control, VELOCITY, POSITION or EFFORT + /// type of control, VELOCITY, POSITION, EFFORT or COMMAND + /// 1 if joint forces are applied, 0 to set joint directly /// velocity pid p gain /// velocity pid i gain /// velocity pid d gain @@ -45,23 +58,64 @@ namespace gazebo /// velocity pid max command torque /// velocity pid min command torque /// motor joint, torque applied here + /// topic to publish commands that are processed by other plugins + /// /// rotor turning direction, 'cw' or 'ccw' - /// frequencyCutoff filter incoming joint state - /// samplingRate sampling rate for filtering incoming joint state + /// filter incoming joint state + /// sampling rate for filtering incoming joint state /// for rotor aliasing problem, experimental /// scoped name for the imu sensor /// timeout before giving up on /// controller synchronization - class GAZEBO_VISIBLE ArduPilotPlugin : public ModelPlugin - { + class GZ_SIM_VISIBLE ArduPilotPlugin: + public gz::sim::System, + public gz::sim::ISystemConfigure, + public gz::sim::ISystemPostUpdate, + public gz::sim::ISystemPreUpdate, + public gz::sim::ISystemReset +{ /// \brief Constructor. public: ArduPilotPlugin(); /// \brief Destructor. public: ~ArduPilotPlugin(); - // Documentation Inherited. - public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); + public: void Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) final; + + /// \brief Load configuration from SDF on startup. + public: void Configure(const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &_eventMgr) final; + + /// \brief Do the part of one update loop that involves making changes to simulation. + public: void PreUpdate(const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) final; + + /// \brief Do the part of one update loop that involves reading results from simulation. + public: void PostUpdate(const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) final; + + /// \brief Load control channels + private: void LoadControlChannels( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \brief Load IMU sensors + private: void LoadImuSensors( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \brief Load GPS sensors + private: void LoadGpsSensors( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); + + /// \brief Load Range sensors + private: void LoadRangeSensors( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm); /// \brief Update the control surfaces controllers. /// \param[in] _info Update information provided by the server. @@ -69,28 +123,38 @@ namespace gazebo /// \brief Update PID Joint controllers. /// \param[in] _dt time step size since last update. - private: void ApplyMotorForces(const double _dt); + private: void ApplyMotorForces( + const double _dt, + gz::sim::EntityComponentManager &_ecm); /// \brief Reset PID Joint controllers. private: void ResetPIDs(); - /// \brief Receive motor commands from ArduPilot - private: void ReceiveMotorCommand(); + /// \brief Receive a servo packet from ArduPilot + /// + /// Returns true if a servo packet was received, otherwise false. + private: bool ReceiveServoPacket(); + + /// \brief Update the motor commands given servo PWM values + private: void UpdateMotorCommands(const servo_packet &_pkt); + + /// \brief Create the state JSON + private: void CreateStateJSON( + double _simTime, + const gz::sim::EntityComponentManager &_ecm) const; /// \brief Send state to ArduPilot private: void SendState() const; - /// \brief Init ardupilot socket - private: bool InitArduPilotSockets(sdf::ElementPtr _sdf) const; + /// \brief Initialise flight dynamics model socket + private: bool InitSockets(sdf::ElementPtr _sdf) const; /// \brief Private data pointer. private: std::unique_ptr dataPtr; + }; - /// \brief transform from model orientation to x-forward and z-up - private: ignition::math::Pose3d modelXYZToAirplaneXForwardZDown; +} // namespace systems +} // namespace gazebo +} // namespace gz - /// \brief transform from world frame to NED frame - private: ignition::math::Pose3d gazeboXYZToNED; - }; -} -#endif +#endif // GAZEBO_PLUGINS_ARDUPILOTPLUGIN_HH_ diff --git a/include/Socket.h b/include/Socket.h new file mode 100644 index 0000000..3143fa9 --- /dev/null +++ b/include/Socket.h @@ -0,0 +1,69 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple socket handling class for systems with BSD socket API + */ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include + +class SocketAPM { +public: + SocketAPM(bool _datagram); + SocketAPM(bool _datagram, int _fd); + ~SocketAPM(); + + bool connect(const char *address, uint16_t port); + bool bind(const char *address, uint16_t port); + bool reuseaddress(); + bool set_blocking(bool blocking); + bool set_cloexec(); + void set_broadcast(void); + + ssize_t send(const void *pkt, size_t size); + ssize_t sendto(const void *buf, size_t size, const char *address, uint16_t port); + ssize_t recv(void *pkt, size_t size, uint32_t timeout_ms); + + // return the IP address and port of the last received packet + void last_recv_address(const char *&ip_addr, uint16_t &port); + + // return true if there is pending data for input + bool pollin(uint32_t timeout_ms); + + // return true if there is room for output data + bool pollout(uint32_t timeout_ms); + + // start listening for new tcp connections + bool listen(uint16_t backlog); + + // accept a new connection. Only valid for TCP connections after + // listen has been used. A new socket is returned + SocketAPM *accept(uint32_t timeout_ms); + +private: + bool datagram; + struct sockaddr_in in_addr {}; + + int fd = -1; + + void make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr); +}; diff --git a/models/iris_with_ardupilot/model.sdf b/models/iris_with_ardupilot/model.sdf index 8d522c7..23c62b9 100755 --- a/models/iris_with_ardupilot/model.sdf +++ b/models/iris_with_ardupilot/model.sdf @@ -1,5 +1,5 @@ - + model://iris_with_standoffs @@ -381,7 +381,7 @@ --> - + 0.3 1.4 4.2500 @@ -397,7 +397,7 @@ 0 0 1 iris::rotor_0 - + 0.3 1.4 4.2500 @@ -414,7 +414,7 @@ iris::rotor_0 - + 0.3 1.4 4.2500 @@ -430,7 +430,7 @@ 0 0 1 iris::rotor_1 - + 0.3 1.4 4.2500 @@ -447,7 +447,7 @@ iris::rotor_1 - + 0.3 1.4 4.2500 @@ -463,7 +463,7 @@ 0 0 1 iris::rotor_2 - + 0.3 1.4 4.2500 @@ -480,7 +480,7 @@ iris::rotor_2 - + 0.3 1.4 4.2500 @@ -496,7 +496,7 @@ 0 0 1 iris::rotor_3 - + 0.3 1.4 4.2500 @@ -512,7 +512,7 @@ 0 0 1 iris::rotor_3 - + 127.0.0.1 9002 9003 diff --git a/models/iris_with_standoffs/model.sdf b/models/iris_with_standoffs/model.sdf index d192c05..9e6ed0f 100755 --- a/models/iris_with_standoffs/model.sdf +++ b/models/iris_with_standoffs/model.sdf @@ -48,9 +48,12 @@ - + + .175 .175 .175 1.0 + .175 .175 .175 1.0 + .175 .175 .175 1.000000 @@ -63,10 +66,13 @@ - + + 0.1 0.1 0.1 + 0.1 0.1 0.1 + 0.01 0.01 0.01 1.0 @@ -78,10 +84,13 @@ - + + 0.1 0.1 0.1 + 0.1 0.1 0.1 + 0.01 0.01 0.01 1.0 @@ -93,10 +102,13 @@ - + + 0.1 0.1 0.1 + 0.1 0.1 0.1 + 0.01 0.01 0.01 1.0 @@ -108,10 +120,13 @@ - + + 0.1 0.1 0.1 + 0.1 0.1 0.1 + 0.01 0.01 0.01 1.0 @@ -152,7 +167,7 @@ - + 0 0 0 0 0 0 0.15 @@ -172,7 +187,7 @@ - iris/imu_link + imu_link base_link 0 0 1 @@ -268,10 +283,13 @@ - + + 0 0 1 + 0 0 1 + 0.1 0.1 0.1 1 1 @@ -338,10 +356,13 @@ - + + 1 1 1 1 + 1 1 1 1 + .1 .1 .1 1 1 @@ -408,10 +429,13 @@ - + + 0 0 1 + 0 0 1 + 0.1 0.1 0.1 1 1 @@ -478,10 +502,13 @@ - + + 1 1 1 1 + 1 1 1 1 + .1 .1 .1 1 1 @@ -509,5 +536,263 @@ 0 + + + + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + 0.084 0 0 + 0 1 0 + 0 0 1 + rotor_0 + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + -0.084 0 0 + 0 -1 0 + 0 0 1 + rotor_0 + + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + 0.084 0 0 + 0 1 0 + 0 0 1 + rotor_1 + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + -0.084 0 0 + 0 -1 0 + 0 0 1 + rotor_1 + + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + 0.084 0 0 + 0 -1 0 + 0 0 1 + rotor_2 + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + -0.084 0 0 + 0 1 0 + 0 0 1 + rotor_2 + + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + 0.084 0 0 + 0 -1 0 + 0 0 1 + rotor_3 + + + 0.3 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.002 + 1.2041 + -0.084 0 0 + 0 1 0 + 0 0 1 + rotor_3 + + + + rotor_0_joint + + + rotor_1_joint + + + rotor_2_joint + + + rotor_3_joint + + + + + 127.0.0.1 + 9002 + 5 + 1 + + + 0 0 0 3.141593 0 0 + 0 0 0 3.141593 0 0 + + + imu_sensor + + + + rotor_0_joint + 1 + 838 + 0 + 1100 + 1900 + VELOCITY + 0.20 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + 1 + + + + rotor_1_joint + 1 + 838 + 0 + 1100 + 1900 + VELOCITY + 0.20 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + 1 + + + + rotor_2_joint + 1 + -838 + 0 + 1100 + 1900 + VELOCITY + 0.20 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + 1 + + + + rotor_3_joint + 1 + -838 + 0 + 1100 + 1900 + VELOCITY + 0.20 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + 1 + + + diff --git a/models/runway/materials/textures/Grass004_2K_AmbientOcclusion.jpg b/models/runway/materials/textures/Grass004_2K_AmbientOcclusion.jpg new file mode 100644 index 0000000..0a57806 Binary files /dev/null and b/models/runway/materials/textures/Grass004_2K_AmbientOcclusion.jpg differ diff --git a/models/runway/materials/textures/Grass004_2K_Color copy.jpg b/models/runway/materials/textures/Grass004_2K_Color copy.jpg new file mode 100644 index 0000000..0e52b22 Binary files /dev/null and b/models/runway/materials/textures/Grass004_2K_Color copy.jpg differ diff --git a/models/runway/materials/textures/Grass004_2K_Color.jpg b/models/runway/materials/textures/Grass004_2K_Color.jpg new file mode 100644 index 0000000..3ad03cc Binary files /dev/null and b/models/runway/materials/textures/Grass004_2K_Color.jpg differ diff --git a/models/runway/materials/textures/Grass004_2K_Displacement.jpg b/models/runway/materials/textures/Grass004_2K_Displacement.jpg new file mode 100644 index 0000000..4ee9b27 Binary files /dev/null and b/models/runway/materials/textures/Grass004_2K_Displacement.jpg differ diff --git a/models/runway/materials/textures/Grass004_2K_NormalGL.jpg b/models/runway/materials/textures/Grass004_2K_NormalGL.jpg new file mode 100644 index 0000000..7794b24 Binary files /dev/null and b/models/runway/materials/textures/Grass004_2K_NormalGL.jpg differ diff --git a/models/runway/materials/textures/Grass004_2K_Roughness.jpg b/models/runway/materials/textures/Grass004_2K_Roughness.jpg new file mode 100644 index 0000000..538b81d Binary files /dev/null and b/models/runway/materials/textures/Grass004_2K_Roughness.jpg differ diff --git a/models/runway/materials/textures/runway.png b/models/runway/materials/textures/runway.png new file mode 100644 index 0000000..5a7c662 Binary files /dev/null and b/models/runway/materials/textures/runway.png differ diff --git a/models/runway/meshes/airfield.dae b/models/runway/meshes/airfield.dae new file mode 100644 index 0000000..ab60d34 --- /dev/null +++ b/models/runway/meshes/airfield.dae @@ -0,0 +1,188 @@ + + + + + Blender User + Blender 2.82.7 commit date:2020-03-12, commit time:05:06, hash:375c7dc4caf4 + + 2021-09-15T16:31:09 + 2021-09-15T16:31:09 + + Z_UP + + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + + 1000 1000 1000 + 1 + 0 + 0.00111109 + + + + + 0 + 0 + 1 + 1 + 1 + 1 + 1 + 0 + 0 + 0 + 1000 + 29.99998 + 75 + 0.15 + 0 + 1 + 2 + 0.04999995 + 30.002 + 1 + 3 + 2880 + 3 + 1 + 1 + 0.1 + 0.1 + 1 + + + + + + + + + + Grass004_2K_Color_jpg + + + + + Grass004_2K_Color_jpg-surface + + + + + + 0 0 0 1 + + + + + + 1.45 + + + + + + + + + ../materials/textures/Grass004_2K_Color.jpg + + + + + + + + + + + + -1 -1 0 1 -1 0 -1 1 0 1 1 0 + + + + + + + + + + 0 0 1 + + + + + + + + + + -250 250 250 -250 250 250 -250 250 -250 -250 250 -250 + + + + + + + + + + + + + + +

1 0 0 2 0 1 0 0 2 1 0 3 3 0 4 2 0 5

+ + + + + + + + 5000 0 0 0 0 5000 0 0 0 0 1 0 0 0 0 1 + + + + + + + + + + + + 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1 + + + + -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 + + + + + + + + \ No newline at end of file diff --git a/models/runway/model.config b/models/runway/model.config new file mode 100755 index 0000000..cfe70c1 --- /dev/null +++ b/models/runway/model.config @@ -0,0 +1,15 @@ + + + Runway + 1.0 + model.sdf + + + Rhys Mainwaring + rhys.mainwaring@me.com + + + + Runway + + diff --git a/models/runway/model.sdf b/models/runway/model.sdf new file mode 100755 index 0000000..cc892b3 --- /dev/null +++ b/models/runway/model.sdf @@ -0,0 +1,62 @@ + + + + true + + + + + 0 0 1 + + + + + 0 0 0 0 0 3.141592653589793 + false + + + 0 0 1 + 1500 100 + + + + 10 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + + materials/textures/runway.png + 0.6 + 0 + + + + + + 0 0 -0.5 0 0 0 + false + + + model://runway/meshes/airfield.dae + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.1 0.1 0.1 1 + + + materials/textures/Grass004_2K_Color.jpg + materials/textures/Grass004_2K_AmbientOcclusion.jpg + materials/textures/Grass004_2K_NormalGL.jpg + materials/textures/Grass004_2K_Roughness.jpg + 0.6 + 0 + + + + + + + diff --git a/models/zephyr_with_ardupilot/materials/textures/wing.png b/models/zephyr_with_ardupilot/materials/textures/wing.png new file mode 100644 index 0000000..5876237 Binary files /dev/null and b/models/zephyr_with_ardupilot/materials/textures/wing.png differ diff --git a/models/zephyr_with_ardupilot/meshes/wing.dae b/models/zephyr_with_ardupilot/meshes/wing.dae new file mode 100644 index 0000000..fc3f453 --- /dev/null +++ b/models/zephyr_with_ardupilot/meshes/wing.dae @@ -0,0 +1,651 @@ + + + + + ColeB + OpenCOLLADA for 3ds Max; Version: 1.6; Revision: 24 + + 2016-08-02T19:43:34 + 2016-08-02T19:43:34 + + Z_UP + + + + + + + Wing_png + + + + + Wing_png-surface + + + + + + 0 0 0 1 + + + 0.5882353 0.5882353 0.5882353 1 + + + + + + 0 0 0 1 + + + 9.999999 + + + 0 0 0 1 + + + 1 1 1 1 + + + 1 + + + + + + + + 0 + 0 + 0 + 1.5 + 1 + 0 + 0 + 0 + 3 + + + 1 + 1 + 0 + 0 + 0 + 0 + 0.1 + + + + + + + + + + + + + + + -0.02032846 -0.1830921 -0.1073283 -0.04307937 -0.1830919 -0.1013461 + + + + + + + + + + -0.02032846 -0.1830921 -0.1073283 -0.03549574 -0.183092 -0.1033402 + + + + + + + + + + -0.0279121 -0.183092 -0.1053342 -0.04307937 -0.1830919 -0.1013461 + + + + + + + + + + BEZIER BEZIER + + + + + + + + + + + + + + + + + + 4.594768 -1.131593 -0.001338064 4.594901 -0.03871506 -0.4340888 + + + + + + + + + + 4.594768 -1.131593 -0.001338064 4.594857 -0.4030077 -0.2898386 + + + + + + + + + + 4.594812 -0.7673004 -0.1455883 4.594901 -0.03871506 -0.4340888 + + + + + + + + + + BEZIER BEZIER + + + + + + + + + + + + + + + + + + -761.9454 148.6509 -1.459439 -761.9454 148.6506 -7.551793 -761.9454 10.60691 -16.09295 -761.9454 56.6459 18.19161 -761.9454 -87.77074 -6.753847 -761.9454 -95.02206 2.229903 -761.9454 -94.05078 13.03501 -761.9454 -77.98257 23.34099 -761.9454 -42.08063 -15.7231 -761.9454 -20.24841 29.26244 0 -279.7483 18.1917 0 -554.3892 13.0351 0 -556.1829 2.229994 -133.0563 -113.406 3.898923 -133.0563 -113.406 -9.784805 0 -113.406 3.899301 -160.0896 -57.80275 -1.459368 -160.0895 -57.80333 -7.551721 -154.3607 -95.12251 2.629556 -154.3607 -95.12251 -9.526692 -147.6312 -108.8351 3.948506 -147.6312 -108.8351 -9.874151 -198.5163 -194.0566 18.19168 -252.9071 -290.2486 29.2625 -291.6786 -357.5907 23.34105 -302.4201 -375.4781 13.03506 -228.1367 -254.2275 -16.09288 -264.1905 -318.1728 -15.72304 -295.3648 -370.2626 -6.753791 -301.361 -377.5951 2.229958 -21.13519 -452.9894 -15.72301 -23.62915 -534.7852 -6.753758 -24.10883 -547.692 2.22999 -15.88123 -275.3216 18.1917 -20.23254 -413.6621 29.26253 -23.33423 -517.1411 23.34108 -24.19354 -545.8724 13.03509 -18.25085 -358.3565 -16.09286 0 -458.4125 -15.72301 0 -524.7148 23.34108 0 -363.3957 -16.09286 0 -419.7873 29.26253 0 -542.7914 -6.753756 -9.44176 -152.4788 -19.29639 -16.35361 -152.4788 -12.38455 -18.88352 -152.4788 -2.942793 -16.35361 -152.4788 6.498965 -9.441765 -152.4788 13.41081 0 -152.4788 15.94073 0 -95.4848 -21.8263 -9.44176 -95.4848 -19.29639 -16.35361 -95.4848 -12.38454 -18.88352 -95.4848 -2.942784 -16.35361 -95.4848 6.498975 -9.441765 -95.48481 13.41082 -7.801962 -92.31696 -16.45617 -13.51339 -92.31696 -10.74474 -15.60392 -92.31696 -2.942783 -13.5134 -92.31696 4.859178 -7.801966 -92.31696 10.57061 252.9071 -290.2486 29.2625 20.23254 -413.6621 29.26253 15.88123 -275.3216 18.1917 198.5163 -194.0566 18.19168 761.9454 -42.08063 -15.7231 761.9454 10.60691 -16.09295 228.1367 -254.2275 -16.09288 264.1905 -318.1728 -15.72304 291.6786 -357.5907 23.34105 23.33423 -517.1411 23.34108 761.9454 -87.77074 -6.753847 295.3648 -370.2626 -6.753791 302.4201 -375.4781 13.03506 24.19354 -545.8724 13.03509 761.9454 -95.02206 2.229903 301.361 -377.5951 2.229958 761.9454 -94.05078 13.03501 160.0895 -57.80333 -7.551721 761.9454 148.6506 -7.551793 761.9454 148.6509 -1.459439 160.0896 -57.80275 -1.459368 761.9454 -77.98257 23.34099 761.9454 -20.24841 29.26244 761.9454 56.6459 18.19161 133.0563 -113.406 3.898923 154.3607 -95.12251 -9.526692 154.3607 -95.12251 2.629556 147.6312 -108.8351 3.948506 147.6312 -108.8351 -9.874151 133.0563 -113.406 -9.784805 0 -113.406 -9.78479 18.25085 -358.3565 -16.09286 21.13519 -452.9894 -15.72301 23.62915 -534.7852 -6.753758 24.10883 -547.692 2.22999 0 -152.4788 -21.82631 9.44176 -152.4788 -19.29639 9.44176 -95.4848 -19.29639 16.35361 -152.4788 -12.38455 16.35361 -95.4848 -12.38454 18.88352 -152.4788 -2.942793 18.88352 -95.4848 -2.942784 16.35361 -152.4788 6.498965 16.35361 -95.4848 6.498975 9.441765 -152.4788 13.41081 9.441765 -95.48481 13.41082 0 -95.48481 15.94074 0 -92.31696 -18.5467 7.801962 -92.31696 -16.45617 13.51339 -92.31696 -10.74474 15.60392 -92.31696 -2.942783 13.5134 -92.31696 4.859178 7.801966 -92.31696 10.57061 0 -92.31696 12.66114 -3.078444 -81.79979 2.38923 3.078442 -81.79979 2.389231 -6.156886 -77.59107 -2.942791 -3.078444 -77.59107 2.38923 3.078443 -77.59107 2.389232 -2.018117 -71.96751 0.5526907 2.018116 -71.96751 0.5526912 4.036235 -71.96751 -2.94279 -4.036233 -71.96751 -2.942791 -6.156887 -81.79979 -2.942791 -3.078443 -81.79979 -8.274811 -3.078443 -77.59107 -8.274811 3.078443 -81.79979 -8.274811 3.078444 -77.59107 -8.274812 6.156887 -81.79979 -2.94279 6.156888 -77.59107 -2.94279 -2.018116 -71.96751 -6.438272 2.018117 -71.96751 -6.438272 2.14577e-6 -67.0032 -2.94279 -766.8485 -95.02254 2.227657 -766.8485 -94.04874 13.03752 -766.8485 246.8982 56.97721 -766.8485 234.1205 25.78339 -766.8485 229.3291 -7.225792 -766.8485 235.0911 -42.65179 -766.8485 250.5328 -78.46158 -766.8485 -87.77036 -6.756571 -761.8485 -95.02254 2.227657 -761.8485 -94.04874 13.03752 -761.8485 -77.98442 23.34088 -761.8485 246.8982 56.97721 -761.8485 234.1205 25.78339 -761.8485 229.3291 -7.225792 -761.8485 235.0911 -42.65179 -761.8485 250.5328 -78.46158 -761.8485 -87.77036 -6.756571 -766.8485 120.6044 -70.25448 -766.8485 123.6801 56.62332 -761.8485 120.6044 -70.25449 -761.8485 123.6801 56.62332 -766.8485 -77.98442 23.34088 766.8485 123.6801 56.62332 766.8485 120.6044 -70.25448 766.8485 -87.77036 -6.756571 766.8485 -77.98442 23.34088 761.8485 120.6044 -70.25449 761.8485 250.5328 -78.46158 761.8485 235.0911 -42.65179 766.8485 250.5328 -78.46158 766.8485 235.0911 -42.65179 761.8485 -87.77036 -6.756571 766.8485 246.8982 56.97721 761.8485 123.6801 56.62332 761.8485 246.8982 56.97721 766.8485 234.1205 25.78339 761.8485 234.1205 25.78339 766.8485 229.3291 -7.225792 761.8485 229.3291 -7.225792 766.8485 -94.04874 13.03752 761.8485 -94.04874 13.03752 761.8485 -77.98442 23.34088 766.8485 -95.02254 2.227657 761.8485 -95.02254 2.227657 + + + + + + + + + + 0.003962809 0.01347221 0.9999014 -7.19502e-4 0.006542019 0.9999783 0.02987603 0.0880049 0.9956719 0.04824483 0.1176199 0.9918861 -0.03209855 -0.05575672 -0.9979283 0.01115724 0.02788763 -0.9995488 0.005880708 0.01666997 -0.9998438 -0.04018344 -0.06948041 -0.9967736 -0.1417034 -0.2372242 0.9610644 -0.1048368 -0.2129787 0.9714162 -0.212677 -0.3539859 -0.9107483 -0.2334159 -0.3863601 -0.8923244 -0.4297255 -0.7045036 0.5648103 -0.340344 -0.7217911 0.6026471 -0.490994 -0.8031842 -0.3373722 -0.493067 -0.8063021 -0.3267446 -0.4183563 -0.6868706 0.5942951 0.3244694 0.9458961 -6.95304e-5 0.3244694 0.9458961 -6.95304e-5 0.3244694 0.9458963 -6.95305e-5 0.3244694 0.9458961 -6.95304e-5 -1 1.96318e-14 0 -0.9999999 1.96318e-14 0 -1 1.96318e-14 0 -1 1.96318e-14 0 -1 1.96318e-14 0 -1 1.96318e-14 0 -1 1.96318e-14 0 -0.9999999 1.96318e-14 0 -1 1.96318e-14 0 -1 1.96318e-14 0 2.2242e-10 0.1064701 0.9943159 0.03427743 0.1181745 0.9924011 0.01882985 0.04650464 -0.9987406 0.01912932 0.05576557 -0.9982607 0.01835955 0.0457616 -0.9987836 0.9884217 0.1517325 -1.54469e-6 0.9884215 0.1517325 -1.54469e-6 0.9511698 0.3086678 -7.32725e-7 0.9529538 0.3031157 -7.62511e-7 0.642369 0.7663956 0 0.6494867 0.7603731 0 0.1515213 0.9884539 0 0.1516465 0.9884348 0 0 1 0 0 0.9999999 0 0.05633355 0.1317699 0.9896784 0.05792006 0.1407148 0.9883545 0.03937946 0.1147998 0.9926078 0.06555038 0.1639255 0.9842924 0.06183137 0.1454782 0.9874274 0.01948966 0.04449693 0.9988195 -0.1242694 -0.209689 0.969839 0.01666212 0.03351608 -0.9992993 0.01191135 0.03131852 -0.9994385 0.002763382 0.01061045 -0.9999399 -0.02953718 -0.06261034 -0.9976009 -0.1773081 -0.3619099 -0.9151956 -0.3905371 -0.8452066 -0.3648377 0 0.02978525 -0.9995563 1.85297e-11 0.008175306 -0.9999666 -5.94247e-10 -0.001414966 0.999999 -2.07562e-9 0.08169232 0.9966576 7.2505e-9 -0.2224095 0.9749533 -2.48737e-8 -0.8040273 0.5945924 0 -0.06364953 -0.9979723 -1.00534e-8 -0.3872559 -0.9219723 -3.27372e-8 -0.9337761 -0.3578577 0 1.66589e-7 -1 -0.5 1.39119e-7 -0.8660254 -0.5000001 1.39119e-7 -0.8660254 0 1.66589e-7 -1 -0.8660253 7.79778e-8 -0.5000001 -0.8660254 7.79778e-8 -0.5000001 -1 9.36605e-15 -2.45525e-7 -1 0 -1.86599e-7 -0.8660255 -8.41805e-8 0.4999997 -0.8660256 -8.41806e-8 0.4999998 -0.5000002 -1.46208e-7 0.8660253 -0.5000002 -1.46208e-7 0.8660253 0 -1.68361e-7 1 0 -1.68361e-7 1 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -0.9999999 0 2.22635e-7 0.9999999 2.22635e-7 2.22635e-7 1 2.22635e-7 2.22635e-7 1 2.22635e-7 2.22635e-7 1 2.22635e-7 2.22635e-7 1 2.22635e-7 2.22635e-7 1 2.22635e-7 2.22635e-7 1 2.22635e-7 -0.3473733 0.7192546 -0.6016682 -0.3473734 0.7192546 -0.6016683 0 0.7192545 -0.6947467 0 0.7192545 -0.6947467 -0.6016681 0.7192547 -0.3473733 -0.6016682 0.7192547 -0.3473733 -0.6947466 0.7192546 -1.64329e-7 -0.6947466 0.7192546 -1.46567e-7 -0.6016681 0.7192546 0.3473734 -0.6016681 0.7192547 0.3473734 -0.3473731 0.7192546 0.6016684 -0.3473732 0.7192546 0.6016683 -1.09553e-8 0.7192547 0.6947466 -1.72432e-8 0.7192546 0.6947466 -0.00396281 0.01347221 0.9999014 -0.04824483 0.1176199 0.9918861 -0.02987603 0.08800489 0.9956719 7.195e-4 0.006542018 0.9999783 0.03209855 -0.05575672 -0.9979283 0.04018344 -0.06948042 -0.9967736 -0.005880708 0.01666997 -0.9998438 -0.01115724 0.02788763 -0.9995488 0.1417034 -0.2372242 0.9610644 0.1048368 -0.2129787 0.9714162 0.212677 -0.3539859 -0.9107483 0.2334158 -0.3863601 -0.8923244 0.4297255 -0.7045036 0.5648102 0.340344 -0.7217911 0.6026471 0.4909941 -0.8031843 -0.3373722 0.4930669 -0.8063021 -0.3267446 0.4183564 -0.6868706 0.5942951 -0.3244694 0.9458961 -6.95304e-5 -0.3244694 0.9458961 -6.95304e-5 -0.3244694 0.9458963 -6.95305e-5 -0.3244694 0.9458961 -6.95304e-5 1 1.17791e-14 -1.57055e-14 1 1.17791e-14 -1.57055e-14 1 1.17791e-14 -1.57055e-14 0.9999999 1.17791e-14 -1.57055e-14 1 1.17791e-14 -1.57055e-14 1 1.17791e-14 -1.57055e-14 1 1.17791e-14 -1.57055e-14 1 1.17791e-14 -1.57055e-14 1 1.17791e-14 -1.57055e-14 0.9999999 1.17791e-14 -1.57055e-14 -0.03427743 0.1181745 0.9924011 -0.01882985 0.04650465 -0.9987406 -0.01835955 0.0457616 -0.9987836 -0.01912932 0.05576557 -0.9982607 -0.9884216 0.1517326 -1.54469e-6 -0.9529538 0.3031157 -7.62511e-7 -0.9511699 0.3086678 -7.32725e-7 -0.9884216 0.1517326 -1.54469e-6 -0.6494867 0.7603731 0 -0.642369 0.7663956 0 -0.1515213 0.9884539 0 -0.1516465 0.9884348 0 -0.05633355 0.1317699 0.9896784 -0.05792006 0.1407148 0.9883545 -0.06555038 0.1639255 0.9842924 -0.03937946 0.1147998 0.9926078 -0.06183137 0.1454781 0.9874274 -0.01948966 0.04449693 0.9988195 0.1242694 -0.2096891 0.969839 -0.01666212 0.03351608 -0.9992993 -0.01191135 0.03131852 -0.9994385 0.02953718 -0.06261034 -0.9976009 -0.002763382 0.01061045 -0.9999399 0.177308 -0.3619098 -0.9151956 0.3905371 -0.8452066 -0.3648378 0.5000001 1.39119e-7 -0.8660254 0.5000001 1.39119e-7 -0.8660254 0.8660254 7.79778e-8 -0.5 0.8660254 7.79778e-8 -0.5000001 1 0 -1.86599e-7 1 9.36605e-15 -2.45525e-7 0.8660256 -8.24083e-8 0.4999998 0.8660255 -8.24083e-8 0.4999997 0.5000002 -1.44436e-7 0.8660253 0.5000002 -1.44436e-7 0.8660253 0 -1 0 0 -0.9999999 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 -2.22635e-7 1 2.22635e-7 -2.22635e-7 0.9999999 2.22635e-7 -2.22635e-7 1 2.22635e-7 -2.22635e-7 1 2.22635e-7 -2.22635e-7 1 2.22635e-7 -2.22635e-7 1 2.22635e-7 -2.22635e-7 1 2.22635e-7 0.3473733 0.7192546 -0.6016682 0.3473733 0.7192546 -0.6016682 0.6016682 0.7192547 -0.3473733 0.6016682 0.7192547 -0.3473733 0.6947466 0.7192546 -1.64329e-7 0.6947466 0.7192546 -1.55189e-7 0.6016681 0.7192546 0.3473734 0.6016681 0.7192547 0.3473734 0.3473731 0.7192546 0.6016683 0.3473732 0.7192546 0.6016683 -1 1.59364e-7 -4.38157e-8 -0.5000002 8.95749e-9 0.8660253 -0.4927216 0.1700067 0.8534185 -0.9854429 0.1700068 -7.0274e-8 0.4999998 -2.354e-7 0.8660256 0.4927213 0.1700067 0.8534188 1 -3.55913e-7 6.57235e-8 0.985443 0.1700066 9.36987e-8 -0.4393802 0.477264 0.7610283 -0.8787599 0.477264 -1.01412e-7 0.4393798 0.477264 0.7610286 0.87876 0.477264 7.88764e-8 3.36063e-7 1 -2.6885e-8 -0.4927214 0.1700068 -0.8534186 -0.5 1.95817e-8 -0.8660254 0.4927214 0.1700066 -0.8534186 0.4999999 -2.51337e-7 -0.8660254 -0.43938 0.477264 -0.7610285 0.4393799 0.477264 -0.7610286 -1 0 0 -1 0 0 -1 0 0 -1 0 0 0.9999999 0 0 0.9999999 0 0 0.9999999 0 0 0 0.9182646 0.3959674 0 0.9598919 0.2803704 0 0.9598919 0.2803704 0 0.9182646 0.3959674 -7.03448e-7 -0.5594578 -0.828859 -1.12545e-6 -0.1784747 -0.9839446 -1.12545e-6 -0.1784751 -0.9839444 -7.03448e-7 -0.5594578 -0.828859 0 -0.002872076 0.9999959 0 -0.08312213 0.9965394 0 -0.08312213 0.9965394 0 -0.002872076 0.9999959 0 0.9647087 -0.2633197 0 0.925374 -0.3790554 0 0.925374 -0.3790554 0 0.9647087 -0.2633197 0 0.9999635 0.008545281 0 0.9999635 0.008545281 0 -0.3587501 0.9334336 -2.44587e-9 -0.8550377 0.5185657 -2.44587e-9 -0.8550377 0.5185657 0 -0.3587501 0.9334336 -2.36967e-9 -0.9569065 -0.2903961 -2.36967e-9 -0.9569065 -0.2903961 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -8.90769e-7 -0.0630407 -0.998011 -8.90769e-7 -0.0630407 -0.998011 -1 0 0 -0.9999999 0 0 -1 0 0 1 0 0 1 0 0 0.9999999 0 0 1 0 0 -1 0 0 -1 0 0 1 0 0 1 0 0 1 0 0 1 0 0 -0.9999999 0 0 -0.9999999 0 0 -0.9999999 0 0 0 0.9182646 0.3959674 0 0.9182646 0.3959674 0 0.9598919 0.2803704 0 0.9598919 0.2803704 5.68926e-7 -0.5594578 -0.8288589 5.68926e-7 -0.5594578 -0.8288589 1.01261e-6 -0.1784751 -0.9839444 1.01261e-6 -0.1784747 -0.9839446 0 -0.002872076 0.9999959 0 -0.002872076 0.9999959 0 -0.08312213 0.9965394 0 -0.08312213 0.9965394 0 0.9647087 -0.2633197 0 0.9647087 -0.2633197 0 0.925374 -0.3790554 0 0.925374 -0.3790554 0 0.9999635 0.008545281 0 0.9999635 0.008545281 0 -0.3587501 0.9334336 0 -0.3587501 0.9334336 0 -0.8550377 0.5185657 0 -0.8550377 0.5185657 -1.78206e-8 -0.9569065 -0.2903961 -1.78206e-8 -0.9569065 -0.290396 1 0 0 1 0 0 -1 0 0 -1 0 0 -1 0 0 -1 0 0 8.90769e-7 -0.0630407 -0.998011 8.90769e-7 -0.0630407 -0.998011 1 0 0 0.9999999 0 0 1 0 0 -1 0 0 -1 0 0 -0.9999999 0 0 -1 0 0 1 0 0 1 0 0 + + + + + + + + + + 0.991057 0.6094222 0 0.9917967 0.6614385 0 0.9839013 0.7067277 0 0.3445118 0.4906312 0 0.08526292 0.5251576 0 0.1287412 0.3943396 0 0.3270248 0.3815657 0 0.09554852 0.8623219 0 0.07264107 0.8156047 0 0.3573347 0.5670778 0 0.0524618 0.6224915 0 0.6610828 0.8183016 0 0.6569276 0.8906822 0 0.361562 0.58987 0 0.04309042 0.6511338 0 0.8879871 0.4877244 0 0.1153645 0.9038144 0 0.6519234 0.9510814 0 0.1181069 0.9148815 0 0.8895604 0.4984099 0 0.3616583 0.6008948 0 0.3383214 0.2337245 0 0.9713069 0.2484444 0 0.9711661 0.254505 0 0.08402972 0.3654689 0 0.07801435 0.3654674 0 0.3381807 0.239785 0 0.6494874 0.9637509 0 0.07398809 0.3281865 0 0.1694829 0.2371654 0 0.08599068 0.3281893 0 0.01262125 0.6929203 0 0.6360269 0.6143854 0 0.9751854 0.7140738 0 0.9644991 0.7133419 0 0.9539892 0.6976969 0 0.9469331 0.640831 0 0.9562486 0.5646958 0 0.9737169 0.4734644 0 0.9797308 0.4733369 0 0.6573347 0.6450433 0 0.07268895 0.3131052 0 0.2937383 0.2827291 0 0.08633657 0.3131076 0 0.3089317 0.283468 0 0.669267 0.6542912 0 0.6841801 0.6520165 0 0.8923461 0.8194093 0 0.3199641 0.2728962 0 0.9394271 0.3425786 0 0.912883 0.4151847 0 0.8929329 0.4693824 0 0.04269529 0.6622527 0 0.0643795 0.5239632 0 0.1123721 0.3930084 0 0.02865361 0.6214415 0 0.9308782 0.9044638 0 0.9643151 0.9784456 0 0.9697632 0.9931603 0 0.8021658 0.5942514 0 0.9106965 0.8159626 0 0.9518903 0.9001184 0 0.9885777 0.9750686 0 0.01828056 0.6497432 0 0.9956362 0.9894894 0 0.01459204 0.6598067 0 0.07273981 0.2980243 0 0.08624999 0.2980253 0 0.07274593 0.1666552 0 0.08625673 0.1666553 0 0.004923361 0.9966148 0 0.02438729 0.9913994 0 0.2652021 0.02999086 0 0.2707498 0.009287906 0 0.03863581 0.9771509 0 0.2500467 0.0451469 0 0.04385154 0.9576866 0 0.2293435 0.0506943 0 0.03863614 0.9382228 0 0.20864 0.0451469 0 0.02438729 0.9239741 0 0.1934844 0.02999087 0 0.004923361 0.9187587 0 0.187937 0.009287917 0 0.2635585 0.009287906 0 0.2589746 0.02639527 0 0.2464511 0.03891893 0 0.2293435 0.04350264 0 0.2122359 0.03891893 0 0.1997121 0.02639528 0 0.1951281 0.009287917 0 0.01300555 0.01079731 0 0.03315613 0.01079731 0 0.03315613 0.1282894 0 0.01300555 0.1282894 0 0.05330711 0.01079731 0 0.05330711 0.1282894 0 0.07345775 0.01079731 0 0.07345775 0.1282894 0 0.09360833 0.01079731 0 0.09360833 0.1282894 0 0.1137589 0.01079731 0 0.1137589 0.1282894 0 0.1339095 0.01079731 0 0.1339095 0.1282894 0 0.03069195 0.2856472 0 0.03052663 0.3083949 0 0.0227879 0.3091352 0 0.015264 0.2870694 0 0.03781429 0.3075859 0 0.04017615 0.2853729 0 0.01246118 0.2478532 0 0.02921706 0.2479268 0 0.03929659 0.2482101 0 0.01523333 0.1962023 0 0.02727896 0.1986993 0 0.03566712 0.1996531 0 0.01960447 0.1609631 0 0.02602353 0.1629586 0 0.02626535 0.1674558 0 0.03142625 0.1681834 0 0.03234455 0.1724453 0 0.03006583 0.165942 0 0.02664836 0.5105492 0 0.01888445 0.509584 0 0.0195348 0.4868689 0 0.03480509 0.4887201 0 0.01193389 0.5085939 0 0.01019157 0.4863386 0 0.02227712 0.449232 0 0.03864864 0.4496018 0 0.01224555 0.4492508 0 0.02547421 0.4000906 0 0.03746906 0.3978921 0 0.01719868 0.4008328 0 0.02735482 0.3688894 0 0.02772205 0.3644824 0 0.03411508 0.3625515 0 0.02127807 0.373745 0 0.02230623 0.3695484 0 0.0236849 0.3673811 0 0.08301225 0.9982153 0 0.08921304 0.9874753 0 0.09097254 0.990523 0 0.08653137 0.9982153 0 0.1016146 0.9874753 0 0.09985507 0.990523 0 0.1078155 0.9982153 0 0.1042964 0.9982153 0 0.09311613 0.9942356 0 0.09081841 0.9982153 0 0.09771156 0.9942356 0 0.1000093 0.9982153 0 0.0954138 0.9982153 0 0.00439369 0.8598915 0 0.004393631 0.8541524 0 0.01464381 0.8541524 0 0.01464381 0.8598915 0 0.004393631 0.8484132 0 0.01464381 0.8484132 0 0.004393631 0.842674 0 0.01464381 0.842674 0 0.004393631 0.8369352 0 0.01464381 0.8369352 0 0.004393571 0.8311962 0 0.01464381 0.8311958 0 0.004393571 0.8254569 0 0.01464381 0.8254567 0 0.1720212 0.9985639 0 0.1735064 0.9930203 0 0.1775646 0.988962 0 0.1831082 0.9874766 0 0.1886517 0.988962 0 0.1927099 0.9930203 0 0.1941952 0.9985639 0 0.3415256 0.0631672 0 0.3369047 0.1178331 0 0.3336721 0.1210473 0 0.3313836 0.1168725 0 0.3406219 0.0630121 0 0.962397 0.1178207 0 0.9134955 0.01996307 0 0.9134955 0.01996307 0 0.9674023 0.1149765 0 0.9666662 0.1196795 0 0.9135695 0.2244072 0 0.3415694 0.181588 0 0.3369038 0.1271233 0 0.9623945 0.1266231 0 0.9640611 0.1221648 0 0.3365864 0.1223754 0 0.9134274 0.01904893 0 0.3414567 0.06225318 0 1.344512 0.4906312 0 1.085263 0.5251576 0 1.128741 0.3943396 0 1.327025 0.3815657 0 1.095549 0.8623219 0 1.072641 0.8156047 0 1.661083 0.8183016 0 1.656928 0.8906822 0 1.357335 0.5670778 0 1.052462 0.6224915 0 1.115364 0.9038144 0 1.651923 0.9510814 0 1.361562 0.58987 0 1.04309 0.6511338 0 1.118107 0.9148815 0 1.649487 0.9637509 0 1.887987 0.4877244 0 1.88956 0.4984099 0 1.361658 0.6008948 0 1.338321 0.2337245 0 1.971307 0.2484444 0 1.971166 0.254505 0 1.338181 0.239785 0 1.991057 0.6094222 0 1.991797 0.6614385 0 1.983901 0.7067277 0 1.975185 0.7140738 0 1.964499 0.7133419 0 1.953989 0.6976969 0 1.946933 0.640831 0 1.956249 0.5646958 0 1.973717 0.4734644 0 1.979731 0.4733369 0 1.169483 0.2371654 0 1.293738 0.2827291 0 1.012621 0.6929203 0 1.636027 0.6143854 0 1.657335 0.6450433 0 1.08403 0.3654689 0 1.078014 0.3654674 0 1.073988 0.3281865 0 1.085991 0.3281893 0 1.072689 0.3131052 0 1.086337 0.3131076 0 1.07274 0.2980243 0 1.08625 0.2980253 0 1.072746 0.1666552 0 1.086257 0.1666553 0 1.308932 0.283468 0 1.319964 0.2728962 0 1.939427 0.3425786 0 1.912883 0.4151847 0 1.892933 0.4693824 0 1.669267 0.6542912 0 1.68418 0.6520165 0 1.892346 0.8194093 0 1.930878 0.9044638 0 1.964315 0.9784456 0 1.969763 0.9931603 0 1.042695 0.6622527 0 1.802166 0.5942514 0 1.910697 0.8159626 0 1.064379 0.5239632 0 1.112372 0.3930084 0 1.028654 0.6214415 0 1.018281 0.6497432 0 1.95189 0.9001184 0 1.988578 0.9750686 0 1.995636 0.9894894 0 1.014592 0.6598067 0 1.013005 0.01079731 0 1.033156 0.01079731 0 1.033156 0.1282894 0 1.013005 0.1282894 0 1.053307 0.01079731 0 1.053307 0.1282894 0 1.073458 0.01079731 0 1.073458 0.1282894 0 1.093608 0.01079731 0 1.093608 0.1282894 0 1.113759 0.01079731 0 1.113759 0.1282894 0 1.133909 0.01079731 0 1.133909 0.1282894 0 1.004923 0.9187587 0 1.024387 0.9239741 0 1.038636 0.9382228 0 1.043851 0.9576866 0 1.038636 0.9771509 0 1.024387 0.9913994 0 1.004923 0.9966148 0 1.263559 0.009287906 0 1.258975 0.02639527 0 1.246451 0.03891893 0 1.229343 0.04350264 0 1.212236 0.03891893 0 1.199712 0.02639528 0 1.195128 0.009287917 0 1.265202 0.02999086 0 1.27075 0.009287906 0 1.250047 0.0451469 0 1.229343 0.0506943 0 1.20864 0.0451469 0 1.193484 0.02999087 0 1.187937 0.009287917 0 1.030692 0.2856472 0 1.030527 0.3083949 0 1.022788 0.3091352 0 1.015264 0.2870694 0 1.037814 0.3075859 0 1.040176 0.2853729 0 1.012461 0.2478532 0 1.029217 0.2479268 0 1.039297 0.2482101 0 1.015233 0.1962023 0 1.027279 0.1986993 0 1.035667 0.1996531 0 1.019604 0.1609631 0 1.026024 0.1629586 0 1.026265 0.1674558 0 1.031426 0.1681834 0 1.032345 0.1724453 0 1.030066 0.165942 0 1.026648 0.5105492 0 1.018884 0.509584 0 1.019535 0.4868689 0 1.034805 0.4887201 0 1.011934 0.5085939 0 1.010192 0.4863386 0 1.022277 0.449232 0 1.038649 0.4496018 0 1.012246 0.4492508 0 1.025474 0.4000906 0 1.037469 0.3978921 0 1.017199 0.4008328 0 1.027355 0.3688894 0 1.027722 0.3644824 0 1.034115 0.3625515 0 1.021278 0.373745 0 1.022306 0.3695484 0 1.023685 0.3673811 0 1.083012 0.9982153 0 1.089213 0.9874753 0 1.090973 0.990523 0 1.086531 0.9982153 0 1.101615 0.9874753 0 1.099855 0.990523 0 1.107816 0.9982153 0 1.104296 0.9982153 0 1.093116 0.9942356 0 1.090818 0.9982153 0 1.097712 0.9942356 0 1.100009 0.9982153 0 1.095414 0.9982153 0 1.004394 0.8598915 0 1.004394 0.8541524 0 1.014644 0.8541524 0 1.014644 0.8598915 0 1.004394 0.8484132 0 1.014644 0.8484132 0 1.004394 0.842674 0 1.014644 0.842674 0 1.004394 0.8369352 0 1.014644 0.8369352 0 1.004394 0.8311962 0 1.014644 0.8311958 0 1.004394 0.8254569 0 1.014644 0.8254567 0 1.172021 0.9985639 0 1.173506 0.9930203 0 1.177565 0.988962 0 1.183108 0.9874766 0 1.188652 0.988962 0 1.19271 0.9930203 0 1.194195 0.9985639 0 1.336905 0.1178331 0 1.341526 0.0631672 0 1.913496 0.01996307 0 1.962397 0.1178207 0 1.913569 0.2244072 0 1.341569 0.181588 0 1.336904 0.1271233 0 1.962394 0.1266231 0 1.333672 0.1210473 0 1.331384 0.1168725 0 1.340622 0.0630121 0 1.341457 0.06225318 0 1.913427 0.01904893 0 1.913496 0.01996307 0 1.967402 0.1149765 0 1.966666 0.1196795 0 1.964061 0.1221648 0 1.336586 0.1223754 0 0.08301225 0.9982153 0 0.08921304 0.9874753 0 0.09097254 0.990523 0 0.08653137 0.9982153 0 0.1016146 0.9874753 0 0.09985507 0.990523 0 0.1078155 0.9982153 0 0.1042964 0.9982153 0 0.09311613 0.9942356 0 0.09081841 0.9982153 0 0.09771156 0.9942356 0 0.1000093 0.9982153 0 0.0954138 0.9982153 0 1.083012 0.9982153 0 1.089213 0.9874753 0 1.090973 0.990523 0 1.086531 0.9982153 0 1.101615 0.9874753 0 1.099855 0.990523 0 1.107816 0.9982153 0 1.104296 0.9982153 0 1.093116 0.9942356 0 1.090818 0.9982153 0 1.097712 0.9942356 0 1.100009 0.9982153 0 1.095414 0.9982153 0 0.644373 0.98832 0 0.6317615 0.9883068 0 0.2442412 0.942984 0 0.204701 0.9430045 0 0.1655418 0.9430304 0 0.1233784 0.9430614 0 0.07755148 0.9430959 0 0.6577327 0.9883361 0 0.6443719 0.9941173 0 0.6317617 0.9941286 0 0.6094946 0.9941443 0 0.2442441 0.9488453 0 0.2047048 0.9488718 0 0.1655462 0.9489023 0 0.123383 0.9489362 0 0.07755601 0.9489715 0 0.6577303 0.9941092 0 0.1525673 0.1001417 0 0.1689451 0.09962992 0 0.1845458 0.09521351 0 0.1991272 0.08563405 0 0.2119602 0.07168221 0 0.2471925 0.2360484 0 0.2446425 0.2410481 0 0.2396514 0.2427586 0 0.2886854 0.223971 0 0.2760261 0.2343747 0 0.2451099 0.078647 0 0.2475947 0.06970912 0 0.2524763 0.06770821 0 0.2576886 0.06978977 0 0.3394589 0.2166612 0 0.320613 0.2146183 0 0.3033322 0.2170166 0 0.5194229 0.9661904 0 0.5194198 0.9719514 0 0.226799 0.9884464 0 0.226802 0.9943075 0 0.1210463 0.9659684 0 0.1210439 0.9717415 0 0.2340718 0.1309766 0 0.3704957 0.9661055 0 0.1771485 0.1547609 0 0.370897 0.9883911 0 0.3170341 0.1574846 0 0.3704929 0.9718714 0 0.2585391 0.1770888 0 0.3708995 0.9942456 0 0.2318942 0.2376709 0 0.6094928 0.9882984 0 1.177148 0.1547609 0 1.234072 0.1309766 0 1.247193 0.2360484 0 1.231894 0.2376709 0 1.317034 0.1574846 0 1.339459 0.2166612 0 1.320613 0.2146183 0 1.077551 0.9430959 0 1.123378 0.9430614 0 1.123383 0.9489362 0 1.077556 0.9489715 0 1.121046 0.9659684 0 1.370496 0.9661055 0 1.370493 0.9718714 0 1.121044 0.9717415 0 1.226799 0.9884464 0 1.370897 0.9883911 0 1.370899 0.9942456 0 1.226802 0.9943075 0 1.204701 0.9430045 0 1.244241 0.942984 0 1.244244 0.9488453 0 1.204705 0.9488718 0 1.165542 0.9430304 0 1.165546 0.9489023 0 1.609493 0.9882984 0 1.631762 0.9883068 0 1.631762 0.9941286 0 1.609495 0.9941443 0 1.644373 0.98832 0 1.657733 0.9883361 0 1.65773 0.9941092 0 1.644372 0.9941173 0 1.152567 0.1001417 0 1.168945 0.09962992 0 1.24511 0.078647 0 1.247595 0.06970912 0 1.252476 0.06770821 0 1.257689 0.06978977 0 1.519423 0.9661904 0 1.51942 0.9719514 0 1.184546 0.09521351 0 1.199127 0.08563405 0 1.21196 0.07168221 0 1.303332 0.2170166 0 1.258539 0.1770888 0 1.288685 0.223971 0 1.276026 0.2343747 0 1.244642 0.2410481 0 1.239651 0.2427586 0 + + + + + + + + + + 0.7117885 0.02192005 0 0.962148 0.02192 0 0.3342428 0.6219453 0 0.5405073 0.3171241 0 0.06112731 0.4034153 0 0.05959666 0.3990169 0 0.309315 0.06882671 0 0.6354422 0.2667694 0 0.3342428 0.600547 0 0.6372176 0.3232146 0 0.5626997 0.356372 0 0.309315 0.06408487 0 0.5510426 0.7081062 0 0.5246879 0.2896474 0 0.5363998 0.7340767 0 0.6341767 0.2245483 0 0.962148 0.02445435 0 0.7117885 0.0244544 0 0.3455285 0.2577011 0 0.3342428 0.6405018 0 0.3455288 0.2602229 0 0.5237387 0.7552323 0 0.5203052 0.2823491 0 0.6338261 0.2128255 0 0.6436974 0.3892787 0 0.5114441 0.06408487 0 0.3342428 0.5444822 0 0.5786791 0.6283309 0 0.2808295 0.3985932 0 0.2803006 0.4038391 0 0.5810058 0.6434876 0 0.5837389 0.6490568 0 0.5896583 0.6509132 0 0.5894083 0.3892787 0 0.5834616 0.3911436 0 0.5807158 0.3967386 0 0.5109794 0.06882676 0 0.6362851 0.7503968 0 0.6351136 0.7888309 0 0.5783783 0.4119657 0 0.6341007 0.8220512 0 0.3328117 0.4962021 0 0.6339059 0.8272931 0 0.239978 0.3269021 0 0.2440508 0.3269021 0 0.2630213 0.3776577 0 0.2589485 0.3776577 0 0.2589485 0.3530728 0 0.2598956 0.3509252 0 0.266001 0.3420251 0 0.267732 0.3427494 0 0.2440508 0.351487 0 0.2797447 0.3709783 0 0.2779449 0.3705733 0 0.2630213 0.3530728 0 0.2670421 0.346001 0 0.2770952 0.3764377 0 0.2780563 0.3673074 0 0.2798794 0.3670259 0 0.2763062 0.3750712 0 0.7872447 0.2457788 0 0.7873552 0.2427659 0 0.7965195 0.2422379 0 0.7963158 0.2483609 0 0.78751 0.2397516 0 0.7964013 0.2382587 0 0.6521086 0.4562721 0 0.6515402 0.4655035 0 0.6487074 0.4656059 0 0.6461065 0.4564689 0 0.6547784 0.4653385 0 0.8117629 0.2419719 0 0.8121926 0.2484036 0 0.811397 0.2376636 0 0.8316573 0.2413394 0 0.8329612 0.246241 0 0.8310373 0.2377136 0 0.8442576 0.2406831 0 0.846033 0.2406337 0 0.8470542 0.2433667 0 0.8420517 0.2382205 0 0.8437769 0.2384745 0 0.8447059 0.2389826 0 0.2798869 0.3569664 0 0.2839889 0.3623274 0 0.2798869 0.3546483 0 0.2817166 0.3546483 0 0.2839889 0.3646455 0 0.5979604 0.480852 0 0.4659228 0.2374744 0 0.4659228 0.2399015 0 0.5979604 0.4784249 0 0.2820718 0.373235 0 0.2820718 0.3707514 0 0.2695574 0.3526779 0 0.2695574 0.3550998 0 0.2650791 0.3550998 0 0.2650791 0.3526779 0 0.286664 0.3707514 0 0.286664 0.373235 0 0.2695574 0.3575218 0 0.2650791 0.3575218 0 0.2695574 0.3592947 0 0.2650791 0.3592947 0 0.2745944 0.3671629 0 0.4634737 0.2215011 0 0.371469 0.1656763 0 0.4853454 0.2078106 0 0.440519 0.207501 0 0.1364561 0.1656763 0 0.440505 0.2071253 0 0.4852557 0.2054524 0 0.4414084 0.2234515 0 0.4399091 0.2223517 0 0.4868279 0.2066082 0 0.3328117 0.4586626 0 0.3328117 0.4272884 0 0.3328117 0.403732 0 0.3328117 0.397176 0 0.5213034 0.7582103 0 0.6436974 0.6509132 0 0.6436974 0.7524434 0 0.6436974 0.7910333 0 0.6436974 0.2642702 0 0.6436974 0.3214084 0 0.6388716 0.1651713 0 0.4196984 0.165595 0 0.6331173 0.06408492 0 0.63308 0.06882676 0 0.4191694 0.1603491 0 0.6404022 0.1607728 0 0.6436974 0.06408492 0 0.2798807 0.6396669 0 0.2798806 0.6421316 0 0.02245928 0.6421316 0 0.02245939 0.6396668 0 0.6436974 0.2214581 0 0.6436974 0.2093505 0 0.7468875 0.3171241 0 0.7246952 0.356372 0 0.6436974 0.8253028 0 0.8909824 0.2378014 0 0.8917999 0.2532491 0 0.6436974 0.8307416 0 0.9531522 0.6219453 0 0.7509952 0.7340767 0 0.7363523 0.7081062 0 0.9531522 0.600547 0 0.9531522 0.6405018 0 0.7636562 0.7552323 0 0.239978 0.351487 0 0.2470322 0.3269021 0 0.2651661 0.3459601 0 0.2501514 0.3527598 0 0.2501514 0.3779706 0 0.247094 0.3779706 0 0.2718974 0.3457083 0 0.247094 0.3527598 0 0.2718772 0.3496629 0 0.2429175 0.3779706 0 0.2429175 0.3527598 0 0.256009 0.3509252 0 0.2658344 0.349663 0 0.2675943 0.3490609 0 0.2790541 0.3723233 0 0.2700665 0.3493102 0 0.2700832 0.3460425 0 0.2804205 0.3731123 0 0.2725525 0.3776549 0 0.6281314 0.414696 0 0.6221294 0.4144993 0 0.600773 0.4653889 0 0.5981908 0.4563177 0 0.604314 0.4561141 0 0.6037858 0.4652783 0 0.6082931 0.4562322 0 0.6247303 0.4053623 0 0.6068002 0.4651234 0 0.5981482 0.440441 0 0.627563 0.4054646 0 0.6045798 0.4408706 0 0.6088883 0.4412365 0 0.6003108 0.4196723 0 0.6308011 0.4056296 0 0.6052124 0.4209763 0 0.6088381 0.4215962 0 0.6058688 0.4083759 0 0.6031851 0.4055793 0 0.605918 0.4066006 0 0.6083313 0.4105817 0 0.6080772 0.4088566 0 0.6075692 0.4079276 0 0.2817166 0.3569664 0 0.2817166 0.3592844 0 0.2798869 0.3592844 0 0.2821591 0.3646455 0 0.2821591 0.3623274 0 0.2839889 0.3669635 0 0.2821591 0.3669635 0 0.6005201 0.4792608 0 0.4633631 0.2399015 0 0.6005201 0.480852 0 0.2820718 0.3689333 0 0.286664 0.3689333 0 0.2724678 0.3526779 0 0.2769461 0.3526779 0 0.2769461 0.3550998 0 0.2724678 0.3550998 0 0.2769461 0.3575218 0 0.2724678 0.3575218 0 0.2769461 0.3592947 0 0.2724678 0.3592947 0 0.2745944 0.3694741 0 0.2805839 0.8196483 0 0.04526968 0.06871392 0 0.9049747 0.2078104 0 0.9034945 0.2066053 0 0.9050688 0.2054525 0 0.04526968 0.06833798 0 0.9264595 0.2213758 0 0.9498159 0.207208 0 0.022865 0.8333253 0 0.9498011 0.2075837 0 0.3687896 0.2057421 0 0.4246382 0.2091976 0 0.4246384 0.2116624 0 0.3874158 0.2196126 0 0.3563065 0.2240916 0 0.3329489 0.2216959 0 0.3264482 0.2175264 0 0.3260552 0.213155 0 0.3289889 0.2095204 0 0.3474738 0.2058917 0 0.8448932 0.2057421 0 0.8662091 0.2058917 0 0.884694 0.2095204 0 0.8876277 0.213155 0 0.8872348 0.2175264 0 0.880734 0.2216959 0 0.8573765 0.2240916 0 0.8262672 0.2196126 0 0.7890446 0.2116624 0 0.7890447 0.2091976 0 0.4401195 0.2433462 0 0.4401195 0.2377969 0 0.4460301 0.2377607 0 0.4460301 0.2433663 0 0.3861606 0.2433464 0 0.3861606 0.237797 0 0.3322018 0.2433462 0 0.3262912 0.2433663 0 0.3262912 0.2377607 0 0.3322018 0.2377969 0 0.6562821 0.4563827 0 0.6460636 0.4404769 0 0.6524008 0.4409057 0 0.6568816 0.4412783 0 0.6482419 0.4195576 0 0.6531013 0.4208538 0 0.6568311 0.4214954 0 0.6511371 0.4053623 0 0.6538898 0.406391 0 0.6538034 0.4081556 0 0.6560647 0.4086633 0 0.6563205 0.410401 0 0.6555529 0.4077276 0 0.6501772 0.3232146 0 0.6519527 0.2667694 0 0.7627069 0.2896474 0 0.6532182 0.2245483 0 0.7670896 0.2823491 0 0.6535687 0.2128255 0 0.6979865 0.3892787 0 0.7039334 0.3911436 0 0.706679 0.3967386 0 0.9545833 0.4962021 0 0.7090165 0.4119657 0 0.9545833 0.4586626 0 0.9545833 0.4272884 0 0.9545833 0.403732 0 0.9545833 0.397176 0 0.9407476 0.1650625 0 0.7041439 0.1597421 0 0.7010481 0.1374013 0 0.9587584 0.1237205 0 0.700837 0.1358774 0 0.9593728 0.12231 0 0.3300809 0.2619154 0 0.3300809 0.2568836 0 0.3244048 0.2624614 0 0.3244048 0.2567397 0 0.8867681 0.2532491 0 0.8884605 0.2378012 0 0.8919438 0.2589251 0 0.8862221 0.2589251 0 0.8726985 0.2456742 0 0.8710445 0.2484924 0 0.8682029 0.2501061 0 0.8649352 0.2500827 0 0.8621169 0.2484286 0 0.8605033 0.245587 0 0.8605267 0.2423192 0 0.8621808 0.239501 0 0.8650224 0.2378874 0 0.8682901 0.2379108 0 0.8711084 0.2395649 0 0.872722 0.2424065 0 0.632305 0.4145854 0 0.6220863 0.4304912 0 0.6284236 0.4300624 0 0.6329044 0.4296899 0 0.6242647 0.4514106 0 0.6291242 0.4501144 0 0.6328539 0.4494727 0 0.6271598 0.4656058 0 0.6298262 0.4628125 0 0.6299126 0.4645771 0 0.6323434 0.4605671 0 0.6320876 0.4623048 0 0.6315758 0.4632405 0 0.280636 0.4629306 0 0.2783763 0.4853776 0 0.04212987 0.4995334 0 0.02258609 0.4588906 0 0.02192 0.4575054 0 0.2807895 0.4614053 0 0.2605765 0.6957551 0 0.280119 0.7363954 0 0.02207782 0.7323514 0 0.02433669 0.7099105 0 0.2807858 0.737782 0 0.02192368 0.7338821 0 0.4414139 0.2211195 0 0.4634737 0.2211195 0 0.9264886 0.2217563 0 0.9044933 0.223444 0 0.9028986 0.2223305 0 0.9043094 0.2211192 0 0.6436974 0.06882676 0 0.9780799 0.06882671 0 0.7764156 0.06882676 0 0.7759508 0.06408487 0 0.9780799 0.06408487 0 0.6543149 0.06882676 0 0.6542777 0.06408492 0 0.0408768 0.7919807 0 0.2774873 0.7973014 0 0.2807942 0.8211668 0 0.0222511 0.8347344 0 0.9531522 0.5444822 0 0.7063891 0.6434876 0 0.7087158 0.6283309 0 0.7036561 0.6490568 0 0.6977366 0.6509132 0 0.6522812 0.7888309 0 0.6511098 0.7503968 0 0.6532941 0.8220512 0 0.7660915 0.7582103 0 0.6534889 0.8272931 0 0.6247225 0.4785935 0 0.6242734 0.4809756 0 0.6225635 0.4809756 0 0.6221144 0.4785935 0 0.6234185 0.4830785 0 0.3615006 0.261165 0 0.3588926 0.261165 0 0.3593417 0.2587829 0 0.3610514 0.2587829 0 0.3601966 0.25668 0 0.4633631 0.2383103 0 0.4611034 0.2399015 0 0.4659228 0.2423285 0 0.4633631 0.2414925 0 0.6027798 0.480852 0 0.6005201 0.4824431 0 0.5979604 0.483279 0 0.1364561 0.1653004 0 0.371469 0.1653004 0 0.2802826 0.06833798 0 0.2802826 0.06871392 0 0.2866472 0.6156204 0 0.2854568 0.6176485 0 0.2834119 0.6188097 0 0.2810603 0.6187929 0 0.2790322 0.6176025 0 0.2778709 0.6155576 0 0.2778877 0.613206 0 0.2790781 0.6111778 0 0.281123 0.6100166 0 0.2834747 0.6100335 0 0.2855028 0.6112238 0 0.286664 0.6132688 0 0.2616158 0.6092778 0 0.2636175 0.6058672 0 0.2670564 0.6039144 0 0.2710109 0.6039427 0 0.2744215 0.6059445 0 0.2763743 0.6093833 0 0.2763459 0.6133379 0 0.2743442 0.6167484 0 0.2709053 0.6187011 0 0.2669508 0.6186728 0 0.2635403 0.6166711 0 0.2615875 0.6132323 0 0.2668757 0.3396312 0 0.265173 0.3396312 0 0.265173 0.3371403 0 0.2668757 0.3371403 0 0.283992 0.3591565 0 0.283992 0.3566656 0 0.2856948 0.3566656 0 0.2856948 0.3591565 0 0.2725525 0.376077 0 0.2660164 0.3739381 0 0.2673829 0.3731491 0 0.2701308 0.375897 0 0.2693418 0.3772635 0 0.2647992 0.3693954 0 0.2663771 0.3693954 0 0.2770952 0.3623533 0 0.2804205 0.3656787 0 0.2790541 0.3664676 0 0.2763062 0.3637197 0 0.2725525 0.3611361 0 0.2725525 0.3627139 0 0.2660164 0.3648529 0 0.2693418 0.3615274 0 0.2701308 0.3628939 0 0.2673829 0.3656418 0 0.256009 0.3274639 0 0.2598956 0.3274639 0 0.2521223 0.3509252 0 0.2521223 0.3274639 0 0.2703209 0.3694741 0 0.2703209 0.3671629 0 0.2745944 0.3717853 0 0.2703209 0.3717853 0 0.238741 0.3779706 0 0.238741 0.3527598 0 0.2356836 0.3779706 0 0.2356836 0.3527598 0 0.2548757 0.3776577 0 0.2548757 0.3530728 0 0.2518942 0.3776577 0 0.2518942 0.3530728 0 0.286664 0.3757186 0 0.2820718 0.3757186 0 0.286664 0.3775367 0 0.2820718 0.3775367 0 0.2470322 0.351487 0 0.2359052 0.351487 0 0.2359052 0.3269021 0 0.2798869 0.3569664 0 0.2839889 0.3623274 0 0.2798869 0.3546483 0 0.2817166 0.3546483 0 0.2839889 0.3646455 0 0.5979604 0.480852 0 0.4659228 0.2374744 0 0.4659228 0.2399015 0 0.5979604 0.4784249 0 0.2817166 0.3569664 0 0.2817166 0.3592844 0 0.2798869 0.3592844 0 0.2821591 0.3646455 0 0.2821591 0.3623274 0 0.2839889 0.3669635 0 0.2821591 0.3669635 0 0.6005201 0.4792608 0 0.4633631 0.2399015 0 0.6005201 0.480852 0 0.6247225 0.4785935 0 0.6242734 0.4809756 0 0.6225635 0.4809756 0 0.6221144 0.4785935 0 0.6234185 0.4830785 0 0.3615006 0.261165 0 0.3588926 0.261165 0 0.3593417 0.2587829 0 0.3610514 0.2587829 0 0.3601966 0.25668 0 0.4633631 0.2383103 0 0.4611034 0.2399015 0 0.4659228 0.2423285 0 0.4633631 0.2414925 0 0.6027798 0.480852 0 0.6005201 0.4824431 0 0.5979604 0.483279 0 0.2668757 0.3396312 0 0.265173 0.3396312 0 0.265173 0.3371403 0 0.2668757 0.3371403 0 0.283992 0.3591565 0 0.283992 0.3566656 0 0.2856948 0.3566656 0 0.2856948 0.3591565 0 142.9615 1.653531 5 141.9877 12.46339 5 -198.9593 56.40309 5.00001 -186.1815 25.20926 5.00001 -181.3901 -7.799919 5.00001 -187.1521 -43.22592 5.00001 -202.5938 -79.03571 5.00001 135.7093 -7.330697 5 142.9615 1.653531 -1.71515e-9 141.9877 12.46339 -2.84147e-9 125.9234 22.76675 -6.60459e-10 -198.9593 56.40309 9.45723e-6 -186.1815 25.20926 9.91314e-6 -181.3901 -7.799919 9.91314e-6 -187.1521 -43.22592 1.0236e-5 -202.5938 -79.03571 1.0359e-5 135.7093 -7.330697 -1.1487e-8 -73.4422 -60.63268 6.572104 -76.20958 50.38225 5.000006 -74.42668 -61.66427 1.601273 -76.04163 49.76011 5.61901e-6 125.9234 22.76675 5 -76.20958 50.38225 5.000006 -73.4422 -60.63268 6.572104 135.7093 -7.330697 5 125.9234 22.76675 5 -74.42668 -61.66427 1.601273 -202.5938 -79.03571 1.0359e-5 -187.1521 -43.22592 1.0236e-5 -202.5938 -79.03571 5.00001 -187.1521 -43.22592 5.00001 135.7093 -7.330697 -1.1487e-8 -198.9593 56.40309 5.00001 -76.04163 49.76011 5.61901e-6 -198.9593 56.40309 9.45723e-6 -186.1815 25.20926 5.00001 -186.1815 25.20926 9.91314e-6 -181.3901 -7.799919 5.00001 -181.3901 -7.799919 9.91314e-6 141.9877 12.46339 5 141.9877 12.46339 -2.84147e-9 125.9234 22.76675 -6.60459e-10 142.9615 1.653531 5 142.9615 1.653531 -1.71515e-9 + + + + + + + + + + + + + + + + 4 4 4 4 4 4 4 4 10 4 3 4 4 4 4 3 3 3 4 4 4 4 3 4 4 4 4 3 3 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 7 7 4 4 4 4 4 4 4 4 4 4 4 4 4 4 10 4 3 4 4 4 4 3 3 3 4 4 4 4 3 4 4 4 4 3 3 4 4 4 4 4 4 4 4 3 4 4 4 4 4 4 4 7 7 4 4 4 4 4 4 4 4 4 4 4 4 3 3 3 4 4 4 4 4 4 3 3 3 4 3 4 4 4 4 4 4 4 4 4 3 4 4 4 3 3 3 3 3 3 3 3 4 4 4 3 4 4 4 4 4 4 4 4 4 3 4 4 4 3 3 3 3 3 3 3 3 4 4 +

23 0 3 34 1 4 33 2 5 22 3 6 8 4 7 2 5 8 26 6 11 27 7 12 24 8 9 35 9 10 34 1 4 23 0 3 4 10 16 8 4 7 27 7 12 28 11 17 25 12 13 36 13 14 35 9 10 24 8 9 5 14 18 4 10 16 28 11 17 29 15 27 6 16 15 5 14 19 29 15 20 25 12 13 17 17 21 1 18 22 0 19 23 16 20 26 2 21 0 8 22 1 4 23 2 5 24 33 6 25 34 7 26 35 9 27 36 3 28 37 0 29 38 1 30 39 15 31 29 13 32 42 22 3 6 33 2 5 1 33 31 17 34 32 19 35 40 17 36 24 16 37 25 18 38 28 19 39 30 18 38 28 20 40 41 21 41 43 19 39 30 13 42 66 14 43 67 21 41 43 20 40 41 15 44 68 90 45 69 14 43 67 13 42 66 20 46 44 18 47 48 22 3 6 22 3 6 13 32 42 20 46 44 18 47 48 16 48 26 0 49 23 18 47 48 0 49 23 3 50 49 22 3 6 22 3 6 3 50 49 9 51 50 23 0 3 23 0 3 9 51 50 7 52 51 24 8 9 24 8 9 7 52 51 6 16 15 25 12 13 21 53 45 14 54 46 26 6 11 26 6 11 37 55 47 30 56 56 27 7 12 27 7 12 30 56 56 31 57 57 28 11 17 28 11 17 31 57 57 32 58 58 29 15 27 29 15 20 32 58 52 36 13 14 25 12 13 90 59 59 40 60 60 37 55 47 19 35 40 21 53 45 26 6 11 26 6 11 2 5 8 1 33 31 19 35 40 34 1 4 41 61 53 10 62 54 33 2 5 35 9 10 39 63 55 41 61 53 34 1 4 36 13 14 11 64 63 39 63 55 35 9 10 37 55 47 40 60 60 38 65 61 30 56 56 30 56 56 38 65 61 42 66 62 31 57 57 31 57 57 42 66 62 12 67 64 32 58 58 32 58 52 12 67 65 11 64 63 36 13 14 33 2 5 10 62 54 15 31 29 37 55 47 26 6 11 14 54 46 90 59 59 95 68 91 43 69 92 50 70 93 49 71 94 43 69 92 44 72 95 51 73 96 50 70 93 44 72 95 45 74 97 52 75 98 51 73 96 45 74 97 46 76 99 53 77 100 52 75 98 46 76 99 47 78 101 54 79 102 53 77 100 47 78 101 48 80 103 106 81 104 54 79 102 48 82 82 47 83 80 46 84 78 45 85 76 44 86 74 43 87 71 95 88 70 107 89 84 55 90 85 56 91 86 57 92 87 58 93 88 59 94 89 113 95 90 50 96 72 55 97 85 107 98 84 49 99 73 51 100 75 56 101 86 55 97 85 50 96 72 52 102 77 57 103 87 56 101 86 51 100 75 53 104 79 58 105 88 57 103 87 52 102 77 54 106 81 59 107 89 58 105 88 53 104 79 106 108 83 113 109 90 59 107 89 54 106 81 60 110 193 63 111 196 62 112 195 61 113 194 64 114 197 67 115 200 66 116 199 65 117 198 68 118 201 60 110 193 61 113 194 69 119 202 70 120 203 71 121 204 67 115 200 64 114 197 72 122 205 68 118 201 69 119 202 73 123 206 74 124 207 75 125 208 71 121 204 70 120 203 76 126 209 72 122 205 75 125 211 74 124 210 77 127 212 80 128 215 79 129 214 78 130 213 65 131 216 78 132 225 79 133 224 83 134 223 82 135 222 81 136 221 76 137 220 74 138 219 70 139 218 64 140 217 15 31 226 62 112 195 63 111 196 84 141 227 78 142 228 85 143 230 77 144 229 77 145 231 85 146 234 86 147 233 80 148 232 86 147 233 85 146 234 88 149 236 87 150 235 84 151 237 87 150 235 88 149 236 89 152 238 15 44 239 84 151 237 89 152 238 90 45 240 87 153 241 63 111 196 86 154 242 63 111 196 87 153 241 84 141 227 86 154 242 79 155 214 80 156 215 86 154 242 63 111 196 83 157 243 79 155 214 63 111 196 60 110 193 82 158 244 83 157 243 60 110 193 68 118 201 81 159 245 82 158 244 68 118 201 72 122 205 76 126 209 81 159 245 88 160 246 66 116 199 89 161 247 66 116 199 67 115 200 92 162 249 91 163 248 67 115 200 71 121 204 93 164 250 92 162 249 71 121 204 75 125 208 94 165 251 93 164 250 75 125 211 72 122 205 73 123 206 94 165 252 90 59 253 91 163 248 40 60 254 85 143 230 66 116 199 88 160 246 66 116 199 85 143 230 78 142 228 65 117 198 61 113 194 62 112 195 10 62 256 41 61 255 69 119 202 61 113 194 41 61 255 39 63 257 73 123 206 69 119 202 39 63 257 11 64 258 91 163 248 92 162 249 38 65 259 40 60 254 92 162 249 93 164 250 42 66 260 38 65 259 93 164 250 94 165 251 12 67 261 42 66 260 94 165 252 73 123 206 11 64 258 12 67 262 62 112 195 15 31 226 10 62 256 91 163 248 90 59 253 89 161 247 66 116 199 95 68 263 49 71 266 97 166 265 96 167 264 96 167 264 97 166 265 99 168 268 98 169 267 98 169 267 99 168 268 101 170 270 100 171 269 100 171 269 101 170 270 103 172 272 102 173 271 102 173 271 103 172 272 105 174 274 104 175 273 104 175 273 105 174 274 106 81 276 48 80 275 48 176 277 95 177 283 96 178 282 98 179 281 100 180 280 102 181 279 104 182 278 107 183 284 113 184 290 112 185 289 111 186 288 110 187 287 109 188 286 108 189 285 97 190 291 49 99 292 107 98 284 108 191 285 99 192 293 97 190 291 108 191 285 109 193 286 101 194 294 99 192 293 109 193 286 110 195 287 103 196 295 101 194 294 110 195 287 111 197 288 105 198 296 103 196 295 111 197 288 112 199 289 106 108 297 105 198 296 112 199 289 113 109 290 123 200 386 114 201 387 117 202 388 116 203 389 114 201 387 115 204 390 118 205 391 117 202 388 115 204 390 128 206 392 129 207 393 118 205 391 116 203 389 117 202 388 119 208 394 122 209 395 117 202 388 118 205 391 120 210 396 119 208 394 118 205 391 129 207 393 121 211 397 120 210 396 122 209 395 119 208 394 132 212 398 119 208 394 120 210 396 132 212 398 120 210 396 121 211 397 132 212 398 123 200 399 116 203 402 125 213 401 124 214 400 124 214 400 125 213 401 127 215 404 126 216 403 126 216 403 127 215 404 129 207 406 128 206 405 116 203 402 122 209 408 130 217 407 125 213 401 125 213 401 130 217 407 131 218 409 127 215 404 127 215 404 131 218 409 121 211 410 129 207 406 122 209 408 132 212 411 130 217 407 130 217 407 132 212 411 131 218 409 131 218 409 132 212 411 121 211 410 151 219 454 150 220 452 140 221 434 154 222 460 152 223 456 148 224 443 147 225 444 139 226 418 138 227 417 147 228 426 148 229 427 140 230 450 150 231 453 152 232 457 149 233 451 135 234 448 151 235 455 153 236 459 144 237 449 136 238 415 135 239 414 144 240 423 145 241 424 138 227 417 137 242 416 146 243 425 147 228 426 137 242 416 136 238 415 145 241 424 146 243 425 154 244 461 134 245 413 142 246 421 143 247 422 133 248 412 140 230 419 149 233 428 141 249 420 134 245 413 133 248 412 141 249 420 142 246 421 151 219 454 135 250 429 136 251 430 143 252 439 142 253 440 141 254 441 149 255 442 150 231 453 139 256 446 148 257 447 152 232 457 151 235 455 154 244 461 143 247 422 153 236 459 136 251 430 137 258 431 151 219 454 137 258 431 150 220 452 151 219 454 138 259 432 139 260 433 150 220 452 150 220 452 137 258 431 138 259 432 146 261 445 153 262 458 152 223 456 152 223 456 147 225 444 146 261 445 145 263 437 144 264 438 153 262 458 153 262 458 146 261 445 145 263 437 140 221 434 133 265 435 134 266 436 154 222 460 149 255 442 152 223 456 153 262 458 143 252 439 155 267 462 158 268 465 157 269 464 156 270 463 159 271 466 161 272 468 160 273 467 162 274 469 160 275 472 161 276 471 163 277 470 157 278 473 164 279 476 159 280 475 156 281 474 165 282 477 167 283 480 166 284 479 155 285 478 168 286 481 169 287 484 167 288 483 165 289 482 163 277 470 161 276 471 171 290 486 170 291 485 170 291 485 171 290 486 169 287 484 168 286 481 158 292 487 174 293 490 173 294 489 172 295 488 175 296 491 176 297 494 164 279 493 157 278 492 172 295 488 173 294 489 176 297 494 175 296 491 155 267 462 168 298 496 165 299 495 174 300 497 164 301 500 176 302 499 173 303 498 156 281 474 159 280 475 160 304 502 162 305 501 155 285 478 166 284 479 174 293 490 158 292 487 168 298 496 155 267 462 170 306 503 170 306 503 155 267 462 156 270 463 163 307 504 156 270 463 162 308 505 156 270 463 163 307 504 170 306 503 171 309 506 159 271 466 166 310 507 159 271 466 171 309 506 161 272 468 169 311 508 166 310 507 167 312 509 166 310 507 169 311 508 171 309 506 157 269 464 158 268 465 172 313 511 175 314 510 164 301 500 174 300 497 166 310 507 159 271 466

+ + + + + + + 159.9404 -53.78705 -1.874754 146.4978 -0.0682373 -6.622977 680.3284 229.024 -6.622977 759.2513 151.4088 -1.719254 680.3284 229.024 -7.552118 146.4978 -0.0682373 -7.552118 159.9371 -53.77365 -7.552118 759.2487 151.4114 -7.552118 160.8539 -57.43716 -4.552118 761.9413 148.7634 -4.552118 + + + + + + + + + + 0.09980783 -0.2950636 0.9502504 0.1254768 -0.3691205 0.9208723 -0.0222517 0.0577016 0.9980859 -0.0222517 0.05770161 0.9980859 0 0 -1 0.1403501 -0.4095334 -0.9014345 0.1163292 -0.3394419 -0.933406 0 0 -1 -0.9700876 -0.2427552 -3.84271e-6 -0.9700876 -0.2427552 -3.84271e-6 -0.9700876 -0.2427552 -3.84271e-6 -0.9700875 -0.2427552 -3.84271e-6 -0.9700876 -0.2427552 -3.84271e-6 -0.3943667 0.9189531 0 -0.3943667 0.9189531 0 -0.3943667 0.9189531 0 -0.3943667 0.9189531 0 0.7011749 0.7129893 -4.75289e-6 0.701175 0.7129893 -4.75289e-6 0.7011749 0.7129893 -4.75289e-6 0.701175 0.7129893 -4.75289e-6 0.7011749 0.7129893 -4.75289e-6 0.3240692 -0.9458407 0.01908834 0.3240864 -0.9458901 0.01612872 + + + + + + + + + + 1.336905 0.1178331 0 1.341526 0.0631672 0 1.913496 0.01996307 0 1.962397 0.1178207 0 1.913569 0.2244072 0 1.341569 0.181588 0 1.336904 0.1271233 0 1.962394 0.1266231 0 1.333672 0.1210473 0 1.331384 0.1168725 0 1.340622 0.0630121 0 1.341457 0.06225318 0 1.913427 0.01904893 0 1.913496 0.01996307 0 1.967402 0.1149765 0 1.966666 0.1196795 0 1.964061 0.1221648 0 1.336586 0.1223754 0 + + + + + + + + + + 0.2805839 0.8196483 0 0.04526968 0.06871392 0 0.9049747 0.2078104 0 0.9034945 0.2066053 0 0.9050688 0.2054525 0 0.04526968 0.06833798 0 0.9264595 0.2213758 0 0.9498159 0.207208 0 0.022865 0.8333253 0 0.9498011 0.2075837 0 0.2605765 0.6957551 0 0.280119 0.7363954 0 0.02207782 0.7323514 0 0.02433669 0.7099105 0 0.2807858 0.737782 0 0.02192368 0.7338821 0 0.9264886 0.2217563 0 0.9044933 0.223444 0 0.9028986 0.2223305 0 0.9043094 0.2211192 0 0.0408768 0.7919807 0 0.2774873 0.7973014 0 0.2807942 0.8211668 0 0.0222511 0.8347344 0 0.2802826 0.06833798 0 0.2802826 0.06871392 0 + + + + + + + + + + + + + + + + 4 4 5 4 5 4 4 +

0 0 0 3 1 3 2 2 2 1 3 1 4 4 4 7 5 7 6 6 6 5 7 5 1 8 1 5 9 10 6 10 9 8 11 8 0 12 0 2 13 2 4 14 12 5 15 11 1 16 1 3 17 3 9 18 15 7 19 14 4 20 13 2 21 2 3 1 3 0 0 0 8 22 17 9 23 16 6 6 6 7 5 7 9 23 16 8 22 17

+ + + + + + + -159.9404 -53.78705 -1.874754 -146.4978 -0.0682373 -6.622977 -680.3284 229.024 -6.622977 -759.2513 151.4088 -1.719254 -146.4978 -0.0682373 -7.552118 -680.3284 229.024 -7.552118 -761.9413 148.7634 -4.552118 -160.8539 -57.43716 -4.552118 -159.9371 -53.77365 -7.552118 -759.2487 151.4114 -7.552118 + + + + + + + + + + -0.09980783 -0.2950636 0.9502504 0.02225169 0.0577016 0.9980859 0.02225169 0.0577016 0.9980859 -0.1254768 -0.3691206 0.9208723 0 0 -1 0 0 -1 -0.1163293 -0.3394422 -0.9334058 -0.1403502 -0.4095337 -0.9014344 0.9700876 -0.2427552 -3.84271e-6 0.9700876 -0.2427552 -3.84271e-6 0.9700875 -0.2427552 -3.84271e-6 0.9700876 -0.2427552 -3.84271e-6 0.9700876 -0.2427552 -3.84271e-6 0.3943667 0.9189531 0 0.3943667 0.9189531 0 0.3943667 0.9189531 0 0.3943667 0.9189531 0 -0.701175 0.7129892 -4.75289e-6 -0.701175 0.7129892 -4.75289e-6 -0.701175 0.7129892 -4.75289e-6 -0.701175 0.7129892 -4.75289e-6 -0.701175 0.7129893 -4.75289e-6 -0.3240863 -0.9458899 0.01612903 -0.3240693 -0.9458408 0.01908865 + + + + + + + + + + 0.3415256 0.0631672 0 0.3369047 0.1178331 0 0.3336721 0.1210473 0 0.3313836 0.1168725 0 0.3406219 0.0630121 0 0.962397 0.1178207 0 0.9134955 0.01996307 0 0.9134955 0.01996307 0 0.9674023 0.1149765 0 0.9666662 0.1196795 0 0.9135695 0.2244072 0 0.3415694 0.181588 0 0.3369038 0.1271233 0 0.9623945 0.1266231 0 0.9640611 0.1221648 0 0.3365864 0.1223754 0 0.9134274 0.01904893 0 0.3414567 0.06225318 0 + + + + + + + + + + 0.4634737 0.2215011 0 0.371469 0.1656763 0 0.4853454 0.2078106 0 0.440519 0.207501 0 0.1364561 0.1656763 0 0.440505 0.2071253 0 0.4852557 0.2054524 0 0.4414084 0.2234515 0 0.4399091 0.2223517 0 0.4868279 0.2066082 0 0.9407476 0.1650625 0 0.7041439 0.1597421 0 0.7010481 0.1374013 0 0.9587584 0.1237205 0 0.700837 0.1358774 0 0.9593728 0.12231 0 0.280636 0.4629306 0 0.2783763 0.4853776 0 0.04212987 0.4995334 0 0.02258609 0.4588906 0 0.02192 0.4575054 0 0.2807895 0.4614053 0 0.4414139 0.2211195 0 0.4634737 0.2211195 0 0.1364561 0.1653004 0 0.371469 0.1653004 0 + + + + + + + + + + + + + + + + 4 4 5 4 5 4 4 +

0 0 1 1 1 0 2 2 6 3 3 5 5 4 10 4 5 11 8 6 12 9 7 13 1 8 0 0 9 1 7 10 2 8 11 3 4 12 4 2 13 6 1 14 0 4 15 17 5 16 16 3 17 5 2 18 6 5 19 7 9 20 8 6 21 9 3 3 5 6 22 14 7 23 15 0 0 1 8 6 12 7 23 15 6 22 14 9 7 13

+ + + + + + + -148.418 -85.50945 -1.102791 -140.6044 -87.09894 -12.76663 -110.2305 -89.75458 -14.58069 -63.40411 -92.02258 -14.70018 -27.65153 -92.22304 -13.28114 -6.453091 -90.47677 -9.721732 -6.062522 -89.09258 -2.057115 -27.91339 -90.148 -3.402355 -64.28602 -90.6501 -4.093974 -111.7494 -89.54975 -5.752198 -141.8063 -87.72169 -7.41406 -5.820303 -84.25722 4.648178 -27.44772 -81.78413 10.80457 -65.30094 -81.04269 10.90642 -114.8176 -84.26933 5.750248 -145.9831 -86.43643 -7.618556 -142.8192 -86.95889 -11.55532 -136.4912 -87.42779 -13.37228 -6.083979 -88.46382 -2.534964 -27.93325 -89.19062 -3.794164 -64.2766 -89.26363 -4.428123 -111.7086 -88.78247 -5.936151 -141.7505 -86.93833 -7.500984 -1.06996e-6 -81.99092 8.286549 -5.614649 -81.99092 6.782066 -9.724856 -81.99092 2.671859 -11.2293 -81.99092 -2.942788 -9.724858 -81.99092 -8.557436 -5.614652 -81.99092 -12.66764 -1.06996e-6 -92.37257 8.286549 -5.614649 -92.37257 6.782067 -9.724856 -92.37257 2.67186 -11.2293 -92.37257 -2.942788 -9.724858 -92.37257 -8.557436 -5.614652 -92.37257 -12.66764 6.25614e-7 -92.37257 -14.17205 6.25614e-7 -81.99092 -14.17205 148.418 -85.50945 -4.782685 140.6044 -87.09894 6.881151 110.2305 -89.75458 8.69521 63.4041 -92.02258 8.814693 27.65153 -92.22304 7.395648 6.45309 -90.47677 3.836235 6.062521 -89.09258 -3.828382 27.91339 -90.148 -2.483139 64.28602 -90.6501 -1.791515 111.7494 -89.54975 -0.1332831 141.8063 -87.72169 1.528583 5.820303 -84.25722 -10.53367 27.44772 -81.78413 -16.69006 65.30094 -81.04269 -16.79191 114.8176 -84.26933 -11.63573 145.9831 -86.43643 1.73308 142.8192 -86.95889 5.669845 136.4912 -87.42779 7.486804 6.083979 -88.46382 -3.350533 27.93325 -89.19062 -2.09133 64.2766 -89.26363 -1.457366 111.7086 -88.78247 0.05066967 141.7505 -86.93833 1.615508 5.61465 -81.99092 -12.66756 9.724856 -81.99092 -8.557356 11.2293 -81.99092 -2.942708 9.724857 -81.99092 2.67194 5.61465 -81.99092 6.782147 5.61465 -92.37257 -12.66756 9.724856 -92.37257 -8.557357 11.2293 -92.37257 -2.942709 9.724857 -92.37257 2.67194 5.61465 -92.37257 6.782146 + + + + + + + + + + 0.01640192 -0.9338064 0.3574024 0.03683284 -0.9236181 0.3815402 0.03137429 -0.8457066 0.5327251 0.01056091 -0.8491468 0.5280513 0.04181187 -0.9803091 0.1929917 0.019377 -0.9837002 0.1787697 -0.01103819 -0.8624499 0.506022 -0.01551752 -0.9472788 0.3200347 -0.01802185 -0.9922167 0.1232124 -0.03715853 -0.9057476 0.4221856 -0.04596881 -0.9721126 0.2299649 -0.05281925 -0.9982753 0.02562444 -0.1342113 -0.9561889 0.2601734 -0.2932671 -0.9558679 -0.01763404 -0.1495724 -0.9845206 0.09136277 -0.112204 -0.9838221 -0.1396581 -0.07924332 -0.9949006 -0.06239468 -0.1924958 -0.9723758 -0.1320256 -0.01748423 0.8825334 -0.4699247 -0.01995242 0.9215932 -0.3876441 -0.00455488 0.9265871 -0.3760527 -0.001838297 0.8847819 -0.4660017 -0.02257595 0.9569293 -0.2894421 -0.005568477 0.9585438 -0.2848911 0.01434956 0.9396729 -0.3417732 0.007469072 0.8958577 -0.4442782 0.02007815 0.9711662 -0.2375565 0.03887418 0.9680912 -0.2475643 0.02670994 0.934648 -0.3545698 0.05012629 0.9894401 -0.1359988 0.07831604 0.9871551 -0.1392529 0.1195049 0.9898769 -0.07656528 0.07020286 0.9707441 -0.2296249 0.07496622 0.9957287 -0.05389105 0.07006285 0.9974272 -0.01517308 0.07527069 0.9968524 -0.02489441 -6.70775e-6 -7.25869e-14 1 -0.500003 5.52185e-8 0.8660237 -0.500003 5.52185e-8 0.8660237 -6.70775e-6 -7.25869e-14 1 -0.8660254 4.29478e-8 0.5000001 -0.8660254 4.29478e-8 0.5000001 -1 6.13539e-9 1.76778e-7 -1 6.1354e-9 1.9642e-7 -0.8660255 -2.04513e-8 -0.4999998 -0.8660255 -2.04513e-8 -0.4999998 -0.4999973 -3.88576e-8 -0.8660269 -0.4999973 -3.88576e-8 -0.866027 6.75686e-6 1.0225e-9 -1 6.75686e-6 1.0225e-9 -1 0 0.9999999 0 0 1 0 0 1 0 0 1 0 0 1 0 0 0.9999999 0 0 0.9999999 0 -0.01640187 -0.9338064 -0.3574024 -0.03683279 -0.9236181 -0.3815401 -0.03137423 -0.8457067 -0.5327251 -0.01056084 -0.8491468 -0.5280513 -0.04181184 -0.9803091 -0.1929917 -0.01937697 -0.9837002 -0.1787697 0.01103827 -0.8624499 -0.506022 0.01551757 -0.9472789 -0.3200346 0.01802187 -0.9922166 -0.1232124 0.03715859 -0.9057475 -0.4221856 0.04596885 -0.9721127 -0.2299649 0.05281925 -0.9982753 -0.02562443 0.1342114 -0.9561888 -0.2601734 0.2932672 -0.9558679 0.01763404 0.1495725 -0.9845206 -0.09136277 0.1122039 -0.9838221 0.1396581 0.07924331 -0.9949006 0.06239468 0.1924957 -0.9723758 0.1320256 0.01748418 0.8825334 0.4699247 0.01995237 0.9215932 0.387644 0.004554827 0.9265872 0.3760526 0.001838234 0.884782 0.4660017 0.0225759 0.9569293 0.2894421 0.005568434 0.9585438 0.2848911 -0.01434962 0.9396729 0.3417732 -0.00746914 0.8958578 0.4442782 -0.02007818 0.9711662 0.2375565 -0.03887423 0.9680913 0.2475644 -0.02670999 0.9346479 0.3545698 -0.05012631 0.9894401 0.1359988 -0.07831606 0.987155 0.1392529 -0.119505 0.9898769 0.07656528 -0.07020289 0.9707441 0.2296249 -0.07496623 0.9957287 0.05389105 -0.07006285 0.9974272 0.01517309 -0.0752707 0.9968524 0.02489441 0.5000031 5.6241e-8 -0.8660237 0.5000031 5.6241e-8 -0.8660237 0.8660254 4.29478e-8 -0.5 0.8660254 4.29478e-8 -0.5 1 6.13539e-9 -7.85681e-8 1 6.1354e-9 -9.82102e-8 0.8660254 -2.04513e-8 0.5 0.8660254 -2.04513e-8 0.5 0.4999971 -3.88576e-8 0.866027 0.4999972 -3.88576e-8 0.866027 0 1 0 0 1 0 0 1 0 0 1 0 0 1 0 + + + + + + + + + + 0.03069195 0.2856472 0 0.03052663 0.3083949 0 0.0227879 0.3091352 0 0.015264 0.2870694 0 0.03781429 0.3075859 0 0.04017615 0.2853729 0 0.01246118 0.2478532 0 0.02921706 0.2479268 0 0.03929659 0.2482101 0 0.01523333 0.1962023 0 0.02727896 0.1986993 0 0.03566712 0.1996531 0 0.01960447 0.1609631 0 0.02602353 0.1629586 0 0.02626535 0.1674558 0 0.03142625 0.1681834 0 0.03234455 0.1724453 0 0.03006583 0.165942 0 0.02664836 0.5105492 0 0.01888445 0.509584 0 0.0195348 0.4868689 0 0.03480509 0.4887201 0 0.01193389 0.5085939 0 0.01019157 0.4863386 0 0.02227712 0.449232 0 0.03864864 0.4496018 0 0.01224555 0.4492508 0 0.02547421 0.4000906 0 0.03746906 0.3978921 0 0.01719868 0.4008328 0 0.02735482 0.3688894 0 0.02772205 0.3644824 0 0.03411508 0.3625515 0 0.02127807 0.373745 0 0.02230623 0.3695484 0 0.0236849 0.3673811 0 0.00439369 0.8598915 0 0.004393631 0.8541524 0 0.01464381 0.8541524 0 0.01464381 0.8598915 0 0.004393631 0.8484132 0 0.01464381 0.8484132 0 0.004393631 0.842674 0 0.01464381 0.842674 0 0.004393631 0.8369352 0 0.01464381 0.8369352 0 0.004393571 0.8311962 0 0.01464381 0.8311958 0 0.004393571 0.8254569 0 0.01464381 0.8254567 0 0.1720212 0.9985639 0 0.1735064 0.9930203 0 0.1775646 0.988962 0 0.1831082 0.9874766 0 0.1886517 0.988962 0 0.1927099 0.9930203 0 0.1941952 0.9985639 0 0.03069195 0.2856472 0 0.03052663 0.3083949 0 0.0227879 0.3091352 0 0.015264 0.2870694 0 0.03781429 0.3075859 0 0.04017615 0.2853729 0 0.01246118 0.2478532 0 0.02921706 0.2479268 0 0.03929659 0.2482101 0 0.01523333 0.1962023 0 0.02727896 0.1986993 0 0.03566712 0.1996531 0 0.01960447 0.1609631 0 0.02602353 0.1629586 0 0.02626535 0.1674558 0 0.03142625 0.1681834 0 0.03234455 0.1724453 0 0.03006583 0.165942 0 0.02664836 0.5105492 0 0.01888445 0.509584 0 0.0195348 0.4868689 0 0.03480509 0.4887201 0 0.01193389 0.5085939 0 0.01019157 0.4863386 0 0.02227712 0.449232 0 0.03864864 0.4496018 0 0.01224555 0.4492508 0 0.02547421 0.4000906 0 0.03746906 0.3978921 0 0.01719868 0.4008328 0 0.02735482 0.3688894 0 0.02772205 0.3644824 0 0.03411508 0.3625515 0 0.02127807 0.373745 0 0.02230623 0.3695484 0 0.0236849 0.3673811 0 0.00439369 0.8598915 0 0.004393631 0.8541524 0 0.01464381 0.8541524 0 0.01464381 0.8598915 0 0.004393631 0.8484132 0 0.01464381 0.8484132 0 0.004393631 0.842674 0 0.01464381 0.842674 0 0.004393631 0.8369352 0 0.01464381 0.8369352 0 0.004393571 0.8311962 0 0.01464381 0.8311958 0 0.004393571 0.8254569 0 0.01464381 0.8254567 0 0.1720212 0.9985639 0 0.1735064 0.9930203 0 0.1775646 0.988962 0 0.1831082 0.9874766 0 0.1886517 0.988962 0 0.1927099 0.9930203 0 0.1941952 0.9985639 0 + + + + + + + + + + 0.7872447 0.2457788 0 0.7873552 0.2427659 0 0.7965195 0.2422379 0 0.7963158 0.2483609 0 0.78751 0.2397516 0 0.7964013 0.2382587 0 0.6521086 0.4562721 0 0.6515402 0.4655035 0 0.6487074 0.4656059 0 0.6461065 0.4564689 0 0.6547784 0.4653385 0 0.8117629 0.2419719 0 0.8121926 0.2484036 0 0.811397 0.2376636 0 0.8316573 0.2413394 0 0.8329612 0.246241 0 0.8310373 0.2377136 0 0.8442576 0.2406831 0 0.846033 0.2406337 0 0.8470542 0.2433667 0 0.8420517 0.2382205 0 0.8437769 0.2384745 0 0.8447059 0.2389826 0 0.2820718 0.373235 0 0.2820718 0.3707514 0 0.2695574 0.3526779 0 0.2695574 0.3550998 0 0.2650791 0.3550998 0 0.2650791 0.3526779 0 0.286664 0.3707514 0 0.286664 0.373235 0 0.2695574 0.3575218 0 0.2650791 0.3575218 0 0.2695574 0.3592947 0 0.2650791 0.3592947 0 0.2745944 0.3671629 0 0.2820718 0.3689333 0 0.286664 0.3689333 0 0.2745944 0.3694741 0 0.6562821 0.4563827 0 0.6460636 0.4404769 0 0.6524008 0.4409057 0 0.6568816 0.4412783 0 0.6482419 0.4195576 0 0.6531013 0.4208538 0 0.6568311 0.4214954 0 0.6511371 0.4053623 0 0.6538898 0.406391 0 0.6538034 0.4081556 0 0.6560647 0.4086633 0 0.6563205 0.410401 0 0.6555529 0.4077276 0 0.2866472 0.6156204 0 0.2854568 0.6176485 0 0.2834119 0.6188097 0 0.2810603 0.6187929 0 0.2790322 0.6176025 0 0.2778709 0.6155576 0 0.2778877 0.613206 0 0.2703209 0.3694741 0 0.2703209 0.3671629 0 0.7872447 0.2457788 0 0.7873552 0.2427659 0 0.7965195 0.2422379 0 0.7963158 0.2483609 0 0.78751 0.2397516 0 0.7964013 0.2382587 0 0.6521086 0.4562721 0 0.6515402 0.4655035 0 0.6487074 0.4656059 0 0.6461065 0.4564689 0 0.6547784 0.4653385 0 0.8117629 0.2419719 0 0.8121926 0.2484036 0 0.811397 0.2376636 0 0.8316573 0.2413394 0 0.8329612 0.246241 0 0.8310373 0.2377136 0 0.8442576 0.2406831 0 0.846033 0.2406337 0 0.8470542 0.2433667 0 0.8420517 0.2382205 0 0.8437769 0.2384745 0 0.8447059 0.2389826 0 0.2820718 0.373235 0 0.2820718 0.3707514 0 0.2695574 0.3526779 0 0.2695574 0.3550998 0 0.2650791 0.3550998 0 0.2650791 0.3526779 0 0.286664 0.3707514 0 0.286664 0.373235 0 0.2695574 0.3575218 0 0.2650791 0.3575218 0 0.2695574 0.3592947 0 0.2650791 0.3592947 0 0.2745944 0.3671629 0 0.2820718 0.3689333 0 0.286664 0.3689333 0 0.2745944 0.3694741 0 0.6562821 0.4563827 0 0.6460636 0.4404769 0 0.6524008 0.4409057 0 0.6568816 0.4412783 0 0.6482419 0.4195576 0 0.6531013 0.4208538 0 0.6568311 0.4214954 0 0.6511371 0.4053623 0 0.6538898 0.406391 0 0.6538034 0.4081556 0 0.6560647 0.4086633 0 0.6563205 0.410401 0 0.6555529 0.4077276 0 0.2866472 0.6156204 0 0.2854568 0.6176485 0 0.2834119 0.6188097 0 0.2810603 0.6187929 0 0.2790322 0.6176025 0 0.2778709 0.6155576 0 0.2778877 0.613206 0 0.2703209 0.3694741 0 0.2703209 0.3671629 0 + + + + + + + + + + + + + + + + 4 4 4 4 4 4 3 3 4 4 3 3 4 4 4 4 4 4 3 3 4 4 3 3 4 4 4 4 4 4 7 4 4 4 4 4 4 3 3 4 4 3 3 4 4 4 4 4 4 3 3 4 4 3 3 4 4 4 4 4 4 7 +

7 0 0 6 1 1 11 2 2 12 3 3 5 4 4 6 1 1 7 0 0 4 5 5 13 6 6 8 7 7 7 0 0 12 3 3 8 7 7 3 8 8 4 5 5 7 0 0 14 9 9 9 10 10 8 7 7 13 6 6 8 7 7 9 10 10 2 11 11 3 8 8 0 12 12 15 13 13 10 14 14 10 14 14 1 15 15 17 16 16 17 16 16 2 11 11 9 10 10 10 14 14 10 14 14 9 10 10 14 9 9 0 12 12 10 14 14 15 13 13 16 17 17 10 14 14 16 17 17 1 15 15 11 18 18 18 19 19 19 20 20 12 21 21 19 20 20 18 19 19 5 22 22 4 23 23 19 20 20 20 24 24 13 25 25 12 21 21 4 23 23 3 26 26 20 24 24 19 20 20 20 24 24 21 27 27 14 28 28 13 25 25 2 29 29 21 27 27 20 24 24 3 26 26 22 30 30 15 31 31 0 32 32 17 33 33 1 34 34 22 30 30 21 27 27 2 29 29 17 33 33 22 30 30 14 28 28 21 27 27 22 30 30 0 32 32 16 35 35 15 31 31 22 30 30 1 34 34 16 35 35 22 30 30 23 36 36 24 37 37 30 38 38 29 39 39 24 37 37 25 40 40 31 41 41 30 38 38 25 40 40 26 42 42 32 43 43 31 41 41 26 42 42 27 44 44 33 45 45 32 43 43 27 44 44 28 46 46 34 47 47 33 45 45 28 46 46 36 48 48 35 49 49 34 47 47 36 50 50 28 51 51 27 52 52 26 53 53 25 54 54 24 55 55 23 56 56 44 57 57 43 58 58 48 59 59 49 60 60 42 61 61 43 58 58 44 57 57 41 62 62 50 63 63 45 64 64 44 57 57 49 60 60 45 64 64 40 65 65 41 62 62 44 57 57 51 66 66 46 67 67 45 64 64 50 63 63 45 64 64 46 67 67 39 68 68 40 65 65 37 69 69 52 70 70 47 71 71 47 71 71 38 72 72 54 73 73 54 73 73 39 68 68 46 67 67 47 71 71 47 71 71 46 67 67 51 66 66 37 69 69 47 71 71 52 70 70 53 74 74 47 71 71 53 74 74 38 72 72 48 75 75 55 76 76 56 77 77 49 78 78 56 77 77 55 76 76 42 79 79 41 80 80 56 77 77 57 81 81 50 82 82 49 78 78 41 80 80 40 83 83 57 81 81 56 77 77 57 81 81 58 84 84 51 85 85 50 82 82 39 86 86 58 84 84 57 81 81 40 83 83 59 87 87 52 88 88 37 89 89 54 90 90 38 91 91 59 87 87 58 84 84 39 86 86 54 90 90 59 87 87 51 85 85 58 84 84 59 87 87 37 89 89 53 92 92 52 88 88 59 87 87 38 91 91 53 92 92 59 87 87 36 48 93 60 93 94 65 94 95 35 49 96 60 93 94 61 95 97 66 96 98 65 94 95 61 95 97 62 97 99 67 98 100 66 96 98 62 97 99 63 99 101 68 100 102 67 98 100 63 99 101 64 101 103 69 102 104 68 100 102 64 101 103 23 36 105 29 39 106 69 102 104 23 56 107 64 103 108 63 104 109 62 105 110 61 106 111 60 107 112 36 50 113

+ + + + + + + -0.02032846 -0.1830921 -0.1073283 -0.04307937 -0.1830919 -0.1013461 + + + + + + + + + + -0.02032846 -0.1830921 -0.1073283 -0.03549574 -0.183092 -0.1033402 + + + + + + + + + + -0.0279121 -0.183092 -0.1053342 -0.04307937 -0.1830919 -0.1013461 + + + + + + + + + + BEZIER BEZIER + + + + + + + + + + + + + + + + + + 4.594768 -1.131593 -0.001338064 4.594901 -0.03871506 -0.4340888 + + + + + + + + + + 4.594768 -1.131593 -0.001338064 4.594857 -0.4030077 -0.2898386 + + + + + + + + + + 4.594812 -0.7673004 -0.1455883 4.594901 -0.03871506 -0.4340888 + + + + + + + + + + BEZIER BEZIER + + + + + + + + + + + + + + + + + + + + 0 0 0 + + + + + + + ../materials/textures/wing.png + + + + + + + + + 0 -149.4418 0 + + 0 323.1962 -10.05121 + + + + + + + + + + + + 1 + 1 + 1 + 1 + + + + + 761.941 471.9598 -14.603 + 0 0 -1 -18.926 + + -769.0001 106.4119 4.551788 + 0 0 1 -18.92599 + + + + + + + + + + + + 1 + 1 + 1 + 1 + + + + + + 1 + 1 + 1 + 1 + + + + + -761.941 471.9598 -14.603 + 0 0 1 -18.92629 + + 768.9996 106.4157 4.551788 + 0 0 -1 -18.92628 + + + + + + + + + + + + 1 + 1 + 1 + 1 + + + + + + 1 + 1 + 1 + 1 + + + + + 0 236.0148 -12.994 + + 0 1 0 -89.99998 + + 1 0 0 0 0 1 0 87.18144 0 0 1 2.94279 0 0 0 1 + + + + + + + + + + + + + 1 + 1 + 1 + 1 + + + + + + 1 + 1 + 1 + 1 + + + + + + 1 + 1 + 1 + 1 + + + + + + + + + diff --git a/models/zephyr_with_ardupilot/model.config b/models/zephyr_with_ardupilot/model.config new file mode 100644 index 0000000..0dca7f9 --- /dev/null +++ b/models/zephyr_with_ardupilot/model.config @@ -0,0 +1,22 @@ + + + + Zephyr Delta Wing With Ardupilot + 1.0 + model.sdf + + + Cole Biesemeyer + cole.bsmr@gmail.com + + + + Nate Koenig + natekoenig@gmail.com + + + + A model of a Zephyr delta wing with Ardupilot integration. + + + diff --git a/models/zephyr_with_ardupilot/model.sdf b/models/zephyr_with_ardupilot/model.sdf new file mode 100644 index 0000000..3557d0a --- /dev/null +++ b/models/zephyr_with_ardupilot/model.sdf @@ -0,0 +1,646 @@ + + + + 0 0 0 0 0 0 + + + 0 -0.12 0 0 0 0 + 1.5 + + 0.083137104 + 0 + 0.387382402 + 0 + 0 + 0.469845106 + + + + + + model://zephyr_with_ardupilot/meshes/wing.dae + + Wing +
true
+ + + + + + + + model://zephyr_with_ardupilot/meshes/wing.dae + + Wing +
true
+ + + + + + -0.76435 0.33918 0.002 -0.03 0 0 + + + .005 0.12993 .12688 + + + + + 0.76435 0.33918 0.002 -0.03 0 0 + + + .005 0.12993 .12688 + + + + + + 0 -0.1 0.0 0 0 0 + + + 0.03 + + + + 0 0 1 + 0 0 1 + 0.1 0.1 0.1 1 + + + + 0.7 0.20 0 0 0 0 + + + 0.03 + + + + 1 0 0 + 1 0 0 + 0.1 0.1 0.1 1 + + + + -0.7 0.20 0 0 0 0 + + + 0.03 + + + + 0 1 0 + 0 1 0 + 0.1 0.1 0.1 1 + + + + + + + + 0 0.07 0.008 0 1.5707 0 + + .05 + + 0.000367571 + 0 + 0.00036985 + 0 + 0 + 0.000003187 + + + + + + model://zephyr_with_ardupilot/meshes/wing.dae + + Propeller +
true
+ + + + + + 0 0 0.074205 0 0 0.3 + + + 0.02561 0.00541 0.14841 + + + + + 0 0 -0.074205 0 0 -0.3 + + + 0.02561 0.00541 0.14841 + + + + + + + 0.4530 .239 0.007 0 0 0 + + 0 0 0 0 0 0.32445671 + .1 + + 0.000102319 + 0 + 0.00334417 + 0 + 0 + 0.003446072 + + + + + + model://zephyr_with_ardupilot/meshes/wing.dae + + Flap_Left +
true
+ + + + + + -0.01 0.01 0 0 0 0.32445671 + + + 0.633463031 0.110694312 0.005 + + + + + + -0.4530 .239 0.007 0 0 0 + + 0 0 0 0 0 -0.32445671 + .1 + + 0.000102319 + 0 + 0.00334417 + 0 + 0 + 0.003446072 + + + + + + model://zephyr_with_ardupilot/meshes/wing.dae + + Flap_Right +
true
+ + + + + + 0.01 0.01 0 0 0 -0.32445671 + + + 0.633463031 0.110694312 0.005 + + + + + + wing + propeller + + 0 -1 0 + + 0.002 + + + + + wing + flap_left + 0 -0.04 0 0 0 0 + + 1 0.330321014 0 + + -0.524 + 0.524 + + + 0.1 + + + + + wing + flap_right + 0 -0.04 0 0 0 0 + + 1 -0.330321014 0 + + -0.524 + 0.524 + + + 0.1 + + + + + + + + 0 0 0 0 0 0 + 0.15 + + 0.00002 + 0 + 0 + 0.00002 + 0 + 0.00002 + + + + 0 0 0 3.141593 0 -1.5707963 + 1 + 1000.0 + + + + imu_link + wing + + 0 0 1 + + 0 + 0 + 0 + 0 + + + 1.0 + + 1 + + + + + + + + + 0.13 + 3.7 + 0.06417112299 + -1.8 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + 0 -0.1 0 + 0.50 + 1.2041 + 0 -1 0 + 0 0 1 + wing + + + + 0.15 + 6.8 + 0.06417112299 + -1.8 + 0.6391428111 + -3.85 + -0.9233984055 + 0 + 0.7 0.20 0 + 0.10 + 1.2041 + 0 -1 0 + 0 0 1 + wing + flap_left_joint + -5.0 + + + + 0.15 + 6.8 + 0.06417112299 + -1.8 + 0.6391428111 + -3.85 + -0.9233984055 + 0 + -0.7 0.20 0 + 0.10 + 1.2041 + 0 -1 0 + 0 0 1 + wing + flap_right_joint + -5.0 + + + + 0.0 + 4.752798721 + 0.6417112299 + -1.8 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + -0.76 0.30 0.025 + 0.12 + 1.2041 + 0 -1 0 + 1 0 0 + wing + + + + 0.0 + 4.752798721 + 0.6417112299 + -1.8 + 0.3391428111 + -3.85 + -0.9233984055 + 0 + 0.76 0.30 0.025 + 0.12 + 1.2041 + 0 -1 0 + 1 0 0 + wing + + + + 0.30 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.02 + 1.2041 + 0 0 0.074205 + -1 0 0 + 0 -1 0 + propeller + + + + 0.30 + 1.4 + 4.2500 + 0.10 + 0.00 + -0.025 + 0.0 + 0.0 + 0.02 + 1.2041 + 0 0 -0.074205 + 1 0 0 + 0 -1 0 + propeller + + + + propeller_joint + + + flap_left_joint + + + flap_right_joint + + + + + 127.0.0.1 + 9002 + 5 + 1 + + + 0 0 0 3.141593 0 -1.5707963 + 0 0 0 3.141593 0 0 + + + imu_sensor + + + + + + propeller_joint + 1 + 838 + 0 + 1100 + 1900 + VELOCITY + 0.20 + 0 + 0 + 0 + 0 + 2.5 + 0.0 + + + + + flap_left_joint + 1 + 1.048 + -0.5 + 1100 + 1900 + POSITION + 10.0 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + + + + + flap_right_joint + 1 + 1.048 + -0.5 + 1100 + 1900 + POSITION + 10.0 + 0 + 0 + 0 + 0 + 2.5 + -2.5 + + + + + + diff --git a/models/zephyr_with_ardupilot/thumbnails/1.png b/models/zephyr_with_ardupilot/thumbnails/1.png new file mode 100644 index 0000000..d093d83 Binary files /dev/null and b/models/zephyr_with_ardupilot/thumbnails/1.png differ diff --git a/models/zephyr_with_ardupilot/thumbnails/2.png b/models/zephyr_with_ardupilot/thumbnails/2.png new file mode 100644 index 0000000..735d556 Binary files /dev/null and b/models/zephyr_with_ardupilot/thumbnails/2.png differ diff --git a/models/zephyr_with_ardupilot/thumbnails/3.png b/models/zephyr_with_ardupilot/thumbnails/3.png new file mode 100644 index 0000000..81d4094 Binary files /dev/null and b/models/zephyr_with_ardupilot/thumbnails/3.png differ diff --git a/models/zephyr_with_ardupilot/thumbnails/4.png b/models/zephyr_with_ardupilot/thumbnails/4.png new file mode 100644 index 0000000..7228b6e Binary files /dev/null and b/models/zephyr_with_ardupilot/thumbnails/4.png differ diff --git a/models/zephyr_with_ardupilot/thumbnails/5.png b/models/zephyr_with_ardupilot/thumbnails/5.png new file mode 100644 index 0000000..5e43f35 Binary files /dev/null and b/models/zephyr_with_ardupilot/thumbnails/5.png differ diff --git a/src/ArduCopterIRLockPlugin.cc b/src/ArduCopterIRLockPlugin.cc index 5b98eef..3064452 100755 --- a/src/ArduCopterIRLockPlugin.cc +++ b/src/ArduCopterIRLockPlugin.cc @@ -37,9 +37,9 @@ typedef SSIZE_T ssize_t; #endif -#include -#include -#include +#include +#include +#include #include #include @@ -89,7 +89,7 @@ namespace gazebo } ///////////////////////////////////////////////// -ignition::math::Vector2i GetScreenSpaceCoords(ignition::math::Vector3d _pt, +gz::math::Vector2i GetScreenSpaceCoords(gz::math::Vector3d _pt, gazebo::rendering::CameraPtr _cam) { // Convert from 3D world pos to 2D screen pos @@ -97,7 +97,7 @@ ignition::math::Vector2i GetScreenSpaceCoords(ignition::math::Vector3d _pt, _cam->OgreCamera()->getViewMatrix() * gazebo::rendering::Conversions::Convert(_pt); - ignition::math::Vector2i screenPos; + gz::math::Vector2i screenPos; screenPos.X() = ((pos.x / 2.0) + 0.5) * _cam->ViewportWidth(); screenPos.Y() = (1 - ((pos.y / 2.0) + 0.5)) * _cam->ViewportHeight(); @@ -169,7 +169,7 @@ void ArduCopterIRLockPlugin::Load(sensors::SensorPtr _sensor, } this->dataPtr->irlock_addr = _sdf->Get("irlock_addr", static_cast("127.0.0.1")).first; - this->dataPtr->irlock_addr = + this->dataPtr->irlock_port = _sdf->Get("irlock_port", 9005).first; this->dataPtr->parentSensor->SetActive(true); @@ -207,7 +207,7 @@ void ArduCopterIRLockPlugin::OnNewFrame(const unsigned char * /*_image*/, if (!camera->IsVisible(vis)) continue; - ignition::math::Vector2i pt = GetScreenSpaceCoords( + gz::math::Vector2i pt = GetScreenSpaceCoords( vis->WorldPose().Pos(), camera); // use selection buffer to check if visual is occluded by other entities diff --git a/src/ArduPilotPlugin.cc b/src/ArduPilotPlugin.cc index 0145882..466c9cd 100755 --- a/src/ArduPilotPlugin.cc +++ b/src/ArduPilotPlugin.cc @@ -14,98 +14,64 @@ * limitations under the License. * */ -#include -#include -#ifdef _WIN32 - #include - #include - #include - #include - using raw_type = char; -#else - #include - #include - #include - #include - using raw_type = void; -#endif +#include "ArduPilotPlugin.hh" +#include "Socket.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -#if defined(_MSC_VER) - #include - typedef SSIZE_T ssize_t; -#endif -#include -#include -#include #include -#include -#include -#include -#include -#include -#include -#include "include/ArduPilotPlugin.hh" -#define MAX_MOTORS 255 - -using namespace gazebo; - -GZ_REGISTER_MODEL_PLUGIN(ArduPilotPlugin) - -/// \brief A servo packet. -struct ServoPacket -{ - /// \brief Motor speed data. - /// should rename to servo_command here and in ArduPilot SIM_Gazebo.cpp - float motorSpeed[MAX_MOTORS] = {0.0f}; -}; +#include +#include -/// \brief Flight Dynamics Model packet that is sent back to the ArduPilot -struct fdmPacket -{ - /// \brief packet timestamp - double timestamp; - - /// \brief IMU angular velocity - double imuAngularVelocityRPY[3]; - - /// \brief IMU linear acceleration - double imuLinearAccelerationXYZ[3]; - - /// \brief IMU quaternion orientation - double imuOrientationQuat[4]; - - /// \brief Model velocity in NED frame - double velocityXYZ[3]; - - /// \brief Model position in NED frame - double positionXYZ[3]; -/* NOT MERGED IN MASTER YET - /// \brief Model latitude in WGS84 system - double latitude = 0.0; - - /// \brief Model longitude in WGS84 system - double longitude = 0.0; - - /// \brief Model altitude from GPS - double altitude = 0.0; - - /// \brief Model estimated from airspeed sensor (e.g. Pitot) in m/s - double airspeed = 0.0; - - /// \brief Battery voltage. Default to -1 to use sitl estimator. - double battery_voltage = -1.0; +#include +#include +#include +#include +#include +#include - /// \brief Battery Current. - double battery_current = 0.0; +#define DEBUG_JSON_IO 0 - /// \brief Model rangefinder value. Default to -1 to use sitl rangefinder. - double rangefinder = -1.0; -*/ -}; +// MAX_MOTORS limits the maximum number of elements that +// can be defined in the . +#define MAX_MOTORS 255 -/// \brief Control class +// SITL JSON interface supplies 16 servo channels +#define MAX_SERVO_CHANNELS 16 + +// Register plugin +GZ_ADD_PLUGIN(gz::sim::systems::ArduPilotPlugin, + gz::sim::System, + gz::sim::systems::ArduPilotPlugin::ISystemConfigure, + gz::sim::systems::ArduPilotPlugin::ISystemPostUpdate, + gz::sim::systems::ArduPilotPlugin::ISystemReset, + gz::sim::systems::ArduPilotPlugin::ISystemPreUpdate) +// Add plugin alias so that we can refer to the plugin without the version +// namespace +GZ_ADD_PLUGIN_ALIAS(gz::sim::systems::ArduPilotPlugin, "ArduPilotPlugin") + +/// \brief class Control is responsible for controlling a joint class Control { /// \brief Constructor @@ -119,41 +85,62 @@ class Control this->pid.Init(0.1, 0, 0, 0, 0, 1.0, -1.0); } - /// \brief control id / channel + public: ~Control() {} + + /// \brief The PWM channel used to command this control public: int channel = 0; - /// \brief Next command to be applied to the propeller + /// \brief Next command to be applied to the joint public: double cmd = 0; /// \brief Velocity PID for motor control - public: common::PID pid; - - /// \brief Control type. Can be: - /// VELOCITY control velocity of joint - /// POSITION control position of joint - /// EFFORT control effort of joint + public: gz::math::PID pid; + + /// \brief The controller type + /// + /// Valid controller types are: + /// VELOCITY control velocity of joint + /// POSITION control position of joint + /// EFFORT control effort of joint + /// COMMAND control sends command to topic public: std::string type; - /// \brief use force controler + /// \brief Use force controller public: bool useForce = true; - /// \brief Control propeller joint. + /// \brief The name of the joint being controlled public: std::string jointName; - /// \brief Control propeller joint. - public: physics::JointPtr joint; + /// \brief The name of the topic to forward this command + public: std::string cmdTopic; + + /// \brief The joint being controlled + public: gz::sim::Entity joint; + + /// \brief A multiplier to scale the raw input command + public: double multiplier = 1.0; + + /// \brief An offset to shift the zero-point of the raw input command + public: double offset = 0.0; + + /// \brief Lower bound of PWM input, has default (1000). + /// + /// The lower bound of PWM input should match SERVOX_MIN for this channel. + public: double servo_min = 1000.0; - /// \brief direction multiplier for this control - public: double multiplier = 1; + /// \brief Upper limit of PWM input, has default (2000). + /// + /// The upper limit of PWM input should match SERVOX_MAX for this channel. + public: double servo_max = 2000.0; - /// \brief input command offset - public: double offset = 0; + /// \brief Publisher for sending commands + public: gz::transport::Node::Publisher pub; /// \brief unused coefficients public: double rotorVelocitySlowdownSim; public: double frequencyCutoff; public: double samplingRate; - public: ignition::math::OnePole filter; + public: gz::math::OnePole filter; public: static double kDefaultRotorVelocitySlowdownSim; public: static double kDefaultFrequencyCutoff; @@ -165,252 +152,270 @@ double Control::kDefaultFrequencyCutoff = 5.0; double Control::kDefaultSamplingRate = 0.2; // Private data class -class gazebo::ArduPilotSocketPrivate +class gz::sim::systems::ArduPilotPluginPrivate { - /// \brief constructor - public: ArduPilotSocketPrivate() - { - // initialize socket udp socket - fd = socket(AF_INET, SOCK_DGRAM, 0); - #ifndef _WIN32 - // Windows does not support FD_CLOEXEC - fcntl(fd, F_SETFD, FD_CLOEXEC); - #endif - } + /// \brief The model + public: gz::sim::Model model{gz::sim::kNullEntity}; - /// \brief destructor - public: ~ArduPilotSocketPrivate() - { - if (fd != -1) - { - ::close(fd); - fd = -1; - } - } + /// \brief The entity representing the base or root link of the model + public: gz::sim::Entity modelLink{gz::sim::kNullEntity}; - /// \brief Bind to an adress and port - /// \param[in] _address Address to bind to. - /// \param[in] _port Port to bind to. - /// \return True on success. - public: bool Bind(const char *_address, const uint16_t _port) - { - struct sockaddr_in sockaddr; - this->MakeSockAddr(_address, _port, sockaddr); + /// \brief The model name; + public: std::string modelName; - if (bind(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) - { - shutdown(this->fd, 0); - #ifdef _WIN32 - closesocket(this->fd); - #else - close(this->fd); - #endif - return false; - } - int one = 1; - setsockopt(this->fd, SOL_SOCKET, SO_REUSEADDR, - reinterpret_cast(&one), sizeof(one)); - - #ifdef _WIN32 - u_long on = 1; - ioctlsocket(this->fd, FIONBIO, - reinterpret_cast(&on)); - #else - fcntl(this->fd, F_SETFL, - fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); - #endif - return true; - } + /// \brief The world + public: gz::sim::World world{gz::sim::kNullEntity}; - /// \brief Connect to an adress and port - /// \param[in] _address Address to connect to. - /// \param[in] _port Port to connect to. - /// \return True on success. - public : bool Connect(const char *_address, const uint16_t _port) - { - struct sockaddr_in sockaddr; - this->MakeSockAddr(_address, _port, sockaddr); + /// \brief The world name; + public: std::string worldName; - if (connect(this->fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) - { - shutdown(this->fd, 0); - #ifdef _WIN32 - closesocket(this->fd); - #else - close(this->fd); - #endif - return false; - } - int one = 1; - setsockopt(this->fd, SOL_SOCKET, SO_REUSEADDR, - reinterpret_cast(&one), sizeof(one)); - - #ifdef _WIN32 - u_long on = 1; - ioctlsocket(this->fd, FIONBIO, - reinterpret_cast(&on)); - #else - fcntl(this->fd, F_SETFL, - fcntl(this->fd, F_GETFL, 0) | O_NONBLOCK); - #endif - return true; - } + /// \brief Array of controllers + public: std::vector controls; - /// \brief Make a socket - /// \param[in] _address Socket address. - /// \param[in] _port Socket port - /// \param[out] _sockaddr New socket address structure. - public: void MakeSockAddr(const char *_address, const uint16_t _port, - struct sockaddr_in &_sockaddr) - { - memset(&_sockaddr, 0, sizeof(_sockaddr)); + /// \brief keep track of controller update sim-time. + public: std::chrono::steady_clock::duration lastControllerUpdateTime{0}; - #ifdef HAVE_SOCK_SIN_LEN - _sockaddr.sin_len = sizeof(_sockaddr); - #endif + /// \brief Keep track of the time the last servo packet was received. + public: std::chrono::steady_clock::duration lastServoPacketRecvTime{0}; - _sockaddr.sin_port = htons(_port); - _sockaddr.sin_family = AF_INET; - _sockaddr.sin_addr.s_addr = inet_addr(_address); - } + /// \brief Controller update mutex. + public: std::mutex mutex; - public: ssize_t Send(const void *_buf, size_t _size) - { - return send(this->fd, _buf, _size, 0); - } + /// \brief Socket manager + public: SocketAPM sock = SocketAPM(true); - /// \brief Receive data - /// \param[out] _buf Buffer that receives the data. - /// \param[in] _size Size of the buffer. - /// \param[in] _timeoutMS Milliseconds to wait for data. - public: ssize_t Recv(void *_buf, const size_t _size, uint32_t _timeoutMs) - { - fd_set fds; - struct timeval tv; + /// \brief The address for the flight dynamics model (i.e. this plugin) + public: std::string fdm_address{"127.0.0.1"}; - FD_ZERO(&fds); - FD_SET(this->fd, &fds); + /// \brief The address for the SITL flight controller - auto detected + public: const char* fcu_address{nullptr}; - tv.tv_sec = _timeoutMs / 1000; - tv.tv_usec = (_timeoutMs % 1000) * 1000UL; + /// \brief The port for the flight dynamics model + public: uint16_t fdm_port_in{9002}; - if (select(this->fd+1, &fds, NULL, NULL, &tv) != 1) - { - return -1; - } + /// \brief The port for the SITL flight controller - auto detected + public: uint16_t fcu_port_out; - #ifdef _WIN32 - return recv(this->fd, reinterpret_cast(_buf), _size, 0); - #else - return recv(this->fd, _buf, _size, 0); - #endif - } + /// \brief The name of the IMU sensor + public: std::string imuName; - /// \brief Socket handle - private: int fd; -}; + /// \brief Set true to enforce lock-step simulation + public: bool isLockStep{false}; -// Private data class -class gazebo::ArduPilotPluginPrivate -{ - /// \brief Pointer to the update event connection. - public: event::ConnectionPtr updateConnection; + /// \brief Have we initialized subscription to the IMU data yet? + public: bool imuInitialized{false}; - /// \brief Pointer to the model; - public: physics::ModelPtr model; + /// \brief We need an gz-transport Node to subscribe to IMU data + public: gz::transport::Node node; - /// \brief String of the model name; - public: std::string modelName; + /// \brief A copy of the most recently received IMU data message + public: gz::msgs::IMU imuMsg; - /// \brief array of propellers - public: std::vector controls; + /// \brief Have we received at least one IMU data message? + public: bool imuMsgValid{false}; - /// \brief keep track of controller update sim-time. - public: gazebo::common::Time lastControllerUpdateTime; + /// \brief This mutex should be used when accessing imuMsg or imuMsgValid + public: std::mutex imuMsgMutex; - /// \brief Controller update mutex. - public: std::mutex mutex; + /// \brief This subscriber callback latches the most recently received IMU data message for later use. + public: void imuCb(const gz::msgs::IMU &_msg) + { + std::lock_guard lock(this->imuMsgMutex); + imuMsg = _msg; + imuMsgValid = true; + } + + /// \brief Pointer to an IMU sensor [required] +// public: sensors::ImuSensorPtr imuSensor; - /// \brief Ardupilot Socket for receive motor command on gazebo - public: ArduPilotSocketPrivate socket_in; + /// \brief Pointer to an GPS sensor [optional] +// public: sensors::GpsSensorPtr gpsSensor; - /// \brief Ardupilot Socket to send state to Ardupilot - public: ArduPilotSocketPrivate socket_out; + /// \brief Pointer to an Rangefinder sensor [optional] +// public: sensors::RaySensorPtr rangefinderSensor; - /// \brief Ardupilot address - public: std::string fdm_addr; + /// \brief Set to true when the ArduPilot flight controller is online + /// + /// Set to false when Gazebo starts to prevent blocking, true when + /// the ArduPilot controller is detected and online, and false if the + /// connection to the ArduPilot controller times out. + public: bool arduPilotOnline{false}; - /// \brief The Ardupilot listen address - public: std::string listen_addr; + /// \brief Number of consecutive missed ArduPilot controller messages + public: int connectionTimeoutCount{0}; - /// \brief Ardupilot port for receiver socket - public: uint16_t fdm_port_in; + /// \brief Max number of consecutive missed ArduPilot controller messages before timeout + public: int connectionTimeoutMaxCount; - /// \brief Ardupilot port for sender socket - public: uint16_t fdm_port_out; + /// \brief Transform from model orientation to x-forward and z-up + public: gz::math::Pose3d modelXYZToAirplaneXForwardZDown; - /// \brief Pointer to an IMU sensor - public: sensors::ImuSensorPtr imuSensor; + /// \brief Transform from world frame to NED frame + public: gz::math::Pose3d gazeboXYZToNED; - /// \brief Pointer to an GPS sensor - public: sensors::GpsSensorPtr gpsSensor; + /// \brief Last received frame rate from the ArduPilot controller + public: uint16_t fcu_frame_rate; - /// \brief Pointer to an Rangefinder sensor - public: sensors::RaySensorPtr rangefinderSensor; + /// \brief Last received frame count from the ArduPilot controller + public: uint32_t fcu_frame_count = -1; - /// \brief false before ardupilot controller is online - /// to allow gazebo to continue without waiting - public: bool arduPilotOnline; + /// \brief Last sent JSON string, so we can resend if needed. + public: std::string json_str; - /// \brief number of times ArduCotper skips update - public: int connectionTimeoutCount; + /// \brief A copy of the most recently received signal. + public: int signal{0}; - /// \brief number of times ArduCotper skips update - /// before marking ArduPilot offline - public: int connectionTimeoutMaxCount; + /// \brief Signal handler. + public: gz::common::SignalHandler sigHandler; + + /// \brief Signal handler callback. + public: void OnSignal(int _sig) + { + gzdbg << "Plugin received signal[" << _sig << "]\n"; + this->signal = _sig; + } }; ///////////////////////////////////////////////// -ArduPilotPlugin::ArduPilotPlugin() - : dataPtr(new ArduPilotPluginPrivate) +gz::sim::systems::ArduPilotPlugin::ArduPilotPlugin() + : dataPtr(new ArduPilotPluginPrivate()) { - this->dataPtr->arduPilotOnline = false; - this->dataPtr->connectionTimeoutCount = 0; } ///////////////////////////////////////////////// -ArduPilotPlugin::~ArduPilotPlugin() +gz::sim::systems::ArduPilotPlugin::~ArduPilotPlugin() { } ///////////////////////////////////////////////// -void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) +void gz::sim::systems::ArduPilotPlugin::Reset(const UpdateInfo &_info, + EntityComponentManager &_ecm) { - GZ_ASSERT(_model, "ArduPilotPlugin _model pointer is null"); - GZ_ASSERT(_sdf, "ArduPilotPlugin _sdf pointer is null"); + if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldPose::typeId)) + { + _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldPose()); + } + if(!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldLinearVelocity::typeId)) + { + _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldLinearVelocity()); + } - this->dataPtr->model = _model; - this->dataPtr->modelName = this->dataPtr->model->GetName(); + // update velocity PID for controls and apply force to joint + for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) + { + gz::sim::components::JointForceCmd* jfcComp = nullptr; + gz::sim::components::JointVelocityCmd* jvcComp = nullptr; + if (this->dataPtr->controls[i].useForce || this->dataPtr->controls[i].type == "EFFORT") + { + jfcComp = _ecm.Component( + this->dataPtr->controls[i].joint); + if (jfcComp == nullptr) + { + jfcComp = _ecm.CreateComponent(this->dataPtr->controls[i].joint, + gz::sim::components::JointForceCmd({0})); + } + } + else if (this->dataPtr->controls[i].type == "VELOCITY") + { + jvcComp = _ecm.Component( + this->dataPtr->controls[i].joint); + if (jvcComp == nullptr) + { + jvcComp = _ecm.CreateComponent(this->dataPtr->controls[i].joint, + gz::sim::components::JointVelocityCmd({0})); + } + } + } +} + +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::Configure( + const gz::sim::Entity &_entity, + const std::shared_ptr &_sdf, + gz::sim::EntityComponentManager &_ecm, + gz::sim::EventManager &/*&_eventMgr*/) +{ + // Make a clone so that we can call non-const methods + sdf::ElementPtr sdfClone = _sdf->Clone(); + + this->dataPtr->model = gz::sim::Model(_entity); + if (!this->dataPtr->model.Valid(_ecm)) + { + gzerr << "ArduPilotPlugin should be attached to a model " + << "entity. Failed to initialize." << "\n"; + return; + } + this->dataPtr->modelName = this->dataPtr->model.Name(_ecm); + + this->dataPtr->world = gz::sim::World( + _ecm.EntityByComponents(components::World())); + if (!this->dataPtr->world.Valid(_ecm)) + { + gzerr << "World entity not found" <dataPtr->world.Name(_ecm).has_value()) + { + this->dataPtr->worldName = this->dataPtr->world.Name(_ecm).value(); + } // modelXYZToAirplaneXForwardZDown brings us from gazebo model frame: // x-forward, y-right, z-down // to the aerospace convention: x-forward, y-left, z-up - this->modelXYZToAirplaneXForwardZDown = - ignition::math::Pose3d(0, 0, 0, 0, 0, 0); - if (_sdf->HasElement("modelXYZToAirplaneXForwardZDown")) + this->dataPtr->modelXYZToAirplaneXForwardZDown = + gz::math::Pose3d(0, 0, 0, GZ_PI, 0, 0); + if (sdfClone->HasElement("modelXYZToAirplaneXForwardZDown")) { - this->modelXYZToAirplaneXForwardZDown = - _sdf->Get("modelXYZToAirplaneXForwardZDown"); + this->dataPtr->modelXYZToAirplaneXForwardZDown = + sdfClone->Get("modelXYZToAirplaneXForwardZDown"); } // gazeboXYZToNED: from gazebo model frame: x-forward, y-right, z-down // to the aerospace convention: x-forward, y-left, z-up - this->gazeboXYZToNED = ignition::math::Pose3d(0, 0, 0, IGN_PI, 0, 0); - if (_sdf->HasElement("gazeboXYZToNED")) + this->dataPtr->gazeboXYZToNED = gz::math::Pose3d(0, 0, 0, GZ_PI, 0, 0); + if (sdfClone->HasElement("gazeboXYZToNED")) + { + this->dataPtr->gazeboXYZToNED = sdfClone->Get("gazeboXYZToNED"); + } + + // Load control channel params + this->LoadControlChannels(sdfClone, _ecm); + + // Load sensor params + this->LoadImuSensors(sdfClone, _ecm); + this->LoadGpsSensors(sdfClone, _ecm); + this->LoadRangeSensors(sdfClone, _ecm); + + // Initialise sockets + if (!InitSockets(sdfClone)) { - this->gazeboXYZToNED = _sdf->Get("gazeboXYZToNED"); + return; } + // Missed update count before we declare arduPilotOnline status false + this->dataPtr->connectionTimeoutMaxCount = + sdfClone->Get("connectionTimeoutMaxCount", 10).first; + + // Enforce lock-step simulation (has default: false) + this->dataPtr->isLockStep = + sdfClone->Get("lock_step", this->dataPtr->isLockStep).first; + + // Add the signal handler + this->dataPtr->sigHandler.AddCallback( + std::bind( + &gz::sim::systems::ArduPilotPluginPrivate::OnSignal, + this->dataPtr.get(), + std::placeholders::_1)); + + gzlog << "[" << this->dataPtr->modelName << "] " + << "ArduPilot ready to fly. The force will be with you" << "\n"; +} + +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::LoadControlChannels( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &_ecm) +{ // per control channel sdf::ElementPtr controlSDF; if (_sdf->HasElement("control")) @@ -462,11 +467,12 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) if (control.type != "VELOCITY" && control.type != "POSITION" && - control.type != "EFFORT") + control.type != "EFFORT" && + control.type != "COMMAND") { gzwarn << "[" << this->dataPtr->modelName << "] " << "Control type [" << control.type - << "] not recognized, must be one of VELOCITY, POSITION, EFFORT." + << "] not recognized, must be one of VELOCITY, POSITION, EFFORT, COMMAND." << " default to VELOCITY.\n"; control.type = "VELOCITY"; } @@ -488,8 +494,8 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) } // Get the pointer to the joint. - control.joint = _model->GetJoint(control.jointName); - if (control.joint == nullptr) + control.joint = this->dataPtr->model.JointByName(_ecm, control.jointName); + if (control.joint == gz::sim::kNullEntity) { gzerr << "[" << this->dataPtr->modelName << "] " << "Couldn't find specified joint [" @@ -497,6 +503,29 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) return; } + // set up publisher if relaying the command + if (control.type == "COMMAND") + { + if (controlSDF->HasElement("cmd_topic")) + { + control.cmdTopic = controlSDF->Get("cmd_topic"); + } + else + { + control.cmdTopic = + "/world/" + this->dataPtr->worldName + + "/model/" + this->dataPtr->modelName + + "/joint/" + control.jointName + "/cmd"; + gzwarn << "[" << this->dataPtr->modelName << "] " + << "Control type [" << control.type + << "] requires a valid . Using default\n"; + } + + gzmsg << "[" << this->dataPtr->modelName << "] " + << "Advertising on " << control.cmdTopic << ".\n"; + control.pub = this->dataPtr->node.Advertise(control.cmdTopic); + } + if (controlSDF->HasElement("multiplier")) { // overwrite turningDirection, deprecated. @@ -528,9 +557,10 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) else { gzdbg << "[" << this->dataPtr->modelName << "] " - << " (or deprecated ) not specified," - << " Default 1 (or deprecated 'ccw').\n"; - control.multiplier = 1; + << "channel[" << control.channel + << "]: (or deprecated ) not specified, " + << " default to " << control.multiplier + << " (or deprecated 'ccw').\n"; } if (controlSDF->HasElement("offset")) @@ -540,14 +570,39 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) else { gzdbg << "[" << this->dataPtr->modelName << "] " - << " not specified, default to 0.\n"; - control.offset = 0; + << "channel[" << control.channel + << "]: not specified, default to " + << control.offset << "\n"; + } + + if (controlSDF->HasElement("servo_min")) + { + control.servo_min = controlSDF->Get("servo_min"); + } + else + { + gzdbg << "[" << this->dataPtr->modelName << "] " + << "channel[" << control.channel + << "]: not specified, default to " + << control.servo_min << "\n"; + } + + if (controlSDF->HasElement("servo_max")) + { + control.servo_max = controlSDF->Get("servo_max"); + } + else + { + gzdbg << "[" << this->dataPtr->modelName << "] " + << "channel[" << control.channel + << "]: not specified, default to " + << control.servo_max << "\n"; } control.rotorVelocitySlowdownSim = controlSDF->Get("rotorVelocitySlowdownSim", 1).first; - if (ignition::math::equal(control.rotorVelocitySlowdownSim, 0.0)) + if (gz::math::equal(control.rotorVelocitySlowdownSim, 0.0)) { gzwarn << "[" << this->dataPtr->modelName << "] " << "control for joint [" << control.jointName @@ -573,47 +628,47 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) // Overload the PID parameters if they are available. double param; // carry over from ArduCopter plugin - param = controlSDF->Get("vel_p_gain", control.pid.GetPGain()).first; + param = controlSDF->Get("vel_p_gain", control.pid.PGain()).first; control.pid.SetPGain(param); - param = controlSDF->Get("vel_i_gain", control.pid.GetIGain()).first; + param = controlSDF->Get("vel_i_gain", control.pid.IGain()).first; control.pid.SetIGain(param); - param = controlSDF->Get("vel_d_gain", control.pid.GetDGain()).first; + param = controlSDF->Get("vel_d_gain", control.pid.DGain()).first; control.pid.SetDGain(param); - param = controlSDF->Get("vel_i_max", control.pid.GetIMax()).first; + param = controlSDF->Get("vel_i_max", control.pid.IMax()).first; control.pid.SetIMax(param); - param = controlSDF->Get("vel_i_min", control.pid.GetIMin()).first; + param = controlSDF->Get("vel_i_min", control.pid.IMin()).first; control.pid.SetIMin(param); - param = controlSDF->Get("vel_cmd_max", control.pid.GetCmdMax()).first; + param = controlSDF->Get("vel_cmd_max", control.pid.CmdMax()).first; control.pid.SetCmdMax(param); - param = controlSDF->Get("vel_cmd_min", control.pid.GetCmdMin()).first; + param = controlSDF->Get("vel_cmd_min", control.pid.CmdMin()).first; control.pid.SetCmdMin(param); // new params, overwrite old params if exist - param = controlSDF->Get("p_gain", control.pid.GetPGain()).first; + param = controlSDF->Get("p_gain", control.pid.PGain()).first; control.pid.SetPGain(param); - param = controlSDF->Get("i_gain", control.pid.GetIGain()).first; + param = controlSDF->Get("i_gain", control.pid.IGain()).first; control.pid.SetIGain(param); - param = controlSDF->Get("d_gain", control.pid.GetDGain()).first; + param = controlSDF->Get("d_gain", control.pid.DGain()).first; control.pid.SetDGain(param); - param = controlSDF->Get("i_max", control.pid.GetIMax()).first; + param = controlSDF->Get("i_max", control.pid.IMax()).first; control.pid.SetIMax(param); - param = controlSDF->Get("i_min", control.pid.GetIMin()).first; + param = controlSDF->Get("i_min", control.pid.IMin()).first; control.pid.SetIMin(param); - param = controlSDF->Get("cmd_max", control.pid.GetCmdMax()).first; + param = controlSDF->Get("cmd_max", control.pid.CmdMax()).first; control.pid.SetCmdMax(param); - param = controlSDF->Get("cmd_min", control.pid.GetCmdMin()).first; + param = controlSDF->Get("cmd_min", control.pid.CmdMin()).first; control.pid.SetCmdMin(param); // set pid initial command @@ -622,71 +677,25 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) this->dataPtr->controls.push_back(control); controlSDF = controlSDF->GetNextElement("control"); } +} - // Get sensors - std::string imuName = - _sdf->Get("imuName", static_cast("imu_sensor")).first; - std::vector imuScopedName = - this->dataPtr->model->SensorScopedName(imuName); - - if (imuScopedName.size() > 1) - { - gzwarn << "[" << this->dataPtr->modelName << "] " - << "multiple names match [" << imuName << "] using first found" - << " name.\n"; - for (unsigned k = 0; k < imuScopedName.size(); ++k) - { - gzwarn << " sensor " << k << " [" << imuScopedName[k] << "].\n"; - } - } - - if (imuScopedName.size() > 0) - { - this->dataPtr->imuSensor = std::dynamic_pointer_cast - (sensors::SensorManager::Instance()->GetSensor(imuScopedName[0])); - } - - if (!this->dataPtr->imuSensor) - { - if (imuScopedName.size() > 1) - { - gzwarn << "[" << this->dataPtr->modelName << "] " - << "first imu_sensor scoped name [" << imuScopedName[0] - << "] not found, trying the rest of the sensor names.\n"; - for (unsigned k = 1; k < imuScopedName.size(); ++k) - { - this->dataPtr->imuSensor = std::dynamic_pointer_cast - (sensors::SensorManager::Instance()->GetSensor(imuScopedName[k])); - if (this->dataPtr->imuSensor) - { - gzwarn << "found [" << imuScopedName[k] << "]\n"; - break; - } - } - } - - if (!this->dataPtr->imuSensor) - { - gzwarn << "[" << this->dataPtr->modelName << "] " - << "imu_sensor scoped name [" << imuName - << "] not found, trying unscoped name.\n" << "\n"; - // TODO: this fails for multi-nested models. - // TODO: and transforms fail for rotated nested model, - // joints point the wrong way. - this->dataPtr->imuSensor = std::dynamic_pointer_cast - (sensors::SensorManager::Instance()->GetSensor(imuName)); - } +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::LoadImuSensors( + sdf::ElementPtr _sdf, + gz::sim::EntityComponentManager &/*_ecm*/) +{ + this->dataPtr->imuName = + _sdf->Get("imuName", static_cast("imu_sensor")).first; +} - if (!this->dataPtr->imuSensor) - { - gzerr << "[" << this->dataPtr->modelName << "] " - << "imu_sensor [" << imuName - << "] not found, abort ArduPilot plugin.\n" << "\n"; - return; - } - } -/* NOT MERGED IN MASTER YET - // Get GPS +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::LoadGpsSensors( + sdf::ElementPtr /*_sdf*/, + gz::sim::EntityComponentManager &/*_ecm*/) +{ + /* + NOT MERGED IN MASTER YET + // Get GPS std::string gpsName = _sdf->Get("imuName", static_cast("gps_sensor")).first; std::vector gpsScopedName = SensorScopedName(this->dataPtr->model, gpsName); if (gpsScopedName.size() > 1) @@ -746,7 +755,15 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) << " found " << " [" << gpsName << "].\n"; } } + */ +} +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::LoadRangeSensors( + sdf::ElementPtr /*_sdf*/, + gz::sim::EntityComponentManager &/*_ecm*/) +{ + /* // Get Rangefinder // TODO add sonar std::string rangefinderName = _sdf->Get("rangefinderName", @@ -811,54 +828,121 @@ void ArduPilotPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) << " found " << " [" << rangefinderName << "].\n"; } } -*/ - // Controller time control. - this->dataPtr->lastControllerUpdateTime = 0; - - // Initialise ardupilot sockets - if (!InitArduPilotSockets(_sdf)) - { - return; - } + */ +} - // Missed update count before we declare arduPilotOnline status false - this->dataPtr->connectionTimeoutMaxCount = - _sdf->Get("connectionTimeoutMaxCount", 10).first; +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::PreUpdate( + const gz::sim::UpdateInfo &_info, + gz::sim::EntityComponentManager &_ecm) +{ + // This lookup is done in PreUpdate() because in Configure() it's not possible to get the fully qualified topic name we want + if (!this->dataPtr->imuInitialized) + { + // Set unconditionally because we're only going to try this once. + this->dataPtr->imuInitialized = true; + std::string imuTopicName; + _ecm.Each( + [&](const gz::sim::Entity &_imu_entity, + const gz::sim::components::Imu * /*_imu*/, + const gz::sim::components::Name *_name)->bool + { + if (_name->Data() == this->dataPtr->imuName) + { + // The parent of the imu is imu_link + gz::sim::Entity parent = _ecm.ParentEntity(_imu_entity); + if (parent != gz::sim::kNullEntity) + { + // The grandparent of the imu is the quad itself, which is where this plugin is attached + gz::sim::Entity gparent = _ecm.ParentEntity(parent); + if (gparent != gz::sim::kNullEntity) + { + gz::sim::Model gparent_model(gparent); + if (gparent_model.Name(_ecm) == this->dataPtr->modelName) + { + this->dataPtr->modelLink = parent; + imuTopicName = gz::sim::scopedName(_imu_entity, _ecm) + "/imu"; + gzdbg << "Computed IMU topic to be: " << imuTopicName << std::endl; + } + } + } + } + return true; + }); + + if(imuTopicName.empty()) + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "imu_sensor [" << this->dataPtr->imuName + << "] not found, abort ArduPilot plugin." << "\n"; + return; + } - // Listen to the update event. This event is broadcast every simulation - // iteration. - this->dataPtr->updateConnection = event::Events::ConnectWorldUpdateBegin( - std::bind(&ArduPilotPlugin::OnUpdate, this)); + this->dataPtr->node.Subscribe(imuTopicName, &gz::sim::systems::ArduPilotPluginPrivate::imuCb, this->dataPtr.get()); - gzlog << "[" << this->dataPtr->modelName << "] " - << "ArduPilot ready to fly. The force will be with you" << std::endl; + // Make sure that the "imu_link" entity has WorldPose and WorldLinearVelocity + // components, which we'll need later. + if (!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldPose::typeId)) + { + _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldPose()); + } + if(!_ecm.EntityHasComponentType(this->dataPtr->modelLink, components::WorldLinearVelocity::typeId)) + { + _ecm.CreateComponent(this->dataPtr->modelLink, gz::sim::components::WorldLinearVelocity()); + } + } + else + { + // Update the control surfaces. + if (!_info.paused && _info.simTime > this->dataPtr->lastControllerUpdateTime) + { + if (this->dataPtr->isLockStep) + { + while (!this->ReceiveServoPacket() && this->dataPtr->arduPilotOnline) + { + // SIGNINT should interrupt this loop. + if (this->dataPtr->signal != 0) + { + break; + } + } + this->dataPtr->lastServoPacketRecvTime = _info.simTime; + } + else if (this->ReceiveServoPacket()) + { + this->dataPtr->lastServoPacketRecvTime = _info.simTime; + } + + if (this->dataPtr->arduPilotOnline) + { + double dt = std::chrono::duration_cast >( + _info.simTime - this->dataPtr->lastControllerUpdateTime).count(); + this->ApplyMotorForces(dt, _ecm); + } + } + } } ///////////////////////////////////////////////// -void ArduPilotPlugin::OnUpdate() +void gz::sim::systems::ArduPilotPlugin::PostUpdate( + const gz::sim::UpdateInfo &_info, + const gz::sim::EntityComponentManager &_ecm) { - std::lock_guard lock(this->dataPtr->mutex); + std::lock_guard lock(this->dataPtr->mutex); - const gazebo::common::Time curTime = - this->dataPtr->model->GetWorld()->SimTime(); - - // Update the control surfaces and publish the new state. - if (curTime > this->dataPtr->lastControllerUpdateTime) - { - this->ReceiveMotorCommand(); - if (this->dataPtr->arduPilotOnline) + // Publish the new state. + if (!_info.paused && _info.simTime > this->dataPtr->lastControllerUpdateTime + && this->dataPtr->arduPilotOnline) { - this->ApplyMotorForces((curTime - - this->dataPtr->lastControllerUpdateTime).Double()); - this->SendState(); + double t = std::chrono::duration_cast>(_info.simTime).count(); + this->CreateStateJSON(t, _ecm); + this->SendState(); + this->dataPtr->lastControllerUpdateTime = _info.simTime; } - } - - this->dataPtr->lastControllerUpdateTime = curTime; } ///////////////////////////////////////////////// -void ArduPilotPlugin::ResetPIDs() +void gz::sim::systems::ArduPilotPlugin::ResetPIDs() { // Reset velocity PID for controls for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) @@ -869,67 +953,114 @@ void ArduPilotPlugin::ResetPIDs() } ///////////////////////////////////////////////// -bool ArduPilotPlugin::InitArduPilotSockets(sdf::ElementPtr _sdf) const +bool gz::sim::systems::ArduPilotPlugin::InitSockets(sdf::ElementPtr _sdf) const { - this->dataPtr->fdm_addr = - _sdf->Get("fdm_addr", static_cast("127.0.0.1")).first; - this->dataPtr->listen_addr = - _sdf->Get("listen_addr", static_cast("127.0.0.1")).first; - this->dataPtr->fdm_port_in = - _sdf->Get("fdm_port_in", static_cast(9002)).first; - this->dataPtr->fdm_port_out = - _sdf->Get("fdm_port_out", static_cast(9003)).first; - - if (!this->dataPtr->socket_in.Bind(this->dataPtr->listen_addr.c_str(), - this->dataPtr->fdm_port_in)) - { - gzerr << "[" << this->dataPtr->modelName << "] " - << "failed to bind with " << this->dataPtr->listen_addr - << ":" << this->dataPtr->fdm_port_in << " aborting plugin.\n"; - return false; - } + // configure port + this->dataPtr->sock.set_blocking(false); + this->dataPtr->sock.reuseaddress(); - if (!this->dataPtr->socket_out.Connect(this->dataPtr->fdm_addr.c_str(), - this->dataPtr->fdm_port_out)) - { - gzerr << "[" << this->dataPtr->modelName << "] " - << "failed to bind with " << this->dataPtr->fdm_addr - << ":" << this->dataPtr->fdm_port_out << " aborting plugin.\n"; - return false; - } + // get the fdm address if provided, otherwise default to localhost + this->dataPtr->fdm_address = + _sdf->Get("fdm_addr", static_cast("127.0.0.1")).first; - return true; + this->dataPtr->fdm_port_in = + _sdf->Get("fdm_port_in", static_cast(9002)).first; + + // output port configuration is automatic + if (_sdf->HasElement("listen_addr")) { + gzwarn << "Param is deprecated, connection is auto detected\n"; + } + if (_sdf->HasElement("fdm_port_out")) { + gzwarn << "Param is deprecated, connection is auto detected\n"; + } + + // bind the socket + if (!this->dataPtr->sock.bind(this->dataPtr->fdm_address.c_str(), this->dataPtr->fdm_port_in)) + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "failed to bind with " + << this->dataPtr->fdm_address << ":" << this->dataPtr->fdm_port_in + << " aborting plugin.\n"; + return false; + } + gzlog << "[" << this->dataPtr->modelName << "] " + << "flight dynamics model @ " + << this->dataPtr->fdm_address << ":" << this->dataPtr->fdm_port_in + << "\n"; + return true; } ///////////////////////////////////////////////// -void ArduPilotPlugin::ApplyMotorForces(const double _dt) +void gz::sim::systems::ArduPilotPlugin::ApplyMotorForces( + const double _dt, + gz::sim::EntityComponentManager &_ecm) { // update velocity PID for controls and apply force to joint for (size_t i = 0; i < this->dataPtr->controls.size(); ++i) { + // Publish commands to be relayed to other plugins + if (this->dataPtr->controls[i].type == "COMMAND") + { + msgs::Double cmd; + cmd.set_data(this->dataPtr->controls[i].cmd); + this->dataPtr->controls[i].pub.Publish(cmd); + continue; + } + + gz::sim::components::JointForceCmd* jfcComp = nullptr; + gz::sim::components::JointVelocityCmd* jvcComp = nullptr; + if (this->dataPtr->controls[i].useForce || this->dataPtr->controls[i].type == "EFFORT") + { + jfcComp = _ecm.Component( + this->dataPtr->controls[i].joint); + if (jfcComp == nullptr) + { + jfcComp = _ecm.CreateComponent(this->dataPtr->controls[i].joint, + gz::sim::components::JointForceCmd({0})); + } + } + else if (this->dataPtr->controls[i].type == "VELOCITY") + { + jvcComp = _ecm.Component( + this->dataPtr->controls[i].joint); + if (jvcComp == nullptr) + { + jvcComp = _ecm.CreateComponent(this->dataPtr->controls[i].joint, + gz::sim::components::JointVelocityCmd({0})); + } + } + if (this->dataPtr->controls[i].useForce) { if (this->dataPtr->controls[i].type == "VELOCITY") { const double velTarget = this->dataPtr->controls[i].cmd / this->dataPtr->controls[i].rotorVelocitySlowdownSim; - const double vel = this->dataPtr->controls[i].joint->GetVelocity(0); + gz::sim::components::JointVelocity* vComp = + _ecm.Component( + this->dataPtr->controls[i].joint); + const double vel = vComp->Data()[0]; const double error = vel - velTarget; - const double force = this->dataPtr->controls[i].pid.Update(error, _dt); - this->dataPtr->controls[i].joint->SetForce(0, force); + const double force = this->dataPtr->controls[i].pid.Update( + error, std::chrono::duration(_dt)); + jfcComp->Data()[0] = force; } else if (this->dataPtr->controls[i].type == "POSITION") { const double posTarget = this->dataPtr->controls[i].cmd; - const double pos = this->dataPtr->controls[i].joint->Position(); + gz::sim::components::JointPosition* pComp = + _ecm.Component( + this->dataPtr->controls[i].joint); + const double pos = pComp->Data()[0]; const double error = pos - posTarget; - const double force = this->dataPtr->controls[i].pid.Update(error, _dt); - this->dataPtr->controls[i].joint->SetForce(0, force); + const double force = this->dataPtr->controls[i].pid.Update( + error, std::chrono::duration(_dt)); + jfcComp->Data()[0] = force; } else if (this->dataPtr->controls[i].type == "EFFORT") { const double force = this->dataPtr->controls[i].cmd; - this->dataPtr->controls[i].joint->SetForce(0, force); + jfcComp->Data()[0] = force; } else { @@ -940,16 +1071,18 @@ void ArduPilotPlugin::ApplyMotorForces(const double _dt) { if (this->dataPtr->controls[i].type == "VELOCITY") { - this->dataPtr->controls[i].joint->SetVelocity(0, this->dataPtr->controls[i].cmd); + jvcComp->Data()[0] = this->dataPtr->controls[i].cmd; } else if (this->dataPtr->controls[i].type == "POSITION") { - this->dataPtr->controls[i].joint->SetPosition(0, this->dataPtr->controls[i].cmd); + //TODO: figure out whether position control matters, and if so, how to use it. + gzwarn << "Failed to do position control on joint " << i << + " because there's no JointPositionCmd component (yet?)" << "/n"; } else if (this->dataPtr->controls[i].type == "EFFORT") { const double force = this->dataPtr->controls[i].cmd; - this->dataPtr->controls[i].joint->SetForce(0, force); + jvcComp->Data()[0] = force; } else { @@ -960,264 +1093,439 @@ void ArduPilotPlugin::ApplyMotorForces(const double _dt) } ///////////////////////////////////////////////// -void ArduPilotPlugin::ReceiveMotorCommand() +bool gz::sim::systems::ArduPilotPlugin::ReceiveServoPacket() { - // Added detection for whether ArduPilot is online or not. - // If ArduPilot is detected (receive of fdm packet from someone), - // then socket receive wait time is increased from 1ms to 1 sec - // to accomodate network jitter. - // If ArduPilot is not detected, receive call blocks for 1ms - // on each call. - // Once ArduPilot presence is detected, it takes this many - // missed receives before declaring the FCS offline. - - ServoPacket pkt; - uint32_t waitMs; - if (this->dataPtr->arduPilotOnline) - { - // increase timeout for receive once we detect a packet from - // ArduPilot FCS. - waitMs = 1000; - } - else - { - // Otherwise skip quickly and do not set control force. - waitMs = 1; - } - ssize_t recvSize = - this->dataPtr->socket_in.Recv(&pkt, sizeof(ServoPacket), waitMs); - - // Drain the socket in the case we're backed up - int counter = 0; - ServoPacket last_pkt; - while (true) - { - // last_pkt = pkt; - const ssize_t recvSize_last = - this->dataPtr->socket_in.Recv(&last_pkt, sizeof(ServoPacket), 0ul); - if (recvSize_last == -1) + // Added detection for whether ArduPilot is online or not. + // If ArduPilot is detected (receive of fdm packet from someone), + // then socket receive wait time is increased from 1ms to 1 sec + // to accomodate network jitter. + // If ArduPilot is not detected, receive call blocks for 1ms + // on each call. + // Once ArduPilot presence is detected, it takes this many + // missed receives before declaring the FCS offline. + + uint32_t waitMs; + if (this->dataPtr->arduPilotOnline) { - break; + // Increase timeout for recv once we detect a packet from ArduPilot FCS. + // If this value is too high then it will block the main Gazebo update loop + // and adversely affect the RTF. + waitMs = 10; + } + else + { + // Otherwise skip quickly and do not set control force. + waitMs = 1; } - counter++; - pkt = last_pkt; - recvSize = recvSize_last; - } - if (counter > 0) - { - gzdbg << "[" << this->dataPtr->modelName << "] " - << "Drained n packets: " << counter << std::endl; - } - if (recvSize == -1) - { - // didn't receive a packet - // gzdbg << "no packet\n"; - gazebo::common::Time::NSleep(100); - if (this->dataPtr->arduPilotOnline) + servo_packet pkt; + auto recvSize = this->dataPtr->sock.recv(&pkt, sizeof(servo_packet), waitMs); + + this->dataPtr->sock.last_recv_address(this->dataPtr->fcu_address, this->dataPtr->fcu_port_out); + + // drain the socket in the case we're backed up + int counter = 0; + while (true) + { + servo_packet last_pkt; + auto recvSize_last = this->dataPtr->sock.recv(&last_pkt, sizeof(servo_packet), 0ul); + if (recvSize_last == -1) + { + break; + } + counter++; + pkt = last_pkt; + recvSize = recvSize_last; + } + if (counter > 0) { - gzwarn << "[" << this->dataPtr->modelName << "] " - << "Broken ArduPilot connection, count [" - << this->dataPtr->connectionTimeoutCount - << "/" << this->dataPtr->connectionTimeoutMaxCount - << "]\n"; - if (++this->dataPtr->connectionTimeoutCount > - this->dataPtr->connectionTimeoutMaxCount) - { - this->dataPtr->connectionTimeoutCount = 0; - this->dataPtr->arduPilotOnline = false; gzwarn << "[" << this->dataPtr->modelName << "] " - << "Broken ArduPilot connection, resetting motor control.\n"; - this->ResetPIDs(); - } + << "Drained n packets: " << counter << "\n"; } - } - else - { - const ssize_t expectedPktSize = - sizeof(pkt.motorSpeed[0]) * this->dataPtr->controls.size(); - if (recvSize < expectedPktSize) + + // didn't receive a packet, increment timeout count if online, then return + if (recvSize == -1) { - gzerr << "[" << this->dataPtr->modelName << "] " - << "got less than model needs. Got: " << recvSize - << "commands, expected size: " << expectedPktSize << "\n"; + if (this->dataPtr->arduPilotOnline) + { + if (++this->dataPtr->connectionTimeoutCount > + this->dataPtr->connectionTimeoutMaxCount) + { + this->dataPtr->connectionTimeoutCount = 0; + + // for lock-step resend last state rather than time out + if (this->dataPtr->isLockStep) + { + this->SendState(); + } + else + { + this->dataPtr->arduPilotOnline = false; + gzwarn << "[" << this->dataPtr->modelName << "] " + << "Broken ArduPilot connection, resetting motor control.\n"; + this->ResetPIDs(); + } + } + } + return false; } - const ssize_t recvChannels = recvSize / sizeof(pkt.motorSpeed[0]); - // for(unsigned int i = 0; i < recvChannels; ++i) - // { - // gzdbg << "servo_command [" << i << "]: " << pkt.motorSpeed[i] << "\n"; + +#if DEBUG_JSON_IO + // debug: inspect sitl packet + std::ostringstream oss; + oss << "recv " << recvSize << " bytes from " + << this->dataPtr->fcu_address << ":" << this->dataPtr->fcu_port_out << "\n"; + // oss << "magic: " << pkt.magic << "\n"; + // oss << "frame_rate: " << pkt.frame_rate << "\n"; + oss << "frame_count: " << pkt.frame_count << "\n"; + // oss << "pwm: ["; + // for (auto i=0; idataPtr->arduPilotOnline) { - gzdbg << "[" << this->dataPtr->modelName << "] " - << "ArduPilot controller online detected.\n"; - // made connection, set some flags - this->dataPtr->connectionTimeoutCount = 0; - this->dataPtr->arduPilotOnline = true; + this->dataPtr->arduPilotOnline = true; + + gzlog << "[" << this->dataPtr->modelName << "] " + << "Connected to ArduPilot controller @ " + << this->dataPtr->fcu_address << ":" << this->dataPtr->fcu_port_out + << "\n"; + } + + // update frame rate + this->dataPtr->fcu_frame_rate = pkt.frame_rate; + + // check for controller reset + if (pkt.frame_count < this->dataPtr->fcu_frame_count) + { + // @TODO - implement re-initialisation + gzwarn << "ArduPilot controller has reset\n"; } + // check for duplicate frame + else if (pkt.frame_count == this->dataPtr->fcu_frame_count) + { + gzwarn << "Duplicate input frame\n"; + + // for lock-step resend last state rather than ignore + if (this->dataPtr->isLockStep) + { + this->SendState(); + } + + return false; + } + + // check for skipped frames + else if (pkt.frame_count != this->dataPtr->fcu_frame_count + 1 + && this->dataPtr->arduPilotOnline) + { + gzwarn << "Missed " + << pkt.frame_count - this->dataPtr->fcu_frame_count + << " input frames\n"; + } + + // update frame count + this->dataPtr->fcu_frame_count = pkt.frame_count; + + // reset the connection timeout so we don't accumulate + this->dataPtr->connectionTimeoutCount = 0; + + this->UpdateMotorCommands(pkt); + + return true; +} + +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::UpdateMotorCommands(const servo_packet &_pkt) +{ // compute command based on requested motorSpeed for (unsigned i = 0; i < this->dataPtr->controls.size(); ++i) { - if (i < MAX_MOTORS) - { - if (this->dataPtr->controls[i].channel < recvChannels) + // enforce limit on the number of elements + if (i < MAX_MOTORS) { - // bound incoming cmd between 0 and 1 - const double cmd = ignition::math::clamp( - pkt.motorSpeed[this->dataPtr->controls[i].channel], - -1.0f, 1.0f); - this->dataPtr->controls[i].cmd = - this->dataPtr->controls[i].multiplier * - (this->dataPtr->controls[i].offset + cmd); - // gzdbg << "apply input chan[" << this->dataPtr->controls[i].channel - // << "] to control chan[" << i - // << "] with joint name [" - // << this->dataPtr->controls[i].jointName - // << "] raw cmd [" - // << pkt.motorSpeed[this->dataPtr->controls[i].channel] - // << "] adjusted cmd [" << this->dataPtr->controls[i].cmd - // << "].\n"; + if (this->dataPtr->controls[i].channel < MAX_SERVO_CHANNELS) + { + // convert pwm to raw cmd: [servo_min, servo_max] => [0, 1], + // default is: [1000, 2000] => [0, 1] + const double pwm = _pkt.pwm[this->dataPtr->controls[i].channel]; + const double pwm_min = this->dataPtr->controls[i].servo_min; + const double pwm_max = this->dataPtr->controls[i].servo_max; + const double multiplier = this->dataPtr->controls[i].multiplier; + const double offset = this->dataPtr->controls[i].offset; + + // bound incoming cmd between 0 and 1 + double raw_cmd = (pwm - pwm_min)/(pwm_max - pwm_min); + raw_cmd = gz::math::clamp(raw_cmd, 0.0, 1.0); + this->dataPtr->controls[i].cmd = multiplier * (raw_cmd + offset); + +#if 0 + gzdbg << "apply input chan[" << this->dataPtr->controls[i].channel + << "] to control chan[" << i + << "] with joint name [" + << this->dataPtr->controls[i].jointName + << "] pwm [" << pwm + << "] raw cmd [" << raw_cmd + << "] adjusted cmd [" << this->dataPtr->controls[i].cmd + << "].\n"; +#endif + } + else + { + gzerr << "[" << this->dataPtr->modelName << "] " + << "control[" << i << "] channel [" + << this->dataPtr->controls[i].channel + << "] is greater than the number of servo channels [" + << MAX_SERVO_CHANNELS + << "], control not applied.\n"; + } } else { - gzerr << "[" << this->dataPtr->modelName << "] " - << "control[" << i << "] channel [" - << this->dataPtr->controls[i].channel - << "] is greater than incoming commands size[" - << recvChannels - << "], control not applied.\n"; + gzerr << "[" << this->dataPtr->modelName << "] " + << "too many motors, skipping [" << i + << " > " << MAX_MOTORS << "].\n"; } - } - else - { - gzerr << "[" << this->dataPtr->modelName << "] " - << "too many motors, skipping [" << i - << " > " << MAX_MOTORS << "].\n"; - } } - } } ///////////////////////////////////////////////// -void ArduPilotPlugin::SendState() const +void gz::sim::systems::ArduPilotPlugin::CreateStateJSON( + double _simTime, + const gz::sim::EntityComponentManager &_ecm) const { - // send_fdm - fdmPacket pkt; - - pkt.timestamp = this->dataPtr->model->GetWorld()->SimTime().Double(); - - // asssumed that the imu orientation is: - // x forward - // y right - // z down - - // get linear acceleration in body frame - const ignition::math::Vector3d linearAccel = - this->dataPtr->imuSensor->LinearAcceleration(); - - // copy to pkt - pkt.imuLinearAccelerationXYZ[0] = linearAccel.X(); - pkt.imuLinearAccelerationXYZ[1] = linearAccel.Y(); - pkt.imuLinearAccelerationXYZ[2] = linearAccel.Z(); - // gzerr << "lin accel [" << linearAccel << "]\n"; - - // get angular velocity in body frame - const ignition::math::Vector3d angularVel = - this->dataPtr->imuSensor->AngularVelocity(); - - // copy to pkt - pkt.imuAngularVelocityRPY[0] = angularVel.X(); - pkt.imuAngularVelocityRPY[1] = angularVel.Y(); - pkt.imuAngularVelocityRPY[2] = angularVel.Z(); - - // get inertial pose and velocity - // position of the uav in world frame - // this position is used to calcualte bearing and distance - // from starting location, then use that to update gps position. - // The algorithm looks something like below (from ardupilot helper - // libraries): - // bearing = to_degrees(atan2(position.y, position.x)); - // distance = math.sqrt(self.position.x**2 + self.position.y**2) - // (self.latitude, self.longitude) = util.gps_newpos( - // self.home_latitude, self.home_longitude, bearing, distance) - // where xyz is in the NED directions. - // Gazebo world xyz is assumed to be N, -E, -D, so flip some stuff - // around. - // orientation of the uav in world NED frame - - // assuming the world NED frame has xyz mapped to NED, - // imuLink is NED - z down - - // model world pose brings us to model, - // which for example zephyr has -y-forward, x-left, z-up - // adding modelXYZToAirplaneXForwardZDown rotates - // from: model XYZ - // to: airplane x-forward, y-left, z-down - const ignition::math::Pose3d gazeboXYZToModelXForwardZDown = - this->modelXYZToAirplaneXForwardZDown + - this->dataPtr->model->WorldPose(); - - // get transform from world NED to Model frame - const ignition::math::Pose3d NEDToModelXForwardZUp = - gazeboXYZToModelXForwardZDown - this->gazeboXYZToNED; - - // gzerr << "ned to model [" << NEDToModelXForwardZUp << "]\n"; - - // N - pkt.positionXYZ[0] = NEDToModelXForwardZUp.Pos().X(); - - // E - pkt.positionXYZ[1] = NEDToModelXForwardZUp.Pos().Y(); - - // D - pkt.positionXYZ[2] = NEDToModelXForwardZUp.Pos().Z(); - - // imuOrientationQuat is the rotation from world NED frame - // to the uav frame. - pkt.imuOrientationQuat[0] = NEDToModelXForwardZUp.Rot().W(); - pkt.imuOrientationQuat[1] = NEDToModelXForwardZUp.Rot().X(); - pkt.imuOrientationQuat[2] = NEDToModelXForwardZUp.Rot().Y(); - pkt.imuOrientationQuat[3] = NEDToModelXForwardZUp.Rot().Z(); - - // gzdbg << "imu [" << gazeboXYZToModelXForwardZDown.rot.GetAsEuler() - // << "]\n"; - // gzdbg << "ned [" << this->gazeboXYZToNED.rot.GetAsEuler() << "]\n"; - // gzdbg << "rot [" << NEDToModelXForwardZUp.rot.GetAsEuler() << "]\n"; - - // Get NED velocity in body frame * - // or... - // Get model velocity in NED frame - const ignition::math::Vector3d velGazeboWorldFrame = - this->dataPtr->model->GetLink()->WorldLinearVel(); - const ignition::math::Vector3d velNEDFrame = - this->gazeboXYZToNED.Rot().RotateVectorReverse(velGazeboWorldFrame); - pkt.velocityXYZ[0] = velNEDFrame.X(); - pkt.velocityXYZ[1] = velNEDFrame.Y(); - pkt.velocityXYZ[2] = velNEDFrame.Z(); -/* NOT MERGED IN MASTER YET - if (!this->dataPtr->gpsSensor) + // Make a local copy of the latest IMU data (it's filled in on receipt by imuCb()). + gz::msgs::IMU imuMsg; { - - } - else { - pkt.longitude = this->dataPtr->gpsSensor->Longitude().Degree(); - pkt.latitude = this->dataPtr->gpsSensor->Latitude().Degree(); - pkt.altitude = this->dataPtr->gpsSensor->Altitude(); + std::lock_guard lock(this->dataPtr->imuMsgMutex); + // Wait until we've received a valid message. + if(!this->dataPtr->imuMsgValid) + { + return; + } + imuMsg = this->dataPtr->imuMsg; } - // TODO : make generic enough to accept sonar/gpuray etc. too - if (!this->dataPtr->rangefinderSensor) - { + // it is assumed that the imu orientation conforms to the aircraft convention: + // x-forward + // y-right + // z-down - } else { - // Rangefinder value can not be send as Inf to ardupilot - const double range = this->dataPtr->rangefinderSensor->Range(0); - pkt.rangefinder = std::isinf(range) ? 0.0 : range; - } + // get linear acceleration + gz::math::Vector3d linearAccel{ + imuMsg.linear_acceleration().x(), + imuMsg.linear_acceleration().y(), + imuMsg.linear_acceleration().z() + }; - // airspeed : wind = Vector3(environment.wind.x, environment.wind.y, environment.wind.z) - // pkt.airspeed = (pkt.velocity - wind).length() -*/ - this->dataPtr->socket_out.Send(&pkt, sizeof(pkt)); + // get angular velocity + gz::math::Vector3d angularVel{ + imuMsg.angular_velocity().x(), + imuMsg.angular_velocity().y(), + imuMsg.angular_velocity().z(), + }; + + /* + Gazebo versus ArduPilot frame conventions + ========================================= + + 1. ArduPilot assumes an aircraft convention for body and world frames: + + ArduPilot world frame is: x-north, y-east, z-down (NED) + ArduPilot body frame is: x-forward, y-right, z-down + + 2. The Gazebo frame convention is: + + Gazebo world frame is: x-east, y-north, z-up (ENU) + Gazebo body frame is: x-forward, y-left, z-up + + Reference: https://gazebosim.org/api/gazebo/7.0/spherical_coordinates.html + + In some cases the Gazebo body frame may use a non-standard convention, + for example the Zephyr delta wing model has x-left, y-back, z-up. + + 3. The position and orientation provided to ArduPilot must be given as the + transform from the Aircraft world frame to the Aircraft body frame. + We denote this as: + + wldAToBdyA = ^{w_A}\xi_{b_A} + + By which we mean that a vector expressed in Aircraft body frame coordinates + may be represented in Aircraft world frame coordinates by the transform: + + ^{wldA}v = ^{wldA}\xi_{bdyA} ^{bdyA}v + = ^{wldA}rot_{bdyA} ^{bdyA}v + ^{wldA}pos_{bdyA} + + i.e. a rotation from the world to body frame followed by a translation. + + Combining transforms + ==================== + + 1. Gazebo supplies us with the transform from the Gazebo world frame to the + Gazebo body frame (which we recall may be non-standard) + + wldGToBdyG = ^{wldG}\xi_{bdyG} + + 2. The transform from the Aircraft world frame to the Gazebo world frame + is fixed (provided Gazebo does not modify its convention) + + wldAToWldG = ^{wldA}\xi_{wldG} = Pose3d(0, 0, 0, -GZ_PI, 0, 0) + + Note that this is the inverse of the plugin parameter + which tells us how to rotate a Gazebo world frame into an Aircraft world frame. + + 3. The transform from the Aircraft body frame to the Gazebo body frame is denoted: + + bdyAToBdyG = ^{bdyA}\xi_{bdyG} + + This will typically be Pose3d(0, 0, 0, -GZ_PI, 0, 0) but in some cases will vary. + For instance the Zephyr uses the transform Pose3d(0, 0, 0, -GZ_PI, 0, GZ_PI/2) + + Note this is also the inverse of the plugin parameter + which tells us how to rotate a Gazebo model frame into an Aircraft body frame + + 4. Finally we compose the transforms to get the one required for the ArduPilot + JSON interface: + + ^{wldA}\xi_{bdyA} = ^{wldA}\xi_{wldG} * ^{wldG}\xi_{bdyG} * (^{bdyA}\xi_{bdyG})^{-1} + + where we use: + + ^{bdyG}\xi_{bdyA} = (^{bdyA}\xi_{bdyG})^{-1} + + In C++ variables names this is: + + wldAToBdyA = wldAToWldG * wldGToBdyG * bdyAToBdyG.Inverse() + */ + + // get pose and velocity in Gazebo world frame + const gz::sim::components::WorldPose* worldPose = + _ecm.Component( + this->dataPtr->modelLink); + + const gz::sim::components::WorldLinearVelocity* worldLinearVel = + _ecm.Component( + this->dataPtr->modelLink); + + // position and orientation transform (Aircraft world to Aircraft body) + gz::math::Pose3d bdyAToBdyG = this->dataPtr->modelXYZToAirplaneXForwardZDown.Inverse(); + gz::math::Pose3d wldAToWldG = this->dataPtr->gazeboXYZToNED.Inverse(); + gz::math::Pose3d wldGToBdyG = worldPose->Data(); + gz::math::Pose3d wldAToBdyA = wldAToWldG * wldGToBdyG * bdyAToBdyG.Inverse(); + + // velocity transformation + gz::math::Vector3d velWldG = worldLinearVel->Data(); + gz::math::Vector3d velWldA = wldAToWldG.Rot() * velWldG + wldAToWldG.Pos(); + + // require the duration since sim start in seconds + double timestamp = _simTime; + + using namespace rapidjson; + + // build JSON document + StringBuffer s; + Writer writer(s); + + writer.StartObject(); + + writer.Key("timestamp"); + writer.Double(timestamp); + + writer.Key("imu"); + writer.StartObject(); + writer.Key("gyro"); + writer.StartArray(); + writer.Double(angularVel.X()); + writer.Double(angularVel.Y()); + writer.Double(angularVel.Z()); + writer.EndArray(); + writer.Key("accel_body"); + writer.StartArray(); + writer.Double(linearAccel.X()); + writer.Double(linearAccel.Y()); + writer.Double(linearAccel.Z()); + writer.EndArray(); + writer.EndObject(); + + writer.Key("position"); + writer.StartArray(); + writer.Double(wldAToBdyA.Pos().X()); + writer.Double(wldAToBdyA.Pos().Y()); + writer.Double(wldAToBdyA.Pos().Z()); + writer.EndArray(); + + // ArduPilot quaternion convention: q[0] = 1 for identity. + writer.Key("quaternion"); + writer.StartArray(); + writer.Double(wldAToBdyA.Rot().W()); + writer.Double(wldAToBdyA.Rot().X()); + writer.Double(wldAToBdyA.Rot().Y()); + writer.Double(wldAToBdyA.Rot().Z()); + writer.EndArray(); + + writer.Key("velocity"); + writer.StartArray(); + writer.Double(velWldA.X()); + writer.Double(velWldA.Y()); + writer.Double(velWldA.Z()); + writer.EndArray(); + + // SITL/SIM_JSON supports these additional sensor fields + // rng_1 : 0 + // rng_2 : 0 + // rng_3 : 0 + // rng_4 : 0 + // rng_5 : 0 + // rng_6 : 0 + // windvane : { direction: 0, speed: 0 } + + // writer.Key("rng_1"); + // writer.Double(0.0); + + // writer.Key("windvane"); + // writer.StartObject(); + // writer.Key("direction"); + // writer.Double(1.57079633); + // writer.Key("speed"); + // writer.Double(5.5); + // writer.EndObject(); + + // get JSON + this->dataPtr->json_str = "\n" + std::string(s.GetString()) + "\n"; + // gzdbg << this->dataPtr->json_str << "\n"; +} + +///////////////////////////////////////////////// +void gz::sim::systems::ArduPilotPlugin::SendState() const +{ +#if DEBUG_JSON_IO + auto bytes_sent = +#endif + this->dataPtr->sock.sendto( + this->dataPtr->json_str.c_str(), + this->dataPtr->json_str.size(), + this->dataPtr->fcu_address, + this->dataPtr->fcu_port_out); + +#if DEBUG_JSON_IO + gzdbg << "sent " << bytes_sent << " bytes to " + << this->dataPtr->fcu_address << ":" << this->dataPtr->fcu_port_out << "\n" + << "frame_count: " << this->dataPtr->fcu_frame_count << "\n"; +#endif } diff --git a/src/GimbalSmall2dPlugin.cc b/src/GimbalSmall2dPlugin.cc index bcfbcf8..5d1b8f7 100644 --- a/src/GimbalSmall2dPlugin.cc +++ b/src/GimbalSmall2dPlugin.cc @@ -50,7 +50,7 @@ class gazebo::GimbalSmall2dPluginPrivate public: physics::JointPtr tiltJoint; /// \brief Command that updates the gimbal tilt angle - public: double command = IGN_PI_2; + public: double command = GZ_PI_2; /// \brief Pointer to the transport node public: transport::NodePtr node; diff --git a/src/Socket.cpp b/src/Socket.cpp new file mode 100644 index 0000000..ab447bb --- /dev/null +++ b/src/Socket.cpp @@ -0,0 +1,233 @@ +/* + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + */ +/* + simple socket handling class for systems with BSD socket API + */ + +#include "Socket.h" +#include + +/* + constructor + */ +SocketAPM::SocketAPM(bool _datagram) : + SocketAPM(_datagram, + socket(AF_INET, _datagram?SOCK_DGRAM:SOCK_STREAM, 0)) +{} + +SocketAPM::SocketAPM(bool _datagram, int _fd) : + datagram(_datagram), + fd(_fd) +{ + fcntl(fd, F_SETFD, FD_CLOEXEC); + if (!datagram) { + int one = 1; + setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); + } +} + +SocketAPM::~SocketAPM() +{ + if (fd != -1) { + ::close(fd); + fd = -1; + } +} + +void SocketAPM::make_sockaddr(const char *address, uint16_t port, struct sockaddr_in &sockaddr) +{ + memset(&sockaddr, 0, sizeof(sockaddr)); + +#ifdef HAVE_SOCK_SIN_LEN + sockaddr.sin_len = sizeof(sockaddr); +#endif + sockaddr.sin_port = htons(port); + sockaddr.sin_family = AF_INET; + sockaddr.sin_addr.s_addr = inet_addr(address); +} + +/* + connect the socket + */ +bool SocketAPM::connect(const char *address, uint16_t port) +{ + struct sockaddr_in sockaddr; + make_sockaddr(address, port, sockaddr); + + if (::connect(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { + return false; + } + return true; +} + +/* + bind the socket + */ +bool SocketAPM::bind(const char *address, uint16_t port) +{ + struct sockaddr_in sockaddr; + make_sockaddr(address, port, sockaddr); + + if (::bind(fd, (struct sockaddr *)&sockaddr, sizeof(sockaddr)) != 0) { + return false; + } + return true; +} + + +/* + set SO_REUSEADDR + */ +bool SocketAPM::reuseaddress(void) +{ + int one = 1; + return (setsockopt(fd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != -1); +} + +/* + set blocking state + */ +bool SocketAPM::set_blocking(bool blocking) +{ + int fcntl_ret; + if (blocking) { + fcntl_ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) & ~O_NONBLOCK); + } else { + fcntl_ret = fcntl(fd, F_SETFL, fcntl(fd, F_GETFL, 0) | O_NONBLOCK); + } + return fcntl_ret != -1; +} + +/* + set cloexec state + */ +bool SocketAPM::set_cloexec() +{ + return (fcntl(fd, F_SETFD, FD_CLOEXEC) != -1); +} + +/* + send some data + */ +ssize_t SocketAPM::send(const void *buf, size_t size) +{ + return ::send(fd, buf, size, 0); +} + +/* + send some data + */ +ssize_t SocketAPM::sendto(const void *buf, size_t size, const char *address, uint16_t port) +{ + struct sockaddr_in sockaddr; + make_sockaddr(address, port, sockaddr); + return ::sendto(fd, buf, size, 0, (struct sockaddr *)&sockaddr, sizeof(sockaddr)); +} + +/* + receive some data + */ +ssize_t SocketAPM::recv(void *buf, size_t size, uint32_t timeout_ms) +{ + if (!pollin(timeout_ms)) { + return -1; + } + socklen_t len = sizeof(in_addr); + return ::recvfrom(fd, buf, size, MSG_DONTWAIT, (sockaddr *)&in_addr, &len); +} + +/* + return the IP address and port of the last received packet + */ +void SocketAPM::last_recv_address(const char *&ip_addr, uint16_t &port) +{ + ip_addr = inet_ntoa(in_addr.sin_addr); + port = ntohs(in_addr.sin_port); +} + +void SocketAPM::set_broadcast(void) +{ + int one = 1; + setsockopt(fd,SOL_SOCKET,SO_BROADCAST,(char *)&one,sizeof(one)); +} + +/* + return true if there is pending data for input + */ +bool SocketAPM::pollin(uint32_t timeout_ms) +{ + fd_set fds; + struct timeval tv; + + FD_ZERO(&fds); + FD_SET(fd, &fds); + + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000UL; + + if (select(fd+1, &fds, nullptr, nullptr, &tv) != 1) { + return false; + } + return true; +} + + +/* + return true if there is room for output data + */ +bool SocketAPM::pollout(uint32_t timeout_ms) +{ + fd_set fds; + struct timeval tv; + + FD_ZERO(&fds); + FD_SET(fd, &fds); + + tv.tv_sec = timeout_ms / 1000; + tv.tv_usec = (timeout_ms % 1000) * 1000UL; + + if (select(fd+1, nullptr, &fds, nullptr, &tv) != 1) { + return false; + } + return true; +} + +/* + start listening for new tcp connections + */ +bool SocketAPM::listen(uint16_t backlog) +{ + return ::listen(fd, (int)backlog) == 0; +} + +/* + accept a new connection. Only valid for TCP connections after + listen has been used. A new socket is returned +*/ +SocketAPM *SocketAPM::accept(uint32_t timeout_ms) +{ + if (!pollin(timeout_ms)) { + return nullptr; + } + + int newfd = ::accept(fd, nullptr, nullptr); + if (newfd == -1) { + return nullptr; + } + // turn off nagle for lower latency + int one = 1; + setsockopt(newfd, IPPROTO_TCP, TCP_NODELAY, &one, sizeof(one)); + return new SocketAPM(false, newfd); +} diff --git a/worlds/iris_arducopter_cmac.world b/worlds/iris_arducopter_cmac.world index dffc832..621a2b2 100755 --- a/worlds/iris_arducopter_cmac.world +++ b/worlds/iris_arducopter_cmac.world @@ -33,7 +33,7 @@ - +
-35.363261 149.165230
300 satellite diff --git a/worlds/iris_arducopter_edifice_demo.world b/worlds/iris_arducopter_edifice_demo.world new file mode 100644 index 0000000..0495bf6 --- /dev/null +++ b/worlds/iris_arducopter_edifice_demo.world @@ -0,0 +1,459 @@ + + + + + false + + + 0.002 + 1.0 + + + + + ogre2 + + + + + + + + + + + + + + + 3D View + false + docked + + + ogre2 + scene + 0.4 0.4 0.4 + 0.8 0.8 0.8 + 6 0 6 0 0.5 3.14 + + + + + + World control + false + false + 72 + 121 + 1 + + floating + + + + + + + true + true + true + + + + + + + + floating + 5 + 5 + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + false + 5 + 5 + floating + false + + + + + false + 5 + 5 + floating + false + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + World stats + false + false + 110 + 290 + 1 + + floating + + + + + + + true + true + true + true + + + + + + + false + 0 + 0 + 250 + 50 + floating + false + #666666 + + + + + + + false + 250 + 0 + 150 + 50 + floating + false + #666666 + + + + + + + + + + false + 250 + 50 + 50 + 50 + floating + false + #777777 + + + + + + + false + docked + + + + + + + false + docked + + + + + + + docked_collapsed + + + + + + + docked_collapsed + + Panda + + + + + + + + + + false + 5 + 5 + floating + false + + + + + + + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Depot + + + + -6 0 0.25 0 0 0 + model://iris_with_standoffs + + + + -9.4 1.6 0 0 0 -0.5 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Mecanum lift + + + + -13.2 -6.0 0.15 0 0 0 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/fog emitter + + + + -9.0 -1.8 1.038 0 0 0 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Panda with Ignition position controller model + + + + -9.0 -2.4 0 0 0 1.57 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Reflective table + + + + -13.2 -6.0 0 0 0 1.57 + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Rescue Randy Sitting + + + + -9.0 -6.21 5.0 0 0.9 -3.14 + true + https://fuel.gazebosim.org/1.0/OpenRobotics/models/16-Bit Thermal Camera + + + + -9.0 -5.95 5.0 0 0.9 -3.14 + true + https://fuel.gazebosim.org/1.0/OpenRobotics/models/8-Bit Thermal Camera + + + + -9.075 -6.15 4.995 0 0.9 -3.14 + true + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Camera + + + + -9.04 -6.04 4.97 0 0.9 -3.14 + true + https://fuel.gazebosim.org/1.0/OpenRobotics/models/Depth Camera + + + + + -13.2 -5.3 0.2 0 0 0 + + + + 1 + 0 + 0 + 1 + 0 + 1 + + 1.0 + + + + + 0.4 0.4 0.4 + + + + + + + + 0.4 0.4 0.4 + + + + 1 0 0 1 + 1 0 0 1 + 1 0 0 1 + + + 295.0 + + + + + + + + + + + 87 + + + linear: {x: 1.0, y: 0.0}, angular: {z: 0.0} + + + + + + + 88 + + + linear: {x: -1.0, y: 0.0}, angular: {z: 0.0} + + + + + + + 68 + + + linear: {x: 0.0, y: -1.0}, angular: {z: 0.0} + + + + + + + 65 + + + linear: {x: 0.0, y: 1.0}, angular: {z: 0.0} + + + + + + + 69 + + + linear: {x: 0.0, y: 0.0}, angular: {z: -1.0} + + + + + + + 81 + + + linear: {x: 0.0, y: 0.0}, angular: {z: 1.0} + + + + + + + 83 + + + linear: {x: 0.0, y: 0.0}, angular: {z: 0.0} + + + + + diff --git a/worlds/iris_arducopter_runway.world b/worlds/iris_arducopter_runway.world index f3269a3..3149f17 100755 --- a/worlds/iris_arducopter_runway.world +++ b/worlds/iris_arducopter_runway.world @@ -1,93 +1,46 @@ - - - - - -5 0 1 0 0.2 0 - - - - - - quick - 100 - 1.0 - - - 0.0 - 0.2 - 0.1 - 0.0 - - - -1 - - - 0 0 -9.8 - - model://sun - + + + + + + + + + + - - true - - - - - 0 0 1 - 5000 5000 - - - - - - 100 - 50 - - - - - - 000 0 0.005 0 0 0 - false - - - 0 0 1 - 1829 45 - - - - - - + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + - - 0 0 -0.1 0 0 0 - false - - - 0 0 1 - 5000 5000 - - - - - - + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + - - + + model://runway + + + + model://iris_with_standoffs + - - - model://iris_with_ardupilot - - diff --git a/worlds/zephyr_arduplane_runway.world b/worlds/zephyr_arduplane_runway.world new file mode 100755 index 0000000..159717b --- /dev/null +++ b/worlds/zephyr_arduplane_runway.world @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + 1.0 1.0 1.0 + 0.8 0.8 0.8 + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + model://runway + + + + + 0 0 0.422 -1.5707963 0 1.5707963 + + model://zephyr_with_ardupilot + + + +