├── .github └── workflows │ └── ros-base-docker.yml ├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── config ├── operator.yaml ├── repos │ └── simulator.repos └── robot.yaml ├── doc ├── CAMP_3Click_Survey_Pattern.gif ├── Installation.md └── Installation_Melodic.md ├── include └── project11 │ ├── gggs.h │ ├── gggs │ ├── bounds.h │ ├── cell_area_iterator.h │ ├── cell_index.h │ ├── core.h │ ├── grid_area_iterator.h │ ├── grid_index.h │ ├── level.h │ └── level_spec.h │ ├── gz4d_geo.h │ ├── pid.h │ ├── tf2_utils.h │ └── utils.h ├── launch ├── local_remote_operator.launch ├── observer_core.launch ├── operator_core_launch.py ├── operator_ui_launch.py ├── platform_nav_source_launch.py ├── platform_sender_launch.py ├── remote_operator.launch └── robot_core_launch.py ├── package.xml ├── project11 ├── __init__.py ├── geodesic.py ├── nav.py └── wgs84.py ├── scripts └── platform_send.py └── src └── utils.cpp /.github/workflows/ros-base-docker.yml: -------------------------------------------------------------------------------- 1 | name: docker-noetic-ros-base 2 | 3 | on: 4 | push: 5 | branches: [ noetic ] 6 | pull_request: 7 | branches: [ noetic ] 8 | 9 | jobs: 10 | build: 11 | runs-on: ubuntu-20.04 12 | container: 13 | image: ros:noetic-ros-base 14 | 15 | steps: 16 | - name: Update apt 17 | run: apt update 18 | - name: Install git and vcstool 19 | run: DEBIAN_FRONTEND=noninteractive apt -y install git python3-vcstool 20 | - name: Checkout Project11 21 | uses: actions/checkout@v3 22 | - name: Update rosdep 23 | run: rosdep update 24 | - name: Create workspace 25 | run: mkdir -p ../catkin_ws/src 26 | - name: Install repos 27 | run: vcs import --input config/repos/simulator.repos ../catkin_ws/src/ 28 | - name: Install ROS dependencies 29 | working-directory: ../catkin_ws/ 30 | run: DEBIAN_FRONTEND=noninteractive rosdep install --from-paths src --ignore-src -r -y 31 | - name: Catkin_make 32 | working-directory: ../catkin_ws/ 33 | run: bash -c 'source /opt/ros/noetic/setup.bash && catkin_make' 34 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | *.~ 2 | *~ 3 | *.pyc 4 | .kdev4/ 5 | *.kdev4 6 | logs -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | 3 | project(project11) 4 | 5 | set(CMAKE_CXX_STANDARD 17) 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(ament_cmake_python REQUIRED) 12 | 13 | find_package(geographic_msgs REQUIRED) 14 | find_package(geometry_msgs REQUIRED) 15 | find_package(project11_msgs REQUIRED) 16 | find_package(tf2 REQUIRED) 17 | find_package(tf2_geometry_msgs REQUIRED) 18 | find_package(tf2_ros REQUIRED) 19 | find_package(rclcpp REQUIRED) 20 | find_package(rclcpp_lifecycle REQUIRED) 21 | 22 | ament_python_install_package(${PROJECT_NAME}) 23 | 24 | add_library(${PROJECT_NAME} src/utils.cpp) 25 | add_library(${PROJECT_NAME}::${PROJECT_NAME} ALIAS ${PROJECT_NAME}) 26 | 27 | target_include_directories(${PROJECT_NAME} 28 | PUBLIC 29 | "$" 30 | "$" 31 | ) 32 | target_link_libraries(${PROJECT_NAME} PUBLIC 33 | rclcpp::rclcpp 34 | rclcpp_lifecycle::rclcpp_lifecycle 35 | tf2::tf2 36 | tf2_ros::tf2_ros 37 | tf2_geometry_msgs::tf2_geometry_msgs 38 | ) 39 | 40 | install( 41 | DIRECTORY include/ 42 | DESTINATION include/${PROJECT_NAME} 43 | ) 44 | 45 | install( 46 | TARGETS ${PROJECT_NAME} 47 | EXPORT export_${PROJECT_NAME} 48 | ARCHIVE DESTINATION lib 49 | LIBRARY DESTINATION lib 50 | RUNTIME DESTINATION bin 51 | ) 52 | 53 | install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) 54 | install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) 55 | 56 | install(PROGRAMS scripts/platform_send.py DESTINATION lib/${PROJECT_NAME}) 57 | 58 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 59 | ament_export_dependencies(geographic_msgs geometry_msgs project11_msgs tf2_geometry_msgs tf2_ros rclcpp_lifecycle) 60 | ament_export_include_directories("include/${PROJECT_NAME}") 61 | 62 | ament_package() 63 | 64 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2016-2020, Roland Arsenault 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | [![docker-noetic-ros-base](../../actions/workflows/ros-base-docker.yml/badge.svg)](../../actions/workflows/ros-base-docker.yml) 2 | 3 | # Project11: A mapping focused open-sourced software framework for Autonomous Surface Vehicles 4 | 5 | The Project 11 framework was developed as a backseat driver for Autonomous Surface Vehicles 6 | (ASVs). Key design features include the ability to quickly and easily specify survey plans; monitoring of mission progress, even 7 | over unreliable wireless networks; and to provide an environment to develop advanced autonomous technologies. 8 | 9 | ## Installation 10 | 11 | See the following for detailed instructions on setting up a Simulation Environment: 12 | 13 | [Installing Project11's backseat driver system and simulator.](doc/Installation.md) 14 | 15 | ### Quick Installation Guide 16 | 17 | If you have an available ROS Noetic system, you can quickly install and run Project11 with the following: 18 | 19 | mkdir -p project11/catkin_ws/src 20 | cd project11/catkin_ws/src 21 | git clone https://github.com/CCOMJHC/project11.git 22 | 23 | sudo apt-get install python3-rosdep python3-vcstool 24 | sudo rosdep init 25 | rosdep update 26 | 27 | vcs import < project11/config/repos/simulator.repos 28 | 29 | rosdep install --from-paths . --ignore-src -r -y 30 | 31 | cd .. 32 | catkin_make 33 | 34 | source devel/setup.bash 35 | roslaunch project11_simulation sim_local.launch 36 | 37 | ## Major components and concepts 38 | 39 | A typical setup has a ROS core running on the robot with some key nodes including the `mission_manager`, the `helm_manager` and the `udp_bridge`. The operator station runs a separate ROS core that also runs a `udp_bridge` node as well as `camp`, the CCOM Autonomous Mission Planner which provide a planning and monitoring interface. 40 | 41 | ### Operator user interface - CAMP 42 | 43 | The [CCOM Autonomous Mission Planner](../../../camp), also known as CAMP, displays the vehicle's position on background georeferenced charts and maps. It also allows the planning of missions to be sent to the vehicle and to manage the vehicle's piloting mode. 44 | 45 | ![Camp](doc/CAMP_3Click_Survey_Pattern.gif) 46 | 47 | ### UDP Bridge - udp_bridge 48 | 49 | The [UDP Bridge](../../../udp_bridge) sends select ROS topics between ROS cores. It allows control and monitoring over wireless unreliable networks. 50 | 51 | ### Mission Manager - mission_manager 52 | 53 | The [Mission Manager](../../../mission_manager) receives missions from CAMP and executes them. It also handles requests such as hover. 54 | 55 | ### Helm Manager - helm_manager 56 | 57 | The [Helm Manager](../../../helm_manager) controls which commands get sent to the underlying hardware. It reacts to changes in piloting mode by sending messages to the piloting mode enable topics and only allowing incoming control messages from the current piloting mode to be sent to the hardware interface. 58 | 59 | ### Piloting modes 60 | 61 | Project11 operates in 3 major piloting modes: "manual", "autonomous" and "standby". 62 | 63 | In "manual" mode, the vehicle responds to commands sent from a device such as a joystick or a gamepad. The commands are converted to "helm" messages by the `joy_to_helm` node and are sent from the operator station to the vehicle via the `udp_bridge`. 64 | 65 | In "autonomous" mode, the `mission_manager` sends mission items to the navigation stack and responds to override commands such as the hover command that can allow the vehicle to station keep for a while, then resume the mission when the hover is canceled. 66 | 67 | The "standby" mode is used to when no control commands are to be sent by Project11. 68 | 69 | 70 | 71 | ### Navigation Stack 72 | 73 | The `mission_manager` receives and executes the higher level missions. Depending on the task, track lines may be sent to the `path_follower` or another node will receive a higher level directive, such as "survey this area", and generate and send out track lines or other navigation segments to lower level planners or controllers. 74 | Eventually, a "helm" or "cmd_velocity" message gets sent to the autonomous/helm or autonomous/cmd_vel topic reaching the `helm_manager`. 75 | -------------------------------------------------------------------------------- /config/operator.yaml: -------------------------------------------------------------------------------- 1 | udp_bridge: 2 | ros__parameters: 3 | name: operator 4 | remotes: 5 | robot: 6 | connections: 7 | default: 8 | # host: localhost 9 | port: 4200 10 | -------------------------------------------------------------------------------- /config/repos/simulator.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | asv_helm: 3 | type: git 4 | url: https://github.com/CCOMJHC/asv_helm.git 5 | version: noetic 6 | asv_sim: 7 | type: git 8 | url: https://github.com/CCOMJHC/asv_sim.git 9 | version: noetic 10 | ben_description: 11 | type: git 12 | url: https://github.com/CCOMJHC/ben_description.git 13 | version: noetic 14 | ben_project11: 15 | type: git 16 | url: https://github.com/CCOMJHC/ben_project11.git 17 | version: noetic 18 | camp: 19 | type: git 20 | url: https://github.com/CCOMJHC/camp.git 21 | version: noetic 22 | command_bridge: 23 | type: git 24 | url: https://github.com/CCOMJHC/command_bridge 25 | version: noetic 26 | dubins_curves: 27 | type: git 28 | url: https://github.com/CCOMJHC/dubins_curves.git 29 | version: noetic 30 | geographic_visualization_msgs: 31 | type: git 32 | url: https://github.com/CCOMJHC/geographic_visualization_msgs.git 33 | version: noetic 34 | helm_manager: 35 | type: git 36 | url: https://github.com/CCOMJHC/helm_manager.git 37 | version: noetic 38 | hover: 39 | type: git 40 | url: https://github.com/CCOMJHC/hover.git 41 | version: noetic 42 | joy_to_helm: 43 | type: git 44 | url: https://github.com/CCOMJHC/joy_to_helm.git 45 | version: noetic 46 | marine_ais: 47 | type: git 48 | url: https://github.com/CCOMJHC/marine_ais.git 49 | marine_msgs: 50 | type: git 51 | url: https://github.com/CCOMJHC/marine_msgs.git 52 | version: main 53 | mission_manager: 54 | type: git 55 | url: https://github.com/CCOMJHC/mission_manager.git 56 | version: noetic 57 | mru_transform: 58 | type: git 59 | url: https://github.com/CCOMJHC/mru_transform.git 60 | version: noetic 61 | path_follower: 62 | type: git 63 | url: https://github.com/CCOMJHC/path_follower.git 64 | version: noetic 65 | project11: 66 | type: git 67 | url: https://github.com/CCOMJHC/project11.git 68 | version: noetic 69 | project11_msgs: 70 | type: git 71 | url: https://github.com/CCOMJHC/project11_msgs.git 72 | version: noetic 73 | project11_navigation: 74 | type: git 75 | url: https://github.com/CCOMJHC/project11_navigation.git 76 | version: noetic 77 | project11_nav_msgs: 78 | type: git 79 | url: https://github.com/CCOMJHC/project11_nav_msgs.git 80 | version: noetic 81 | project11_simulation: 82 | type: git 83 | url: https://github.com/CCOMJHC/project11_simulation.git 84 | version: noetic 85 | remote_manager: 86 | type: git 87 | url: https://github.com/CCOMJHC/remote_manager.git 88 | s57_layer: 89 | type: git 90 | url: https://github.com/CCOMJHC/s57_layer.git 91 | udp_bridge: 92 | type: git 93 | url: https://github.com/CCOMJHC/udp_bridge.git 94 | version: noetic 95 | -------------------------------------------------------------------------------- /config/robot.yaml: -------------------------------------------------------------------------------- 1 | udp_bridge: 2 | ros__parameters: 3 | name: robot 4 | remotes: 5 | operator: 6 | connections: 7 | default: 8 | port: 4200 9 | maximumBytesPerSecond: 1000000 10 | topics: 11 | platforms: {source: /project11/platforms} 12 | nav_orientation: {source: project11/nav/orientation} 13 | nav_position: {source: project11/nav/position} 14 | nav_velocity: {source: project11/nav/velocity} 15 | nav_active_sensor: {source: project11/nav/active_sensor} 16 | project11_display: {source: project11/display} 17 | project11_heartbeat: {source: project11/heartbeat} 18 | project11_status_mission_manager: {source: project11/status/mission_manager} 19 | project11_response: {source: project11/response} 20 | # diagnostics: {source: /diagnostics} 21 | tf: {source: /tf} 22 | tf_static: {source: /tf_static} 23 | control_helm: {source: project11/control/helm} 24 | odom: {source: odom} 25 | nav_visualization_markers: {source: navigator/visualization_markers} 26 | 27 | helm_manager: 28 | ros__parameters: 29 | piloting_modes: 30 | active: ['manual', 'autonomous'] 31 | standby: ['standby'] 32 | -------------------------------------------------------------------------------- /doc/CAMP_3Click_Survey_Pattern.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/rolker/project11/c599a278fa59fd6f8fcb4a8e1c68d7a52f13eec6/doc/CAMP_3Click_Survey_Pattern.gif -------------------------------------------------------------------------------- /doc/Installation.md: -------------------------------------------------------------------------------- 1 | 2 | # Installing Project 11's backseat driver system and simulator. 3 | 4 | ## Introduction 5 | 6 | CCOM/JHC's ASVs are capable of operating with a backseat driver, which is a computer not supplied by the manufaturer capable of taking control of the vehicle by supplying throttle or speed and rudder or heading commands. The vehicle's manufacturer supplied computer accepts such commands via a [ROS](http://www.ros.org/) interface. 7 | 8 | ROS is offically supported on Ubuntu Linux so the Project 11 system runs on Ubuntu. The actual system consists of two separate computers, each running Ubuntu, comunicating over an unreliable wireless connection. For simulation puposes, the system has been designed to also work on a single computer. 9 | 10 | ## Background knowledge 11 | 12 | ### Ubuntu Linux 13 | 14 | The ROS tutorials recommend [this page](http://www.ee.surrey.ac.uk/Teaching/Unix/) for beginner Unix tutorials. 15 | 16 | ### ROS 17 | 18 | Project 11 uses the Noetic version of ROS. It is recomended to go through the [ROS Tutorials](http://wiki.ros.org/ROS/Tutorials) in order to better understand how it works. 19 | 20 | ### Git 21 | 22 | TODO: Add git tutorials. [git book](https://git-scm.com/book/en/v2). 23 | 24 | ### Programing 25 | 26 | Knowledge of Python or C++ is necessary for development. 27 | 28 | #### Python 29 | 30 | TODO Pyhton tutorials. (Python's own tutorial has served me well) 31 | 32 | #### C++ 33 | 34 | TODO C++ tutorials. 35 | 36 | ## Prerequisite 37 | 38 | The Project 11 system is based on ROS Noetic so it is designed to run on an Ubuntu 20.04 system. 39 | 40 | If an Ubuntu 20.04 machine is not readily available, a virtual machine may be setup for this purpose. 41 | 42 | ### Setting up an Ubuntu virtual machine 43 | 44 | These instructions were developed using VirtualBox on Ubuntu. Similar steps should work with VMWare as well. 45 | 46 | * Obtain an Ubuntu 20.04 iso install image from the [Ubuntu download page](https://ubuntu.com/download/desktop). 47 | * Create a virtual machine for a 64-bit Ubuntu install with the following specs: 48 | * 4GB or more of memory. (TODO: do we need more? At 1GB, ran out of memory compiling QT5) 49 | * 50GB or more of hard drive space. 50 | * Insert the downloaded Ubuntu iso image in the virtual machine's optical drive and start the virtual machine. 51 | * Chose "Install Ubuntu" when presented the choice. 52 | * Select "Download updates while installing Ubuntu" and optonally select "Install third-party software" (TODO: do this last one make a difference?) 53 | * Continue with default "Erase disk and install Ubuntu". 54 | * Pick reasonable values for timezone and languages. 55 | * Create a user account for yourself (for a shared machine, you may create a field account instead). 56 | * Install guest additions or equivalent. (Caution, skipping this step with VMWare may have caused diplay errors on boot later on.) 57 | 58 | Once Ubuntu is installed, you can make sure it's up to date by doing a apt-get update followed by an apt-get upgrade. 59 | 60 | sudo apt-get update 61 | sudo apt-get upgrade 62 | 63 | ## Installing ROS 64 | 65 | Follow the instructions on the [Ubuntu install of ROS Noetic](http://wiki.ros.org/noetic/Installation/Ubuntu) page, picking the Desktop-Full installation. 66 | 67 | If you are new to ROS, now is a good time to follow the ROS tutorials. 68 | 69 | ## Configuring git. 70 | 71 | Make sure git is installed. 72 | 73 | sudo apt install git 74 | 75 | The version control client git should already be installed so it only needs to be configured. The git book's [First Time Git Setup page](https://git-scm.com/book/en/v2/Getting-Started-First-Time-Git-Setup) contains instructions on how to configure your name and email address so that commits get attributed to you. 76 | 77 | git config --global user.name "Field User" 78 | git config --global user.email field@ccom.unh.edu 79 | 80 | ## Creating the Project 11 directory structure. 81 | 82 | Create the Project11 workspace. 83 | 84 | mkdir -p project11/catkin_ws/src 85 | 86 | git clone https://github.com/CCOMJHC/project11.git project11/catkin_ws/src/project11/ 87 | 88 | 89 | Install additional packages available from the package manager. 90 | 91 | sudo apt install python3-rosdep python3-vcstool 92 | 93 | Clone remaining repos using vcs. 94 | 95 | vcs import --input project11/catkin_ws/src/project11/config/repos/simulator.repos project11/catkin_ws/src/ 96 | 97 | Install ROS dependencies. 98 | 99 | rosdep update 100 | rosdep install --from-paths project11/catkin_ws/src --ignore-src -r -y 101 | 102 | 103 | ## Build the packages 104 | Build in the catkin workspace. 105 | 106 | cd project11/catkin_ws 107 | catkin_make 108 | 109 | Normally, the Project 11 Catkin workspace is the only one used, so we can make it the default workspace when logging in by sourcing the setup script from .bashrc. 110 | 111 | echo "source ~/project11/catkin_ws/devel/setup.bash" >> ~/.bashrc 112 | source ~/.bashrc 113 | 114 | ### Optional approach if using multiple workspaces 115 | 116 | A more general approach if working with several catkin workspaces is to use the following alias to source the setup script (this alias is to be added to ~/.bashrc) 117 | 118 | 119 | function wsource() { 120 | WS_SETUP_SCRIPT=`catkin locate -qd`/setup.bash 121 | if [ -e $WS_SETUP_SCRIPT ]; 122 | then 123 | echo -e "\e[32mAuto sourcing ros workspace at $WS_SETUP_SCRIPT\e[0m" 124 | source $WS_SETUP_SCRIPT 125 | fi 126 | } 127 | 128 | Usage: call wsource once inside your catkin workspace. 129 | 130 | ## Run the simulation 131 | 132 | Once catkin_make completes without error in ~/project11/catkin_ws/, we can try starting the simulation. 133 | 134 | roslaunch project11_simulation sim_local.launch 135 | 136 | You may encouter an error about a python script not being able to import a module. That may indicate that the catkin workspace's setup script needs to be resourced. 137 | 138 | source ~/project11/catkin_ws/devel/setup.bash 139 | 140 | Once sim_local.launch is succesfully launch, we can verify that things are running: 141 | 142 | rostopic list 143 | rostopic echo /ben/odom 144 | 145 | 146 | ## Contributing back to Project11 147 | 148 | The standard way in "github" to contribute code and other changes back into a repository is to "fork" the repository. This creates a copy of the repository in your own github account to which you can push modifications as you wish. 149 | But you cannot modify the code in the forked version of the repository directly, because it is on the github server. Rather you add a remote in the previously cloned copy on your local machine pointing to your github fork. You can make changes and "push" them back to your forked copy on github. 150 | Finally, if you want to contribute your changes back to the original offical CCOMJHC repository you navigate to that repo on github and create a "pull request". This is a request for the maintainer of the CCOMJHC repository to "pull" your changes back into the original repository. If he/she agrees with your suggested changes, your request will be granted and your code will become part of the official archive. 151 | 152 | Thus in addition to cloning the repositories as shown above, you may also want to fork the ones you wish to modify into your own github account and add remotes pointing to the forked copies. 153 | 154 | ## rviz 155 | 156 | With the simulation running, rviz may be launched in a new terminal. 157 | 158 | rviz 159 | 160 | A configuration file showing the map frame of reference as well as the simulated boat, named base_link, can be found at ~/project11/catkin_ws/src/project11/rviz/transforms.rviz. 161 | 162 | ## Downloading NOAA raster navigation chart for CAMP 163 | 164 | CAMP uses georeferenced raster images as background layers. The simulations defaults to Portsmouth Harbor in New Hampshire, so a raster nautical chart works fine as a background layer. NOAA chart 13283 has been included for your convenince. 165 | 166 | In CAMP, File->Open Background or the BG button, and open 13283_2.KAP. 167 | -------------------------------------------------------------------------------- /doc/Installation_Melodic.md: -------------------------------------------------------------------------------- 1 | 2 | # Installing Project 11's backseat driver system and simulator. 3 | 4 | ## Introduction 5 | 6 | CCOM/JHC's ASVs are capable of operating with a backseat driver, which is a computer not supplied by the manufaturer capable of taking control of the vehicle by supplying throttle or speed and rudder or heading commands. The vehicle's manufacturer supplied computer accepts such commands via a [ROS](http://www.ros.org/) interface. 7 | 8 | ROS is offically supported on Ubuntu Linux so the Project 11 system runs on Ubuntu. The actual system consists of two separate computers, each running Ubuntu, comunicating over an unreliable wireless connection. For simulation puposes, the system has been designed to also work on a single computer. 9 | 10 | A common directory structure has been created that typically resides in the user's home directory. The same structure is used both on the vehicle and on the operator station. 11 | 12 | project11/ 13 | ├── catkin_ws 14 | ├── configuration 15 | ├── documentation 16 | ├── executed_missions 17 | ├── log 18 | ├── ros 19 | └── scripts 20 | 21 | ROS development is done in the catkin_ws directory while ROS uses the ros directory at runtime. The documentation directory is where this and other documents live. The scripts directory contains useful shell scripts that help during development and operation. 22 | 23 | ## Background knowledge 24 | 25 | ### Ubuntu Linux 26 | 27 | The ROS tutorials recommend [this page](http://www.ee.surrey.ac.uk/Teaching/Unix/) for beginner Unix tutorials. 28 | 29 | ### ROS 30 | 31 | Project 11 uses the Melodic version of ROS. It is recomended to go through the [ROS Tutorials](http://wiki.ros.org/ROS/Tutorials) in order to better understand how it works. 32 | 33 | ### Git 34 | 35 | TODO: Add git tutorials. [git book](https://git-scm.com/book/en/v2). 36 | 37 | ### Programing 38 | 39 | Knowledge of Python or C++ is necessary for development. 40 | 41 | #### Python 42 | 43 | TODO Pyhton tutorials. (Python's own tutorial has served me well) 44 | 45 | #### C++ 46 | 47 | TODO C++ tutorials. 48 | 49 | ## Prerequisite 50 | 51 | The Project 11 system is based on ROS Melodic so it is designed to run on an Ubuntu 18.04 system. 52 | 53 | If an Ubuntu 18.04 machine is not readily available, a virtual machine may be setup for this purpose. 54 | 55 | ### Setting up an Ubuntu virtual machine 56 | 57 | These instructions were developed using VirtualBox on Ubuntu. Similar steps should work with VMWare as well. 58 | 59 | * Obtain an Ubuntu iso install image from the [Bionic release page](http://releases.ubuntu.com/bionic/). We are looking for the [64-bit PC (AMD64) desktop image.](http://releases.ubuntu.com/bionic/ubuntu-18.04.4-desktop-amd64.iso) 60 | * Create a virtual machine for a 64-bit Ubuntu install with the following specs: 61 | * 4GB or more of memory. (TODO: do we need more? At 1GB, ran out of memory compiling QT5) 62 | * 50GB or more of hard drive space. 63 | * Insert the downloaded Ubuntu iso image in the virtual machine's optical drive and start the virtual machine. 64 | * Chose "Install Ubuntu" when presented the choice. 65 | * Select "Download updates while installing Ubuntu" and optonally select "Install third-party software" (TODO: do this last one make a difference?) 66 | * Continue with default "Erase disk and install Ubuntu". 67 | * Pick reasonable values for timezone and languages. 68 | * Create a user account for yourself (for a shared machine, you may create a field account instead). 69 | * Install guest additions or equivalent. (Caution, skipping this step with VMWare may have caused diplay errors on boot later on.) 70 | 71 | Once Ubuntu is installed, you can make sure it's up to date by doing a apt-get update followed by an apt-get upgrade. 72 | 73 | sudo apt-get update 74 | sudo apt-get upgrade 75 | 76 | ## Installing ROS 77 | 78 | Follow the instructions on the [Ubuntu install of ROS Melodic](http://wiki.ros.org/melodic/Installation/Ubuntu) page, picking the Desktop-Full installation. 79 | 80 | If you are new to ROS, now is a good time to follow the ROS tutorials. 81 | 82 | ## Configuring git. 83 | 84 | Make sure git is installed. 85 | 86 | sudo apt install git 87 | 88 | The version control client git should already be installed so it only needs to be configured. The git book's [First Time Git Setup page](https://git-scm.com/book/en/v2/Getting-Started-First-Time-Git-Setup) contains instructions on how to configure your name and email address so that commits get attributed to you. 89 | 90 | git config --global user.name "Field User" 91 | git config --global user.email field@ccom.unh.edu 92 | 93 | ## Creating the Project 11 directory structure. 94 | 95 | Fetching the Project11 workspace. 96 | 97 | git clone --recursive https://github.com/CCOMJHC/project11.git 98 | 99 | Alternatively, you may want to use SSH if you have an SSH key setup on github. 100 | 101 | git clone --recursive git@github.com:CCOMJHC/project11.git 102 | 103 | Install additional packages available from the package manager. 104 | 105 | sudo project11/scripts/install_prereq_packages.bash 106 | 107 | ## Build the packages 108 | Build in the catkin workspace. 109 | 110 | cd project11/catkin_ws 111 | catkin_make 112 | 113 | Normally, the Project 11 Catkin workspace is the only one used, so we can make it the default workspace when logging in by sourcing the setup script from .bashrc. 114 | 115 | echo "source ~/project11/catkin_ws/devel/setup.bash" >> ~/.bashrc 116 | source ~/.bashrc 117 | 118 | ### Optional approach if using multiple workspaces 119 | 120 | A more general approach if working with several catkin workspaces is to use the following alias to source the setup script (this alias is to be added to ~/.bashrc) 121 | 122 | 123 | function wsource() { 124 | WS_SETUP_SCRIPT=`catkin locate -qd`/setup.bash 125 | if [ -e $WS_SETUP_SCRIPT ]; 126 | then 127 | echo -e "\e[32mAuto sourcing ros workspace at $WS_SETUP_SCRIPT\e[0m" 128 | source $WS_SETUP_SCRIPT 129 | fi 130 | } 131 | 132 | Usage: call wsource once inside your catkin workspace. 133 | 134 | ## Run the simulation 135 | 136 | Once catkin_make completes without error in ~/project11/catkin_ws/, we can try starting the simulation. 137 | 138 | roslaunch project11 sim_local.launch 139 | 140 | You may encouter an error about a python script not being able to import a module. That may indicate that the catkin workspace's setup script needs to be resourced. 141 | 142 | source ~/project11/catkin_ws/devel/setup.bash 143 | 144 | Once sim_local.launch is succesfully launch, we can verify that things are running: 145 | 146 | rostopic list 147 | rostopic echo /udp/position 148 | 149 | 150 | ## Contributing back to Project11 151 | 152 | The standard way in "github" to contribute code and other changes back into a repository is to "fork" the repository. This creates a copy of the repository in your own github account to which you can push modifications as you wish. 153 | But you cannot modify the code in the forked version of the repository directly, because it is on the github server. Rather you add a remote in the previously cloned copy on your local machine pointing to your github fork. You can make changes and "push" them back to your forked copy on github. 154 | Finally, if you want to contribute your changes back to the original offical CCOMJHC repository you navigate to that repo on github and create a "pull request". This is a request for the maintainer of the CCOMJHC repository to "pull" your changes back into the original repository. If he/she agrees with your suggested changes, your request will be granted and your code will become part of the official archive. 155 | 156 | Thus in addition to cloning the repositories as shown above, you may also want to fork the ones you wish to modify into your own github account and add remotes pointing to the forked copies. 157 | 158 | ## rviz 159 | 160 | With the simulation running, rviz may be launched in a new terminal. 161 | 162 | rviz 163 | 164 | A configuration file showing the map frame of reference as well as the simulated boat, named base_link, can be found at ~/project11/catkin_ws/src/project11/rviz/transforms.rviz. 165 | 166 | ## Downloading NOAA raster navigation chart for CAMP 167 | 168 | CAMP uses georeferenced raster images as background layers. The simulations defaults to Portsmouth Harbor in New Hampshire, so a raster nautical chart works fine as a background layer. NOAA chart 13283 has been included for your convenince. 169 | 170 | In CAMP, File->Open Background or the BG button, and open 13283_2.KAP. 171 | -------------------------------------------------------------------------------- /include/project11/gggs.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_H 2 | #define PROJECT11_GGGS_H 3 | 4 | #include "gggs/core.h" 5 | #include "gggs/level_spec.h" 6 | #include "gggs/grid_index.h" 7 | #include "gggs/cell_index.h" 8 | #include "gggs/bounds.h" 9 | #include "gggs/level.h" 10 | #include "gggs/grid_area_iterator.h" 11 | #include "gggs/cell_area_iterator.h" 12 | 13 | // based on "A global geographic grid system for visualizing bathymetry" by Colin Ware et al. 14 | // https://gi.copernicus.org/articles/9/375/2020/ 15 | // 16 | // A system for indexing tiles in a quad tree using geographic coordinates. 17 | // At level 0, tiles are 8x8 degrees for most of the earth. Tiles at or above 18 | // 72 degrees of latitude cover 24 degrees of longitude while tiles at or 19 | // above 80 degrees of latitude cover 72 degrees of longitude. Similar 20 | // scale changes are also applied towards the south pole. 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /include/project11/gggs/bounds.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_BOUNDS_H 2 | #define PROJECT11_GGGS_BOUNDS_H 3 | 4 | #include "grid_index.h" 5 | 6 | namespace gggs 7 | { 8 | 9 | class GridBounds 10 | { 11 | public: 12 | bool valid() const 13 | { 14 | return min_.valid() && max_.valid() && min_.level() == max_.level(); 15 | } 16 | 17 | void expand(const GridIndex& index) 18 | { 19 | if(index.valid()) 20 | { 21 | if(valid()) 22 | { 23 | min_ = min(min_, index); 24 | max_ = max(max_, index); 25 | } 26 | else 27 | { 28 | min_ = index; 29 | max_ = index; 30 | } 31 | } 32 | } 33 | 34 | const GridIndex& minimum() const 35 | { 36 | return min_; 37 | } 38 | 39 | const GridIndex& maximum() const 40 | { 41 | return max_; 42 | } 43 | 44 | uint32_t gridRowCount() const 45 | { 46 | return 1+max_.row()-min_.row(); 47 | } 48 | 49 | uint32_t gridColumnCount() const 50 | { 51 | if(min_.valid()) 52 | return 1+max_.column()-min_.column(); 53 | return 0; 54 | } 55 | 56 | uint64_t cellRowCount() const 57 | { 58 | return gridRowCount()*cell_rows_per_grid; 59 | } 60 | 61 | uint64_t cellColumnCount() const 62 | { 63 | return gridColumnCount()*cell_columns_per_grid; 64 | } 65 | 66 | friend std::ostream& operator<< (std::ostream& stream, const GridBounds& bounds) 67 | { 68 | stream << "min: " << bounds.min_ << " max: " << bounds.max_; 69 | return stream; 70 | } 71 | 72 | 73 | private: 74 | GridIndex min_; 75 | GridIndex max_; 76 | 77 | }; 78 | 79 | } 80 | 81 | #endif 82 | -------------------------------------------------------------------------------- /include/project11/gggs/cell_area_iterator.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_CELL_AREA_ITERATOR_H 2 | #define PROJECT11_GGGS_CELL_AREA_ITERATOR_H 3 | 4 | #include "cell_index.h" 5 | 6 | namespace gggs 7 | { 8 | 9 | /// @brief Iterates over rectangular area of CellIndexes within a grid at given GridIndex. 10 | class CellAreaIterator 11 | { 12 | public: 13 | CellAreaIterator(GridIndex grid) 14 | { 15 | from_ = CellIndex(grid, 0, 0); 16 | to_ = CellIndex(grid, cell_rows_per_grid-1, cell_columns_per_grid-1); 17 | current_ = from_; 18 | } 19 | 20 | /// Indicies are inclusive, so "to" is included and not considered past the array like std containers end(). 21 | CellAreaIterator(GridIndex grid, const gz4d::BoundsDegrees& bounds) 22 | { 23 | CellIndex from(grid, bounds.minimum()); 24 | CellIndex to(grid, bounds.maximum()); 25 | 26 | auto from_row = from.row(); 27 | if(from.grid().row() < grid.row()) 28 | from_row = 0; 29 | if(from.grid().row() > grid.row()) 30 | from_row = cell_rows_per_grid; 31 | 32 | auto from_column = from.column(); 33 | if(from.grid().column() < grid.column()) 34 | from_column = 0; 35 | if(from.grid().column() > grid.column()) 36 | from_column = cell_columns_per_grid; 37 | 38 | from_ = CellIndex(grid, from_row, from_column); 39 | 40 | auto to_row = to.row(); 41 | if(to.grid().row() < grid.row()) 42 | to_row = cell_rows_per_grid; 43 | if(to.grid().row() > grid.row()) 44 | to_row = cell_rows_per_grid - 1; 45 | 46 | auto to_column = to.column(); 47 | if(to.grid().column() < grid.column()) 48 | to_column = cell_rows_per_grid; 49 | if(to.grid().column() > grid.column()) 50 | to_column = cell_columns_per_grid - 1; 51 | 52 | to_ = CellIndex(grid, to_row, to_column); 53 | 54 | if(to_row < from_row || to_column < from_column) 55 | from_ = CellIndex(grid); // invalid index 56 | 57 | current_ = from_; 58 | } 59 | 60 | const CellIndex& operator*() const 61 | { 62 | return current_; 63 | } 64 | 65 | const CellIndex* operator->() const 66 | { 67 | return ¤t_; 68 | } 69 | 70 | bool reset() 71 | { 72 | current_ = from_; 73 | return current_.valid(); 74 | } 75 | 76 | bool valid() const 77 | { 78 | return current_.valid(); 79 | } 80 | 81 | bool next() 82 | { 83 | if(valid()) 84 | { 85 | auto new_row = current_.row(); 86 | auto new_column = current_.column()+1; 87 | if(new_column > to_.column()) 88 | { 89 | new_row += 1; 90 | new_column = from_.column(); 91 | } 92 | if(new_row > to_.row()) 93 | current_ = CellIndex(); 94 | else 95 | current_ = CellIndex(current_.grid(), new_row, new_column); 96 | } 97 | return valid(); 98 | } 99 | private: 100 | CellIndex from_; 101 | CellIndex to_; 102 | CellIndex current_; 103 | }; 104 | 105 | } // namespace gggs 106 | 107 | #endif 108 | -------------------------------------------------------------------------------- /include/project11/gggs/cell_index.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_CELL_INDEX_H 2 | #define PROJECT11_GGGS_CELL_INDEX_H 3 | 4 | #include "level_spec.h" 5 | 6 | namespace gggs 7 | { 8 | /// Index of a cell within a grid 9 | class CellIndex 10 | { 11 | public: 12 | CellIndex(){} 13 | CellIndex(GridIndex grid):grid_index_(grid){} 14 | CellIndex(GridIndex grid, uint16_t row, uint16_t column): 15 | grid_index_(grid), row_(row), column_(column) 16 | { 17 | } 18 | 19 | // CellIndex(double latitude, double longitude, GridIndex grid) 20 | // { 21 | // initialize(latitude, longitude, grid); 22 | // } 23 | 24 | CellIndex(GridIndex grid, const gz4d::PositionDegrees &position): 25 | grid_index_(grid) 26 | { 27 | grid_index_ = grid; 28 | 29 | // Note, we constrain to 1.0, but what we really need is up to 1.0. 30 | double row_p = std::max(0.0, std::min(1.0, position.latitude-grid_index_.southLatitude())/grid_index_.latitudinalSpan()); 31 | // This std::min should filter out 1.0 from above 32 | row_ = std::min(cell_rows_per_grid-1, cell_rows_per_grid*row_p); 33 | 34 | double column_p = std::max(0.0, (position.longitude-grid_index_.westLongitude())/grid_index_.longitudinalSpan()); 35 | column_ = std::min(cell_columns_per_grid-1, cell_columns_per_grid*column_p); 36 | } 37 | 38 | bool valid() const 39 | { 40 | return grid_index_.valid() && row_ < cell_rows_per_grid && column_ < cell_columns_per_grid; 41 | } 42 | 43 | const GridIndex& grid() const 44 | { 45 | return grid_index_; 46 | } 47 | 48 | uint8_t level() const 49 | { 50 | return grid_index_.level(); 51 | } 52 | 53 | const uint16_t &row() const 54 | { 55 | return row_; 56 | } 57 | 58 | const uint16_t &column() const 59 | { 60 | return column_; 61 | } 62 | 63 | gz4d::PositionDegrees position() const 64 | { 65 | double row_p = row_/double(cell_rows_per_grid); 66 | double column_p = column_/double(cell_columns_per_grid); 67 | auto latitude = grid_index_.southLatitude()+row_p*grid_index_.latitudinalSpan(); 68 | auto longitude = grid_index_.westLongitude()+column_p*grid_index_.longitudinalSpan(); 69 | return gz4d::PositionDegrees(latitude, longitude); 70 | } 71 | 72 | /// Less than operator allowing use as std::map key. 73 | friend bool operator<(const CellIndex& lhs, const CellIndex& rhs) 74 | { 75 | if (lhs.grid_index_ != rhs.grid_index_) 76 | return lhs.grid_index_ < rhs.grid_index_; 77 | if (lhs.row_ != rhs.row_) 78 | return lhs.row_ < rhs.row_; 79 | return lhs.column_ < rhs.column_; 80 | } 81 | 82 | friend bool operator!=(const CellIndex& lhs, const CellIndex& rhs) 83 | { 84 | return !lhs.valid() || !rhs.valid() || lhs.grid_index_ != rhs.grid_index_ || 85 | lhs.row_ != rhs.row_ || lhs.column_ != rhs.column_; 86 | } 87 | 88 | friend bool operator==(const CellIndex& lhs, const CellIndex& rhs) 89 | { 90 | return !(lhs != rhs); 91 | } 92 | 93 | 94 | friend std::ostream& operator<< (std::ostream& stream, const CellIndex& index) 95 | { 96 | stream << index.grid_index_ << " CellIndex row: " << index.row_ << " col: " << index.column_; 97 | return stream; 98 | } 99 | 100 | private: 101 | GridIndex grid_index_; 102 | 103 | /// Positive integer row starting the bottom of the grid. 104 | uint16_t row_ = cell_rows_per_grid; 105 | 106 | /// Positive integer column starting at left edge of the grid. 107 | uint16_t column_ = cell_columns_per_grid; 108 | }; 109 | 110 | } 111 | 112 | #endif 113 | -------------------------------------------------------------------------------- /include/project11/gggs/core.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_CORE_H 2 | #define PROJECT11_GGGS_CORE_H 3 | 4 | #include 5 | #include 6 | //#include 7 | 8 | #include "project11/utils.h" 9 | 10 | // based on "A global geographic grid system for visualizing bathymetry" by Colin Ware et al. 11 | // https://gi.copernicus.org/articles/9/375/2020/ 12 | // 13 | // A system for indexing tiles in a quad tree using geographic coordinates. 14 | // At level 0, tiles are 8x8 degrees for most of the earth. Tiles at or above 15 | // 72 degrees of latitude cover 24 degrees of longitude while tiles at or 16 | // above 80 degrees of latitude cover 72 degrees of longitude. Similar 17 | // scale changes are also applied towards the south pole. 18 | 19 | namespace gggs 20 | { 21 | 22 | /// Default grid size compatible with GEBCO 2020 grid 23 | constexpr uint16_t cell_rows_per_grid = 960; 24 | constexpr uint16_t cell_columns_per_grid = 960; 25 | constexpr uint32_t grid_total_cell_count = cell_rows_per_grid*cell_columns_per_grid; 26 | 27 | constexpr double earth_radius_at_equator = project11::WGS84::specs::a; 28 | constexpr double equator_circumference = 2.0 * M_PI * earth_radius_at_equator; 29 | 30 | /// approximate size in meters of an 8 degree area around the equator 31 | constexpr double level_0_grid_size = equator_circumference * 8.0 / 360.0; 32 | 33 | constexpr uint32_t level_0_row_count = 24; // (96+96)/8; 34 | constexpr uint32_t level_0_column_count = 45; // 360/8; 35 | 36 | /// Calculate grid width factor to account for longitudes 37 | /// bunching up near the poles. 38 | inline static uint8_t latitudeScaleFactor(double latitude) 39 | { 40 | if (latitude < 72.0 || latitude > -72.0) 41 | return 1; 42 | if (latitude < 80.0 || latitude > -80.0) 43 | return 3; 44 | return 9; 45 | } 46 | 47 | template 48 | void verifySameLevel(const T1& a, const T2& b) 49 | { 50 | if(a.level() != b.level()) 51 | { 52 | std::stringstream error_message; 53 | error_message << "Level mismatch between " << a << " and " << b; 54 | throw(std::out_of_range(error_message.str())); 55 | } 56 | } 57 | 58 | 59 | } // namespace gggs 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /include/project11/gggs/grid_area_iterator.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_GRID_AREA_ITERATOR_H 2 | #define PROJECT11_GGGS_GRID_AREA_ITERATOR_H 3 | 4 | #include "grid_index.h" 5 | 6 | namespace gggs 7 | { 8 | 9 | /// Iterates over a rectangular area of GridIndexs. 10 | class GridAreaIterator 11 | { 12 | public: 13 | GridAreaIterator(GridIndex from, GridIndex to): 14 | from_(from), to_(to), current_(from) 15 | { 16 | verifySameLevel(from, to); 17 | } 18 | 19 | const GridIndex& operator*() const 20 | { 21 | return current_; 22 | } 23 | 24 | const GridIndex* operator->() const 25 | { 26 | return ¤t_; 27 | } 28 | 29 | bool reset() 30 | { 31 | current_ = from_; 32 | return valid(); 33 | } 34 | 35 | bool valid() const 36 | { 37 | if(current_.valid()) 38 | { 39 | return 40 | current_.row() >= from_.row() && 41 | current_.column() >= from_.column() && 42 | current_.row() <= to_.row() && 43 | current_.column() <= to_.column(); 44 | } 45 | return false; 46 | } 47 | 48 | bool next() 49 | { 50 | if(valid()) 51 | { 52 | auto new_row = current_.row(); 53 | auto new_column = current_.column()+1; 54 | if(new_column > to_.column()) 55 | { 56 | new_row += 1; 57 | new_column = from_.column(); 58 | } 59 | if(new_row > to_.row()) 60 | current_ = GridIndex(); 61 | else 62 | current_ = GridIndex(current_.level_, new_row, new_column); 63 | } 64 | return valid(); 65 | } 66 | 67 | private: 68 | GridIndex from_; 69 | GridIndex to_; 70 | GridIndex current_; 71 | }; 72 | 73 | } // namespace gggs 74 | 75 | #endif 76 | -------------------------------------------------------------------------------- /include/project11/gggs/grid_index.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_GRID_INDEX_H 2 | #define PROJECT11_GGGS_GRID_INDEX_H 3 | 4 | #include "level_spec.h" 5 | 6 | namespace gggs 7 | { 8 | // Index for a Global Geographic Grid System grid. 9 | class GridIndex 10 | { 11 | public: 12 | GridIndex(){} 13 | 14 | // GridIndex(double latitude, double longitude, uint8_t level) : level_(level) 15 | // { 16 | // row_ = (latitude + 96.0) / levels[level].grid_angular_span; 17 | // column_ = (longitude + 180.0) / (levels[level].grid_angular_span * latitudeScaleFactor(latitude)); 18 | // } 19 | 20 | bool valid() const 21 | { 22 | return level_ < levels.size() && row_ < levels[level_].row_count && column_ < levels[level_].columnCount(row_); 23 | } 24 | 25 | uint8_t level() const 26 | { 27 | return level_; 28 | } 29 | 30 | const uint32_t &row() const 31 | { 32 | return row_; 33 | } 34 | 35 | const uint32_t &column() const 36 | { 37 | return column_; 38 | } 39 | 40 | static uint16_t cellRowCount() 41 | { 42 | return cell_rows_per_grid; 43 | } 44 | 45 | static uint16_t cellColumnCount() 46 | { 47 | return cell_columns_per_grid; 48 | } 49 | 50 | double northLatitude() const 51 | { 52 | return std::max(-90.0, -96.0+(row_+1)*levels[level_].grid_angular_span); 53 | } 54 | 55 | double southLatitude() const 56 | { 57 | return std::max(-90.0, -96.0+row_*levels[level_].grid_angular_span); 58 | } 59 | 60 | double eastLongitude() const 61 | { 62 | return -180.0+(column_+1)*levels[level_].gridLongitudinalSpan(row_); 63 | } 64 | 65 | double westLongitude() const 66 | { 67 | return -180.0+column_*levels[level_].gridLongitudinalSpan(row_); 68 | } 69 | 70 | gz4d::PositionDegrees southWestPosition() const 71 | { 72 | return gz4d::PositionDegrees(southLatitude(), westLongitude()); 73 | } 74 | 75 | gz4d::PositionDegrees northEastPosition() const 76 | { 77 | return gz4d::PositionDegrees(northLatitude(), eastLongitude()); 78 | } 79 | 80 | double latitudinalSpan() const 81 | { 82 | return levels[level_].grid_angular_span; 83 | } 84 | 85 | double longitudinalSpan() const 86 | { 87 | return levels[level_].gridLongitudinalSpan(row_); 88 | } 89 | 90 | /// Less than operator allowing use as std::map key. 91 | friend bool operator<(const GridIndex& lhs, const GridIndex& rhs) 92 | { 93 | if (lhs.level_ != rhs.level_) 94 | return lhs.level_ < rhs.level_; 95 | if (lhs.row_ != rhs.row_) 96 | return lhs.row_ < rhs.row_; 97 | return lhs.column_ < rhs.column_; 98 | } 99 | 100 | friend bool operator!=(const GridIndex& lhs, const GridIndex& rhs) 101 | { 102 | return !lhs.valid() || !rhs.valid() || lhs.level_ != rhs.level_ || 103 | lhs.row_ != rhs.row_ || lhs.column_ != rhs.column_; 104 | } 105 | 106 | friend bool operator==(const GridIndex& lhs, const GridIndex& rhs) 107 | { 108 | return !(lhs != rhs); 109 | } 110 | 111 | friend GridIndex max(const GridIndex& lhs, const GridIndex& rhs) 112 | { 113 | verifySameLevel(lhs, rhs); 114 | return GridIndex(lhs.level_, std::max(lhs.row_, rhs.row_), std::max(lhs.column_, rhs.column_)); 115 | } 116 | 117 | friend GridIndex min(const GridIndex& lhs, const GridIndex& rhs) 118 | { 119 | verifySameLevel(lhs, rhs); 120 | return GridIndex(lhs.level_, std::min(lhs.row_, rhs.row_), std::min(lhs.column_, rhs.column_)); 121 | } 122 | 123 | 124 | friend std::ostream& operator<< (std::ostream& stream, const GridIndex& index) 125 | { 126 | stream << "GridIndex level " << int(index.level_) << " row: " << index.row_ << " col: " << index.column_; 127 | return stream; 128 | } 129 | 130 | private: 131 | GridIndex(uint8_t level, uint32_t row, uint32_t column): 132 | level_(level), row_(row), column_(column) 133 | {} 134 | 135 | friend class Level; 136 | friend class GridAreaIterator; 137 | 138 | /// Level of the quad tree where 0 is top with 8 degree tiles 139 | uint8_t level_ = 255; 140 | 141 | /// Positive integer row starting -96 degrees. 142 | /// The -96 degrees starting value accounts for rows by the poles only being 2 degrees. 143 | uint32_t row_ = 0; 144 | 145 | /// Positive integer column starting at -180 degrees. 146 | uint32_t column_ = 0; 147 | }; 148 | 149 | 150 | } 151 | 152 | #endif 153 | -------------------------------------------------------------------------------- /include/project11/gggs/level.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_LEVEL_H 2 | #define PROJECT11_GGGS_LEVEL_H 3 | 4 | #include "cell_index.h" 5 | 6 | namespace gggs 7 | { 8 | 9 | /// Represents a level in the quadtree where level 0 is 8 degrees square 10 | /// (except close to the poles). 11 | class Level 12 | { 13 | public: 14 | Level(uint8_t level):level_(level){} 15 | 16 | /// Constructs an instance where the level supports requested cell_size 17 | /// or smaller 18 | static Level fromCellSize(float cell_size) 19 | { 20 | auto grid_size = cell_size * cell_rows_per_grid; 21 | return Level(std::ceil(std::log2(level_0_grid_size / grid_size))); 22 | } 23 | 24 | /// Approximate cell size in meters 25 | double cellSize() const 26 | { 27 | return levels[level_].nominal_cell_size; 28 | } 29 | 30 | GridIndex gridIndex(double latitude, double longitude) const 31 | { 32 | uint32_t row = (latitude + 96.0) / levels[level_].grid_angular_span; 33 | uint32_t column = (longitude + 180.0) / (levels[level_].grid_angular_span * latitudeScaleFactor(latitude)); 34 | 35 | return GridIndex(level_, row, column); 36 | } 37 | 38 | GridIndex gridIndex(const gz4d::PositionDegrees & position) const 39 | { 40 | return gridIndex(position.latitude, position.longitude); 41 | } 42 | 43 | CellIndex cellIndex(const gz4d::PositionDegrees & position) const 44 | { 45 | return CellIndex(gridIndex(position), position); 46 | } 47 | 48 | /// @brief Angular span of a cell 49 | /// @return Degrees (in latitude direction, not accounting for polar scales) 50 | double cellAngularSpan() const 51 | { 52 | return levels[level_].cell_angular_span; 53 | } 54 | 55 | protected: 56 | /// Level of the quad tree where 0 is top with 8 degree tiles 57 | uint8_t level_ = 0; 58 | }; 59 | 60 | } 61 | 62 | #endif 63 | 64 | -------------------------------------------------------------------------------- /include/project11/gggs/level_spec.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_GGGS_LEVEL_SPEC_H 2 | #define PROJECT11_GGGS_LEVEL_SPEC_H 3 | 4 | #include "core.h" 5 | 6 | namespace gggs 7 | { 8 | /// Metadata for a given level in the quadtree 9 | class LevelSpecs 10 | { 11 | public: 12 | LevelSpecs(uint8_t level):level(level) 13 | { 14 | auto multiplier = pow(2, level); 15 | row_count = level_0_row_count*multiplier; 16 | column_count = level_0_column_count*multiplier; 17 | 18 | double shrink_factor = 1.0/multiplier; 19 | grid_angular_span = 8.0*shrink_factor; 20 | cell_angular_span = grid_angular_span / cell_rows_per_grid; 21 | nominal_grid_size = level_0_grid_size*shrink_factor; 22 | nominal_cell_size = nominal_grid_size / cell_rows_per_grid; 23 | 24 | row_minus_80 = (-80.0+96.0)/grid_angular_span; 25 | row_minus_72 = (-72.0+96.0)/grid_angular_span; 26 | row_plus_72 = (72.0+96.0)/grid_angular_span; 27 | row_plus_80 = (80.0+96.0)/grid_angular_span; 28 | } 29 | 30 | uint8_t latitudeScaleFactor(uint32_t row) const 31 | { 32 | if(row >= row_minus_72 && row < row_plus_72) 33 | return 1; 34 | if(row >= row_minus_80 && row < row_plus_80) 35 | return 3; 36 | return 9; 37 | } 38 | 39 | double gridLongitudinalSpan(uint32_t row) const 40 | { 41 | return grid_angular_span*latitudeScaleFactor(row); 42 | } 43 | 44 | uint32_t columnCount(uint32_t row) const 45 | { 46 | return column_count/latitudeScaleFactor(row); 47 | } 48 | 49 | uint8_t level; 50 | 51 | // How many degrees a grid spans at this level 52 | double grid_angular_span; 53 | 54 | // How many degrees a grid cell spans at this level 55 | double cell_angular_span; 56 | 57 | // Estimate of the size in meters of a grid at this level 58 | double nominal_grid_size; 59 | // Estimate of the size in meters of a grid cell at this level 60 | double nominal_cell_size; 61 | 62 | uint32_t row_count; 63 | uint32_t column_count; 64 | 65 | uint32_t row_minus_80; 66 | uint32_t row_minus_72; 67 | uint32_t row_plus_72; 68 | uint32_t row_plus_80; 69 | }; 70 | 71 | // cache the metadata speed up index calculations 72 | inline std::array levels = {{ 73 | LevelSpecs(0), LevelSpecs(1), LevelSpecs(2), LevelSpecs(3), LevelSpecs(4), LevelSpecs(5), 74 | LevelSpecs(6), LevelSpecs(7), LevelSpecs(8), LevelSpecs(9), LevelSpecs(10), LevelSpecs(11), 75 | LevelSpecs(12), LevelSpecs(13), LevelSpecs(14), LevelSpecs(15), LevelSpecs(16), LevelSpecs(17), 76 | LevelSpecs(18), LevelSpecs(19), LevelSpecs(20), 77 | }}; 78 | 79 | } // namespace gggs 80 | 81 | #endif 82 | 83 | -------------------------------------------------------------------------------- /include/project11/gz4d_geo.h: -------------------------------------------------------------------------------- 1 | #ifndef GZ4D_GEO_H 2 | #define GZ4D_GEO_H 3 | 4 | // Roland Arsenault 5 | // Center for Coastal and Ocean Mapping 6 | // University of New Hampshire 7 | // Copyright 2017, All rights reserved. 8 | // 9 | // Condensed from libgz4d. 10 | 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | 18 | namespace gz4d 19 | { 20 | /// Weighted interpolation between two values. 21 | /// @param start First value. 22 | /// @param end Second value. 23 | /// @param p Proportion of b relative to a. 24 | /// @return Weighted interpolated value. 25 | template inline T interpolate(T const &start, T const &end, double p) 26 | { 27 | return (1.0-p)*start + p*end; 28 | } 29 | 30 | /// Interpolates two angles, using the shorter distance between them. 31 | /// With a weight of 0, degree1 is essentialy returned. degree2 is returned when weight is 1. 32 | /// A weight of 0.5 returns the average of the two angles, or the mid point between them. 33 | /// @param a First angle. 34 | /// @param b Second angle. 35 | /// @param p Proportion of b relative to a. 36 | /// @return Weighted interpolated value. 37 | template inline T InterpolateDegrees(T a, T b, T p = .5) 38 | { 39 | while(a < b-180.0) 40 | a+=360.0; 41 | while(a > b+180.0) 42 | a-=360.0; 43 | 44 | return interpolate(a,b,p); 45 | } 46 | 47 | template inline double ratio(T const &a, T const &b) {return a/b;} 48 | 49 | template inline bool IsEven(T i) 50 | { 51 | return !(i%2); 52 | } 53 | 54 | /// Used by std::shared_ptr's to hold pointers it shouldn't auto-delete. 55 | struct NullDeleter 56 | { 57 | void operator()(void const *) const {} 58 | }; 59 | 60 | template inline T Radians(T degrees) {return degrees*0.01745329251994329577;} 61 | template inline T Degrees(T radians) {return radians*57.2957795130823208768;} 62 | 63 | /// Use with lexical_cast to convert hex string to integer type. 64 | /// From: http://stackoverflow.com/questions/1070497/c-convert-hex-string-to-signed-integer 65 | /// Example: uint32_t value = boost::lexical_cast >("0x2a"); 66 | template 67 | struct HexTo { 68 | ElemT value; 69 | operator ElemT() const {return value;} 70 | friend std::istream& operator>>(std::istream& in, HexTo& out) { 71 | in >> std::hex >> out.value; 72 | return in; 73 | } 74 | }; 75 | 76 | 77 | /// Base type for n-dimensional vectors. 78 | /// \ingroup base 79 | template 80 | class Vector 81 | { 82 | public: 83 | static const std::size_t _size = N; 84 | protected: 85 | T values[N]; 86 | public: 87 | Vector(); 88 | Vector(Vector const &v); 89 | template Vector(Vector const &v); 90 | template Vector(Vector const &v, std::size_t i); 91 | explicit Vector(T val); 92 | Vector(T v1, T v2); 93 | Vector(T v1, T v2, T v3); 94 | Vector(T v1, T v2, T v3, T v4); 95 | Vector(const T v[N]); 96 | 97 | template Vector(const std::pair &iterators) 98 | { 99 | T* vp = values; 100 | for(VI v = iterators.first; v != iterators.second; ++v) 101 | *vp++ = *v; 102 | } 103 | 104 | Vector &operator=(Vector const &rvalue); 105 | 106 | bool operator==(Vector const &rvalue) const; 107 | bool operator!=(Vector const &rvalue) const {return !(*this == rvalue);} 108 | 109 | Vector const &operator+=(Vector const &rvalue); 110 | Vector const &operator+=(T rvalue); 111 | Vector const &operator-=(Vector const &rvalue); 112 | Vector const &operator-=(T rvalue); 113 | Vector const &operator*=(Vector const &rvalue); 114 | Vector const &operator*=(T rvalue); 115 | Vector const &operator/=(Vector const &rvalue); 116 | Vector const &operator/=(T rvalue); 117 | 118 | Vector operator-() const; 119 | 120 | template Vector operator+(RT const &rvalue) const {return Vector(*this) += rvalue;} 121 | template Vector operator-(RT const &rvalue) const {return Vector(*this) -= rvalue;} 122 | template Vector operator*(RT const &rvalue) const {return Vector(*this) *= rvalue;} 123 | template Vector operator/(RT const &rvalue) const {return Vector(*this) /= rvalue;} 124 | 125 | /// Dot product 126 | /// theta = acos((a.b)/(|a||b|)) where |a| means norm(a) 127 | T dot(Vector const &rvalue) const; 128 | 129 | /// Returns length(1D), area(2D), volume(3D) or higher dim equivalent. 130 | T volume() const 131 | { 132 | T ret = values[0]; 133 | for(int i = 1; i < N; ++i) 134 | ret *= values[i]; 135 | return ret; 136 | } 137 | 138 | T &operator[](std::size_t index){return values[index];} 139 | T const &operator[](std::size_t index) const{return values[index];} 140 | 141 | T &front() {return *values;} 142 | T const &front() const {return *values;} 143 | 144 | static std::size_t size() {return N;} 145 | 146 | typedef T value_type; 147 | }; 148 | 149 | template inline Vector::Vector() 150 | { 151 | for(std::size_t i = 0; i < N; ++i) 152 | values[i] = 0; 153 | } 154 | 155 | template inline Vector::Vector(Vector const &v) 156 | { 157 | //for(std::size_t i = 0; i < N; ++i) 158 | //values[i] = v[i]; 159 | memcpy(&values,&v.values,sizeof(T)*N); 160 | } 161 | 162 | template template inline Vector::Vector(Vector const &v) 163 | { 164 | for(std::size_t i = 0; i < N; ++i) 165 | values[i] = v[i]; 166 | } 167 | 168 | template template inline Vector::Vector(Vector const &v, std::size_t offset) 169 | { 170 | for(std::size_t i = 0; i < N && i < ON+offset; ++i) 171 | values[i] = v[i+offset]; 172 | for(std::size_t i = ON+offset; i < N; ++i) 173 | values[i] = 0; 174 | } 175 | 176 | template inline Vector::Vector(T val) 177 | { 178 | for(std::size_t i = 0; i < N; ++i) 179 | values[i] = val; 180 | } 181 | 182 | template inline Vector::Vector(T v1, T v2) 183 | { 184 | BOOST_STATIC_ASSERT(N == 2); 185 | values[0] = v1; 186 | values[1] = v2; 187 | } 188 | 189 | template inline Vector::Vector(T v1, T v2, T v3) 190 | { 191 | BOOST_STATIC_ASSERT(N == 3); 192 | values[0] = v1; 193 | values[1] = v2; 194 | values[2] = v3; 195 | } 196 | 197 | template inline Vector::Vector(T v1, T v2, T v3, T v4) 198 | { 199 | BOOST_STATIC_ASSERT(N == 4); 200 | values[0] = v1; 201 | values[1] = v2; 202 | values[2] = v3; 203 | values[3] = v4; 204 | } 205 | 206 | template inline Vector::Vector(const T v[N]) 207 | { 208 | for (std::size_t i = 0; i < N; i++) 209 | values[i] = v[i]; 210 | } 211 | 212 | template inline Vector &Vector::operator=(Vector const &rvalue) 213 | { 214 | for(std::size_t i = 0; i < N; ++i) 215 | values[i] = rvalue[i]; 216 | return *this; 217 | } 218 | 219 | template inline bool Vector::operator==(Vector const &rvalue) const 220 | { 221 | bool ret=true; 222 | for(std::size_t i = 0; ret && i < N; ++i) 223 | ret = values[i] == rvalue[i]; 224 | return ret; 225 | } 226 | 227 | 228 | template inline Vector const &Vector::operator+=(Vector const &rvalue) 229 | { 230 | for(std::size_t i = 0; i < N; ++i) 231 | values[i] += rvalue[i]; 232 | return *this; 233 | } 234 | 235 | template inline Vector const &Vector::operator+=(T rvalue) 236 | { 237 | for(std::size_t i = 0; i < N; ++i) 238 | values[i] += rvalue; 239 | return *this; 240 | } 241 | 242 | template inline Vector const &Vector::operator-=(Vector const &rvalue) 243 | { 244 | for(std::size_t i = 0; i < N; ++i) 245 | values[i] -= rvalue[i]; 246 | return *this; 247 | } 248 | 249 | template inline Vector const &Vector::operator-=(T rvalue) 250 | { 251 | for(std::size_t i = 0; i < N; ++i) 252 | values[i] -= rvalue; 253 | return *this; 254 | } 255 | 256 | template inline Vector const &Vector::operator*=(Vector const &rvalue) 257 | { 258 | for(std::size_t i = 0; i < N; ++i) 259 | values[i] *= rvalue[i]; 260 | return *this; 261 | } 262 | 263 | template inline Vector const &Vector::operator*=(T rvalue) 264 | { 265 | for(std::size_t i = 0; i < N; ++i) 266 | values[i] *= rvalue; 267 | return *this; 268 | } 269 | 270 | 271 | template inline Vector const &Vector::operator/=(Vector const &rvalue) 272 | { 273 | for(std::size_t i = 0; i < N; ++i) 274 | values[i] /= rvalue[i]; 275 | return *this; 276 | } 277 | 278 | template inline Vector const &Vector::operator/=(T rvalue) 279 | { 280 | for(std::size_t i = 0; i < N; ++i) 281 | values[i] /= rvalue; 282 | return *this; 283 | } 284 | 285 | template inline Vector Vector::operator-() const 286 | { 287 | Vector ret; 288 | for(std::size_t i = 0; i < N; ++i) 289 | ret.values[i] = -values[i]; 290 | return ret; 291 | } 292 | 293 | template inline T Vector::dot(Vector const &rvalue) const 294 | { 295 | T sum = 0; 296 | 297 | for(std::size_t i = 0; i < N; ++i) 298 | sum += values[i] * rvalue[i]; 299 | 300 | return sum; 301 | } 302 | 303 | 304 | template class Point : public Vector 305 | { 306 | public: 307 | Point():Vector(0.0){} 308 | Point(Vector const &v):Vector(v){} 309 | Point(Vector const &v):Vector(v[0]/v[3],v[1]/v[3],v[2]/v[3]){} 310 | Point(T x, T y, T z):Vector(x, y, z){} 311 | using Vector::operator=; 312 | static Point Invalid(){return Vector(std::nan(""));} 313 | bool IsValid() const {return !(std::isnan(Vector::values[0])||std::isnan(Vector::values[1])||std::isnan(Vector::values[2]));} 314 | operator Vector() const {return Vector(Vector::values[0],Vector::values[1],Vector::values[2],1);} 315 | }; 316 | 317 | 318 | 319 | template class ValueScaler 320 | { 321 | double scale; 322 | double offset; 323 | public: 324 | ValueScaler():scale(1.0),offset(0.0){} 325 | ValueScaler(ValueScaler const &v):scale(v.scale),offset(v.offset){} 326 | ValueScaler(double s, double o):scale(s),offset(o){} 327 | vType Value(rType const &r) const {return r*scale+offset;} 328 | rType Representation(vType const &v) const {return (v-offset)/scale;} 329 | }; 330 | 331 | 332 | template class Interval 333 | { 334 | T start; 335 | T end; 336 | public: 337 | Interval():start(0.0),end(1.0){} 338 | Interval(Interval const &i):start(i.start),end(i.end){} 339 | Interval(T s, T e):start(s),end(e){} 340 | 341 | T GetRange() const {return end-start;} 342 | T GetStart() const {return start;} 343 | T GetEnd() const {return end;} 344 | T Map(double p) const {return start+p*GetRange();} 345 | }; 346 | 347 | template class Box 348 | { 349 | public: 350 | typedef T value_type; 351 | private: 352 | value_type min_; 353 | value_type max_; 354 | public: 355 | Box(Box const &b):min_(b.min_),max_(b.max_){} 356 | Box(){min_ +=1;} 357 | //:min(T(typename T::value_type(1))),max(typename T::value_type(0)){} 358 | 359 | Box(T const &min, T const &max) 360 | :min_(min),max_(max){} 361 | 362 | bool operator!=(Box const &other) const 363 | { 364 | return min_ != other.min_ || max_ != other.max_; 365 | } 366 | 367 | bool operator==(Box const &other) const 368 | { 369 | return !((*this)==other); 370 | } 371 | 372 | T const &getMin() const {return min_;} 373 | T const &getMax() const {return max_;} 374 | 375 | void setMin(T const &m) {min_ = m;} 376 | void setMax(T const &m) {max_ = m;} 377 | 378 | bool empty() const 379 | { 380 | return std::isnan(min_[0]) || min_[0] > max_[0]; 381 | } 382 | 383 | T getCenter() const 384 | { 385 | if(empty()) 386 | return min_; 387 | T ret; 388 | for(std::size_t i = 0; i < T::size(); ++i) 389 | ret[i] = min_[i] + (max_[i] - min_[i]) *0.5; 390 | return ret; 391 | } 392 | 393 | T getSizes() const 394 | { 395 | //if(empty()) 396 | // throw(Exception("Empty box doesn't have a size")); 397 | 398 | T ret; 399 | for(std::size_t i = 0; i < T::size(); ++i) 400 | ret[i] = max_[i] - min_[i]; 401 | return ret; 402 | } 403 | 404 | typename T::value_type getMaxLength() const 405 | { 406 | //if(empty()) 407 | // throw(Exception("Empty box doesn't have a length")); 408 | 409 | typename T::value_type ret = max_[0]-min_[0]; 410 | for(std::size_t i = 1; i < T::size(); ++i) 411 | ret = std::max(ret,max_[i] - min_[i]); 412 | return ret; 413 | } 414 | 415 | typename T::value_type getMinLength() const 416 | { 417 | //if(empty()) 418 | // throw(Exception("Empty box doesn't have a length")); 419 | 420 | typename T::value_type ret = max_[0]-min_[0]; 421 | for(std::size_t i = 1; i < T::size(); ++i) 422 | ret = std::min(ret,max_[i] - min_[i]); 423 | return ret; 424 | } 425 | 426 | typename T::value_type getVolume() const 427 | { 428 | //if(empty()) 429 | // throw(Exception("Empty box doesn't have a volume")); 430 | return getSizes().volume(); 431 | } 432 | 433 | 434 | Box &expand(T const &p) 435 | { 436 | if(empty()) 437 | { 438 | min_ = p; 439 | max_ = p; 440 | } 441 | else 442 | for(std::size_t i = 0; i < T::size(); ++i) 443 | { 444 | if(p[i] < min_[i]) 445 | { 446 | if(p[i] > max_[i]) // for types that can wrap, such as Angles 447 | { 448 | if(min_[i]-p[i] max_[i]) 457 | max_[i] = p[i]; 458 | } 459 | return *this; 460 | } 461 | 462 | Box &expand(Box const &other) 463 | { 464 | expand(other.min_); 465 | expand(other.max_); 466 | } 467 | 468 | typename T::value_type distance(T const &p) const 469 | { 470 | if(contains(p)) 471 | return 0; 472 | typename T::value_type d2 = 0; 473 | for(std::size_t i = 0; i < T::size(); ++i) 474 | { 475 | if(p[i] > max_[i]) 476 | d2 += (p[i]-max_[i])*(p[i]-max_[i]); 477 | else if (p[i] < min_[i]) 478 | d2 += (min_[i]-p[i])*(min_[i]-p[i]); 479 | } 480 | return sqrt(d2); 481 | } 482 | 483 | template bool contains(OT const &p) const 484 | { 485 | assert(T::size() == OT::size()); 486 | if(empty()) 487 | return false; 488 | for(std::size_t i = 0; i < T::size(); ++i) 489 | if(p[i] < min_[i] || p[i] > max_[i]) 490 | return false; 491 | return true; 492 | } 493 | 494 | bool contains(Box const &other) const 495 | { 496 | if(other.empty()) 497 | return false; 498 | return contains(other.min_) && contains(other.max_); 499 | } 500 | 501 | bool intersects(Box const &other) const 502 | { 503 | for(std::size_t i = 0; i < T::size(); ++i) 504 | if(min_[i] > other.max_[i] || max_[i] < other.min_[i]) 505 | return false; 506 | return true; 507 | } 508 | 509 | void setSizesFromCenter(T const &s) 510 | { 511 | T c = getCenter(); 512 | min_ = c-s/2.0; 513 | max_ = c+s/2.0; 514 | } 515 | 516 | void setSizesFromMin(T const &s) 517 | { 518 | if(empty()) 519 | min_ = T(0); 520 | max_ = min_ + s; 521 | } 522 | 523 | Box operator&(Box const &o) const 524 | { 525 | if(intersects(o)) 526 | { 527 | T newMin, newMax; 528 | for(std::size_t i = 0; i < T::size(); ++i) 529 | { 530 | newMin[i] = std::max(min_[i],o.min_[i]); 531 | newMax[i] = std::min(max_[i],o.max_[i]); 532 | } 533 | return Box(newMin,newMax); 534 | } 535 | return Box(); 536 | } 537 | 538 | Box operator|(Box const &o) const 539 | { 540 | return Box(*this).expand(o); 541 | } 542 | 543 | Box &operator+=(const value_type &v) 544 | { 545 | min_ += v; 546 | max_ += v; 547 | return *this; 548 | } 549 | 550 | Box &operator-=(const value_type &v) 551 | { 552 | return this->operator+=(-v); 553 | } 554 | 555 | Box operator+(const value_type &v) const 556 | { 557 | return Box(*this)+=v; 558 | } 559 | 560 | Box operator-(const value_type &v) const 561 | { 562 | return Box(*this)-=v; 563 | } 564 | }; 565 | 566 | typedef Box< Vector > Box3f; 567 | typedef Box< Vector > Box3d; 568 | typedef Box< Vector > Box2f; 569 | typedef Box< Vector > Box2d; 570 | 571 | 572 | 573 | 574 | /// Column-major matrix with M rows and N columns. 575 | /// \ingroup base 576 | template class Matrix 577 | { 578 | Vector values; 579 | Matrix(Vector const &v):values(v){} 580 | public: 581 | Matrix(){} 582 | Matrix(Matrix const &m):values(m.values){} 583 | explicit Matrix(T val):values(val){} 584 | template Matrix(const std::pair &iterators):values(iterators){} 585 | template Matrix(Matrix const &cm, std::size_t row, std::size_t col); 586 | 587 | static Matrix Identity(); 588 | 589 | T const &operator()(std::size_t row, std::size_t col) const {return values[col*M+row];} 590 | T &operator()(std::size_t row, std::size_t col) {return values[col*M+row];} 591 | 592 | template Matrix operator*(Matrix const &rvalue) const; 593 | Vector operator*(Vector const &rvalue) const; 594 | 595 | T &front() {return values.front();} 596 | T const &front() const {return values.front();} 597 | 598 | Matrix operator-(Matrix const &o) const {return Matrix(values-o.values);} 599 | Matrix operator+(Matrix const &o) const {return Matrix(values+o.values);} 600 | 601 | Matrix operator-() const {return Matrix(-values);} 602 | 603 | /// Element-wise arithmetic 604 | Matrix &operator*=(T value) { values*=value; return *this;} 605 | Matrix operator*(T value) const { return Matrix(*this)*=value;} 606 | 607 | Matrix &operator/=(T value) { values/=value; return *this;} 608 | Matrix operator/(T value) const { return Matrix(*this)/=value;} 609 | 610 | Matrix &operator+=(T value) { values+=value; return *this;} 611 | Matrix operator+(T value) const { return Matrix(*this)+=value;} 612 | 613 | Matrix &operator-=(T value) { values-=value; return *this;} 614 | Matrix operator-(T value) const { return Matrix(*this)-=value;} 615 | 616 | bool operator!=(Matrix const &o) const {return values != o.values;} 617 | bool operator==(Matrix const &o) const {return !(*this != o);} 618 | }; 619 | 620 | template inline Matrix Matrix::Identity() 621 | { 622 | Matrix ret; 623 | for(std::size_t i = 0; i < N && i < M; ++i) 624 | ret.values[i*M+i] = 1; 625 | return ret; 626 | } 627 | 628 | template template inline Matrix Matrix::operator*(Matrix const &rvalue) const 629 | { 630 | Matrix ret; 631 | for(std::size_t r = 0; r < M; ++r) 632 | for(std::size_t c = 0; c < P; ++c) 633 | for(std::size_t i = 0; i < N; ++i) 634 | ret(r,c) += operator()(r,i)*rvalue(i,c); 635 | return ret; 636 | } 637 | 638 | template inline Vector Matrix::operator*(Vector const &rvalue) const 639 | { 640 | Vector ret; 641 | for(std::size_t r = 0; r < M; ++r) 642 | for(std::size_t c = 0; c < N; ++c) 643 | ret[r] += operator()(r,c)*rvalue[c]; 644 | return ret; 645 | } 646 | 647 | template template inline Matrix::Matrix(Matrix const &cm, std::size_t row, std::size_t col) 648 | { 649 | for(std::size_t r = 0; r < M; ++r) 650 | for(std::size_t c = 0; c < N; ++c) 651 | values[c*M+r] = cm(row+r,col+c); 652 | } 653 | 654 | template static Matrix Frustum(T l, T r, T b, T t, T n, T f=std::numeric_limits::infinity()) 655 | { 656 | Matrix ret(0); 657 | T w = r-l; 658 | T h = t - b; 659 | ret(0,0) = 2.0*n/w; 660 | ret(0,2) = (r+l)/w; 661 | ret(1,1) = 2.0*n/h; 662 | ret(1,2) = (t+b)/h; 663 | ret(2,2) = -(f+n)/(f-n); 664 | //ret(2,2) = -1.0; 665 | ret(2,3) = -2.0*f*n/(f-n); 666 | //ret(2,3) = -2.0*n; 667 | ret(3,2) = -1.0; 668 | return ret; 669 | } 670 | 671 | template Matrix transpose(Matrix const &m) 672 | { 673 | Matrix ret; 674 | for(std::size_t r = 0; r < M; ++r) 675 | for(std::size_t c = 0; c < N; ++c) 676 | ret(c,r) = m(r,c); 677 | return ret; 678 | } 679 | 680 | template inline Matrix inverse(Matrix const &m) 681 | { 682 | return Matrix(1.0/m(0,0)); 683 | } 684 | 685 | template inline T determinant(Matrix const &m) 686 | { 687 | return m(0,0)*m(1,1)-m(0,1)*m(1,0); 688 | } 689 | 690 | template inline T determinant(Matrix const &m) 691 | { 692 | return m(0,0)*m(1,1)*m(2,2)+m(0,1)*m(1,2)*m(2,0)+m(0,2)*m(1,0)*m(2,1)-m(0,0)*m(1,2)*m(2,1)-m(0,1)*m(1,0)*m(2,2)-m(0,2)*m(1,1)*m(2,0); 693 | } 694 | 695 | template inline T determinant(Matrix const &m) 696 | { 697 | T ret = 0; 698 | for(std::size_t i = 0; i < N; ++i) 699 | ret += m(i,0)*cofactor(m,i,0); 700 | return ret; 701 | } 702 | 703 | /// Calculates Cij of matrix m. 704 | /// http://en.wikipedia.org/wiki/Cofactor_(linear_algebra) 705 | /// Cij=(-1)^(i+j)*Mij 706 | /// Where Mij is determinant of the submatrix obtained by removing from m its i-th row and j-th column. 707 | template inline T cofactor(Matrix const &m, std::size_t i, std::size_t j) 708 | { 709 | Matrix minor_matrix; 710 | std::size_t mi = 0; 711 | for(std::size_t in_row = 0; in_row < N && mi < N-1; ++in_row) 712 | { 713 | std::size_t mj = 0; 714 | for(std::size_t in_col = 0; in_col < N && mj < N-1; ++in_col) 715 | { 716 | minor_matrix(mi,mj)=m(in_row,in_col); 717 | if(in_col != j) 718 | ++mj; 719 | } 720 | if (in_row != i) 721 | ++mi; 722 | } 723 | T ret = determinant(minor_matrix); 724 | if((i+j)%2==1) 725 | return -ret; 726 | return ret; 727 | } 728 | 729 | /// Transpose of the cofactor matrix. 730 | /// http://en.wikipedia.org/wiki/Adjugate_matrix 731 | template inline Matrix adjugate(Matrix const &m) 732 | { 733 | Matrix ret; 734 | for(std::size_t i = 0; i < N; ++i) 735 | for(std::size_t j = 0; j < N; ++j) 736 | ret(i,j)=cofactor(m,j,i); // i and j reversed so we get transpose of cofactor matrix 737 | return ret; 738 | } 739 | 740 | 741 | template inline Matrix inverse(Matrix const &m) 742 | { 743 | Matrix ret; 744 | T a = m(0,0); 745 | T b = m(0,1); 746 | T c = m(1,0); 747 | T d = m(1,1); 748 | ret(0,0) = d/(a*d-b*c); 749 | ret(0,1) = -b/(a*d-b*c); 750 | ret(1,0) = -c/(a*d-b*c); 751 | ret(1,1) = a/(a*d-b*c); 752 | return ret; 753 | } 754 | 755 | /// Invert a matrix using Cramer's rule. 756 | /// http://en.wikipedia.org/wiki/Invertible_matrix 757 | /// inverse of A is: 758 | /// (1/det(A))*transpose(C) 759 | /// where C is matrix of cofactors. 760 | template inline Matrix inverse_cramer(Matrix const &m) 761 | { 762 | return adjugate(m)/determinant(m); 763 | } 764 | 765 | template inline Matrix inverse(Matrix const &m) 766 | { 767 | // Attempt Blockwise inversion first 768 | Matrix A(m,0,0); 769 | Matrix B(m,0,2); 770 | Matrix C(m,2,0); 771 | Matrix D(m,2,2); 772 | 773 | if(determinant(A) != 0.0) 774 | { 775 | 776 | Matrix A_inv = inverse(A); 777 | 778 | Matrix tmp = D-C*A_inv*B; 779 | if(determinant(tmp) != 0.0) 780 | { 781 | Matrix D_ret = inverse(tmp); 782 | 783 | Matrix A_ret = A_inv+A_inv*B*D_ret*C*A_inv; 784 | Matrix B_ret = -A_inv*B*D_ret; 785 | Matrix C_ret = -D_ret*C*A_inv; 786 | 787 | Matrix ret; 788 | for(std::size_t r = 0; r < 2; ++r) 789 | for(std::size_t c = 0; c < 2; ++c) 790 | { 791 | ret(r,c) = A_ret(r,c); 792 | ret(r,c+2) = B_ret(r,c); 793 | ret(r+2,c) = C_ret(r,c); 794 | ret(r+2,c+2) = D_ret(r,c); 795 | } 796 | return ret; 797 | } 798 | } 799 | 800 | // Blockwise didn't work, try Cramer's rule 801 | return inverse_cramer(m); 802 | } 803 | 804 | template class Translation: public Point 805 | { 806 | public: 807 | Translation(); 808 | Translation(Vector const & v); 809 | Translation(T x, T y, T z); 810 | Matrix GetMatrix() const; 811 | Matrix GetInverseMatrix() const; 812 | using Point::operator=; 813 | }; 814 | 815 | template inline Translation::Translation() 816 | {} 817 | 818 | template inline Translation::Translation(Vector const & v) 819 | :Point(v) 820 | {} 821 | 822 | template inline Translation::Translation(T x, T y, T z) 823 | :Point(x,y,z) 824 | {} 825 | 826 | template inline Matrix Translation::GetMatrix() const 827 | { 828 | Matrix ret = Matrix::Identity(); 829 | ret(0,3) = (*this)[0]; 830 | ret(1,3) = (*this)[1]; 831 | ret(2,3) = (*this)[2]; 832 | return ret; 833 | } 834 | 835 | template inline Matrix Translation::GetInverseMatrix() const 836 | { 837 | Matrix ret = Matrix::Identity(); 838 | ret(0,3) = -(*this)[0]; 839 | ret(1,3) = -(*this)[1]; 840 | ret(2,3) = -(*this)[2]; 841 | return ret; 842 | } 843 | 844 | 845 | ////////// Angles 846 | 847 | 848 | // period units: ranges for periodic types, such as angles 849 | namespace pu 850 | { 851 | struct Degree 852 | { 853 | static constexpr double period = 360.0; 854 | static constexpr double half_period = 180.0; 855 | // template static T period(){return 360.0;} 856 | // template static T half_period(){return 180.0;} 857 | }; 858 | 859 | struct Radian 860 | { 861 | static constexpr double half_period = M_PI; 862 | static constexpr double period = half_period*2.0; 863 | // template static T half_period(){return 3.14159265358979323846;} 864 | // template static T period(){return half_period()*2.0;} 865 | }; 866 | } 867 | 868 | // range types, where does a periodic value roll over 869 | namespace rt 870 | { 871 | // zero to period length 872 | struct PositivePeriod 873 | { 874 | template static T fix(T v) 875 | { 876 | if(v >= 0.0 && v < PU::period) 877 | return v; 878 | T ret = fmod(v,PU::period); 879 | if(ret < 0) 880 | ret += PU::period; 881 | return ret; 882 | } 883 | }; 884 | 885 | // minus half period to plus half period 886 | struct ZeroCenteredPeriod 887 | { 888 | template static T fix(T v) 889 | { 890 | T half_period = PU::half_period; 891 | if(v <= half_period && v > -half_period) 892 | return v; 893 | T period = PU::period; 894 | T ret = fmod(v,period); 895 | if(ret > half_period) 896 | return ret-period; 897 | if(ret < -half_period) 898 | return ret+period; 899 | return ret; 900 | } 901 | }; 902 | 903 | // unclamped, so doesn't roll over 904 | struct Unclamped 905 | { 906 | template static T fix(T v) 907 | { 908 | return v; 909 | } 910 | }; 911 | 912 | /// cover half a circle, such as latitudes 913 | /// Throws out_of_range if necessary 914 | struct ZeroCenteredHalfPeriodChecked 915 | { 916 | template static T fix(T v) 917 | { 918 | static constexpr T quarter_period = PU::half_period/2.0; 919 | if(v < -quarter_period || v > quarter_period) 920 | { 921 | std::stringstream ss; 922 | ss << "Angle " << v << " out of range (" << -quarter_period << " to " << quarter_period << ")"; 923 | throw(std::range_error(ss.str())); 924 | } 925 | return v; 926 | } 927 | }; 928 | } 929 | 930 | // Angle class aware of its units and range so it automatically rolls over 931 | // T is type of value 932 | // PU is period units, degrees, radians or other 933 | // RT is range type, such as zero centered, positive, or other 934 | template class Angle 935 | { 936 | T _value; 937 | 938 | public: 939 | typedef T value_type; 940 | typedef PU period_units; 941 | typedef RT range_type; 942 | 943 | Angle():_value(RT::template fix(0.0)){} 944 | explicit Angle(T v):_value(RT::template fix(v)){} 945 | Angle(Angle const &a):_value(a._value){} 946 | template Angle(Angle const &a):_value(RT::template fix(a.value())){} 947 | template Angle(Angle const &a):_value(RT::template fix(a.normalized()*PU::period)){} 948 | 949 | T normalized() const{return _value/PU::period;} 950 | 951 | operator T() const {return _value;} 952 | T value() const {return _value;} 953 | 954 | // period aware operators, if the other value is more than half a period away, consider wrapping 955 | //bool operator<(Angle o) const {return (o._value-_value > 0.0 && o._value-_value < PU::template half_period()) || o._value-_value < -PU::template half_period();} 956 | //bool operator==(Angle o) const {return _value==o._value;} 957 | template bool operator!=(Angle o) const {return ! *this==o;} 958 | template bool operator>(Angle o) const {return o<*this;} 959 | template bool operator<=(Angle o) const {return !*this>o;} 960 | template bool operator>=(Angle o) const {return !*this const &operator-=(Angle o) {_value = RT::template fix(_value-o._value); return *this;} 963 | Angle const &operator+=(Angle o) {_value = RT::template fix(_value+o._value); return *this;} 964 | Angle const &operator*=(T o) {_value = RT::template fix(_value*o); return *this;} 965 | Angle const &operator/=(T o) {_value = RT::template fix(_value/o); return *this;} 966 | 967 | Angle operator+(Angle o) const {return Angle(*this)+= o;} 968 | Angle operator-(Angle o) const {return Angle(*this)-= o;} 969 | Angle operator*(T o) const {return Angle(*this)*= o;} 970 | Angle operator/(T o) const {return Angle(*this)/= o;} 971 | 972 | Angle operator-() const {return Angle(-_value);} 973 | 974 | friend Angle min(const Angle& a1, const Angle& a2) 975 | { 976 | if(a1 < a2) 977 | return a1; 978 | return a2; 979 | } 980 | 981 | friend Angle max(const Angle& a1, const Angle& a2) 982 | { 983 | if(a1 > a2) 984 | return a1; 985 | return a2; 986 | } 987 | }; 988 | 989 | // period aware < operator, if the other value is more than half a period away, consider wrapping 990 | template bool operator<(Angle l, Angle r) 991 | { 992 | return (r.value()-l.value() > 0.0 && r.value()-l.value() < PU::half_period || r.value()-l.value() < -PU::half_period); 993 | } 994 | 995 | template bool operator<(Angle l, Angle r) 996 | { 997 | return l < Angle(r); 998 | } 999 | 1000 | template bool operator<(Angle l, Angle r) 1001 | { 1002 | return Angle(l) < r; 1003 | } 1004 | 1005 | template bool operator==(Angle l, Angle r) 1006 | { 1007 | return l._value==r._value; 1008 | } 1009 | 1010 | template bool operator==(Angle l, Angle r) 1011 | { 1012 | return l == Angle(r); 1013 | } 1014 | 1015 | template bool operator==(Angle l, Angle r) 1016 | { 1017 | return Angle(l) == r; 1018 | } 1019 | 1020 | template bool isnan(const Angle& angle) 1021 | { 1022 | return std::isnan(angle.value()); 1023 | } 1024 | 1025 | // interpolates period aware angles 1026 | // a and b are the input angles, p is proportion of a vs b to weight. 1027 | // 1028 | template Angle interpolate(Angle const &a, Angle const &b, double p=0.5) 1029 | { 1030 | Angle ret; 1031 | if(a inline T sin(Angle const &a) 1039 | { 1040 | return ::sin(a.value()); 1041 | } 1042 | 1043 | template inline T sin(Angle const &a) 1044 | { 1045 | return sin(Angle(a)); 1046 | } 1047 | 1048 | template inline T cos(Angle const &a) 1049 | { 1050 | return ::cos(a.value()); 1051 | } 1052 | 1053 | template inline T cos(Angle const &a) 1054 | { 1055 | return cos(Angle(a)); 1056 | } 1057 | 1058 | template inline T cos(T val) 1059 | { 1060 | return ::cos(val); 1061 | } 1062 | 1063 | template inline T sin(T val) 1064 | { 1065 | return ::sin(val); 1066 | } 1067 | 1068 | 1069 | //////////// geo 1070 | 1071 | namespace geo 1072 | { 1073 | 1074 | template 1075 | using LatitudeType = Angle; 1076 | 1077 | using LatitudeDegrees = LatitudeType; 1078 | using LatitudeRadians = LatitudeType; 1079 | 1080 | template 1081 | using LatitudeSpan = Angle; 1082 | 1083 | using LatitudeSpanDegrees = LatitudeSpan; 1084 | using LatitudeSpanRadians = LatitudeSpan; 1085 | 1086 | template 1087 | using LongitudeType = Angle; 1088 | 1089 | using LongitudeDegrees = LongitudeType; 1090 | using LongitudeRadians = LongitudeType; 1091 | 1092 | template 1093 | using LongitudeSpan = Angle; 1094 | 1095 | using LongitudeSpanDegrees = LongitudeSpan; 1096 | using LongitudeSpanRadians = LongitudeSpan; 1097 | 1098 | // coordinate formats, defines order of components 1099 | namespace cf 1100 | { 1101 | struct LatLon 1102 | { 1103 | enum Coordinates 1104 | { 1105 | Latitude = 0, Longitude = 1, Height = 2 1106 | }; 1107 | }; 1108 | 1109 | struct LonLat 1110 | { 1111 | enum Coordinates 1112 | { 1113 | Longitude = 0, Latitude = 1, Height = 2 1114 | }; 1115 | }; 1116 | 1117 | struct XYZ 1118 | { 1119 | enum Coordinates 1120 | { 1121 | X = 0, Y = 1, Z = 2 1122 | }; 1123 | }; 1124 | } 1125 | 1126 | 1127 | /// 3D geographic point 1128 | /// T is numeric type of components 1129 | /// RF is the reference frame 1130 | template class Point: public gz4d::Point 1131 | { 1132 | public: 1133 | 1134 | Point(){} 1135 | Point(T x, T y, T z=0):gz4d::Point(x,y,z){} 1136 | explicit Point(gz4d::Point const &op):gz4d::Point(op){} 1137 | //using gz4d::Point::operator=; 1138 | 1139 | //template explicit Point(Point const&op) 1140 | template Point(Point const&op) 1141 | { 1142 | *this = typename RF::coordinate_type()(op); 1143 | } 1144 | 1145 | T latitude() const {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Latitude);} 1146 | T longitude() const {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Longitude);}; 1147 | T altitude() const {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Height);}; 1148 | 1149 | T x() const {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::X);} 1150 | T y() const {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Y);} 1151 | T z() const {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Z);} 1152 | 1153 | T &latitude() {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Latitude);} 1154 | T &longitude() {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Longitude);}; 1155 | T &altitude() {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Height);}; 1156 | 1157 | T &x() {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::X);} 1158 | T &y() {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Y);} 1159 | T &z() {return gz4d::Point::operator[](RF::coordinate_type::coordinate_format::Z);} 1160 | 1161 | }; 1162 | 1163 | // define a reference frame 1164 | // CT is coordinate type 1165 | // ET is ellipsoid type 1166 | template struct ReferenceFrame 1167 | { 1168 | typedef CT coordinate_type; 1169 | typedef ET ellipsoid_type; 1170 | }; 1171 | 1172 | // coordinate types, such as cartesian, geodetic (lat/long), etc. 1173 | namespace ct 1174 | { 1175 | template struct ECEF; 1176 | 1177 | // Geodetic coordinate type 1178 | // CF determines order of lat and long 1179 | // PU is period units for angles 1180 | template struct Geodetic 1181 | { 1182 | typedef CF coordinate_format; 1183 | typedef PU period_units; 1184 | 1185 | // convert from a different coordinate format 1186 | template Point,ET> > operator()(Point,ET> > const &p) 1187 | { 1188 | Point,ET> > ret; 1189 | ret[CF::Latitude] = p[OCF::Latitude]; 1190 | ret[CF::Longitude] = p[OCF::Longitude]; 1191 | ret[CF::Height] = p[OCF::Height]; 1192 | return ret; 1193 | } 1194 | 1195 | // convert from a different coordinate format and angular unit 1196 | template Point,ET> > operator()(Point,ET> > const &p) 1197 | { 1198 | Point,ET> > ret; 1199 | ret[CF::Latitude] = p[OCF::Latitude]*PU::period/OPU::period; 1200 | ret[CF::Longitude] = p[OCF::Longitude]*PU::period/OPU::period; 1201 | ret[CF::Height] = p[OCF::Height]; 1202 | return ret; 1203 | } 1204 | 1205 | 1206 | // convert from ECEF 1207 | template Point,ET> > operator()(Point,ET> > const &p) const 1208 | { 1209 | Point,ET> > ret; 1210 | ET::ToGeodetic(p, ret); 1211 | return ret; 1212 | } 1213 | 1214 | }; 1215 | 1216 | // Earth center earth fixed coordinate type 1217 | // CF determines order of cartesian coordinates 1218 | template struct ECEF 1219 | { 1220 | typedef CF coordinate_format; 1221 | 1222 | template Point,ET> > operator()(Point,ET> > const &p) 1223 | { 1224 | return ET::ToEarthCenteredEarthFixed(p); 1225 | } 1226 | }; 1227 | } 1228 | 1229 | 1230 | /// 2D geographic position 1231 | /// PU is period units, degrees, radians or other 1232 | /// ET is ellipsoid type 1233 | template 1234 | struct Position 1235 | { 1236 | using ellipsoid = ET; 1237 | LatitudeType latitude; 1238 | LongitudeType longitude; 1239 | 1240 | Position(): 1241 | latitude(std::nan("")), longitude(std::nan("")) 1242 | {} 1243 | 1244 | Position(double latitude, double longitude): 1245 | latitude(latitude), longitude(longitude) 1246 | {} 1247 | 1248 | template 1249 | Position(const Position& other): 1250 | latitude(other.latitude), longitude(other.longitude) 1251 | {} 1252 | 1253 | Position(const Point, ET> > &point): 1254 | latitude(point.latitude()), longitude(point.longitude()) 1255 | {} 1256 | 1257 | 1258 | double distanceFrom(Position other) 1259 | { 1260 | auto ad = ellipsoid::inverse(*this, other); 1261 | return ad.second; 1262 | } 1263 | 1264 | friend std::ostream& operator<< (std::ostream& stream, const Position& p) 1265 | { 1266 | stream << "lat: " << p.latitude << " lon: " << p.longitude; 1267 | return stream; 1268 | } 1269 | 1270 | friend bool valid(const Position& p) 1271 | { 1272 | return !std::isnan(p.latitude) && !std::isnan(p.longitude); 1273 | } 1274 | 1275 | friend Position min(const Position& p1, const Position& p2) 1276 | { 1277 | return Position(min(p1.latitude, p2.latitude), min(p1.longitude, p2.longitude)); 1278 | } 1279 | 1280 | friend Position max(const Position& p1, const Position& p2) 1281 | { 1282 | return Position(max(p1.latitude, p2.latitude), max(p1.longitude, p2.longitude)); 1283 | } 1284 | 1285 | }; 1286 | 1287 | 1288 | /// @brief 2D bounding box for geographic coordinates 1289 | /// @tparam T Position type 1290 | template 1291 | class Bounds 1292 | { 1293 | public: 1294 | Bounds(){} 1295 | 1296 | Bounds(const T& p): 1297 | min_(p), max_(p) 1298 | {} 1299 | 1300 | Bounds(const T& p1, const T& p2): 1301 | min_(p1), max_(p1) 1302 | { 1303 | expand(p2); 1304 | } 1305 | 1306 | const T& minimum() const 1307 | { 1308 | return min_; 1309 | } 1310 | 1311 | const T& maximum() const 1312 | { 1313 | return max_; 1314 | } 1315 | 1316 | void expand(const T& position) 1317 | { 1318 | if(valid(position)) 1319 | { 1320 | if(valid(*this)) 1321 | { 1322 | min_ = min(min_, position); 1323 | max_ = max(max_, position); 1324 | } 1325 | else 1326 | { 1327 | min_ = position; 1328 | max_ = position; 1329 | } 1330 | } 1331 | } 1332 | 1333 | static Bounds radiusFromCenter(const T& center, double radius) 1334 | { 1335 | auto delta_lat = T::ellipsoid::latitudinalSpan(center.latitude, radius); 1336 | auto delta_lon = T::ellipsoid::longitudinalSpan(center.latitude, radius); 1337 | return Bounds(T(center.latitude-delta_lat, center.longitude-delta_lon), T(center.latitude+delta_lat, center.longitude+delta_lon)); 1338 | } 1339 | 1340 | friend bool valid(const Bounds& bounds) 1341 | { 1342 | return valid(bounds.min_) && valid(bounds.max_); 1343 | } 1344 | 1345 | friend std::ostream& operator<< (std::ostream& stream, const Bounds& bounds) 1346 | { 1347 | stream << "min: " << bounds.getMin() << " max: " << bounds.getMax(); 1348 | return stream; 1349 | } 1350 | private: 1351 | T min_; 1352 | T max_; 1353 | 1354 | }; 1355 | 1356 | 1357 | 1358 | 1359 | 1360 | // Static ellipsoid class, does not need to be instantiated to be used. 1361 | // S determines the specs 1362 | template class Ellipsoid 1363 | { 1364 | public: 1365 | using specs = S; 1366 | 1367 | using azimuth_type = Angle; 1368 | using azimuth_distance_type = std::pair; 1369 | 1370 | /// Meridional radius of curvature. 1371 | /// Radius of curvature in north-south direction. 1372 | /// @param latitude Latitude in radians. 1373 | static double M(double latitude) 1374 | { 1375 | return S::a*(1.0-S::e2)/pow((1.0-S::e2)*pow(sin(latitude),2.0),3.0/2.0); 1376 | } 1377 | 1378 | template static double M(Angle latitude){return M(latitude.value());} 1379 | template static double M(Angle latitude){return M(Radians(latitude.value()));} 1380 | 1381 | /// Transverse radius of curvature. 1382 | /// Radius of curvature in east-west direction. 1383 | /// @param latitude Latitude in radians. 1384 | static double N(double latitude) 1385 | { 1386 | if(S::e2 == 0.0) 1387 | return S::a; 1388 | return S::a/sqrt(1-S::e2*pow(sin(latitude),2.0)); 1389 | } 1390 | 1391 | template static double N(Angle latitude){return N(latitude.value());} 1392 | template static double N(Angle latitude){return N(Radians(latitude.value()));} 1393 | 1394 | /// Calculate angle of longitude covered by distance in meters at given latitude in radians. 1395 | /// From https://en.wikipedia.org/wiki/Longitude#Length_of_a_degree_of_longitude 1396 | /// delta 1 long = (pi/180)a*cos(B) where tan(B) = (b/a)tan(phi) where B is reduced latitude 1397 | static inline LongitudeSpanRadians longitudinalSpan(double latitude, double distance) 1398 | { 1399 | //U is 'reduced latitude' 1400 | double tanU1 = (1.0-S::f)*tan(latitude); 1401 | double cosU1 = 1/sqrt(1+tanU1*tanU1); 1402 | return LongitudeSpanRadians(distance/(S::a*cosU1)); 1403 | } 1404 | 1405 | template 1406 | static inline LongitudeSpanRadians longitudinalSpan(LatitudeRadians latitude, double distance) 1407 | { 1408 | return longitudinalSpan(latitude.value(), distance); 1409 | } 1410 | 1411 | template 1412 | static inline LongitudeSpanRadians longitudinalSpan(LatitudeDegrees latitude, double distance) 1413 | { 1414 | return longitudinalSpan(LatitudeRadians(latitude), distance); 1415 | } 1416 | 1417 | /// Calculates approximate angle of latitude covered by distance in meters 1418 | /// along longitudinal lines at given latitude. 1419 | /// https://en.wikipedia.org/wiki/Latitude#Length_of_a_degree_of_latitude 1420 | /// The length of a small meridian arc is given by: 1421 | /// delta m(phi) = M(phi)*delta phi = a(1-e2)((1-e2*sin(phi)^2)^(-3/2)) *delta phi 1422 | static inline LatitudeSpanRadians latitudinalSpan(LatitudeRadians latitude, double distance) 1423 | { 1424 | return LatitudeSpanRadians(distance*pow(1.0-S::e2*pow(sin(latitude),2),3.0/2.0)/(S::a*(1-S::e2))); 1425 | } 1426 | 1427 | template static Point, Ellipsoid > > ToEarthCenteredEarthFixed( Point, Ellipsoid > > const &p) 1428 | { 1429 | double latr = p[CF::Latitude]; 1430 | double lonr = p[CF::Longitude]; 1431 | double height = p[CF::Height]; 1432 | double n = N(latr); 1433 | return Point, Ellipsoid > >((n+height)*cos(latr)*cos(lonr),(n+height)*cos(latr)*sin(lonr),(n*(1.0-S::e2)+height)*sin(latr)); 1434 | } 1435 | 1436 | template static Point, Ellipsoid > > ToEarthCenteredEarthFixed( Point, Ellipsoid > > const &p) 1437 | { 1438 | return ToEarthCenteredEarthFixed(Point, Ellipsoid > >(p)); 1439 | } 1440 | 1441 | template static void ToGeodetic(Point, Ellipsoid > > const &p, Point, Ellipsoid > > &ret) 1442 | { 1443 | Point, Ellipsoid > > ret_rad; 1444 | ToGeodetic(p, ret_rad); 1445 | ret = ret_rad; 1446 | } 1447 | 1448 | template static void ToGeodetic(Point, Ellipsoid > > const &p, Point, Ellipsoid > > &ret) 1449 | { 1450 | double ep2 = (S::a*S::a)/(S::b*S::b)-1.0; 1451 | double r = sqrt(p[0]*p[0]+p[1]*p[1]); 1452 | double E2 = S::a*S::a-S::b*S::b; 1453 | double F = 54*S::b*S::b*p[2]*p[2]; 1454 | double G = r*r +(1.0-S::e2)*p[2]*p[2]-S::e2*E2; 1455 | double C = (S::e2*S::e2*F*r*r)/(G*G*G); 1456 | double s = pow(1.0+C+sqrt(C*C+2*C),1/3.0); 1457 | double P = F/(3.0*pow((s+(1.0/s)+1.0),2.0)*G*G); 1458 | double Q = sqrt(1.0+2.0*S::e2*S::e2*P); 1459 | double r0 = (-(P*S::e2*r)/(1.0+Q))+sqrt((1.0/2.0)*S::a*S::a*(1.0+1.0/Q)-((P*(1-S::e2)*p[2]*p[2])/(Q*(1.0+Q)))-(1.0/2.0)*P*r*r); 1460 | double U = sqrt(pow(r-S::e2*r0,2.0)+p[2]*p[2]); 1461 | double V = sqrt(pow(r-S::e2*r0,2.0)+(1.0-S::e2)*p[2]*p[2]); 1462 | double Z0 = S::b*S::b*p[2]/(S::a*V); 1463 | 1464 | ret[CF::Height] = U*(1.0-(S::b*S::b)/(S::a*V)); 1465 | ret[CF::Latitude] = atan((p[2]+ep2*Z0)/r); 1466 | ret[CF::Longitude] =atan2(p[1],p[0]); 1467 | } 1468 | 1469 | /// Calculates the postion P2 from azimuth and distance from P1 on the specified ellipsoid. 1470 | /// @param p1 starting point 1471 | /// @param azimuth clockwise angle in radians relative to north. 1472 | /// @param distance distance in meters. 1473 | template 1474 | static Point, Ellipsoid > > direct(Point, Ellipsoid > > const &p1, Angle azimuth, double distance) 1475 | { 1476 | double phi1 = p1[CF::Latitude]; 1477 | double alpha1 = static_cast(azimuth); 1478 | 1479 | double epsilon = 1e-12; 1480 | 1481 | //U is 'reduced latitude' 1482 | double tanU1 = (1.0-S::f)*tan(phi1); 1483 | double cosU1 = 1/sqrt(1+tanU1*tanU1); 1484 | double sinU1 = tanU1*cosU1; 1485 | 1486 | double cosAlpha1 = cos(alpha1); 1487 | double sinAlpha1 = sin(alpha1); 1488 | 1489 | double sigma1 = atan2(tanU1, cosAlpha1); // angular distance on sphere from equator to P1 along geodesic 1490 | double sinAlpha = cosU1*sinAlpha1; 1491 | double cos2Alpha = 1.0-sinAlpha*sinAlpha; 1492 | 1493 | double a = S::a; 1494 | double b = S::b; 1495 | double f = S::f; 1496 | 1497 | double u2 = cos2Alpha*(a*a-b*b)/(b*b); 1498 | 1499 | double k1 = (sqrt(1.0+u2)-1.0)/(sqrt(1.0+u2)+1.0); 1500 | double A = (1.0+k1*k1/4.0)/(1.0-k1); 1501 | double B = k1*(1.0-3.0*k1*k1/8.0); 1502 | 1503 | double sigma = distance/(b*A); 1504 | double last_sigma; 1505 | double cos2Sigmam; 1506 | while (true) 1507 | { 1508 | cos2Sigmam = cos(2.0*sigma1+sigma); 1509 | double sinSigma = sin(sigma); 1510 | double cosSigma = cos(sigma); 1511 | 1512 | double deltaSigma = B*sinSigma*(cos2Sigmam+.25*B*(cosSigma*(-1.0+2.0*cos2Sigmam*cos2Sigmam)-(B/6.0)*cos2Sigmam*(-3.0+4.0*sinSigma*sinSigma)*(-3.0+4.0*cos2Sigmam*cos2Sigmam))); 1513 | last_sigma = sigma; 1514 | sigma = (distance/(b*A))+deltaSigma; 1515 | if (fabs(last_sigma-sigma) <= epsilon) 1516 | break; 1517 | } 1518 | 1519 | cos2Sigmam = cos(2.0*sigma1+sigma); 1520 | 1521 | double phi2 = atan2(sinU1*cos(sigma)+cosU1*sin(sigma)*cosAlpha1,(1-f)*sqrt(sinAlpha*sinAlpha+pow(sinU1*sin(sigma)-cosU1*cos(sigma)*cosAlpha1,2))); 1522 | double l = atan2(sin(sigma)*sinAlpha1,cosU1*cos(sigma)-sinU1*sin(sigma)*cosAlpha1); 1523 | double C = (f/16.0)*cos2Alpha*(4.0+f*(4.0-3.0*cos2Alpha)); 1524 | double L = l-(1.0-C)*f*sinAlpha*(sigma+C*sin(sigma)*(cos2Sigmam+C*cos(sigma)*(-1+2.0*cos2Sigmam*cos2Sigmam))); 1525 | 1526 | double lat2 = phi2; 1527 | double lon2 = p1[CF::Longitude] + L; 1528 | 1529 | Point, Ellipsoid > > ret; 1530 | ret[CF::Latitude] = lat2; 1531 | ret[CF::Longitude] = lon2; 1532 | ret[CF::Height] = p1[CF::Height]; 1533 | return ret; 1534 | } 1535 | 1536 | template 1537 | static Point, Ellipsoid > > direct(Point, Ellipsoid > > const &p1, Angle azimuth, double distance) 1538 | { 1539 | return direct(Point, Ellipsoid > >(p1), azimuth, distance); 1540 | } 1541 | 1542 | 1543 | 1544 | /// Calculates the azimuth and distance from P1 to P2 on the WGS84 ellipsoid. 1545 | /// @param p1: Position P1 in radians 1546 | /// @param p2: Position P2 in radians 1547 | /// @return: azimuth in radians, distance in meters 1548 | template 1549 | static azimuth_distance_type inverse(Point, Ellipsoid > > const &p1,Point, Ellipsoid > > const &p2) 1550 | { 1551 | if(p1 == p2) 1552 | return std::pair,double>(0.0,0.0); 1553 | 1554 | double a = S::a; 1555 | double b = S::b; 1556 | double f = S::f; 1557 | 1558 | double epsilon = 1e-12; 1559 | 1560 | double phi1 = p1[CF::Latitude]; 1561 | double phi2 = p2[CF::Latitude]; 1562 | 1563 | double L = p2[CF::Longitude]-p1[CF::Longitude]; 1564 | 1565 | double U1 = atan((1.0-f)*tan(phi1)); 1566 | double U2 = atan((1.0-f)*tan(phi2)); 1567 | double cosU1 = cos(U1); 1568 | double cosU2 = cos(U2); 1569 | double sinU1 = sin(U1); 1570 | double sinU2 = sin(U2); 1571 | 1572 | double l = L; 1573 | double last_l = std::nan(""); 1574 | double cosl; 1575 | double sinl; 1576 | double sinSigma; 1577 | double cosSigma; 1578 | double sigma; 1579 | double cos2Alpha; 1580 | double cos2Sigmam; 1581 | 1582 | while (true) 1583 | { 1584 | cosl = cos(l); 1585 | sinl = sin(l); 1586 | 1587 | sinSigma = sqrt((pow((cosU2*sinl),2))+pow((cosU1*sinU2-sinU1*cosU2*cosl),2)); 1588 | cosSigma = sinU1*sinU2+cosU1*cosU2*cosl; 1589 | sigma = atan2(sinSigma,cosSigma); 1590 | double sinAlpha = (cosU1*cosU2*sinl)/sinSigma; 1591 | 1592 | cos2Alpha = 1-sinAlpha*sinAlpha; 1593 | if (cos2Alpha == 0) 1594 | cos2Sigmam = 0; 1595 | else 1596 | cos2Sigmam = cosSigma-((2.0*sinU1*sinU2)/cos2Alpha); 1597 | 1598 | if (!std::isnan(last_l) && fabs(last_l - l) <= epsilon) 1599 | break; 1600 | last_l = l; 1601 | 1602 | double C = (f/16.0)*cos2Alpha*(4.0+f*(4.0-3.0*cos2Alpha)); 1603 | l = L+(1.0-C)*f*sinAlpha*(sigma+C*sinSigma*(cos2Sigmam+C*cosSigma*(-1.0+2.0*pow(cos2Sigmam,2)))); 1604 | } 1605 | 1606 | double u2 = cos2Alpha*(a*a-b*b)/(b*b); 1607 | double k1 = (sqrt(1.0+u2)-1.0)/(sqrt(1.0+u2)+1.0); 1608 | double A = (1.0+k1*k1/4.0)/(1.0-k1); 1609 | double B = k1*(1.0-3.0*k1*k1/8.0); 1610 | double deltaSigma = B*sinSigma*(cos2Sigmam+.25*B*(cosSigma*(-1.0+2.0*cos2Sigmam*cos2Sigmam)-(B/6.0)*cos2Sigmam*(-3.0+4.0*sinSigma*sinSigma)*(-3.0+4.0*cos2Sigmam*cos2Sigmam))); 1611 | double s = b*A*(sigma-deltaSigma); 1612 | double alpha1 = atan2(cosU2*sinl,cosU1*sinU2-sinU1*cosU2*cosl); 1613 | 1614 | Angle azimuth(alpha1); 1615 | 1616 | return std::pair,double>(azimuth,s); 1617 | } 1618 | 1619 | /// Calculates the azimuth and distance from P1 to P2 on the WGS84 ellipsoid. 1620 | /// @param p1: Position P1 1621 | /// @param p2: Position P2 1622 | /// @return: azimuth in radians, distance in meters 1623 | template 1624 | static azimuth_distance_type inverse(Point, Ellipsoid > > const &p1,Point, Ellipsoid > > const &p2) 1625 | { 1626 | return inverse(Point, Ellipsoid > >(p1), Point, Ellipsoid > >(p2)); 1627 | } 1628 | 1629 | static azimuth_distance_type inverse(const Position > &p1, const Position > &p2) 1630 | { 1631 | return inverse(Point, Ellipsoid > >(p1.latitude, p1.longitude), Point, Ellipsoid > >(p2.latitude, p2.longitude)); 1632 | } 1633 | 1634 | }; 1635 | 1636 | namespace WGS84 1637 | { 1638 | struct EllipsoidSpecs 1639 | { 1640 | static constexpr double a = 6378137.0; 1641 | static constexpr double b = 6356752.3142; 1642 | static constexpr double f = 1.0/298.257223563; 1643 | static constexpr double w = 7292115e-11; 1644 | static constexpr double e2 = 1.0-(b*b)/(a*a); 1645 | }; 1646 | 1647 | typedef geo::Ellipsoid Ellipsoid; 1648 | 1649 | typedef ReferenceFrame, Ellipsoid> LatLonRadians; 1650 | typedef ReferenceFrame, Ellipsoid> LonLatRadians; 1651 | typedef ReferenceFrame, Ellipsoid> LatLonDegrees; 1652 | typedef ReferenceFrame, Ellipsoid> LonLatDegrees; 1653 | typedef ReferenceFrame, Ellipsoid> ECEF; 1654 | } 1655 | 1656 | 1657 | template class LocalENU 1658 | { 1659 | Matrix transform; 1660 | Matrix inverse; 1661 | template void initialize(Point, ET> > const &ref, Point, ET> > const &refECEF) 1662 | { 1663 | double lat = ref[CF::Latitude]; 1664 | double lon = ref[CF::Longitude]; 1665 | 1666 | transform(0,0) = -sin(lon); 1667 | transform(0,1) = cos(lon); 1668 | transform(0,2) = 0.0; 1669 | transform(0,3) = 0.0; 1670 | transform(1,0) = -sin(lat)*cos(lon); 1671 | transform(1,1) = -sin(lat)*sin(lon); 1672 | transform(1,2) = cos(lat); 1673 | transform(1,3) = 0.0; 1674 | transform(2,0) = cos(lat)*cos(lon); 1675 | transform(2,1) = cos(lat)*sin(lon); 1676 | transform(2,2) = sin(lat); 1677 | transform(2,3) = 0.0; 1678 | transform(3,0) = 0.0; 1679 | transform(3,1) = 0.0; 1680 | transform(3,2) = 0.0; 1681 | transform(3,3) = 1.0; 1682 | inverse = Translation(refECEF).GetMatrix()*transpose(transform); 1683 | transform = transform*Translation(-refECEF).GetMatrix(); 1684 | } 1685 | public: 1686 | typedef std::shared_ptr > Ptr; 1687 | 1688 | LocalENU() 1689 | { 1690 | } 1691 | 1692 | template LocalENU(Point, ET> > const &ref) 1693 | { 1694 | initialize(Point, ET> >(ref), ET::ToEarthCenteredEarthFixed(ref)); 1695 | } 1696 | 1697 | Matrix GetMatrix() const 1698 | { 1699 | return transform; 1700 | } 1701 | 1702 | Matrix GetInverseMatrix() const 1703 | { 1704 | return inverse; 1705 | } 1706 | 1707 | Point, ET> > toECEF(gz4d::Point const &p) const 1708 | { 1709 | Vector t(p,0); 1710 | t[3] = 1.0; 1711 | t = inverse*t; 1712 | return Point, ET> >(Vector(t,0)); 1713 | } 1714 | 1715 | Point, ET> > toLatLong(gz4d::Point const &p) const 1716 | { 1717 | auto ecef = toECEF(p); 1718 | Point, ET> > ret(ecef); 1719 | return ret; 1720 | } 1721 | 1722 | gz4d::Point toLocal(Point, ET> > const &p) const 1723 | { 1724 | Vector t(p,0); 1725 | t[3] = 1.0; 1726 | t = transform*t; 1727 | return Vector(t,0); 1728 | } 1729 | 1730 | template gz4d::Point toLocal(Point > const &p) const 1731 | { 1732 | Point, ET> > p_ecef(p); 1733 | return toLocal(p_ecef); 1734 | } 1735 | 1736 | std::vector > toLocal(std::vector, ET> > > const &pv) const 1737 | { 1738 | std::vector > ret; 1739 | for(const auto &p: pv) 1740 | ret.push_back(toLocal(p)); 1741 | return ret; 1742 | } 1743 | 1744 | Box2d toLonLatBox(const Box2d &local_box) const 1745 | { 1746 | Point, ET> > min(toECEF(Vector(local_box.getMin(),0))); 1747 | Point, ET> > max(toECEF(Vector(local_box.getMax(),0))); 1748 | return Box2d(Vector(min[1],min[0]),Vector(max[1],max[0])); 1749 | } 1750 | }; 1751 | 1752 | 1753 | 1754 | 1755 | } 1756 | 1757 | typedef geo::Point GeoPointLatLongDegrees; 1758 | typedef geo::Point GeoPointLatLongRadians; 1759 | typedef geo::Point GeoPointECEF; 1760 | typedef geo::LocalENU<> LocalENU; 1761 | 1762 | using PositionDegrees = geo::Position; 1763 | using PositionRadians = geo::Position; 1764 | using BoundsDegrees = geo::Bounds; 1765 | 1766 | template T norm2(Vector const &v) 1767 | { 1768 | T ret = v[0]*v[0]; 1769 | for(std::size_t i = 1; i < N; ++i) 1770 | ret += v[i]*v[i]; 1771 | return ret; 1772 | } 1773 | 1774 | template T norm(Vector const &v) 1775 | { 1776 | return sqrt(norm2(v)); 1777 | } 1778 | 1779 | template Vector normalize(Vector const &v) 1780 | { 1781 | return v/norm(v); 1782 | } 1783 | 1784 | template Vector cross(Vector const &a, Vector const &b) 1785 | { 1786 | return Vector(a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]); 1787 | } 1788 | 1789 | template Vector cross(Vector const &a, Vector const &b) 1790 | { 1791 | return Vector(0,0,(a[0]*b[1])-(a[1]*b[0])); 1792 | } 1793 | 1794 | template 1795 | class Rotation: public boost::math::quaternion 1796 | { 1797 | public: 1798 | Rotation() 1799 | :boost::math::quaternion(1) 1800 | {} 1801 | 1802 | template Rotation(Rotation const &r) 1803 | :boost::math::quaternion(r) 1804 | {} 1805 | 1806 | template Rotation(boost::math::quaternion const &q) 1807 | :boost::math::quaternion(q) 1808 | {} 1809 | 1810 | template Rotation(Angle angle, Point const &axis) 1811 | { 1812 | Rotation::Set(Angle(angle),axis); 1813 | } 1814 | 1815 | template Rotation(Angle angle, Point const &axis) 1816 | { 1817 | Rotation::Set(angle ,axis); 1818 | } 1819 | 1820 | 1821 | template Rotation(Point const &v1, Point const &v2) 1822 | :boost::math::quaternion(1) 1823 | { 1824 | T2 n = norm(v1)*norm(v2); 1825 | if(n > 0.0) 1826 | { 1827 | Angle angle = acos(v1.dot(v2)/n); 1828 | Point axis = cross(v1,v2); 1829 | Rotation::Set(angle,axis); 1830 | } 1831 | } 1832 | 1833 | 1834 | Angle angle() const 1835 | { 1836 | return acos(boost::math::quaternion::real())*2.0; 1837 | } 1838 | 1839 | /// Sets rotation using angle around axis 1840 | template void Set(Angle angle, Point const &axis) 1841 | { 1842 | if(norm2(axis) > 0.0) 1843 | { 1844 | Point a = normalize(axis) * sin(angle/2.0); 1845 | *this = boost::math::quaternion(cos(angle/2.0),a[0],a[1],a[2]); 1846 | } 1847 | } 1848 | 1849 | Point operator()(Point const &vector) const 1850 | { 1851 | boost::math::quaternion v(0.0,vector[0],vector[1],vector[2]); 1852 | v = (*this) * v * conj(*this); 1853 | return Point(v.R_component_2(),v.R_component_3(),v.R_component_4()); 1854 | } 1855 | 1856 | Rotation Inverse() const 1857 | { 1858 | return conj(*this); 1859 | } 1860 | 1861 | Matrix GetMatrix() const 1862 | { 1863 | Matrix ret = Matrix::Identity(); 1864 | 1865 | T a = this->R_component_1(); 1866 | T b = this->R_component_2(); 1867 | T c = this->R_component_3(); 1868 | T d = this->R_component_4(); 1869 | T a2 = a*a; 1870 | T b2 = b*b; 1871 | T c2 = c*c; 1872 | T d2 = d*d; 1873 | 1874 | ret(0,0) = a2 + b2 - c2 - d2; 1875 | ret(1,0) = 2.0*(a*d + b*c); 1876 | ret(2,0) = 2.0*(b*d - a*c); 1877 | 1878 | ret(0,1) = 2.0*(b*c - a*d); 1879 | ret(1,1) = a2 - b2 + c2 - d2; 1880 | ret(2,1) = 2.0*(a*b + c*d); 1881 | 1882 | ret(0,2) = 2.0*(a*c + b*d); 1883 | ret(1,2) = 2.0*(c*d - a*b); 1884 | ret(2,2) = a2 - b2 - c2 + d2; 1885 | return ret; 1886 | } 1887 | 1888 | Matrix GetInverseMatrix() const 1889 | { 1890 | return Inverse().GetMatrix(); 1891 | } 1892 | }; 1893 | 1894 | } 1895 | 1896 | template gz4d::Vector operator+(T l, gz4d::Vector const &r) 1897 | { 1898 | return r+l; 1899 | } 1900 | 1901 | template gz4d::Vector operator-(T l, gz4d::Vector const &r) 1902 | { 1903 | return -r+l; 1904 | } 1905 | 1906 | template gz4d::Vector operator*(T l, gz4d::Vector const &r) 1907 | { 1908 | return r*l; 1909 | } 1910 | 1911 | 1912 | #endif 1913 | -------------------------------------------------------------------------------- /include/project11/pid.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_PID_H 2 | #define PROJECT11_PID_H 3 | 4 | #include 5 | #include "rclcpp_lifecycle/lifecycle_node.hpp" 6 | #include 7 | #include 8 | 9 | namespace project11 10 | { 11 | 12 | class PID 13 | { 14 | public: 15 | 16 | PID(rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string prefix = "pid") 17 | { 18 | node_ = node; 19 | update_parameters_callback_ = node->add_post_set_parameters_callback(std::bind(&PID::updateParameters, this, std::placeholders::_1)); 20 | 21 | parameter_prefix_ = prefix; 22 | if(!parameter_prefix_.empty()) 23 | parameter_prefix_ += "."; 24 | 25 | if(!node->has_parameter(parameter_prefix_+"kp")) 26 | node->declare_parameter(parameter_prefix_+"kp", Kp_); 27 | if(!node->has_parameter(parameter_prefix_+"ki")) 28 | node->declare_parameter(parameter_prefix_+"ki", Ki_); 29 | if(!node->has_parameter(parameter_prefix_+"kd")) 30 | node->declare_parameter(parameter_prefix_+"kd", Kd_); 31 | if(!node->has_parameter(parameter_prefix_+"windup_limit")) 32 | node->declare_parameter(parameter_prefix_+"windup_limit", windup_limit_); 33 | if(!node->has_parameter(parameter_prefix_+"upper_limit")) 34 | node->declare_parameter(parameter_prefix_+"upper_limit", upper_limit_); 35 | if(!node->has_parameter(parameter_prefix_+"lower_limit")) 36 | node->declare_parameter(parameter_prefix_+"lower_limit", lower_limit_); 37 | 38 | if(!node->has_parameter(parameter_prefix_+"max_dt")) 39 | node->declare_parameter(parameter_prefix_+"max_dt", max_dt_.seconds()); 40 | if(!node->has_parameter(parameter_prefix_+"publish_debug")) 41 | node->declare_parameter(parameter_prefix_+"publish_debug", false); 42 | 43 | topic_prefix_ = prefix; 44 | if(!topic_prefix_.empty()) 45 | topic_prefix_ += "/"; 46 | } 47 | 48 | double setPoint(double set_point) 49 | { 50 | set_point_ = set_point; 51 | return set_point_; 52 | } 53 | 54 | double setPoint() const 55 | { 56 | return set_point_; 57 | } 58 | 59 | void reset() 60 | { 61 | integral_ = 0.0; 62 | last_error_ = 0.0; 63 | last_timestamp_= rclcpp::Time(); 64 | } 65 | 66 | double update(double process_variable, rclcpp::Time timestamp=rclcpp::Time()) 67 | { 68 | if(timestamp.nanoseconds() == 0) 69 | timestamp = node_.lock()->get_clock()->now(); 70 | 71 | double error = set_point_ - process_variable; 72 | double derivative = 0.0; 73 | auto dt = rclcpp::Duration::from_seconds(0.0); 74 | 75 | if(last_timestamp_.nanoseconds() != 0) 76 | { 77 | if(timestamp > last_timestamp_) 78 | { 79 | dt = timestamp - last_timestamp_; 80 | if(dt > max_dt_) 81 | { 82 | // it's been too long, reset. 83 | reset(); 84 | } 85 | else 86 | { 87 | integral_ += error*dt.seconds(); 88 | integral_ = std::min(integral_, windup_limit_); 89 | integral_ = std::max(integral_, -windup_limit_); 90 | 91 | derivative = (error - last_error_)/dt.seconds(); 92 | } 93 | } 94 | } 95 | last_timestamp_ = timestamp; 96 | last_error_ = error; 97 | 98 | double control_effort = Kp_*error + Ki_*integral_ + Kd_*derivative; 99 | 100 | control_effort = std::min(control_effort, upper_limit_); 101 | control_effort = std::max(control_effort, lower_limit_); 102 | 103 | if(debug_publisher_) 104 | { 105 | project11_msgs::msg::PIDDebug debug; 106 | debug.header.stamp = timestamp; 107 | debug.parameters.kp = Kp_; 108 | debug.parameters.ki = Ki_; 109 | debug.parameters.kd = Kd_; 110 | debug.parameters.windup_limit = windup_limit_; 111 | debug.parameters.upper_limit = upper_limit_; 112 | debug.parameters.lower_limit = lower_limit_; 113 | debug.set_point = set_point_; 114 | debug.process_variable = process_variable; 115 | debug.error = error; 116 | debug.dt = dt.seconds(); 117 | debug.integral = integral_; 118 | debug.derivative = derivative; 119 | debug.control_effort = control_effort; 120 | debug.p = Kp_*error; 121 | debug.i = Ki_*integral_; 122 | debug.d = Kd_*derivative; 123 | debug_publisher_->publish(debug); 124 | } 125 | 126 | return control_effort; 127 | } 128 | 129 | double Kp() const 130 | { 131 | return Kp_; 132 | } 133 | 134 | double Ki() const 135 | { 136 | return Ki_; 137 | } 138 | 139 | double Kd() const 140 | { 141 | return Kd_; 142 | } 143 | 144 | private: 145 | void updateParameters(const std::vector & parameters) 146 | { 147 | for(const auto& param: parameters) 148 | { 149 | if(param.get_name() == parameter_prefix_+"kp") 150 | Kp_ = param.as_double(); 151 | if(param.get_name() == parameter_prefix_+"ki") 152 | Ki_ = param.as_double(); 153 | if(param.get_name() == parameter_prefix_+"kd") 154 | Kd_ = param.as_double(); 155 | if(param.get_name() == parameter_prefix_+"windup_limit") 156 | windup_limit_ = param.as_double(); 157 | if(param.get_name() == parameter_prefix_+"upper_limit") 158 | upper_limit_ = param.as_double(); 159 | if(param.get_name() == parameter_prefix_+"lower_limit") 160 | lower_limit_ = param.as_double(); 161 | if(param.get_name() == parameter_prefix_+"max_dt") 162 | max_dt_ = rclcpp::Duration::from_seconds(param.as_double()); 163 | if(param.get_name() == parameter_prefix_+"publish_debug") 164 | { 165 | if(param.as_bool()) 166 | { 167 | if(!debug_publisher_) 168 | debug_publisher_ = node_.lock()->create_publisher(topic_prefix_+"debug", 10); 169 | } 170 | else 171 | debug_publisher_.reset(); 172 | } 173 | } 174 | } 175 | 176 | double set_point_ = 0.0; 177 | 178 | rclcpp::Time last_timestamp_; 179 | double last_error_; 180 | 181 | double integral_ = 0.0; 182 | 183 | double Kp_ = 1.0; 184 | double Ki_ = 0.0; 185 | double Kd_ = 0.0; 186 | 187 | double windup_limit_ = 1000.0; 188 | double upper_limit_ = 1000.0; 189 | double lower_limit_ = -1000.0; 190 | 191 | rclcpp::Duration max_dt_ = rclcpp::Duration::from_seconds(5.0); 192 | 193 | /// Prepended to parameter names 194 | std::string parameter_prefix_; 195 | 196 | /// Prepended to topic names 197 | std::string topic_prefix_; 198 | 199 | //rclcpp::Node::SharedPtr node_; 200 | rclcpp_lifecycle::LifecycleNode::WeakPtr node_; 201 | rclcpp::node_interfaces::PostSetParametersCallbackHandle::SharedPtr update_parameters_callback_; 202 | rclcpp::Publisher::SharedPtr debug_publisher_; 203 | 204 | }; 205 | 206 | } // namespace project11 207 | 208 | #endif -------------------------------------------------------------------------------- /include/project11/tf2_utils.h: -------------------------------------------------------------------------------- 1 | #ifndef PROJECT11_TF2_UTILS_H 2 | #define PROJECT11_TF2_UTILS_H 3 | 4 | #include "rclcpp/rclcpp.hpp" 5 | #include "tf2_ros/transform_listener.h" 6 | #include "tf2_ros/buffer.h" 7 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 8 | #include "geographic_msgs/msg/geo_point.hpp" 9 | #include "geometry_msgs/msg/point.hpp" 10 | #include "project11/utils.h" 11 | #include 12 | 13 | namespace project11 14 | { 15 | class Transformations 16 | { 17 | public: 18 | Transformations(rclcpp::Node::SharedPtr node): 19 | node_(node) 20 | { 21 | tf_buffer_ = std::make_unique(node_->get_clock()); 22 | tf_listener_ = std::make_shared(*tf_buffer_, node); 23 | } 24 | 25 | bool canTransform(std::string map_frame="map", rclcpp::Time target_time = rclcpp::Time(), tf2::Duration timeout = tf2::durationFromSec(0.5)) 26 | { 27 | return buffer()->canTransform("earth", map_frame, target_time, timeout); 28 | } 29 | 30 | geometry_msgs::msg::Point wgs84_to_map(geographic_msgs::msg::GeoPoint const &position, std::string map_frame="map", rclcpp::Time target_time = rclcpp::Time()) 31 | { 32 | geometry_msgs::msg::Point ret; 33 | try 34 | { 35 | geometry_msgs::msg::TransformStamped t = buffer()->lookupTransform(map_frame,"earth",target_time); 36 | LatLongDegrees p_ll; 37 | fromMsg(position, p_ll); 38 | bool alt_is_nan = std::isnan(position.altitude); 39 | if(alt_is_nan) 40 | p_ll.altitude() = 0.0; 41 | ECEF p_ecef = p_ll; 42 | geometry_msgs::msg::Point in; 43 | toMsg(p_ecef, in); 44 | tf2::doTransform(in,ret,t); 45 | if(alt_is_nan) 46 | ret.z = std::nan(""); 47 | } 48 | catch (tf2::TransformException &ex) 49 | { 50 | RCLCPP_WARN_STREAM(node_->get_logger(), "Transformations: " << ex.what()); 51 | } 52 | return ret; 53 | } 54 | 55 | geographic_msgs::msg::GeoPoint map_to_wgs84(geometry_msgs::msg::Point const &point, std::string map_frame="map", rclcpp::Time target_time = rclcpp::Time()) 56 | { 57 | geographic_msgs::msg::GeoPoint ret; 58 | try 59 | { 60 | geometry_msgs::msg::TransformStamped t = buffer()->lookupTransform("earth",map_frame, target_time); 61 | geometry_msgs::msg::Point out; 62 | tf2::doTransform(point,out,t); 63 | ECEF out_ecef; 64 | fromMsg(out,out_ecef); 65 | LatLongDegrees latlon = out_ecef; 66 | toMsg(latlon, ret); 67 | } 68 | catch (tf2::TransformException &ex) 69 | { 70 | RCLCPP_WARN_STREAM(node_->get_logger(), "Transformations: " << ex.what()); 71 | } 72 | return ret; 73 | } 74 | 75 | const std::unique_ptr &operator()() const 76 | { 77 | return buffer(); 78 | } 79 | 80 | private: 81 | const std::unique_ptr &buffer() const 82 | { 83 | return tf_buffer_; 84 | } 85 | 86 | rclcpp::Node::SharedPtr node_; 87 | 88 | std::unique_ptr tf_buffer_; 89 | std::shared_ptr tf_listener_; 90 | }; 91 | } 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /include/project11/utils.h: -------------------------------------------------------------------------------- 1 | #ifndef P11_UTILS_H 2 | #define P11_UTILS_H 3 | 4 | #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" 5 | #include 6 | #include "gz4d_geo.h" 7 | 8 | namespace project11 9 | { 10 | template double quaternionToHeadingDegrees(const A& a) 11 | { 12 | return 90.0-180.0*tf2::getYaw(a)/M_PI; 13 | } 14 | 15 | template double quaternionToHeadingRadians(const A& a) 16 | { 17 | return (M_PI/2.0)-tf2::getYaw(a); 18 | } 19 | 20 | template inline double speedOverGround(const T & v) 21 | { 22 | tf2::Vector3 v3; 23 | tf2::fromMsg(v, v3); 24 | return v3.length(); 25 | } 26 | 27 | using LatLongDegrees = gz4d::GeoPointLatLongDegrees; 28 | using ECEF = gz4d::GeoPointECEF; 29 | using Point = gz4d::Point; 30 | using ENUFrame = gz4d::LocalENU; 31 | 32 | // angle types that do not wrap 33 | using AngleDegrees = gz4d::Angle; 34 | using AngleRadians = gz4d::Angle; 35 | 36 | // angle types that wrap at +/- half a circle 37 | using AngleDegreesZeroCentered = gz4d::Angle; 38 | using AngleRadiansZeroCentered = gz4d::Angle; 39 | 40 | // angle types that wrap at 0 and full circle 41 | using AngleDegreesPositive = gz4d::Angle; 42 | using AngleRadiansPositive = gz4d::Angle; 43 | 44 | using WGS84 = gz4d::geo::WGS84::Ellipsoid; 45 | 46 | template void fromMsg(const A& a, LatLongDegrees &b) 47 | { 48 | b.latitude() = a.latitude; 49 | b.longitude() = a.longitude; 50 | b.altitude() = a.altitude; 51 | } 52 | 53 | template void toMsg(const LatLongDegrees &a, B &b) 54 | { 55 | b.latitude = a.latitude(); 56 | b.longitude = a.longitude(); 57 | b.altitude = a.altitude(); 58 | } 59 | 60 | template void fromMsg(const A& a, Point &b) 61 | { 62 | b = Point(a.x, a.y, a.z); 63 | } 64 | 65 | template void toMsg(const Point& a, B &b) 66 | { 67 | b.x = a[0]; 68 | b.y = a[1]; 69 | b.z = a[2]; 70 | } 71 | } 72 | 73 | std::ostream& operator<< (std::ostream &out, const project11::LatLongDegrees &p); 74 | std::ostream& operator<< (std::ostream &out, const project11::ECEF &p); 75 | std::ostream& operator<< (std::ostream &out, const project11::AngleDegrees &p); 76 | std::ostream& operator<< (std::ostream &out, const project11::AngleRadians &p); 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /launch/local_remote_operator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/observer_core.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /launch/operator_core_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | 3 | from launch.actions import DeclareLaunchArgument 4 | from launch.actions import GroupAction 5 | from launch.actions import IncludeLaunchDescription 6 | from launch.conditions import IfCondition 7 | from launch.launch_description_sources import PythonLaunchDescriptionSource 8 | from launch.substitutions import LaunchConfiguration 9 | from launch.substitutions import PathJoinSubstitution 10 | 11 | from launch_ros.actions import Node 12 | from launch_ros.actions import PushROSNamespace 13 | from launch_ros.actions import SetRemap 14 | from launch_ros.actions import SetParametersFromFile 15 | from launch_ros.substitutions import FindPackageShare 16 | 17 | def generate_launch_description(): 18 | operator_namespace = LaunchConfiguration('operator_namespace') 19 | robot_namespace = LaunchConfiguration('robot_namespace') 20 | operator_joystick = LaunchConfiguration('operator_joystick') 21 | enable_bridge = LaunchConfiguration('enable_bridge') 22 | enable_sound = LaunchConfiguration('enable_sound') 23 | 24 | operator_namespace_arg = DeclareLaunchArgument( 25 | "operator_namespace", default_value="operator" 26 | ) 27 | robot_namespace_arg = DeclareLaunchArgument( 28 | "robot_namespace", default_value="robot" 29 | ) 30 | local_port_arg = DeclareLaunchArgument( 31 | "local_port", default_value="4200" 32 | ) 33 | operator_joystick_arg = DeclareLaunchArgument( 34 | "operator_joystick", default_value="true" 35 | ) 36 | enable_bridge_arg = DeclareLaunchArgument( 37 | "enable_bridge", default_value="false" 38 | ) 39 | robot_bridge_name_arg = DeclareLaunchArgument( 40 | "robot_bridge_name", default_value=robot_namespace 41 | ) 42 | enable_sound_arg = DeclareLaunchArgument( 43 | "enable_sound", default_value="false" 44 | ) 45 | 46 | # The udp_bridge_node is what sends and receives messages from select topics to the robot. 47 | 48 | udp_bridge_include = GroupAction( 49 | actions=[ 50 | PushROSNamespace(operator_namespace), 51 | IncludeLaunchDescription( 52 | PythonLaunchDescriptionSource( 53 | PathJoinSubstitution([ 54 | FindPackageShare('udp_bridge'), 55 | 'launch', 56 | 'udp_bridge_launch.py' 57 | ]) 58 | ), 59 | condition = IfCondition(enable_bridge) 60 | ) 61 | ] 62 | ) 63 | 64 | # Command bridge is used to robustly send operator commands over an unreliable connection. 65 | command_bridge_sender_node = Node( 66 | package='command_bridge', 67 | executable='command_bridge_sender', 68 | name='command_bridge_sender', 69 | namespace=robot_namespace 70 | ) 71 | 72 | # Driver for joystick device. 73 | joy_group = GroupAction( 74 | actions=[ 75 | PushROSNamespace(robot_namespace), 76 | Node( 77 | package='joy', 78 | executable='joy_node', 79 | name='joy_node', 80 | parameters=[{'autorepeat_rate': 10.0}, {'deadzone': 0.0}], 81 | ), 82 | # joy_to_helm converts joystick messages to Helm messages with throttle and 83 | # rudder values, and sends piloting_mode change commands based on button clicks. 84 | SetRemap( src='helm', dst='piloting_mode/manual/helm'), 85 | IncludeLaunchDescription( 86 | PythonLaunchDescriptionSource( 87 | PathJoinSubstitution([ 88 | FindPackageShare('joy_to_helm'), 89 | 'launch', 90 | 'joy_to_helm_launch.py' 91 | ]) 92 | ) 93 | ), 94 | ], 95 | condition=IfCondition(operator_joystick) 96 | ) 97 | 98 | sound_node = Node( 99 | package='sound_play', 100 | executable='soundplay_node.py', 101 | name='soundplay_node', 102 | namespace=operator_namespace, 103 | condition=IfCondition(enable_sound), 104 | remappings=[('robotsound', 'project11/robotsound')] 105 | ) 106 | 107 | return LaunchDescription([ 108 | operator_namespace_arg, 109 | robot_namespace_arg, 110 | local_port_arg, 111 | operator_joystick_arg, 112 | enable_bridge_arg, 113 | robot_bridge_name_arg, 114 | enable_sound_arg, 115 | udp_bridge_include, 116 | command_bridge_sender_node, 117 | joy_group, 118 | sound_node 119 | ]) 120 | -------------------------------------------------------------------------------- /launch/operator_ui_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.conditions import IfCondition 4 | from launch_ros.actions import Node 5 | from launch_ros.substitutions import FindPackageShare 6 | from launch.substitutions import PathJoinSubstitution 7 | from launch.substitutions import LaunchConfiguration 8 | 9 | def generate_launch_description(): 10 | namespace = LaunchConfiguration('namespace') 11 | robot_namespace = LaunchConfiguration('robot_namespace') 12 | background_chart = LaunchConfiguration('background_chart') 13 | rqt = LaunchConfiguration('rqt') 14 | rviz = LaunchConfiguration('rviz') 15 | rviz_configuration = LaunchConfiguration('rviz_configuration') 16 | dual_camp = LaunchConfiguration('dual_camp') 17 | 18 | namespace_arg = DeclareLaunchArgument( 19 | "namespace", default_value="operator" 20 | ) 21 | robot_namespace_arg = DeclareLaunchArgument( 22 | "robot_namespace", default_value="ben" 23 | ) 24 | background_chart_arg = DeclareLaunchArgument( 25 | "background_chart", 26 | default_value=PathJoinSubstitution([ 27 | FindPackageShare('camp'), 28 | 'workspace/13283/13283_2.KAP' 29 | ]) 30 | ) 31 | rqt_arg = DeclareLaunchArgument( 32 | "rqt", default_value="false" 33 | ) 34 | 35 | rviz_arg = DeclareLaunchArgument( 36 | "rviz", default_value="false" 37 | ) 38 | 39 | rviz_configuration_arg = DeclareLaunchArgument( 40 | "rqt_configuration", default_value="" 41 | ) 42 | 43 | dual_camp_arg = DeclareLaunchArgument( 44 | "dual_camp", default_value="false" 45 | ) 46 | 47 | rqt_node = Node( 48 | package='rqt_gui', 49 | executable='rqt_gui', 50 | name='rqt', 51 | condition=IfCondition(rqt), 52 | ) 53 | 54 | rviz_node = Node( 55 | package='rviz2', 56 | executable='rviz2', 57 | namespace=robot_namespace, 58 | condition=IfCondition(rviz), 59 | arguments=['-d', rviz_configuration], 60 | # remappings=[ 61 | # ('/tf', 'tf'), 62 | # ('/tf_static', 'tf_static') 63 | # ] 64 | ) 65 | 66 | camp_node = Node( 67 | package='camp', 68 | executable='CCOMAutonomousMissionPlanner', 69 | name='camp', 70 | arguments=[ 71 | PathJoinSubstitution([ FindPackageShare('camp'), 'workspace/']), 72 | background_chart], 73 | namespace=namespace, 74 | parameters=[{'robot_namespace': robot_namespace}], 75 | respawn=True, 76 | respawn_delay=5, 77 | ) 78 | 79 | camp2_node = Node( 80 | package='camp', 81 | executable='CCOMAutonomousMissionPlanner', 82 | name='camp2', 83 | arguments=[ 84 | PathJoinSubstitution([ FindPackageShare('camp'), '/workspace/']), 85 | background_chart], 86 | namespace=namespace, 87 | parameters=[{'robot_namespace': robot_namespace}], 88 | condition=IfCondition(dual_camp), 89 | ) 90 | 91 | return LaunchDescription([ 92 | namespace_arg, 93 | robot_namespace_arg, 94 | background_chart_arg, 95 | rqt_arg, 96 | rviz_arg, 97 | rviz_configuration_arg, 98 | dual_camp_arg, 99 | rqt_node, 100 | rviz_node, 101 | camp_node, 102 | camp2_node, 103 | ]) 104 | 105 | 106 | -------------------------------------------------------------------------------- /launch/platform_nav_source_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.actions import GroupAction 4 | from launch.substitutions import LaunchConfiguration 5 | from launch_ros.actions import PushROSNamespace 6 | from launch_ros.actions import SetParameter 7 | 8 | 9 | 10 | def generate_launch_description(): 11 | namespace = LaunchConfiguration('namespace') 12 | platform_name = LaunchConfiguration('platform_name') 13 | nav_source = LaunchConfiguration('nav_source') 14 | position_topic = LaunchConfiguration('position_topic') 15 | orientation_topic = LaunchConfiguration('orientation_topic') 16 | velocity_topic = LaunchConfiguration('velocity_topic') 17 | 18 | namespace_arg = DeclareLaunchArgument( 19 | 'namespace' 20 | ) 21 | platform_name_arg = DeclareLaunchArgument( 22 | 'platform_name', default_value=namespace 23 | ) 24 | nav_source_arg = DeclareLaunchArgument( 25 | 'nav_source', default_value='nav' 26 | ) 27 | position_topic_arg = DeclareLaunchArgument( 28 | 'position_topic', default_value=['sensors',nav_source,'position'] 29 | ) 30 | orientation_topic_arg = DeclareLaunchArgument( 31 | 'orientation_topic', default_value=['sensors',nav_source,'orientation'] 32 | ) 33 | velocity_topic_arg = DeclareLaunchArgument( 34 | 'velocity_topic', default_value=['sensors',nav_source,'velocity'] 35 | ) 36 | 37 | namespace_group = GroupAction( 38 | actions=[ 39 | PushROSNamespace(namespace), 40 | SetParameter( 41 | name=[nav_source,'position_topic'], 42 | value=position_topic 43 | ), 44 | SetParameter( 45 | name=[nav_source,'orientation_topic'], 46 | value=orientation_topic 47 | ), 48 | SetParameter( 49 | name=[nav_source,'velocity_topic'], 50 | value=velocity_topic 51 | ) 52 | ] 53 | ) 54 | 55 | return LaunchDescription([ 56 | namespace_arg, 57 | platform_name_arg, 58 | nav_source_arg, 59 | position_topic_arg, 60 | orientation_topic_arg, 61 | velocity_topic_arg, 62 | namespace_group 63 | ]) 64 | -------------------------------------------------------------------------------- /launch/platform_sender_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.substitutions import LaunchConfiguration 4 | from launch.substitutions import PythonExpression 5 | from launch_ros.actions import LifecycleNode 6 | from launch_ros.actions import LifecycleTransition 7 | 8 | from lifecycle_msgs.msg import Transition 9 | 10 | def generate_launch_description(): 11 | return LaunchDescription([ 12 | DeclareLaunchArgument("name"), 13 | DeclareLaunchArgument( 14 | "namespace", 15 | default_value=LaunchConfiguration("name") 16 | ), 17 | LifecycleNode( 18 | package='project11', 19 | executable='platform_send.py', 20 | name='platform_sender', 21 | namespace='', 22 | parameters=[{ 23 | 'name': LaunchConfiguration("name"), 24 | 'platform_namespace': LaunchConfiguration("namespace") 25 | }] 26 | ), 27 | LifecycleTransition( 28 | lifecycle_node_names=( 29 | PythonExpression( 30 | expression = [ 31 | '"', 32 | LaunchConfiguration("ros_namespace", default=''), 33 | '" + "/platform_sender"' 34 | ], 35 | ), 36 | ), 37 | transition_ids=( 38 | Transition.TRANSITION_CONFIGURE, 39 | Transition.TRANSITION_ACTIVATE, 40 | ) 41 | ) 42 | ]) 43 | 44 | -------------------------------------------------------------------------------- /launch/remote_operator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 58 | 59 | -------------------------------------------------------------------------------- /launch/robot_core_launch.py: -------------------------------------------------------------------------------- 1 | from launch import LaunchDescription 2 | from launch.actions import DeclareLaunchArgument 3 | from launch.actions import GroupAction 4 | from launch.actions import IncludeLaunchDescription 5 | from launch.conditions import IfCondition 6 | from launch.substitutions import LaunchConfiguration 7 | from launch.substitutions import PathJoinSubstitution 8 | from launch.substitutions import TextSubstitution 9 | from launch.launch_description_sources import PythonLaunchDescriptionSource 10 | from launch_ros.actions import Node 11 | from launch_ros.actions import SetParametersFromFile 12 | from launch_ros.actions import SetRemap 13 | from launch_ros.substitutions import FindPackageShare 14 | 15 | 16 | def generate_launch_description(): 17 | namespace = LaunchConfiguration('namespace') 18 | enable_bridge = LaunchConfiguration('enable_bridge') 19 | local_port = LaunchConfiguration('local_port') 20 | bridge_name = LaunchConfiguration('bridge_name') 21 | map_frame = LaunchConfiguration('map_frame') 22 | 23 | namespace_arg = DeclareLaunchArgument( 24 | "namespace", default_value=TextSubstitution(text="ben") 25 | ) 26 | enable_bridge_arg = DeclareLaunchArgument( 27 | "enable_bridge", default_value=TextSubstitution(text="false") 28 | ) 29 | local_port_arg = DeclareLaunchArgument( 30 | "local_port", default_value=TextSubstitution(text="4200") 31 | ) 32 | bridge_name_arg = DeclareLaunchArgument( 33 | "bridge_name", default_value=namespace 34 | ) 35 | map_frame_arg = DeclareLaunchArgument( 36 | "map_frame", default_value=PathJoinSubstitution([namespace, 'map']) 37 | ) 38 | 39 | namespace_group = GroupAction( 40 | actions=[ 41 | SetParametersFromFile( 42 | filename=PathJoinSubstitution([FindPackageShare('project11'), 'config', 'robot.yaml']) 43 | ), 44 | # The udp_bridge_node is what sends and receives messages from select topics to the operator. 45 | Node( 46 | package='udp_bridge', 47 | executable='udp_bridge_node', 48 | name='udp_bridge', 49 | parameters=[{'port': local_port}, {'name': bridge_name}], 50 | condition=IfCondition(enable_bridge) 51 | ), 52 | # Command bridge is used to robustly send operator commands over an unreliable connection. 53 | Node( 54 | package='command_bridge', 55 | executable='command_bridge_receiver', 56 | name='command_bridge_receiver', 57 | ), 58 | # helm_manager is the low level heart of project11. It manages which control messages get sent to the robot based on piloting mode and reports the piloting mode and other status as a heartbeat message. 59 | GroupAction( 60 | actions=[ 61 | SetRemap(src='out/helm', dst='project11/control/helm'), 62 | SetRemap(src='out/cmd_vel', dst='project11/control/cmd_vel'), 63 | SetRemap(src='heartbeat', dst='project11/heartbeat'), 64 | SetRemap(src='status/helm', dst='project11/status/helm'), 65 | IncludeLaunchDescription( 66 | PythonLaunchDescriptionSource( 67 | PathJoinSubstitution([ 68 | FindPackageShare('helm_manager'), 69 | 'launch', 70 | 'helm_manager_launch.py' 71 | ]) 72 | ), 73 | ) 74 | ], 75 | ), 76 | # mission_manager handles commands from the operator and manages the task list sent to the navigator. 77 | IncludeLaunchDescription( 78 | PythonLaunchDescriptionSource( 79 | PathJoinSubstitution([FindPackageShare('mission_manager'), 'launch', 'mission_manager_launch.py']) 80 | ), 81 | launch_arguments={ 82 | 'map_frame': map_frame 83 | }.items() 84 | ), 85 | # project11_navigation is the plugin based navigation stack that uses a list of tasks to plan trajectories and generate commands to drive the robot. 86 | # IncludeLaunchDescription( 87 | # PythonLaunchDescriptionSource( 88 | # PathJoinSubstitution([FindPackageShare('project11_navigation'), 'launch', 'navigator_launch.py']) 89 | # ), 90 | # launch_arguments={ 91 | # 'map_frame': map_frame 92 | # }.items() 93 | # ), 94 | IncludeLaunchDescription( 95 | PythonLaunchDescriptionSource( 96 | PathJoinSubstitution([ 97 | FindPackageShare('project11'), 98 | 'launch', 99 | 'platform_sender_launch.py']) 100 | ), 101 | launch_arguments={ 102 | 'name': namespace 103 | }.items() 104 | ), 105 | ] 106 | ) 107 | 108 | return LaunchDescription([ 109 | namespace_arg, 110 | enable_bridge_arg, 111 | local_port_arg, 112 | bridge_name_arg, 113 | map_frame_arg, 114 | namespace_group 115 | ]) 116 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | project11 3 | 0.0.0 4 | The Project 11 package 5 | Roland Arsenault 6 | 7 | BSD 8 | 9 | ament_cmake 10 | ament_cmake_python 11 | 12 | geometry_msgs 13 | project11_msgs 14 | rclpy 15 | rclcpp 16 | rclcpp_lifecycle 17 | tf2 18 | tf2_geometry_msgs 19 | tf_transformations 20 | tf2_ros 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /project11/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | from . import geodesic 3 | from . import nav 4 | from . import wgs84 5 | 6 | __all__ = [ 7 | 'geodesic', 8 | 'nav', 9 | 'wgs84' 10 | ] 11 | -------------------------------------------------------------------------------- /project11/geodesic.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | __author__ = 'Roland Arsenault' 4 | 5 | __doc__ =''' 6 | Implements Vincenty's formula to calculate the forward (position1+azimuth and range to position2) 7 | and inverse (position1, position2 to azimuth and range) solutions of geodesics on the WGS84 ellipsoid. 8 | 9 | Vincenty, T. (April 1975a)."DIRECT AND INVERSE SOLUTIONS OF GEODESICS 0N THE ELLIPSOID WITH APPLICATION 10 | OF NESTED EQUATIONS" 11 | http://www.ngs.noaa.gov/PUBS_LIB/inverse.pdf 12 | ''' 13 | 14 | import math 15 | 16 | def direct(lon1,lat1,azimuth,distance): 17 | '''Calculates the postion P2 from azimuth and distance from P1 on the WGS84 ellipsoid. 18 | @param lon1,lat1: Position P1 in rads 19 | @param azimuth: Clockwise angle in rads relative to true north. 20 | @param distance: Distance in meters 21 | @return: Position P2 in rads (lon2,lat2) 22 | ''' 23 | 24 | phi1 = lat1 25 | alpha1 = azimuth 26 | 27 | a = 6378137.0 # length of major axis of the ellipsoid (radius at equator in meters) 28 | b = 6356752.314245 # length of minor axis of the ellipsoid (radius at the poles) 29 | f = 1/298.257223563 # flattening of the ellipsoid 30 | 31 | epsilon = 1e-12 32 | 33 | #U is 'reduced latitude' 34 | tanU1 = (1.0-f)*math.tan(phi1) 35 | cosU1 = 1/math.sqrt(1+tanU1*tanU1) 36 | sinU1 = tanU1*cosU1 37 | 38 | cosAlpha1 = math.cos(alpha1) 39 | sinAlpha1 = math.sin(alpha1) 40 | 41 | sigma1 = math.atan2(tanU1, cosAlpha1) # angular distance on sphere from equator to P1 along geodesic 42 | sinAlpha = cosU1*sinAlpha1 43 | cos2Alpha = 1.0-sinAlpha*sinAlpha 44 | 45 | u2 = cos2Alpha*(a*a-b*b)/(b*b) 46 | 47 | k1 = (math.sqrt(1.0+u2)-1.0)/(math.sqrt(1.0+u2)+1.0) 48 | A = (1.0+k1*k1/4.0)/(1.0-k1) 49 | B = k1*(1.0-3.0*k1*k1/8.0) 50 | 51 | sigma = distance/(b*A) 52 | 53 | while True: 54 | cos2Sigmam = math.cos(2.0*sigma1+sigma) 55 | sinSigma = math.sin(sigma) 56 | cosSigma = math.cos(sigma) 57 | 58 | deltaSigma = B*sinSigma*(cos2Sigmam+.25*B*(cosSigma*(-1.0+2.0*cos2Sigmam*cos2Sigmam)-(B/6.0)*cos2Sigmam*(-3.0+4.0*sinSigma*sinSigma)*(-3.0+4.0*cos2Sigmam*cos2Sigmam))) 59 | last_sigma = sigma 60 | sigma = (distance/(b*A))+deltaSigma 61 | if abs(last_sigma-sigma) <= epsilon: 62 | break 63 | 64 | cos2Sigmam = math.cos(2.0*sigma1+sigma) 65 | 66 | phi2 = math.atan2(sinU1*math.cos(sigma)+cosU1*math.sin(sigma)*cosAlpha1,(1-f)*math.sqrt(sinAlpha*sinAlpha+pow(sinU1*math.sin(sigma)-cosU1*math.cos(sigma)*cosAlpha1,2))) 67 | l = math.atan2(math.sin(sigma)*sinAlpha1,cosU1*math.cos(sigma)-sinU1*math.sin(sigma)*cosAlpha1) 68 | C = (f/16.0)*cos2Alpha*(4.0+f*(4.0-3.0*cos2Alpha)) 69 | L = l-(1.0-C)*f*sinAlpha*(sigma+C*math.sin(sigma)*(cos2Sigmam+C*math.cos(sigma)*(-1+2.0*cos2Sigmam*cos2Sigmam))) 70 | 71 | lat2 = phi2 72 | lon2 = lon1 + L 73 | return lon2,lat2 74 | 75 | def inverse(lon1,lat1,lon2,lat2): 76 | '''Calculates the azimuth and distance from P1 to P2 on the WGS84 ellipsoid. 77 | @param lon1,lat1: Position P1 in rads 78 | @param lon2,lat2: Position P2 in rads 79 | @return: azimuth in rads, distance in meters 80 | ''' 81 | 82 | if(lon1 == lon2 and lat1 == lat2): #short circuit if same points, avoids div by zero later 83 | return 0.0,0.0 84 | 85 | 86 | a = 6378137.0 # length of major axis of the ellipsoid (radius at equator in meters) 87 | b = 6356752.314245 # length of minor axis of the ellipsoid (radius at the poles) 88 | f = 1/298.257223563 # flattening of the ellipsoid 89 | 90 | epsilon = 1e-12 91 | 92 | phi1 = lat1 93 | phi2 = lat2 94 | 95 | L = lon2-lon1 96 | 97 | U1 = math.atan((1.0-f)*math.tan(phi1)) 98 | U2 = math.atan((1.0-f)*math.tan(phi2)) 99 | cosU1 = math.cos(U1) 100 | cosU2 = math.cos(U2) 101 | sinU1 = math.sin(U1) 102 | sinU2 = math.sin(U2) 103 | 104 | l = L 105 | last_l = None 106 | 107 | while True: 108 | cosl = math.cos(l) 109 | sinl = math.sin(l) 110 | 111 | sinSigma = math.sqrt(((cosU2*sinl)**2)+(cosU1*sinU2-sinU1*cosU2*cosl)**2) 112 | cosSigma = sinU1*sinU2+cosU1*cosU2*cosl 113 | sigma = math.atan2(sinSigma,cosSigma) 114 | try: 115 | sinAlpha = (cosU1*cosU2*sinl)/sinSigma 116 | except Exception as e: 117 | return 0,0 118 | 119 | cos2Alpha = 1-sinAlpha*sinAlpha 120 | if cos2Alpha == 0: 121 | cos2Sigmam = 0 122 | else: 123 | cos2Sigmam = cosSigma-((2.0*sinU1*sinU2)/cos2Alpha) 124 | 125 | if last_l is not None and abs(last_l - l) <= epsilon: 126 | break 127 | last_l = l 128 | 129 | C = (f/16.0)*cos2Alpha*(4.0+f*(4.0-3.0*cos2Alpha)) 130 | l = L+(1.0-C)*f*sinAlpha*(sigma+C*sinSigma*(cos2Sigmam+C*cosSigma*(-1.0+2.0*cos2Sigmam**2))) 131 | 132 | u2 = cos2Alpha*(a*a-b*b)/(b*b) 133 | k1 = (math.sqrt(1.0+u2)-1.0)/(math.sqrt(1.0+u2)+1.0) 134 | A = (1.0+k1*k1/4.0)/(1.0-k1) 135 | B = k1*(1.0-3.0*k1*k1/8.0) 136 | deltaSigma = B*sinSigma*(cos2Sigmam+.25*B*(cosSigma*(-1.0+2.0*cos2Sigmam*cos2Sigmam)-(B/6.0)*cos2Sigmam*(-3.0+4.0*sinSigma*sinSigma)*(-3.0+4.0*cos2Sigmam*cos2Sigmam))) 137 | s = b*A*(sigma-deltaSigma) 138 | alpha1 = math.atan2(cosU2*sinl,cosU1*sinU2-sinU1*cosU2*cosl) 139 | 140 | azimuth = alpha1 141 | if azimuth < 0.0: 142 | azimuth += math.radians(360.0) 143 | 144 | return azimuth,s 145 | 146 | -------------------------------------------------------------------------------- /project11/nav.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rclpy 4 | 5 | import rclpy.duration 6 | import rclpy.node 7 | from tf2_ros.buffer import Buffer 8 | from tf2_ros.transform_listener import TransformListener 9 | 10 | from geographic_msgs.msg import GeoPose 11 | from geographic_msgs.msg import GeoPointStamped 12 | from geometry_msgs.msg import PoseStamped 13 | from geometry_msgs.msg import PointStamped 14 | from geometry_msgs.msg import Quaternion 15 | from nav_msgs.msg import Odometry 16 | 17 | import project11.wgs84 18 | import project11.geodesic 19 | 20 | from tf2_geometry_msgs import do_transform_pose_stamped 21 | from tf2_geometry_msgs import do_transform_point 22 | 23 | from tf_transformations import quaternion_about_axis 24 | from tf_transformations import euler_from_quaternion 25 | 26 | import math 27 | import copy 28 | 29 | def distanceBearingDegrees(start_lat, start_lon, dest_lat, dest_lon): 30 | """ Returns distance and bearing of a geodesic line. 31 | 32 | Uses project11.geodesic library to determine bearing (degrees, NED) 33 | from start lat/lon to destination lat/lon. 34 | 35 | TODO: Write Args documentation. 36 | 37 | Returns: 38 | A float, bearing from start lat/lon to dest lat/lon, degrees, NED. 39 | """ 40 | start_lat_rad = math.radians(start_lat) 41 | start_lon_rad = math.radians(start_lon) 42 | 43 | dest_lat_rad = math.radians(dest_lat) 44 | dest_lon_rad = math.radians(dest_lon) 45 | 46 | path_azimuth, path_distance = project11.geodesic.inverse( start_lon_rad, start_lat_rad, dest_lon_rad, dest_lat_rad) 47 | return path_distance, math.degrees(path_azimuth) 48 | 49 | def headingToYaw(heading): 50 | """ Convert heading (degrees, NED) to yaw (degrees, ENU). 51 | 52 | Args: 53 | heading: float heading: degrees, NED 54 | 55 | Returns: 56 | Float, yaw: degrees, ENU 57 | """ 58 | return 90.0-heading 59 | 60 | def yawToHeading(yaw): 61 | """ Convert yaw (degrees, ENU) to heading (degrees, NED). 62 | 63 | Args: 64 | yaw: float yaw: degrees, ENU 65 | 66 | Returns: 67 | Float, heading: degrees, NED 68 | """ 69 | return 90.0-yaw 70 | 71 | def yawRadiansToQuaternionMsg(yaw): 72 | """ Convert yaw (radians, ENU) to a quaternion msg. 73 | 74 | Args: 75 | yaw: float yaw: degrees, ENU 76 | 77 | Returns: 78 | geometry_msgs/Quaternion 79 | """ 80 | ret = Quaternion() 81 | if yaw is not None: 82 | quat = quaternion_about_axis(yaw, (0,0,1)) 83 | ret.x = quat[0] 84 | ret.y = quat[1] 85 | ret.z = quat[2] 86 | ret.w = quat[3] 87 | return ret 88 | 89 | def headingToQuaternionMsg(heading): 90 | """ Convert heading (degrees, NED) to a quaternion msg. 91 | 92 | Args: 93 | heading: float heading: degrees, NED 94 | 95 | Returns: 96 | geometry_msgs/Quaternion 97 | """ 98 | ret = Quaternion() 99 | if heading is not None: 100 | yaw = math.radians(headingToYaw(heading)) 101 | else: 102 | yaw = None 103 | return yawRadiansToQuaternionMsg(yaw) 104 | 105 | def transformPointToGeoPoint(point, transform): 106 | """Transforms a Point or PointStamped to a GeoPointStamped using given transform to earth frame""" 107 | ps = point 108 | if not hasattr(point, 'header'): 109 | ps = PointStamped() 110 | ps.point = point 111 | ecef = do_transform_point(point, transform) 112 | latlon = project11.wgs84.fromECEFtoLatLong(ecef.point.x, ecef.point.y, ecef.point.z) 113 | gp = GeoPointStamped() 114 | gp.position.latitude = math.degrees(latlon[0]) 115 | gp.position.longitude = math.degrees(latlon[1]) 116 | gp.position.altitude = latlon[2] 117 | gp.header = copy.deepcopy(ps.header) 118 | gp.header.frame_id = 'wgs84' 119 | return gp 120 | 121 | def transformPoseToGeoPose(pose, transform): 122 | """Transforms a Pose to a GeoPose using given transform to earth frame""" 123 | gp = GeoPose() 124 | gp.position = transformPointToGeoPoint(pose.position, transform) 125 | gp.orientation = copy.deepcopy(pose.pose.orientation) 126 | return gp 127 | 128 | class EarthTransforms(object): 129 | """Help transform coordinates between Lat/Lon and map space""" 130 | def __init__(self, node: rclpy.node.Node, tf_buffer: Buffer = None, map_frame = None): 131 | 132 | if tf_buffer is None: 133 | self.tf_buffer = Buffer() 134 | self.listener = TransformListener(self.tf_buffer, node) 135 | else: 136 | self.tf_buffer = tf_buffer 137 | 138 | if map_frame is None: 139 | node.declare_parameter("map_frame", "map") 140 | self.map_frame = node.get_parameter("map_frame").get_parameter_value().string_value 141 | else: 142 | self.map_frame = map_frame 143 | self.node = node 144 | 145 | 146 | 147 | def geoToPose(self, lat, lon, heading=None, frame_id=None, time=rclpy.time.Time(), timeout=rclpy.duration.Duration()): 148 | """ Returns a PoseStamped in given frame from given geographic position. 149 | 150 | Args: 151 | lat: A float, latitude, degrees 152 | lon: A float, longitude, degrees 153 | heading: degrees, NED. 154 | frame_id: A string, target TF frame. 155 | 156 | """ 157 | 158 | if frame_id is None: 159 | frame_id = self.map_frame 160 | 161 | try: 162 | earth_to_frame = self.tf_buffer.lookup_transform(frame_id, "earth", time, timeout) 163 | except Exception as e: 164 | self.node.get_logger().error("mission_manager: Cannot lookup transform from to {}".format(frame_id)) 165 | self.node.get_logger().error(str(e)) 166 | return None 167 | 168 | ecef = project11.wgs84.toECEFfromDegrees(lat, lon) 169 | ecef_pose = PoseStamped() 170 | ecef_pose.pose.position.x = ecef[0] 171 | ecef_pose.pose.position.y = ecef[1] 172 | ecef_pose.pose.position.z = ecef[2] 173 | ret = do_transform_pose_stamped(ecef_pose, earth_to_frame) 174 | 175 | if heading is not None: 176 | ret.pose.orientation = headingToQuaternionMsg(heading) 177 | 178 | return ret 179 | 180 | def mapToEarthTransform(self, map_frame = None, timestamp=rclpy.time.Time()): 181 | if map_frame is None: 182 | map_frame = self.map_frame 183 | 184 | try: 185 | return self.tf_buffer.lookup_transform("earth", map_frame, timestamp) 186 | except Exception as e: 187 | self.node.get_logger().error("Cannot lookup transform from to {}".format(map_frame)) 188 | self.node.get_logger().error(e) 189 | 190 | def poseListToGeoPoseList(self, poses, map_frame = None): 191 | map_to_earth = self.mapToEarthTransform(map_frame) 192 | if map_to_earth is not None: 193 | ret = [] 194 | for p in poses: 195 | ret.append(transformPoseToGeoPose(p, map_to_earth)) 196 | return ret 197 | 198 | def pointListToGeoPointList(self, points, map_frame = None): 199 | """ Transforms a list of Point or PointStamped messages to GeoPointStamped 200 | 201 | The frame_id of a PointStamped will be used if not empty. 202 | The map_frame if not None, or the default map_frame will be used for Point 203 | messages and PointStamped messages with an empty frame_id. 204 | """ 205 | transforms = {} 206 | ret = [] 207 | default_time = rclpy.time.Time() 208 | for p in points: 209 | if hasattr(p, 'header'): 210 | transform_key = (p.header.frame_id, p.header.stamp) 211 | else: 212 | transform_key = ('', default_time) 213 | if not transform_key in transforms: 214 | frame_id = transform_key[0] 215 | if frame_id == '': 216 | frame_id = map_frame 217 | transforms[transform_key] = self.mapToEarthTransform(frame_id, transform_key[1]) 218 | if transforms[transform_key] is not None: 219 | ret.append(transformPointToGeoPoint(p, transforms[transform_key])) 220 | else: 221 | ret.append(None) 222 | return ret 223 | 224 | def pointToGeoPoint(self, point): 225 | try: 226 | map_to_earth = self.tfBuffer.lookup_transform("earth", point.header.frame_id, rclpy.time.Time()) 227 | except Exception as e: 228 | self.node.get_logger().error("Cannot lookup transform from to {}".format(point.header.frame_id)) 229 | self.node.get_logger().error(e) 230 | return None 231 | ecef = do_transform_point(point, map_to_earth) 232 | latlon = project11.wgs84.fromECEFtoLatLong(ecef.point.x, ecef.point.y, ecef.point.z) 233 | gp = GeoPointStamped() 234 | gp.position.latitude = math.degrees(latlon[0]) 235 | gp.position.longitude = math.degrees(latlon[1]) 236 | gp.position.altitude = latlon[2] 237 | gp.header = copy.deepcopy(point.header) 238 | gp.header.frame_id = 'wgs84' 239 | return gp 240 | 241 | 242 | 243 | 244 | 245 | class RobotNavigation(EarthTransforms): 246 | """Keeps track of robot's navigation state""" 247 | 248 | def __init__(self, node: rclpy.node.Node, tfBuffer = None): 249 | EarthTransforms.__init__(self, node, tfBuffer) 250 | self.odometry: Odometry = None 251 | 252 | self.odom_sub = node.create_subscription(Odometry, 'odom', self.odometryCallback, 1) 253 | 254 | 255 | def odometryCallback(self, msg: Odometry): 256 | """ Stores navigation Odometry as class attribute. 257 | 258 | Args: 259 | msg: A nav_msgs/Odometry message. 260 | """ 261 | self.odometry = msg 262 | if self.map_frame is None: 263 | self.map_frame = msg.header.frame_id 264 | 265 | def positionLatLon(self): 266 | """ Return last known position lat/lon of the vehicle. 267 | 268 | Position is determined as... 269 | 1. Use the frame_id value in the odometry message to lookup the 270 | tf transfrom from the "earth" frame to the frame_id. 271 | 2. Transform odometry.pose to ECEF frame 272 | 2. The wgs84.py module from project11 is used to transfrom 273 | ECEF -> lat/lon 274 | 275 | Returns: 276 | A tuple of length three with 277 | Lat/Long in radians and altitude in meters. 278 | """ 279 | if self.odometry is None: 280 | self.node.get_logger().warning("There is no current odometry, so can't determine position!", skip_first=True, throttle_duration_sec=2.0) 281 | return None 282 | 283 | try: 284 | odom_to_earth = self.tf_buffer.lookup_transform("earth", self.odometry.header.frame_id, rclpy.time.Time()) 285 | except Exception as e: 286 | self.node.get_logger().error("Cannot lookup transform from to odometry frame_id") 287 | self.node.get_logger().error(e) 288 | return None 289 | # Function from tf2_geoemetry_msgs 290 | ecef = do_transform_pose_stamped(self.odometry.pose, odom_to_earth).pose.position 291 | return project11.wgs84.fromECEFtoLatLong(ecef.x, ecef.y, ecef.z) 292 | 293 | def heading(self): 294 | """Uses current odometry message to return heading in degrees NED. 295 | 296 | TODO: This should not be a method of the object. Should be a general 297 | purpose function, probably in project11 module. 298 | Not specific to this program. 299 | 300 | Returns: 301 | A float for heading, degrees, NED. 302 | """ 303 | 304 | if self.odometry is not None: 305 | o = self.odometry.pose.pose.orientation 306 | q = (o.x, o.y, o.z, o.w) 307 | return 90.0-math.degrees(euler_from_quaternion(q)[2]) 308 | 309 | def distanceBearingTo(self, lat, lon): 310 | """ Returns distance from current vehicle position to given location. 311 | 312 | Uses position() function and lat/lon arguments to report 313 | distance in meters. 314 | 315 | Args: 316 | lat: A float, latitude, degrees 317 | lon: A float, longitude, degrees 318 | 319 | Returns: 320 | Tuple containing a float, distance in meters from current position (lat, lon) to lat, lon) 321 | and a float, bearing to target location, degrees, NED. 322 | 323 | """ 324 | p_rad = self.positionLatLon() 325 | if p_rad is None: 326 | return None, None 327 | current_lat_rad = p_rad[0] 328 | current_lon_rad = p_rad[1] 329 | target_lat_rad = math.radians(lat) 330 | target_lon_rad = math.radians(lon) 331 | azimuth, distance = project11.geodesic.inverse(current_lon_rad, current_lat_rad, target_lon_rad, target_lat_rad) 332 | return distance, math.degrees(azimuth) 333 | 334 | -------------------------------------------------------------------------------- /project11/wgs84.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import math 4 | 5 | a = 6378137.0 6 | b = 6356752.3142 7 | f = 0.0033528106647474805 #1.0/298.257223563 8 | w = 7292115e-11 9 | e2 = 0.006694380004260814 # 1.0-( 6356752.3142* 6356752.3142)/(6378137.0*6378137.0) 10 | ep2 = ((a*a)/(b*b))-1.0 11 | E2 = a*a-b*b 12 | 13 | def M(latitude): 14 | """ 15 | Meridional radius of curvature. 16 | Returns radius of curvature in north-south direction. 17 | latitude: Latitude in radians. 18 | """ 19 | return a*(1.0-e2)/pow((1.0-e2)*pow(math.sin(latitude),2.0),3.0/2.0); 20 | 21 | def N(latitude): 22 | """ 23 | Transverse radius of curvature. 24 | Returns radius of curvature in east-west direction. 25 | latitude: Latitude in radians. 26 | """ 27 | return a/math.sqrt(1-e2*pow(math.sin(latitude),2.0)) 28 | 29 | def toECEF(latitude, longitude, altitude=0.0): 30 | """ 31 | Returns ECEF coordinates as x,y,z in meters. 32 | latitude: Latitude in radians 33 | longitude: Longitude in radians 34 | altitude: Altitude in meters, defaults to 0 35 | """ 36 | n = N(latitude) 37 | cos_lat = math.cos(latitude) 38 | return (n+altitude)*cos_lat*math.cos(longitude), (n+altitude)*cos_lat*math.sin(longitude), (n*(1.0-e2)+altitude)*math.sin(latitude) 39 | 40 | def toECEFfromDegrees(latitude, longitude, altitude=0.0): 41 | """ 42 | Returns ECEF coordinates as x,y,z in meters. 43 | latitude: Latitude in degrees 44 | longitude: Longitude in degrees 45 | altitude: Altitude in meters, defaults to 0 46 | """ 47 | return toECEF(math.radians(latitude), math.radians(longitude), altitude) 48 | 49 | def fromECEFtoLatLong(x, y, z): 50 | """ 51 | Returns Lat/Long in radians and altitude in meters. 52 | x,y,z : ECEF coordinates in meters 53 | """ 54 | r = math.sqrt(x*x+y*y) 55 | F = 54*b*b*z*z 56 | G = r*r +(1.0-e2)*z*z-e2*E2 57 | C = (e2*e2*F*r*r)/(G*G*G) 58 | s = pow(1.0+C+math.sqrt(C*C+2*C),1.0/3.0) 59 | P = F/(3.0*pow((s+(1.0/s)+1.0),2.0)*G*G) 60 | Q = math.sqrt(1.0+2.0*e2*e2*P) 61 | r0 = (-(P*e2*r)/(1.0+Q))+math.sqrt((1.0/2.0)*a*a*(1.0+1.0/Q)-((P*(1-e2)*z*z)/(Q*(1.0+Q)))-(1.0/2.0)*P*r*r) 62 | U = math.sqrt(pow(r-e2*r0,2.0)+z*z) 63 | V = math.sqrt(pow(r-e2*r0,2.0)+(1.0-e2)*z*z) 64 | Z0 = b*b*z/(a*V) 65 | return math.atan((z+ep2*Z0)/r), math.atan2(y, x), U*(1.0-(b*b)/(a*V)) 66 | 67 | def fromECEFtoLatLongDegrees(x, y, z): 68 | """ 69 | Returns Lat/Long in degrees and altitude in meters. 70 | x,y,z : ECEF coordinates in meters 71 | """ 72 | ret = fromECEFtoLatLong(x, y, z) 73 | return math.degrees(ret[0]), math.degrees(ret[1]), ret[2] 74 | 75 | 76 | if __name__ == "__main__": 77 | import sys 78 | lat = float(sys.argv[1]) 79 | lon = float(sys.argv[2]) 80 | print (lat,lon) 81 | ecef = toECEFfromDegrees(lat,lon) 82 | print (ecef) 83 | lldeg = fromECEFtoLatLongDegrees(ecef[0],ecef[1],ecef[2]) 84 | print (lldeg) 85 | -------------------------------------------------------------------------------- /scripts/platform_send.py: -------------------------------------------------------------------------------- 1 | #!/bin/env python3 2 | 3 | from typing import Optional 4 | 5 | import rclpy 6 | from rclpy.executors import ExternalShutdownException 7 | from rclpy.executors import SingleThreadedExecutor 8 | 9 | from rclpy.lifecycle import Node 10 | from rclpy.lifecycle import Publisher 11 | from rclpy.lifecycle import State 12 | from rclpy.lifecycle import TransitionCallbackReturn 13 | from rclpy.timer import Timer 14 | 15 | from project11_msgs.msg import NavSource 16 | from project11_msgs.msg import Platform 17 | from project11_msgs.msg import PlatformList 18 | 19 | class PlatformPublisher(Node): 20 | def __init__(self, node_name, **kwargs): 21 | self._platform_publisher: Optional[Publisher] = None 22 | self._timer: Optional[Timer] = None 23 | super().__init__(node_name, **kwargs) 24 | 25 | 26 | def update_nav_source_parameters(self): 27 | nav_sources = self.get_parameter('nav.source_names').get_parameter_value().string_array_value 28 | for ns in nav_sources: 29 | for key, default in ( 30 | ('topics.orientation', ''), 31 | ('topics.position', ''), 32 | ('topics.velocity', ''), 33 | ('priority', 0) 34 | ): 35 | if not self.has_parameter('nav.sources.'+ns+'.'+key): 36 | self.declare_parameter('nav.sources.'+ns+'.'+key, default) 37 | 38 | 39 | def publish(self): 40 | self.update_nav_source_parameters() 41 | 42 | platform = Platform() 43 | platform.name = self.get_parameter('name').get_parameter_value().string_value 44 | platform.platform_namespace = self.get_parameter('platform_namespace').get_parameter_value().string_value 45 | platform.robot_description = self.get_parameter('robot_description').get_parameter_value().string_value 46 | nav_sources = self.get_parameter('nav.source_names').get_parameter_value().string_array_value 47 | for ns in nav_sources: 48 | if ns == '': 49 | continue 50 | nav_source = NavSource() 51 | nav_source.name = ns 52 | nav_source.position_topic = self.get_parameter('nav.sources.'+ns+'.topics.position').get_parameter_value().string_value 53 | nav_source.orientation_topic = self.get_parameter('nav.sources.'+ns+'.topics.orientation').get_parameter_value().string_value 54 | nav_source.velocity_topic = self.get_parameter('nav.sources.'+ns+'.topics.velocity').get_parameter_value().string_value 55 | nav_source.priority = self.get_parameter('nav.sources.'+ns+'.priority').get_parameter_value().integer_value 56 | platform.nav_sources.append(nav_source) 57 | platform.width = self.get_parameter('width').get_parameter_value().double_value 58 | platform.length = self.get_parameter('length').get_parameter_value().double_value 59 | platform.reference_x = self.get_parameter('reference_x').get_parameter_value().double_value 60 | platform.reference_y = self.get_parameter('reference_y').get_parameter_value().double_value 61 | platform.color.r = self.get_parameter('color.red').get_parameter_value().double_value 62 | platform.color.g = self.get_parameter('color.green').get_parameter_value().double_value 63 | platform.color.b = self.get_parameter('color.blue').get_parameter_value().double_value 64 | platform.color.a = self.get_parameter('color.alpha').get_parameter_value().double_value 65 | 66 | pl = PlatformList() 67 | pl.platforms.append(platform) 68 | self._platform_publisher.publish(pl) 69 | 70 | 71 | 72 | def on_configure(self, state: State) -> TransitionCallbackReturn: 73 | try: 74 | self.declare_parameter('name', 'robot') 75 | self.declare_parameter('platform_namespace', 'robot') 76 | self.declare_parameter('robot_description', 'robot_description') 77 | self.declare_parameter('width', 1.0) 78 | self.declare_parameter('length', 1.0) 79 | self.declare_parameter('reference_x', 0.5) 80 | self.declare_parameter('reference_y', 0.5) 81 | self.declare_parameter('color.red', 1.0) 82 | self.declare_parameter('color.green', 1.0) 83 | self.declare_parameter('color.blue', 1.0) 84 | self.declare_parameter('color.alpha', 1.0) 85 | 86 | self.declare_parameter('nav.source_names', ['',]) 87 | self.update_nav_source_parameters() 88 | 89 | self._platform_publisher = self.create_lifecycle_publisher(PlatformList, "/project11/platforms", 5) 90 | 91 | self._timer = self.create_timer(1.0, self.publish) 92 | except Exception as e: 93 | import traceback 94 | traceback.print_exc() 95 | return TransitionCallbackReturn.ERROR 96 | 97 | return TransitionCallbackReturn.SUCCESS 98 | 99 | def on_activate(self, state: State) -> TransitionCallbackReturn: 100 | return super().on_activate(state) 101 | 102 | 103 | def on_deactivate(self, state: State) -> TransitionCallbackReturn: 104 | return super().on_deactivate(state) 105 | 106 | def on_cleanup(self, state: State) -> TransitionCallbackReturn: 107 | self.destroy_timer(self._timer) 108 | self.destroy_publisher(self._platform_publisher) 109 | return TransitionCallbackReturn.SUCCESS 110 | 111 | def on_shutdown(self, state: State) -> TransitionCallbackReturn: 112 | self.destroy_timer(self._timer) 113 | self.destroy_publisher(self._platform_publisher) 114 | return TransitionCallbackReturn.SUCCESS 115 | 116 | 117 | def main(args=None): 118 | rclpy.init() 119 | executor = SingleThreadedExecutor() 120 | pp = PlatformPublisher('platform_publisher') 121 | executor.add_node(pp) 122 | try: 123 | executor.spin() 124 | except (KeyboardInterrupt, ExternalShutdownException): 125 | pass 126 | 127 | 128 | if __name__ == '__main__': 129 | main() 130 | -------------------------------------------------------------------------------- /src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "project11/utils.h" 2 | 3 | std::ostream& operator<< (std::ostream &out, const project11::LatLongDegrees &p) 4 | { 5 | out << std::setprecision (10) << "lat (deg): " << p.latitude() << ", lon (deg): " << p.longitude() << ", alt (m): " << p.altitude(); 6 | return out; 7 | } 8 | 9 | std::ostream& operator<< (std::ostream &out, const project11::ECEF &p) 10 | { 11 | out << "ECEF xyz(m): " << p.x() << ", " << p.y() << ", " << p.z(); 12 | return out; 13 | } 14 | 15 | std::ostream& operator<< (std::ostream &out, const project11::AngleDegrees &a) 16 | { 17 | out << a.value() << " degrees (" << project11::AngleRadians(a).value() << " radians)"; 18 | return out; 19 | } 20 | 21 | std::ostream& operator<< (std::ostream &out, const project11::AngleRadians &a) 22 | { 23 | out << a.value() << " radians (" << project11::AngleDegrees(a).value() << " degrees)"; 24 | return out; 25 | } 26 | --------------------------------------------------------------------------------