diff --git a/.github/workflows/build_test.yml b/.github/workflows/build_test.yml new file mode 100644 index 0000000..7e7f7c3 --- /dev/null +++ b/.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 diff --git a/.github/workflows/check_style.yml b/.github/workflows/check_style.yml new file mode 100644 index 0000000..7f6339a --- /dev/null +++ b/.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 .. diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..23ae519 --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +**/.vscode/ +adaptive_snowsampler/scripts/__pycache__/* diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..5d1a66c --- /dev/null +++ b/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. diff --git a/README.md b/README.md new file mode 100644 index 0000000..67a1bd0 --- /dev/null +++ b/README.md @@ -0,0 +1,84 @@ + + +# 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) diff --git a/Tools/check_code_format.sh b/Tools/check_code_format.sh new file mode 100755 index 0000000..7ac8c29 --- /dev/null +++ b/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 diff --git a/Tools/configure.sh b/Tools/configure.sh new file mode 100755 index 0000000..f9cac56 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh new file mode 100755 index 0000000..ccb0b27 --- /dev/null +++ b/Tools/fix_code_style.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +STYLE="google" + +if [ "$#" -eq 0 ]; then + echo "Usage: $0 " + 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 diff --git a/adaptive_snowsampler/CMakeLists.txt b/adaptive_snowsampler/CMakeLists.txt new file mode 100644 index 0000000..2c231a9 --- /dev/null +++ b/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}) diff --git a/adaptive_snowsampler/cmake/CheckGeographicLibDatasets.cmake b/adaptive_snowsampler/cmake/CheckGeographicLibDatasets.cmake new file mode 100644 index 0000000..561ff59 --- /dev/null +++ b/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() diff --git a/adaptive_snowsampler/cmake/FindGeographicLib.cmake b/adaptive_snowsampler/cmake/FindGeographicLib.cmake new file mode 100644 index 0000000..7cc8e34 --- /dev/null +++ b/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}") diff --git a/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h b/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h new file mode 100644 index 0000000..83bdac2 --- /dev/null +++ b/adaptive_snowsampler/include/adaptive_snowsampler/adaptive_snowsampler.h @@ -0,0 +1,269 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * 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 PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file px4_avoidance.cpp + * + * px4 manipulation + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "snowsampler_msgs/SetAngle.h" +#include "snowsampler_msgs/TakeMeasurement.h" +#include "snowsampler_msgs/Trigger.h" + +// #include "px4_msgs/msg/distance_sensor.hpp" +// #include "px4_msgs/msg/vehicle_attitude.hpp" +// #include "px4_msgs/msg/vehicle_command.hpp" +// #include "px4_msgs/msg/vehicle_global_position.hpp" + +enum SSPState { + Error, + Ready_To_Measure, + Taking_Measurement, + Stopped_No_Home, + Going_Home, + Moving, + Position_Not_Reached, + ENUM_LENGTH +}; + +using namespace std::chrono_literals; + +/* This example creates a subclass of Node and uses std::bind() to register a + * member function as a callback from the timer. */ + +class AdaptiveSnowSampler { + public: + AdaptiveSnowSampler(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private); + + private: + /** + * @brief Status loop for running decisions + * + */ + void statusloopCallback(const ros::TimerEvent &event); + + /** + * @brief Status loop for running decisions + * + */ + void cmdloopCallback(const ros::TimerEvent &event); + + /** + * @brief Callback for vehicle attitude + * + * @param msg + */ + void vehicleAttitudeCallback(const geometry_msgs::PoseStamped &msg); + + /** + * @brief Callback for vehicle local position + * + * @param msg + */ + void sspStateCallback(const std_msgs::Int8::ConstPtr msg); + + void vehicleGlobalPositionCallback(const sensor_msgs::NavSatFix &msg); + + // void distanceSensorCallback(const px4_msgs::DistanceSensor &msg); + + bool startPositionCallback(planner_msgs::SetVector3::Request &request, planner_msgs::SetVector3::Response &response); + + bool goalPositionCallback(planner_msgs::SetVector3::Request &request, planner_msgs::SetVector3::Response &response); + + bool takeoffCallback(planner_msgs::SetService::Request &request, planner_msgs::SetService::Response &response); + + bool landCallback(planner_msgs::SetService::Request &request, planner_msgs::SetService::Response &response); + + bool gotoCallback(planner_msgs::SetService::Request &request, planner_msgs::SetService::Response &response); + + bool returnCallback(planner_msgs::SetService::Request &request, planner_msgs::SetService::Response &response); + + void callSetAngleService(double angle); + + bool takeMeasurementCallback(snowsampler_msgs::Trigger::Request &request, + snowsampler_msgs::Trigger::Response &response); + + void publishTargetNormal(const ros::Publisher &pub, const Eigen::Vector3d &position, const Eigen::Vector3d &normal); + + void publishSetpointPosition(const ros::Publisher &pub, const Eigen::Vector3d &position, + const Eigen::Vector3d color = Eigen::Vector3d(1.0, 1.0, 0.0)); + + void publishPositionHistory(const ros::Publisher &pub, const Eigen::Vector3d &position, + std::vector &history_vector); + + void writeLog(); + + void loadMap(); + void publishMap(); + + void tiltCheckCallback(const ros::TimerEvent &event); + + /** + * @brief Convert 3D vector into arrow marker + * + * @param position position of the root of the arrow + * @param normal arrow vector from the root position + * @param id marker id + * @param color color of the marker + * @param marker_namespace + * @return visualization_msgs::Marker + */ + visualization_msgs::Marker vector2ArrowsMsg(const Eigen::Vector3d &position, const Eigen::Vector3d &normal, int id, + Eigen::Vector3d color, const std::string marker_namespace = "arrow"); + /** + * @brief Convert 3D vector into arrow marker + * + * @param position + * @param normal + * @param id + * @param color + * @param marker_namespace + * @return visualization_msgs::Marker + */ + visualization_msgs::Marker position2SphereMsg(const Eigen::Vector3d &position, int id, Eigen::Vector3d color, + const std::string marker_namespace = "sphere"); + geometry_msgs::PoseStamped vector3d2PoseStampedMsg(const Eigen::Vector3d position, const Eigen::Vector4d orientation); + + ros::NodeHandle nh_; + ros::NodeHandle nh_private_; + + ros::Timer statusloop_timer_; + ros::Timer cmdloop_timer_; + ros::Timer measurement_tilt_timer_; + + ros::CallbackQueue cmdloop_queue_; + ros::CallbackQueue statusloop_queue_; + ros::CallbackQueue measurementloop_queue_; + std::unique_ptr cmdloop_spinner_; + std::unique_ptr statusloop_spinner_; + std::unique_ptr measurementloop_spinner_; + + ros::Publisher original_map_pub_; + ros::Publisher target_normal_pub_; + ros::Publisher setpoint_position_pub_; + ros::Publisher home_position_pub_; + ros::Publisher target_slope_pub_; + ros::Publisher vehicle_command_pub_; + ros::Publisher referencehistory_pub_; + ros::Publisher snow_depth_pub_; + ros::Publisher vehicle_pose_pub_; + ros::Publisher map_info_pub_; + + ros::Subscriber vehicle_attitude_sub_; + ros::Subscriber vehicle_global_position_sub_; + ros::Subscriber ssp_status_sub_; + ros::Subscriber distance_sensor_sub_; + + ros::ServiceServer setgoal_serviceserver_; + ros::ServiceServer setstart_serviceserver_; + ros::ServiceServer takeoff_serviceserver_; + ros::ServiceServer land_serviceserver_; + ros::ServiceServer goto_serviceserver_; + ros::ServiceServer return_serviceserver_; + ros::ServiceServer measurement_serviceserver_; + + ros::ServiceClient ssp_measurement_serviceclient_; + ros::ServiceClient mavcmd_long_service_client_; + ros::ServiceClient mavcmd_int_service_client_; + + std::unique_ptr tf_broadcaster_; + std::unique_ptr map_tf_broadcaster_; + + Eigen::Vector3d vehicle_position_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + Eigen::Vector3d lv03_vehicle_position_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + Eigen::Vector3d map_origin_{Eigen::Vector3d{0.0, 0.0, 0.0}}; + std::vector positionhistory_vector_; + Eigen::Quaterniond vehicle_attitude_{Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)}; + boost::circular_buffer vehicle_attitude_buffer_{20}; + Eigen::Vector3d vehicle_attitude_filtered_ref_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + + bool map_initialized_{false}; + std::string file_path_; + std::string color_path_; + std::string frame_id_; + + // Planner states + Eigen::Vector3d target_position_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + Eigen::Vector3d target_normal_{Eigen::Vector3d(0.0, 0.0, 1.0)}; + Eigen::Vector3d setpoint_positon_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + Eigen::Vector3d start_position_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + Eigen::Vector3d home_position_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + Eigen::Vector3d home_normal_{Eigen::Vector3d(0.0, 0.0, 0.0)}; + + // tilt prevention parameters + double tilt_treshold_{0.035}; // ~2deg + double tilt_window_size_{3}; + bool tilt_prevention_{false}; + + double target_heading_{0.0}; + double target_slope_{0.0}; + double home_heading_{0.0}; + double home_slope_{0.0}; + const float neutral_angle_ = 35; + double relative_altitude_ = 20.0; + std::shared_ptr map_; + std::shared_ptr egm96_5; + bool global_position_received_{false}; + double snow_depth_; + double lidar_distance_; + SSPState sspState_ = SSPState::Ready_To_Measure; + int sspLogId_ = 1; + std::ofstream sspLogfile_; + std::string sspLogfilePath_; +}; diff --git a/adaptive_snowsampler/include/adaptive_snowsampler/geo_conversions.h b/adaptive_snowsampler/include/adaptive_snowsampler/geo_conversions.h new file mode 100644 index 0000000..9e12b89 --- /dev/null +++ b/adaptive_snowsampler/include/adaptive_snowsampler/geo_conversions.h @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2023 Jaeyoung Lim, Autonomous Systems Lab, + * ETH Zürich. All rights reserved. + * + * 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 OWNER 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. + * + ****************************************************************************/ + +#ifndef GEOCONVERSIONS_H +#define GEOCONVERSIONS_H + +#include + +class GeoConversions { + public: + GeoConversions(); + virtual ~GeoConversions(); + + /** + * @brief Convert WGS84 (LLA) to LV03/CH1903 + * + * @param lat latitude (degrees) WGS84 + * @param lon lontitude (degrees) WGS84 + * @param alt Altitude WGS84 + * @param x + * @param y + * @param h + */ + static void forward(const double lat, const double lon, const double alt, double &y, double &x, double &h) { + // 1. Convert the ellipsoidal latitudes φ and longitudes λ into arcseconds ["] + const double lat_arc = lat * 3600.0; + const double lon_arc = lon * 3600.0; + + // 2. Calculate the auxiliary values (differences of latitude and longitude relative to Bern in the unit [10000"]): + // φ' = (φ – 169028.66 ")/10000 + // λ' = (λ – 26782.5 ")/10000 + const double lat_aux = (lat_arc - 169028.66) / 10000.0; + const double lon_aux = (lon_arc - 26782.5) / 10000.0; + + // 3. Calculate projection coordinates in LV95 (E, N, h) or in LV03 (y, x, h) + // E [m] = 2600072.37 + 211455.93 * λ' - 10938.51 * λ' * φ' - 0.36 * λ' * φ'2 - 44.54 * λ'3 + // y [m] = E – 2000000.00 N [m] = 1200147.07 + 308807.95 * φ' + 3745.25 * λ'2 + 76.63 * φ'2 - 194.56 * λ'2 * φ' + + // 119.79 * φ'3 x [m] = N – 1000000.00 + // hCH [m] =hWGS – 49.55 + 2.73 * λ' + 6.94 * φ' + const double E = 2600072.37 + 211455.93 * lon_aux - 10938.51 * lon_aux * lat_aux - + 0.36 * lon_aux * std::pow(lat_aux, 2) - 44.54 * std::pow(lon_aux, 3); + y = E - 2000000.00; + const double N = 1200147.07 + 308807.95 * lat_aux + 3745.25 * std::pow(lon_aux, 2) + 76.63 * std::pow(lat_aux, 2) - + 194.56 * std::pow(lon_aux, 2) * lat_aux + 119.79 * std::pow(lat_aux, 3); + x = N - 1000000.00; + + h = alt - 49.55 + 2.73 * lon_aux + 6.94 * lat_aux; + }; + + /** + * @brief LV03/CH1903 to Convert WGS84 (LLA) + * + * @param x + * @param y + * @param h + * @param lat latitude + * @param lon longitude + * @param alt altitude + */ + static void reverse(const double y, const double x, const double h, double &lat, double &lon, double &alt) { + // 1. Convert the projection coordinates E (easting) and N (northing) in LV95 (or y / x in LV03) into the civilian + // system (Bern = 0 / 0) and express in the unit [1000 km]: E' = (E – 2600000 m)/1000000 = (y – 600000 m)/1000000 + // N' = (N – 1200000 m)/1000000 = (x – 200000 m)/1000000 + const double y_aux = (y - 600000.0) / 1000000.0; + const double x_aux = (x - 200000.0) / 1000000.0; + + // 2. Calculate longitude λ and latitude φ in the unit [10000"]: + // λ' = 2.6779094 + 4.728982 * y' + 0.791484* y' * x' + 0.1306 * y' * x'2 - 0.0436 * y'3 + // φ' = 16.9023892 + 3.238272 * x' - 0.270978 * y'2 - 0.002528 * x'2 - 0.0447 * y'2 * x' - 0.0140 * x'3 + // hWGS [m] = hCH + 49.55 - 12.60 * y' - 22.64 * x' + const double lon_aux = 2.6779094 + 4.728982 * y_aux + 0.791484 * y_aux * x_aux + + 0.1306 * y_aux * std::pow(x_aux, 2) - 0.0436 * std::pow(y_aux, 3); + const double lat_aux = 16.9023892 + 3.238272 * x_aux - 0.270978 * std::pow(y_aux, 2) - + 0.002528 * std::pow(x_aux, 2) - 0.0447 * std::pow(y_aux, 2) * x_aux - + 0.0140 * std::pow(x_aux, 3); + alt = h + 49.55 - 12.60 * y_aux - 22.64 * x_aux; + + lon = lon_aux * 100.0 / 36.0; + lat = lat_aux * 100.0 / 36.0; + }; + + private: +}; + +#endif diff --git a/adaptive_snowsampler/include/adaptive_snowsampler/visualization.h b/adaptive_snowsampler/include/adaptive_snowsampler/visualization.h new file mode 100644 index 0000000..e7fb16f --- /dev/null +++ b/adaptive_snowsampler/include/adaptive_snowsampler/visualization.h @@ -0,0 +1,95 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * 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 PX4 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 OWNER 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. + * + ****************************************************************************/ + +#ifndef ADAPTIVE_SNOWSAMPLER_VISUALIZATION_H +#define ADAPTIVE_SNOWSAMPLER_VISUALIZATION_H + +#include +#include +#include + +#include + +inline geometry_msgs::Pose vector3d2PoseMsg(const Eigen::Vector3d position, const Eigen::Quaterniond orientation) { + geometry_msgs::Pose encode_msg; + + encode_msg.orientation.w = orientation.w(); + encode_msg.orientation.x = orientation.x(); + encode_msg.orientation.y = orientation.y(); + encode_msg.orientation.z = orientation.z(); + encode_msg.position.x = position(0); + encode_msg.position.y = position(1); + encode_msg.position.z = position(2); + return encode_msg; +} + +inline void publishVehiclePose(const ros::Publisher pub, const Eigen::Vector3d& position, + const Eigen::Quaterniond& attitude) { + Eigen::Quaterniond mesh_attitude = + attitude * Eigen::Quaterniond(std::cos(1.5 * M_PI / 2), 0.0, 0.0, std::sin(1.5 * M_PI / 2)); + geometry_msgs::Pose vehicle_pose = vector3d2PoseMsg(position, mesh_attitude); + visualization_msgs::MarkerArray marker_array; + visualization_msgs::Marker marker; + marker.header.stamp = ros::Time::now(); + marker.header.frame_id = "map"; + marker.type = visualization_msgs::Marker::MESH_RESOURCE; + marker.ns = "body"; + marker.mesh_resource = "package://adaptive_snowsampler/resources/snowsampler.stl"; + marker.scale.x = 0.005; + marker.scale.y = 0.005; + marker.scale.z = 0.005; + marker.color.a = 1.0; // Don't forget to set the alpha! + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.pose = vehicle_pose; + marker_array.markers.push_back(marker); + visualization_msgs::Marker leg_marker; + leg_marker.header.stamp = ros::Time::now(); + leg_marker.header.frame_id = "map"; + leg_marker.type = visualization_msgs::Marker::MESH_RESOURCE; + leg_marker.ns = "leg"; + leg_marker.mesh_resource = "package://adaptive_snowsampler/resources/snowsampler_legs.stl"; + leg_marker.scale.x = 0.005; + leg_marker.scale.y = 0.005; + leg_marker.scale.z = 0.005; + leg_marker.color.a = 1.0; // Don't forget to set the alpha! + leg_marker.color.r = 1.0; + leg_marker.color.g = 1.0; + leg_marker.color.b = 0.0; + leg_marker.pose = vehicle_pose; + marker_array.markers.push_back(leg_marker); + pub.publish(marker_array); +} + +#endif diff --git a/adaptive_snowsampler/launch/replay_bag.launch b/adaptive_snowsampler/launch/replay_bag.launch new file mode 100644 index 0000000..a68a447 --- /dev/null +++ b/adaptive_snowsampler/launch/replay_bag.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/adaptive_snowsampler/launch/run.launch b/adaptive_snowsampler/launch/run.launch new file mode 100644 index 0000000..d4c285d --- /dev/null +++ b/adaptive_snowsampler/launch/run.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/adaptive_snowsampler/launch/run_rosbag.launch b/adaptive_snowsampler/launch/run_rosbag.launch new file mode 100644 index 0000000..9790be8 --- /dev/null +++ b/adaptive_snowsampler/launch/run_rosbag.launch @@ -0,0 +1,11 @@ + + + + diff --git a/adaptive_snowsampler/launch/sitl_run.launch b/adaptive_snowsampler/launch/sitl_run.launch new file mode 100644 index 0000000..cacd6cc --- /dev/null +++ b/adaptive_snowsampler/launch/sitl_run.launch @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/adaptive_snowsampler/package.xml b/adaptive_snowsampler/package.xml new file mode 100644 index 0000000..e139ce7 --- /dev/null +++ b/adaptive_snowsampler/package.xml @@ -0,0 +1,30 @@ + + adaptive_snowsampler + 0.0.0 + Package for autonomous adpative snow sampling + jaeyoung + BSD-3 + + catkin + roscpp + rospy + std_msgs + nav_msgs + sensor_msgs + mavros + mavros_extras + mavros_msgs + eigen_catkin + grid_map_geo + grid_map_geo_msgs + grid_map_ros + eigen_catkin + snowsampler_msgs + roscpp + rospy + planner_msgs + tf2_ros + + + + diff --git a/adaptive_snowsampler/requirements.txt b/adaptive_snowsampler/requirements.txt new file mode 100644 index 0000000..0c1d993 --- /dev/null +++ b/adaptive_snowsampler/requirements.txt @@ -0,0 +1,3 @@ +rospy +numpy +pyusb \ No newline at end of file diff --git a/adaptive_snowsampler/resources/Braemabuehl_1m_V2.tif b/adaptive_snowsampler/resources/Braemabuehl_1m_V2.tif new file mode 100644 index 0000000..d222f8f Binary files /dev/null and b/adaptive_snowsampler/resources/Braemabuehl_1m_V2.tif differ diff --git a/adaptive_snowsampler/resources/Braemabuehl_1m_V2_color.tif b/adaptive_snowsampler/resources/Braemabuehl_1m_V2_color.tif new file mode 100644 index 0000000..64676c8 Binary files /dev/null and b/adaptive_snowsampler/resources/Braemabuehl_1m_V2_color.tif differ diff --git a/adaptive_snowsampler/resources/Braemabuehl_upper_1m.tif b/adaptive_snowsampler/resources/Braemabuehl_upper_1m.tif new file mode 100644 index 0000000..152ff98 Binary files /dev/null and b/adaptive_snowsampler/resources/Braemabuehl_upper_1m.tif differ diff --git a/adaptive_snowsampler/resources/Braemabuehl_upper_1m_color.tif b/adaptive_snowsampler/resources/Braemabuehl_upper_1m_color.tif new file mode 100644 index 0000000..f18bc48 Binary files /dev/null and b/adaptive_snowsampler/resources/Braemabuehl_upper_1m_color.tif differ diff --git a/adaptive_snowsampler/resources/braemabuel.tif b/adaptive_snowsampler/resources/braemabuel.tif new file mode 100644 index 0000000..bf74429 Binary files /dev/null and b/adaptive_snowsampler/resources/braemabuel.tif differ diff --git a/adaptive_snowsampler/resources/braemabuel_1m_crop.tif b/adaptive_snowsampler/resources/braemabuel_1m_crop.tif new file mode 100644 index 0000000..ebd447b Binary files /dev/null and b/adaptive_snowsampler/resources/braemabuel_1m_crop.tif differ diff --git a/adaptive_snowsampler/resources/braemabuel_1m_crop_color.tif b/adaptive_snowsampler/resources/braemabuel_1m_crop_color.tif new file mode 100644 index 0000000..0275c92 Binary files /dev/null and b/adaptive_snowsampler/resources/braemabuel_1m_crop_color.tif differ diff --git a/adaptive_snowsampler/resources/braemabuel_2m_crop.tif b/adaptive_snowsampler/resources/braemabuel_2m_crop.tif new file mode 100644 index 0000000..6259614 Binary files /dev/null and b/adaptive_snowsampler/resources/braemabuel_2m_crop.tif differ diff --git a/adaptive_snowsampler/resources/braemabuel_2m_crop_color.tif b/adaptive_snowsampler/resources/braemabuel_2m_crop_color.tif new file mode 100644 index 0000000..75a9163 Binary files /dev/null and b/adaptive_snowsampler/resources/braemabuel_2m_crop_color.tif differ diff --git a/adaptive_snowsampler/resources/braemabuel_color.tif b/adaptive_snowsampler/resources/braemabuel_color.tif new file mode 100644 index 0000000..ed25780 Binary files /dev/null and b/adaptive_snowsampler/resources/braemabuel_color.tif differ diff --git a/adaptive_snowsampler/resources/eth_zentrum.tif b/adaptive_snowsampler/resources/eth_zentrum.tif new file mode 100644 index 0000000..6683596 Binary files /dev/null and b/adaptive_snowsampler/resources/eth_zentrum.tif differ diff --git a/adaptive_snowsampler/resources/eth_zentrum_color.tif b/adaptive_snowsampler/resources/eth_zentrum_color.tif new file mode 100644 index 0000000..cb6b2ed Binary files /dev/null and b/adaptive_snowsampler/resources/eth_zentrum_color.tif differ diff --git a/adaptive_snowsampler/resources/hinwil.tif b/adaptive_snowsampler/resources/hinwil.tif new file mode 100644 index 0000000..06ec2b2 Binary files /dev/null and b/adaptive_snowsampler/resources/hinwil.tif differ diff --git a/adaptive_snowsampler/resources/hinwil_color.tif b/adaptive_snowsampler/resources/hinwil_color.tif new file mode 100644 index 0000000..5276604 Binary files /dev/null and b/adaptive_snowsampler/resources/hinwil_color.tif differ diff --git a/adaptive_snowsampler/resources/hoenggerberg.tif b/adaptive_snowsampler/resources/hoenggerberg.tif new file mode 100644 index 0000000..eb826c1 Binary files /dev/null and b/adaptive_snowsampler/resources/hoenggerberg.tif differ diff --git a/adaptive_snowsampler/resources/hoenggerberg_3m.tif b/adaptive_snowsampler/resources/hoenggerberg_3m.tif new file mode 100644 index 0000000..8adf263 Binary files /dev/null and b/adaptive_snowsampler/resources/hoenggerberg_3m.tif differ diff --git a/adaptive_snowsampler/resources/hoenggerberg_3m_color.tif b/adaptive_snowsampler/resources/hoenggerberg_3m_color.tif new file mode 100644 index 0000000..f26f090 Binary files /dev/null and b/adaptive_snowsampler/resources/hoenggerberg_3m_color.tif differ diff --git a/adaptive_snowsampler/resources/hoenggerberg_color.tif b/adaptive_snowsampler/resources/hoenggerberg_color.tif new file mode 100644 index 0000000..355288f Binary files /dev/null and b/adaptive_snowsampler/resources/hoenggerberg_color.tif differ diff --git a/adaptive_snowsampler/resources/prodkamm_1m.tif b/adaptive_snowsampler/resources/prodkamm_1m.tif new file mode 100644 index 0000000..83e00fb Binary files /dev/null and b/adaptive_snowsampler/resources/prodkamm_1m.tif differ diff --git a/adaptive_snowsampler/resources/prodkamm_1m_color.tif b/adaptive_snowsampler/resources/prodkamm_1m_color.tif new file mode 100644 index 0000000..77fd597 Binary files /dev/null and b/adaptive_snowsampler/resources/prodkamm_1m_color.tif differ diff --git a/adaptive_snowsampler/resources/prodkamm_1m_crop.tif b/adaptive_snowsampler/resources/prodkamm_1m_crop.tif new file mode 100644 index 0000000..b0a7eac Binary files /dev/null and b/adaptive_snowsampler/resources/prodkamm_1m_crop.tif differ diff --git a/adaptive_snowsampler/resources/prodkamm_1m_crop_color.tif b/adaptive_snowsampler/resources/prodkamm_1m_crop_color.tif new file mode 100644 index 0000000..9d22e89 Binary files /dev/null and b/adaptive_snowsampler/resources/prodkamm_1m_crop_color.tif differ diff --git a/adaptive_snowsampler/resources/prodkamm_3m.tif b/adaptive_snowsampler/resources/prodkamm_3m.tif new file mode 100644 index 0000000..e390e49 Binary files /dev/null and b/adaptive_snowsampler/resources/prodkamm_3m.tif differ diff --git a/adaptive_snowsampler/resources/prodkamm_3m_color.tif b/adaptive_snowsampler/resources/prodkamm_3m_color.tif new file mode 100644 index 0000000..0d75cdb Binary files /dev/null and b/adaptive_snowsampler/resources/prodkamm_3m_color.tif differ diff --git a/adaptive_snowsampler/resources/riemenstalden_2m.tif b/adaptive_snowsampler/resources/riemenstalden_2m.tif new file mode 100644 index 0000000..7afd965 Binary files /dev/null and b/adaptive_snowsampler/resources/riemenstalden_2m.tif differ diff --git a/adaptive_snowsampler/resources/riemenstalden_2m_color.tif b/adaptive_snowsampler/resources/riemenstalden_2m_color.tif new file mode 100644 index 0000000..86dd500 Binary files /dev/null and b/adaptive_snowsampler/resources/riemenstalden_2m_color.tif differ diff --git a/adaptive_snowsampler/resources/riemenstalden_3m.tif b/adaptive_snowsampler/resources/riemenstalden_3m.tif new file mode 100644 index 0000000..976718b Binary files /dev/null and b/adaptive_snowsampler/resources/riemenstalden_3m.tif differ diff --git a/adaptive_snowsampler/resources/riemenstalden_3m_color.tif b/adaptive_snowsampler/resources/riemenstalden_3m_color.tif new file mode 100644 index 0000000..5a1a94b Binary files /dev/null and b/adaptive_snowsampler/resources/riemenstalden_3m_color.tif differ diff --git a/adaptive_snowsampler/resources/snowsampler.stl b/adaptive_snowsampler/resources/snowsampler.stl new file mode 100644 index 0000000..dc1d22b Binary files /dev/null and b/adaptive_snowsampler/resources/snowsampler.stl differ diff --git a/adaptive_snowsampler/resources/snowsampler_legs.stl b/adaptive_snowsampler/resources/snowsampler_legs.stl new file mode 100644 index 0000000..69ca762 Binary files /dev/null and b/adaptive_snowsampler/resources/snowsampler_legs.stl differ diff --git a/adaptive_snowsampler/resources/spilau_all_3m.tif b/adaptive_snowsampler/resources/spilau_all_3m.tif new file mode 100644 index 0000000..16f170f Binary files /dev/null and b/adaptive_snowsampler/resources/spilau_all_3m.tif differ diff --git a/adaptive_snowsampler/resources/spilau_all_3m_color.tif b/adaptive_snowsampler/resources/spilau_all_3m_color.tif new file mode 100644 index 0000000..309b3ce Binary files /dev/null and b/adaptive_snowsampler/resources/spilau_all_3m_color.tif differ diff --git a/adaptive_snowsampler/resources/splauhuetten_1m_crop.tif b/adaptive_snowsampler/resources/splauhuetten_1m_crop.tif new file mode 100644 index 0000000..2c66b07 Binary files /dev/null and b/adaptive_snowsampler/resources/splauhuetten_1m_crop.tif differ diff --git a/adaptive_snowsampler/resources/splauhuetten_1m_crop_color.tif b/adaptive_snowsampler/resources/splauhuetten_1m_crop_color.tif new file mode 100644 index 0000000..db6d0be Binary files /dev/null and b/adaptive_snowsampler/resources/splauhuetten_1m_crop_color.tif differ diff --git a/adaptive_snowsampler/resources/splauhuetten_2m_crop.tif b/adaptive_snowsampler/resources/splauhuetten_2m_crop.tif new file mode 100644 index 0000000..a1be187 Binary files /dev/null and b/adaptive_snowsampler/resources/splauhuetten_2m_crop.tif differ diff --git a/adaptive_snowsampler/resources/splauhuetten_2m_crop_color.tif b/adaptive_snowsampler/resources/splauhuetten_2m_crop_color.tif new file mode 100644 index 0000000..3046c7d Binary files /dev/null and b/adaptive_snowsampler/resources/splauhuetten_2m_crop_color.tif differ diff --git a/adaptive_snowsampler/scripts/__init__.py b/adaptive_snowsampler/scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/adaptive_snowsampler/scripts/log_replay.py b/adaptive_snowsampler/scripts/log_replay.py new file mode 100755 index 0000000..07684fd --- /dev/null +++ b/adaptive_snowsampler/scripts/log_replay.py @@ -0,0 +1,119 @@ +#! /usr/bin/env python + +""" +Convert a ULog file into rosbag file(s) +""" + +from collections import defaultdict +import argparse +import re +import rospy # pylint: disable=import-error +import rosbag # pylint: disable=import-error +# from px4_msgs import msg as px4_msgs # pylint: disable=import-error +from sensor_msgs.msg import NavSatFix +from geometry_msgs.msg import PoseStamped +import os + +from pyulog import core + +#pylint: disable=too-many-locals, invalid-name + +def main(): + """Command line interface""" + + parser = argparse.ArgumentParser(description='Convert ULog to rosbag') + parser.add_argument('filename', metavar='file.ulg', help='ULog input file or directory') + parser.add_argument('bag', metavar='file.bag', help='rosbag output file') + + parser.add_argument('-i', '--ignore', dest='ignore', action='store_true', + help='Ignore string parsing exceptions', default=False) + + args = parser.parse_args() + + convert_ulog2rosbag(args.filename, args.bag, args.ignore) + +# https://stackoverflow.com/questions/19053707/converting-snake-case-to-lower-camel-case-lowercamelcase +def to_camel_case(snake_str): + """ Convert snake case string to camel case """ + components = snake_str.split("_") + return ''.join(x.title() for x in components) + +def parseData(d, topic, idx): + return d.data[topic][idx] + + +def appendBag(path, bag): + msg_filter={'vehicle_global_position', 'vehicle_attitude'} + disable_str_exceptions=False + ulog = core.ULog(path, msg_filter, disable_str_exceptions) + data = ulog.data_list + + multiids = defaultdict(set) + for d in data: + multiids[d.name].add(d.multi_id) + + items = [] + for d in data: + for i in range(len(d.data['timestamp'])): + if d.name == "vehicle_global_position": + topic = "/mavros/global_position/global" + + msg = NavSatFix() + msg.latitude = parseData(d, 'lat', i) + msg.longitude = parseData(d, 'lon', i) + msg.altitude = parseData(d, 'alt_ellipsoid', i) + elif d.name == "vehicle_attitude": + topic = "mavros/local_position/pose" + msg = PoseStamped() + # msg.pose.orientation.w + msg.pose.orientation.w = parseData(d, 'q[0]', i) + msg.pose.orientation.x = parseData(d, 'q[1]', i) + msg.pose.orientation.y = parseData(d, 'q[2]', i) + msg.pose.orientation.z = parseData(d, 'q[3]', i) + + # for f in d.field_data: + # result = array_pattern.match(f.field_name) + # value = d.data[f.field_name][i] + # print(f.field_name) + # print(value) + # if result: + # field, array_index = result.groups() + # array_index = int(array_index) + # if isinstance(getattr(msg, field), bytes): + # attr = bytearray(getattr(msg, field)) + # attr[array_index] = value + # setattr(msg, field, bytes(attr)) + # else: + # getattr(msg, field)[array_index] = value + # else: + # setattr(msg, f.field_name, value) + ts = rospy.Time(nsecs=d.data['timestamp'][i]*1000) + items.append((topic, msg, ts)) + items.sort(key=lambda x: x[2]) + for topic, msg, ts in items: + bag.write(topic, msg, ts) + +def convert_ulog2rosbag(path, rosbag_file_name, messages, disable_str_exceptions=False): + """ + Coverts and ULog file to a CSV file. + + :param ulog_file_name: The ULog filename to open and read + :param rosbag_file_name: The rosbag filename to open and write + :param messages: A list of message names + + :return: No + """ + + with rosbag.Bag(rosbag_file_name, 'w') as bag: + if os.path.isdir(path): + list = os.listdir(path) + list.sort() + + for filename in list: + print(filename) + appendBag(os.path.join(path, filename), bag) + else: + appendBag(path, bag) + +if __name__ == "__main__": + main() diff --git a/adaptive_snowsampler/scripts/snowsampler_lac.py b/adaptive_snowsampler/scripts/snowsampler_lac.py new file mode 100755 index 0000000..a65e23e --- /dev/null +++ b/adaptive_snowsampler/scripts/snowsampler_lac.py @@ -0,0 +1,369 @@ +#!/usr/bin/env python3 +#!/usr/bin/env python +import sys +import os + +sys.path.append(os.path.dirname(__file__)) +import rospy +from std_msgs.msg import Float64 +from snowsampler_msgs.srv import SetAngle +import numpy as np +import time +from snowsampler_msgs.srv import SetAngle +from usb.core import find as find_usb +from typing import Tuple +from struct import pack +from time import sleep + + +class LAC: + + # All of these constants are used to command the LAC; all were copied directly from the configuration datasheet + + SET_ACCURACY = 0x01 + SET_RETRACT_LIMIT = 0x02 + SET_EXTEND_LIMIT = 0x03 + SET_MOVEMENT_THRESHOLD = 0x04 + SET_STALL_TIME = 0x05 + SET_PWM_THRESHOLD = 0x06 + SET_DERIVATIVE_THRESHOLD = 0x07 + SET_MAX_DERIVATIVE = 0x08 + SET_MIN_DERIVATIVE = 0x09 + SET_MAX_PWM_VALUE = 0x0A + SET_MIN_PWM_VALUE = 0x0B + SET_Kp = 0x0C + SET_Kd = 0x0D + SET_AVERAGE_RC = 0x0E + SET_AVERAGE_ADC = 0x0F + + GET_FEEDBACK = 0x10 + + SET_POSITION = 0x20 + SET_SPEED = 0x21 + + DISABLE_MANUAL = 0x30 + + RESET = 0xFF + + def __init__(self, stroke: float, vendorID: int = 0x04D8, productID: int = 0xFC5F): + """ + LAC class initializer. stroke is the maximum length of your specific actuator in millimeters, for example 30 + would be used as the stroke for a 30mm LAC. + + The default vendorID and productID came from our own actuator; be sure to test for your own values! Good + operating systems have the `lsusb` command, which contains an ID field in the form VID:PID. + + Winblows users can (allegedly) go to "Device manager", find the LAC, right click, choose "Properties", then + "Details", then "Hardware IDs", and find an entry in the form `HID\VID_XXXX&PID_YYYY` which correspond to the + vendor (XXXX) and product (YYYY) IDs. That's according to this: + https://superuser.com/a/1106248 + """ + if stroke < 0 or vendorID < 0 or productID < 0: + raise ValueError("No parameters can be negative values") + + self.device = find_usb(idVendor=vendorID, idProduct=productID) + if self.device is None: + raise Exception( + "No board found, ensure board is connected and powered with matching ID" + ) + + self.device.set_configuration() + self.stroke = stroke + + def send_data(self, function: int, value: int = 0) -> Tuple[int, int]: + """ + Send data to the LAC. The return value is an echo of the sent packet, unless otherwise stated. + + The first item in the response is (probably?) the current control mode (like `function`). The second item is the + two-byte data as an integer (like `value`). On account of endianness, the bytes are manually swapped + """ + if not 0 <= value <= 1023: + raise ValueError( + "Value is out of bounds. Must be 2-byte integer in range [0, 1023]" + ) + + # Bytes are separated: low byte first, then high byte. Former is masked, latter is shifted into position + data = pack(b"BBB", function, value & 0xFF, value >> 8) + + # These magic numbers were gleefully stolen from the PyUSB tutorial + # TODO: is there a better way to ensure data is fully sent than sleeping for 50ms? + self.device.write(1, data, 1000) + sleep(0.05) + + # Three bytes per packet + # I assume the 0x81 and 100 here also came from the PyUSB tutorial, but I never actually wrote that down... + response = self.device.read(0x81, 3, 1000) + + # Just as before, the bytes are separated, so here they're put back together as one integer + return response[0], (response[2] << 8) + response[1] + + def set_accuracy(self, accuracy: float = 0.117) -> Tuple[int, int]: + """ + Describes how close the LAC needs to get to its target position before stopping, in millimeters. The default + value gives the arm ±0.117mm of lenience to its requested destination. According to the datasheet, with too low + a value, the arm could move back and forth around the desired position endlessly. + + Because of the natures of floats and the LAC's integer-based interface, there is ironically some inaccuracy + between the parameter and the result + """ + return self.send_data( + self.SET_ACCURACY, int(round(accuracy / self.stroke * 1024)) + ) + + def set_retract_limit(self, length: float) -> Tuple[int, int]: + """ + Retract limit gives the actuator a minimum length, in millimeters. + + A value of zero would contact the mechanical stop, but according to the datasheet, "it is recommended to offset + [this value] to ensure the actuator is never driven into the physical end stops. This increases cycle life + considerably." In English, don't do that + """ + return self.send_data( + self.SET_RETRACT_LIMIT, int(round(length / self.stroke * 1023)) + ) + + def set_extend_limit(self, length: float) -> Tuple[int, int]: + """ + Extend limit gives the actuator a maximum length, in millimeters. + + A value equal to `stroke` from the constructor would contact the mechanical stop, but according to the + datasheet, "it is recommended to offset [this value] to ensure the actuator is never driven to the physical end + stops. This increases cycle life considerably." In English, don't do that + """ + return self.send_data( + self.SET_EXTEND_LIMIT, int(round(length / self.stroke * 1023)) + ) + + def set_movement_threshold(self, value: int) -> Tuple[int, int]: + """ + The datasheet describes this value as "the minimum actuator speed that is considered a stall", and warns that + "the stall timer begins counting" when the movement speed of the actuator falls below the given value. See also + `set_stall_time`. + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_MOVEMENT_THRESHOLD, value) + + def set_stall_time(self, time: int) -> Tuple[int, int]: + """ + The amount of time, in milliseconds, the actuator will wait before stopping the motor after a stall is detected + (see also `set_movement_threshold`). According to the datasheet, "the actuator will exit this state when the + input signal tells the actuator to move in the opposite direction". If the stall is resolved in time, the stall + timer will reset. + + Whether "this state" refers to the pre- or post-stop stall is unclear to me. + + The value is almost certainly a 2-byte integer, but that is my extrapolation from technical details -- it's not + officially stated + """ + return self.send_data(self.SET_STALL_TIME, time) + + def set_pwm_threshold(self, value: int) -> Tuple[int, int]: + """ + Described as setting "the distance around the set point where the PWM PD controller is active", whatever that + means (particularly the "set point". Perhaps that relates to `set_position`?). + + If the difference between the feedback (see `get_feedback`) and set point is greater than the given value, the + actuator's speed is maxed. + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_PWM_THRESHOLD, value) + + def set_derivative_threshold(self, value: int) -> Tuple[int, int]: + """ + The derivative threshold is used to determine when PWM should be increased to exit a stall, based on the current + speed. The datasheet claims it's normally set equal to the movement threshold (see `set_movement_threshold`). + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_DERIVATIVE_THRESHOLD, value) + + def set_max_derivative(self, value: int) -> Tuple[int, int]: + """ + Maximum value that the derivative term contributes to controlling speed. Probably involced in PD control? See + also `set_min_derivative`. + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_MAX_DERIVATIVE, value) + + def set_min_derivative(self, value: int) -> Tuple[int, int]: + """ + Minimum value that the derivative term contributes to controlling speed. Probably involced in PD control? See + also `set_max_derivative`. + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_MIN_DERIVATIVE, value) + + def set_max_pwm_value(self, value: int) -> Tuple[int, int]: + """ + Sets the value "manually controlled by the speed potentiometer". When outside the PWM threshold (see + `set_pwm_threshold`), this is the actuator's speed. 1023 represents maximum speed. Regardless of the value set, + the actually may actually exceed it while attempting to exit a stall. Whether 1023 can be exceeded is unclear. + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_MAX_PWM_VALUE, value) + + def set_min_pwm_value(self, value: int) -> Tuple[int, int]: + """ + Sets the value that the datasheet simply calls "the minimum PWM value that can be applied by the PD control". + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_MIN_PWM_VALUE, value) + + def set_proportional_gain(self, value: int) -> Tuple[int, int]: + """ + This sets the Kp value, the proportional control term. Higher values result in a faster approach to the target + position, but that can also make overshoot more likely. If that happens, reducing Kp will reduce overshoot. + + No units, equations, range, or other helpful info are provided + """ + return self.send_data(self.SET_PROPORTIONAL_GAIN, value) + + def set_derivative_gain(self, value: int) -> Tuple[int, int]: + """ + Sets the rate at which the differential control term increases when stalling. This is not an actual differential + term, but it has a similar effect. If a stall is detected, the derivative term increments. + + No units, equations, range, or other helpful info are provided. I presume the increment is equal to this value, + but that is entirely unclear from the datasheet + """ + return self.send_data(self.SET_DERIVATIVE_GAIN, value) + + # TODO: does this value affect the `sleep(.05)` in the contructor? + def set_average_rc(self, samples: int = 4) -> Tuple[int, int]: + """ + Sets the value which determines how many samples are used when filtering the RC input before the actuator is + moved. Increasing this value can improve stability, but will affect response time. One sample costs 20ms of + delay. Feedback filter delay and control response to a valid input signal are unaffected + """ + return self.send_data(self.SET_AVERAGE_RC, samples) + + def set_average_adc(self, samples: int) -> Tuple[int, int]: + """ + Sets the value which detemines the number of samples used in filtering the feedback and analog input signals, if + active. Increasing this value increases the feedback filter delay, where one sample costs 20ms of delay. Unlike + `set_average_rc`, this delay affects actuator control response. This delay allows the actuator to move before + using PD to update the speed, so other values may need retuning after changing this + """ + return self.send_data(self.SET_AVERAGE_ADC, samples) + + def get_feedback(self) -> int: + """ + Per the datasheet, "This command causes the actuator to respond with a feedback packet containing the current + actuator position. This is read directly from the ADC and may not be equal to the set point if the actuator has + not yet reached it." + + Because the control mode seems irrelevant, it is left out of this function's return value. How the return value + relates to the millimeter length of the arm isn't described, but I suspect it's the inverse equation of the + `set_position` function + """ + return self.send_data(self.GET_FEEDBACK)[1] + + def set_position(self, distance: int) -> Tuple[int, int]: + """ + Tells the actuator what distance to extend to, in millimeters from the base. + + Using this command disables RC, I, and V inputs until reboot, but enables USB control. + + This command's response isn't a simple echo. Instead, the second value of the tuple represents the arm's current + position + """ + return self.send_data( + self.SET_POSITION, int(round(distance / self.stroke * 1023)) + ) + + def set_speed(self, value: int) -> Tuple[int, int]: + """ + This command is not documented in the datasheet at all. + + There are four potentiometers on the board, and only one does not have a matching documented USB control + function. For that reason, I suspect this function mirrors the speed potentiometer in the same way that the + `set_accuracy` function mirrors the accuracy potentiometer + """ + return self.send_data(self.SET_SPEED, value) + + def disable_manual(self, value: int = 0): + """ + Save the current configration to EEPROM and disable all four potentiometers. + + On reboot, these values will be used instead of the potentiometer values. Analog inputs are not affected. What + "these values" refers to is unclear, though I suspect it is the accuracy, retract limit, extend limit, and speed + functions' values. + + Whether the `value` parameter has any effect is also unclear. For that reason, I have it defaulting to zero + """ + self.send_data(self.DISABLE_MANUAL) + + def reset(self, value: int = 0): + """ + Enable the manual control potentiometers and reset the configuration settings. + + I doubt the `value` parameter has any effect, so I have defaulted it to zero + """ + self.send_data(self.RESET) + + +class SnowsamplerLAC: + def __init__(self): + rospy.init_node("snowsampler_lac") + self.stroke = 150 + self.stroke_min = 5 # derived in colab ipynb | exact lower limit: 11.3 + self.stroke_max = 135 # derived in colab ipynb | exact upper limit: 129.34 + self.topic = "snowsampler/landing_leg_angle" + + # Publisher for the current angle + self.angle_publisher = rospy.Publisher(self.topic, Float64, queue_size=10) + + # Service for setting the desired angle + rospy.Service( + "snowsampler/set_landing_leg_angle", SetAngle, self.set_angle_callback + ) + + # Initialize the LAC instance + self.lac = LAC(stroke=self.stroke) + self.lac.set_retract_limit(self.stroke_min) + self.lac.set_extend_limit(self.stroke_max) + + # Start the node's main loop + self.timer = rospy.Timer(rospy.Duration(1), self.publish_current_angle) + rospy.loginfo("snowsampler_lac node has been started") + + def set_angle_callback(self, request): + # Call the set_position function of the LAC class + l_act = self.angle_to_length(request.angle) + resp = self.lac.set_position(l_act) + return True + + def publish_current_angle(self, event): + # Get the current feedback from the LAC class + feedback = self.lac.get_feedback() + current_length = feedback / 1023 * self.stroke + self.current_angle = self.length_to_angle(current_length) + # Publish the current angle + msg = Float64() + msg.data = self.current_angle + self.angle_publisher.publish(msg) + + def angle_to_length(self, angle): # angle in degrees, length in mm + # derived LSQ fit, l_act[mm] = m*phi[°] + c + m = 2.64 + c = 12.45 + length = m * angle + c + return length + + def length_to_angle(self, length): # angle in degrees, length in mm + # derived LSQ fit, phi[°] = m*l_act[mm] + c + m = 2.64 + c = 12.45 + angle = (length - c) / m + return angle + + +if __name__ == "__main__": + node = SnowsamplerLAC() + rospy.spin() \ No newline at end of file diff --git a/adaptive_snowsampler/snowsampler_lac/__init__.py b/adaptive_snowsampler/snowsampler_lac/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/adaptive_snowsampler/src/adaptive_snowsampler.cpp b/adaptive_snowsampler/src/adaptive_snowsampler.cpp new file mode 100644 index 0000000..c738eec --- /dev/null +++ b/adaptive_snowsampler/src/adaptive_snowsampler.cpp @@ -0,0 +1,656 @@ +/**************************************************************************** + * + * 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 PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file px4_avoidance.cpp + * + * px4 manipulation + * + */ + +#include "adaptive_snowsampler/adaptive_snowsampler.h" + +#include +#include +#include +#include +#include + +#include + +#include "adaptive_snowsampler/geo_conversions.h" +#include "adaptive_snowsampler/visualization.h" +#include "grid_map_geo_msgs/GeographicMapInfo.h" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_ros/static_transform_broadcaster.h" + +AdaptiveSnowSampler::AdaptiveSnowSampler(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private) + : nh_(nh), nh_private_(nh_private) { + egm96_5 = std::make_shared("egm96-5", "", true, true); + + // Publishers + original_map_pub_ = nh_.advertise("elevation_map", 1); + map_info_pub_ = nh_.advertise("elevation_map_info", 1); + target_normal_pub_ = nh_.advertise("target_normal", 1); + setpoint_position_pub_ = nh_.advertise("setpoint_position", 1); + home_position_pub_ = nh_.advertise("home_position", 1); + home_position_pub_ = nh_.advertise("home_position", 1); + target_slope_pub_ = nh_.advertise("target_slope", 1); + referencehistory_pub_ = nh_.advertise("reference/path", 1); + snow_depth_pub_ = nh_.advertise("/snow_depth", 1); + vehicle_pose_pub_ = nh_.advertise("vehicle_pose_marker", 1); + + // Subscribers + vehicle_global_position_sub_ = + nh_.subscribe("mavros/global_position/global", 1, &AdaptiveSnowSampler::vehicleGlobalPositionCallback, this, + ros::TransportHints().tcpNoDelay()); + vehicle_attitude_sub_ = nh_.subscribe("mavros/local_position/pose", 1, &AdaptiveSnowSampler::vehicleAttitudeCallback, + this, ros::TransportHints().tcpNoDelay()); + // distance_sensor_sub_ = this->create_subscription( + // "/fmu/out/distance_sensor", qos_profile, + // std::bind(&AdaptiveSnowSampler::distanceSensorCallback, this, std::placeholders::_1)); + mavcmd_long_service_client_ = nh_.serviceClient("mavros/cmd/command"); + mavcmd_int_service_client_ = nh_.serviceClient("mavros/cmd/command_int"); + + ssp_status_sub_ = + nh_.subscribe("/SSP/state", 10, &AdaptiveSnowSampler::sspStateCallback, this, ros::TransportHints().tcpNoDelay()); + + /// Service servers + setgoal_serviceserver_ = nh_.advertiseService("/set_goal", &AdaptiveSnowSampler::goalPositionCallback, this); + setstart_serviceserver_ = nh_.advertiseService("/set_start", &AdaptiveSnowSampler::startPositionCallback, this); + + takeoff_serviceserver_ = + nh_.advertiseService("/adaptive_sampler/takeoff", &AdaptiveSnowSampler::takeoffCallback, this); + + land_serviceserver_ = nh_.advertiseService("/adaptive_sampler/land", &AdaptiveSnowSampler::landCallback, this); + + goto_serviceserver_ = nh_.advertiseService("/adaptive_sampler/goto", &AdaptiveSnowSampler::gotoCallback, this); + + return_serviceserver_ = nh_.advertiseService("/adaptive_sampler/return", &AdaptiveSnowSampler::returnCallback, this); + + measurement_serviceserver_ = + nh_.advertiseService("/adaptive_sampler/take_measurement", &AdaptiveSnowSampler::takeMeasurementCallback, this); + + // Setup loop timers + ros::TimerOptions cmdlooptimer_options(ros::Duration(0.1), + boost::bind(&AdaptiveSnowSampler::cmdloopCallback, this, _1), &cmdloop_queue_); + cmdloop_timer_ = nh_.createTimer(cmdlooptimer_options); // Define timer for constant loop rate + + cmdloop_spinner_.reset(new ros::AsyncSpinner(1, &cmdloop_queue_)); + cmdloop_spinner_->start(); + + ros::TimerOptions statuslooptimer_options( + ros::Duration(1.0), boost::bind(&AdaptiveSnowSampler::statusloopCallback, this, _1), &statusloop_queue_); + statusloop_timer_ = nh_.createTimer(statuslooptimer_options); // Define timer for constant loop rate + + statusloop_spinner_.reset(new ros::AsyncSpinner(1, &statusloop_queue_)); + statusloop_spinner_->start(); + + ros::TimerOptions measurementtilttimer_options( + ros::Duration(0.1), boost::bind(&AdaptiveSnowSampler::tiltCheckCallback, this, _1), &measurementloop_queue_); + measurement_tilt_timer_ = nh_.createTimer(measurementtilttimer_options); // Define timer for constant loop rate + + measurementloop_spinner_.reset(new ros::AsyncSpinner(1, &measurementloop_queue_)); + measurementloop_spinner_->start(); + + tf_broadcaster_ = std::make_unique(); + nh_private.param("tif_path", file_path_, "."); + nh_private.param("tif_color_path", color_path_, "."); + nh_private.param("frame_id", frame_id_, "map"); + + // ssp logfiles + std::string homeDir = getenv("HOME"); + std::time_t t = std::time(nullptr); + std::tm *now = std::localtime(&t); + std::stringstream ss; + ss << std::put_time(now, "%Y_%m_%d_%H_%M_%S"); + sspLogfilePath_ = homeDir + "/rosbag/ssp_" + ss.str() + ".txt"; + sspLogfile_.open(sspLogfilePath_, std::ofstream::out | std::ofstream::app); +} + +void AdaptiveSnowSampler::cmdloopCallback(const ros::TimerEvent &event) { + if (global_position_received_) { + publishPositionHistory(referencehistory_pub_, vehicle_position_, positionhistory_vector_); + } + publishVehiclePose(vehicle_pose_pub_, vehicle_position_, vehicle_attitude_); +} + +void AdaptiveSnowSampler::statusloopCallback(const ros::TimerEvent &event) { + if (!map_initialized_) { + loadMap(); + map_initialized_ = true; + return; + } + publishMap(); + publishSetpointPosition(setpoint_position_pub_, setpoint_positon_); + publishSetpointPosition(home_position_pub_, home_position_, Eigen::Vector3d(1.0, 0.0, 0.0)); + publishTargetNormal(target_normal_pub_, target_position_ + 20.0 * target_normal_, -20.0 * target_normal_); +} + +visualization_msgs::Marker AdaptiveSnowSampler::vector2ArrowsMsg(const Eigen::Vector3d &position, + const Eigen::Vector3d &normal, int id, + Eigen::Vector3d color, + const std::string marker_namespace) { + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros::Time::now(); + marker.ns = marker_namespace; + marker.id = id; + marker.type = visualization_msgs::Marker::ARROW; + marker.action = visualization_msgs::Marker::ADD; + std::vector points; + geometry_msgs::Point head; + head.x = position(0); + head.y = position(1); + head.z = position(2); + points.push_back(head); + geometry_msgs::Point tail; + tail.x = position(0) + normal(0); + tail.y = position(1) + normal(1); + tail.z = position(2) + normal(2); + points.push_back(tail); + + marker.points = points; + marker.scale.x = 2.0 * std::min(normal.norm(), 1.0); + marker.scale.y = 2.0 * std::min(normal.norm(), 2.0); + marker.scale.z = 0.0; + marker.color.a = 0.8; + marker.color.r = color(0); + marker.color.g = color(1); + marker.color.b = color(2); + return marker; +} + +visualization_msgs::Marker AdaptiveSnowSampler::position2SphereMsg(const Eigen::Vector3d &position, int id, + Eigen::Vector3d color, + const std::string marker_namespace) { + visualization_msgs::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = ros::Time::now(); + marker.ns = marker_namespace; + marker.id = id; + marker.type = visualization_msgs::Marker::SPHERE; + marker.action = visualization_msgs::Marker::ADD; + marker.pose.orientation.w = 1.0; + marker.pose.orientation.x = 0.0; + marker.pose.orientation.y = 0.0; + marker.pose.orientation.z = 0.0; + marker.pose.position.x = position.x(); + marker.pose.position.y = position.y(); + marker.pose.position.z = position.z(); + marker.scale.x = 5.0; + marker.scale.y = 5.0; + marker.scale.z = 5.0; + marker.color.a = 0.5; + marker.color.r = color(0); + marker.color.g = color(1); + marker.color.b = color(2); + return marker; +} + +geometry_msgs::PoseStamped AdaptiveSnowSampler::vector3d2PoseStampedMsg(const Eigen::Vector3d position, + const Eigen::Vector4d orientation) { + geometry_msgs::PoseStamped encode_msg; + + encode_msg.header.stamp = ros::Time::now(); + encode_msg.header.frame_id = "map"; + encode_msg.pose.orientation.w = orientation(0); + encode_msg.pose.orientation.x = orientation(1); + encode_msg.pose.orientation.y = orientation(2); + encode_msg.pose.orientation.z = orientation(3); + encode_msg.pose.position.x = position(0); + encode_msg.pose.position.y = position(1); + encode_msg.pose.position.z = position(2); + return encode_msg; +}; + +void AdaptiveSnowSampler::publishTargetNormal(const ros::Publisher &pub, const Eigen::Vector3d &position, + const Eigen::Vector3d &normal) { + visualization_msgs::Marker marker = vector2ArrowsMsg(position, normal, 0, Eigen::Vector3d(1.0, 0.0, 1.0)); + pub.publish(marker); +} + +void AdaptiveSnowSampler::publishSetpointPosition(const ros::Publisher &pub, const Eigen::Vector3d &position, + const Eigen::Vector3d color) { + visualization_msgs::Marker marker = position2SphereMsg(position, 1, color); + pub.publish(marker); +} + +void AdaptiveSnowSampler::loadMap() { + std::cout << "file_path " << file_path_ << std::endl; + std::cout << "color_path " << color_path_ << std::endl; + + map_ = std::make_shared(frame_id_); + map_->Load(file_path_, color_path_); + map_->AddLayerNormals("elevation"); +} + +void AdaptiveSnowSampler::publishMap() { + static tf2_ros::StaticTransformBroadcaster static_broadcaster; + + grid_map_msgs::GridMap msg; + grid_map::GridMapRosConverter::toMessage(map_->getGridMap(), msg); + original_map_pub_.publish(msg); + ESPG epsg; + Eigen::Vector3d map_origin; + map_->getGlobalOrigin(epsg, map_origin); + map_origin_ = map_origin; + + grid_map_geo_msgs::GeographicMapInfo map_info_msg; + map_info_msg.header.stamp = ros::Time::now(); + map_info_msg.geo_coordinate = static_cast(epsg); + map_info_msg.width = map_->getGridMap().getSize()(0); + map_info_msg.height = map_->getGridMap().getSize()(1); + map_info_msg.x_resolution = map_->getGridMap().getResolution(); + map_info_msg.y_resolution = map_->getGridMap().getResolution(); + map_info_msg.origin_x = map_origin(0); + map_info_msg.origin_y = map_origin(1); + map_info_msg.origin_altitude = map_origin(2); + + map_info_pub_.publish(map_info_msg); + + geometry_msgs::TransformStamped static_transformStamped_; + static_transformStamped_.header.stamp = ros::Time::now(); + static_transformStamped_.header.frame_id = "CH1903"; + static_transformStamped_.child_frame_id = "map"; + static_transformStamped_.transform.translation.x = map_origin.x(); + static_transformStamped_.transform.translation.y = map_origin.y(); + static_transformStamped_.transform.translation.z = 0.0; + static_transformStamped_.transform.rotation.x = 0.0; + static_transformStamped_.transform.rotation.y = 0.0; + static_transformStamped_.transform.rotation.z = 0.0; + static_transformStamped_.transform.rotation.w = 1.0; + + static_broadcaster.sendTransform(static_transformStamped_); +} + +void AdaptiveSnowSampler::vehicleAttitudeCallback(const geometry_msgs::PoseStamped &msg) { + Eigen::Quaterniond vehicle_attitude; + vehicle_attitude.w() = msg.pose.orientation.w; + vehicle_attitude.x() = msg.pose.orientation.x; + vehicle_attitude.y() = msg.pose.orientation.y; + vehicle_attitude.z() = msg.pose.orientation.z; + + vehicle_attitude_ = vehicle_attitude; + // add value to the moving average filters + vehicle_attitude_buffer_.push_back(vehicle_attitude_.toRotationMatrix().eulerAngles(0, 1, 2)); +} + +void AdaptiveSnowSampler::vehicleGlobalPositionCallback(const sensor_msgs::NavSatFix &msg) { + if (!global_position_received_) global_position_received_ = true; + const double vehicle_latitude = msg.latitude; + const double vehicle_longitude = msg.longitude; + const double vehicle_altitude = msg.altitude; // Elliposoidal altitude + + // LV03 / WGS84 ellipsoid + GeoConversions::forward(vehicle_latitude, vehicle_longitude, vehicle_altitude, lv03_vehicle_position_.x(), + lv03_vehicle_position_.y(), lv03_vehicle_position_.z()); + + geometry_msgs::TransformStamped t; + // corresponding tf variables + t.header.stamp = ros::Time::now(); + t.header.frame_id = "CH1903"; + t.child_frame_id = "base_link"; + + // Turtle only exists in 2D, thus we get x and y translation + // coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = lv03_vehicle_position_(0); + t.transform.translation.y = lv03_vehicle_position_(1); + t.transform.translation.z = lv03_vehicle_position_(2); + + vehicle_position_ = lv03_vehicle_position_ - map_origin_; // AMSL altitude + + // For the same reason, turtle can only rotate around one axis + // and this why we set rotation in x and y to 0 and obtain + // rotation in z axis from the message + t.transform.rotation.x = vehicle_attitude_.x(); + t.transform.rotation.y = vehicle_attitude_.y(); + t.transform.rotation.z = vehicle_attitude_.z(); + t.transform.rotation.w = vehicle_attitude_.w(); + + // Send the transformation + // tf_broadcaster_->sendTransform(t); +} +// void AdaptiveSnowSampler::distanceSensorCallback(const px4_msgs::DistanceSensor &msg) { +// lidar_distance_ = msg.current_distance; +// } + +bool AdaptiveSnowSampler::takeMeasurementCallback(snowsampler_msgs::Trigger::Request &request, + snowsampler_msgs::Trigger::Response &response) { + double ground_elevation = map_->getGridMap().atPosition("elevation", vehicle_position_.head(2)); + double drone_elevation = lv03_vehicle_position_.z(); + snow_depth_ = drone_elevation - ground_elevation - lidar_distance_; + std::cout << " - drone_elevation: " << drone_elevation << std::endl; + std::cout << " - ground_elevation: " << ground_elevation << std::endl; + std::cout << " - lidar_distance: " << lidar_distance_ << std::endl; + + std_msgs::Float64 msg; + msg.data = snow_depth_; + snow_depth_pub_.publish(msg); + tilt_prevention_ = true; + + std::string service_name = "/SSP/take_measurement"; + int id = sspLogId_; + + std::thread t([service_name, id] { + snowsampler_msgs::TakeMeasurement req; + req.request.id = id; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception &e) { + std::cout << "Service Exception: " << e.what() << std::endl; + std::cout << " - measurement ID : " << id << " failed" << std::endl; + } + }); + t.detach(); + return true; +} + +void AdaptiveSnowSampler::sspStateCallback(const std_msgs::Int8::ConstPtr msg) { + // If the state changes from taking measurement to something else, stop the tilt prevention timer + if (sspState_ == SSPState::Taking_Measurement && msg->data != SSPState::Taking_Measurement) { + tilt_prevention_ = false; + } + + sspState_ = static_cast(msg->data); + auto vehicle_attitude_euler = vehicle_attitude_.toRotationMatrix().eulerAngles(0, 1, 2); // roll, pitch, yaw + + if (sspState_ == SSPState::Ready_To_Measure) { + // + Eigen::Vector3d sum(0.0, 0.0, 0.0); + for (int i = 0; i < vehicle_attitude_buffer_.size(); i++) { + sum += vehicle_attitude_buffer_[i]; + ; + } + + vehicle_attitude_filtered_ref_ = sum / vehicle_attitude_buffer_.size(); + } +} + +void AdaptiveSnowSampler::tiltCheckCallback(const ros::TimerEvent &event) { + if (tilt_prevention_) { + std::cout << "Tilt prevention is active" << std::endl; + Eigen::Vector3d sum(0.0, 0.0, 0.0); + for (int i = vehicle_attitude_buffer_.size() - tilt_window_size_; i < vehicle_attitude_buffer_.size(); i++) { + sum += vehicle_attitude_buffer_[i]; + } + Eigen::Vector3d vehicle_attitude_filtered = sum / tilt_window_size_; + + // check if the drone is tilting too much + if ((vehicle_attitude_filtered_ref_ - vehicle_attitude_filtered).cwiseAbs().maxCoeff() >= tilt_treshold_) { + // stop measurement + std::cout << "drone is tilting, stopping measurement" << std::endl; + std::cout << "tilt: " << (vehicle_attitude_filtered_ref_ - vehicle_attitude_filtered).cwiseAbs().maxCoeff() + << std::endl; + + std::string service_name = "SSP/stop_measurement"; + std::thread t([service_name] { + snowsampler_msgs::Trigger req; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception &e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); + tilt_prevention_ = false; + } + } +} + +/// TODO: Add service caller for setting start and goal states + +bool AdaptiveSnowSampler::goalPositionCallback(planner_msgs::SetVector3::Request &request, + planner_msgs::SetVector3::Response &response) { + response.success = true; + target_position_.x() = request.vector.x; + target_position_.y() = request.vector.y; + + target_position_.z() = map_->getGridMap().atPosition("elevation", target_position_.head(2)); + target_normal_ = Eigen::Vector3d(map_->getGridMap().atPosition("elevation_normal_x", target_position_.head(2)), + map_->getGridMap().atPosition("elevation_normal_y", target_position_.head(2)), + map_->getGridMap().atPosition("elevation_normal_z", target_position_.head(2))); + setpoint_positon_ = target_position_ + Eigen::Vector3d(0.0, 0.0, relative_altitude_); + + target_heading_ = std::atan2(target_normal_.y(), target_normal_.x()); + + if (target_normal_.norm() > 1e-6) { + target_slope_ = std::acos(target_normal_.dot(Eigen::Vector3d::UnitZ()) / target_normal_.norm()); + } else { + target_slope_ = 0.0; + } + + // RCLCPP_INFO_STREAM(get_logger(), " - Vehicle Target Heading: " << target_heading_); + double target_yaw = -target_heading_ - 0.5 * M_PI; + while (std::abs(target_yaw) > M_PI) { // mod2pi + target_yaw = (target_yaw > 0.0) ? target_yaw - M_PI : target_yaw + M_PI; + } + // RCLCPP_INFO_STREAM(get_logger(), " - Vehicle Target Slope: " << target_slope_); + + /// Publish target slope + std_msgs::Float64 slope_msg; + slope_msg.data = target_slope_ * 180.0 / M_PI; // rad to deg + target_slope_pub_.publish(slope_msg); + return true; +} + +bool AdaptiveSnowSampler::startPositionCallback(planner_msgs::SetVector3::Request &request, + planner_msgs::SetVector3::Response &response) { + start_position_.x() = vehicle_position_.x(); + start_position_.y() = vehicle_position_.y(); + + start_position_.z() = map_->getGridMap().atPosition("elevation", start_position_.head(2)); + home_position_ = start_position_ + Eigen::Vector3d(0.0, 0.0, relative_altitude_); + + home_normal_ = Eigen::Vector3d(map_->getGridMap().atPosition("elevation_normal_x", home_position_.head(2)), + map_->getGridMap().atPosition("elevation_normal_y", home_position_.head(2)), + map_->getGridMap().atPosition("elevation_normal_z", home_position_.head(2))); + + home_heading_ = std::atan2(home_normal_.y(), home_normal_.x()); + + if (home_normal_.head(2).norm() > 1e-6) { + home_slope_ = std::acos(home_normal_.dot(Eigen::Vector3d::UnitZ()) / home_normal_.norm()); + } else { + home_slope_ = 0.0; + } + + /// Publish target slope + std_msgs::Float64 slope_msg; + slope_msg.data = home_slope_ * 180.0 / M_PI; // rad to deg + target_slope_pub_.publish(slope_msg); + response.success = true; + return true; +} + +bool AdaptiveSnowSampler::takeoffCallback(planner_msgs::SetService::Request &request, + planner_msgs::SetService::Response &response) { + Eigen::Vector3d target_position_lv03 = + vehicle_position_ + Eigen::Vector3d(0.0, 0.0, relative_altitude_) + map_origin_; + double target_position_latitude; + double target_position_longitude; + double target_position_altitude; + target_position_lv03.z() = target_position_lv03.z() + relative_altitude_; + GeoConversions::reverse(target_position_lv03.x(), target_position_lv03.y(), target_position_lv03.z(), + target_position_latitude, target_position_longitude, target_position_altitude); + double target_position_amsl = + target_position_altitude - + GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(target_position_latitude, target_position_longitude); + mavros_msgs::CommandLong msg; + msg.request.command = mavros_msgs::CommandCode::NAV_TAKEOFF; + msg.request.param1 = -1; + msg.request.param5 = NAN; + msg.request.param6 = NAN; + msg.request.param7 = target_position_amsl; + std::cout << "Vehicle commanded altitude: " << vehicle_position_.z() + relative_altitude_ << std::endl; + mavcmd_long_service_client_.call(msg); + + response.success = true; + return true; +} + +bool AdaptiveSnowSampler::landCallback(planner_msgs::SetService::Request &request, + planner_msgs::SetService::Response &response) { + callSetAngleService(target_slope_ * 180.0 / M_PI); // Set the landing leg angle to match the slope + + mavros_msgs::CommandLong msg; + msg.request.command = mavros_msgs::CommandCode::NAV_LAND; + msg.request.param5 = NAN; + msg.request.param6 = NAN; + msg.request.param7 = NAN; + mavcmd_long_service_client_.call(msg); + + response.success = true; + return true; +} + +bool AdaptiveSnowSampler::gotoCallback(planner_msgs::SetService::Request &request, + planner_msgs::SetService::Response &response) { + callSetAngleService(neutral_angle_); // Set the landing leg angle to the neutral angle + + mavros_msgs::CommandLong msg; + msg.request.command = mavros_msgs::CommandCode::DO_REPOSITION; + + // transform target position to wgs84 and amsl + Eigen::Vector3d target_position_lv03 = target_position_ + map_origin_; + double target_position_latitude; + double target_position_longitude; + double target_position_altitude; + target_position_lv03.z() = target_position_lv03.z() + relative_altitude_; + GeoConversions::reverse(target_position_lv03.x(), target_position_lv03.y(), target_position_lv03.z(), + target_position_latitude, target_position_longitude, target_position_altitude); + // /// TODO: Do I need to send average mean sea level altitude? or ellipsoidal? + msg.request.param2 = true; + double target_yaw = -target_heading_ - 0.5 * M_PI; + while (std::abs(target_yaw) > M_PI) { // mod2pi + target_yaw = (target_yaw > 0.0) ? target_yaw - M_PI : target_yaw + M_PI; + } + msg.request.param4 = target_yaw; + msg.request.param5 = target_position_latitude; + msg.request.param6 = target_position_longitude; + double target_position_amsl = + target_position_altitude - + GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(target_position_latitude, target_position_longitude); + + msg.request.param7 = target_position_amsl; + mavcmd_long_service_client_.call(msg); + response.success = true; + return true; +} + +bool AdaptiveSnowSampler::returnCallback(planner_msgs::SetService::Request &request, + planner_msgs::SetService::Response &response) { + mavros_msgs::CommandLong msg; + msg.request.command = mavros_msgs::CommandCode::DO_REPOSITION; + callSetAngleService(home_slope_ * 180.0 / M_PI); // Adjust the legs for the Home slope + // Transform target position to wgs84 and amsl + Eigen::Vector3d home_position_lv03 = home_position_ + map_origin_; + double target_position_latitude; + double target_position_longitude; + double target_position_altitude; + GeoConversions::reverse(home_position_lv03.x(), home_position_lv03.y(), home_position_lv03.z(), + target_position_latitude, target_position_longitude, target_position_altitude); + /// TODO: Do I need to send average mean sea level altitude? or ellipsoidal? + msg.request.param2 = true; + double home_yaw = -home_heading_ - 0.5 * M_PI; + while (std::abs(home_yaw) > M_PI) { // mod2pi + home_yaw = (home_yaw > 0.0) ? home_yaw - M_PI : home_yaw + M_PI; + } + msg.request.param4 = home_yaw; + msg.request.param5 = target_position_latitude; + msg.request.param6 = target_position_longitude; + double target_position_amsl = + target_position_altitude - + GeographicLib::Geoid::GEOIDTOELLIPSOID * (*egm96_5)(target_position_latitude, target_position_longitude); + + msg.request.param7 = target_position_amsl; + + mavcmd_long_service_client_.call(msg); + + response.success = true; + return true; +} + +void AdaptiveSnowSampler::publishPositionHistory(const ros::Publisher &pub, const Eigen::Vector3d &position, + std::vector &history_vector) { + unsigned int posehistory_window_ = 2000; + Eigen::Vector4d vehicle_attitude(1.0, 0.0, 0.0, 0.0); + history_vector.insert(history_vector.begin(), vector3d2PoseStampedMsg(position, vehicle_attitude)); + if (history_vector.size() > posehistory_window_) { + history_vector.pop_back(); + } + + nav_msgs::Path msg; + msg.header.stamp = ros::Time::now(); + msg.header.frame_id = "map"; + msg.poses = history_vector; + + pub.publish(msg); +} + +void AdaptiveSnowSampler::callSetAngleService(double angle) { + std::cout << "Calling service" << std::endl; + std::string service_name = "snowsampler/set_landing_leg_angle"; + std::thread t([service_name, angle] { + snowsampler_msgs::SetAngle req; + req.request.angle = angle; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception &e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); +} + +void AdaptiveSnowSampler::writeLog() { + if (sspLogfile_.is_open()) { + sspLogfile_ << "Measurement " << sspLogId_ << " at vehicle position: " << lv03_vehicle_position_.x() << ", " + << lv03_vehicle_position_.y() << ", " << lv03_vehicle_position_.z() + << ", with snowdepth: " << snow_depth_ << std::endl; + std::cout << "Measurement " << sspLogId_ << " at vehicle position: " << lv03_vehicle_position_.x() << ", " + << lv03_vehicle_position_.y() << ", " << lv03_vehicle_position_.z() << ", with snowdepth: " << snow_depth_ + << std::endl; + sspLogId_++; + } else { + std::cout << "Could not open logfile" << std::endl; + } +} diff --git a/adaptive_snowsampler/src/main.cpp b/adaptive_snowsampler/src/main.cpp new file mode 100644 index 0000000..04f1384 --- /dev/null +++ b/adaptive_snowsampler/src/main.cpp @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2023 PX4 Development Team. All rights reserved. + * + * 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 PX4 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 OWNER 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. + * + ****************************************************************************/ + +/** + * @file main.cpp + * + * Publisher node + * + */ + +#include "adaptive_snowsampler/adaptive_snowsampler.h" + +int main(int argc, char* argv[]) { + ros::init(argc, argv, "adaptive_snowsampler"); + ros::NodeHandle nh(""); + ros::NodeHandle nh_private("~"); + + std::shared_ptr adaptive_snowsampler = std::make_shared(nh, nh_private); + + ros::spin(); + return 0; +} diff --git a/adaptive_snowsampler/worlds/prodkamm_1m_crop.world b/adaptive_snowsampler/worlds/prodkamm_1m_crop.world new file mode 100644 index 0000000..f21802a --- /dev/null +++ b/adaptive_snowsampler/worlds/prodkamm_1m_crop.world @@ -0,0 +1,68 @@ + + + + + + + 12 + + + 0.95 0.95 0.95 1 + 0.3 0.3 0.3 1 + true + + + + model://sun + + + + model://ground_plane + + + model://asphalt_plane + + + + EARTH_WGS84 + 47.071259 + 9.268516 + 1939.8 + + + + base_link + + 1 0 0 + 0 1 0 + 0.0 + 0 0 0 + 0 + 0 + 0 + world_wind + + + + 0 0 -9.8066 + + + quick + 10 + 1.3 + 0 + + + 0 + 0.2 + 100 + 0.001 + + + 0.004 + 1 + 250 + 6.0e-6 2.3e-5 -4.2e-5 + + + diff --git a/dependencies.rosinstall b/dependencies.rosinstall new file mode 100644 index 0000000..e804cb6 --- /dev/null +++ b/dependencies.rosinstall @@ -0,0 +1,6 @@ +- git: {local-name: catkin_simple, uri: 'https://github.com/catkin/catkin_simple', version: master} +- git: {local-name: eigen_catkin, uri: 'https://github.com/ethz-asl/eigen_catkin', version: master} +- git: {local-name: grid_map, uri: 'https://github.com/Jaeyoung-Lim/grid_map.git', version: master} +- git: {local-name: grid_map_geo, uri: 'https://github.com/ethz-asl/grid_map_geo.git', version: master} +- git: {local-name: mav_comm, uri: 'https://github.com/ethz-asl/mav_comm.git', version: master} +- git: {local-name: terrain-navigation, uri: 'https://github.com/ethz-asl/terrain-navigation.git', version: main} diff --git a/snowsampler_msgs/CMakeLists.txt b/snowsampler_msgs/CMakeLists.txt new file mode 100644 index 0000000..14ad80e --- /dev/null +++ b/snowsampler_msgs/CMakeLists.txt @@ -0,0 +1,22 @@ +cmake_minimum_required(VERSION 2.8.3) +project(snowsampler_msgs) + +find_package(catkin REQUIRED COMPONENTS message_generation std_msgs) + +include_directories(include) + +add_service_files( + FILES + Trigger.srv + SetAngle.srv + SetMaxSpeed.srv + GetPosition.srv + TakeMeasurement.srv + SetMeasurementDepth.srv +) + +generate_messages(DEPENDENCIES std_msgs) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS message_runtime std_msgs) diff --git a/snowsampler_msgs/include/dummy b/snowsampler_msgs/include/dummy new file mode 100644 index 0000000..e69de29 diff --git a/snowsampler_msgs/package.xml b/snowsampler_msgs/package.xml new file mode 100644 index 0000000..64d7bef --- /dev/null +++ b/snowsampler_msgs/package.xml @@ -0,0 +1,25 @@ + + + + snowsampler_msgs + 0.0.2 + + snowsampler_msgs includes the definition for custom services for the adaptive-snowsampler package. + + + Jaeyoung Lim + Claudio Chies + BSD + + catkin + message_generation + message_runtime + std_msgs + + + + + + diff --git a/snowsampler_msgs/srv/GetPosition.srv b/snowsampler_msgs/srv/GetPosition.srv new file mode 100644 index 0000000..a47802c --- /dev/null +++ b/snowsampler_msgs/srv/GetPosition.srv @@ -0,0 +1,4 @@ +# GetPosition.srv +--- +float64 position +bool success diff --git a/snowsampler_msgs/srv/SetAngle.srv b/snowsampler_msgs/srv/SetAngle.srv new file mode 100644 index 0000000..363874c --- /dev/null +++ b/snowsampler_msgs/srv/SetAngle.srv @@ -0,0 +1,4 @@ +# SetAngle.srv +float64 angle +--- +bool success diff --git a/snowsampler_msgs/srv/SetMaxSpeed.srv b/snowsampler_msgs/srv/SetMaxSpeed.srv new file mode 100644 index 0000000..aa57e33 --- /dev/null +++ b/snowsampler_msgs/srv/SetMaxSpeed.srv @@ -0,0 +1,4 @@ +# SetMaxSpeed.srv +int8 data +--- +bool success diff --git a/snowsampler_msgs/srv/SetMeasurementDepth.srv b/snowsampler_msgs/srv/SetMeasurementDepth.srv new file mode 100644 index 0000000..351d07d --- /dev/null +++ b/snowsampler_msgs/srv/SetMeasurementDepth.srv @@ -0,0 +1,4 @@ +# SetMeasurementDepth.srv +int8 data +--- +bool success diff --git a/snowsampler_msgs/srv/TakeMeasurement.srv b/snowsampler_msgs/srv/TakeMeasurement.srv new file mode 100644 index 0000000..6d5a113 --- /dev/null +++ b/snowsampler_msgs/srv/TakeMeasurement.srv @@ -0,0 +1,4 @@ +# Trigger.srv +int8 id +--- +bool success diff --git a/snowsampler_msgs/srv/Trigger.srv b/snowsampler_msgs/srv/Trigger.srv new file mode 100644 index 0000000..6b064b4 --- /dev/null +++ b/snowsampler_msgs/srv/Trigger.srv @@ -0,0 +1,3 @@ +# Trigger.srv +--- +bool success diff --git a/snowsampler_rviz/CMakeLists.txt b/snowsampler_rviz/CMakeLists.txt new file mode 100644 index 0000000..d54ed6b --- /dev/null +++ b/snowsampler_rviz/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 2.8.3) +project(snowsampler_rviz) + +find_package(catkin_simple REQUIRED) +catkin_simple(ALL_DEPS_REQUIRED) + +add_definitions(-std=c++11) + +# These settings all stolen from visualization_tutorials: +# This setting causes Qt's "MOC" generation to happen automatically. +set(CMAKE_AUTOMOC OFF) + +set(HEADER_FILES_QT + include/snowsampler_rviz/planning_panel.h +) + +set(SRC_FILES + src/planning_panel.cpp + src/goal_marker.cpp +) + +# This plugin includes Qt widgets, so we must include Qt. +# We'll use the version that rviz used so they are compatible. +if(rviz_QT_VERSION VERSION_LESS "5") + message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) + # pull in all required include dirs, define QT_LIBRARIES, etc. + include(${QT_USE_FILE}) + qt4_wrap_cpp(MOC_FILES + ${HEADER_FILES_QT} + ) +else() + message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") + find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) + # make target_link_libraries(${QT_LIBRARIES}) pull in all required dependencies + set(QT_LIBRARIES Qt5::Widgets) + QT5_WRAP_CPP(MOC_FILES + ${HEADER_FILES_QT} + ) +endif() +# I prefer the Qt signals and slots to avoid defining "emit", "slots", +# etc because they can conflict with boost signals, so define QT_NO_KEYWORDS here. +add_definitions(-DQT_NO_KEYWORDS) + +# In case someone else struggles with getting panels to build, see this +# solution: +# https://answers.ros.org/question/215487/could-not-load-panel-in-rviz-pluginlibfactory-the-plugin-for-class/ + +############# +# LIBRARIES # +############# +cs_add_library(${PROJECT_NAME} + ${SRC_FILES} + ${MOC_FILES} +) +target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES}) + + + +########## +# EXPORT # +########## +cs_install() +cs_export() \ No newline at end of file diff --git a/snowsampler_rviz/include/snowsampler_rviz/goal_marker.h b/snowsampler_rviz/include/snowsampler_rviz/goal_marker.h new file mode 100644 index 0000000..6ec5e1c --- /dev/null +++ b/snowsampler_rviz/include/snowsampler_rviz/goal_marker.h @@ -0,0 +1,56 @@ + +#ifndef snowsampler_rviz_GOAL_MARKER_H_ +#define snowsampler_rviz_GOAL_MARKER_H_ + +#include +#include +#include + +#include +#include +#include +#include + +#include "interactive_markers/menu_handler.h" + +class GoalMarker { + public: + GoalMarker(const ros::NodeHandle &nh); + virtual ~GoalMarker(); + Eigen::Vector3d getGoalPosition() { return goal_pos_; }; + void setGoalPosition(const Eigen::Vector2d &position); + + private: + Eigen::Vector3d toEigen(const geometry_msgs::Pose &p) { + Eigen::Vector3d position(p.position.x, p.position.y, p.position.z); + return position; + } + void processSetPoseFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void GridmapCallback(const grid_map_msgs::GridMap &msg); + + void initializeMenu(); + + void setStartCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + void setGoalCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback); + + void callPlannerService(const std::string service_name, const Eigen::Vector3d vector); + + visualization_msgs::InteractiveMarkerControl makeMovePlaneControl(); + visualization_msgs::InteractiveMarkerControl makeMenuControl(); + + ros::NodeHandle nh_; + ros::Subscriber grid_map_sub_; + // rclcpp::Client<>::SharedPtr goal_serviceclient_; + + interactive_markers::InteractiveMarkerServer marker_server_; + visualization_msgs::InteractiveMarker set_goal_marker_; + interactive_markers::MenuHandler menu_handler_; + interactive_markers::MenuHandler::EntryHandle menu_handler_first_entry_; + interactive_markers::MenuHandler::EntryHandle menu_handler_mode_last_; + Eigen::Vector3d goal_pos_{Eigen::Vector3d::Zero()}; + grid_map::GridMap map_; + std::mutex goal_mutex_; + double relative_altitude_{20.0}; +}; + +#endif // snowsampler_rviz_GOAL_MARKER_H_ diff --git a/snowsampler_rviz/include/snowsampler_rviz/planning_panel.h b/snowsampler_rviz/include/snowsampler_rviz/planning_panel.h new file mode 100644 index 0000000..7d5835a --- /dev/null +++ b/snowsampler_rviz/include/snowsampler_rviz/planning_panel.h @@ -0,0 +1,143 @@ +#ifndef snowsampler_rviz_PLANNING_PANEL_H_ +#define snowsampler_rviz_PLANNING_PANEL_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +enum PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 }; +enum SSPState { + Error, + Ready_To_Measure, + Taking_Measurement, + Stopped_No_Home, + Going_Home, + Moving, + Position_Not_Reached, + ENUM_LENGTH +}; +static const char* SSPState_string[] = {"Error", "Ready_To_Measure", "Taking_Measurement", "Stopped_No_Home", + "Going_Home", "Moving", "Position_Not_Reached"}; + +class QLineEdit; +class QCheckBox; +namespace snowsampler_rviz { + +class PlanningPanel : public rviz::Panel { + // This class uses Qt slots and is a subclass of QObject, so it needs + // the Q_OBJECT macro. + Q_OBJECT + public: + // QWidget subclass constructors usually take a parent widget + // parameter (which usually defaults to 0). At the same time, + // pluginlib::ClassLoader creates instances by calling the default + // constructor (with no arguments). Taking the parameter and giving + // a default of 0 lets the default constructor work and also lets + // someone using the class for something else to pass in a parent + // widget as they normally would with Qt. + explicit PlanningPanel(QWidget* parent = 0); + + // Now we declare overrides of rviz::Panel functions for saving and + // loading data from the config file. Here the data is the + // topic name. + virtual void load(const rviz::Config& config); + virtual void save(rviz::Config config) const; + virtual void onInitialize(); + + // Callback from ROS when the pose updates: + void legAngleCallback(const std_msgs::Float64& msg); + void targetAngleCallback(const std_msgs::Float64& msg); + void snowDepthCallback(const std_msgs::Float64& msg); + void sspStateCallback(const std_msgs::Int8& msg); + void mapInfoCallback(const grid_map_geo_msgs::GeographicMapInfo& msg); + // Next come a couple of public Qt slots. + public Q_SLOTS: + + void callSetAngleService(double angle); + void setPlannerModeServiceTakeoff(); + void setPlannerModeServiceLand(); + void setPlannerModeServiceGoTo(); + void setPlannerModeServiceReturn(); + void callSspTakeMeasurementService(); + void callSspStopMeasurementService(); + void callSspGoHomeService(); + void callSspSetMeasurementDepthService(int depth); + + protected: + // Set up the layout, only called by the constructor. + void createLayout(); + void callSetPlannerStateService(std::string service_name, const int mode); + void callSspService(std::string service_name); + + QGroupBox* createPlannerModeGroup(); + QGroupBox* createLegControlGroup(); + QGroupBox* createSspControlGroup(); + + // ROS Stuff: + ros::NodeHandle nh_; + ros::Publisher waypoint_pub_; + ros::Publisher controller_pub_; + ros::Subscriber leg_angle_sub_; + ros::Subscriber target_angle_sub_; + ros::Subscriber ssp_state_sub_; + ros::Subscriber snow_depth_subscriber_; + ros::Subscriber map_info_sub_; + + std::shared_ptr goal_marker_; + + // QT stuff: + QLabel* current_angle_label_; + QLabel* target_angle_label_; + QLabel* snow_depth_label_; + QLineEdit* angle_input_; + QLineEdit* measurement_depth_input_; + QPushButton* set_leg_angle_button_; + QPushButton* ssp_take_measurement_button_; + QPushButton* ssp_stop_measurement_button_; + QPushButton* ssp_go_home_button_; + QPushButton* ssp_set_measurement_depth_button_; + QPushButton* set_goal_button_; + QPushButton* set_start_button_; + std::vector set_planner_state_buttons_; + QPushButton* controller_button_; + + Eigen::Vector3d map_origin_; + + // QT state: + QString namespace_; + QString planner_name_; + QString planning_budget_value_{"100.0"}; + QString odometry_topic_; + + // Other state: + std::string currently_editing_; + + SSPState ssp_state_{Error}; + QLabel* ssp_state_label_; +}; + +} // end namespace snowsampler_rviz + +#endif // snowsampler_rviz_PLANNING_PANEL_H_ diff --git a/snowsampler_rviz/launch/config.rviz b/snowsampler_rviz/launch/config.rviz new file mode 100644 index 0000000..bcad9ce --- /dev/null +++ b/snowsampler_rviz/launch/config.rviz @@ -0,0 +1,222 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /GridMap1 + - /Path1 + - /InteractiveMarkers1 + - /Marker1 + - /Marker2 + - /MarkerArray1 + Splitter Ratio: 0.5 + Tree Height: 161 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Name: Time + SyncMode: 0 + SyncSource: "" + - Class: snowsampler_rviz/PlanningPanel + Name: PlanningPanel + namespace: "" + odometry_topic: "" + planner_name: "" + planning_budget: 100.0 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Class: grid_map_rviz_plugin/GridMap + Color: 200; 200; 200 + Color Layer: color + Color Transformer: ColorLayer + ColorMap: default + Enabled: true + Grid Cell Decimation: 1 + Grid Line Thickness: 0.10000000149011612 + Height Layer: elevation + Height Transformer: GridMapLayer + History Length: 1 + Invert ColorMap: false + Max Color: 255; 255; 255 + Max Intensity: 10 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: GridMap + Show Grid Lines: false + Topic: /elevation_map + Unreliable: false + Use ColorMap: true + Value: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Queue Size: 10 + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: /reference/path + Unreliable: false + Value: true + - Class: rviz/InteractiveMarkers + Enable Transparency: true + Enabled: true + Name: InteractiveMarkers + Show Axes: true + Show Descriptions: true + Show Visual Aids: false + Update Topic: /goal/update + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /home_position + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /setpoint_position + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/Marker + Enabled: true + Marker Topic: /target_normal + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true + - Class: rviz/MarkerArray + Enabled: true + Marker Topic: /vehicle_pose_marker + Name: MarkerArray + Namespaces: + {} + Queue Size: 100 + Value: true + Enabled: true + Global Options: + Background Color: 255; 255; 255 + Default Light: true + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 242.64480590820312 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 15.703259468078613 + Y: 103.41791534423828 + Z: 1887.617431640625 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5903987884521484 + Target Frame: + Yaw: 5.393568515777588 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 842 + Hide Left Dock: false + Hide Right Dock: false + PlanningPanel: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001c4000002b0fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000012a000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001a0050006c0061006e006e0069006e006700500061006e0065006c010000016b000001800000018000ffffff000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000056a0000003efc0100000002fb0000000800540069006d006501000000000000056a0000030700fffffffb0000000800540069006d006501000000000000045000000000000000000000028b000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1386 + X: 432 + Y: 82 diff --git a/snowsampler_rviz/launch/run.launch b/snowsampler_rviz/launch/run.launch new file mode 100644 index 0000000..1c6cce7 --- /dev/null +++ b/snowsampler_rviz/launch/run.launch @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/snowsampler_rviz/package.xml b/snowsampler_rviz/package.xml new file mode 100644 index 0000000..2c55953 --- /dev/null +++ b/snowsampler_rviz/package.xml @@ -0,0 +1,32 @@ + + + + snowsampler_rviz + 0.0.0 + Planning plugin for RVIZ. + Helen Oleynikova + Helen Oleynikova + BSD + + catkin + catkin_simple + grid_map_geo_msgs + + mav_msgs + roscpp + std_msgs + tf + visualization_msgs + mavros_msgs + planner_msgs + snowsampler_msgs + grid_map_core + grid_map_ros + grid_map_geo + grid_map_rviz_plugin + rviz + + + + + diff --git a/snowsampler_rviz/plugin_description.xml b/snowsampler_rviz/plugin_description.xml new file mode 100644 index 0000000..29543db --- /dev/null +++ b/snowsampler_rviz/plugin_description.xml @@ -0,0 +1,9 @@ + + + + A panel that allows easy control for the snow sampler. + + + diff --git a/snowsampler_rviz/src/goal_marker.cpp b/snowsampler_rviz/src/goal_marker.cpp new file mode 100644 index 0000000..5686c2a --- /dev/null +++ b/snowsampler_rviz/src/goal_marker.cpp @@ -0,0 +1,154 @@ +#include "snowsampler_rviz/goal_marker.h" + +#include + +#include +#include + +GoalMarker::GoalMarker(const ros::NodeHandle &nh) : nh_(nh), marker_server_("goal") { + set_goal_marker_.header.frame_id = "map"; + set_goal_marker_.name = "set_pose"; + set_goal_marker_.scale = 20.0; + set_goal_marker_.controls.clear(); + + // Set up controls: x, y, z, and yaw. + set_goal_marker_.controls.clear(); + set_goal_marker_.controls.push_back(makeMovePlaneControl()); + set_goal_marker_.controls.push_back(makeMenuControl()); + + marker_server_.insert(set_goal_marker_); + marker_server_.setCallback(set_goal_marker_.name, boost::bind(&GoalMarker::processSetPoseFeedback, this, _1)); + + initializeMenu(); + menu_handler_.apply(marker_server_, "set_pose"); + marker_server_.applyChanges(); + grid_map_sub_ = + nh_.subscribe("/elevation_map", 1, &GoalMarker::GridmapCallback, this, ros::TransportHints().tcpNoDelay()); +} + +GoalMarker::~GoalMarker() = default; + +void GoalMarker::setGoalPosition(const Eigen::Vector2d &position) { + const std::lock_guard lock(goal_mutex_); + goal_pos_(0) = position(0); + goal_pos_(1) = position(1); + if (map_.isInside(position)) { + double elevation = map_.atPosition("elevation", position); + // Update the marker's pose based on the manually set position and elevation + set_goal_marker_.pose.position.x = position(0); + set_goal_marker_.pose.position.y = position(1); + set_goal_marker_.pose.position.z = elevation + relative_altitude_; + marker_server_.setPose(set_goal_marker_.name, set_goal_marker_.pose); + goal_pos_(2) = elevation; + } + marker_server_.applyChanges(); + // call the planner service to set the goal + callPlannerService("/set_goal", goal_pos_); + + menu_handler_.reApply(marker_server_); + marker_server_.applyChanges(); +} + +void GoalMarker::processSetPoseFeedback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + const std::lock_guard lock(goal_mutex_); + if (feedback->event_type == visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE) { + set_goal_marker_.pose = feedback->pose; + Eigen::Vector2d marker_position_2d(set_goal_marker_.pose.position.x, set_goal_marker_.pose.position.y); + if (map_.isInside(marker_position_2d)) { + double elevation = map_.atPosition("elevation", marker_position_2d); + set_goal_marker_.pose.position.z = elevation + relative_altitude_; + marker_server_.setPose(set_goal_marker_.name, set_goal_marker_.pose); + goal_pos_ = toEigen(feedback->pose); + goal_pos_(2) = elevation; + } + } + marker_server_.applyChanges(); +} + +void GoalMarker::GridmapCallback(const grid_map_msgs::GridMap &msg) { + const std::lock_guard lock(goal_mutex_); + grid_map::GridMapRosConverter::fromMessage(msg, map_); + Eigen::Vector2d marker_position_2d(set_goal_marker_.pose.position.x, set_goal_marker_.pose.position.y); + if (map_.isInside(marker_position_2d)) { + // set_goal_marker_.pose.position.z + double elevation = map_.atPosition("elevation", marker_position_2d); + if (elevation + 200.0 > set_goal_marker_.pose.position.z) { + set_goal_marker_.pose.position.z = elevation + relative_altitude_; + marker_server_.setPose(set_goal_marker_.name, set_goal_marker_.pose); + goal_pos_(2) = elevation; + } + } + marker_server_.applyChanges(); +} + +visualization_msgs::InteractiveMarkerControl GoalMarker::makeMovePlaneControl() { + const double kSqrt2Over2 = sqrt(2.0) / 2.0; + visualization_msgs::InteractiveMarkerControl control; + control.orientation.w = kSqrt2Over2; + control.orientation.x = 0; + control.orientation.y = kSqrt2Over2; + control.orientation.z = 0; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MOVE_PLANE; + control.name = "move plane"; + return control; +} + +visualization_msgs::InteractiveMarkerControl GoalMarker::makeMenuControl() { + visualization_msgs::Marker marker; + double scale = 10.0; + marker.type = visualization_msgs::Marker::SPHERE; + marker.scale.x = scale; + marker.scale.y = scale; + marker.scale.z = scale; + marker.color.r = 0.0; + marker.color.g = 1.0; + marker.color.b = 0.0; + marker.color.a = 1.0; + + visualization_msgs::InteractiveMarkerControl control; + control.interaction_mode = visualization_msgs::InteractiveMarkerControl::MENU; + control.always_visible = true; + control.name = "goal menu"; + control.markers.push_back(marker); + + return control; +} + +void GoalMarker::initializeMenu() { + menu_handler_first_entry_ = menu_handler_.insert( + "Set Vehicle Position as Home", std::bind(&GoalMarker::setStartCallback, this, std::placeholders::_1)); + + menu_handler_.insert("Set as Goal", std::bind(&GoalMarker::setGoalCallback, this, std::placeholders::_1)); +} + +void GoalMarker::setStartCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + callPlannerService("/set_start", goal_pos_); + menu_handler_.reApply(marker_server_); + marker_server_.applyChanges(); +} + +void GoalMarker::setGoalCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback) { + callPlannerService("/set_goal", goal_pos_); + menu_handler_.reApply(marker_server_); + marker_server_.applyChanges(); +} + +void GoalMarker::callPlannerService(const std::string service_name, const Eigen::Vector3d vector) { + std::cout << "Planner Service" << std::endl; + std::thread t([service_name, vector] { + planner_msgs::SetVector3 req; + req.request.vector.x = vector(0); + req.request.vector.y = vector(1); + req.request.vector.z = vector(2); + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception &e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); + return; +} diff --git a/snowsampler_rviz/src/planning_panel.cpp b/snowsampler_rviz/src/planning_panel.cpp new file mode 100644 index 0000000..dba4d4a --- /dev/null +++ b/snowsampler_rviz/src/planning_panel.cpp @@ -0,0 +1,252 @@ +#include "snowsampler_rviz/planning_panel.h" + +namespace snowsampler_rviz { + +PlanningPanel::PlanningPanel(QWidget* parent) : rviz::Panel(parent) { createLayout(); } + +void PlanningPanel::onInitialize() { + goal_marker_ = std::make_shared(nh_); + + map_info_sub_ = nh_.subscribe("/elevation_map_info", 1, &PlanningPanel::mapInfoCallback, this, + ros::TransportHints().tcpNoDelay()); + + leg_angle_sub_ = nh_.subscribe("/snowsampler/landing_leg_angle", 1, &PlanningPanel::legAngleCallback, this, + ros::TransportHints().tcpNoDelay()); + target_angle_sub_ = + nh_.subscribe("/target_slope", 1, &PlanningPanel::targetAngleCallback, this, ros::TransportHints().tcpNoDelay()); + snow_depth_subscriber_ = + nh_.subscribe("/snow_depth", 1, &PlanningPanel::snowDepthCallback, this, ros::TransportHints().tcpNoDelay()); + ssp_state_sub_ = + nh_.subscribe("/SSP/state", 1, &PlanningPanel::sspStateCallback, this, ros::TransportHints().tcpNoDelay()); +} + +void PlanningPanel::createLayout() { + QGridLayout* service_layout = new QGridLayout; + + // Planner services and publications. + service_layout->addWidget(createPlannerModeGroup(), 0, 0, 4, 1); + // log something + service_layout->addWidget(createSspControlGroup(), 4, 0, 4, 1); + service_layout->addWidget(createLegControlGroup(), 8, 0, 4, 1); + + // First the names, then the start/goal, then service buttons. + QVBoxLayout* layout = new QVBoxLayout; + layout->addLayout(service_layout); + setLayout(layout); +} + +QGroupBox* PlanningPanel::createSspControlGroup() { + QGroupBox* groupBox = new QGroupBox(tr("SSP control Actions")); + QGridLayout* service_layout = new QGridLayout; + ssp_take_measurement_button_ = new QPushButton("Take Measurement"); + ssp_stop_measurement_button_ = new QPushButton("Stop Measurement"); + ssp_go_home_button_ = new QPushButton("Go Home"); + snow_depth_label_ = new QLabel("Snow Depth: "); + ssp_state_label_ = new QLabel("SSP State: " + QString::fromStdString(SSPState_string[ssp_state_])); + ssp_set_measurement_depth_button_ = new QPushButton("Set Measurement Depth"); + QLineEdit* measurement_depth_input_ = new QLineEdit(); + + connect(ssp_take_measurement_button_, &QPushButton::released, this, &PlanningPanel::callSspTakeMeasurementService); + connect(ssp_stop_measurement_button_, &QPushButton::released, this, &PlanningPanel::callSspStopMeasurementService); + connect(ssp_go_home_button_, &QPushButton::released, this, &PlanningPanel::callSspGoHomeService); + connect(ssp_set_measurement_depth_button_, &QPushButton::clicked, [this, measurement_depth_input_]() { + QString depth_str = measurement_depth_input_->text(); + callSspSetMeasurementDepthService(depth_str.toInt()); + }); + service_layout->addWidget(ssp_take_measurement_button_, 0, 0, 1, 2); + service_layout->addWidget(ssp_stop_measurement_button_, 1, 0, 1, 1); + service_layout->addWidget(ssp_go_home_button_, 1, 1, 1, 1); + service_layout->addWidget(snow_depth_label_, 0, 2, 1, 2); + service_layout->addWidget(ssp_state_label_, 1, 2, 1, 2); + service_layout->addWidget(ssp_set_measurement_depth_button_, 2, 0, 1, 1); + service_layout->addWidget(measurement_depth_input_, 2, 1, 1, 1); + + groupBox->setLayout(service_layout); + return groupBox; +} +QGroupBox* PlanningPanel::createLegControlGroup() { + QGroupBox* groupBox = new QGroupBox(tr("Leg control Actions")); + QGridLayout* service_layout = new QGridLayout; + QPushButton* set_leg_angle_button_ = new QPushButton("SET LEG ANGLE"); + QLineEdit* angle_input_ = new QLineEdit(); + current_angle_label_ = new QLabel("none"); + target_angle_label_ = new QLabel("none"); + + connect(set_leg_angle_button_, &QPushButton::clicked, [this, angle_input_]() { + QString angle_str = angle_input_->text(); + callSetAngleService(angle_str.toDouble()); + }); + + service_layout->addWidget(set_leg_angle_button_, 0, 0, 1, 1); + service_layout->addWidget(angle_input_, 0, 1, 1, 1); + service_layout->addWidget(new QLabel("Current Angle: "), 0, 2, 1, 1); + service_layout->addWidget(current_angle_label_, 0, 3, 1, 1); + service_layout->addWidget(new QLabel("Target Angle: "), 1, 2, 1, 1); + service_layout->addWidget(target_angle_label_, 1, 3, 1, 1); + groupBox->setLayout(service_layout); + return groupBox; +} + +QGroupBox* PlanningPanel::createPlannerModeGroup() { + QGroupBox* groupBox = new QGroupBox(tr("Planner Actions")); + QGridLayout* service_layout = new QGridLayout; + set_planner_state_buttons_.push_back(new QPushButton("TAKE OFF")); + set_planner_state_buttons_.push_back(new QPushButton("LAND")); + set_planner_state_buttons_.push_back(new QPushButton("GOTO TARGET")); + set_planner_state_buttons_.push_back(new QPushButton("RETURN")); + + QLineEdit* goal_x_input_ = new QLineEdit(); + goal_x_input_->setPlaceholderText("Easting"); + QLineEdit* goal_y_input_ = new QLineEdit(); + goal_y_input_->setPlaceholderText("Northing"); + QPushButton* set_goal_button_ = new QPushButton("SET GOAL"); + + service_layout->addWidget(set_planner_state_buttons_[0], 0, 0, 1, 1); + service_layout->addWidget(set_planner_state_buttons_[1], 0, 1, 1, 1); + service_layout->addWidget(set_planner_state_buttons_[3], 0, 2, 1, 1); + service_layout->addWidget(set_planner_state_buttons_[2], 0, 3, 1, 1); + service_layout->addWidget(goal_x_input_, 3, 0, 1, 2); + service_layout->addWidget(goal_y_input_, 3, 2, 1, 2); + service_layout->addWidget(set_goal_button_, 4, 0, 1, 4); + + groupBox->setLayout(service_layout); + + connect(set_planner_state_buttons_[0], SIGNAL(released()), this, SLOT(setPlannerModeServiceTakeoff())); + connect(set_planner_state_buttons_[1], SIGNAL(released()), this, SLOT(setPlannerModeServiceLand())); + connect(set_planner_state_buttons_[2], SIGNAL(released()), this, SLOT(setPlannerModeServiceGoTo())); + connect(set_planner_state_buttons_[3], SIGNAL(released()), this, SLOT(setPlannerModeServiceReturn())); + + connect(set_goal_button_, &QPushButton::clicked, [this, goal_x_input_, goal_y_input_]() { + bool ok_x, ok_y; + double x_ch1903 = goal_x_input_->text().toDouble(&ok_x); + double y_ch1903 = goal_y_input_->text().toDouble(&ok_y); + // transform CH1903 to map frame + geometry_msgs::TransformStamped ch1903_to_map_transform; + if (ok_x && ok_y) { + // transforms are already in negative direction so add instead of subtract + double x = x_ch1903 - map_origin_.x(); + double y = y_ch1903 - map_origin_.y(); + + goal_marker_->setGoalPosition(Eigen::Vector2d(x, y)); + std::cout << "Goal position set to: " << x_ch1903 << "," << y_ch1903 << std::endl; + } else { + std::cout << "Invalid input for goal coordinates." << std::endl; + } + }); + + return groupBox; +} + +// Save all configuration data from this panel to the given +// Config object. It is important here that you call save() +// on the parent class so the class id and panel name get saved. +void PlanningPanel::save(rviz::Config config) const { rviz::Panel::save(config); } + +// Load all configuration data for this panel from the given Config object. +void PlanningPanel::load(const rviz::Config& config) { rviz::Panel::load(config); } + +void PlanningPanel::setPlannerModeServiceTakeoff() { callSetPlannerStateService("/adaptive_sampler/takeoff", 2); } + +void PlanningPanel::setPlannerModeServiceGoTo() { callSetPlannerStateService("/adaptive_sampler/goto", 4); } + +void PlanningPanel::setPlannerModeServiceReturn() { callSetPlannerStateService("/adaptive_sampler/return", 3); } + +void PlanningPanel::setPlannerModeServiceLand() { callSetPlannerStateService("/adaptive_sampler/land", 3); } + +void PlanningPanel::callSetPlannerStateService(std::string service_name, const int mode) { + std::thread t([service_name] { + planner_msgs::SetService req; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception& e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); +} + +void PlanningPanel::callSspTakeMeasurementService() { callSspService("/adaptive_sampler/take_measurement"); } +void PlanningPanel::callSspStopMeasurementService() { callSspService("/SSP/stop_measurement"); } +void PlanningPanel::callSspGoHomeService() { callSspService("/SSP/go_home"); } + +// TODO: this could be combined with callSetPlannerStateService and callSspSetMeasurementDepthService through templating +void PlanningPanel::callSspService(std::string service_name) { + std::cout << "Calling SSP service: " << service_name << std::endl; + std::thread t([service_name] { + snowsampler_msgs::Trigger req; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception& e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); +} + +void PlanningPanel::callSspSetMeasurementDepthService(int depth) { + std::string service_name = "/SSP/set_measurement_depth"; + std::thread t([service_name, depth] { + snowsampler_msgs::SetMeasurementDepth req; + req.request.data = depth; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception& e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); +} + +void PlanningPanel::legAngleCallback(const std_msgs::Float64& msg) { + current_angle_label_->setText(QString::number(msg.data) + " deg"); +} + +void PlanningPanel::targetAngleCallback(const std_msgs::Float64& msg) { + target_angle_label_->setText(QString::number(msg.data) + " deg"); +} + +void PlanningPanel::snowDepthCallback(const std_msgs::Float64& msg) { + QString depth = QString::number(msg.data, 'f', 2); + snow_depth_label_->setText("Snow Depth: " + depth + " m"); +} + +void PlanningPanel::mapInfoCallback(const grid_map_geo_msgs::GeographicMapInfo& msg) { + map_origin_(0) = msg.origin_x; + map_origin_(1) = msg.origin_y; + map_origin_(2) = msg.origin_altitude; +} + +void PlanningPanel::sspStateCallback(const std_msgs::Int8& msg) { + ssp_state_label_->setText("SSP State: " + QString::fromStdString(SSPState_string[msg.data])); +} + +void PlanningPanel::callSetAngleService(double angle) { + std::string service_name = "snowsampler/set_landing_leg_angle"; + std::thread t([service_name, angle] { + snowsampler_msgs::SetAngle req; + req.request.angle = angle; + try { + ROS_DEBUG_STREAM("Service name: " << service_name); + if (!ros::service::call(service_name, req)) { + std::cout << "Couldn't call service: " << service_name << std::endl; + } + } catch (const std::exception& e) { + std::cout << "Service Exception: " << e.what() << std::endl; + } + }); + t.detach(); +} + +} // namespace snowsampler_rviz + +#include // NOLINT +PLUGINLIB_EXPORT_CLASS(snowsampler_rviz::PlanningPanel, rviz::Panel) diff --git a/ssp_bridge/CMakeLists.txt b/ssp_bridge/CMakeLists.txt new file mode 100644 index 0000000..b0beb20 --- /dev/null +++ b/ssp_bridge/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.0.2) +project(ssp_bridge) + +# Find catkin macros and libraries +find_package(catkin REQUIRED COMPONENTS + rospy + std_msgs + snowsampler_msgs +) + +# Declare a catkin package +catkin_package( + CATKIN_DEPENDS rospy std_msgs snowsampler_msgs +) + +# Specify additional locations of header files +include_directories( + ${catkin_INCLUDE_DIRS} +) + +# Declare Python scripts to be installed +catkin_install_python(PROGRAMS + src/ssp_bridge/ssp_bridge.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Install all other scripts +install(PROGRAMS + scripts/* + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) \ No newline at end of file diff --git a/ssp_bridge/launch/run.launch b/ssp_bridge/launch/run.launch new file mode 100644 index 0000000..29ee174 --- /dev/null +++ b/ssp_bridge/launch/run.launch @@ -0,0 +1,3 @@ + + + diff --git a/ssp_bridge/package.xml b/ssp_bridge/package.xml new file mode 100644 index 0000000..4488986 --- /dev/null +++ b/ssp_bridge/package.xml @@ -0,0 +1,21 @@ + + + + ssp_bridge + 0.1.1 + a Package to handle the communication between the Snowsmpler and SSP + Claudio Chies + BSD-3 + + + catkin + rospy + snowsampler_msgs + std_msgs + rospy + snowsampler_msgs + std_msgs + rospy + snowsampler_msgs + std_msgs + diff --git a/ssp_bridge/params/params.yaml b/ssp_bridge/params/params.yaml new file mode 100644 index 0000000..0db5694 --- /dev/null +++ b/ssp_bridge/params/params.yaml @@ -0,0 +1,7 @@ +/**: + ros__parameters: + device_name: /dev/ttySsp + baud_rate: 115200 + flow_control: none + parity: none + stop_bits: "1" diff --git a/ssp_bridge/scripts/__init__.py b/ssp_bridge/scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ssp_bridge/setup.py b/ssp_bridge/setup.py new file mode 100644 index 0000000..e3bf4db --- /dev/null +++ b/ssp_bridge/setup.py @@ -0,0 +1,12 @@ +from os.path import abspath, dirname, basename +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +setup_args = generate_distutils_setup( + packages=[basename(dirname(abspath(__file__)))], + package_dir={'': 'src'}, + install_requires=['pyserial', 'threading', 'time'], +) + +setup(**setup_args) + diff --git a/ssp_bridge/src/ssp_bridge/__init__.py b/ssp_bridge/src/ssp_bridge/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/ssp_bridge/src/ssp_bridge/ssp_bridge.py b/ssp_bridge/src/ssp_bridge/ssp_bridge.py new file mode 100644 index 0000000..1286beb --- /dev/null +++ b/ssp_bridge/src/ssp_bridge/ssp_bridge.py @@ -0,0 +1,180 @@ +#!/usr/bin/env python +import rospy +import serial +import threading +import time +from std_msgs.msg import Int8, Float64, UInt8MultiArray +from snowsampler_msgs.srv import ( + TakeMeasurement, + Trigger, + GetPosition, + SetMaxSpeed, + SetMeasurementDepth, + TakeMeasurementResponse, + TriggerResponse, + GetPositionResponse, + SetMaxSpeedResponse, + SetMeasurementDepthResponse, +) + + +class SSPState: + Error = 0 + Ready_To_Measure = 1 + Taking_Measurement = 2 + Stopped_No_Home = 3 + Going_Home = 4 + Moving = 5 + Position_Not_Reached = 6 + ENUM_LENGTH = 7 + + +class SSPBridge: + def __init__(self, port="/dev/ttySsp", baud_rate=115200): + self.state_ = SSPState.Error + self.position_ = 0.0 + self.port = port + self.baud_rate = baud_rate + self.serial = serial.Serial(port, baud_rate, timeout=1) + self.read_thread = threading.Thread(target=self.read_serial) + self.read_thread.daemon = True + self.running = False + self.start_serial() + + # Publishers + self.state_publisher_ = rospy.Publisher("SSP/state", Int8, queue_size=100) + self.position_publisher_ = rospy.Publisher( + "SSP/position", Float64, queue_size=100 + ) + self.serial_publisher_ = rospy.Publisher( + "serial_write", UInt8MultiArray, queue_size=100 + ) + + # Services + self.srv_take_measurement_ = rospy.Service( + "SSP/take_measurement", TakeMeasurement, self.take_measurement_service + ) + self.srv_stop_measurement_ = rospy.Service( + "SSP/stop_measurement", Trigger, self.stop_measurement_service + ) + self.srv_go_home_ = rospy.Service("SSP/go_home", Trigger, self.go_home_service) + self.srv_get_position_ = rospy.Service( + "SSP/get_position", GetPosition, self.get_position_service + ) + self.srv_set_max_speed_ = rospy.Service( + "SSP/set_max_speed", SetMaxSpeed, self.set_max_speed_service + ) + self.srv_set_measurement_depth_ = rospy.Service( + "SSP/set_measurement_depth", + SetMeasurementDepth, + self.set_measurement_depth_service, + ) + + # Serial Subscriber + self.serial_subscriber_ = rospy.Subscriber( + "serial_read", UInt8MultiArray, self.process_serial + ) + rospy.loginfo("ssp_bridge sucessfuly started") + + def start_serial(self): + """Starts the reading thread.""" + self.running = True + self.read_thread.start() + + def stop_serial(self): + """Stops the reading thread.""" + self.running = False + self.serial.close() + + def write_serial(self, data): + """Writes data to the serial device.""" + if self.running: + self.serial.write(f"{data}\n".encode()) + # rospy.loginfo("Sent: " + data) + + def take_measurement_service(self, req): + self.write_serial("take measurement:" + str(req.id)) + return TakeMeasurementResponse(success=True) + + def stop_measurement_service(self, req): + self.write_serial("stop measurement") + return TriggerResponse(success=True) + + def go_home_service(self, req): + self.write_serial("go home") + return TriggerResponse(success=True) + + def get_position_service(self, req): + self.write_serial("get position") + return GetPositionResponse(position=self.position_, success=True) + + def set_max_speed_service(self, req): + self.write_serial("set max speed: " + str(req.data)) + return SetMaxSpeedResponse(success=True) + + def set_measurement_depth_service(self, req): + self.write_serial("set measurement depth: " + str(req.data)) + return SetMeasurementDepthResponse(success=True) + + def read_serial(self): + """Reads data from the serial device.""" + while self.running: + if self.serial.in_waiting: + data = self.serial.readline().decode().strip() + if data: # If data is not empty + # print(f"Received: {data}") + self.process_serial(data) + time.sleep(0.1) + + def process_serial(self, out_str): + + # Find positions of the state and position in the string + state_pos = out_str.find("State: ") + position_pos = out_str.find("Position: ") + + if state_pos != -1 and position_pos != -1: + try: + # Extract state and position from the string + state_str = out_str[ + state_pos + 7 : position_pos - 2 + ] # Adjust indices as needed + position_str = out_str[position_pos + 10 :].replace( + "mm", "" + ) # Remove 'mm' and extract position + + state_num = int(state_str) + position = float(position_str) + + #rospy.loginfo("state_: %d", state_num) + #rospy.loginfo("position_: %f", position) + + # Update state and position if valid + if 0 <= state_num < SSPState.ENUM_LENGTH: + self.state_ = state_num + self.position_ = position + + # Publish state and position + state_msg = Int8() + state_msg.data = state_num + self.state_publisher_.publish(state_msg) + + position_msg = Float64() + position_msg.data = position + self.position_publisher_.publish(position_msg) + + except ValueError as e: + rospy.logerr("Error processing serial data: %s", e) + + def on_shutdown(self): + self.stop_serial() + rospy.loginfo("Shutting down SSP bridge") + + +if __name__ == "__main__": + try: + rospy.init_node("ssp_bridge") + ssp_bridge = SSPBridge() + rospy.on_shutdown(ssp_bridge.on_shutdown) + rospy.spin() + except rospy.ROSInterruptException: + pass diff --git a/systemd/adaptive-snowsampler.service b/systemd/adaptive-snowsampler.service new file mode 100644 index 0000000..95ef4e7 --- /dev/null +++ b/systemd/adaptive-snowsampler.service @@ -0,0 +1,12 @@ +[Unit] +Description=Launch SnowSampler Drone node as a service +After=network.target systemd-udevd.service + +[Service] +EnvironmentFile=/home/user/catkin_ws/src/adaptive-snowsampler/systemd/environment.conf +ExecStart=/bin/bash /home/user/catkin_ws/src/adaptive-snowsampler/systemd/run_controller.sh +User=user +Group=user + +[Install] +WantedBy=multi-user.target \ No newline at end of file diff --git a/systemd/environment.conf b/systemd/environment.conf new file mode 100644 index 0000000..d4c9a53 --- /dev/null +++ b/systemd/environment.conf @@ -0,0 +1,2 @@ +HOME=/home/user +ROS_HOSTNAME=172.30.132.111 diff --git a/systemd/mavlink-router.service b/systemd/mavlink-router.service new file mode 100644 index 0000000..c199330 --- /dev/null +++ b/systemd/mavlink-router.service @@ -0,0 +1,10 @@ +[Unit] +Description=Launch mavlink router as a service +After=network.target systemd-udevd.service + +[Service] +ExecStart=/usr/bin/mavlink-routerd -c /home/user/catkin_ws/src/adaptive-snowsampler/systemd/mavlinkrouter.config +ProtectSystem=full + +[Install] +WantedBy=multi-user.target diff --git a/systemd/mavlinkrouter.config b/systemd/mavlinkrouter.config new file mode 100644 index 0000000..5a1a256 --- /dev/null +++ b/systemd/mavlinkrouter.config @@ -0,0 +1,146 @@ +# mavlink-router configuration file is composed of sections, +# each section has some keys and values. They +# are case insensitive, so `Key=Value` is the same as `key=value`. +# +# [This-is-a-section name-of-section] +# ThisIsAKey=ThisIsAValuye +# +# Following specifications of expected sessions and key/values. +# +# Section [General]: This section doesn't have a name. +# +# Keys: +# TcpServerPort +# A numeric value defining in which port mavlink-router will +# listen for TCP connections. A zero value disables TCP listening. +# Default: 5760 +# +# ReportStats +# Boolean value or case insensitive, or <0> or <1> +# defining if traffic statistics should be reported. +# Default: false +# +# MavlinkDialect +# One of , or . Defines which MAVLink +# dialect is being used by flight stack, so mavlink-router can log +# appropiately. If , mavlink-router will try to decide based +# on heartbeat received from flight stack. +# Default: auto +# +# Log +# Path to directory where to store flightstack log. +# No default value. If absent, no flightstack log will be stored. +# +# LogMode +# One of , +# Default: always, log from start until mavlink-router is stopped. +# If set to while-armed, a new log file is created whenever the vehicle is +# armed, and closed when disarmed. +# +# MinFreeSpace +# The Log Endpoint will delete old log files until there are MinFreeSpace bytes +# available on the storage device of the logs. Set to 0 to ignore this limit. +# Default: 0 (disabled) +# +# MaxLogFiles +# Maximum number of log files to keep. The Log Endpoint will delete old +# log files to keep the total below this number. Set to 0 to ignore this limit. +# Default: 0 (disabled) +# +# DebugLogLevel +# One of , , or . Which debug log +# level is being used by mavlink-router, with being the +# most verbose. +# Default: +# +# Section [UartEndpoint]: This section must have a name +# +# Keys: +# Device +# Path to UART device, like `/dev/ttyPixhawk` +# No default value. Must be defined. +# +# Baud +# Numeric value stating baudrate of UART device +# Default: 115200 +# +# FlowControl +# Boolean value or case insensitive, or <0> or <1> +# defining if flow control should be enabled +# Default: false +# +# +# Section [UdpEndpoint]: This section must have a name +# +# Keys: +# Address +# If on `Normal` mode, IP to which mavlink-router will +# route messages to (and from). If on `Server` mode, +# IP of interface to which mavlink-router will listen for +# incoming packets. In this case, `0.0.0.0` means that +# mavlink-router will listen on all interfaces. +# IPv6 addresses must be enclosed in square brackets. +# No default value. Must be defined. +# +# Mode +# One of or . See `Address` for more +# information. is also accepted for compatibility +# reasons and has the same behavior as . +# No default value. Must be defined +# +# Port +# Numeric value defining in which port mavlink-router will send +# packets (or listen for them). +# Default value: Increasing value, starting from 14550, when +# mode is `Normal`. Must be defined if on `Server` mode: mavlink-router +# will bind to this port and wait for a first packet to be received to +# know to where it needs to send packets. +# +# Section [TcpEndpoint]: This section must have a name +# +# Keys: +# Address: +# IP to which mavlink-router will connect to. +# IPv6 addresses must be enclosed in square brackets. +# No default value. Must be defined. +# +# Port: +# Numeric value with port to which mavlink-router will connect to. +# No default value. Must be defined. +# +# RetryTimeout: +# Numeric value defining how many seconds mavlink-router should wait +# to reconnect to IP in case of disconnection. A value of 0 disables +# reconnection. +# Default value: 5 +# +# Following, an example of configuration file: +[General] +#Mavlink-router serves on this TCP port +TcpServerPort=5760 +ReportStats=false +MavlinkDialect=auto + +[UartEndpoint alpha] +Device=/dev/ttyPixhawk +Baud=921600 + +[UdpEndpoint beta] +Mode = Normal +Address = 127.0.0.1 +Port = 14540 + +[UdpEndpoint gcs] +Mode = Normal +Address = 172.30.181.0 +Port = 14550 + +[UdpEndpoint gcs2] +Mode = Normal +Address = 172.30.146.222 +Port = 14550 + +[UdpEndpoint gcs3] +Mode = Normal +Address = 172.30.255.38 +Port = 14550 \ No newline at end of file diff --git a/systemd/rosbag-record.service b/systemd/rosbag-record.service new file mode 100644 index 0000000..27280d2 --- /dev/null +++ b/systemd/rosbag-record.service @@ -0,0 +1,11 @@ +[Unit] +Description=Launch terrain navigation node as a service +After=network.target systemd-udevd.service + +[Service] +EnvironmentFile=/home/user/catkin_ws/src/adaptive-snowsampler/systemd/environment.conf +ExecStart=/bin/bash /home/user/catkin_ws/src/adaptive-snowsampler/systemd/run_record.sh +KillSignal=SIGINT + +[Install] +WantedBy=multi-user.target \ No newline at end of file diff --git a/systemd/run_controller.sh b/systemd/run_controller.sh new file mode 100644 index 0000000..ba9dd02 --- /dev/null +++ b/systemd/run_controller.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +set -e + +source $HOME/catkin_ws/devel/setup.bash +roslaunch adaptive_snowsampler run.launch diff --git a/systemd/run_record.sh b/systemd/run_record.sh new file mode 100644 index 0000000..e33aa0a --- /dev/null +++ b/systemd/run_record.sh @@ -0,0 +1,9 @@ +#!/bin/bash + +set -e + + +gst-launch-1.0 -e rtspsrc location=rtsp://127.0.0.1:8553/stream protocols=tcp ! rtph264depay ! h264parse ! mp4mux ! filesink location=~/rosbag/onboard_$(date +"%Y_%m_%d_%H_%M_%S").mp4& + +source $HOME/catkin_ws/devel/setup.bash +roslaunch adaptive_snowsampler run_rosbag.launch diff --git a/systemd/run_ssp_bridge.sh b/systemd/run_ssp_bridge.sh new file mode 100644 index 0000000..221b2e7 --- /dev/null +++ b/systemd/run_ssp_bridge.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +set -e + +source $HOME/catkin_ws/devel/setup.bash +roslaunch ssp_bridge run.launch diff --git a/systemd/run_system_monitor.sh b/systemd/run_system_monitor.sh new file mode 100644 index 0000000..ec6b101 --- /dev/null +++ b/systemd/run_system_monitor.sh @@ -0,0 +1,6 @@ +#!/bin/bash + +set -e + +source $HOME/catkin_ws/devel/setup.bash +roslaunch system_monitor_ros system_monitor.launch diff --git a/systemd/ssp-bridge.service b/systemd/ssp-bridge.service new file mode 100644 index 0000000..682b66e --- /dev/null +++ b/systemd/ssp-bridge.service @@ -0,0 +1,13 @@ +[Unit] +Description=Launch SSP Bridge node as a service +After=network.target systemd-udevd.service +Requires=udev.service + +[Service] +EnvironmentFile=/home/user/catkin_ws/src/adaptive-snowsampler/systemd/environment.conf +ExecStart=/bin/bash /home/user/catkin_ws/src/adaptive-snowsampler/systemd/run_ssp_bridge.sh +User=user +Group=user + +[Install] +WantedBy=multi-user.target udev.service diff --git a/systemd/system-monitor.service b/systemd/system-monitor.service new file mode 100644 index 0000000..1d1f215 --- /dev/null +++ b/systemd/system-monitor.service @@ -0,0 +1,12 @@ +[Unit] +Description=Launch auterion system monitor node as a service +After=network.target systemd-udevd.service + +[Service] +EnvironmentFile=/home/user/catkin_ws/src/adaptive-snowsampler/systemd/environment.conf +ExecStart=/bin/bash /home/user/catkin_ws/src/adaptive-snowsampler/systemd/run_system_monitor.sh +User=user +Group=user + +[Install] +WantedBy=multi-user.target \ No newline at end of file