Implementation of snowsampler drone

.github/workflows/build_test.yml

@@ -0,0 +1,59 @@
+name: ROS1 Build Test
+on:
+  push:
+    branches:
+    - 'ros1'
+  pull_request:
+    branches:
+    - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        config:
+          - {rosdistro: 'noetic', container: 'ros:noetic-ros-base-focal'}
+    container: ${{ matrix.config.container }}
+    steps:
+    - uses: actions/checkout@v1
+      with:
+        token: ${{ secrets.ACCESS_TOKEN }}
+        github-token: ${{ secrets.GITHUB_TOKEN }}
+    - name: Install Dependencies
+      working-directory: 
+      env:
+        DEBIAN_FRONTEND: noninteractive
+      run: |
+        apt update
+        apt install -y python3-wstool python3-catkin-tools git
+      shell: bash
+    - name: Build Test
+      working-directory: 
+      env:
+        DEBIAN_FRONTEND: noninteractive
+      run: |
+        mkdir -p $HOME/catkin_ws/src;
+        cd $HOME/catkin_ws
+        catkin init
+        catkin config --extend "/opt/ros/${{matrix.config.rosdistro}}"
+        catkin config --merge-devel
+        cd $HOME/catkin_ws/src
+        ln -s $GITHUB_WORKSPACE
+        cd $HOME/catkin_ws
+        wstool init src src/adaptive-snowsampler/dependencies.rosinstall
+        wstool update -t src -j4
+        rosdep update
+        rosdep install --from-paths src --ignore-src -y --rosdistro ${{matrix.config.rosdistro}}
+        catkin config --cmake-args -DCMAKE_BUILD_TYPE=Release -DCATKIN_ENABLE_TESTING=False
+        catkin build -j$(nproc) -l$(nproc) adaptive_snowsampler
+    # - name: unit_tests
+    #   working-directory:
+    #   run: |
+    #     cd $HOME/catkin_ws/src
+    #     catkin config --cmake-args -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCATKIN_ENABLE_TESTING=True
+    #     catkin build grid_map_geo --no-deps -i --catkin-make-args tests
+    #     source $HOME/catkin_ws/devel/setup.bash
+    #     status=0 && for f in $HOME/catkin_ws/devel/lib/*/*-test; do $f || exit 1; done
+    #   shell: bash

.github/workflows/check_style.yml

@@ -0,0 +1,34 @@
+name: Style Checks
+
+on:
+  push:
+    branches:
+      - main
+  pull_request:
+    branches:
+      - '*'
+
+jobs:
+  build:
+    runs-on: ubuntu-latest
+    strategy:
+      fail-fast: false
+      matrix:
+        config:
+          - {rosdistro: 'humble', container: 'osrf/ros:humble-desktop'}
+    container: ${{ matrix.config.container }}
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        path: src/adaptive-snowsampler
+    - name: Install Dependencies with Rosdep
+      run: |
+        apt update
+        rosdep update
+        source /opt/ros/${{matrix.config.rosdistro}}/setup.bash
+        rosdep install --from-paths src --ignore-src -y --dependency-types test
+      shell: bash
+    - name: Check Code format
+      working-directory: src/adaptive-snowsampler/Tools
+      run: |
+        ./check_code_format.sh ..

.gitignore

