diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index db27116..09797ed 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,7 +1,7 @@ // For format details, see https://aka.ms/vscode-remote/devcontainer.json or this file's README at: // https://github.com/microsoft/vscode-dev-containers/tree/v0.128.0/containers/docker-existing-dockerfile { - "name": "", + "name": "provider_underwater_com", // Sets the run context to one level up instead of the .devcontainer folder. "context": "..", // Update the 'dockerFile' property if you aren't using the standard 'Dockerfile' filename. @@ -19,12 +19,14 @@ "--security-opt", "seccomp=unconfined", "--rm", + "--privileged", "--network", "host" ], "mounts": [ - "source=vscode-server-extension,target=/home/sonia/.vscode-server/extensions,type=volume" + "source=vscode-server-extension,target=/home/sonia/.vscode-server/extensions,type=volume", + "source=/dev,target=/dev,type=bind" ], - "workspaceMount": "target=/home/sonia/ros_sonia_ws/src/,type=volume", - "workspaceFolder": "/home/sonia/ros_sonia_ws/src/" + "workspaceMount": "target=/home/sonia/ros_sonia_ws/src/provider_underwater_com,type=volume", + "workspaceFolder": "/home/sonia/ros_sonia_ws/src/provider_underwater_com" } diff --git a/.github/workflows/docker-image-perception-develop.yml b/.github/workflows/docker-image-perception-develop.yml index b06a6dc..6f98854 100644 --- a/.github/workflows/docker-image-perception-develop.yml +++ b/.github/workflows/docker-image-perception-develop.yml @@ -13,7 +13,7 @@ jobs: ARCH: x86 TARGET_TYPE: perception TARGET_VERSION: develop - IMAGE_NAME: template_name + IMAGE_NAME: provider_underwater_com GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} steps: - uses: actions/checkout@v2 @@ -45,7 +45,7 @@ jobs: ARCH: arm64 TARGET_TYPE: perception TARGET_VERSION: develop - IMAGE_NAME: template_name + IMAGE_NAME: provider_underwater_com GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} steps: - uses: actions/checkout@v2 diff --git a/.github/workflows/docker-image-perception-feature.yml b/.github/workflows/docker-image-perception-feature.yml index bdf5538..dd79b7d 100644 --- a/.github/workflows/docker-image-perception-feature.yml +++ b/.github/workflows/docker-image-perception-feature.yml @@ -13,7 +13,7 @@ jobs: ARCH: x86 TARGET_TYPE: perception TARGET_VERSION: feature - IMAGE_NAME: template_name + IMAGE_NAME: provider_underwater_com GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} steps: - uses: actions/checkout@v2 @@ -40,7 +40,7 @@ jobs: ARCH: arm64 TARGET_TYPE: perception TARGET_VERSION: feature - IMAGE_NAME: template_name + IMAGE_NAME: provider_underwater_com GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} steps: - uses: actions/checkout@v2 diff --git a/.github/workflows/docker-image-perception-fix.yml b/.github/workflows/docker-image-perception-fix.yml new file mode 100644 index 0000000..31989f3 --- /dev/null +++ b/.github/workflows/docker-image-perception-fix.yml @@ -0,0 +1,103 @@ +name: Docker Image CI - Fix Branch + +on: + push: + branches: [fix/**] + +jobs: + build-ros-perception-x86-64: + name: "Build ROS perception X86/64" + runs-on: ubuntu-latest + env: + BASE_IMAGE: "docker.pkg.github.com/sonia-auv/sonia_common/sonia_common:x86-perception-latest" + ARCH: x86 + TARGET_TYPE: perception + TARGET_VERSION: fix + IMAGE_NAME: provider_underwater_com + GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} + steps: + - uses: actions/checkout@v2 + - name: Login to Github Package Registry + run: | + echo "${{ secrets.GITHUB_TOKEN }}" | docker login docker.pkg.github.com -u ${{ github.actor }} --password-stdin + - name: Build the docker image (perception based) + run: | + docker build . --tag build-fix-${GITHUB_REF##*/}-${GITHUB_RUN_NUMBER} --build-arg BUILD_DATE=$(date '+%Y-%m-%d_%H:%M:%S') --build-arg VERSION=${GITHUB_REF##*/}-$(date ' +%Y-%m-%d_%H:%M:%S') --build-arg BASE_IMAGE=${BASE_IMAGE} + + - name: Create Docker Image Tag + run: | + docker tag build-fix-${GITHUB_REF##*/}-${GITHUB_RUN_NUMBER} ${GITHUB_REMOTE_URL}/${IMAGE_NAME}:${ARCH}-${TARGET_TYPE}-${TARGET_VERSION}-${GITHUB_REF##*/} + + - name: Push Image to Github Packages Registry + run: | + docker push --all-tags ${GITHUB_REMOTE_URL}/${IMAGE_NAME} + + build-ros-perception-arm64: + name: "Build ROS perception ARM64" + runs-on: ubuntu-latest + env: + BASE_IMAGE: "docker.pkg.github.com/sonia-auv/sonia_common/sonia_common:arm64-perception-latest" + ARCH: arm64 + TARGET_TYPE: perception + TARGET_VERSION: fix + IMAGE_NAME: provider_underwater_com + GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} + steps: + - uses: actions/checkout@v2 + - name: Login to Github Package Registry + run: | + echo "${{ secrets.GITHUB_TOKEN }}" | docker login docker.pkg.github.com -u ${{ github.actor }} --password-stdin + - name: Enable Docker Daemon Experimental Fixes + run: | + sudo rm /etc/docker/daemon.json + echo '{"experimental": true , "cgroup-parent": "/actions_job" }' | sudo tee -a /etc/docker/daemon.json + sudo service docker restart + docker version + + - name: Install QEMU to be able to compile on X86 into ARM64 + run: | + sudo apt-get update + sudo apt-get install qemu binfmt-support qemu-user-static + docker run --rm --privileged multiarch/qemu-user-static --reset -p yes + + - name: Build the docker image (perception based) + run: | + docker build . --tag build-fix-${GITHUB_REF##*/}-${GITHUB_RUN_NUMBER} --build-arg BUILD_DATE=$(date '+%Y-%m-%d_%H:%M:%S') --build-arg VERSION=${GITHUB_REF##*/}-$(date ' +%Y-%m-%d_%H:%M:%S') --build-arg BASE_IMAGE=${BASE_IMAGE} + + - name: Create Docker Image Tag + run: | + docker tag build-fix-${GITHUB_REF##*/}-${GITHUB_RUN_NUMBER} ${GITHUB_REMOTE_URL}/${IMAGE_NAME}:${ARCH}-${TARGET_TYPE}-${TARGET_VERSION}-${GITHUB_REF##*/} + + - name: Push Image to Github Packages Registry + run: | + docker push --all-tags ${GITHUB_REMOTE_URL}/${IMAGE_NAME} + + notify-success: + name: "Notify Slack - Success" + runs-on: ubuntu-latest + needs: [build-ros-perception-x86-64, build-ros-perception-arm64] + if: success() + steps: + - name: Notify Slack Success + env: + SLACK_BOT_TOKEN: ${{ secrets.SLACK_BOT_TOKEN }} + uses: voxmedia/github-action-slack-notify-build@v1 + with: + channel: github-ci-notifications + status: SUCCESS + color: good + + notify-fail: + name: "Notify Slack - Failure" + runs-on: ubuntu-latest + needs: [build-ros-perception-x86-64, build-ros-perception-arm64] + if: failure() + steps: + - name: Notify Slack Fail + env: + SLACK_BOT_TOKEN: ${{ secrets.SLACK_BOT_TOKEN }} + uses: voxmedia/github-action-slack-notify-build@v1 + with: + channel: github-ci-notifications + status: FAILED + color: danger diff --git a/.github/workflows/docker-image-perception-master.yml b/.github/workflows/docker-image-perception-master.yml index c45f072..c654d17 100644 --- a/.github/workflows/docker-image-perception-master.yml +++ b/.github/workflows/docker-image-perception-master.yml @@ -12,7 +12,7 @@ jobs: BASE_IMAGE: "docker.pkg.github.com/sonia-auv/sonia_common/sonia_common:x86-perception-latest" ARCH: x86 TARGET_TYPE: perception - IMAGE_NAME: template_name + IMAGE_NAME: provider_underwater_com GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} steps: - uses: actions/checkout@v2 @@ -48,7 +48,7 @@ jobs: BASE_IMAGE: "docker.pkg.github.com/sonia-auv/sonia_common/sonia_common:arm64-perception-latest" ARCH: arm64 TARGET_TYPE: perception - IMAGE_NAME: template_name + IMAGE_NAME: provider_underwater_com GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} steps: - uses: actions/checkout@v2 diff --git a/.vscode/launch.json b/.vscode/launch.json index 2c39fb0..74e813b 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -9,7 +9,7 @@ "type": "ros", "request": "launch", "preLaunchTask": "build", - "target": "/home/sonia/ros_sonia_ws/src//launch/.launch", + "target": "/home/sonia/ros_sonia_ws/src/provider_underwater_com/launch/provider_underwater_com.launch", } ] -} +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index e542113..c9a9be3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,5 +2,71 @@ "python.autoComplete.extraPaths": [ "/home/sonia/base_lib_ws/devel/lib/python2.7/dist-packages", "/opt/ros/melodic/lib/python2.7/dist-packages" - ] + ], + "python.analysis.extraPaths": [ + "/home/sonia/base_lib_ws/devel/lib/python2.7/dist-packages", + "/opt/ros/melodic/lib/python2.7/dist-packages" + ], + "files.associations": { + "cctype": "cpp", + "clocale": "cpp", + "cmath": "cpp", + "cstdarg": "cpp", + "cstddef": "cpp", + "cstdio": "cpp", + "cstdlib": "cpp", + "cstring": "cpp", + "ctime": "cpp", + "cwchar": "cpp", + "cwctype": "cpp", + "array": "cpp", + "atomic": "cpp", + "strstream": "cpp", + "*.tcc": "cpp", + "bitset": "cpp", + "chrono": "cpp", + "complex": "cpp", + "condition_variable": "cpp", + "cstdint": "cpp", + "deque": "cpp", + "list": "cpp", + "unordered_map": "cpp", + "vector": "cpp", + "exception": "cpp", + "fstream": "cpp", + "functional": "cpp", + "initializer_list": "cpp", + "iomanip": "cpp", + "iosfwd": "cpp", + "iostream": "cpp", + "istream": "cpp", + "limits": "cpp", + "mutex": "cpp", + "new": "cpp", + "ostream": "cpp", + "numeric": "cpp", + "ratio": "cpp", + "sstream": "cpp", + "stdexcept": "cpp", + "streambuf": "cpp", + "system_error": "cpp", + "thread": "cpp", + "cfenv": "cpp", + "cinttypes": "cpp", + "tuple": "cpp", + "type_traits": "cpp", + "utility": "cpp", + "typeindex": "cpp", + "typeinfo": "cpp", + "algorithm": "cpp", + "iterator": "cpp", + "map": "cpp", + "memory": "cpp", + "memory_resource": "cpp", + "optional": "cpp", + "random": "cpp", + "set": "cpp", + "string": "cpp", + "string_view": "cpp" + } } diff --git a/BOOTSTRAP.md b/BOOTSTRAP.md deleted file mode 100644 index c1dcde1..0000000 --- a/BOOTSTRAP.md +++ /dev/null @@ -1,53 +0,0 @@ -# Templated solution -*Remember this is a template repository, you will have to remove this documentation once you are done preparing your repository* - -**ROS Package Repo Template** is used to predefine certain values in your repository by importing this repository to your new module. -The template should be used to create a ROS Module. Depending on whether the module you want to create should be used for "perception" modules please follow the right instructions - -## Bootstrap your project -Once you have used the `ros-package-repo-template` to create your own package repository you can now use the `get_started.sh` script. - -For further informations on how to use the script you can simply run : -``` -$ ./get_started.sh -h -``` -Which should output : -``` -USAGE: ./get_started.sh [flags] -flags: - -n The name of your package (Required) (default: '') - -t The type of project to be created (e.g. perception) (default: 'perception') - -g If the package needs GPU (default: false) - -c Command to be executed by the script (e.g. create or prepare) (default: 'create') - -d Give extra dependencies separated by commas (e.g. sonia_common,turtlebot3,qt_dotgraph) (default: 'sonia_common') - -h show this help (default: false) -``` -### Preparing your repository -**IMPORTANT** *: We recommend using the `ros:melodic-ros-core` in order to run the script. This allows to have a valid and genuine installation of ROS to execute the script.* -*If you already have ROS installed on your computer you can try to simply run the script `./get_started.sh [flags]`* - -#### Project perception (non-GPU) -``` -$ cd /path/to/your/repository -$ docker run --rm -it -v $(pwd):/tmp ros:melodic-ros-core /bin/sh -c 'cd /tmp && ./get_started.sh -n [your_desired_node_name] -c prepare' -``` -#### Project perception (GPU required) -``` -$ cd path/to/your/repository -$ docker run --rm -it -v $(pwd):/tmp ros:melodic-ros-core /bin/sh -c 'cd /tmp && ./get_started.sh -n [your_desired_node_name] -c prepare -g true' -``` - -### Creating your ROS basic files -#### Standard procedure -By default it is recommend to only have `sonia_common` as a dependency. To create your ROS files for a standard module run : -``` -$ cd /path/to/your/repository -$ docker run --rm -it -v $(pwd):/tmp ros:melodic-ros-core /bin/sh -c 'cd /tmp && ./get_started.sh -n [your_desired_node_name] -c create' -``` -#### Specifying extra dependencies -In some cases you might need extra dependencies. The flag `-d` allows to override the included dependencies. Here is an example on how to specify your own dependencies (no spaces and seperated by commas). -``` -$ cd /path/to/your/repository -$ docker run --rm -it -v $(pwd):/tmp ros:melodic-ros-core /bin/sh -c 'cd /tmp && ./get_started.sh -n [your_desired_node_name] -c create -d sonia_common,turtlebot3,qt_dotgraph' -``` -*NOTE : In most cases you will need to include `sonia_common`. In doubt, leave it in the list of dependencies.* diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..aa42b0a --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,97 @@ +# \file CMakeLists.txt +# \author Thibaut Mattio +# \date 08/05/2015 +# +# \copyright Copyright (c) 2015 S.O.N.I.A. All rights reserved. +# +# \section LICENSE +# +# This file is part of S.O.N.I.A. software. +# +# S.O.N.I.A. software is free software: you can redistribute it and/or modify +# it under the terms of the GNU General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# S.O.N.I.A. software is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU General Public License for more details. +# +# You should have received a copy of the GNU General Public License +# along with S.O.N.I.A. software. If not, see . + +#=============================================================================== +# G L O B A L P R O J E C T C O N F I G U R A T I O N + +cmake_minimum_required(VERSION 2.8.3) +project(provider_underwater_com) + +if (NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Debug CACHE STRING + "Choose the type of build, options are: None Debug Release RelWithDebInfo MinSizeRel." + FORCE) +endif (NOT CMAKE_BUILD_TYPE) + +set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -std=c++11 -Wall") +set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -std=c++11 -O3 -s") + +set(provider_underwater_com_SRC_DIR "src") + +#=============================================================================== +# C A T K I N C O N F I G U R A T I O N + +find_package(catkin REQUIRED COMPONENTS + roscpp + self_test + diagnostic_updater + tf + std_msgs + sensor_msgs + message_generation + sonia_common + ) + +find_library(LOG4CXX_LIBRARY log4cxx) +if (NOT LOG4CXX_LIBRARY) + message(FATAL_ERROR "Couldn't find log4cxx library") +endif () + +catkin_package( + INCLUDE_DIRS ${provider_underwater_com_SRC_DIR} + LIBRARIES + CATKIN_DEPENDS + roscpp + self_test + diagnostic_updater + tf + std_msgs + sensor_msgs + message_runtime + sonia_common +) + +#=============================================================================== +# I N C L U D E F I L E S + +file(GLOB_RECURSE provider_underwater_com_FILES "${provider_underwater_com_SRC_DIR}/*.cc" "${provider_underwater_com_SRC_DIR}/*.h") + +include_directories( + ${catkin_INCLUDE_DIRS} + ${provider_underwater_com_SRC_DIR} + ${sonia_common_INCLUDE_DIRS} +) + +#========================================================================== +# C R E A T E E X E C U T A B L E + +add_executable(${PROJECT_NAME}_node ${provider_underwater_com_FILES}) +target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${sonia_common_LIBRARIES}) +add_dependencies(${PROJECT_NAME}_node sonia_common_generate_messages_cpp) + +#========================================================================== +# U N I T T E S T S + +if (CATKIN_ENABLE_TESTING AND ${CMAKE_CURRENT_SOURCE_DIR}/test) + add_subdirectory(${CMAKE_CURRENT_SOURCE_DIR}/test) +endif () \ No newline at end of file diff --git a/Dockerfile b/Dockerfile index c33bc58..5c22f73 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -ARG BASE_IMAGE="docker.pkg.github.com/sonia-auv/sonia_common/sonia_common:x86-perception-latest" +ARG BASE_IMAGE="ghcr.io/sonia-auv/sonia_common/sonia_common:x86-perception-latest" FROM ${BASE_IMAGE} @@ -7,7 +7,7 @@ USER root ARG BUILD_DATE ARG VERSION -ENV NODE_NAME= +ENV NODE_NAME=provider_underwater_com LABEL net.etsmtl.sonia-auv.node.build-date=${BUILD_DATE} LABEL net.etsmtl.sonia-auv.node.version=${VERSION} @@ -32,6 +32,7 @@ COPY . ${NODE_PATH} RUN bash -c "source ${ROS_WS_SETUP}; source ${BASE_LIB_WS_SETUP}; catkin_make" RUN chown -R ${SONIA_USER}: ${SONIA_WS} +RUN usermod -a -G dialout ${SONIA_USER} USER ${SONIA_USER} RUN mkdir ${SCRIPT_DIR} diff --git a/README.md b/README.md index 391e9f1..a4e1112 100644 --- a/README.md +++ b/README.md @@ -1,9 +1,9 @@ -# +# Provider Underwater COM -![Docker Image CI - Master Branch](https://github.com/sonia-auv//workflows/Docker%20Image%20CI%20-%20Master%20Branch/badge.svg) -![Docker Image CI - Develop Branch](https://github.com/sonia-auv//workflows/Docker%20Image%20CI%20-%20Develop%20Branch/badge.svg?branch=develop) -![GitHub release (latest by date)](https://img.shields.io/github/v/release/sonia-auv/) -![Average time to resolve an issue](https://isitmaintained.com/badge/resolution/sonia-auv/.svg) +![Docker Image CI - Master Branch](https://github.com/sonia-auv/provider_underwater_com/workflows/Docker%20Image%20CI%20-%20Master%20Branch/badge.svg) +![Docker Image CI - Develop Branch](https://github.com/sonia-auv/provider_underwater_com/workflows/Docker%20Image%20CI%20-%20Develop%20Branch/badge.svg?branch=develop) +![GitHub release (latest by date)](https://img.shields.io/github/v/release/sonia-auv/provider_underwater_com) +![Average time to resolve an issue](https://isitmaintained.com/badge/resolution/sonia-auv/provider_underwater_com.svg) *Please read the instructions and fill in the blanks* @@ -15,7 +15,7 @@ One Paragraph of project description goes here Clone current project by using following command : ```bash - git clone git@github.com:sonia-auv/.git + git clone git@github.com:sonia-auv/provider_underwater_com.git ``` These instructions will get you a copy of the project up and running on your local machine for development and testing purposes. See deployment for notes on how to deploy the project on a live system. diff --git a/config/default.yaml b/config/default.yaml new file mode 100644 index 0000000..28d07ce --- /dev/null +++ b/config/default.yaml @@ -0,0 +1,7 @@ +#********** Driver Parameters ************ + +/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" + +/provider_underwater_com/settings/role : "a" + +/provider_underwater_com/settings/channel : "4" \ No newline at end of file diff --git a/get_started.sh b/get_started.sh deleted file mode 100755 index 498d31e..0000000 --- a/get_started.sh +++ /dev/null @@ -1,94 +0,0 @@ -#!/bin/bash -# ------------------------------------------------------------------ -# Author : Kevin Charbonneau -# *get_started.sh* -# This script allows the users to create a package from the -# ROS package repo template. -# -# This script uses shFlags -- Advanced command-line flag -# library for Unix shell scripts. -# http://code.google.com/p/shflags/ -# -# Dependency: -# http://shflags.googlecode.com/svn/trunk/source/1.0/src/shflags -# ------------------------------------------------------------------ - -. ./scripts/shflags - -DEFINE_string 'name' '' 'The name of your package (Required)' 'n' -DEFINE_string 'type' 'perception' 'The type of project to be created (e.g. perception or other)' 't' -DEFINE_boolean 'gpu' 'false' 'If the package needs GPU' 'g' -DEFINE_string 'command' 'create' 'Command to be executed by the script (e.g. create or prepare)' 'c' -DEFINE_string 'dep' 'sonia_common' 'Give extra dependencies separated by colons (e.g. sonia_common,turtlebot3,qt_dotgraph)' 'd' -FLAGS_HELP="USAGE: $0 [flags]" - -main() { - if [ -z $FLAGS_name ]; then - echo "You must provide a name for your package" - echo "" - flags_help - exit 1 - fi - if [ $FLAGS_command == "create" ]; then - CREATE_PKG_COMMAND="catkin_create_pkg ${FLAGS_name}" - IFS=',' read -ra ADDR <<< "$FLAGS_dep" - for i in "${ADDR[@]}"; do - CREATE_PKG_COMMAND="${CREATE_PKG_COMMAND} ${i}" - done - $CREATE_PKG_COMMAND - mv $FLAGS_name/* . - rm -r $FLAGS_name - elif [ $FLAGS_command == "prepare" ]; then - clean_workflows "$@" - insert_name "$@" - else - echo "The command '${FLAGS_command}' does not exist" - echo "" - flags_help - exit 1 - fi -} - -clean_workflows() { - if [ $FLAGS_gpu == true ]; then - if [ $FLAGS_type == "perception" ]; then - rm .github/workflows/docker-image-perception-feature.yml - rm .github/workflows/docker-image-perception-develop.yml - rm .github/workflows/docker-image-perception-master.yml - else - echo "${FLAGS_type} is an invalid type of package." - echo "Provide either perception or other" - echo "" - flags_help - exit 1 - fi - else - if [ $FLAGS_type == "perception" ]; then - rm .github/workflows/docker-image-perception-l4t* - else - echo "${FLAGS_type} is an invalid type of package." - echo "Provide either perception or other" - echo "" - flags_help - exit 1 - fi - fi -} - -insert_name() { - for i in .github/workflows/*; do - sed -i.bak "s//${FLAGS_name}/g" "${i}" && rm "${i}.bak" - done - - sed -i.bak "s//${FLAGS_name}/g" "Dockerfile" && rm Dockerfile.bak - sed -i.bak "s//${FLAGS_name}/g" ".vscode/launch.json" && rm .vscode/launch.json.bak - sed -i.bak "s//${FLAGS_name}/g" ".devcontainer/devcontainer.json" && rm .devcontainer/devcontainer.json.bak - sed -i.bak "s//${FLAGS_name}/g" "README.md" && rm README.md.bak - sed -i.bak "s//${FLAGS_name}/g" "LICENSE" && rm LICENSE.bak - sed -i.bak "s//$(date +'%Y')/g" "LICENSE" && rm LICENSE.bak -} - -FLAGS "$@" || exit $? -eval set -- "${FLAGS_ARGV}" -main "$@" -# TODO: It could be good to add error handling and prints to the script diff --git a/launch/provider_underwater_com.launch b/launch/provider_underwater_com.launch new file mode 100644 index 0000000..60ead1c --- /dev/null +++ b/launch/provider_underwater_com.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/package.xml b/package.xml new file mode 100644 index 0000000..2726c75 --- /dev/null +++ b/package.xml @@ -0,0 +1,34 @@ + + + provider_underwater_com + 0.6.1 + The provider_underwater_com package + + Francis Alonzo + + Francis Alonzo + GPLv3 + + catkin + + roscpp + log4cxx + self_test + diagnostic_updater + tf + std_msgs + sensor_msgs + message_generation + sonia_common + + roscpp + log4cxx + self_test + diagnostic_updater + tf + std_msgs + sensor_msgs + message_runtime + sonia_common + + diff --git a/scripts/shflags b/scripts/shflags deleted file mode 100644 index 2949d02..0000000 --- a/scripts/shflags +++ /dev/null @@ -1,1214 +0,0 @@ -# vim:et:ft=sh:sts=2:sw=2 -# -# Copyright 2008-2020 Kate Ward. All Rights Reserved. -# Released under the Apache License 2.0 license. -# http://www.apache.org/licenses/LICENSE-2.0 -# -# shFlags is an advanced command-line flag library for Unix shell scripts. -# -# Author: kate.ward@forestent.com (Kate Ward) -# https://github.com/kward/shflags -# -# This module implements something like the gflags library available -# from https://github.com/gflags/gflags. -# -# FLAG TYPES: This is a list of the DEFINE_*'s that you can do. All flags take -# a name, default value, help-string, and optional 'short' name (one-letter -# name). Some flags have other arguments, which are described with the flag. -# -# DEFINE_string: takes any input, and interprets it as a string. -# -# DEFINE_boolean: does not take any arguments. Say --myflag to set -# FLAGS_myflag to true, or --nomyflag to set FLAGS_myflag to false. For short -# flags, passing the flag on the command-line negates the default value, i.e. -# if the default is true, passing the flag sets the value to false. -# -# DEFINE_float: takes an input and interprets it as a floating point number. As -# shell does not support floats per-se, the input is merely validated as -# being a valid floating point value. -# -# DEFINE_integer: takes an input and interprets it as an integer. -# -# SPECIAL FLAGS: There are a few flags that have special meaning: -# --help (or -?) prints a list of all the flags in a human-readable fashion -# --flagfile=foo read flags from foo. (not implemented yet) -# -- as in getopt(), terminates flag-processing -# -# EXAMPLE USAGE: -# -# -- begin hello.sh -- -# #! /bin/sh -# . ./shflags -# DEFINE_string name 'world' "somebody's name" n -# FLAGS "$@" || exit $? -# eval set -- "${FLAGS_ARGV}" -# echo "Hello, ${FLAGS_name}." -# -- end hello.sh -- -# -# $ ./hello.sh -n Kate -# Hello, Kate. -# -# CUSTOMIZABLE BEHAVIOR: -# -# A script can override the default 'getopt' command by providing the path to -# an alternate implementation by defining the FLAGS_GETOPT_CMD variable. -# -# NOTES: -# -# * Not all systems include a getopt version that supports long flags. On these -# systems, only short flags are recognized. - -#============================================================================== -# shFlags -# -# Shared attributes: -# flags_error: last error message -# flags_output: last function output (rarely valid) -# flags_return: last return value -# -# __flags_longNames: list of long names for all flags -# __flags_shortNames: list of short names for all flags -# __flags_boolNames: list of boolean flag names -# -# __flags_opts: options parsed by getopt -# -# Per-flag attributes: -# FLAGS_: contains value of flag named 'flag_name' -# __flags__default: the default flag value -# __flags__help: the flag help string -# __flags__short: the flag short name -# __flags__type: the flag type -# -# Notes: -# - lists of strings are space separated, and a null value is the '~' char. -# -### ShellCheck (http://www.shellcheck.net/) -# expr may be antiquated, but it is the only solution in some cases. -# shellcheck disable=SC2003 -# $() are not fully portable (POSIX != portable). -# shellcheck disable=SC2006 -# [ p -a q ] are well defined enough (vs [ p ] && [ q ]). -# shellcheck disable=SC2166 - -# Return if FLAGS already loaded. -if [ -n "${FLAGS_VERSION:-}" ]; then return 0; fi -FLAGS_VERSION='1.4.0pre' - -# Return values that scripts can use. -FLAGS_TRUE=0 -FLAGS_FALSE=1 -FLAGS_ERROR=2 - -# shlib_expr_cmd determines a reasonable default `expr` command. -# https://github.com/kward/shlib -# -# Use like: -# EXPR_CMD=$(shlib_expr_cmd) -# ${EXPR_CMD} 1 + 1 -# -# Args: -# none -# Output: -# string: expr command -# Return -# int: 0 upon success -shlib_expr_cmd() { - if [ "$(uname -s)" = 'BSD' ]; then - echo 'gexpr --' - return 0 - fi - - _shlib_expr_cmd_='expr --' - # shellcheck disable=SC2003 - if _shlib_output_=$(${_shlib_expr_cmd_} 2>&1); then - if [ "${_shlib_output_}" = '--' ]; then - # We are likely running inside BusyBox. - _shlib_expr_cmd_='expr' - fi - fi - - echo "${_shlib_expr_cmd_}" - unset _shlib_expr_cmd_ _shlib_output_ -} -__FLAGS_EXPR_CMD=`shlib_expr_cmd` - -# Commands a user can override if desired. -FLAGS_EXPR_CMD=${FLAGS_EXPR_CMD:-${__FLAGS_EXPR_CMD}} -FLAGS_GETOPT_CMD=${FLAGS_GETOPT_CMD:-getopt} - -# -# Logging functions. -# - -# Logging levels. -FLAGS_LEVEL_DEBUG=0 -FLAGS_LEVEL_INFO=1 -FLAGS_LEVEL_WARN=2 -FLAGS_LEVEL_ERROR=3 -FLAGS_LEVEL_FATAL=4 -__FLAGS_LEVEL_DEFAULT=${FLAGS_LEVEL_WARN} -__flags_level=${__FLAGS_LEVEL_DEFAULT} # Current logging level. - -_flags_debug() { - if [ ${__flags_level} -le ${FLAGS_LEVEL_DEBUG} ]; then echo "flags:DEBUG $*" >&2; fi -} -_flags_info() { - if [ ${__flags_level} -le ${FLAGS_LEVEL_INFO} ]; then echo "flags:INFO $*" >&2; fi -} -_flags_warn() { - if [ ${__flags_level} -le ${FLAGS_LEVEL_WARN} ]; then echo "flags:WARN $*" >&2; fi -} -_flags_error() { - if [ ${__flags_level} -le ${FLAGS_LEVEL_ERROR} ]; then echo "flags:ERROR $*" >&2; fi -} -_flags_fatal() { - echo "flags:FATAL $*" >&2 - exit ${FLAGS_ERROR} -} - -# Get the logging level. -flags_loggingLevel() { echo ${__flags_level}; } - -# Set the logging level by overriding the `__flags_level` variable. -# -# Args: -# _flags_level_: integer: new logging level -# Returns: -# nothing -flags_setLoggingLevel() { - [ $# -ne 1 ] && _flags_fatal "flags_setLevel(): logging level missing" - _flags_level_=$1 - if ! [ "${_flags_level_}" -ge "${FLAGS_LEVEL_DEBUG}" -a "${_flags_level_}" -le "${FLAGS_LEVEL_FATAL}" ]; then - _flags_fatal "Invalid logging level '${_flags_level_}' specified." - fi - __flags_level=$1 - unset _flags_level_ -} - -# -# Shell checks. -# - -if [ -n "${ZSH_VERSION:-}" ]; then - setopt |grep "^shwordsplit$" >/dev/null - if [ $? -ne ${FLAGS_TRUE} ]; then - _flags_fatal 'zsh shwordsplit option is required for proper zsh operation' - fi - if [ -z "${FLAGS_PARENT:-}" ]; then - _flags_fatal "zsh does not pass \$0 through properly. please declare' \ -\"FLAGS_PARENT=\$0\" before calling shFlags" - fi -fi - -# Can we use built-ins? -( echo "${FLAGS_TRUE#0}"; ) >/dev/null 2>&1 -if [ $? -eq ${FLAGS_TRUE} ]; then - __FLAGS_USE_BUILTIN=${FLAGS_TRUE} -else - __FLAGS_USE_BUILTIN=${FLAGS_FALSE} -fi - -# -# Constants. -# - -# Reserved flag names. -__FLAGS_RESERVED_LIST=' ARGV ERROR FALSE GETOPT_CMD HELP PARENT TRUE ' -__FLAGS_RESERVED_LIST="${__FLAGS_RESERVED_LIST} VERSION " - -# Determined getopt version (standard or enhanced). -__FLAGS_GETOPT_VERS_STD=0 -__FLAGS_GETOPT_VERS_ENH=1 - -# shellcheck disable=SC2120 -_flags_getopt_vers() { - _flags_getopt_cmd_=${1:-${FLAGS_GETOPT_CMD}} - case "`${_flags_getopt_cmd_} -lfoo '' --foo 2>&1`" in - ' -- --foo') echo ${__FLAGS_GETOPT_VERS_STD} ;; - ' --foo --') echo ${__FLAGS_GETOPT_VERS_ENH} ;; - # Unrecognized output. Assuming standard getopt version. - *) echo ${__FLAGS_GETOPT_VERS_STD} ;; - esac - unset _flags_getopt_cmd_ -} -# shellcheck disable=SC2119 -__FLAGS_GETOPT_VERS=`_flags_getopt_vers` - -# getopt optstring lengths -__FLAGS_OPTSTR_SHORT=0 -__FLAGS_OPTSTR_LONG=1 - -__FLAGS_NULL='~' - -# Flag info strings. -__FLAGS_INFO_DEFAULT='default' -__FLAGS_INFO_HELP='help' -__FLAGS_INFO_SHORT='short' -__FLAGS_INFO_TYPE='type' - -# Flag lengths. -__FLAGS_LEN_SHORT=0 -__FLAGS_LEN_LONG=1 - -# Flag types. -__FLAGS_TYPE_NONE=0 -__FLAGS_TYPE_BOOLEAN=1 -__FLAGS_TYPE_FLOAT=2 -__FLAGS_TYPE_INTEGER=3 -__FLAGS_TYPE_STRING=4 - -# Set the constants readonly. -__flags_constants=`set |awk -F= '/^FLAGS_/ || /^__FLAGS_/ {print $1}'` -for __flags_const in ${__flags_constants}; do - # Skip certain flags. - case ${__flags_const} in - FLAGS_HELP) continue ;; - FLAGS_PARENT) continue ;; - esac - # Set flag readonly. - if [ -z "${ZSH_VERSION:-}" ]; then - readonly "${__flags_const}" - continue - fi - case ${ZSH_VERSION} in - [123].*) readonly "${__flags_const}" ;; - *) - # Declare readonly constants globally. - # shellcheck disable=SC2039 - readonly -g "${__flags_const}" ;; - esac -done -unset __flags_const __flags_constants - -# -# Internal variables. -# - -# Space separated lists. -__flags_boolNames=' ' # Boolean flag names. -__flags_longNames=' ' # Long flag names. -__flags_shortNames=' ' # Short flag names. -__flags_definedNames=' ' # Defined flag names (used for validation). - -__flags_columns='' # Screen width in columns. -__flags_opts='' # Temporary storage for parsed getopt flags. - -# Define a flag. -# -# Calling this function will define the following info variables for the -# specified flag: -# FLAGS_flagname - the name for this flag (based upon the long flag name) -# __flags__default - the default value -# __flags_flagname_help - the help string -# __flags_flagname_short - the single letter alias -# __flags_flagname_type - the type of flag (one of __FLAGS_TYPE_*) -# -# Args: -# _flags_type_: integer: internal type of flag (__FLAGS_TYPE_*) -# _flags_name_: string: long flag name -# _flags_default_: default flag value -# _flags_help_: string: help string -# _flags_short_: string: (optional) short flag name -# Returns: -# integer: success of operation, or error -_flags_define() { - if [ $# -lt 4 ]; then - flags_error='DEFINE error: too few arguments' - flags_return=${FLAGS_ERROR} - _flags_error "${flags_error}" - return ${flags_return} - fi - - _flags_type_=$1 - _flags_name_=$2 - _flags_default_=$3 - _flags_help_=${4:-§} # Special value '§' indicates no help string provided. - _flags_short_=${5:-${__FLAGS_NULL}} - - _flags_debug "type:${_flags_type_} name:${_flags_name_}" \ - "default:'${_flags_default_}' help:'${_flags_help_}'" \ - "short:${_flags_short_}" - - _flags_return_=${FLAGS_TRUE} - _flags_usName_="`_flags_underscoreName "${_flags_name_}"`" - - # Check whether the flag name is reserved. - if _flags_itemInList "${_flags_usName_}" "${__FLAGS_RESERVED_LIST}"; then - flags_error="flag name (${_flags_name_}) is reserved" - _flags_return_=${FLAGS_ERROR} - fi - - # Require short option for getopt that don't support long options. - if [ ${_flags_return_} -eq ${FLAGS_TRUE} \ - -a "${__FLAGS_GETOPT_VERS}" -ne "${__FLAGS_GETOPT_VERS_ENH}" \ - -a "${_flags_short_}" = "${__FLAGS_NULL}" ] - then - flags_error="short flag required for (${_flags_name_}) on this platform" - _flags_return_=${FLAGS_ERROR} - fi - - # Check for existing long name definition. - if [ ${_flags_return_} -eq ${FLAGS_TRUE} ]; then - if _flags_itemInList "${_flags_usName_}" "${__flags_definedNames}"; then - flags_error="definition for ([no]${_flags_name_}) already exists" - _flags_warn "${flags_error}" - _flags_return_=${FLAGS_FALSE} - fi - fi - - # Check for existing short name definition. - if [ ${_flags_return_} -eq ${FLAGS_TRUE} -a "${_flags_short_}" != "${__FLAGS_NULL}" ]; then - if _flags_itemInList "${_flags_short_}" "${__flags_shortNames}"; then - flags_error="flag short name (${_flags_short_}) already defined" - _flags_warn "${flags_error}" - _flags_return_=${FLAGS_FALSE} - fi - fi - - # Handle default value. Note, on several occasions the 'if' portion of an - # if/then/else contains just a ':' which does nothing. A binary reversal via - # '!' is not done because it does not work on all shells. - if [ ${_flags_return_} -eq ${FLAGS_TRUE} ]; then - case ${_flags_type_} in - ${__FLAGS_TYPE_BOOLEAN}) - if _flags_validBool "${_flags_default_}"; then - case ${_flags_default_} in - true|t|0) _flags_default_=${FLAGS_TRUE} ;; - false|f|1) _flags_default_=${FLAGS_FALSE} ;; - esac - else - flags_error="invalid default flag value '${_flags_default_}'" - _flags_return_=${FLAGS_ERROR} - fi - ;; - - ${__FLAGS_TYPE_FLOAT}) - if _flags_validFloat "${_flags_default_}"; then - : - else - flags_error="invalid default flag value '${_flags_default_}'" - _flags_return_=${FLAGS_ERROR} - fi - ;; - - ${__FLAGS_TYPE_INTEGER}) - if _flags_validInt "${_flags_default_}"; then - : - else - flags_error="invalid default flag value '${_flags_default_}'" - _flags_return_=${FLAGS_ERROR} - fi - ;; - - ${__FLAGS_TYPE_STRING}) ;; # Everything in shell is a valid string. - - *) - flags_error="unrecognized flag type '${_flags_type_}'" - _flags_return_=${FLAGS_ERROR} - ;; - esac - fi - - if [ ${_flags_return_} -eq ${FLAGS_TRUE} ]; then - # Store flag information. - eval "FLAGS_${_flags_usName_}='${_flags_default_}'" - eval "__flags_${_flags_usName_}_${__FLAGS_INFO_TYPE}=${_flags_type_}" - eval "__flags_${_flags_usName_}_${__FLAGS_INFO_DEFAULT}=\ -\"${_flags_default_}\"" - eval "__flags_${_flags_usName_}_${__FLAGS_INFO_HELP}=\"${_flags_help_}\"" - eval "__flags_${_flags_usName_}_${__FLAGS_INFO_SHORT}='${_flags_short_}'" - - # append flag names to name lists - __flags_shortNames="${__flags_shortNames}${_flags_short_} " - __flags_longNames="${__flags_longNames}${_flags_name_} " - [ "${_flags_type_}" -eq "${__FLAGS_TYPE_BOOLEAN}" ] && \ - __flags_boolNames="${__flags_boolNames}no${_flags_name_} " - - # Append flag names to defined names for later validation checks. - __flags_definedNames="${__flags_definedNames}${_flags_usName_} " - [ "${_flags_type_}" -eq "${__FLAGS_TYPE_BOOLEAN}" ] && \ - __flags_definedNames="${__flags_definedNames}no${_flags_usName_} " - fi - - flags_return=${_flags_return_} - unset _flags_default_ _flags_help_ _flags_name_ _flags_return_ \ - _flags_short_ _flags_type_ _flags_usName_ - [ ${flags_return} -eq ${FLAGS_ERROR} ] && _flags_error "${flags_error}" - return ${flags_return} -} - -# Underscore a flag name by replacing dashes with underscores. -# -# Args: -# unnamed: string: log flag name -# Output: -# string: underscored name -_flags_underscoreName() { - echo "$1" |tr z- z_ -} - -# Return valid getopt options using currently defined list of long options. -# -# This function builds a proper getopt option string for short (and long) -# options, using the current list of long options for reference. -# -# Args: -# _flags_optStr: integer: option string type (__FLAGS_OPTSTR_*) -# Output: -# string: generated option string for getopt -# Returns: -# boolean: success of operation (always returns True) -_flags_genOptStr() { - _flags_optStrType_=$1 - - _flags_opts_='' - - for _flags_name_ in ${__flags_longNames}; do - _flags_usName_="`_flags_underscoreName "${_flags_name_}"`" - _flags_type_="`_flags_getFlagInfo "${_flags_usName_}" "${__FLAGS_INFO_TYPE}"`" - if [ $? -ne ${FLAGS_TRUE} ]; then - _flags_fatal 'call to _flags_type_ failed' - fi - case ${_flags_optStrType_} in - ${__FLAGS_OPTSTR_SHORT}) - _flags_shortName_="`_flags_getFlagInfo \ - "${_flags_usName_}" "${__FLAGS_INFO_SHORT}"`" - if [ "${_flags_shortName_}" != "${__FLAGS_NULL}" ]; then - _flags_opts_="${_flags_opts_}${_flags_shortName_}" - # getopt needs a trailing ':' to indicate a required argument. - [ "${_flags_type_}" -ne "${__FLAGS_TYPE_BOOLEAN}" ] && \ - _flags_opts_="${_flags_opts_}:" - fi - ;; - - ${__FLAGS_OPTSTR_LONG}) - _flags_opts_="${_flags_opts_:+${_flags_opts_},}${_flags_name_}" - # getopt needs a trailing ':' to indicate a required argument - [ "${_flags_type_}" -ne "${__FLAGS_TYPE_BOOLEAN}" ] && \ - _flags_opts_="${_flags_opts_}:" - ;; - esac - done - - echo "${_flags_opts_}" - unset _flags_name_ _flags_opts_ _flags_optStrType_ _flags_shortName_ \ - _flags_type_ _flags_usName_ - return ${FLAGS_TRUE} -} - -# Returns flag details based on a flag name and flag info. -# -# Args: -# string: underscored flag name -# string: flag info (see the _flags_define function for valid info types) -# Output: -# string: value of dereferenced flag variable -# Returns: -# integer: one of FLAGS_{TRUE|FALSE|ERROR} -_flags_getFlagInfo() { - # Note: adding gFI to variable names to prevent naming conflicts with calling - # functions - _flags_gFI_usName_=$1 - _flags_gFI_info_=$2 - - # Example: given argument usName (underscored flag name) of 'my_flag', and - # argument info of 'help', set the _flags_infoValue_ variable to the value of - # ${__flags_my_flag_help}, and see if it is non-empty. - _flags_infoVar_="__flags_${_flags_gFI_usName_}_${_flags_gFI_info_}" - _flags_strToEval_="_flags_infoValue_=\"\${${_flags_infoVar_}:-}\"" - eval "${_flags_strToEval_}" - if [ -n "${_flags_infoValue_}" ]; then - # Special value '§' indicates no help string provided. - [ "${_flags_gFI_info_}" = ${__FLAGS_INFO_HELP} \ - -a "${_flags_infoValue_}" = '§' ] && _flags_infoValue_='' - flags_return=${FLAGS_TRUE} - else - # See if the _flags_gFI_usName_ variable is a string as strings can be - # empty... - # Note: the DRY principle would say to have this function call itself for - # the next three lines, but doing so results in an infinite loop as an - # invalid _flags_name_ will also not have the associated _type variable. - # Because it doesn't (it will evaluate to an empty string) the logic will - # try to find the _type variable of the _type variable, and so on. Not so - # good ;-) - # - # Example cont.: set the _flags_typeValue_ variable to the value of - # ${__flags_my_flag_type}, and see if it equals '4'. - _flags_typeVar_="__flags_${_flags_gFI_usName_}_${__FLAGS_INFO_TYPE}" - _flags_strToEval_="_flags_typeValue_=\"\${${_flags_typeVar_}:-}\"" - eval "${_flags_strToEval_}" - # shellcheck disable=SC2154 - if [ "${_flags_typeValue_}" = "${__FLAGS_TYPE_STRING}" ]; then - flags_return=${FLAGS_TRUE} - else - flags_return=${FLAGS_ERROR} - flags_error="missing flag info variable (${_flags_infoVar_})" - fi - fi - - echo "${_flags_infoValue_}" - unset _flags_gFI_usName_ _flags_gfI_info_ _flags_infoValue_ _flags_infoVar_ \ - _flags_strToEval_ _flags_typeValue_ _flags_typeVar_ - [ ${flags_return} -eq ${FLAGS_ERROR} ] && _flags_error "${flags_error}" - return ${flags_return} -} - -# Check for presence of item in a list. -# -# Passed a string (e.g. 'abc'), this function will determine if the string is -# present in the list of strings (e.g. ' foo bar abc '). -# -# Args: -# _flags_str_: string: string to search for in a list of strings -# unnamed: list: list of strings -# Returns: -# boolean: true if item is in the list -_flags_itemInList() { - _flags_str_=$1 - shift - - case " ${*:-} " in - *\ ${_flags_str_}\ *) flags_return=${FLAGS_TRUE} ;; - *) flags_return=${FLAGS_FALSE} ;; - esac - - unset _flags_str_ - return ${flags_return} -} - -# Returns the width of the current screen. -# -# Output: -# integer: width in columns of the current screen. -_flags_columns() { - if [ -z "${__flags_columns}" ]; then - if eval stty size >/dev/null 2>&1; then - # stty size worked :-) - # shellcheck disable=SC2046 - set -- `stty size` - __flags_columns="${2:-}" - fi - fi - if [ -z "${__flags_columns}" ]; then - if eval tput cols >/dev/null 2>&1; then - # shellcheck disable=SC2046 - set -- `tput cols` - __flags_columns="${1:-}" - fi - fi - echo "${__flags_columns:-80}" -} - -# Validate a boolean. -# -# Args: -# _flags__bool: boolean: value to validate -# Returns: -# bool: true if the value is a valid boolean -_flags_validBool() { - _flags_bool_=$1 - - flags_return=${FLAGS_TRUE} - case "${_flags_bool_}" in - true|t|0) ;; - false|f|1) ;; - *) flags_return=${FLAGS_FALSE} ;; - esac - - unset _flags_bool_ - return ${flags_return} -} - -# Validate a float. -# -# Args: -# _flags_float_: float: value to validate -# Returns: -# bool: true if the value is a valid integer -_flags_validFloat() { - flags_return=${FLAGS_FALSE} - if [ -z "$1" ]; then - return ${flags_return} - fi - _flags_float_=$1 - - if _flags_validInt "${_flags_float_}"; then - flags_return=${FLAGS_TRUE} - elif _flags_useBuiltin; then - _flags_float_whole_=${_flags_float_%.*} - _flags_float_fraction_=${_flags_float_#*.} - [ "${_flags_float_whole_}" = '-' ] && _flags_float_whole_='-0' - if _flags_validInt "${_flags_float_whole_:-0}" -a \ - _flags_validInt "${_flags_float_fraction_}"; then - flags_return=${FLAGS_TRUE} - fi - unset _flags_float_whole_ _flags_float_fraction_ - else - flags_return=${FLAGS_TRUE} - case ${_flags_float_} in - -*) # Negative floats. - _flags_test_=`${FLAGS_EXPR_CMD} "${_flags_float_}" :\ - '\(-[0-9]*\.[0-9]*\)'` - ;; - *) # Positive floats. - _flags_test_=`${FLAGS_EXPR_CMD} "${_flags_float_}" :\ - '\([0-9]*\.[0-9]*\)'` - ;; - esac - [ "${_flags_test_}" != "${_flags_float_}" ] && flags_return=${FLAGS_FALSE} - unset _flags_test_ - fi - - unset _flags_float_ _flags_float_whole_ _flags_float_fraction_ - return ${flags_return} -} - -# Validate an integer. -# -# Args: -# _flags_int_: integer: value to validate -# Returns: -# bool: true if the value is a valid integer -_flags_validInt() { - expr \( "$1" + '0' \) '=' "$1" >/dev/null 2>&1 -} - -# Parse command-line options using the standard getopt. -# -# Note: the flag options are passed around in the global __flags_opts so that -# the formatting is not lost due to shell parsing and such. -# -# Args: -# @: varies: command-line options to parse -# Returns: -# integer: a FLAGS success condition -_flags_getoptStandard() { - flags_return=${FLAGS_TRUE} - _flags_shortOpts_=`_flags_genOptStr ${__FLAGS_OPTSTR_SHORT}` - - # Check for spaces in passed options. - for _flags_opt_ in "$@"; do - # Note: the silliness with the x's is purely for ksh93 on Ubuntu 6.06. - _flags_match_=`echo "x${_flags_opt_}x" |sed 's/ //g'` - if [ "${_flags_match_}" != "x${_flags_opt_}x" ]; then - flags_error='the available getopt does not support spaces in options' - flags_return=${FLAGS_ERROR} - break - fi - done - - if [ ${flags_return} -eq ${FLAGS_TRUE} ]; then - __flags_opts=`getopt "${_flags_shortOpts_}" "$@" 2>&1` - _flags_rtrn_=$? - if [ ${_flags_rtrn_} -ne ${FLAGS_TRUE} ]; then - _flags_warn "${__flags_opts}" - flags_error='unable to parse provided options with getopt.' - flags_return=${FLAGS_ERROR} - fi - fi - - unset _flags_match_ _flags_opt_ _flags_rtrn_ _flags_shortOpts_ - return ${flags_return} -} - -# Parse command-line options using the enhanced getopt. -# -# Note: the flag options are passed around in the global __flags_opts so that -# the formatting is not lost due to shell parsing and such. -# -# Args: -# @: varies: command-line options to parse -# Returns: -# integer: a FLAGS success condition -_flags_getoptEnhanced() { - flags_return=${FLAGS_TRUE} - _flags_shortOpts_=`_flags_genOptStr ${__FLAGS_OPTSTR_SHORT}` - _flags_boolOpts_=`echo "${__flags_boolNames}" \ - |sed 's/^ *//;s/ *$//;s/ /,/g'` - _flags_longOpts_=`_flags_genOptStr ${__FLAGS_OPTSTR_LONG}` - - __flags_opts=`${FLAGS_GETOPT_CMD} \ - -o "${_flags_shortOpts_}" \ - -l "${_flags_longOpts_},${_flags_boolOpts_}" \ - -- "$@" 2>&1` - _flags_rtrn_=$? - if [ ${_flags_rtrn_} -ne ${FLAGS_TRUE} ]; then - _flags_warn "${__flags_opts}" - flags_error='unable to parse provided options with getopt.' - flags_return=${FLAGS_ERROR} - fi - - unset _flags_boolOpts_ _flags_longOpts_ _flags_rtrn_ _flags_shortOpts_ - return ${flags_return} -} - -# Dynamically parse a getopt result and set appropriate variables. -# -# This function does the actual conversion of getopt output and runs it through -# the standard case structure for parsing. The case structure is actually quite -# dynamic to support any number of flags. -# -# Args: -# @: varies: output from getopt parsing -# Returns: -# integer: a FLAGS success condition -_flags_parseGetopt() { - flags_return=${FLAGS_TRUE} - - if [ "${__FLAGS_GETOPT_VERS}" -ne "${__FLAGS_GETOPT_VERS_ENH}" ]; then - # The @$ must be unquoted as it needs to be re-split. - # shellcheck disable=SC2068 - set -- $@ - else - # Note the quotes around the `$@` -- they are essential! - eval set -- "$@" - fi - - # Handle options. note options with values must do an additional shift. - while true; do - _flags_opt_=$1 - _flags_arg_=${2:-} - _flags_type_=${__FLAGS_TYPE_NONE} - _flags_name_='' - - # Determine long flag name. - case "${_flags_opt_}" in - --) shift; break ;; # Discontinue option parsing. - - --*) # Long option. - if _flags_useBuiltin; then - _flags_opt_=${_flags_opt_#*--} - else - _flags_opt_=`${FLAGS_EXPR_CMD} "${_flags_opt_}" : '--\(.*\)'` - fi - _flags_len_=${__FLAGS_LEN_LONG} - if _flags_itemInList "${_flags_opt_}" "${__flags_longNames}"; then - _flags_name_=${_flags_opt_} - else - # Check for negated long boolean version. - if _flags_itemInList "${_flags_opt_}" "${__flags_boolNames}"; then - if _flags_useBuiltin; then - _flags_name_=${_flags_opt_#*no} - else - _flags_name_=`${FLAGS_EXPR_CMD} "${_flags_opt_}" : 'no\(.*\)'` - fi - _flags_type_=${__FLAGS_TYPE_BOOLEAN} - _flags_arg_=${__FLAGS_NULL} - fi - fi - ;; - - -*) # Short option. - if _flags_useBuiltin; then - _flags_opt_=${_flags_opt_#*-} - else - _flags_opt_=`${FLAGS_EXPR_CMD} "${_flags_opt_}" : '-\(.*\)'` - fi - _flags_len_=${__FLAGS_LEN_SHORT} - if _flags_itemInList "${_flags_opt_}" "${__flags_shortNames}"; then - # Yes. Match short name to long name. Note purposeful off-by-one - # (too high) with awk calculations. - _flags_pos_=`echo "${__flags_shortNames}" \ - |awk 'BEGIN{RS=" ";rn=0}$0==e{rn=NR}END{print rn}' \ - e="${_flags_opt_}"` - _flags_name_=`echo "${__flags_longNames}" \ - |awk 'BEGIN{RS=" "}rn==NR{print $0}' rn="${_flags_pos_}"` - fi - ;; - esac - - # Die if the flag was unrecognized. - if [ -z "${_flags_name_}" ]; then - flags_error="unrecognized option (${_flags_opt_})" - flags_return=${FLAGS_ERROR} - break - fi - - # Set new flag value. - _flags_usName_=`_flags_underscoreName "${_flags_name_}"` - [ ${_flags_type_} -eq ${__FLAGS_TYPE_NONE} ] && \ - _flags_type_=`_flags_getFlagInfo \ - "${_flags_usName_}" ${__FLAGS_INFO_TYPE}` - case ${_flags_type_} in - ${__FLAGS_TYPE_BOOLEAN}) - if [ ${_flags_len_} -eq ${__FLAGS_LEN_LONG} ]; then - if [ "${_flags_arg_}" != "${__FLAGS_NULL}" ]; then - eval "FLAGS_${_flags_usName_}=${FLAGS_TRUE}" - else - eval "FLAGS_${_flags_usName_}=${FLAGS_FALSE}" - fi - else - _flags_strToEval_="_flags_val_=\ -\${__flags_${_flags_usName_}_${__FLAGS_INFO_DEFAULT}}" - eval "${_flags_strToEval_}" - # shellcheck disable=SC2154 - if [ "${_flags_val_}" -eq ${FLAGS_FALSE} ]; then - eval "FLAGS_${_flags_usName_}=${FLAGS_TRUE}" - else - eval "FLAGS_${_flags_usName_}=${FLAGS_FALSE}" - fi - fi - ;; - - ${__FLAGS_TYPE_FLOAT}) - if _flags_validFloat "${_flags_arg_}"; then - eval "FLAGS_${_flags_usName_}='${_flags_arg_}'" - else - flags_error="invalid float value (${_flags_arg_})" - flags_return=${FLAGS_ERROR} - break - fi - ;; - - ${__FLAGS_TYPE_INTEGER}) - if _flags_validInt "${_flags_arg_}"; then - eval "FLAGS_${_flags_usName_}='${_flags_arg_}'" - else - flags_error="invalid integer value (${_flags_arg_})" - flags_return=${FLAGS_ERROR} - break - fi - ;; - - ${__FLAGS_TYPE_STRING}) - eval "FLAGS_${_flags_usName_}='${_flags_arg_}'" - ;; - esac - - # Handle special case help flag. - if [ "${_flags_usName_}" = 'help' ]; then - # shellcheck disable=SC2154 - if [ "${FLAGS_help}" -eq ${FLAGS_TRUE} ]; then - flags_help - flags_error='help requested' - flags_return=${FLAGS_FALSE} - break - fi - fi - - # Shift the option and non-boolean arguments out. - shift - [ "${_flags_type_}" != ${__FLAGS_TYPE_BOOLEAN} ] && shift - done - - # Give user back non-flag arguments. - FLAGS_ARGV='' - while [ $# -gt 0 ]; do - FLAGS_ARGV="${FLAGS_ARGV:+${FLAGS_ARGV} }'$1'" - shift - done - - unset _flags_arg_ _flags_len_ _flags_name_ _flags_opt_ _flags_pos_ \ - _flags_strToEval_ _flags_type_ _flags_usName_ _flags_val_ - return ${flags_return} -} - -# Perform some path using built-ins. -# -# Args: -# $@: string: math expression to evaluate -# Output: -# integer: the result -# Returns: -# bool: success of math evaluation -_flags_math() { - if [ $# -eq 0 ]; then - flags_return=${FLAGS_FALSE} - elif _flags_useBuiltin; then - # Variable assignment is needed as workaround for Solaris Bourne shell, - # which cannot parse a bare $((expression)). - # shellcheck disable=SC2016 - _flags_expr_='$(($@))' - eval echo ${_flags_expr_} - flags_return=$? - unset _flags_expr_ - else - eval expr "$@" - flags_return=$? - fi - - return ${flags_return} -} - -# Cross-platform strlen() implementation. -# -# Args: -# _flags_str: string: to determine length of -# Output: -# integer: length of string -# Returns: -# bool: success of strlen evaluation -_flags_strlen() { - _flags_str_=${1:-} - - if [ -z "${_flags_str_}" ]; then - flags_output=0 - elif _flags_useBuiltin; then - flags_output=${#_flags_str_} - else - flags_output=`${FLAGS_EXPR_CMD} "${_flags_str_}" : '.*'` - fi - flags_return=$? - - unset _flags_str_ - echo "${flags_output}" - return ${flags_return} -} - -# Use built-in helper function to enable unit testing. -# -# Args: -# None -# Returns: -# bool: true if built-ins should be used -_flags_useBuiltin() { return ${__FLAGS_USE_BUILTIN}; } - -#------------------------------------------------------------------------------ -# public functions -# -# A basic boolean flag. Boolean flags do not take any arguments, and their -# value is either 1 (false) or 0 (true). For long flags, the false value is -# specified on the command line by prepending the word 'no'. With short flags, -# the presence of the flag toggles the current value between true and false. -# Specifying a short boolean flag twice on the command results in returning the -# value back to the default value. -# -# A default value is required for boolean flags. -# -# For example, lets say a Boolean flag was created whose long name was 'update' -# and whose short name was 'x', and the default value was 'false'. This flag -# could be explicitly set to 'true' with '--update' or by '-x', and it could be -# explicitly set to 'false' with '--noupdate'. -DEFINE_boolean() { _flags_define ${__FLAGS_TYPE_BOOLEAN} "$@"; } - -# Other basic flags. -DEFINE_float() { _flags_define ${__FLAGS_TYPE_FLOAT} "$@"; } -DEFINE_integer() { _flags_define ${__FLAGS_TYPE_INTEGER} "$@"; } -DEFINE_string() { _flags_define ${__FLAGS_TYPE_STRING} "$@"; } - -# Parse the flags. -# -# Args: -# unnamed: list: command-line flags to parse -# Returns: -# integer: success of operation, or error -FLAGS() { - # Define a standard 'help' flag if one isn't already defined. - if [ -z "${__flags_help_type:-}" ]; then - DEFINE_boolean 'help' false 'show this help' 'h' - fi - - # Parse options. - if [ $# -gt 0 ]; then - if [ "${__FLAGS_GETOPT_VERS}" -ne "${__FLAGS_GETOPT_VERS_ENH}" ]; then - _flags_getoptStandard "$@" - else - _flags_getoptEnhanced "$@" - fi - flags_return=$? - else - # Nothing passed; won't bother running getopt. - __flags_opts='--' - flags_return=${FLAGS_TRUE} - fi - - if [ ${flags_return} -eq ${FLAGS_TRUE} ]; then - _flags_parseGetopt "${__flags_opts}" - flags_return=$? - fi - - if [ ${flags_return} -eq ${FLAGS_ERROR} ]; then - _flags_fatal "${flags_error}" - fi - return ${flags_return} -} - -# This is a helper function for determining the 'getopt' version for platforms -# where the detection isn't working. It simply outputs debug information that -# can be included in a bug report. -# -# Args: -# none -# Output: -# debug info that can be included in a bug report -# Returns: -# nothing -flags_getoptInfo() { - # Platform info. - _flags_debug "uname -a: `uname -a`" - _flags_debug "PATH: ${PATH}" - - # Shell info. - if [ -n "${BASH_VERSION:-}" ]; then - _flags_debug 'shell: bash' - _flags_debug "BASH_VERSION: ${BASH_VERSION}" - elif [ -n "${ZSH_VERSION:-}" ]; then - _flags_debug 'shell: zsh' - _flags_debug "ZSH_VERSION: ${ZSH_VERSION}" - fi - - # getopt info. - ${FLAGS_GETOPT_CMD} >/dev/null - _flags_getoptReturn=$? - _flags_debug "getopt return: ${_flags_getoptReturn}" - _flags_debug "getopt --version: `${FLAGS_GETOPT_CMD} --version 2>&1`" - - unset _flags_getoptReturn -} - -# Returns whether the detected getopt version is the enhanced version. -# -# Args: -# none -# Output: -# none -# Returns: -# bool: true if getopt is the enhanced version -flags_getoptIsEnh() { - test "${__FLAGS_GETOPT_VERS}" -eq "${__FLAGS_GETOPT_VERS_ENH}" -} - -# Returns whether the detected getopt version is the standard version. -# -# Args: -# none -# Returns: -# bool: true if getopt is the standard version -flags_getoptIsStd() { - test "${__FLAGS_GETOPT_VERS}" -eq "${__FLAGS_GETOPT_VERS_STD}" -} - -# This is effectively a 'usage()' function. It prints usage information and -# exits the program with ${FLAGS_FALSE} if it is ever found in the command line -# arguments. Note this function can be overridden so other apps can define -# their own --help flag, replacing this one, if they want. -# -# Args: -# none -# Returns: -# integer: success of operation (always returns true) -flags_help() { - if [ -n "${FLAGS_HELP:-}" ]; then - echo "${FLAGS_HELP}" >&2 - else - echo "USAGE: ${FLAGS_PARENT:-$0} [flags] args" >&2 - fi - if [ -n "${__flags_longNames}" ]; then - echo 'flags:' >&2 - for flags_name_ in ${__flags_longNames}; do - flags_flagStr_='' - flags_boolStr_='' - flags_usName_=`_flags_underscoreName "${flags_name_}"` - - flags_default_=`_flags_getFlagInfo \ - "${flags_usName_}" ${__FLAGS_INFO_DEFAULT}` - flags_help_=`_flags_getFlagInfo \ - "${flags_usName_}" ${__FLAGS_INFO_HELP}` - flags_short_=`_flags_getFlagInfo \ - "${flags_usName_}" ${__FLAGS_INFO_SHORT}` - flags_type_=`_flags_getFlagInfo \ - "${flags_usName_}" ${__FLAGS_INFO_TYPE}` - - [ "${flags_short_}" != "${__FLAGS_NULL}" ] && \ - flags_flagStr_="-${flags_short_}" - - if [ "${__FLAGS_GETOPT_VERS}" -eq "${__FLAGS_GETOPT_VERS_ENH}" ]; then - [ "${flags_short_}" != "${__FLAGS_NULL}" ] && \ - flags_flagStr_="${flags_flagStr_}," - # Add [no] to long boolean flag names, except the 'help' flag. - [ "${flags_type_}" -eq ${__FLAGS_TYPE_BOOLEAN} \ - -a "${flags_usName_}" != 'help' ] && \ - flags_boolStr_='[no]' - flags_flagStr_="${flags_flagStr_}--${flags_boolStr_}${flags_name_}:" - fi - - case ${flags_type_} in - ${__FLAGS_TYPE_BOOLEAN}) - if [ "${flags_default_}" -eq ${FLAGS_TRUE} ]; then - flags_defaultStr_='true' - else - flags_defaultStr_='false' - fi - ;; - ${__FLAGS_TYPE_FLOAT}|${__FLAGS_TYPE_INTEGER}) - flags_defaultStr_=${flags_default_} ;; - ${__FLAGS_TYPE_STRING}) flags_defaultStr_="'${flags_default_}'" ;; - esac - flags_defaultStr_="(default: ${flags_defaultStr_})" - - flags_helpStr_=" ${flags_flagStr_} ${flags_help_:+${flags_help_} }${flags_defaultStr_}" - _flags_strlen "${flags_helpStr_}" >/dev/null - flags_helpStrLen_=${flags_output} - flags_columns_=`_flags_columns` - - if [ "${flags_helpStrLen_}" -lt "${flags_columns_}" ]; then - echo "${flags_helpStr_}" >&2 - else - echo " ${flags_flagStr_} ${flags_help_}" >&2 - # Note: the silliness with the x's is purely for ksh93 on Ubuntu 6.06 - # because it doesn't like empty strings when used in this manner. - flags_emptyStr_="`echo \"x${flags_flagStr_}x\" \ - |awk '{printf "%"length($0)-2"s", ""}'`" - flags_helpStr_=" ${flags_emptyStr_} ${flags_defaultStr_}" - _flags_strlen "${flags_helpStr_}" >/dev/null - flags_helpStrLen_=${flags_output} - - if [ "${__FLAGS_GETOPT_VERS}" -eq "${__FLAGS_GETOPT_VERS_STD}" \ - -o "${flags_helpStrLen_}" -lt "${flags_columns_}" ]; then - # Indented to match help string. - echo "${flags_helpStr_}" >&2 - else - # Indented four from left to allow for longer defaults as long flag - # names might be used too, making things too long. - echo " ${flags_defaultStr_}" >&2 - fi - fi - done - fi - - unset flags_boolStr_ flags_default_ flags_defaultStr_ flags_emptyStr_ \ - flags_flagStr_ flags_help_ flags_helpStr flags_helpStrLen flags_name_ \ - flags_columns_ flags_short_ flags_type_ flags_usName_ - return ${FLAGS_TRUE} -} - -# Reset shflags back to an uninitialized state. -# -# Args: -# none -# Returns: -# nothing -flags_reset() { - for flags_name_ in ${__flags_longNames}; do - flags_usName_=`_flags_underscoreName "${flags_name_}"` - flags_strToEval_="unset FLAGS_${flags_usName_}" - for flags_type_ in \ - ${__FLAGS_INFO_DEFAULT} \ - ${__FLAGS_INFO_HELP} \ - ${__FLAGS_INFO_SHORT} \ - ${__FLAGS_INFO_TYPE} - do - flags_strToEval_=\ -"${flags_strToEval_} __flags_${flags_usName_}_${flags_type_}" - done - eval "${flags_strToEval_}" - done - - # Reset internal variables. - __flags_boolNames=' ' - __flags_longNames=' ' - __flags_shortNames=' ' - __flags_definedNames=' ' - - # Reset logging level back to default. - flags_setLoggingLevel ${__FLAGS_LEVEL_DEFAULT} - - unset flags_name_ flags_type_ flags_strToEval_ flags_usName_ -} - -#----------------------------------------------------------------------------- -# Initialization -# - -# Set the default logging level. -flags_setLoggingLevel ${__FLAGS_LEVEL_DEFAULT} \ No newline at end of file diff --git a/src/Configuration.cc b/src/Configuration.cc new file mode 100644 index 0000000..f8c9943 --- /dev/null +++ b/src/Configuration.cc @@ -0,0 +1,64 @@ +/** + * \file Configuration.cc + * \author Coumarc9 + * \date 24/07/2017 + * + * \copyright Copyright (c) 2021 S.O.N.I.A. All rights reserved. + * + * \section LICENSE + * + * This file is part of S.O.N.I.A. software. + * + * S.O.N.I.A. software is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * S.O.N.I.A. software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with S.O.N.I.A. software. If not, see . + */ + +#include "Configuration.h" + +namespace provider_underwater_com +{ + + Configuration::Configuration(const ros::NodeHandlePtr &nh) + : nh(nh), + ttyPort("/dev/ttyUSB0"), // "/dev/MODEM" for AUVs, debug only + settingsFile("settings.txt"), + role("master"), + channel("4") + { + Deserialize(); + } + + Configuration::~Configuration() {} + + void Configuration::Deserialize() { + + ROS_INFO("Deserialize params"); + + FindParameter("/connection/tty_port", ttyPort); + FindParameter("/settings/role", role); + FindParameter("/settings/channel", channel); + + ROS_INFO("End deserialize params"); + } + + template + void Configuration::FindParameter(const std::string ¶mName, TType &attribute) { + if (nh->hasParam("/provider_underwater_com" + paramName)) { + nh->getParam("/provider_underwater_com" + paramName, attribute); + } else { + ROS_WARN_STREAM("Did not find /provider_underwater_com" + paramName + << ". Using default."); + } + } + +} diff --git a/src/Configuration.h b/src/Configuration.h new file mode 100644 index 0000000..5d5d684 --- /dev/null +++ b/src/Configuration.h @@ -0,0 +1,66 @@ +/** + * \file Configuration.h + * \author Coumarc9 + * \date 24/07/2017 + * + * \copyright Copyright (c) 2021 S.O.N.I.A. All rights reserved. + * + * \section LICENSE + * + * This file is part of S.O.N.I.A. software. + * + * S.O.N.I.A. software is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * S.O.N.I.A. software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with S.O.N.I.A. software. If not, see . + */ + +#ifndef INTERFACE_CONFIGURATION_H +#define INTERFACE_CONFIGURATION_H + +#include +#include +#include + +namespace provider_underwater_com +{ + class Configuration { + + public: + + Configuration(const ros::NodeHandlePtr &nh); + ~Configuration(); + + std::string getTtyPort() const {return ttyPort;} + std::string getSettingsFile() const {return settingsFile;} + std::string getRole() const {return role;} + std::string getChannel() const {return channel;} + + private: + + ros::NodeHandlePtr nh; + + std::string ttyPort; + std::string settingsFile; + std::string role; + std::string channel; + + void Deserialize(); + void SetParameter(); + + template + void FindParameter(const std::string ¶mName, TType &attribute); + + + }; +} + +#endif //INTERFACE_CONFIGURATION_H diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc new file mode 100644 index 0000000..f9c5725 --- /dev/null +++ b/src/drivers/serial.cc @@ -0,0 +1,109 @@ +/** + * \file serial.cc + * \author Lucas Mongrain + * \date 26/10/2017 + * + * \copyright Copyright (c) 2021 S.O.N.I.A. All rights reserved. + * + * \section LICENSE + * + * This file is part of S.O.N.I.A. software. + * + * S.O.N.I.A. software is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * S.O.N.I.A. software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with S.O.N.I.A. software. If not, see . + */ + +#include "serial.h" +#include +#include +#include + +Serial::Serial(std::string port, int flags) +{ + fd = open(port.c_str(), flags); + if(fd == -1) + { + ROS_ERROR("unable to connect to %s", port.c_str()); + ros::shutdown(); + } + else + { + ROS_INFO("connection to %s succeed", port.c_str()); + } + + tcgetattr(fd, &options); + + // setup le baudrate + cfsetispeed(&options, B115200); + cfsetospeed(&options, B115200); + + options.c_cflag |= (CLOCAL | CREAD); + options.c_cflag &= ~CSIZE; + options.c_cflag |= CS8; + + options.c_cflag &= ~(PARENB | PARODD); + options.c_cflag &= ~CSTOPB; + options.c_cflag &= ~CRTSCTS; + + //Input Flags + options.c_iflag &= ~IGNBRK; + options.c_iflag &= ~(IXON | IXOFF | IXANY); + + //Local Flags + options.c_lflag = 0; + + //Output Flags + options.c_oflag = 0; + + + tcsetattr(fd, TCSANOW, &options); +} + +Serial::~Serial() +{ + close(fd); +} + +std::string Serial::receive(size_t count) +{ + ROS_DEBUG("provider_underwater_com receive data"); + char data[1024]; + data[0] = 0; + + read(fd, data, count); + return std::string(data); +} + +void Serial::readOnce(char* data, int offset) +{ + ROS_DEBUG("provider_underwater_com receive Once"); + read(fd, (data+offset), 1); +} + +void Serial::flush() +{ + ROS_DEBUG("provider_underwater_com flush data"); + tcflush(fd,TCIOFLUSH); +} + +ssize_t Serial::transmit(const std::string data) +{ + ROS_DEBUG("provider_underwater_com transmit data"); + return write(fd, data.c_str(), data.size()); +} + +ssize_t Serial::transmit(const void *data, const ssize_t size) +{ + ROS_DEBUG("serial transmit data"); + return write(fd, data, size); +} \ No newline at end of file diff --git a/src/drivers/serial.h b/src/drivers/serial.h new file mode 100644 index 0000000..c1e04e6 --- /dev/null +++ b/src/drivers/serial.h @@ -0,0 +1,52 @@ +/** + * \file serial.h + * \author Lucas Mongrain + * \date 26/10/2017 + * + * \copyright Copyright (c) 2021 S.O.N.I.A. All rights reserved. + * + * \section LICENSE + * + * This file is part of S.O.N.I.A. software. + * + * S.O.N.I.A. software is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * S.O.N.I.A. software is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with S.O.N.I.A. software. If not, see . + */ + +#ifndef INTERFACE_RS485_SERIAL_H +#define INTERFACE_RS485_SERIAL_H + +#include +#include +#include +#include + + +class Serial{ +public: + Serial(std::string port, int flags); + ~Serial(); + + std::string receive(size_t count); + void readOnce(char* data, int offset); + void flush(); + ssize_t transmit(const std::string data); + ssize_t transmit(const void *data, const ssize_t size); + +private: + + struct termios options; + int fd; +}; + +#endif //INTERFACE_RS485_SERIAL_H diff --git a/src/main.cc b/src/main.cc new file mode 100644 index 0000000..a132f49 --- /dev/null +++ b/src/main.cc @@ -0,0 +1,37 @@ +/** + * \file main.cc + * \author Francis Alonzo . + */ + +#include +#include "provider_underwater_com_node.h" + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "provider_underwater_com"); + ros::NodeHandlePtr nh(new ros::NodeHandle("~")); + provider_underwater_com::ProviderUnderwaterComNode provider_underwater_com_node{nh}; + provider_underwater_com_node.Spin(); + + return 0; +} \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc new file mode 100644 index 0000000..8e5179c --- /dev/null +++ b/src/provider_underwater_com_node.cc @@ -0,0 +1,536 @@ +/** + * \file provider_underwater_com_node.cc + * \author Francis Alonzo . + */ + +#include "provider_underwater_com_node.h" +#include + +namespace provider_underwater_com +{ + + //Node Construtor + ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) + : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort(), O_RDWR | O_NOCTTY) + { + underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); + underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); + + manage_write_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Write, this)); + read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); + + ros::Duration(1).sleep(); + + ROS_INFO_STREAM("Setting the sensor"); + Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); + + underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); + } + + //Node Destructor + ProviderUnderwaterComNode::~ProviderUnderwaterComNode() + { + manage_write_thread.~thread(); + read_packet_thread.~thread(); + } + + //Node Spin + void ProviderUnderwaterComNode::Spin() + { + ros::Rate r(5); // 5 Hz + + while(ros::ok() && init_error_ == false) + { + ros::spinOnce(); + r.sleep(); + } + ros::shutdown(); + } + + void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt64 &msg) + { + // uint64_t packet = 0; + char packet_array[MODEM_M64_PAYLOAD]; + + // // Add function for multiple paquet support + // modem_data.header.endOfPacket = 0b1; + // modem_data.header.packetId = 0b0; + // modem_data.header.packetNumber = 0b1; + + // modem_data.killSwitchState = msg.kill_switch_state; + // modem_data.missionSwitchState = msg.mission_switch_state; + // modem_data.depth = msg.depth; + // modem_data.missionId = msg.mission_id; + // modem_data.missionState = (uint8_t) msg.mission_state; + // modem_data.torpedosState = msg.torpedos_state; + // modem_data.droppersState = msg.droppers_state; + + // packet = *((uint64_t *)&modem_data); + std::memcpy(packet_array, &(msg.data), sizeof(packet_array)); + Queue_Packet(CMD_QUEUE_PACKET, packet_array, MODEM_M64_PAYLOAD); + } + + bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemSendCmd::Request &req, sonia_common::ModemSendCmd::Response &res) + { + char buffer[BUFFER_SIZE]; + + switch ((char)req.cmd) + { + case CMD_GET_BUFFER_LENGTH: + { + Send_CMD_To_Sensor(buffer, CMD_GET_BUFFER_LENGTH); + std::stringstream ss(buffer); + + std::string queue_length; + + std::getline(ss, queue_length, ','); + std::getline(ss, queue_length, '*'); + + res.queue_length = std::stoi(queue_length); + break; + } + case CMD_GET_SETTINGS: + { + Send_CMD_To_Sensor(buffer, CMD_GET_SETTINGS); + std::stringstream ss(buffer); + + std::string role; + std::string channel; + + std::getline(ss, role, ','); + std::getline(ss, role, ','); + std::getline(ss, channel, '*'); + + res.role = (uint8_t)role.at(0); + res.channel = std::stoi(channel); + break; + } + case CMD_GET_DIAGNOSTIC: + { + Send_CMD_To_Sensor(buffer, CMD_GET_DIAGNOSTIC); + std::stringstream ss(buffer); + + std::string link_up; + std::string packet_count; + std::string packet_loss_count; + std::string bit_error_rate; + + std::getline(ss, link_up, ','); + std::getline(ss, link_up, ','); + std::getline(ss, packet_count, ','); + std::getline(ss, packet_loss_count, ','); + std::getline(ss, bit_error_rate, '*'); + + res.link = (uint8_t)link_up.at(0); + res.packet_count = std::stoi(packet_count); + res.packet_count_loss = std::stoi(packet_loss_count); + res.bit_error_rate = std::stof(bit_error_rate); + break; + } + case CMD_SET_SETTINGS: + { + if(Set_Configuration((char)req.role, req.channel)) return false; + res.role = req.role; + res.channel = req.channel; + break; + } + case CMD_FLUSH: + { + if(Flush_Queue()) return false; + break; + } + default: + { + ROS_ERROR_STREAM("CMD received isn't working with this service"); + return false; + } + } + return true; + } + + uint8_t ProviderUnderwaterComNode::Calculate_Checksum(const char (&buffer)[BUFFER_SIZE], const size_t size) + { + uint8_t check = 0; + + for(uint8_t i = 0; i < size; ++i) + { + check = crc_table[(uint8_t)buffer[i] ^ check]; + } + + return check; + } + + uint8_t ProviderUnderwaterComNode::Append_Checksum(char (&buffer)[BUFFER_SIZE], const size_t size) + { + char checksum_buffer[2]; + + uint8_t checksum = Calculate_Checksum(buffer, size); + sprintf(checksum_buffer, "%02x", checksum); + + buffer[size] = CHECKSUM; + buffer[size + 1] = checksum_buffer[0]; + buffer[size + 2] = checksum_buffer[1]; + buffer[size + 3] = EOP; + + return size + 4; + } + + bool ProviderUnderwaterComNode::Confirm_Checksum(char (&buffer)[BUFFER_SIZE], const size_t size) + { + char checksumData[BUFFER_SIZE]; + + try + { + uint8_t position = Find_Character(buffer, CHECKSUM, size); + Copy_Array(buffer, checksumData, position); + uint8_t calculatedChecksum = Calculate_Checksum(checksumData, position); + std::string checksumSentence {buffer[position + 1], buffer[position + 2]}; + uint8_t originalChecksum = std::stoi(checksumSentence, nullptr, 16); + return originalChecksum == calculatedChecksum; + } + catch(...) + { + ROS_WARN_STREAM("Underwater Com: bad checksum"); + return false; + } + } + + void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const char (&packet)[MODEM_M64_PAYLOAD], const size_t size_packet) + { + char send_msg[BUFFER_SIZE]; + uint8_t index = 0; + + if(Check_CMD(&cmd)) + { + send_msg[0] = SOP; + send_msg[1] = DIR_CMD; + send_msg[2] = cmd; + index = 3; + + if(cmd == CMD_QUEUE_PACKET) + { + send_msg[3] = ','; + send_msg[4] = std::to_string(payload_).at(0); + send_msg[5] = ','; + index = 6; + Append_Packet(send_msg, index, packet, size_packet); + } + else if(cmd == CMD_SET_SETTINGS) + { + send_msg[3] = ','; + index = 4; + Append_Packet(send_msg, index, packet, size_packet); + } + + index = Append_Checksum(send_msg, index + size_packet); + std::unique_lock mlock(write_mutex); + std::copy(std::begin(send_msg), std::end(send_msg), std::begin(writeBuffer)); + writeSize = index; + write_cond.notify_one(); + + ROS_DEBUG_STREAM("Packet sent to Modem"); + } + else + { + ROS_WARN_STREAM("CMD unknow. Can't queue packet"); + } + } + + bool ProviderUnderwaterComNode::Transmit_Packet(bool pop_packet) + { + if(writeBuffer[0] != 0) + { + serialConnection_.transmit(writeBuffer, writeSize); + if(pop_packet) + { + writeBuffer[0] = 0; + writeSize = 0; + } + return true; + } + else + { + ROS_WARN_STREAM("Packet isn't queue"); + return false; + } + } + + bool ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const char (&packet)[MODEM_M64_PAYLOAD], size_t size) + { + Queue_Packet(cmd, packet, size); + std::unique_lock mlock(parse_mutex); + parse_cond.wait(mlock); + + if(!parse_string.empty()) + { + std::string tmp = parse_string; + parse_string.erase(); + + for(uint8_t i = 0; i < tmp.size(); ++i) + { + buffer[i] = tmp.at(i); + } + return false; + } + return true; + } + + bool ProviderUnderwaterComNode::Check_CMD(const char *cmd) + { + for(uint8_t i = 0; i < all_valid_size; ++i) + { + if(cmd[0] == all_valid[i]) + { + return true; + } + } + return false; + } + + void ProviderUnderwaterComNode::Append_Packet(char (&buffer)[BUFFER_SIZE], const size_t index, const char (&packet)[MODEM_M64_PAYLOAD], const size_t size_packet) + { + for(uint8_t i = 0; i < size_packet; ++i) + { + buffer[index+i] = packet[i]; + } + } + + uint8_t ProviderUnderwaterComNode::Find_Character(const char (&buffer)[BUFFER_SIZE], const char to_find, const size_t size) + { + uint8_t i; + + for(i = 0; i < size; ++i) + { + if(buffer[i] == to_find) break; + } + return i; + } + + void ProviderUnderwaterComNode::Copy_Array(const char (&buffer)[BUFFER_SIZE], char (&buffer_returned)[BUFFER_SIZE], const size_t size, const size_t start) + { + for(uint8_t i = 0; i < size; ++i) + { + buffer_returned[i+start] = buffer[i]; + } + } + + void ProviderUnderwaterComNode::Manage_Write() + { + ros::Rate r(1); // 1 Hz + ROS_INFO_STREAM("Manage write thread started"); + + while(!ros::isShuttingDown()) + { + std::unique_lock mlock(write_mutex); + write_cond.wait(mlock); + Transmit_Packet(true); + } + } + + void ProviderUnderwaterComNode::Export_To_ROS(const char (&buffer)[BUFFER_SIZE], const ssize_t size) + { + // Modem_M64_t packet; + // uint64_t data = 0; + std_msgs::UInt64 msg; + uint8_t tmp[MODEM_M64_PAYLOAD]; + + for(uint8_t i = 0; i < MODEM_M64_PAYLOAD; ++i) + { + tmp[i] = (uint8_t) buffer[i + 6]; + } + + std::memcpy(&(msg.data), tmp, sizeof(msg.data)); + // packet = *((Modem_M64_t *)&data); + + // msg.depth = packet.depth; + // msg.kill_switch_state = packet.killSwitchState; + // msg.mission_switch_state = packet.missionSwitchState; + // msg.mission_id = packet.missionId; + // msg.mission_state = (int8_t) packet.missionState; + // msg.torpedos_state = packet.torpedosState; + // msg.droppers_state = packet.droppersState; + + underwaterComPublisher_.publish(msg); + } + + void ProviderUnderwaterComNode::Read_Packet() + { + uint8_t i; + char buffer[BUFFER_SIZE]; + + ROS_INFO_STREAM("Read Packet thread started"); + + while(!ros::isShuttingDown()) + { + do + { + serialConnection_.readOnce(buffer, 0); + } while(buffer[0] != SOP); + + for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) + { + serialConnection_. readOnce(buffer, i); + } + + buffer[i] = 0; + + if(i > BUFFER_SIZE) + { + continue; + } + + if(Confirm_Checksum(buffer, i-1)) + { + if(buffer[2] == RESP_GOT_PACKET) + { + Export_To_ROS(buffer, i-1); + } + else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) + { + ROS_DEBUG_STREAM("Packet queue"); + } + else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) + { + ROS_ERROR_STREAM("Resquest not made properly"); + std::unique_lock mlock(parse_mutex); + parse_cond.notify_one(); + } + else + { + std::unique_lock mlock(parse_mutex); + parse_string = std::string(buffer); + parse_cond.notify_one(); + } + } + } + } + + void ProviderUnderwaterComNode::Set_Sensor(const char role, const uint8_t channel) + { + uint8_t i = 0; + + while(i < 3 && init_error_ == true) + { + init_error_ = false; + init_error_ = Verify_Version() | Get_Payload_Load() | Set_Configuration(role, channel) | Flush_Queue(); + ++i; + } + + if(init_error_) + { + ROS_ERROR_STREAM("Problem with the init. Node shutting down."); + } + else + { + ROS_INFO_STREAM("Initialisation completed"); + } + } + + bool ProviderUnderwaterComNode::Verify_Version() + { + std::string major_version = ""; + char buffer[BUFFER_SIZE]; + + if(Send_CMD_To_Sensor(buffer, CMD_GET_VERSION)) return true; + + std::stringstream ss(buffer); + std::getline(ss, major_version, ','); + std::getline(ss, major_version, ','); + + if(major_version != "1") + { + ROS_ERROR_STREAM("Major Version isn't 1."); + return true; + } + ROS_INFO_STREAM("Major version is 1"); + return false; + } + + bool ProviderUnderwaterComNode::Get_Payload_Load() + { + std::string payload = ""; + char buffer[BUFFER_SIZE]; + + if(Send_CMD_To_Sensor(buffer, CMD_GET_PAYLOAD_SIZE)) return true; + + std::stringstream ss(buffer); + std::getline(ss, payload, ','); + std::getline(ss, payload, '*'); + + if((payload >= "0" || payload <= "9")) + { + payload_ = std::stoi(payload); + ROS_INFO_STREAM("Payload set"); + return false; + } + else + { + ROS_ERROR_STREAM("Payload isn't a integer."); + return true; + } + } + + bool ProviderUnderwaterComNode::Set_Configuration(const char role, const uint8_t channel) + { + std::string acknowledge; + char buffer[BUFFER_SIZE]; + char packet[MODEM_M64_PAYLOAD] = {role, ','}; + + sprintf(buffer, "%d", channel); + packet[2] = buffer[0]; + + if(Send_CMD_To_Sensor(buffer, CMD_SET_SETTINGS, packet, 3)) return true; + + std::stringstream ss(buffer); + std::getline(ss, acknowledge, ','); + std::getline(ss, acknowledge, '*'); + + if(acknowledge != std::string(1, ACK)) + { + ROS_ERROR_STREAM("Could not set the configuration."); + return true; + } + ROS_INFO_STREAM("Configuration set"); + return false; + } + + bool ProviderUnderwaterComNode::Flush_Queue() + { + std::string acknowledge; + char buffer[BUFFER_SIZE]; + + if(Send_CMD_To_Sensor(buffer, CMD_FLUSH)) return true; + + std::stringstream ss(buffer); + std::getline(ss, acknowledge, ','); + std::getline(ss, acknowledge, '*'); + + if(acknowledge != std::string(1, ACK)) + { + ROS_ERROR_STREAM("Couldn't flush the queue."); + return true; + } + ROS_INFO_STREAM("Queue flushed"); + return false; + } +} \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h new file mode 100644 index 0000000..12e36d6 --- /dev/null +++ b/src/provider_underwater_com_node.h @@ -0,0 +1,115 @@ +/** + * \file provider_underwater_com_node.h + * \author Francis Alonzo . + */ + +#ifndef PROVIDER_UNDERWATER_COM_NODE +#define PROVIDER_UNDERWATER_COM_NODE + +#include +#include +#include +#include +#include +#include +#include + +#include "Configuration.h" +#include "drivers/serial.h" +#include +#include + +#define BUFFER_SIZE 256 +#define MODEM_M64_PAYLOAD 8 + +namespace provider_underwater_com { + +class ProviderUnderwaterComNode +{ + public: + + ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh); + ~ProviderUnderwaterComNode(); + + void Spin(); + + private: + + void UnderwaterComCallback(const std_msgs::UInt64 &msg); + bool UnderwaterComService(sonia_common::ModemSendCmd::Request &req, sonia_common::ModemSendCmd::Response &res); + + uint8_t Calculate_Checksum(const char (&buffer)[BUFFER_SIZE], const size_t size); // Prevent decay + uint8_t Append_Checksum(char (&buffer)[BUFFER_SIZE], const size_t size); // Prevent decay + bool Confirm_Checksum(char (&buffer)[BUFFER_SIZE], const size_t size); // Prevent decay + + void Queue_Packet(const char cmd, const char (&packet)[MODEM_M64_PAYLOAD] = {}, const size_t size_packet = 0); // Prevent decay + bool Transmit_Packet(bool pop_packet); + bool Send_CMD_To_Sensor(char *buffer, char cmd, const char (&packet)[MODEM_M64_PAYLOAD] = {}, size_t size = 0); // Prevent decay + bool Check_CMD(const char *cmd); + void Append_Packet(char (&buffer)[BUFFER_SIZE], const size_t index, const char (&packet)[MODEM_M64_PAYLOAD], const size_t size_packet); // Prevent decay + uint8_t Find_Character(const char (&buffer)[BUFFER_SIZE], const char to_find, const size_t size); // Prevent decay + void Copy_Array(const char (&buffer)[BUFFER_SIZE], char (&buffer_returned)[BUFFER_SIZE], const size_t size, const size_t start = 0); // Prevent decay + + void Manage_Write(); + void Export_To_ROS(const char (&buffer)[BUFFER_SIZE], const ssize_t size); // Prevent decay + void Read_Packet(); + + void Set_Sensor(const char role, const uint8_t channel = 4); + bool Verify_Version(); + bool Get_Payload_Load(); + bool Set_Configuration(const char role, const uint8_t channel); + bool Flush_Queue(); + + ros::NodeHandlePtr nh_; + Configuration configuration_; + Serial serialConnection_; + + ros::Subscriber underwaterComSubscriber_; + ros::Publisher underwaterComPublisher_; + ros::ServiceServer underwaterComService_; + + std::thread manage_write_thread; + std::thread manage_response_thread; + std::thread read_packet_thread; + + std::mutex write_mutex; + std::mutex response_mutex; + std::mutex parse_mutex; + + std::condition_variable write_cond; + std::condition_variable response_cond; + std::condition_variable parse_cond; + + std::string write_string = ""; + std::string response_string = ""; + std::string parse_string = ""; + + char writeBuffer[BUFFER_SIZE] = {}; + uint8_t writeSize = 0; + uint8_t payload_; + bool init_error_ = true; + // Modem_M64_t modem_data; +}; +} + +#endif //PROVIDER_UNDERWATER_COM_NODE \ No newline at end of file