@@ -0,0 +1,2 @@
+**/.vscode/
+adaptive_snowsampler/scripts/__pycache__/*

LICENSE

@@ -0,0 +1,28 @@
+BSD 3-Clause License
+
+Copyright (c) 2024, Jaeyoung Lim, Autonomous Systems Lab, ETH Zurich
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice, this
+   list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+   this list of conditions and the following disclaimer in the documentation
+   and/or other materials provided with the distribution.
+
+3. Neither the name of the copyright holder nor the names of its
+   contributors may be used to endorse or promote products derived from
+   this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

README.md

@@ -0,0 +1,84 @@
+<img align="right" height="60" src="https://user-images.githubusercontent.com/5248102/126074528-004a32b9-7911-486a-9e79-8b78e6e66fdc.png">
+
+# adaptive_snowsampler
+
+[![ROS1 Build Test](https://github.com/Jaeyoung-Lim/adaptive-snowsampler/actions/workflows/build_test.yml/badge.svg)](https://github.com/Jaeyoung-Lim/adaptive-snowsampler/actions/workflows/build_test.yml)
+
+This project includes an implementation of operating the snow sampler drone.
+
+> This work has been submitted to [ISSW 2024](https://www.issw2024.com/).
+
+![drone_in_snow](https://github.com/ethz-asl/adaptive-snowsampler/assets/5248102/acb43300-03ae-4160-b090-5c5f373461be)
+
+## Installation
+```
+cd ~
+mkdir -p catkin_ws/src
+cd catkin_ws/src
+git clone https://github.com/Jaeyoung-Lim/adaptive-snowsampler.git -b ros1
+git clone https://github.com/ethz-asl/grid_map_geo.git
+git clone https://github.com/ethz-asl/mav_comm.git
+git clone https://github.com/ethz-asl/terrain-navigation.git
+cd ..
+sudo rosdep init
+rosdep update
+rosdep install --from-paths src --ignore-src -y
+catkin build 
+
+
+# post building tasks
+echo 'ATTRS{idVendor}=="04d8", ATTRS{idProduct}=="fc5f", GROUP="dialout"' | sudo tee /etc/udev/rules.d/99-actuonix.rules #setting usb permissions
+sudo usermod -aG dialout user # add user to the dailout group
+sudo reboot # needed for the usb permissions to take effect
+
+# installation of GeographicLib dependency
+cd ~
+git clone https://github.com/mavlink/mavros.git
+sudo apt install geographiclib-tools libgeographic-dev
+sudo ~/mavros/mavros/scripts/install_geographiclib_datasets.sh 
+
+```
+
+## Running the code
+Run the code with the following launch file
+```
+source ~/ros2_ws/install/setup.bash
+ros2 launch adaptive_snowsampler launch.xml
+```
+
+## Testing with PX4 Software-In-The-Loop(SITL) simulation
+
+Set the Package Paths
+```
+cd ~/PX4-Autopilot/
+DONT_RUN=1 make px4_sitl_default gazebo-classic
+source ~/catkin_ws/devel/setup.bash    # (optional)
+source Tools/simulation/gazebo-classic/setup_gazebo.bash $(pwd) $(pwd)/build/px4_sitl_default
+export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
+export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/simulation/gazebo-classic/sitl_gazebo-classic
+```
+Set the takeoff location
+```
+export PX4_HOME_LAT=46.785479
+export PX4_HOME_LON=9.846803
+export PX4_HOME_ALT=2301.23
+
+```
+
+Run the node
+```
+roslaunch adaptive_snowsampler sitl_run.launch
+
+```
+
+## Running the ground station
+
+To control the vehicle from the ground, we need to connect to the ROS Master on the drone.
+Whereby its important that the IP is correct and that the drone and ground station are in the same zerotier network.
+Run rviz with the following command.
+```
+ROS_MASTER_URI=http://172.30.132.111:11311
+roslaunch snowsampler_rviz run.launch
+```
+
+![rviz](https://github.com/Jaeyoung-Lim/adaptive-snowsampler/assets/5248102/117a296d-01ad-4209-bec7-fb14267628e0)

Tools/check_code_format.sh

@@ -0,0 +1,10 @@
+#!/bin/bash
+
+# Fix style recursively in all the repository
+sh fix_code_style.sh ..
+
+# Print the diff with the remote branch (empty if no diff)
+git --no-pager diff -U0 --color
+
+# Check if there are changes, and failed
+if ! git diff-index --quiet HEAD --; then echo "Code style check failed, please run clang-format (e.g. with scripts/fix_code_style.sh)"; exit 1; fi

Tools/configure.sh

@@ -0,0 +1,30 @@
+#!/bin/bash
+
+set -e
+
+echo "This script will configure your system"
+
+if [ "$EUID" -ne 0 ]
+  then echo "Please run as root"
+  exit
+fi
+
+PACKAGE_PATH=/home/user/catkin_ws/src/adaptive-snowsampler
+
+# Configure systemd service
+echo "Copy systemd service"
+cp -vf $PACKAGE_PATH/systemd/adaptive-snowsampler.service /etc/systemd/system/
+cp -vf $PACKAGE_PATH/systemd/mavlink-router.service /etc/systemd/system/
+cp -vf $PACKAGE_PATH/systemd/rosbag-record.service /etc/systemd/system/
+cp -vf $PACKAGE_PATH/systemd/ssp-bridge.service /etc/systemd/system/
+cp -vf $PACKAGE_PATH/systemd/system-monitor.service /etc/systemd/system/
+
+systemctl enable adaptive-snowsampler.service
+systemctl enable mavlink-router.service
+systemctl enable ssp-bridge.service
+systemctl enable system-monitor.service
+
+sudo systemctl start adaptive-snowsampler.service
+sudo systemctl start mavlink-router.service
+sudo systemctl start ssp-bridge.service
+sudo systemctl start system-monitor.service

Tools/fix_code_style.sh

@@ -0,0 +1,21 @@
+#!/bin/bash
+
+STYLE="google"
+
+if [ "$#" -eq 0 ]; then
+    echo "Usage: $0 <src_file | dir>"
+    echo ""
+    echo "ERROR: At least one source file or one directory must be provided!"
+
+    exit 1
+fi
+
+for arg in "$@"
+do
+    if [ -f $arg ]; then
+        clang-format -i -style='{BasedOnStyle: google, ColumnLimit: 120}' $arg
+    elif [ -d $arg ]; then
+        find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs clang-format -i -style='{BasedOnStyle: google, ColumnLimit: 120}'
+        find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs chmod 644
+    fi
+done

adaptive_snowsampler/CMakeLists.txt

@@ -0,0 +1,56 @@
+cmake_minimum_required(VERSION 3.8)
+project(adaptive_snowsampler)
+add_definitions(-std=c++17)
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
+
+
+# find dependencies
+find_package(Eigen3)
+find_package(GeographicLib REQUIRED)
+include(CheckGeographicLibDatasets)
+
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  tf
+  grid_map_geo
+  grid_map_ros
+  grid_map_geo_msgs
+  eigen_catkin
+  interactive_markers
+  planner_msgs
+  snowsampler_msgs
+)
+
+execute_process(
+    COMMAND "${PYTHON_EXECUTABLE}" -m pip install --no-warn-script-location -r "${CMAKE_CURRENT_SOURCE_DIR}/requirements.txt"
+    RESULT_VARIABLE _pip_install_result
+    OUTPUT_VARIABLE _pip_install_output
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES adaptive_snowsampler
+  CATKIN_DEPENDS roscpp
+)
+
+include_directories(
+  include
+  ${catkin_INCLUDE_DIRS}
+  ${Eigen_INCLUDE_DIRS}
+  ${GeographicLib_INCLUDE_DIRS}
+)
+
+catkin_install_python(PROGRAMS 
+  scripts/snowsampler_lac.py 
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
+
+include_directories(
+  include
+)
+
+add_executable(adaptive_snowsampler
+  src/main.cpp
+  src/adaptive_snowsampler.cpp)
+target_link_libraries(adaptive_snowsampler ${catkin_LIBRARIES} ${planner_msgs_TARGETS} ${GeographicLib_LIBRARIES})
+add_dependencies(adaptive_snowsampler ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

adaptive_snowsampler/cmake/CheckGeographicLibDatasets.cmake

@@ -0,0 +1,27 @@
+##
+# This module verifies the installation of the GeographicLib datasets and warns
+# if it doesn't detect them.
+##
+
+find_path(GEOGRAPHICLIB_GEOID_PATH NAMES geoids PATH_SUFFIXES share/GeographicLib share/geographiclib)
+find_path(GEOGRAPHICLIB_GRAVITY_PATH_ NAMES gravity PATH_SUFFIXES share/GeographicLib)
+find_path(GEOGRAPHICLIB_MAGNETIC_PATH_ NAMES magnetic PATH_SUFFIXES share/GeographicLib)
+
+if(NOT GEOGRAPHICLIB_GEOID_PATH)
+  message(STATUS "No geoid model datasets found. This will result on a SIGINT! Please execute the script install_geographiclib_dataset.sh in /mavros/scripts")
+else()
+  message(STATUS "Geoid model datasets found in: " ${GEOGRAPHICLIB_GEOID_PATH}/geoid)
+  set(GEOGRAPHICLIB_GEOID_PATH ${GEOGRAPHICLIB_GEOID_PATH}/geoid)
+endif()
+if(NOT GEOGRAPHICLIB_GRAVITY_PATH_)
+  message(STATUS "No gravity field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts")
+else()
+  message(STATUS "Gravity Field model datasets found in: " ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity)
+  set(GEOGRAPHICLIB_GRAVITY_PATH ${GEOGRAPHICLIB_GRAVITY_PATH_}/gravity)
+endif()
+if(NOT GEOGRAPHICLIB_MAGNETIC_PATH_)
+  message(STATUS "No magnetic field model datasets found. Please execute the script install_geographiclib_dataset.sh in /mavros/scripts")
+else()
+  message(STATUS "Magnetic Field model datasets found in: " ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic)
+  set(GEOGRAPHICLIB_MAGNETIC_PATH ${GEOGRAPHICLIB_MAGNETIC_PATH_}/magnetic)
+endif()

adaptive_snowsampler/cmake/FindGeographicLib.cmake

@@ -0,0 +1,18 @@
+# Look for GeographicLib
+#
+# Set
+#  GEOGRAPHICLIB_FOUND = TRUE
+#  GeographicLib_INCLUDE_DIRS = /usr/local/include
+#  GeographicLib_LIBRARIES = /usr/local/lib/libGeographic.so
+#  GeographicLib_LIBRARY_DIRS = /usr/local/lib
+
+find_path (GeographicLib_INCLUDE_DIRS NAMES GeographicLib/Config.h)
+
+find_library (GeographicLib_LIBRARIES NAMES Geographic)
+
+include (FindPackageHandleStandardArgs)
+find_package_handle_standard_args (GeographicLib DEFAULT_MSG
+  GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)
+mark_as_advanced (GeographicLib_LIBRARIES GeographicLib_INCLUDE_DIRS)
+
+#message(WARNING "GL: F:${GeographicLib_FOUND} L:${GeographicLib_LIBRARIES} I:${GeographicLib_INCLUDE_DIRS}")