From c37bbc31973abbec08ec06be8dd0d9aa21eb0f56 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:03:38 -0400 Subject: [PATCH 001/140] add files and start node --- .vscode/launch.json | 2 +- CMakeLists.txt | 97 +++++++++++++++++++++++++++++++++++++++++++++ Dockerfile | 2 +- README.md | 12 +++--- package.xml | 38 ++++++++++++++++++ 5 files changed, 143 insertions(+), 8 deletions(-) create mode 100644 CMakeLists.txt create mode 100644 package.xml diff --git a/.vscode/launch.json b/.vscode/launch.json index 2c39fb0..8fa88b0 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", } ] } 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..bdfcb92 100644 --- a/Dockerfile +++ b/Dockerfile @@ -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} 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/package.xml b/package.xml new file mode 100644 index 0000000..d0546f9 --- /dev/null +++ b/package.xml @@ -0,0 +1,38 @@ + + + provider_underwate_com + 0.0.0 + The provider_underwate_com package + + Francis Alonzo + + Francis Alonzo + GPLv3 + + + + + + + + + + + + + + + + + + + + catkin + + sonia_common + + sonia_common + + sonia_common + + From 7ae966582ed21e7c0c767bfb415b310740b961bf Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:03:49 -0400 Subject: [PATCH 002/140] add files start node --- get_started.sh | 94 --------------------------- launch/provider_underwater_com.launch | 6 ++ src/Configuration.cc | 61 +++++++++++++++++ src/Configuration.h | 62 ++++++++++++++++++ src/drivers/serial.cc | 82 +++++++++++++++++++++++ src/drivers/serial.h | 30 +++++++++ src/main.cc | 35 ++++++++++ src/provider_underwater_com_node.cc | 48 ++++++++++++++ src/provider_underwater_com_node.h | 50 ++++++++++++++ 9 files changed, 374 insertions(+), 94 deletions(-) delete mode 100755 get_started.sh create mode 100644 launch/provider_underwater_com.launch create mode 100644 src/Configuration.cc create mode 100644 src/Configuration.h create mode 100644 src/drivers/serial.cc create mode 100644 src/drivers/serial.h create mode 100644 src/main.cc create mode 100644 src/provider_underwater_com_node.cc create mode 100644 src/provider_underwater_com_node.h 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..9e3a580 --- /dev/null +++ b/launch/provider_underwater_com.launch @@ -0,0 +1,6 @@ + + + \ No newline at end of file diff --git a/src/Configuration.cc b/src/Configuration.cc new file mode 100644 index 0000000..f989093 --- /dev/null +++ b/src/Configuration.cc @@ -0,0 +1,61 @@ +/** + * \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/MODEM"), + settingsFile("settings.txt") + { + Deserialize(); + } + + Configuration::~Configuration() {} + + void Configuration::Deserialize() { + + ROS_INFO("Deserialize params"); + + FindParameter("/connection/tty_port", ttyPort); + FindParameter("/settings/setting_file", settingsFile); + + 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..b184e2c --- /dev/null +++ b/src/Configuration.h @@ -0,0 +1,62 @@ +/** + * \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;} + + private: + + ros::NodeHandlePtr nh; + + std::string ttyPort; + std::string settingsFile; + + 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..64a3f1c --- /dev/null +++ b/src/drivers/serial.cc @@ -0,0 +1,82 @@ +// +// Created by dev on 10/26/17. +// + +#include "serial.h" +#include +#include +#include + +Serial::Serial(std::string port) +{ + fd = open(port.c_str(), O_RDWR | O_NOCTTY); + 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_imu 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_imu receive Once"); + read(fd, (data+offset), 1); +} + +void Serial::flush() +{ + ROS_DEBUG("provider_imu flush data"); + tcflush(fd,TCIOFLUSH); +} + +ssize_t Serial::transmit(const std::string data) +{ + ROS_DEBUG("provider_imu transmit data"); + return write(fd, data.c_str(), data.size()); +} diff --git a/src/drivers/serial.h b/src/drivers/serial.h new file mode 100644 index 0000000..568cbca --- /dev/null +++ b/src/drivers/serial.h @@ -0,0 +1,30 @@ +// +// Created by dev on 10/26/17. +// + +#ifndef INTERFACE_RS485_SERIAL_H +#define INTERFACE_RS485_SERIAL_H + +#include +#include +#include +#include + + +class Serial{ +public: + Serial(std::string port); + ~Serial(); + + std::string receive(size_t count); + void readOnce(char* data, int offset); + void flush(); + ssize_t transmit(const std::string data); + +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..7c4c26c --- /dev/null +++ b/src/main.cc @@ -0,0 +1,35 @@ +/** + * \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(); +} \ 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..70d31a4 --- /dev/null +++ b/src/provider_underwater_com_node.cc @@ -0,0 +1,48 @@ +/** + * \file provider_underwater_com_node.cc + * \author Francis Alonzo . + */ + +#include "provider_underwater_com_node.h" + +#include "Configuration.h" + +namespace provider_underwater_com +{ + + //Node Construtor + ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) + : nh(_nh), Configuration(_nh) + { + + } + + //Node Destructor + ProviderUnderwaterComNode::~ProviderUnderwaterComNode(){} + + //Node Spin + void ProviderUnderwaterComNode::Spin() + { + + } +} \ 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..a7209b5 --- /dev/null +++ b/src/provider_underwater_com_node.h @@ -0,0 +1,50 @@ +/** + * \file provider_underwater_com_node.h + * \author Francis Alonzo . + */ + +#ifndef PROVIDER_UNDERWATER_COM_NODE +#define PROVIDER_UNDERWATER_COM_NODE + +#include + + +namespace provider_underwater_com { + +class ProviderUnderwaterComNode +{ + public: + + ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh); + ~ProviderUnderwaterComNode(); + + void Spin(); + + private: + + +} + +} + +#endif //PROVIDER_UNDERWATER_COM_NODE \ No newline at end of file From 05d78ad5c761ed201155f8c85cc727b93e25954e Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:06:23 -0400 Subject: [PATCH 003/140] fix for compilation --- package.xml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/package.xml b/package.xml index d0546f9..e50fd6b 100644 --- a/package.xml +++ b/package.xml @@ -1,8 +1,8 @@ - provider_underwate_com + provider_underwater_com 0.0.0 - The provider_underwate_com package + The provider_underwater_com package Francis Alonzo From aa717e245d413a6292b322d37c5ddc34e5f230a2 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:13:42 -0400 Subject: [PATCH 004/140] fix constructor --- src/provider_underwater_com_node.cc | 2 +- src/provider_underwater_com_node.h | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 70d31a4..358d21b 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -32,7 +32,7 @@ namespace provider_underwater_com //Node Construtor ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) - : nh(_nh), Configuration(_nh) + : nh(_nh), configuration(_nh) { } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index a7209b5..dd57581 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -42,7 +42,9 @@ class ProviderUnderwaterComNode private: + Configuration configuration; + ros::NodeHandlePtr nh; } } From 1d673edad33d15a22a06304af96b96fed919f699 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:16:22 -0400 Subject: [PATCH 005/140] update package.xml --- package.xml | 38 +++++++++++++++++--------------------- 1 file changed, 17 insertions(+), 21 deletions(-) diff --git a/package.xml b/package.xml index e50fd6b..71200f5 100644 --- a/package.xml +++ b/package.xml @@ -9,30 +9,26 @@ Francis Alonzo GPLv3 - - - - - - - - - - - - - - - - - - catkin + roscpp + log4cxx + self_test + diagnostic_updater + tf + std_msgs + sensor_msgs + message_generation sonia_common - sonia_common - - sonia_common + roscpp + log4cxx + self_test + diagnostic_updater + tf + std_msgs + sensor_msgs + message_runtime + sonia_common From 1b1d50fd567ce9a7f7d09193ef80c92174e2b4f3 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:16:30 -0400 Subject: [PATCH 006/140] version bump --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 71200f5..4ed6b9e 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.0.0 + 0.0.1 The provider_underwater_com package Francis Alonzo From dc73c20fd1a6d9e1762d8f289c539549b1fe11d0 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:18:51 -0400 Subject: [PATCH 007/140] fix compilation run_depend to exec_depend --- package.xml | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/package.xml b/package.xml index 4ed6b9e..76f2eed 100644 --- a/package.xml +++ b/package.xml @@ -21,14 +21,14 @@ message_generation sonia_common - roscpp - log4cxx - self_test - diagnostic_updater - tf - std_msgs - sensor_msgs - message_runtime - sonia_common + roscpp + log4cxx + self_test + diagnostic_updater + tf + std_msgs + sensor_msgs + message_runtime + sonia_common From e3d12859b7c15a2bea160f50f4d7dad58fab7f54 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:22:00 -0400 Subject: [PATCH 008/140] change devcontainer.json --- .devcontainer/devcontainer.json | 6 +++--- src/provider_underwater_com_node.cc | 2 -- src/provider_underwater_com_node.h | 4 ++-- 3 files changed, 5 insertions(+), 7 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index db27116..ef3eb3a 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. @@ -25,6 +25,6 @@ "mounts": [ "source=vscode-server-extension,target=/home/sonia/.vscode-server/extensions,type=volume" ], - "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/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 358d21b..be16305 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -25,8 +25,6 @@ #include "provider_underwater_com_node.h" -#include "Configuration.h" - namespace provider_underwater_com { diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index dd57581..1279b66 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -27,7 +27,7 @@ #define PROVIDER_UNDERWATER_COM_NODE #include - +#include "Configuration.h" namespace provider_underwater_com { @@ -45,7 +45,7 @@ class ProviderUnderwaterComNode Configuration configuration; ros::NodeHandlePtr nh; -} +}; } From 100aba59213d633b481d1cc577bc5b03ef2ae484 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 21:24:38 -0400 Subject: [PATCH 009/140] add serial connection --- src/provider_underwater_com_node.cc | 2 +- src/provider_underwater_com_node.h | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index be16305..f90ab83 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -30,7 +30,7 @@ namespace provider_underwater_com //Node Construtor ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) - : nh(_nh), configuration(_nh) + : nh(_nh), configuration(_nh), serialConnection(configuration.getTtyPort()) { } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 1279b66..8b53389 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -28,6 +28,7 @@ #include #include "Configuration.h" +#include "drivers/serial.h" namespace provider_underwater_com { @@ -43,7 +44,7 @@ class ProviderUnderwaterComNode private: Configuration configuration; - + Serial serialConnection; ros::NodeHandlePtr nh; }; From 4d38f8b00c246f4ec950cd0eba9535cf45a895e9 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 3 Sep 2021 01:28:08 +0000 Subject: [PATCH 010/140] change private variables --- .vscode/settings.json | 4 ++++ src/provider_underwater_com_node.cc | 2 +- src/provider_underwater_com_node.h | 6 +++--- 3 files changed, 8 insertions(+), 4 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e542113..ede23ff 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,5 +2,9 @@ "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" ] } diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index f90ab83..2667c84 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -30,7 +30,7 @@ namespace provider_underwater_com //Node Construtor ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) - : nh(_nh), configuration(_nh), serialConnection(configuration.getTtyPort()) + : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 8b53389..16ac2d7 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -43,9 +43,9 @@ class ProviderUnderwaterComNode private: - Configuration configuration; - Serial serialConnection; - ros::NodeHandlePtr nh; + Configuration configuration_; + Serial serialConnection_; + ros::NodeHandlePtr nh_; }; } From 0b00cb7aa2c6078e4242f998096bdb429fd29afa Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 3 Sep 2021 01:29:37 +0000 Subject: [PATCH 011/140] change order for initialization --- src/provider_underwater_com_node.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 16ac2d7..df639ab 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -43,9 +43,10 @@ class ProviderUnderwaterComNode private: + ros::NodeHandlePtr nh_; Configuration configuration_; Serial serialConnection_; - ros::NodeHandlePtr nh_; + }; } From 97e97441dfae24be5957213fa2d567d4aedadcc1 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 3 Sep 2021 01:35:59 +0000 Subject: [PATCH 012/140] test of node --- src/provider_underwater_com_node.cc | 11 +++++++++-- src/provider_underwater_com_node.h | 4 ++-- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 2667c84..69f53ac 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -30,7 +30,7 @@ namespace provider_underwater_com //Node Construtor ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) - : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) + : nh_(_nh) //, configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { } @@ -41,6 +41,13 @@ namespace provider_underwater_com //Node Spin void ProviderUnderwaterComNode::Spin() { - + ros::Rate r(1); + + while(ros::ok()) + { + ROS_INFO_STREAM("Node is working"); + ros::spinOnce(); + r.sleep(); + } } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index df639ab..4317798 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -44,8 +44,8 @@ class ProviderUnderwaterComNode private: ros::NodeHandlePtr nh_; - Configuration configuration_; - Serial serialConnection_; + //Configuration configuration_; + //Serial serialConnection_; }; From 64a3ced8a047133223c8bbf08ccfe10aba69c1a6 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 3 Sep 2021 01:46:28 +0000 Subject: [PATCH 013/140] from unneed files --- scripts/shflags | 1214 ----------------------------------------------- 1 file changed, 1214 deletions(-) delete mode 100644 scripts/shflags 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 From f44c33a1af446cb36ceab99d488ad5eb22601abb Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 3 Sep 2021 01:50:01 +0000 Subject: [PATCH 014/140] change for launch --- .vscode/launch.json | 2 +- launch/provider_underwater_com.launch | 5 +---- 2 files changed, 2 insertions(+), 5 deletions(-) diff --git a/.vscode/launch.json b/.vscode/launch.json index 8fa88b0..74e813b 100644 --- a/.vscode/launch.json +++ b/.vscode/launch.json @@ -12,4 +12,4 @@ "target": "/home/sonia/ros_sonia_ws/src/provider_underwater_com/launch/provider_underwater_com.launch", } ] -} +} \ No newline at end of file diff --git a/launch/provider_underwater_com.launch b/launch/provider_underwater_com.launch index 9e3a580..b8efaba 100644 --- a/launch/provider_underwater_com.launch +++ b/launch/provider_underwater_com.launch @@ -1,6 +1,3 @@ - + \ No newline at end of file From b5064775335167c1f54d527de0ab64254ce73793 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 2 Sep 2021 22:18:24 -0400 Subject: [PATCH 015/140] remove bootstrap --- BOOTSTRAP.md | 53 ---------------------------------------------------- 1 file changed, 53 deletions(-) delete mode 100644 BOOTSTRAP.md 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.* From 5f12872c5de6cb9dd46589edaf9a9cd13e29623f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 6 Sep 2021 18:48:12 +0000 Subject: [PATCH 016/140] starting the node --- .devcontainer/devcontainer.json | 4 +++- src/provider_underwater_com_node.cc | 11 ++++++++--- src/provider_underwater_com_node.h | 16 ++++++++++++++++ 3 files changed, 27 insertions(+), 4 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index ef3eb3a..09797ed 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -19,11 +19,13 @@ "--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/provider_underwater_com,type=volume", "workspaceFolder": "/home/sonia/ros_sonia_ws/src/provider_underwater_com" diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 69f53ac..6cd8a36 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -30,9 +30,10 @@ namespace provider_underwater_com //Node Construtor ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) - : nh_(_nh) //, configuration_(_nh), serialConnection_(configuration_.getTtyPort()) + : nh_(_nh)//, configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { - + underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); + underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); } //Node Destructor @@ -45,9 +46,13 @@ namespace provider_underwater_com while(ros::ok()) { - ROS_INFO_STREAM("Node is working"); ros::spinOnce(); r.sleep(); } } + + void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt8 & msg) + { + + } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 4317798..8583d29 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -27,6 +27,9 @@ #define PROVIDER_UNDERWATER_COM_NODE #include +#include +#include + #include "Configuration.h" #include "drivers/serial.h" @@ -43,10 +46,23 @@ class ProviderUnderwaterComNode private: + void UnderwaterComCallback(const std_msgs::UInt8 & msg); + ros::NodeHandlePtr nh_; //Configuration configuration_; //Serial serialConnection_; + ros::Subscriber underwaterComSubscriber_; + ros::Publisher underwaterComPublisher_; + + std::string str_get_version = "wcv"; + std::string str_get_payload_size = "wcn"; + std::string str_get_modem_config = "wcc"; + std::string str_set_modem_config = "wcs"; + std::string str_get_transmit_queue_length = "wcl"; + std::string str_flush_transmit_queue = "wcf"; + std::string str_get_diagnostic = "wcd"; + std::string str_queue_for_transmit = "wcq"; }; } From 1221b57a74f437925f9d1e4693127bf13e9feec0 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 7 Sep 2021 02:03:57 +0000 Subject: [PATCH 017/140] add checksum same as IMU --- src/ModemM64_definitions.h | 45 +++++++++++++++++++++++++++++ src/provider_underwater_com_node.cc | 45 ++++++++++++++++++++++++++++- src/provider_underwater_com_node.h | 33 +++++++++++---------- 3 files changed, 107 insertions(+), 16 deletions(-) create mode 100644 src/ModemM64_definitions.h diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h new file mode 100644 index 0000000..a721713 --- /dev/null +++ b/src/ModemM64_definitions.h @@ -0,0 +1,45 @@ +/** + * \file ModemM64_defitions.h + * \author Francis Alonzo . + */ + +#ifndef MODEMM64_DEFINITIONS +#define MODEMM64_DEFINITIONS + +#define SOP "w" +#define EOP "\n" +#define DIR_CMD "c" +#define DIR_RESP "r" +#define CHECKSUM "*" + +#define CMD_GET_VERSION "v" +#define CMD_GET_PAYLOAD_SIZE "n" +#define CMD_GET_BUFFER_LENGTH "l" +#define CMD_GET_DIAGNOSTIC "d" +#define CMD_GET_SETTINGS "c" +#define CMD_SET_SETTINGS "s" +#define CMD_QUEUE_PACKET "q" +#define CMD_FLUSH "f" +#define RESP_GOT_PACKET "p" + +#endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 6cd8a36..91693a7 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -51,8 +51,51 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt8 & msg) + void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt8 &msg) { + std::string packet = std::to_string(msg.data); + std::string dir = DIR_CMD; + std::string cmd = CMD_QUEUE_PACKET; + + Queue_Packet(dir, cmd, packet); + } + + uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string sentence) + { + uint8_t check = 0; + + for(unsigned int i = 1; i < sentence.size(); i++) + check ^= sentence[i]; + return check; + } + + void ProviderUnderwaterComNode::AppendChecksum(std::string &sentence) + { + std::stringstream ss; + uint8_t checksum = CalculateChecksum(sentence); + ss << sentence << std::string("*") << std::hex << checksum; + sentence = ss.str(); + } + + bool ProviderUnderwaterComNode::ConfirmChecksum(const std::string &sentence) + { + try + { + std::string checksumData = sentence.substr(0, sentence.find("*", 0)); + uint8_t calculatedChecksum = CalculateChecksum(checksumData); + uint8_t originalChecksum = std::stoi(sentence.substr(sentence.find("*", 0)+1, 2), nullptr, 16); + return originalChecksum == calculatedChecksum; + } + catch(...) + { + ROS_INFO_STREAM("Underwater Com: bad packet checksum"); + return false; + } + } + + void ProviderUnderwaterComNode::Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet) + { + } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 8583d29..053c970 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -29,9 +29,12 @@ #include #include #include +#include +#include #include "Configuration.h" #include "drivers/serial.h" +#include "ModemM64_definitions.h" namespace provider_underwater_com { @@ -46,23 +49,23 @@ class ProviderUnderwaterComNode private: - void UnderwaterComCallback(const std_msgs::UInt8 & msg); + void UnderwaterComCallback(const std_msgs::UInt8 &msg); + + uint8_t CalculateChecksum(const std::string sentence); + void AppendChecksum(std::string &sentence); + bool ConfirmChecksum(const std::string &sentence); + + void Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet); - ros::NodeHandlePtr nh_; - //Configuration configuration_; - //Serial serialConnection_; - - ros::Subscriber underwaterComSubscriber_; - ros::Publisher underwaterComPublisher_; - std::string str_get_version = "wcv"; - std::string str_get_payload_size = "wcn"; - std::string str_get_modem_config = "wcc"; - std::string str_set_modem_config = "wcs"; - std::string str_get_transmit_queue_length = "wcl"; - std::string str_flush_transmit_queue = "wcf"; - std::string str_get_diagnostic = "wcd"; - std::string str_queue_for_transmit = "wcq"; + ros::NodeHandlePtr nh_; + //Configuration configuration_; + //Serial serialConnection_; + + ros::Subscriber underwaterComSubscriber_; + ros::Publisher underwaterComPublisher_; + + }; } From 4140b7bc4d311778348c326a36351b39dfa7a8d7 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 7 Sep 2021 02:29:54 +0000 Subject: [PATCH 018/140] start transmit --- src/provider_underwater_com_node.cc | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 91693a7..edfca3f 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -96,6 +96,22 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet) { + std::stringstream ss; + std::string sentence; + + if(cmd != CMD_QUEUE_PACKET || cmd != CMD_SET_SETTINGS) + { + ss << SOP << direction << cmd; + } + else + { + ss << SOP << direction << cmd << std::string(",") << packet; + } + + sentence = ss.str(); + AppendChecksum(sentence); + + } } \ No newline at end of file From bf063529ebe4c240b70467f32ef73838e4170960 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 7 Sep 2021 02:38:28 +0000 Subject: [PATCH 019/140] fix condition --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index edfca3f..5d44e12 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -99,7 +99,7 @@ namespace provider_underwater_com std::stringstream ss; std::string sentence; - if(cmd != CMD_QUEUE_PACKET || cmd != CMD_SET_SETTINGS) + if(cmd != CMD_QUEUE_PACKET && cmd != CMD_SET_SETTINGS) { ss << SOP << direction << cmd; } From d559c35050ac4ed24a3671a95eec7f738c2d1d76 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 10:04:40 -0400 Subject: [PATCH 020/140] start working with the sensor --- src/provider_underwater_com_node.cc | 2 +- src/provider_underwater_com_node.h | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 5d44e12..03ecdc4 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -30,7 +30,7 @@ namespace provider_underwater_com //Node Construtor ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) - : nh_(_nh)//, configuration_(_nh), serialConnection_(configuration_.getTtyPort()) + : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 053c970..e64c9f1 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -59,8 +59,8 @@ class ProviderUnderwaterComNode ros::NodeHandlePtr nh_; - //Configuration configuration_; - //Serial serialConnection_; + Configuration configuration_; + Serial serialConnection_; ros::Subscriber underwaterComSubscriber_; ros::Publisher underwaterComPublisher_; From d9f6272caf899d397d9977f615f86827db8895b2 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 10:05:46 -0400 Subject: [PATCH 021/140] add license for serial.cc and serial.h --- src/drivers/serial.cc | 27 ++++++++++++++++++++++++--- src/drivers/serial.h | 27 ++++++++++++++++++++++++--- 2 files changed, 48 insertions(+), 6 deletions(-) diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index 64a3f1c..787ff3d 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -1,6 +1,27 @@ -// -// Created by dev on 10/26/17. -// +/** + * \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 diff --git a/src/drivers/serial.h b/src/drivers/serial.h index 568cbca..8f68d43 100644 --- a/src/drivers/serial.h +++ b/src/drivers/serial.h @@ -1,6 +1,27 @@ -// -// Created by dev on 10/26/17. -// +/** + * \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 From 193739adc28c5dfb88f813799ac651c4fdde0300 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 14:09:35 +0000 Subject: [PATCH 022/140] change path for usb to debug --- src/Configuration.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Configuration.cc b/src/Configuration.cc index f989093..df83e68 100644 --- a/src/Configuration.cc +++ b/src/Configuration.cc @@ -30,7 +30,7 @@ namespace provider_underwater_com Configuration::Configuration(const ros::NodeHandlePtr &nh) : nh(nh), - ttyPort("/dev/MODEM"), + ttyPort("/dev/ttyUSB0"), // "/dev/MODEM" for AUVs, debug only settingsFile("settings.txt") { Deserialize(); From 2e838b9db71b0d87634eb76921b9902e91a5e0c2 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 10:21:13 -0400 Subject: [PATCH 023/140] fix dialout for sonia user in the docker --- Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/Dockerfile b/Dockerfile index bdfcb92..37907b5 100644 --- a/Dockerfile +++ b/Dockerfile @@ -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} From b1c8136a2974aaeb7fb75f362cb3bab75846231b Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 18:15:36 +0000 Subject: [PATCH 024/140] changes to the code --- src/ModemM64_definitions.h | 28 ++++++++-------- src/provider_underwater_com_node.cc | 52 +++++++++++++++++++++-------- src/provider_underwater_com_node.h | 6 +++- 3 files changed, 58 insertions(+), 28 deletions(-) diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h index a721713..63ec1fa 100644 --- a/src/ModemM64_definitions.h +++ b/src/ModemM64_definitions.h @@ -26,20 +26,20 @@ #ifndef MODEMM64_DEFINITIONS #define MODEMM64_DEFINITIONS -#define SOP "w" -#define EOP "\n" -#define DIR_CMD "c" -#define DIR_RESP "r" -#define CHECKSUM "*" +#define SOP 'w' +#define EOP '\n' +#define DIR_CMD 'c' +#define DIR_RESP 'r' +#define CHECKSUM '*' -#define CMD_GET_VERSION "v" -#define CMD_GET_PAYLOAD_SIZE "n" -#define CMD_GET_BUFFER_LENGTH "l" -#define CMD_GET_DIAGNOSTIC "d" -#define CMD_GET_SETTINGS "c" -#define CMD_SET_SETTINGS "s" -#define CMD_QUEUE_PACKET "q" -#define CMD_FLUSH "f" -#define RESP_GOT_PACKET "p" +#define CMD_GET_VERSION 'v' +#define CMD_GET_PAYLOAD_SIZE 'n' +#define CMD_GET_BUFFER_LENGTH 'l' +#define CMD_GET_DIAGNOSTIC 'd' +#define CMD_GET_SETTINGS 'c' +#define CMD_SET_SETTINGS 's' +#define CMD_QUEUE_PACKET 'q' +#define CMD_FLUSH 'f' +#define RESP_GOT_PACKET 'p' #endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 03ecdc4..200677a 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -25,6 +25,8 @@ #include "provider_underwater_com_node.h" +#define BUFFER_SIZE 256 + namespace provider_underwater_com { @@ -32,12 +34,20 @@ namespace provider_underwater_com ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { + serialConnection_.flush(); + underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); + + reader_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); } //Node Destructor - ProviderUnderwaterComNode::~ProviderUnderwaterComNode(){} + ProviderUnderwaterComNode::~ProviderUnderwaterComNode() + { + underwaterComSubscriber_.shutdown(); + reader_thread.~thread(); + } //Node Spin void ProviderUnderwaterComNode::Spin() @@ -53,9 +63,9 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt8 &msg) { - std::string packet = std::to_string(msg.data); - std::string dir = DIR_CMD; - std::string cmd = CMD_QUEUE_PACKET; + std::string packet = ",8," + std::to_string(msg.data); // Payload for the CMD to send, always 8 + std::string dir = std::to_string(DIR_CMD); + std::string cmd = std::to_string(CMD_QUEUE_PACKET); Queue_Packet(dir, cmd, packet); } @@ -64,7 +74,7 @@ namespace provider_underwater_com { uint8_t check = 0; - for(unsigned int i = 1; i < sentence.size(); i++) + for(unsigned int i = 0; i < sentence.size(); i++) check ^= sentence[i]; return check; @@ -74,7 +84,7 @@ namespace provider_underwater_com { std::stringstream ss; uint8_t checksum = CalculateChecksum(sentence); - ss << sentence << std::string("*") << std::hex << checksum; + ss << sentence << std::to_string(CHECKSUM) << std::hex << checksum; sentence = ss.str(); } @@ -89,7 +99,7 @@ namespace provider_underwater_com } catch(...) { - ROS_INFO_STREAM("Underwater Com: bad packet checksum"); + ROS_INFO_STREAM("Underwater Com: bad checksum"); return false; } } @@ -99,19 +109,35 @@ namespace provider_underwater_com std::stringstream ss; std::string sentence; - if(cmd != CMD_QUEUE_PACKET && cmd != CMD_SET_SETTINGS) + if(cmd != std::to_string(CMD_QUEUE_PACKET) && cmd != std::to_string(CMD_SET_SETTINGS)) { - ss << SOP << direction << cmd; + ss << SOP << direction << cmd << EOP; } else { - ss << SOP << direction << cmd << std::string(",") << packet; + ss << SOP << direction << cmd << std::string(",") << packet << EOP; } sentence = ss.str(); - AppendChecksum(sentence); + //AppendChecksum(sentence); + serialConnection_.transmit(sentence); - + ROS_DEBUG("Packet sent to Modem"); + } - } + void ProviderUnderwaterComNode::Read_Packet() + { + ROS_INFO_STREAM("Reader thread started"); + char buffer[BUFFER_SIZE]; + + while(!ros::isShuttingDown()) + { + do + { + serialConnection_.readOnce(buffer, 0); + } while (buffer[0] != SOP); + + } + + } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index e64c9f1..3125bca 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -31,6 +31,9 @@ #include #include #include +#include +#include +#include #include "Configuration.h" #include "drivers/serial.h" @@ -56,6 +59,7 @@ class ProviderUnderwaterComNode bool ConfirmChecksum(const std::string &sentence); void Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet); + void Read_Packet(); ros::NodeHandlePtr nh_; @@ -65,7 +69,7 @@ class ProviderUnderwaterComNode ros::Subscriber underwaterComSubscriber_; ros::Publisher underwaterComPublisher_; - + std::thread reader_thread; }; } From fd50b403085d6a9b2b80b4c8eec309300ea4d6fb Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 18:44:24 +0000 Subject: [PATCH 025/140] start to add error gestion --- src/ModemM64_definitions.h | 3 ++ src/provider_underwater_com_node.cc | 58 ++++++++++++++++++++++------- src/provider_underwater_com_node.h | 8 +++- 3 files changed, 55 insertions(+), 14 deletions(-) diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h index 63ec1fa..035042f 100644 --- a/src/ModemM64_definitions.h +++ b/src/ModemM64_definitions.h @@ -42,4 +42,7 @@ #define CMD_FLUSH 'f' #define RESP_GOT_PACKET 'p' +#define ROLE_MASTER 'a' +#define ROLE_SLAVE 'b' + #endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 200677a..3edb6a7 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -40,6 +40,8 @@ namespace provider_underwater_com underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); reader_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); + + Set_Sensor(ROLE_MASTER, 4); } //Node Destructor @@ -123,21 +125,51 @@ namespace provider_underwater_com serialConnection_.transmit(sentence); ROS_DEBUG("Packet sent to Modem"); - } + } - void ProviderUnderwaterComNode::Read_Packet() - { - ROS_INFO_STREAM("Reader thread started"); - char buffer[BUFFER_SIZE]; + void ProviderUnderwaterComNode::Read_Packet() + { + ROS_INFO_STREAM("Reader thread started"); + char buffer[BUFFER_SIZE]; - while(!ros::isShuttingDown()) - { - do + while(!ros::isShuttingDown()) { - serialConnection_.readOnce(buffer, 0); - } while (buffer[0] != SOP); - - } + do + { + serialConnection_.readOnce(buffer, 0); + } + while (buffer[0] != SOP); + + uint8_t i; + + for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) + { + serialConnection_. readOnce(buffer, i); + } + + if(i >= BUFFER_SIZE) + { + continue; + } + + buffer[i] = 0; + + if(buffer[1] != DIR_RESP) + { + ROS_INFO_STREAM("Error on the response"); + } + + ROS_INFO_STREAM(std::string(buffer)); + } + } - } + bool ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) + { + Verify_Version(); + } + + void ProviderUnderwaterComNode::Verify_Version() + { + Queue_Packet(std::to_string(DIR_CMD),std::to_string(CMD_GET_VERSION)); + } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 3125bca..06926b4 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -58,8 +58,10 @@ class ProviderUnderwaterComNode void AppendChecksum(std::string &sentence); bool ConfirmChecksum(const std::string &sentence); - void Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet); + void Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet = ""); void Read_Packet(); + bool Set_Sensor(const char &role = ROLE_MASTER, uint8_t channel = 4); + void Verify_Version(); ros::NodeHandlePtr nh_; @@ -70,6 +72,10 @@ class ProviderUnderwaterComNode ros::Publisher underwaterComPublisher_; std::thread reader_thread; + std::mutex reponse_mutex; + std::condition_variable repsonse_cond; + std::string response_str = ""; + }; } From 2366e249de4387f1e8071c82cbc703ba97d4f5cb Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 18:45:16 +0000 Subject: [PATCH 026/140] add logs --- src/provider_underwater_com_node.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 3edb6a7..4ece900 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -121,7 +121,8 @@ namespace provider_underwater_com } sentence = ss.str(); - //AppendChecksum(sentence); + ROS_INFO_STREAM(sentence); + serialConnection_.transmit(sentence); ROS_DEBUG("Packet sent to Modem"); From 7753fe7da62f9d8d86d0d6f5b849ac3e1b050b69 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 18:48:07 +0000 Subject: [PATCH 027/140] fix bool function --- src/provider_underwater_com_node.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 4ece900..121c90e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -122,7 +122,7 @@ namespace provider_underwater_com sentence = ss.str(); ROS_INFO_STREAM(sentence); - + serialConnection_.transmit(sentence); ROS_DEBUG("Packet sent to Modem"); @@ -167,6 +167,7 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) { Verify_Version(); + return true; } void ProviderUnderwaterComNode::Verify_Version() From 87fafb0a9f74fcdcc26963fcb6539635f63aefe6 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 19:35:01 +0000 Subject: [PATCH 028/140] add verification of sensor version --- src/provider_underwater_com_node.cc | 49 ++++++++++++++++++++++------- src/provider_underwater_com_node.h | 10 +++--- 2 files changed, 43 insertions(+), 16 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 121c90e..d50d154 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -66,8 +66,8 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt8 &msg) { std::string packet = ",8," + std::to_string(msg.data); // Payload for the CMD to send, always 8 - std::string dir = std::to_string(DIR_CMD); - std::string cmd = std::to_string(CMD_QUEUE_PACKET); + std::string dir = std::string(1, DIR_CMD); + std::string cmd = std::string(1, CMD_QUEUE_PACKET); Queue_Packet(dir, cmd, packet); } @@ -86,7 +86,7 @@ namespace provider_underwater_com { std::stringstream ss; uint8_t checksum = CalculateChecksum(sentence); - ss << sentence << std::to_string(CHECKSUM) << std::hex << checksum; + ss << sentence << std::string(1, CHECKSUM) << std::hex << checksum; sentence = ss.str(); } @@ -111,7 +111,7 @@ namespace provider_underwater_com std::stringstream ss; std::string sentence; - if(cmd != std::to_string(CMD_QUEUE_PACKET) && cmd != std::to_string(CMD_SET_SETTINGS)) + if(cmd != std::string(1, CMD_QUEUE_PACKET) && cmd != std::string(1, CMD_SET_SETTINGS)) { ss << SOP << direction << cmd << EOP; } @@ -121,8 +121,6 @@ namespace provider_underwater_com } sentence = ss.str(); - ROS_INFO_STREAM(sentence); - serialConnection_.transmit(sentence); ROS_DEBUG("Packet sent to Modem"); @@ -160,18 +158,45 @@ namespace provider_underwater_com ROS_INFO_STREAM("Error on the response"); } - ROS_INFO_STREAM(std::string(buffer)); + if(buffer[2] != CMD_FLUSH) + { + std::unique_lock mlock(response_mutex); + response_str = std::string(buffer); + response_cond.notify_one(); + } } } - bool ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) + void ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) { - Verify_Version(); - return true; + + if(Verify_Version()) + { + ROS_INFO_STREAM("Major version isn't of 1"); + ros::shutdown(); + } } - void ProviderUnderwaterComNode::Verify_Version() + bool ProviderUnderwaterComNode::Verify_Version() { - Queue_Packet(std::to_string(DIR_CMD),std::to_string(CMD_GET_VERSION)); + std::string major_version = ""; + + Queue_Packet(std::string(1, DIR_CMD),std::string(1, CMD_GET_VERSION)); + + std::unique_lock mlock(response_mutex); + response_cond.wait(mlock); + + std::stringstream ss(response_str); + std::getline(ss, major_version, ','); + + if(std::stoi(major_version) == 1 && ConfirmChecksum(response_str)) + { + return true; + } + else + { + return false; + } + } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 06926b4..78a702b 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -60,8 +60,8 @@ class ProviderUnderwaterComNode void Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet = ""); void Read_Packet(); - bool Set_Sensor(const char &role = ROLE_MASTER, uint8_t channel = 4); - void Verify_Version(); + void Set_Sensor(const char &role = ROLE_MASTER, uint8_t channel = 4); + bool Verify_Version(); ros::NodeHandlePtr nh_; @@ -72,9 +72,11 @@ class ProviderUnderwaterComNode ros::Publisher underwaterComPublisher_; std::thread reader_thread; - std::mutex reponse_mutex; - std::condition_variable repsonse_cond; + std::mutex response_mutex; + std::condition_variable response_cond; std::string response_str = ""; + + uint8_t payload_; }; From f99669a04dc025d381bddf757adbc0a6e0bb63d3 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 20:01:41 +0000 Subject: [PATCH 029/140] test for fixed branch --- .../workflows/docker-image-perception-fix.yml | 103 ++++++++++++++++++ src/ModemM64_definitions.h | 35 ++++++ src/provider_underwater_com_node.cc | 13 ++- src/provider_underwater_com_node.h | 2 +- 4 files changed, 147 insertions(+), 6 deletions(-) create mode 100644 .github/workflows/docker-image-perception-fix.yml diff --git a/.github/workflows/docker-image-perception-fix.yml b/.github/workflows/docker-image-perception-fix.yml new file mode 100644 index 0000000..c973bd7 --- /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: template_name + 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: template_name + 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/src/ModemM64_definitions.h b/src/ModemM64_definitions.h index 035042f..7f7f6ce 100644 --- a/src/ModemM64_definitions.h +++ b/src/ModemM64_definitions.h @@ -45,4 +45,39 @@ #define ROLE_MASTER 'a' #define ROLE_SLAVE 'b' +static const uint8_t lookup_table[256] = { + 0x00U,0x07U,0x0EU,0x09U,0x1CU,0x1BU,0x12U,0x15U, + 0x38U,0x3FU,0x36U,0x31U,0x24U,0x23U,0x2AU,0x2DU, + 0x70U,0x77U,0x7EU,0x79U,0x6CU,0x6BU,0x62U,0x65U, + 0x48U,0x4FU,0x46U,0x41U,0x54U,0x53U,0x5AU,0x5DU, + 0xE0U,0xE7U,0xEEU,0xE9U,0xFCU,0xFBU,0xF2U,0xF5U, + 0xD8U,0xDFU,0xD6U,0xD1U,0xC4U,0xC3U,0xCAU,0xCDU, + 0x90U,0x97U,0x9EU,0x99U,0x8CU,0x8BU,0x82U,0x85U, + 0xA8U,0xAFU,0xA6U,0xA1U,0xB4U,0xB3U,0xBAU,0xBDU, + 0xC7U,0xC0U,0xC9U,0xCEU,0xDBU,0xDCU,0xD5U,0xD2U, + 0xFFU,0xF8U,0xF1U,0xF6U,0xE3U,0xE4U,0xEDU,0xEAU, + 0xB7U,0xB0U,0xB9U,0xBEU,0xABU,0xACU,0xA5U,0xA2U, + 0x8FU,0x88U,0x81U,0x86U,0x93U,0x94U,0x9DU,0x9AU, + 0x27U,0x20U,0x29U,0x2EU,0x3BU,0x3CU,0x35U,0x32U, + 0x1FU,0x18U,0x11U,0x16U,0x03U,0x04U,0x0DU,0x0AU, + 0x57U,0x50U,0x59U,0x5EU,0x4BU,0x4CU,0x45U,0x42U, + 0x6FU,0x68U,0x61U,0x66U,0x73U,0x74U,0x7DU,0x7AU, + 0x89U,0x8EU,0x87U,0x80U,0x95U,0x92U,0x9BU,0x9CU, + 0xB1U,0xB6U,0xBFU,0xB8U,0xADU,0xAAU,0xA3U,0xA4U, + 0xF9U,0xFEU,0xF7U,0xF0U,0xE5U,0xE2U,0xEBU,0xECU, + 0xC1U,0xC6U,0xCFU,0xC8U,0xDDU,0xDAU,0xD3U,0xD4U, + 0x69U,0x6EU,0x67U,0x60U,0x75U,0x72U,0x7BU,0x7CU, + 0x51U,0x56U,0x5FU,0x58U,0x4DU,0x4AU,0x43U,0x44U, + 0x19U,0x1EU,0x17U,0x10U,0x05U,0x02U,0x0BU,0x0CU, + 0x21U,0x26U,0x2FU,0x28U,0x3DU,0x3AU,0x33U,0x34U, + 0x4EU,0x49U,0x40U,0x47U,0x52U,0x55U,0x5CU,0x5BU, + 0x76U,0x71U,0x78U,0x7FU,0x6AU,0x6DU,0x64U,0x63U, + 0x3EU,0x39U,0x30U,0x37U,0x22U,0x25U,0x2CU,0x2BU, + 0x06U,0x01U,0x08U,0x0FU,0x1AU,0x1DU,0x14U,0x13U, + 0xAEU,0xA9U,0xA0U,0xA7U,0xB2U,0xB5U,0xBCU,0xBBU, + 0x96U,0x91U,0x98U,0x9FU,0x8AU,0x8DU,0x84U,0x83U, + 0xDEU,0xD9U,0xD0U,0xD7U,0xC2U,0xC5U,0xCCU,0xCBU, + 0xE6U,0xE1U,0xE8U,0xEFU,0xFAU,0xFDU,0xF4U,0xF3U, +}; + #endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index d50d154..a5e138e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -72,12 +72,14 @@ namespace provider_underwater_com Queue_Packet(dir, cmd, packet); } - uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string sentence) + uint8_t ProviderUnderwaterComNode::CalculateChecksum(const uint8_t *sentence, uint8_t length) { uint8_t check = 0; - for(unsigned int i = 0; i < sentence.size(); i++) - check ^= sentence[i]; + for(unsigned int i = 0; i < length; i++) + { + + } return check; } @@ -85,7 +87,7 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::AppendChecksum(std::string &sentence) { std::stringstream ss; - uint8_t checksum = CalculateChecksum(sentence); + uint8_t checksum = CalculateChecksum((uint8_t *)&sentence, sentence.size()); ss << sentence << std::string(1, CHECKSUM) << std::hex << checksum; sentence = ss.str(); } @@ -170,7 +172,7 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) { - if(Verify_Version()) + if(!Verify_Version()) { ROS_INFO_STREAM("Major version isn't of 1"); ros::shutdown(); @@ -188,6 +190,7 @@ namespace provider_underwater_com std::stringstream ss(response_str); std::getline(ss, major_version, ','); + std::getline(ss, major_version, ','); if(std::stoi(major_version) == 1 && ConfirmChecksum(response_str)) { diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 78a702b..a5e80ca 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -54,7 +54,7 @@ class ProviderUnderwaterComNode void UnderwaterComCallback(const std_msgs::UInt8 &msg); - uint8_t CalculateChecksum(const std::string sentence); + uint8_t CalculateChecksum(const uint8_t *sentence, uint8_t length); void AppendChecksum(std::string &sentence); bool ConfirmChecksum(const std::string &sentence); From b037d0e88fa1fef03cd5ac6a0b97ac62d466853f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 20:05:42 +0000 Subject: [PATCH 030/140] change the checksum --- src/provider_underwater_com_node.cc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index a5e138e..9a86fcb 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -76,9 +76,11 @@ namespace provider_underwater_com { uint8_t check = 0; - for(unsigned int i = 0; i < length; i++) + while(length > 0) { - + check = lookup_table[*sentence ^ check]; + sentence++; + length--; } return check; @@ -97,7 +99,7 @@ namespace provider_underwater_com try { std::string checksumData = sentence.substr(0, sentence.find("*", 0)); - uint8_t calculatedChecksum = CalculateChecksum(checksumData); + uint8_t calculatedChecksum = CalculateChecksum((uint8_t *)&checksumData, checksumData.size()); uint8_t originalChecksum = std::stoi(sentence.substr(sentence.find("*", 0)+1, 2), nullptr, 16); return originalChecksum == calculatedChecksum; } From f64e39f9946c803d2c5f7623e1e66146964d17dc Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 8 Sep 2021 20:50:40 +0000 Subject: [PATCH 031/140] test crc8 failed --- src/ModemM64_definitions.h | 56 ++++++++++++----------------- src/provider_underwater_com_node.cc | 15 ++++---- src/provider_underwater_com_node.h | 2 +- 3 files changed, 31 insertions(+), 42 deletions(-) diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h index 7f7f6ce..c746da4 100644 --- a/src/ModemM64_definitions.h +++ b/src/ModemM64_definitions.h @@ -45,39 +45,29 @@ #define ROLE_MASTER 'a' #define ROLE_SLAVE 'b' -static const uint8_t lookup_table[256] = { - 0x00U,0x07U,0x0EU,0x09U,0x1CU,0x1BU,0x12U,0x15U, - 0x38U,0x3FU,0x36U,0x31U,0x24U,0x23U,0x2AU,0x2DU, - 0x70U,0x77U,0x7EU,0x79U,0x6CU,0x6BU,0x62U,0x65U, - 0x48U,0x4FU,0x46U,0x41U,0x54U,0x53U,0x5AU,0x5DU, - 0xE0U,0xE7U,0xEEU,0xE9U,0xFCU,0xFBU,0xF2U,0xF5U, - 0xD8U,0xDFU,0xD6U,0xD1U,0xC4U,0xC3U,0xCAU,0xCDU, - 0x90U,0x97U,0x9EU,0x99U,0x8CU,0x8BU,0x82U,0x85U, - 0xA8U,0xAFU,0xA6U,0xA1U,0xB4U,0xB3U,0xBAU,0xBDU, - 0xC7U,0xC0U,0xC9U,0xCEU,0xDBU,0xDCU,0xD5U,0xD2U, - 0xFFU,0xF8U,0xF1U,0xF6U,0xE3U,0xE4U,0xEDU,0xEAU, - 0xB7U,0xB0U,0xB9U,0xBEU,0xABU,0xACU,0xA5U,0xA2U, - 0x8FU,0x88U,0x81U,0x86U,0x93U,0x94U,0x9DU,0x9AU, - 0x27U,0x20U,0x29U,0x2EU,0x3BU,0x3CU,0x35U,0x32U, - 0x1FU,0x18U,0x11U,0x16U,0x03U,0x04U,0x0DU,0x0AU, - 0x57U,0x50U,0x59U,0x5EU,0x4BU,0x4CU,0x45U,0x42U, - 0x6FU,0x68U,0x61U,0x66U,0x73U,0x74U,0x7DU,0x7AU, - 0x89U,0x8EU,0x87U,0x80U,0x95U,0x92U,0x9BU,0x9CU, - 0xB1U,0xB6U,0xBFU,0xB8U,0xADU,0xAAU,0xA3U,0xA4U, - 0xF9U,0xFEU,0xF7U,0xF0U,0xE5U,0xE2U,0xEBU,0xECU, - 0xC1U,0xC6U,0xCFU,0xC8U,0xDDU,0xDAU,0xD3U,0xD4U, - 0x69U,0x6EU,0x67U,0x60U,0x75U,0x72U,0x7BU,0x7CU, - 0x51U,0x56U,0x5FU,0x58U,0x4DU,0x4AU,0x43U,0x44U, - 0x19U,0x1EU,0x17U,0x10U,0x05U,0x02U,0x0BU,0x0CU, - 0x21U,0x26U,0x2FU,0x28U,0x3DU,0x3AU,0x33U,0x34U, - 0x4EU,0x49U,0x40U,0x47U,0x52U,0x55U,0x5CU,0x5BU, - 0x76U,0x71U,0x78U,0x7FU,0x6AU,0x6DU,0x64U,0x63U, - 0x3EU,0x39U,0x30U,0x37U,0x22U,0x25U,0x2CU,0x2BU, - 0x06U,0x01U,0x08U,0x0FU,0x1AU,0x1DU,0x14U,0x13U, - 0xAEU,0xA9U,0xA0U,0xA7U,0xB2U,0xB5U,0xBCU,0xBBU, - 0x96U,0x91U,0x98U,0x9FU,0x8AU,0x8DU,0x84U,0x83U, - 0xDEU,0xD9U,0xD0U,0xD7U,0xC2U,0xC5U,0xCCU,0xCBU, - 0xE6U,0xE1U,0xE8U,0xEFU,0xFAU,0xFDU,0xF4U,0xF3U, +static const uint8_t crc_table[] = { + 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, + 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, + 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, + 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, + 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, + 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, + 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, + 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, + 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, + 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, + 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, + 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, + 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, + 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, + 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, + 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, + 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, + 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, + 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, + 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, + 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, + 0xfa, 0xfd, 0xf4, 0xf3 }; #endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 9a86fcb..d2bae5d 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -72,18 +72,18 @@ namespace provider_underwater_com Queue_Packet(dir, cmd, packet); } - uint8_t ProviderUnderwaterComNode::CalculateChecksum(const uint8_t *sentence, uint8_t length) + uint8_t ProviderUnderwaterComNode::CalculateChecksum(uint8_t *sentence, uint8_t length) { - uint8_t check = 0; + uint16_t check = 0; + uint16_t i ; - while(length > 0) + while(length--) { - check = lookup_table[*sentence ^ check]; - sentence++; - length--; + i = (check ^ *sentence++) & 0xFF; + check = (crc_table[i] ^ (check << 8)) & 0xFF; } - return check; + return check & 0xFF; } void ProviderUnderwaterComNode::AppendChecksum(std::string &sentence) @@ -177,7 +177,6 @@ namespace provider_underwater_com if(!Verify_Version()) { ROS_INFO_STREAM("Major version isn't of 1"); - ros::shutdown(); } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index a5e80ca..587f2de 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -54,7 +54,7 @@ class ProviderUnderwaterComNode void UnderwaterComCallback(const std_msgs::UInt8 &msg); - uint8_t CalculateChecksum(const uint8_t *sentence, uint8_t length); + uint8_t CalculateChecksum(uint8_t *sentence, uint8_t length); void AppendChecksum(std::string &sentence); bool ConfirmChecksum(const std::string &sentence); From d4f5c8c453e1596b727ee14e6a21ae3f94e7b056 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 9 Sep 2021 23:54:12 +0000 Subject: [PATCH 032/140] got checksum to work --- src/ModemM64_definitions.h | 54 +++++++++++++++++------------ src/provider_underwater_com_node.cc | 20 +++++------ src/provider_underwater_com_node.h | 2 +- 3 files changed, 42 insertions(+), 34 deletions(-) diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h index c746da4..f85bcdd 100644 --- a/src/ModemM64_definitions.h +++ b/src/ModemM64_definitions.h @@ -46,28 +46,38 @@ #define ROLE_SLAVE 'b' static const uint8_t crc_table[] = { - 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, - 0x24, 0x23, 0x2a, 0x2d, 0x70, 0x77, 0x7e, 0x79, 0x6c, 0x6b, 0x62, 0x65, - 0x48, 0x4f, 0x46, 0x41, 0x54, 0x53, 0x5a, 0x5d, 0xe0, 0xe7, 0xee, 0xe9, - 0xfc, 0xfb, 0xf2, 0xf5, 0xd8, 0xdf, 0xd6, 0xd1, 0xc4, 0xc3, 0xca, 0xcd, - 0x90, 0x97, 0x9e, 0x99, 0x8c, 0x8b, 0x82, 0x85, 0xa8, 0xaf, 0xa6, 0xa1, - 0xb4, 0xb3, 0xba, 0xbd, 0xc7, 0xc0, 0xc9, 0xce, 0xdb, 0xdc, 0xd5, 0xd2, - 0xff, 0xf8, 0xf1, 0xf6, 0xe3, 0xe4, 0xed, 0xea, 0xb7, 0xb0, 0xb9, 0xbe, - 0xab, 0xac, 0xa5, 0xa2, 0x8f, 0x88, 0x81, 0x86, 0x93, 0x94, 0x9d, 0x9a, - 0x27, 0x20, 0x29, 0x2e, 0x3b, 0x3c, 0x35, 0x32, 0x1f, 0x18, 0x11, 0x16, - 0x03, 0x04, 0x0d, 0x0a, 0x57, 0x50, 0x59, 0x5e, 0x4b, 0x4c, 0x45, 0x42, - 0x6f, 0x68, 0x61, 0x66, 0x73, 0x74, 0x7d, 0x7a, 0x89, 0x8e, 0x87, 0x80, - 0x95, 0x92, 0x9b, 0x9c, 0xb1, 0xb6, 0xbf, 0xb8, 0xad, 0xaa, 0xa3, 0xa4, - 0xf9, 0xfe, 0xf7, 0xf0, 0xe5, 0xe2, 0xeb, 0xec, 0xc1, 0xc6, 0xcf, 0xc8, - 0xdd, 0xda, 0xd3, 0xd4, 0x69, 0x6e, 0x67, 0x60, 0x75, 0x72, 0x7b, 0x7c, - 0x51, 0x56, 0x5f, 0x58, 0x4d, 0x4a, 0x43, 0x44, 0x19, 0x1e, 0x17, 0x10, - 0x05, 0x02, 0x0b, 0x0c, 0x21, 0x26, 0x2f, 0x28, 0x3d, 0x3a, 0x33, 0x34, - 0x4e, 0x49, 0x40, 0x47, 0x52, 0x55, 0x5c, 0x5b, 0x76, 0x71, 0x78, 0x7f, - 0x6a, 0x6d, 0x64, 0x63, 0x3e, 0x39, 0x30, 0x37, 0x22, 0x25, 0x2c, 0x2b, - 0x06, 0x01, 0x08, 0x0f, 0x1a, 0x1d, 0x14, 0x13, 0xae, 0xa9, 0xa0, 0xa7, - 0xb2, 0xb5, 0xbc, 0xbb, 0x96, 0x91, 0x98, 0x9f, 0x8a, 0x8d, 0x84, 0x83, - 0xde, 0xd9, 0xd0, 0xd7, 0xc2, 0xc5, 0xcc, 0xcb, 0xe6, 0xe1, 0xe8, 0xef, - 0xfa, 0xfd, 0xf4, 0xf3 + 0x00U,0x07U,0x0EU,0x09U,0x1CU,0x1BU,0x12U,0x15U, + 0x38U,0x3FU,0x36U,0x31U,0x24U,0x23U,0x2AU,0x2DU, + 0x70U,0x77U,0x7EU,0x79U,0x6CU,0x6BU,0x62U,0x65U, + 0x48U,0x4FU,0x46U,0x41U,0x54U,0x53U,0x5AU,0x5DU, + 0xE0U,0xE7U,0xEEU,0xE9U,0xFCU,0xFBU,0xF2U,0xF5U, + 0xD8U,0xDFU,0xD6U,0xD1U,0xC4U,0xC3U,0xCAU,0xCDU, + 0x90U,0x97U,0x9EU,0x99U,0x8CU,0x8BU,0x82U,0x85U, + 0xA8U,0xAFU,0xA6U,0xA1U,0xB4U,0xB3U,0xBAU,0xBDU, + 0xC7U,0xC0U,0xC9U,0xCEU,0xDBU,0xDCU,0xD5U,0xD2U, + 0xFFU,0xF8U,0xF1U,0xF6U,0xE3U,0xE4U,0xEDU,0xEAU, + 0xB7U,0xB0U,0xB9U,0xBEU,0xABU,0xACU,0xA5U,0xA2U, + 0x8FU,0x88U,0x81U,0x86U,0x93U,0x94U,0x9DU,0x9AU, + 0x27U,0x20U,0x29U,0x2EU,0x3BU,0x3CU,0x35U,0x32U, + 0x1FU,0x18U,0x11U,0x16U,0x03U,0x04U,0x0DU,0x0AU, + 0x57U,0x50U,0x59U,0x5EU,0x4BU,0x4CU,0x45U,0x42U, + 0x6FU,0x68U,0x61U,0x66U,0x73U,0x74U,0x7DU,0x7AU, + 0x89U,0x8EU,0x87U,0x80U,0x95U,0x92U,0x9BU,0x9CU, + 0xB1U,0xB6U,0xBFU,0xB8U,0xADU,0xAAU,0xA3U,0xA4U, + 0xF9U,0xFEU,0xF7U,0xF0U,0xE5U,0xE2U,0xEBU,0xECU, + 0xC1U,0xC6U,0xCFU,0xC8U,0xDDU,0xDAU,0xD3U,0xD4U, + 0x69U,0x6EU,0x67U,0x60U,0x75U,0x72U,0x7BU,0x7CU, + 0x51U,0x56U,0x5FU,0x58U,0x4DU,0x4AU,0x43U,0x44U, + 0x19U,0x1EU,0x17U,0x10U,0x05U,0x02U,0x0BU,0x0CU, + 0x21U,0x26U,0x2FU,0x28U,0x3DU,0x3AU,0x33U,0x34U, + 0x4EU,0x49U,0x40U,0x47U,0x52U,0x55U,0x5CU,0x5BU, + 0x76U,0x71U,0x78U,0x7FU,0x6AU,0x6DU,0x64U,0x63U, + 0x3EU,0x39U,0x30U,0x37U,0x22U,0x25U,0x2CU,0x2BU, + 0x06U,0x01U,0x08U,0x0FU,0x1AU,0x1DU,0x14U,0x13U, + 0xAEU,0xA9U,0xA0U,0xA7U,0xB2U,0xB5U,0xBCU,0xBBU, + 0x96U,0x91U,0x98U,0x9FU,0x8AU,0x8DU,0x84U,0x83U, + 0xDEU,0xD9U,0xD0U,0xD7U,0xC2U,0xC5U,0xCCU,0xCBU, + 0xE6U,0xE1U,0xE8U,0xEFU,0xFAU,0xFDU,0xF4U,0xF3U, }; #endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index d2bae5d..180d395 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -72,34 +72,32 @@ namespace provider_underwater_com Queue_Packet(dir, cmd, packet); } - uint8_t ProviderUnderwaterComNode::CalculateChecksum(uint8_t *sentence, uint8_t length) + uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string &sentence, uint8_t length) { - uint16_t check = 0; - uint16_t i ; + uint8_t check = 0; - while(length--) + for(uint8_t i = 0; i < length; ++i) { - i = (check ^ *sentence++) & 0xFF; - check = (crc_table[i] ^ (check << 8)) & 0xFF; + check = crc_table[(uint8_t)sentence.at(i) ^ check]; } - return check & 0xFF; + return check; } void ProviderUnderwaterComNode::AppendChecksum(std::string &sentence) { std::stringstream ss; - uint8_t checksum = CalculateChecksum((uint8_t *)&sentence, sentence.size()); + uint8_t checksum = CalculateChecksum(sentence, sentence.size()); ss << sentence << std::string(1, CHECKSUM) << std::hex << checksum; sentence = ss.str(); } bool ProviderUnderwaterComNode::ConfirmChecksum(const std::string &sentence) - { + { try { std::string checksumData = sentence.substr(0, sentence.find("*", 0)); - uint8_t calculatedChecksum = CalculateChecksum((uint8_t *)&checksumData, checksumData.size()); + uint8_t calculatedChecksum = CalculateChecksum(checksumData, checksumData.size()); uint8_t originalChecksum = std::stoi(sentence.substr(sentence.find("*", 0)+1, 2), nullptr, 16); return originalChecksum == calculatedChecksum; } @@ -176,7 +174,7 @@ namespace provider_underwater_com if(!Verify_Version()) { - ROS_INFO_STREAM("Major version isn't of 1"); + ROS_WARN_STREAM("Major version isn't of 1"); } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 587f2de..58dcd6e 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -54,7 +54,7 @@ class ProviderUnderwaterComNode void UnderwaterComCallback(const std_msgs::UInt8 &msg); - uint8_t CalculateChecksum(uint8_t *sentence, uint8_t length); + uint8_t CalculateChecksum(const std::string &sentence, uint8_t length); void AppendChecksum(std::string &sentence); bool ConfirmChecksum(const std::string &sentence); From 8a4d625f97d035942f99a934bcc9212cc513fa99 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 10 Sep 2021 02:06:08 +0000 Subject: [PATCH 033/140] add features for config of the sensor --- src/ModemM64_definitions.h | 4 + src/provider_underwater_com_node.cc | 128 ++++++++++++++++++++++------ src/provider_underwater_com_node.h | 20 +++-- 3 files changed, 120 insertions(+), 32 deletions(-) diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h index f85bcdd..2873b5a 100644 --- a/src/ModemM64_definitions.h +++ b/src/ModemM64_definitions.h @@ -41,6 +41,10 @@ #define CMD_QUEUE_PACKET 'q' #define CMD_FLUSH 'f' #define RESP_GOT_PACKET 'p' +#define RETURN_ERROR '?' + +#define ACK 'a' +#define NAK 'n' #define ROLE_MASTER 'a' #define ROLE_SLAVE 'b' diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 180d395..028b8e0 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -37,9 +37,10 @@ namespace provider_underwater_com serialConnection_.flush(); underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); - underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); + underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); reader_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); + export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); Set_Sensor(ROLE_MASTER, 4); } @@ -49,6 +50,7 @@ namespace provider_underwater_com { underwaterComSubscriber_.shutdown(); reader_thread.~thread(); + export_to_ros_thread.~thread(); } //Node Spin @@ -63,13 +65,13 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt8 &msg) + void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::String &msg) { - std::string packet = ",8," + std::to_string(msg.data); // Payload for the CMD to send, always 8 + std::string packet = "," + std::to_string(payload_) + "," + msg.data; // TODO add a size check before transmit std::string dir = std::string(1, DIR_CMD); std::string cmd = std::string(1, CMD_QUEUE_PACKET); - Queue_Packet(dir, cmd, packet); + Queue_Packet(cmd, packet); } uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string &sentence, uint8_t length) @@ -87,8 +89,12 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::AppendChecksum(std::string &sentence) { std::stringstream ss; + char buffer[2]; + uint8_t checksum = CalculateChecksum(sentence, sentence.size()); - ss << sentence << std::string(1, CHECKSUM) << std::hex << checksum; + sprintf(buffer, "%x", checksum); + + ss << sentence << std::string(1, CHECKSUM) << buffer << std::string(1, EOP); sentence = ss.str(); } @@ -108,21 +114,20 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet) + void ProviderUnderwaterComNode::Queue_Packet(const std::string &cmd, const std::string &packet) { std::stringstream ss; std::string sentence; - if(cmd != std::string(1, CMD_QUEUE_PACKET) && cmd != std::string(1, CMD_SET_SETTINGS)) - { - ss << SOP << direction << cmd << EOP; - } - else + ss << SOP << DIR_CMD << cmd; + + if(cmd == std::string(1, CMD_QUEUE_PACKET) || cmd == std::string(1, CMD_SET_SETTINGS)) { - ss << SOP << direction << cmd << std::string(",") << packet << EOP; + ss << packet; } sentence = ss.str(); + AppendChecksum(sentence); serialConnection_.transmit(sentence); ROS_DEBUG("Packet sent to Modem"); @@ -130,7 +135,7 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Read_Packet() { - ROS_INFO_STREAM("Reader thread started"); + ROS_INFO_STREAM("Read thread started"); char buffer[BUFFER_SIZE]; while(!ros::isShuttingDown()) @@ -155,12 +160,17 @@ namespace provider_underwater_com buffer[i] = 0; - if(buffer[1] != DIR_RESP) + if(buffer[1] != DIR_RESP || buffer[2] == RETURN_ERROR) { ROS_INFO_STREAM("Error on the response"); } - - if(buffer[2] != CMD_FLUSH) + else if(buffer[2] == RESP_GOT_PACKET) + { + std::unique_lock mlock(export_to_ros_mutex); + export_to_ros_str = std::string(buffer); + export_to_ros_cond.notify_one(); + } + else if(buffer[2] != CMD_FLUSH && ConfirmChecksum(buffer)) { std::unique_lock mlock(response_mutex); response_str = std::string(buffer); @@ -169,20 +179,40 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) + void ProviderUnderwaterComNode::Export_To_ROS() { - - if(!Verify_Version()) + std::string size; + std::string msg; + + while(!ros::isShuttingDown()) { - ROS_WARN_STREAM("Major version isn't of 1"); - } + msg_received.data.clear(); + std::unique_lock mlock(export_to_ros_mutex); + export_to_ros_cond.wait(mlock); + + std::stringstream ss(export_to_ros_str); + std::getline(ss, size, ','); // TODO add a size check before publish + std::getline(ss, size, ','); + std::getline(ss, msg, '*'); + + msg_received.data = msg; + + underwaterComPublisher_.publish(msg_received); + } + } + + void ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) + { + Verify_Version(); + Get_Payload_Load(); + Set_Configuration(role, channel); } - bool ProviderUnderwaterComNode::Verify_Version() + void ProviderUnderwaterComNode::Verify_Version() { std::string major_version = ""; - Queue_Packet(std::string(1, DIR_CMD),std::string(1, CMD_GET_VERSION)); + Queue_Packet(std::string(1, CMD_GET_VERSION)); std::unique_lock mlock(response_mutex); response_cond.wait(mlock); @@ -191,14 +221,60 @@ namespace provider_underwater_com std::getline(ss, major_version, ','); std::getline(ss, major_version, ','); - if(std::stoi(major_version) == 1 && ConfirmChecksum(response_str)) + if(std::stoi(major_version) == 1) { - return true; + ROS_DEBUG("Major version is 1"); } else { - return false; + ROS_INFO_STREAM("Major version isn't 1"); + init_error_ = true; } } + + void ProviderUnderwaterComNode::Get_Payload_Load() + { + std::string payload = ""; + + Queue_Packet(std::string(1, CMD_GET_PAYLOAD_SIZE)); + + std::unique_lock mlock(response_mutex); + response_cond.wait(mlock); + + std::stringstream ss(response_str); + std::getline(ss, payload, ','); + std::getline(ss, payload, '*'); + + payload_ = std::stoi(payload); + + ROS_DEBUG("Payload set"); + } + + void ProviderUnderwaterComNode::Set_Configuration(const char &role, uint8_t channel) + { + std::string acknowledge; + char buffer[2]; + + sprintf(buffer, "%d", channel); + std::string packet = "," + std::string(1, role) + "," + buffer; + + Queue_Packet(std::string(1, CMD_SET_SETTINGS), packet); + + std::unique_lock mlock(response_mutex); + response_cond.wait(mlock); + + std::stringstream ss(response_str); + std::getline(ss, acknowledge, ','); + std::getline(ss, acknowledge, '*'); + + if(acknowledge == std::string(1, NAK)) + { + ROS_WARN_STREAM("Could not set the configuration"); + } + else + { + ROS_DEBUG("Configuration set"); + } + } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 58dcd6e..62f8dcd 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -28,7 +28,7 @@ #include #include -#include +#include #include #include #include @@ -52,17 +52,19 @@ class ProviderUnderwaterComNode private: - void UnderwaterComCallback(const std_msgs::UInt8 &msg); + void UnderwaterComCallback(const std_msgs::String &msg); uint8_t CalculateChecksum(const std::string &sentence, uint8_t length); void AppendChecksum(std::string &sentence); bool ConfirmChecksum(const std::string &sentence); - void Queue_Packet(const std::string &direction, const std::string &cmd, const std::string &packet = ""); + void Queue_Packet(const std::string &cmd, const std::string &packet = ""); void Read_Packet(); + void Export_To_ROS(); void Set_Sensor(const char &role = ROLE_MASTER, uint8_t channel = 4); - bool Verify_Version(); - + void Verify_Version(); + void Get_Payload_Load(); + void Set_Configuration(const char &role, uint8_t channel); ros::NodeHandlePtr nh_; Configuration configuration_; @@ -76,8 +78,14 @@ class ProviderUnderwaterComNode std::condition_variable response_cond; std::string response_str = ""; + std::thread export_to_ros_thread; + std::mutex export_to_ros_mutex; + std::condition_variable export_to_ros_cond; + std::string export_to_ros_str = ""; + uint8_t payload_; - + bool init_error_ = false; + std_msgs::String msg_received; }; } From b14d82808fdd6b65bf53c7d29fd443231a4b3039 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 15:00:41 +0000 Subject: [PATCH 034/140] add check for cmd and flush CMD --- src/ModemM64_definitions.h | 13 ++++++ src/provider_underwater_com_node.cc | 64 +++++++++++++++++++++++++---- src/provider_underwater_com_node.h | 2 + 3 files changed, 70 insertions(+), 9 deletions(-) diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h index 2873b5a..29e23e7 100644 --- a/src/ModemM64_definitions.h +++ b/src/ModemM64_definitions.h @@ -49,6 +49,19 @@ #define ROLE_MASTER 'a' #define ROLE_SLAVE 'b' +static const char all_valid [] = { + CMD_GET_VERSION, + CMD_GET_PAYLOAD_SIZE, + CMD_GET_BUFFER_LENGTH, + CMD_GET_DIAGNOSTIC, + CMD_GET_SETTINGS, + CMD_SET_SETTINGS, + CMD_QUEUE_PACKET, + CMD_FLUSH +}; + +static const uint8_t all_valid_size = 8; + static const uint8_t crc_table[] = { 0x00U,0x07U,0x0EU,0x09U,0x1CU,0x1BU,0x12U,0x15U, 0x38U,0x3FU,0x36U,0x31U,0x24U,0x23U,0x2AU,0x2DU, diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 028b8e0..b3677de 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -119,18 +119,39 @@ namespace provider_underwater_com std::stringstream ss; std::string sentence; - ss << SOP << DIR_CMD << cmd; + if(Check_CMD(cmd)) + { + ss << SOP << DIR_CMD << cmd; + + if(cmd == std::string(1, CMD_QUEUE_PACKET) || cmd == std::string(1, CMD_SET_SETTINGS)) + { + ss << packet; + } + + sentence = ss.str(); + AppendChecksum(sentence); + serialConnection_.transmit(sentence); - if(cmd == std::string(1, CMD_QUEUE_PACKET) || cmd == std::string(1, CMD_SET_SETTINGS)) + ROS_DEBUG("Packet sent to Modem"); + } + else { - ss << packet; + ROS_INFO_STREAM("CMD unknow. Can't queue packet"); } + } - sentence = ss.str(); - AppendChecksum(sentence); - serialConnection_.transmit(sentence); + bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) + { + const char *c_cmd = cmd.data(); - ROS_DEBUG("Packet sent to Modem"); + for(uint8_t i = 0; i < all_valid_size; ++i) + { + if(c_cmd[0] == all_valid[i]) + { + return true; + } + } + return false; } void ProviderUnderwaterComNode::Read_Packet() @@ -164,13 +185,14 @@ namespace provider_underwater_com { ROS_INFO_STREAM("Error on the response"); } - else if(buffer[2] == RESP_GOT_PACKET) + else if((buffer[2] == RESP_GOT_PACKET || buffer[2] == CMD_GET_SETTINGS || buffer[2] == CMD_GET_BUFFER_LENGTH + || buffer[2] == CMD_GET_DIAGNOSTIC) && ConfirmChecksum(buffer)) { std::unique_lock mlock(export_to_ros_mutex); export_to_ros_str = std::string(buffer); export_to_ros_cond.notify_one(); } - else if(buffer[2] != CMD_FLUSH && ConfirmChecksum(buffer)) + else if(ConfirmChecksum(buffer)) { std::unique_lock mlock(response_mutex); response_str = std::string(buffer); @@ -206,6 +228,7 @@ namespace provider_underwater_com Verify_Version(); Get_Payload_Load(); Set_Configuration(role, channel); + Flush_Queue(); } void ProviderUnderwaterComNode::Verify_Version() @@ -277,4 +300,27 @@ namespace provider_underwater_com ROS_DEBUG("Configuration set"); } } + + void ProviderUnderwaterComNode::Flush_Queue() + { + std::string acknowledge; + + Queue_Packet(std::string(1, CMD_FLUSH)); + + std::unique_lock mlock(response_mutex); + response_cond.wait(mlock); + + std::stringstream ss(response_str); + std::getline(ss, acknowledge, ','); + std::getline(ss, acknowledge, '*'); + + if(acknowledge == std::string(1, NAK)) + { + ROS_WARN_STREAM("Could not flush the queue"); + } + else + { + ROS_DEBUG("Queue flushed"); + } + } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 62f8dcd..7f6d1c5 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -59,12 +59,14 @@ class ProviderUnderwaterComNode bool ConfirmChecksum(const std::string &sentence); void Queue_Packet(const std::string &cmd, const std::string &packet = ""); + bool Check_CMD(const std::string &cmd); void Read_Packet(); void Export_To_ROS(); void Set_Sensor(const char &role = ROLE_MASTER, uint8_t channel = 4); void Verify_Version(); void Get_Payload_Load(); void Set_Configuration(const char &role, uint8_t channel); + void Flush_Queue(); ros::NodeHandlePtr nh_; Configuration configuration_; From 6fa57c991791858981e8925b7b98f23097ca7fd7 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 15:36:47 +0000 Subject: [PATCH 035/140] change lauch for 2 subs --- .vscode/launch.json | 2 +- launch/provider_underwater_com.launch | 3 --- launch/provider_underwater_com_auv8.launch | 15 +++++++++++++++ 3 files changed, 16 insertions(+), 4 deletions(-) delete mode 100644 launch/provider_underwater_com.launch create mode 100644 launch/provider_underwater_com_auv8.launch diff --git a/.vscode/launch.json b/.vscode/launch.json index 74e813b..31deb9b 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/provider_underwater_com/launch/provider_underwater_com.launch", + "target": "/home/sonia/ros_sonia_ws/src/provider_underwater_com/launch/provider_underwater_com_auv8.launch", } ] } \ No newline at end of file diff --git a/launch/provider_underwater_com.launch b/launch/provider_underwater_com.launch deleted file mode 100644 index b8efaba..0000000 --- a/launch/provider_underwater_com.launch +++ /dev/null @@ -1,3 +0,0 @@ - - - \ No newline at end of file diff --git a/launch/provider_underwater_com_auv8.launch b/launch/provider_underwater_com_auv8.launch new file mode 100644 index 0000000..88d4156 --- /dev/null +++ b/launch/provider_underwater_com_auv8.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file From f245d9b68a073fe7608edd5cff044cf9efc07d68 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 15:55:49 +0000 Subject: [PATCH 036/140] add config files --- config/auv7.yaml | 7 +++++++ config/auv8.yaml | 7 +++++++ launch/provider_underwater_com_auv7.launch | 8 ++++++++ launch/provider_underwater_com_auv8.launch | 11 ++--------- src/provider_underwater_com_node.h | 3 +++ 5 files changed, 27 insertions(+), 9 deletions(-) create mode 100644 config/auv7.yaml create mode 100644 config/auv8.yaml create mode 100644 launch/provider_underwater_com_auv7.launch diff --git a/config/auv7.yaml b/config/auv7.yaml new file mode 100644 index 0000000..2256e2e --- /dev/null +++ b/config/auv7.yaml @@ -0,0 +1,7 @@ +#********** Driver Parameters ************ + +/provider_underwater_com/connection/tty: "/dev/ttyUSB0" + +/provider_underwater_com/settings/role : "salve" + +/provder_underwater_com/settings/channel : "4" \ No newline at end of file diff --git a/config/auv8.yaml b/config/auv8.yaml new file mode 100644 index 0000000..dbd2732 --- /dev/null +++ b/config/auv8.yaml @@ -0,0 +1,7 @@ +#********** Driver Parameters ************ + +/provider_underwater_com/connection/tty: "/dev/ttyUSB0" + +/provider_underwater_com/settings/role : "master" + +/provder_underwater_com/settings/channel : "4" \ No newline at end of file diff --git a/launch/provider_underwater_com_auv7.launch b/launch/provider_underwater_com_auv7.launch new file mode 100644 index 0000000..dbee510 --- /dev/null +++ b/launch/provider_underwater_com_auv7.launch @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/launch/provider_underwater_com_auv8.launch b/launch/provider_underwater_com_auv8.launch index 88d4156..91f8d10 100644 --- a/launch/provider_underwater_com_auv8.launch +++ b/launch/provider_underwater_com_auv8.launch @@ -1,15 +1,8 @@ - - - - - - - - - + + \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 7f6d1c5..618786b 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -85,6 +85,9 @@ class ProviderUnderwaterComNode std::condition_variable export_to_ros_cond; std::string export_to_ros_str = ""; + char* role; + uint8_t channel_; + uint8_t payload_; bool init_error_ = false; std_msgs::String msg_received; From a6113d175543a4e90ce527586f033f2cfb8b00cc Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 16:04:16 +0000 Subject: [PATCH 037/140] add different launch file for auvs --- config/auv7.yaml | 6 +++--- config/auv8.yaml | 4 ++-- launch/provider_underwater_com_auv8.launch | 5 ++--- src/Configuration.cc | 7 +++++-- src/Configuration.h | 4 ++++ 5 files changed, 16 insertions(+), 10 deletions(-) diff --git a/config/auv7.yaml b/config/auv7.yaml index 2256e2e..5a5c9f0 100644 --- a/config/auv7.yaml +++ b/config/auv7.yaml @@ -1,7 +1,7 @@ #********** Driver Parameters ************ -/provider_underwater_com/connection/tty: "/dev/ttyUSB0" +/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" -/provider_underwater_com/settings/role : "salve" +/provider_underwater_com/settings/role : "slave" -/provder_underwater_com/settings/channel : "4" \ No newline at end of file +/provider_underwater_com/settings/channel : "4" \ No newline at end of file diff --git a/config/auv8.yaml b/config/auv8.yaml index dbd2732..c6795a7 100644 --- a/config/auv8.yaml +++ b/config/auv8.yaml @@ -1,7 +1,7 @@ #********** Driver Parameters ************ -/provider_underwater_com/connection/tty: "/dev/ttyUSB0" +/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" /provider_underwater_com/settings/role : "master" -/provder_underwater_com/settings/channel : "4" \ No newline at end of file +/provider_underwater_com/settings/channel : "4" \ No newline at end of file diff --git a/launch/provider_underwater_com_auv8.launch b/launch/provider_underwater_com_auv8.launch index 91f8d10..eae5884 100644 --- a/launch/provider_underwater_com_auv8.launch +++ b/launch/provider_underwater_com_auv8.launch @@ -1,8 +1,7 @@ - + - + - \ No newline at end of file diff --git a/src/Configuration.cc b/src/Configuration.cc index df83e68..f8c9943 100644 --- a/src/Configuration.cc +++ b/src/Configuration.cc @@ -31,7 +31,9 @@ namespace provider_underwater_com Configuration::Configuration(const ros::NodeHandlePtr &nh) : nh(nh), ttyPort("/dev/ttyUSB0"), // "/dev/MODEM" for AUVs, debug only - settingsFile("settings.txt") + settingsFile("settings.txt"), + role("master"), + channel("4") { Deserialize(); } @@ -43,7 +45,8 @@ namespace provider_underwater_com ROS_INFO("Deserialize params"); FindParameter("/connection/tty_port", ttyPort); - FindParameter("/settings/setting_file", settingsFile); + FindParameter("/settings/role", role); + FindParameter("/settings/channel", channel); ROS_INFO("End deserialize params"); } diff --git a/src/Configuration.h b/src/Configuration.h index b184e2c..5d5d684 100644 --- a/src/Configuration.h +++ b/src/Configuration.h @@ -41,6 +41,8 @@ namespace provider_underwater_com 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: @@ -48,6 +50,8 @@ namespace provider_underwater_com std::string ttyPort; std::string settingsFile; + std::string role; + std::string channel; void Deserialize(); void SetParameter(); From c4d14ddb776f4f17b2634a3e8347a2f776c2adf4 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 16:27:12 +0000 Subject: [PATCH 038/140] change debug msg for errors to ROS_ASSERT_MSG --- src/provider_underwater_com_node.cc | 59 ++++++++++++----------------- src/provider_underwater_com_node.h | 5 +-- 2 files changed, 27 insertions(+), 37 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index b3677de..a89c4b1 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -42,7 +42,8 @@ namespace provider_underwater_com reader_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); - Set_Sensor(ROLE_MASTER, 4); + std::string role_sensor = configuration_.getRole(); + Set_Sensor(role_sensor, std::stoi(configuration_.getChannel())); } //Node Destructor @@ -181,11 +182,11 @@ namespace provider_underwater_com buffer[i] = 0; - if(buffer[1] != DIR_RESP || buffer[2] == RETURN_ERROR) + /*if(buffer[1] != DIR_RESP || buffer[2] == RETURN_ERROR) { - ROS_INFO_STREAM("Error on the response"); - } - else if((buffer[2] == RESP_GOT_PACKET || buffer[2] == CMD_GET_SETTINGS || buffer[2] == CMD_GET_BUFFER_LENGTH + ROS_INFO_STREAM("Error on the response. Resend message or check sensor."); + }*/ + if((buffer[2] == RESP_GOT_PACKET || buffer[2] == CMD_GET_SETTINGS || buffer[2] == CMD_GET_BUFFER_LENGTH || buffer[2] == CMD_GET_DIAGNOSTIC) && ConfirmChecksum(buffer)) { std::unique_lock mlock(export_to_ros_mutex); @@ -213,7 +214,7 @@ namespace provider_underwater_com export_to_ros_cond.wait(mlock); std::stringstream ss(export_to_ros_str); - std::getline(ss, size, ','); // TODO add a size check before publish + std::getline(ss, size, ','); // TODO add a size check before publish and check if message good std::getline(ss, size, ','); std::getline(ss, msg, '*'); @@ -223,11 +224,24 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Set_Sensor(const char &role, uint8_t channel) + void ProviderUnderwaterComNode::Set_Sensor(std::string &role, uint8_t channel) { + char role_[2]; + + ROS_ASSERT_MSG(role == "master" || role == "slave", "Set the role as 'master' or 'slave'. Error in config"); + + if(role == "master") + { + role_[0] = ROLE_MASTER; + } + else + { + role_[0] = ROLE_SLAVE; + } + Verify_Version(); Get_Payload_Load(); - Set_Configuration(role, channel); + Set_Configuration(role_[0], channel); Flush_Queue(); } @@ -244,16 +258,7 @@ namespace provider_underwater_com std::getline(ss, major_version, ','); std::getline(ss, major_version, ','); - if(std::stoi(major_version) == 1) - { - ROS_DEBUG("Major version is 1"); - } - else - { - ROS_INFO_STREAM("Major version isn't 1"); - init_error_ = true; - } - + ROS_ASSERT_MSG(std::stoi(major_version) == 1, "Major Version isn't 1. Error with the sensor"); } void ProviderUnderwaterComNode::Get_Payload_Load() @@ -291,14 +296,7 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK)) - { - ROS_WARN_STREAM("Could not set the configuration"); - } - else - { - ROS_DEBUG("Configuration set"); - } + ROS_ASSERT_MSG(acknowledge == std::string(1, ACK), "Could not set the configuration. Error in settings"); } void ProviderUnderwaterComNode::Flush_Queue() @@ -314,13 +312,6 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK)) - { - ROS_WARN_STREAM("Could not flush the queue"); - } - else - { - ROS_DEBUG("Queue flushed"); - } + ROS_ASSERT_MSG(acknowledge == std::string(1, ACK), "Couldn't flush the queue. Error with the sensor"); } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 618786b..132245b 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -62,7 +62,7 @@ class ProviderUnderwaterComNode bool Check_CMD(const std::string &cmd); void Read_Packet(); void Export_To_ROS(); - void Set_Sensor(const char &role = ROLE_MASTER, uint8_t channel = 4); + void Set_Sensor(std::string &role, uint8_t channel = 4); void Verify_Version(); void Get_Payload_Load(); void Set_Configuration(const char &role, uint8_t channel); @@ -74,6 +74,7 @@ class ProviderUnderwaterComNode ros::Subscriber underwaterComSubscriber_; ros::Publisher underwaterComPublisher_; + std_msgs::String msg_received; std::thread reader_thread; std::mutex response_mutex; @@ -89,8 +90,6 @@ class ProviderUnderwaterComNode uint8_t channel_; uint8_t payload_; - bool init_error_ = false; - std_msgs::String msg_received; }; } From 166badf8027548d80cf2ebd964daf0d36ce94817 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 18:59:38 -0400 Subject: [PATCH 039/140] add definitions to sonia common --- Dockerfile | 2 +- src/provider_underwater_com_node.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Dockerfile b/Dockerfile index 37907b5..105c653 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-feature-modem_service" FROM ${BASE_IMAGE} diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 132245b..5286e5a 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -37,7 +37,7 @@ #include "Configuration.h" #include "drivers/serial.h" -#include "ModemM64_definitions.h" +#include namespace provider_underwater_com { From b91509a0804364d9a009f33976e84466e984b914 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 23:59:35 +0000 Subject: [PATCH 040/140] service starts --- package.xml | 2 +- src/provider_underwater_com_node.cc | 26 ++++++++++++++++++++++++-- src/provider_underwater_com_node.h | 6 ++++-- 3 files changed, 29 insertions(+), 5 deletions(-) diff --git a/package.xml b/package.xml index 76f2eed..7cf250d 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.0.1 + 0.0.2 The provider_underwater_com package Francis Alonzo diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index a89c4b1..c1d54d5 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -38,6 +38,7 @@ namespace provider_underwater_com underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); + underwaterComService_ = nh_->advertiseService("/provider_undewater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); reader_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); @@ -75,6 +76,28 @@ namespace provider_underwater_com Queue_Packet(cmd, packet); } + bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) + { + Queue_Packet(std::string(1, req.cmd)); + + std::unique_lock mlock(response_mutex); + response_cond.wait(mlock); + + const char *buffer = response_str.c_str(); + + switch (buffer[3]) + { + case CMD_GET_BUFFER_LENGTH: + ROS_INFO_STREAM("This thing worked wow"); + break; + + default: + break; + } + + return true; + } + uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string &sentence, uint8_t length) { uint8_t check = 0; @@ -186,8 +209,7 @@ namespace provider_underwater_com { ROS_INFO_STREAM("Error on the response. Resend message or check sensor."); }*/ - if((buffer[2] == RESP_GOT_PACKET || buffer[2] == CMD_GET_SETTINGS || buffer[2] == CMD_GET_BUFFER_LENGTH - || buffer[2] == CMD_GET_DIAGNOSTIC) && ConfirmChecksum(buffer)) + if(buffer[2] == RESP_GOT_PACKET && ConfirmChecksum(buffer)) { std::unique_lock mlock(export_to_ros_mutex); export_to_ros_str = std::string(buffer); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 5286e5a..ea88add 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -38,6 +38,7 @@ #include "Configuration.h" #include "drivers/serial.h" #include +#include namespace provider_underwater_com { @@ -53,6 +54,7 @@ class ProviderUnderwaterComNode private: void UnderwaterComCallback(const std_msgs::String &msg); + bool UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res); uint8_t CalculateChecksum(const std::string &sentence, uint8_t length); void AppendChecksum(std::string &sentence); @@ -74,6 +76,7 @@ class ProviderUnderwaterComNode ros::Subscriber underwaterComSubscriber_; ros::Publisher underwaterComPublisher_; + ros::ServiceServer underwaterComService_; std_msgs::String msg_received; std::thread reader_thread; @@ -87,8 +90,7 @@ class ProviderUnderwaterComNode std::string export_to_ros_str = ""; char* role; - uint8_t channel_; - + uint8_t channel_; uint8_t payload_; }; From b9be89320b8470bbfc022750434566b4e418ad9e Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 00:20:16 +0000 Subject: [PATCH 041/140] change service --- src/provider_underwater_com_node.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index c1d54d5..eae686d 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -83,9 +83,7 @@ namespace provider_underwater_com std::unique_lock mlock(response_mutex); response_cond.wait(mlock); - const char *buffer = response_str.c_str(); - - switch (buffer[3]) + switch (response_str.at(3)) { case CMD_GET_BUFFER_LENGTH: ROS_INFO_STREAM("This thing worked wow"); From ba56b53c2b313a3509600613a33115cc12486393 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 01:13:58 +0000 Subject: [PATCH 042/140] fix the init loop --- src/provider_underwater_com_node.cc | 52 +++++++++++++++++++++++------ src/provider_underwater_com_node.h | 1 + 2 files changed, 43 insertions(+), 10 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index eae686d..a31e759 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -259,10 +259,23 @@ namespace provider_underwater_com role_[0] = ROLE_SLAVE; } - Verify_Version(); - Get_Payload_Load(); - Set_Configuration(role_[0], channel); - Flush_Queue(); + uint8_t i = 0; + + while(i < 3 && init_error_ == true) + { + init_error_ = false; + Verify_Version(); + Get_Payload_Load(); + Set_Configuration(role_[0], channel); + Flush_Queue(); + ++i; + } + + if(i == 3) + { + ROS_ERROR("Problem with the init. Node shutting down."); + ros::shutdown(); + } } void ProviderUnderwaterComNode::Verify_Version() @@ -278,7 +291,11 @@ namespace provider_underwater_com std::getline(ss, major_version, ','); std::getline(ss, major_version, ','); - ROS_ASSERT_MSG(std::stoi(major_version) == 1, "Major Version isn't 1. Error with the sensor"); + if(major_version != "1") + { + ROS_ERROR("Major Version isn't 1. Restarting init"); + init_error_ = true; + } } void ProviderUnderwaterComNode::Get_Payload_Load() @@ -294,9 +311,16 @@ namespace provider_underwater_com std::getline(ss, payload, ','); std::getline(ss, payload, '*'); - payload_ = std::stoi(payload); - - ROS_DEBUG("Payload set"); + if(payload >= "0" || payload <= "9") + { + payload_ = std::stoi(payload); + ROS_DEBUG("Payload set"); + } + else + { + ROS_ERROR("Payload isn't a integer. Restarting init"); + init_error_ = true; + } } void ProviderUnderwaterComNode::Set_Configuration(const char &role, uint8_t channel) @@ -316,7 +340,11 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - ROS_ASSERT_MSG(acknowledge == std::string(1, ACK), "Could not set the configuration. Error in settings"); + if(acknowledge == std::string(1, NAK)) + { + ROS_ERROR("Could not set the configuration. Restarting init"); + init_error_ = true; + } } void ProviderUnderwaterComNode::Flush_Queue() @@ -332,6 +360,10 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - ROS_ASSERT_MSG(acknowledge == std::string(1, ACK), "Couldn't flush the queue. Error with the sensor"); + if(acknowledge == std::string(1, NAK)) + { + ROS_ERROR("Couldn't flush the queue. Restarting init"); + init_error_ = true; + } } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index ea88add..384cb1b 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -92,6 +92,7 @@ class ProviderUnderwaterComNode char* role; uint8_t channel_; uint8_t payload_; + bool init_error_ = true; }; } From e9d7fc3bb222167b9634a047f7a5bdc1b275c8bd Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 01:31:46 +0000 Subject: [PATCH 043/140] structure with the service --- src/provider_underwater_com_node.cc | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index a31e759..e476b54 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -83,7 +83,9 @@ namespace provider_underwater_com std::unique_lock mlock(response_mutex); response_cond.wait(mlock); - switch (response_str.at(3)) + char cmd_rec = response_str.at(2); + + switch (cmd_rec) { case CMD_GET_BUFFER_LENGTH: ROS_INFO_STREAM("This thing worked wow"); @@ -92,7 +94,7 @@ namespace provider_underwater_com default: break; } - + return true; } From 66b588bd35407cd4cbc315e9326a1b8de8af6434 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 12 Sep 2021 21:41:58 -0400 Subject: [PATCH 044/140] add error message --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index e476b54..ae0812c 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -88,10 +88,10 @@ namespace provider_underwater_com switch (cmd_rec) { case CMD_GET_BUFFER_LENGTH: - ROS_INFO_STREAM("This thing worked wow"); break; default: + ROS_ERROR("CMD received isn't working with the service.") break; } From 3d1e2db51236c4c5be257f4c7d9d386be55283d2 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 08:37:44 -0400 Subject: [PATCH 045/140] bump package --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 7cf250d..0780b2a 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.0.2 + 0.2.0 The provider_underwater_com package Francis Alonzo From 3c7f483a44f43d221a17dcdd59f2342adc8fbbff Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 08:39:22 -0400 Subject: [PATCH 046/140] Fix typo --- src/provider_underwater_com_node.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index ae0812c..c76f5e5 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -38,7 +38,7 @@ namespace provider_underwater_com underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); - underwaterComService_ = nh_->advertiseService("/provider_undewater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); + underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); reader_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); @@ -91,7 +91,7 @@ namespace provider_underwater_com break; default: - ROS_ERROR("CMD received isn't working with the service.") + ROS_ERROR("CMD received isn't working with the service."); break; } From a3afba83dd426b33950f86b31ac7002275224ebb Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 13:13:24 +0000 Subject: [PATCH 047/140] add service functionnality --- src/main.cc | 2 ++ src/provider_underwater_com_node.cc | 55 +++++++++++++++++++++++++---- src/provider_underwater_com_node.h | 1 + 3 files changed, 51 insertions(+), 7 deletions(-) diff --git a/src/main.cc b/src/main.cc index 7c4c26c..a132f49 100644 --- a/src/main.cc +++ b/src/main.cc @@ -32,4 +32,6 @@ int main(int argc, char **argv) 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 index c76f5e5..9fdc545 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -84,15 +84,56 @@ namespace provider_underwater_com response_cond.wait(mlock); char cmd_rec = response_str.at(2); + std::stringstream ss(response_str); switch (cmd_rec) { - case CMD_GET_BUFFER_LENGTH: - break; - - default: - ROS_ERROR("CMD received isn't working with the service."); - break; + case CMD_GET_BUFFER_LENGTH: + { + 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: + { + std::string role; + std::string channel; + + std::getline(ss, role, ','); + std::getline(ss, role, ','); + std::getline(ss, channel, '*'); + + res.role = std::stoi(role); + res.channel = std::stoi(channel); + break; + } + case CMD_GET_DIAGNOSTIC: + { + 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 = std::stoi(link_up); + 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); + } + default: + { + ROS_ERROR("CMD received isn't working with the service."); + return false; + } } return true; @@ -156,7 +197,7 @@ namespace provider_underwater_com AppendChecksum(sentence); serialConnection_.transmit(sentence); - ROS_DEBUG("Packet sent to Modem"); + ROS_DEBUG_STREAM("Packet sent to Modem"); } else { diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 384cb1b..ab9ba48 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -38,6 +38,7 @@ #include "Configuration.h" #include "drivers/serial.h" #include +//#include "ModemM64_definitions.h" #include namespace provider_underwater_com { From de43f94d3cffab8762f07cf6846d4323b237f225 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 13:17:54 +0000 Subject: [PATCH 048/140] cleanup code --- src/provider_underwater_com_node.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 9fdc545..2c3f427 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -246,10 +246,6 @@ namespace provider_underwater_com buffer[i] = 0; - /*if(buffer[1] != DIR_RESP || buffer[2] == RETURN_ERROR) - { - ROS_INFO_STREAM("Error on the response. Resend message or check sensor."); - }*/ if(buffer[2] == RESP_GOT_PACKET && ConfirmChecksum(buffer)) { std::unique_lock mlock(export_to_ros_mutex); From 0e2062ab38e6fb85d615da88452dbf14ae5c66d3 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 13:22:09 +0000 Subject: [PATCH 049/140] code cleanup --- src/provider_underwater_com_node.cc | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 2c3f427..091a161 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -70,10 +70,8 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::String &msg) { std::string packet = "," + std::to_string(payload_) + "," + msg.data; // TODO add a size check before transmit - std::string dir = std::string(1, DIR_CMD); - std::string cmd = std::string(1, CMD_QUEUE_PACKET); - Queue_Packet(cmd, packet); + Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet); } bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) From 066feac40c54e5062bd4d6e42281167d8f8c8d4d Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 13:24:49 +0000 Subject: [PATCH 050/140] finish cleanup --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 091a161..7d74074 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -58,7 +58,7 @@ namespace provider_underwater_com //Node Spin void ProviderUnderwaterComNode::Spin() { - ros::Rate r(1); + ros::Rate r(1); // 1 Hz while(ros::ok()) { From 0f9539cb3b3f9bb0cec14b61558fd36292242a7d Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 13:25:58 +0000 Subject: [PATCH 051/140] remove definitions --- src/ModemM64_definitions.h | 100 ------------------------------------- 1 file changed, 100 deletions(-) delete mode 100644 src/ModemM64_definitions.h diff --git a/src/ModemM64_definitions.h b/src/ModemM64_definitions.h deleted file mode 100644 index 29e23e7..0000000 --- a/src/ModemM64_definitions.h +++ /dev/null @@ -1,100 +0,0 @@ -/** - * \file ModemM64_defitions.h - * \author Francis Alonzo . - */ - -#ifndef MODEMM64_DEFINITIONS -#define MODEMM64_DEFINITIONS - -#define SOP 'w' -#define EOP '\n' -#define DIR_CMD 'c' -#define DIR_RESP 'r' -#define CHECKSUM '*' - -#define CMD_GET_VERSION 'v' -#define CMD_GET_PAYLOAD_SIZE 'n' -#define CMD_GET_BUFFER_LENGTH 'l' -#define CMD_GET_DIAGNOSTIC 'd' -#define CMD_GET_SETTINGS 'c' -#define CMD_SET_SETTINGS 's' -#define CMD_QUEUE_PACKET 'q' -#define CMD_FLUSH 'f' -#define RESP_GOT_PACKET 'p' -#define RETURN_ERROR '?' - -#define ACK 'a' -#define NAK 'n' - -#define ROLE_MASTER 'a' -#define ROLE_SLAVE 'b' - -static const char all_valid [] = { - CMD_GET_VERSION, - CMD_GET_PAYLOAD_SIZE, - CMD_GET_BUFFER_LENGTH, - CMD_GET_DIAGNOSTIC, - CMD_GET_SETTINGS, - CMD_SET_SETTINGS, - CMD_QUEUE_PACKET, - CMD_FLUSH -}; - -static const uint8_t all_valid_size = 8; - -static const uint8_t crc_table[] = { - 0x00U,0x07U,0x0EU,0x09U,0x1CU,0x1BU,0x12U,0x15U, - 0x38U,0x3FU,0x36U,0x31U,0x24U,0x23U,0x2AU,0x2DU, - 0x70U,0x77U,0x7EU,0x79U,0x6CU,0x6BU,0x62U,0x65U, - 0x48U,0x4FU,0x46U,0x41U,0x54U,0x53U,0x5AU,0x5DU, - 0xE0U,0xE7U,0xEEU,0xE9U,0xFCU,0xFBU,0xF2U,0xF5U, - 0xD8U,0xDFU,0xD6U,0xD1U,0xC4U,0xC3U,0xCAU,0xCDU, - 0x90U,0x97U,0x9EU,0x99U,0x8CU,0x8BU,0x82U,0x85U, - 0xA8U,0xAFU,0xA6U,0xA1U,0xB4U,0xB3U,0xBAU,0xBDU, - 0xC7U,0xC0U,0xC9U,0xCEU,0xDBU,0xDCU,0xD5U,0xD2U, - 0xFFU,0xF8U,0xF1U,0xF6U,0xE3U,0xE4U,0xEDU,0xEAU, - 0xB7U,0xB0U,0xB9U,0xBEU,0xABU,0xACU,0xA5U,0xA2U, - 0x8FU,0x88U,0x81U,0x86U,0x93U,0x94U,0x9DU,0x9AU, - 0x27U,0x20U,0x29U,0x2EU,0x3BU,0x3CU,0x35U,0x32U, - 0x1FU,0x18U,0x11U,0x16U,0x03U,0x04U,0x0DU,0x0AU, - 0x57U,0x50U,0x59U,0x5EU,0x4BU,0x4CU,0x45U,0x42U, - 0x6FU,0x68U,0x61U,0x66U,0x73U,0x74U,0x7DU,0x7AU, - 0x89U,0x8EU,0x87U,0x80U,0x95U,0x92U,0x9BU,0x9CU, - 0xB1U,0xB6U,0xBFU,0xB8U,0xADU,0xAAU,0xA3U,0xA4U, - 0xF9U,0xFEU,0xF7U,0xF0U,0xE5U,0xE2U,0xEBU,0xECU, - 0xC1U,0xC6U,0xCFU,0xC8U,0xDDU,0xDAU,0xD3U,0xD4U, - 0x69U,0x6EU,0x67U,0x60U,0x75U,0x72U,0x7BU,0x7CU, - 0x51U,0x56U,0x5FU,0x58U,0x4DU,0x4AU,0x43U,0x44U, - 0x19U,0x1EU,0x17U,0x10U,0x05U,0x02U,0x0BU,0x0CU, - 0x21U,0x26U,0x2FU,0x28U,0x3DU,0x3AU,0x33U,0x34U, - 0x4EU,0x49U,0x40U,0x47U,0x52U,0x55U,0x5CU,0x5BU, - 0x76U,0x71U,0x78U,0x7FU,0x6AU,0x6DU,0x64U,0x63U, - 0x3EU,0x39U,0x30U,0x37U,0x22U,0x25U,0x2CU,0x2BU, - 0x06U,0x01U,0x08U,0x0FU,0x1AU,0x1DU,0x14U,0x13U, - 0xAEU,0xA9U,0xA0U,0xA7U,0xB2U,0xB5U,0xBCU,0xBBU, - 0x96U,0x91U,0x98U,0x9FU,0x8AU,0x8DU,0x84U,0x83U, - 0xDEU,0xD9U,0xD0U,0xD7U,0xC2U,0xC5U,0xCCU,0xCBU, - 0xE6U,0xE1U,0xE8U,0xEFU,0xFAU,0xFDU,0xF4U,0xF3U, -}; - -#endif // MODEMM64_DEFINITIONS \ No newline at end of file From 0a1316716798cbf42485d03d7156839d20f0d755 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 15:35:29 +0000 Subject: [PATCH 052/140] Add multiple send --- src/provider_underwater_com_node.cc | 50 +++++++++++++++++++++++++++-- src/provider_underwater_com_node.h | 4 +++ 2 files changed, 51 insertions(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 7d74074..968cb53 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -50,9 +50,9 @@ namespace provider_underwater_com //Node Destructor ProviderUnderwaterComNode::~ProviderUnderwaterComNode() { - underwaterComSubscriber_.shutdown(); reader_thread.~thread(); export_to_ros_thread.~thread(); + underwaterComSubscriber_.shutdown(); } //Node Spin @@ -69,9 +69,24 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::String &msg) { - std::string packet = "," + std::to_string(payload_) + "," + msg.data; // TODO add a size check before transmit + if(Verify_Packet_Size(msg.data) <= 8) + { + std::string packet = "," + std::to_string(payload_) + "," + msg.data; // TODO add a size check before transmit + + Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet); + } + else + { + std::string packet_array[BUFFER_SIZE/8]; + uint8_t nb_packet; - Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet); + Split_Packet(packet_array, BUFFER_SIZE/8, &nb_packet, msg.data); + + for(uint8_t i = 0; i < nb_packet; ++i) + { + Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet_array[i]); + } + } } bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) @@ -203,6 +218,35 @@ namespace provider_underwater_com } } + uint8_t ProviderUnderwaterComNode::Verify_Packet_Size(const std::string &packet) + { + return packet.size(); + } + + void ProviderUnderwaterComNode::Split_Packet(std::string *packet_array, uint8_t size_array, uint8_t *nb_packet, const std::string &msg) + { + uint8_t size_msg = ceil(msg.size()/8); + char buffer[7]; + std::string split_char = "&"; + + for(uint8_t i = 0; i < *nb_packet && i < size_array; ++i) + { + try + { + packet_array[i] = msg.substr(i*7, 7); + packet_array[i] += split_char; + } + catch(const std::out_of_range& e) + { + msg.copy(buffer, msg.size()-(i*7),i*7); + packet_array[i] = std::string(buffer) + split_char; + } + + } + + *nb_packet = size_msg; + } + bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) { const char *c_cmd = cmd.data(); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index ab9ba48..e6b3b74 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -62,9 +62,13 @@ class ProviderUnderwaterComNode bool ConfirmChecksum(const std::string &sentence); void Queue_Packet(const std::string &cmd, const std::string &packet = ""); + uint8_t Verify_Packet_Size(const std::string &packet); + void Split_Packet(std::string *packet_array, uint8_t size_array, uint8_t *nb_packet, const std::string &msg); bool Check_CMD(const std::string &cmd); + void Read_Packet(); void Export_To_ROS(); + void Set_Sensor(std::string &role, uint8_t channel = 4); void Verify_Version(); void Get_Payload_Load(); From e62a5ab5def26b650ccb90f6fe005b59fcfade51 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 11:54:30 -0400 Subject: [PATCH 053/140] small changes to multiples send --- src/provider_underwater_com_node.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 968cb53..e59d68e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -228,6 +228,7 @@ namespace provider_underwater_com uint8_t size_msg = ceil(msg.size()/8); char buffer[7]; std::string split_char = "&"; + std::string end_split_char = "~"; for(uint8_t i = 0; i < *nb_packet && i < size_array; ++i) { From c9d2efc5eddebb0d608659f79386e763382df88c Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 13 Sep 2021 11:57:30 -0400 Subject: [PATCH 054/140] fix typo --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index e59d68e..ca022ba 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -240,7 +240,7 @@ namespace provider_underwater_com catch(const std::out_of_range& e) { msg.copy(buffer, msg.size()-(i*7),i*7); - packet_array[i] = std::string(buffer) + split_char; + packet_array[i] = std::string(buffer) + end_split_char; } } From 907cdfe3fbf7e2a8ea785bbcc2e79c8d67d42c95 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 14 Sep 2021 17:54:45 +0000 Subject: [PATCH 055/140] add feature from multiple package --- Dockerfile | 2 +- src/provider_underwater_com_node.cc | 24 ++++++++++++------------ src/provider_underwater_com_node.h | 6 +++++- 3 files changed, 18 insertions(+), 14 deletions(-) diff --git a/Dockerfile b/Dockerfile index 105c653..5c22f73 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -ARG BASE_IMAGE="ghcr.io/sonia-auv/sonia_common/sonia_common:x86-perception-feature-modem_service" +ARG BASE_IMAGE="ghcr.io/sonia-auv/sonia_common/sonia_common:x86-perception-latest" FROM ${BASE_IMAGE} diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index ca022ba..3d838a0 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -78,14 +78,17 @@ namespace provider_underwater_com else { std::string packet_array[BUFFER_SIZE/8]; - uint8_t nb_packet; + size_t nb_packet = Split_Packet(packet_array, BUFFER_SIZE/8, msg.data); + std::string start_header = HEADER + std::string(1, nb_packet); - Split_Packet(packet_array, BUFFER_SIZE/8, &nb_packet, msg.data); + Queue_Packet(std::string(1, CMD_QUEUE_PACKET), start_header); for(uint8_t i = 0; i < nb_packet; ++i) { Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet_array[i]); } + + Queue_Packet(std::string(1, CMD_QUEUE_PACKET), END); } } @@ -223,29 +226,26 @@ namespace provider_underwater_com return packet.size(); } - void ProviderUnderwaterComNode::Split_Packet(std::string *packet_array, uint8_t size_array, uint8_t *nb_packet, const std::string &msg) + size_t ProviderUnderwaterComNode::Split_Packet(std::string *packet_array, uint8_t size_array, const std::string &msg) { - uint8_t size_msg = ceil(msg.size()/8); + size_t size_packet = ceil(msg.size()/8.0); char buffer[7]; - std::string split_char = "&"; - std::string end_split_char = "~"; - for(uint8_t i = 0; i < *nb_packet && i < size_array; ++i) + for(uint8_t i = 0; i < size_packet && i < size_array; ++i) { try { - packet_array[i] = msg.substr(i*7, 7); - packet_array[i] += split_char; + packet_array[i] = msg.substr(i*8, 8); } catch(const std::out_of_range& e) { - msg.copy(buffer, msg.size()-(i*7),i*7); - packet_array[i] = std::string(buffer) + end_split_char; + msg.copy(buffer, msg.size()-(i*8),i*8); + packet_array[i] = std::string(buffer); } } - *nb_packet = size_msg; + return size_packet; } bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index e6b3b74..f410f76 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -34,6 +34,7 @@ #include #include #include +#include #include "Configuration.h" #include "drivers/serial.h" @@ -41,6 +42,9 @@ //#include "ModemM64_definitions.h" #include +#define HEADER "hd:p=" +#define END "hd:end" + namespace provider_underwater_com { class ProviderUnderwaterComNode @@ -63,7 +67,7 @@ class ProviderUnderwaterComNode void Queue_Packet(const std::string &cmd, const std::string &packet = ""); uint8_t Verify_Packet_Size(const std::string &packet); - void Split_Packet(std::string *packet_array, uint8_t size_array, uint8_t *nb_packet, const std::string &msg); + size_t Split_Packet(std::string *packet_array, uint8_t size_array, const std::string &msg); bool Check_CMD(const std::string &cmd); void Read_Packet(); From 85ef9bc0f59ff184bb627d2130bf2ffd5e17c0a4 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 17 Sep 2021 20:14:07 +0000 Subject: [PATCH 056/140] start the lost packet management --- src/provider_underwater_com_node.cc | 115 +++++++++++----------- src/provider_underwater_com_node.h | 14 ++- src/sharedQueue.h | 142 ++++++++++++++++++++++++++++ 3 files changed, 214 insertions(+), 57 deletions(-) create mode 100644 src/sharedQueue.h diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 3d838a0..57e9638 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -40,8 +40,8 @@ namespace provider_underwater_com underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); - reader_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); - export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); + manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); + //export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); std::string role_sensor = configuration_.getRole(); Set_Sensor(role_sensor, std::stoi(configuration_.getChannel())); @@ -50,7 +50,7 @@ namespace provider_underwater_com //Node Destructor ProviderUnderwaterComNode::~ProviderUnderwaterComNode() { - reader_thread.~thread(); + manage_thread.~thread(); export_to_ros_thread.~thread(); underwaterComSubscriber_.shutdown(); } @@ -69,27 +69,8 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::String &msg) { - if(Verify_Packet_Size(msg.data) <= 8) - { - std::string packet = "," + std::to_string(payload_) + "," + msg.data; // TODO add a size check before transmit - - Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet); - } - else - { - std::string packet_array[BUFFER_SIZE/8]; - size_t nb_packet = Split_Packet(packet_array, BUFFER_SIZE/8, msg.data); - std::string start_header = HEADER + std::string(1, nb_packet); - - Queue_Packet(std::string(1, CMD_QUEUE_PACKET), start_header); - - for(uint8_t i = 0; i < nb_packet; ++i) - { - Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet_array[i]); - } - - Queue_Packet(std::string(1, CMD_QUEUE_PACKET), END); - } + std::string packet = "," + std::to_string(payload_) + "," + msg.data; // TODO add a size check before transmit + Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet); } bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) @@ -211,7 +192,7 @@ namespace provider_underwater_com sentence = ss.str(); AppendChecksum(sentence); - serialConnection_.transmit(sentence); + writerQueue.push_back(sentence); ROS_DEBUG_STREAM("Packet sent to Modem"); } @@ -221,6 +202,31 @@ namespace provider_underwater_com } } + bool ProviderUnderwaterComNode::Read_for_Packet(char *buffer) + { + clock_t start = clock(); + + while((float_t)(clock()-start) / CLOCKS_PER_SEC <= timeout_) + { + serialConnection_.readOnce(buffer, 0); + + if(buffer[0] == SOP) + { + uint8_t i; + + for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) + { + serialConnection_. readOnce(buffer, i); + } + + buffer[i] = 0; + return true; + } + } + ROS_WARN_STREAM("No response from the other sensor"); + return false; + } + uint8_t ProviderUnderwaterComNode::Verify_Packet_Size(const std::string &packet) { return packet.size(); @@ -262,44 +268,43 @@ namespace provider_underwater_com return false; } - void ProviderUnderwaterComNode::Read_Packet() + void ProviderUnderwaterComNode::Manage_Packet() { - ROS_INFO_STREAM("Read thread started"); + ROS_INFO_STREAM("Manage thread started"); char buffer[BUFFER_SIZE]; while(!ros::isShuttingDown()) { - do - { - serialConnection_.readOnce(buffer, 0); - } - while (buffer[0] != SOP); - - uint8_t i; - - for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) - { - serialConnection_. readOnce(buffer, i); - } + ros::Duration(0.1).sleep(); - if(i >= BUFFER_SIZE) + while(!writerQueue.empty()) { - continue; - } - - buffer[i] = 0; - - if(buffer[2] == RESP_GOT_PACKET && ConfirmChecksum(buffer)) - { - std::unique_lock mlock(export_to_ros_mutex); + serialConnection_.transmit(writerQueue.front()); + + Read_for_Packet(buffer); + + if(buffer[2] == RESP_GOT_PACKET && ConfirmChecksum(buffer)) + { + if(buffer[4] == ACK) + { + if(Read_for_Packet(buffer)) + { + writerQueue.get_n_pop_front(); + } + } + } + + /*std::unique_lock mlock(export_to_ros_mutex); export_to_ros_str = std::string(buffer); - export_to_ros_cond.notify_one(); - } - else if(ConfirmChecksum(buffer)) - { - std::unique_lock mlock(response_mutex); - response_str = std::string(buffer); - response_cond.notify_one(); + export_to_ros_cond.notify_one();*/ + + if(ConfirmChecksum(buffer)) + { + std::unique_lock mlock(response_mutex); + response_str = std::string(buffer); + response_cond.notify_one(); + writerQueue.get_n_pop_front(); + } } } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index f410f76..ab42da4 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -35,12 +35,14 @@ #include #include #include +#include #include "Configuration.h" #include "drivers/serial.h" #include //#include "ModemM64_definitions.h" #include +#include "sharedQueue.h" #define HEADER "hd:p=" #define END "hd:end" @@ -66,11 +68,12 @@ class ProviderUnderwaterComNode bool ConfirmChecksum(const std::string &sentence); void Queue_Packet(const std::string &cmd, const std::string &packet = ""); + bool Read_for_Packet(char *buffer); uint8_t Verify_Packet_Size(const std::string &packet); size_t Split_Packet(std::string *packet_array, uint8_t size_array, const std::string &msg); bool Check_CMD(const std::string &cmd); - void Read_Packet(); + void Manage_Packet(); void Export_To_ROS(); void Set_Sensor(std::string &role, uint8_t channel = 4); @@ -88,7 +91,7 @@ class ProviderUnderwaterComNode ros::ServiceServer underwaterComService_; std_msgs::String msg_received; - std::thread reader_thread; + std::thread manage_thread; std::mutex response_mutex; std::condition_variable response_cond; std::string response_str = ""; @@ -102,6 +105,13 @@ class ProviderUnderwaterComNode uint8_t channel_; uint8_t payload_; bool init_error_ = true; + + SharedQueue writerQueue; + SharedQueue readerQueue; + + ros::Duration sleeptime; + + float_t timeout_ = 10; }; } diff --git a/src/sharedQueue.h b/src/sharedQueue.h new file mode 100644 index 0000000..5e8a11f --- /dev/null +++ b/src/sharedQueue.h @@ -0,0 +1,142 @@ +/** + * \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 SHAREDQUEUE_H +#define SHAREDQUEUE_H + +#include +#include +#include + +template +class SharedQueue +{ +public: + SharedQueue(); + ~SharedQueue(); + + T& front(); + void pop_front(); + + T get_n_pop_front(); + + void push_back(const T& item); + void push_back(T&& item); + + + unsigned long size(); + bool empty(); + +private: + std::deque queue_; + std::mutex mutex_; + std::condition_variable cond_; +}; + +template +SharedQueue::SharedQueue(){} + +template +SharedQueue::~SharedQueue(){} + +template +T& SharedQueue::front() +{ + std::unique_lock mlock(mutex_); + while (queue_.empty()) + { + cond_.wait(mlock); + } + return queue_.front(); +} + +template +void SharedQueue::pop_front() +{ + std::unique_lock mlock(mutex_); + while (queue_.empty()) + { + cond_.wait(mlock); + } + queue_.pop_front(); +} + +template +T SharedQueue::get_n_pop_front() +{ + std::unique_lock mlock(mutex_); + while (queue_.empty()) + { + cond_.wait(mlock); + } + T temp = queue_.front(); + queue_.pop_front(); + mlock.unlock(); + cond_.notify_one(); + return temp; +} + +template +void SharedQueue::push_back(const T& item) +{ + std::unique_lock mlock(mutex_); + queue_.push_back(item); + mlock.unlock(); // unlock before notificiation to minimize mutex con + cond_.notify_one(); // notify one waiting thread + +} + +template +void SharedQueue::push_back(T&& item) +{ + std::unique_lock mlock(mutex_); + queue_.push_back(std::move(item)); + mlock.unlock(); // unlock before notificiation to minimize mutex con + cond_.notify_one(); // notify one waiting thread + +} + +template +unsigned long SharedQueue::size() +{ + std::unique_lock mlock(mutex_); + unsigned long size = queue_.size(); + mlock.unlock(); + cond_.notify_one(); + return size; +} + +template +bool SharedQueue::empty() +{ + std::unique_lock mlock(mutex_); + bool empty = queue_.empty(); + mlock.unlock(); + cond_.notify_one(); + return empty; +} + +#endif //SHAREDQUEUE_H From 4853806771955d6855e412f334ce62f733a3aa75 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 17 Sep 2021 21:59:31 +0000 Subject: [PATCH 057/140] added roles for manage packet --- src/provider_underwater_com_node.cc | 2 ++ src/provider_underwater_com_node.h | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 57e9638..61d3858 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -340,10 +340,12 @@ namespace provider_underwater_com if(role == "master") { role_[0] = ROLE_MASTER; + role_ = ROLE_MASTER: } else { role_[0] = ROLE_SLAVE; + role_ = ROLE_SLAVE; } uint8_t i = 0; diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index ab42da4..7b54fde 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -101,7 +101,7 @@ class ProviderUnderwaterComNode std::condition_variable export_to_ros_cond; std::string export_to_ros_str = ""; - char* role; + char role_; uint8_t channel_; uint8_t payload_; bool init_error_ = true; From 2327a5f6cb63ea226878435833c0a00f28526c8e Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 17 Sep 2021 22:06:23 +0000 Subject: [PATCH 058/140] fix typo --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 61d3858..fd976c8 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -277,7 +277,7 @@ namespace provider_underwater_com { ros::Duration(0.1).sleep(); - while(!writerQueue.empty()) + while(!writerQueue.empty() && ) { serialConnection_.transmit(writerQueue.front()); From 928d4ded4834cd16375d3cf424187e8ca472ce50 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 18 Sep 2021 15:01:35 +0000 Subject: [PATCH 059/140] modify serial for timeout to work resend after 10s --- src/drivers/serial.cc | 2 +- src/provider_underwater_com_node.cc | 68 +++++++++++++++++------------ src/provider_underwater_com_node.h | 8 ++-- 3 files changed, 45 insertions(+), 33 deletions(-) diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index 787ff3d..003659d 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -30,7 +30,7 @@ Serial::Serial(std::string port) { - fd = open(port.c_str(), O_RDWR | O_NOCTTY); + fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); if(fd == -1) { ROS_ERROR("unable to connect to %s", port.c_str()); diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index fd976c8..f3bdcc9 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -41,7 +41,7 @@ namespace provider_underwater_com underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); - //export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); + export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); std::string role_sensor = configuration_.getRole(); Set_Sensor(role_sensor, std::stoi(configuration_.getChannel())); @@ -205,8 +205,9 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Read_for_Packet(char *buffer) { clock_t start = clock(); + clock_t now = clock(); - while((float_t)(clock()-start) / CLOCKS_PER_SEC <= timeout_) + while((now-start) / CLOCKS_PER_SEC <= timeout_) { serialConnection_.readOnce(buffer, 0); @@ -222,6 +223,8 @@ namespace provider_underwater_com buffer[i] = 0; return true; } + + now = clock(); } ROS_WARN_STREAM("No response from the other sensor"); return false; @@ -272,39 +275,52 @@ namespace provider_underwater_com { ROS_INFO_STREAM("Manage thread started"); char buffer[BUFFER_SIZE]; + bool new_packet; + bool resend = true; while(!ros::isShuttingDown()) { ros::Duration(0.1).sleep(); - while(!writerQueue.empty() && ) + while(!writerQueue.empty() && role_ == ROLE_MASTER) { - serialConnection_.transmit(writerQueue.front()); + if(resend) serialConnection_.transmit(writerQueue.front()); + + resend = true; - Read_for_Packet(buffer); + new_packet = Read_for_Packet(buffer); - if(buffer[2] == RESP_GOT_PACKET && ConfirmChecksum(buffer)) + if(ConfirmChecksum(buffer) && new_packet) { - if(buffer[4] == ACK) + if(buffer[2] == RESP_GOT_PACKET) + { + std::unique_lock mlock(export_to_ros_mutex); + export_to_ros_str = std::string(buffer); + export_to_ros_cond.notify_one(); + writerQueue.get_n_pop_front(); + } + else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) + { + resend = false; + } + else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) + { + ROS_WARN_STREAM("Resquest not made properly"); + writerQueue.get_n_pop_front(); + } + else { - if(Read_for_Packet(buffer)) - { - writerQueue.get_n_pop_front(); - } + std::unique_lock mlock(response_mutex); + response_str = std::string(buffer); + response_cond.notify_one(); + writerQueue.get_n_pop_front(); } } + } - /*std::unique_lock mlock(export_to_ros_mutex); - export_to_ros_str = std::string(buffer); - export_to_ros_cond.notify_one();*/ - - if(ConfirmChecksum(buffer)) - { - std::unique_lock mlock(response_mutex); - response_str = std::string(buffer); - response_cond.notify_one(); - writerQueue.get_n_pop_front(); - } + while(!readerQueue.empty() && role_ == ROLE_SLAVE) + { + ROS_INFO_STREAM("I am listening"); } } } @@ -333,18 +349,14 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Set_Sensor(std::string &role, uint8_t channel) { - char role_[2]; - ROS_ASSERT_MSG(role == "master" || role == "slave", "Set the role as 'master' or 'slave'. Error in config"); if(role == "master") { - role_[0] = ROLE_MASTER; - role_ = ROLE_MASTER: + role_ = ROLE_MASTER; } else { - role_[0] = ROLE_SLAVE; role_ = ROLE_SLAVE; } @@ -355,7 +367,7 @@ namespace provider_underwater_com init_error_ = false; Verify_Version(); Get_Payload_Load(); - Set_Configuration(role_[0], channel); + Set_Configuration(role_, channel); Flush_Queue(); ++i; } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 7b54fde..9ff95d4 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -44,8 +44,7 @@ #include #include "sharedQueue.h" -#define HEADER "hd:p=" -#define END "hd:end" +#define MALFORMED '!' namespace provider_underwater_com { @@ -92,11 +91,12 @@ class ProviderUnderwaterComNode std_msgs::String msg_received; std::thread manage_thread; + std::thread export_to_ros_thread; + std::mutex response_mutex; std::condition_variable response_cond; std::string response_str = ""; - std::thread export_to_ros_thread; std::mutex export_to_ros_mutex; std::condition_variable export_to_ros_cond; std::string export_to_ros_str = ""; @@ -111,7 +111,7 @@ class ProviderUnderwaterComNode ros::Duration sleeptime; - float_t timeout_ = 10; + float_t timeout_ = 10.0; }; } From 4b555a9b0978f3107770c2d624c969819693992f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 18 Sep 2021 15:09:27 +0000 Subject: [PATCH 060/140] fct for managing packet for master and slave --- src/provider_underwater_com_node.cc | 78 ++++++++++++++++------------- src/provider_underwater_com_node.h | 2 + 2 files changed, 45 insertions(+), 35 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index f3bdcc9..f87ab10 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -271,12 +271,49 @@ namespace provider_underwater_com return false; } - void ProviderUnderwaterComNode::Manage_Packet() + void ProviderUnderwaterComNode::Manage_Packet_Master() { - ROS_INFO_STREAM("Manage thread started"); char buffer[BUFFER_SIZE]; - bool new_packet; bool resend = true; + bool new_packet; + + if(resend) serialConnection_.transmit(writerQueue.front()); + + resend = true; + + new_packet = Read_for_Packet(buffer); + + if(ConfirmChecksum(buffer) && new_packet) + { + if(buffer[2] == RESP_GOT_PACKET) + { + std::unique_lock mlock(export_to_ros_mutex); + export_to_ros_str = std::string(buffer); + export_to_ros_cond.notify_one(); + writerQueue.get_n_pop_front(); + } + else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) + { + resend = false; + } + else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) + { + ROS_WARN_STREAM("Resquest not made properly"); + writerQueue.get_n_pop_front(); + } + else + { + std::unique_lock mlock(response_mutex); + response_str = std::string(buffer); + response_cond.notify_one(); + writerQueue.get_n_pop_front(); + } + } + } + + void ProviderUnderwaterComNode::Manage_Packet() + { + ROS_INFO_STREAM("Manage thread started"); while(!ros::isShuttingDown()) { @@ -284,43 +321,14 @@ namespace provider_underwater_com while(!writerQueue.empty() && role_ == ROLE_MASTER) { - if(resend) serialConnection_.transmit(writerQueue.front()); - - resend = true; - - new_packet = Read_for_Packet(buffer); - - if(ConfirmChecksum(buffer) && new_packet) - { - if(buffer[2] == RESP_GOT_PACKET) - { - std::unique_lock mlock(export_to_ros_mutex); - export_to_ros_str = std::string(buffer); - export_to_ros_cond.notify_one(); - writerQueue.get_n_pop_front(); - } - else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) - { - resend = false; - } - else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) - { - ROS_WARN_STREAM("Resquest not made properly"); - writerQueue.get_n_pop_front(); - } - else - { - std::unique_lock mlock(response_mutex); - response_str = std::string(buffer); - response_cond.notify_one(); - writerQueue.get_n_pop_front(); - } - } + ROS_INFO_STREAM("I am sending"); + Manage_Packet_Master(); } while(!readerQueue.empty() && role_ == ROLE_SLAVE) { ROS_INFO_STREAM("I am listening"); + Manage_Pakcet_Slave(); } } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 9ff95d4..0b2b79a 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -71,6 +71,8 @@ class ProviderUnderwaterComNode uint8_t Verify_Packet_Size(const std::string &packet); size_t Split_Packet(std::string *packet_array, uint8_t size_array, const std::string &msg); bool Check_CMD(const std::string &cmd); + void Manage_Packet_Master(); + void Manage_Pakcet_Slave(); void Manage_Packet(); void Export_To_ROS(); From d00718f1d98b180701820ccd4f2e89f7d1ce8e41 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 18 Sep 2021 18:57:06 +0000 Subject: [PATCH 061/140] bump package --- package.xml | 2 +- src/provider_underwater_com_node.cc | 5 +++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 0780b2a..1a2d835 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.2.0 + 0.3.0 The provider_underwater_com package Francis Alonzo diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index f87ab10..859ceaf 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -311,6 +311,11 @@ namespace provider_underwater_com } } + void ProviderUnderwaterComNode::Manage_Pakcet_Slave() + { + + } + void ProviderUnderwaterComNode::Manage_Packet() { ROS_INFO_STREAM("Manage thread started"); From 6aa43e32d7b40e0f78e8cd57ff08c9a24d9f544f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 18 Sep 2021 21:18:43 +0000 Subject: [PATCH 062/140] slave configuration --- src/provider_underwater_com_node.cc | 66 ++++++++++++++++++++--------- src/provider_underwater_com_node.h | 7 ++- 2 files changed, 52 insertions(+), 21 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index f87ab10..207854e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -41,10 +41,14 @@ namespace provider_underwater_com underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); - export_to_ros_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Export_To_ROS, this)); - + std::string role_sensor = configuration_.getRole(); Set_Sensor(role_sensor, std::stoi(configuration_.getChannel())); + + if(role_ == ROLE_SLAVE) + { + read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); + } } //Node Destructor @@ -290,7 +294,7 @@ namespace provider_underwater_com std::unique_lock mlock(export_to_ros_mutex); export_to_ros_str = std::string(buffer); export_to_ros_cond.notify_one(); - writerQueue.get_n_pop_front(); + writerQueue.pop_front(); } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) { @@ -298,19 +302,31 @@ namespace provider_underwater_com } else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) { - ROS_WARN_STREAM("Resquest not made properly"); - writerQueue.get_n_pop_front(); + ROS_ERROR_STREAM("Resquest not made properly"); + writerQueue.pop_front(); } else { std::unique_lock mlock(response_mutex); response_str = std::string(buffer); response_cond.notify_one(); - writerQueue.get_n_pop_front(); + writerQueue.pop_front(); } } } + void ProviderUnderwaterComNode::Manage_Packet_Slave() + { + Export_To_ROS(readerQueue.get_n_pop_front()); + + while(writerQueue.empty()) + { + ros::Duration(1).sleep(); + } + + serialConnection_.transmit(writerQueue.get_n_pop_front()); + } + void ProviderUnderwaterComNode::Manage_Packet() { ROS_INFO_STREAM("Manage thread started"); @@ -328,30 +344,42 @@ namespace provider_underwater_com while(!readerQueue.empty() && role_ == ROLE_SLAVE) { ROS_INFO_STREAM("I am listening"); - Manage_Pakcet_Slave(); + Manage_Packet_Slave(); } } } - void ProviderUnderwaterComNode::Export_To_ROS() + void ProviderUnderwaterComNode::Export_To_ROS(std::string buffer) { std::string size; std::string msg; - while(!ros::isShuttingDown()) - { - msg_received.data.clear(); - std::unique_lock mlock(export_to_ros_mutex); - export_to_ros_cond.wait(mlock); + msg_received.data.clear(); + std::stringstream ss(buffer); + std::getline(ss, size, ','); // TODO add a size check before publish and check if message good + std::getline(ss, size, ','); + std::getline(ss, msg, '*'); + + msg_received.data = msg; - std::stringstream ss(export_to_ros_str); - std::getline(ss, size, ','); // TODO add a size check before publish and check if message good - std::getline(ss, size, ','); - std::getline(ss, msg, '*'); + underwaterComPublisher_.publish(msg_received); + } + + void ProviderUnderwaterComNode::Read_for_Packet_Slave() + { + char buffer[BUFFER_SIZE]; + bool new_packet = false; - msg_received.data = msg; + while(!ros::isShuttingDown()) + { + ros::Duration(0.1).sleep(); + + new_packet = Read_for_Packet(buffer); - underwaterComPublisher_.publish(msg_received); + if(new_packet && ConfirmChecksum(buffer)) + { + readerQueue.push_back(buffer); + } } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 0b2b79a..44fbc2f 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -71,11 +71,13 @@ class ProviderUnderwaterComNode uint8_t Verify_Packet_Size(const std::string &packet); size_t Split_Packet(std::string *packet_array, uint8_t size_array, const std::string &msg); bool Check_CMD(const std::string &cmd); + void Manage_Packet_Master(); - void Manage_Pakcet_Slave(); + void Manage_Packet_Slave(); void Manage_Packet(); - void Export_To_ROS(); + void Export_To_ROS(std::string buffer); + void Read_for_Packet_Slave(); void Set_Sensor(std::string &role, uint8_t channel = 4); void Verify_Version(); @@ -94,6 +96,7 @@ class ProviderUnderwaterComNode std::thread manage_thread; std::thread export_to_ros_thread; + std::thread read_for_packet_slave; std::mutex response_mutex; std::condition_variable response_cond; From a5744ba643c05eb6e1e706b5b40f1f992cd55902 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 18 Sep 2021 21:56:10 +0000 Subject: [PATCH 063/140] added queue protection and package lost --- src/provider_underwater_com_node.cc | 120 ++++++++++++---------------- src/provider_underwater_com_node.h | 12 +-- 2 files changed, 55 insertions(+), 77 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 207854e..0621206 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -39,8 +39,6 @@ namespace provider_underwater_com underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); - - manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); std::string role_sensor = configuration_.getRole(); Set_Sensor(role_sensor, std::stoi(configuration_.getChannel())); @@ -49,13 +47,14 @@ namespace provider_underwater_com { read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); } + manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); } //Node Destructor ProviderUnderwaterComNode::~ProviderUnderwaterComNode() { manage_thread.~thread(); - export_to_ros_thread.~thread(); + read_for_packet_slave.~thread(); underwaterComSubscriber_.shutdown(); } @@ -79,13 +78,18 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) { + char buffer[BUFFER_SIZE]; + + writerQueue_mutex.lock(); + Queue_Packet(std::string(1, req.cmd)); + Transmit_Packet(true); + Read_for_Packet(buffer); - std::unique_lock mlock(response_mutex); - response_cond.wait(mlock); + writerQueue_mutex.unlock(); - char cmd_rec = response_str.at(2); - std::stringstream ss(response_str); + char cmd_rec = buffer[2]; + std::stringstream ss(buffer); switch (cmd_rec) { @@ -206,6 +210,21 @@ namespace provider_underwater_com } } + bool ProviderUnderwaterComNode::Transmit_Packet(bool pop_packet) + { + if(!writerQueue.empty()) + { + serialConnection_.transmit(writerQueue.front()); + if(pop_packet) writerQueue.pop_front(); + return true; + } + else + { + ROS_WARN_STREAM("Packet isn't queue"); + return false; + } + } + bool ProviderUnderwaterComNode::Read_for_Packet(char *buffer) { clock_t start = clock(); @@ -230,37 +249,9 @@ namespace provider_underwater_com now = clock(); } - ROS_WARN_STREAM("No response from the other sensor"); return false; } - uint8_t ProviderUnderwaterComNode::Verify_Packet_Size(const std::string &packet) - { - return packet.size(); - } - - size_t ProviderUnderwaterComNode::Split_Packet(std::string *packet_array, uint8_t size_array, const std::string &msg) - { - size_t size_packet = ceil(msg.size()/8.0); - char buffer[7]; - - for(uint8_t i = 0; i < size_packet && i < size_array; ++i) - { - try - { - packet_array[i] = msg.substr(i*8, 8); - } - catch(const std::out_of_range& e) - { - msg.copy(buffer, msg.size()-(i*8),i*8); - packet_array[i] = std::string(buffer); - } - - } - - return size_packet; - } - bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) { const char *c_cmd = cmd.data(); @@ -281,19 +272,17 @@ namespace provider_underwater_com bool resend = true; bool new_packet; - if(resend) serialConnection_.transmit(writerQueue.front()); + if(resend) Transmit_Packet(false); resend = true; - + new_packet = Read_for_Packet(buffer); if(ConfirmChecksum(buffer) && new_packet) { if(buffer[2] == RESP_GOT_PACKET) { - std::unique_lock mlock(export_to_ros_mutex); - export_to_ros_str = std::string(buffer); - export_to_ros_cond.notify_one(); + Export_To_ROS(buffer); writerQueue.pop_front(); } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) @@ -305,13 +294,6 @@ namespace provider_underwater_com ROS_ERROR_STREAM("Resquest not made properly"); writerQueue.pop_front(); } - else - { - std::unique_lock mlock(response_mutex); - response_str = std::string(buffer); - response_cond.notify_one(); - writerQueue.pop_front(); - } } } @@ -324,7 +306,7 @@ namespace provider_underwater_com ros::Duration(1).sleep(); } - serialConnection_.transmit(writerQueue.get_n_pop_front()); + Transmit_Packet(true); } void ProviderUnderwaterComNode::Manage_Packet() @@ -333,13 +315,17 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - ros::Duration(0.1).sleep(); + ros::Duration(1).sleep(); + + writerQueue_mutex.lock(); while(!writerQueue.empty() && role_ == ROLE_MASTER) { ROS_INFO_STREAM("I am sending"); Manage_Packet_Master(); } + + writerQueue_mutex.unlock(); while(!readerQueue.empty() && role_ == ROLE_SLAVE) { @@ -410,21 +396,22 @@ namespace provider_underwater_com if(i == 3) { - ROS_ERROR("Problem with the init. Node shutting down."); + ROS_ERROR_STREAM("Problem with the init. Node shutting down."); ros::shutdown(); } + ROS_INFO_STREAM("Initialisation completed"); } void ProviderUnderwaterComNode::Verify_Version() { std::string major_version = ""; + char buffer[BUFFER_SIZE]; Queue_Packet(std::string(1, CMD_GET_VERSION)); + Transmit_Packet(true); + Read_for_Packet(buffer); // TODO cmd and checksum check - std::unique_lock mlock(response_mutex); - response_cond.wait(mlock); - - std::stringstream ss(response_str); + std::stringstream ss(buffer); std::getline(ss, major_version, ','); std::getline(ss, major_version, ','); @@ -438,13 +425,13 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Get_Payload_Load() { std::string payload = ""; + char buffer[BUFFER_SIZE]; Queue_Packet(std::string(1, CMD_GET_PAYLOAD_SIZE)); + Transmit_Packet(true); + Read_for_Packet(buffer); // TODO cmd and checksum check - std::unique_lock mlock(response_mutex); - response_cond.wait(mlock); - - std::stringstream ss(response_str); + std::stringstream ss(buffer); std::getline(ss, payload, ','); std::getline(ss, payload, '*'); @@ -463,17 +450,16 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Set_Configuration(const char &role, uint8_t channel) { std::string acknowledge; - char buffer[2]; + char buffer[BUFFER_SIZE]; sprintf(buffer, "%d", channel); std::string packet = "," + std::string(1, role) + "," + buffer; Queue_Packet(std::string(1, CMD_SET_SETTINGS), packet); - - std::unique_lock mlock(response_mutex); - response_cond.wait(mlock); + Transmit_Packet(true); + Read_for_Packet(buffer); // TODO cmd and checksum check - std::stringstream ss(response_str); + std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); @@ -487,13 +473,13 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Flush_Queue() { std::string acknowledge; + char buffer[BUFFER_SIZE]; Queue_Packet(std::string(1, CMD_FLUSH)); + Transmit_Packet(true); + Read_for_Packet(buffer); // TODO cmd and checksum check - std::unique_lock mlock(response_mutex); - response_cond.wait(mlock); - - std::stringstream ss(response_str); + std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 44fbc2f..dc3e49b 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -67,9 +67,8 @@ class ProviderUnderwaterComNode bool ConfirmChecksum(const std::string &sentence); void Queue_Packet(const std::string &cmd, const std::string &packet = ""); + bool Transmit_Packet(bool pop_packet); bool Read_for_Packet(char *buffer); - uint8_t Verify_Packet_Size(const std::string &packet); - size_t Split_Packet(std::string *packet_array, uint8_t size_array, const std::string &msg); bool Check_CMD(const std::string &cmd); void Manage_Packet_Master(); @@ -95,16 +94,9 @@ class ProviderUnderwaterComNode std_msgs::String msg_received; std::thread manage_thread; - std::thread export_to_ros_thread; std::thread read_for_packet_slave; - std::mutex response_mutex; - std::condition_variable response_cond; - std::string response_str = ""; - - std::mutex export_to_ros_mutex; - std::condition_variable export_to_ros_cond; - std::string export_to_ros_str = ""; + std::mutex writerQueue_mutex; char role_; uint8_t channel_; From 744b54e22f47fe979ccd1201f3083e0edb346fcf Mon Sep 17 00:00:00 2001 From: Francis Alonzo <44241055+supertoto29@users.noreply.github.com> Date: Tue, 21 Sep 2021 19:19:56 -0400 Subject: [PATCH 064/140] Update sharedQueue.h --- src/sharedQueue.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/sharedQueue.h b/src/sharedQueue.h index 5e8a11f..809e9d4 100644 --- a/src/sharedQueue.h +++ b/src/sharedQueue.h @@ -1,7 +1,7 @@ /** * \file serial.h - * \author Lucas Mongrain - * \date 26/10/2017 + * \author user66875 (https://stackoverflow.com/a/36763257) + * \date 21/04/2016 * * \copyright Copyright (c) 2021 S.O.N.I.A. All rights reserved. * From 6e4b554ff2e2d3c460b9722bdeda264e6ee6e439 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 24 Sep 2021 18:13:24 +0000 Subject: [PATCH 065/140] fix transmit for master. Thread was going crazy --- src/provider_underwater_com_node.cc | 44 +++++++++++++++-------------- src/provider_underwater_com_node.h | 1 + 2 files changed, 24 insertions(+), 21 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 0621206..07e559e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -249,6 +249,7 @@ namespace provider_underwater_com now = clock(); } + ROS_WARN_STREAM("No response after 10 secs."); return false; } @@ -269,32 +270,35 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Manage_Packet_Master() { char buffer[BUFFER_SIZE]; - bool resend = true; bool new_packet; - if(resend) Transmit_Packet(false); - - resend = true; + if(resend_) Transmit_Packet(false); new_packet = Read_for_Packet(buffer); - if(ConfirmChecksum(buffer) && new_packet) + if(new_packet) { - if(buffer[2] == RESP_GOT_PACKET) - { - Export_To_ROS(buffer); - writerQueue.pop_front(); - } - else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) + if(ConfirmChecksum(buffer)) { - resend = false; - } - else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) - { - ROS_ERROR_STREAM("Resquest not made properly"); - writerQueue.pop_front(); + if(buffer[2] == RESP_GOT_PACKET) + { + Export_To_ROS(buffer); + writerQueue.pop_front(); + resend_ = true; + } + else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) + { + resend_ = false; + } + else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) + { + ROS_ERROR_STREAM("Resquest not made properly"); + writerQueue.pop_front(); + resend_ = true; + } } } + } void ProviderUnderwaterComNode::Manage_Packet_Slave() @@ -319,17 +323,15 @@ namespace provider_underwater_com writerQueue_mutex.lock(); - while(!writerQueue.empty() && role_ == ROLE_MASTER) + if(!writerQueue.empty() && role_ == ROLE_MASTER) { - ROS_INFO_STREAM("I am sending"); Manage_Packet_Master(); } writerQueue_mutex.unlock(); - while(!readerQueue.empty() && role_ == ROLE_SLAVE) + if(!readerQueue.empty() && role_ == ROLE_SLAVE) { - ROS_INFO_STREAM("I am listening"); Manage_Packet_Slave(); } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index dc3e49b..3725fb3 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -102,6 +102,7 @@ class ProviderUnderwaterComNode uint8_t channel_; uint8_t payload_; bool init_error_ = true; + bool resend_ = true; SharedQueue writerQueue; SharedQueue readerQueue; From 002500dd67bf416fda785cc704bb381f79bee8c1 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 24 Sep 2021 16:46:59 -0400 Subject: [PATCH 066/140] fixing the image name on fix workflow --- .github/workflows/docker-image-perception-fix.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/docker-image-perception-fix.yml b/.github/workflows/docker-image-perception-fix.yml index c973bd7..31989f3 100644 --- a/.github/workflows/docker-image-perception-fix.yml +++ b/.github/workflows/docker-image-perception-fix.yml @@ -13,7 +13,7 @@ jobs: ARCH: x86 TARGET_TYPE: perception TARGET_VERSION: fix - 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: fix - IMAGE_NAME: template_name + IMAGE_NAME: provider_underwater_com GITHUB_REMOTE_URL: docker.pkg.github.com/${{ github.repository }} steps: - uses: actions/checkout@v2 From 873ebbe73c616340f207b68dbf36fbb80385a83f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 24 Sep 2021 23:04:24 +0000 Subject: [PATCH 067/140] test for environement variable --- .devcontainer/devcontainer.json | 1 + .vscode/launch.json | 2 +- config/{auv7.yaml => AUV7.yaml} | 2 +- config/{auv8.yaml => AUV8.yaml} | 2 +- ....launch => provider_underwater_com.launch} | 5 +-- launch/provider_underwater_com_auv7.launch | 8 ----- src/provider_underwater_com_node.cc | 31 ++++++++++--------- src/provider_underwater_com_node.h | 3 +- 8 files changed, 25 insertions(+), 29 deletions(-) rename config/{auv7.yaml => AUV7.yaml} (69%) rename config/{auv8.yaml => AUV8.yaml} (70%) rename launch/{provider_underwater_com_auv8.launch => provider_underwater_com.launch} (88%) delete mode 100644 launch/provider_underwater_com_auv7.launch diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 09797ed..4d7602a 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -20,6 +20,7 @@ "seccomp=unconfined", "--rm", "--privileged", + "--env=AUV", "--network", "host" ], diff --git a/.vscode/launch.json b/.vscode/launch.json index 31deb9b..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/provider_underwater_com/launch/provider_underwater_com_auv8.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/config/auv7.yaml b/config/AUV7.yaml similarity index 69% rename from config/auv7.yaml rename to config/AUV7.yaml index 5a5c9f0..31ef32a 100644 --- a/config/auv7.yaml +++ b/config/AUV7.yaml @@ -1,6 +1,6 @@ #********** Driver Parameters ************ -/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" +/provider_underwater_com/connection/tty_port: "/dev/ttyACM0" /provider_underwater_com/settings/role : "slave" diff --git a/config/auv8.yaml b/config/AUV8.yaml similarity index 70% rename from config/auv8.yaml rename to config/AUV8.yaml index c6795a7..0452e0e 100644 --- a/config/auv8.yaml +++ b/config/AUV8.yaml @@ -1,6 +1,6 @@ #********** Driver Parameters ************ -/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" +/provider_underwater_com/connection/tty_port: "/dev/ttyACM0" /provider_underwater_com/settings/role : "master" diff --git a/launch/provider_underwater_com_auv8.launch b/launch/provider_underwater_com.launch similarity index 88% rename from launch/provider_underwater_com_auv8.launch rename to launch/provider_underwater_com.launch index eae5884..5c4f801 100644 --- a/launch/provider_underwater_com_auv8.launch +++ b/launch/provider_underwater_com.launch @@ -1,7 +1,8 @@ + - - + + \ No newline at end of file diff --git a/launch/provider_underwater_com_auv7.launch b/launch/provider_underwater_com_auv7.launch deleted file mode 100644 index dbee510..0000000 --- a/launch/provider_underwater_com_auv7.launch +++ /dev/null @@ -1,8 +0,0 @@ - - - - - - - - \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 07e559e..e870035 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -34,14 +34,26 @@ namespace provider_underwater_com ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { - serialConnection_.flush(); + //serialConnection_.flush(); underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); - std::string role_sensor = configuration_.getRole(); - Set_Sensor(role_sensor, std::stoi(configuration_.getChannel())); + char *auv; + + auv = getenv("AUV"); + + if(strcmp(auv, "AUV7") == 0) + { + role_ = ROLE_SLAVE; + } + else + { + role_ = ROLE_MASTER; + } + + Set_Sensor(std::stoi(configuration_.getChannel())); if(role_ == ROLE_SLAVE) { @@ -371,19 +383,8 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Set_Sensor(std::string &role, uint8_t channel) + void ProviderUnderwaterComNode::Set_Sensor(uint8_t channel) { - ROS_ASSERT_MSG(role == "master" || role == "slave", "Set the role as 'master' or 'slave'. Error in config"); - - if(role == "master") - { - role_ = ROLE_MASTER; - } - else - { - role_ = ROLE_SLAVE; - } - uint8_t i = 0; while(i < 3 && init_error_ == true) diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 3725fb3..f297800 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -27,6 +27,7 @@ #define PROVIDER_UNDERWATER_COM_NODE #include +#include #include #include #include @@ -78,7 +79,7 @@ class ProviderUnderwaterComNode void Export_To_ROS(std::string buffer); void Read_for_Packet_Slave(); - void Set_Sensor(std::string &role, uint8_t channel = 4); + void Set_Sensor(uint8_t channel = 4); void Verify_Version(); void Get_Payload_Load(); void Set_Configuration(const char &role, uint8_t channel); From adbaf12e8ed0a990c2c31b185b2a18e868599553 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 24 Sep 2021 23:07:32 +0000 Subject: [PATCH 068/140] change in dev container --- .devcontainer/devcontainer.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 4d7602a..840ad3b 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -20,7 +20,7 @@ "seccomp=unconfined", "--rm", "--privileged", - "--env=AUV", + "--env AUV", "--network", "host" ], From d7e8357bc448f47c8997eade3cfd16c5d51197a0 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 25 Sep 2021 00:30:43 +0000 Subject: [PATCH 069/140] env variable is set --- .devcontainer/devcontainer.json | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 840ad3b..c3988a9 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -20,7 +20,7 @@ "seccomp=unconfined", "--rm", "--privileged", - "--env AUV", + "--env", "AUV=AUV7", "--network", "host" ], From d375410ff8d008fc72191814661cdad6535aaeaf Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 25 Sep 2021 14:11:38 +0000 Subject: [PATCH 070/140] use env variables for launch files --- launch/provider_underwater_com.launch | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/provider_underwater_com.launch b/launch/provider_underwater_com.launch index 5c4f801..df586ae 100644 --- a/launch/provider_underwater_com.launch +++ b/launch/provider_underwater_com.launch @@ -3,6 +3,6 @@ - + \ No newline at end of file From 5dc777a76274941073dced849ea273b437968c0f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 26 Sep 2021 13:10:27 +0000 Subject: [PATCH 071/140] fix the init sequence --- .devcontainer/devcontainer.json | 2 +- package.xml | 2 +- src/drivers/serial.cc | 8 +++---- src/provider_underwater_com_node.cc | 34 +++++++++++++++++------------ src/provider_underwater_com_node.h | 1 + 5 files changed, 27 insertions(+), 20 deletions(-) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index c3988a9..24ee7c0 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -20,7 +20,7 @@ "seccomp=unconfined", "--rm", "--privileged", - "--env", "AUV=AUV7", + "--env", "AUV=AUV8", "--network", "host" ], diff --git a/package.xml b/package.xml index 1a2d835..9ab9b1f 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.3.0 + 0.4.0 The provider_underwater_com package Francis Alonzo diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index 003659d..d600012 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -76,7 +76,7 @@ Serial::~Serial() std::string Serial::receive(size_t count) { - ROS_DEBUG("provider_imu receive data"); + ROS_DEBUG("provider_underwater_com receive data"); char data[1024]; data[0] = 0; @@ -86,18 +86,18 @@ std::string Serial::receive(size_t count) void Serial::readOnce(char* data, int offset) { - ROS_DEBUG("provider_imu receive Once"); + ROS_DEBUG("provider_underwater_com receive Once"); read(fd, (data+offset), 1); } void Serial::flush() { - ROS_DEBUG("provider_imu flush data"); + ROS_DEBUG("provider_underwater_com flush data"); tcflush(fd,TCIOFLUSH); } ssize_t Serial::transmit(const std::string data) { - ROS_DEBUG("provider_imu transmit data"); + ROS_DEBUG("provider_underwater_com transmit data"); return write(fd, data.c_str(), data.size()); } diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index e870035..e9f6c81 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -75,11 +75,13 @@ namespace provider_underwater_com { ros::Rate r(1); // 1 Hz - while(ros::ok()) + while(ros::ok() && init_completed_ == true) { ros::spinOnce(); r.sleep(); } + + ros::shutdown(); } void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::String &msg) @@ -400,9 +402,13 @@ namespace provider_underwater_com if(i == 3) { ROS_ERROR_STREAM("Problem with the init. Node shutting down."); - ros::shutdown(); + init_completed_ = false; + } + else + { + init_completed_ = true; + ROS_INFO_STREAM("Initialisation completed"); } - ROS_INFO_STREAM("Initialisation completed"); } void ProviderUnderwaterComNode::Verify_Version() @@ -418,9 +424,9 @@ namespace provider_underwater_com std::getline(ss, major_version, ','); std::getline(ss, major_version, ','); - if(major_version != "1") + if(major_version != "1" && ConfirmChecksum(buffer)) { - ROS_ERROR("Major Version isn't 1. Restarting init"); + ROS_ERROR_STREAM("Major Version isn't 1. Restarting init"); init_error_ = true; } } @@ -432,20 +438,20 @@ namespace provider_underwater_com Queue_Packet(std::string(1, CMD_GET_PAYLOAD_SIZE)); Transmit_Packet(true); - Read_for_Packet(buffer); // TODO cmd and checksum check + Read_for_Packet(buffer); // TODO cmd check std::stringstream ss(buffer); std::getline(ss, payload, ','); std::getline(ss, payload, '*'); - if(payload >= "0" || payload <= "9") + if((payload >= "0" || payload <= "9") && ConfirmChecksum(buffer)) { payload_ = std::stoi(payload); - ROS_DEBUG("Payload set"); + ROS_DEBUG_STREAM("Payload set"); } else { - ROS_ERROR("Payload isn't a integer. Restarting init"); + ROS_ERROR_STREAM("Payload isn't a integer. Restarting init"); init_error_ = true; } } @@ -460,15 +466,15 @@ namespace provider_underwater_com Queue_Packet(std::string(1, CMD_SET_SETTINGS), packet); Transmit_Packet(true); - Read_for_Packet(buffer); // TODO cmd and checksum check + Read_for_Packet(buffer); // TODO cmd check std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK)) + if(acknowledge == std::string(1, NAK) && ConfirmChecksum(buffer)) { - ROS_ERROR("Could not set the configuration. Restarting init"); + ROS_ERROR_STREAM("Could not set the configuration. Restarting init"); init_error_ = true; } } @@ -486,9 +492,9 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK)) + if(acknowledge == std::string(1, NAK) && ConfirmChecksum(buffer)) { - ROS_ERROR("Couldn't flush the queue. Restarting init"); + ROS_ERROR_STREAM("Couldn't flush the queue. Restarting init"); init_error_ = true; } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index f297800..0e456c3 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -102,6 +102,7 @@ class ProviderUnderwaterComNode char role_; uint8_t channel_; uint8_t payload_; + bool init_completed_ = true; bool init_error_ = true; bool resend_ = true; From d04504673f70b59376331e54063be9160109f9e7 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 27 Sep 2021 20:35:12 +0000 Subject: [PATCH 072/140] ready for pool test & change inverted checksum --- config/AUV7.yaml | 2 +- config/AUV8.yaml | 2 +- src/provider_underwater_com_node.cc | 6 +++--- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/config/AUV7.yaml b/config/AUV7.yaml index 31ef32a..5a5c9f0 100644 --- a/config/AUV7.yaml +++ b/config/AUV7.yaml @@ -1,6 +1,6 @@ #********** Driver Parameters ************ -/provider_underwater_com/connection/tty_port: "/dev/ttyACM0" +/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" /provider_underwater_com/settings/role : "slave" diff --git a/config/AUV8.yaml b/config/AUV8.yaml index 0452e0e..c6795a7 100644 --- a/config/AUV8.yaml +++ b/config/AUV8.yaml @@ -1,6 +1,6 @@ #********** Driver Parameters ************ -/provider_underwater_com/connection/tty_port: "/dev/ttyACM0" +/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" /provider_underwater_com/settings/role : "master" diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index e9f6c81..21cb40e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -424,7 +424,7 @@ namespace provider_underwater_com std::getline(ss, major_version, ','); std::getline(ss, major_version, ','); - if(major_version != "1" && ConfirmChecksum(buffer)) + if(major_version != "1" && !ConfirmChecksum(buffer)) { ROS_ERROR_STREAM("Major Version isn't 1. Restarting init"); init_error_ = true; @@ -472,7 +472,7 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK) && ConfirmChecksum(buffer)) + if(acknowledge == std::string(1, NAK) && !ConfirmChecksum(buffer)) { ROS_ERROR_STREAM("Could not set the configuration. Restarting init"); init_error_ = true; @@ -492,7 +492,7 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK) && ConfirmChecksum(buffer)) + if(acknowledge == std::string(1, NAK) && !ConfirmChecksum(buffer)) { ROS_ERROR_STREAM("Couldn't flush the queue. Restarting init"); init_error_ = true; From 2594b2b021ab3fb57e002335590b4fe67db8363c Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 27 Sep 2021 21:54:40 +0000 Subject: [PATCH 073/140] fix service --- src/provider_underwater_com_node.cc | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 21cb40e..0968fd9 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -103,6 +103,7 @@ namespace provider_underwater_com writerQueue_mutex.unlock(); char cmd_rec = buffer[2]; + char tmp; std::stringstream ss(buffer); switch (cmd_rec) @@ -126,7 +127,8 @@ namespace provider_underwater_com std::getline(ss, role, ','); std::getline(ss, channel, '*'); - res.role = std::stoi(role); + tmp = role_; + res.role = (uint8_t)tmp; res.channel = std::stoi(channel); break; } @@ -143,7 +145,8 @@ namespace provider_underwater_com std::getline(ss, packet_loss_count, ','); std::getline(ss, bit_error_rate, '*'); - res.link = std::stoi(link_up); + tmp = link_up; + res.link = (uint8_t)link_up; 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); From 1e6fa503377b2706ef7091051a2a30beea6b9630 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 27 Sep 2021 21:58:30 +0000 Subject: [PATCH 074/140] fix typo --- src/provider_underwater_com_node.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 0968fd9..d9791c9 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -127,7 +127,7 @@ namespace provider_underwater_com std::getline(ss, role, ','); std::getline(ss, channel, '*'); - tmp = role_; + tmp = role.at(0); res.role = (uint8_t)tmp; res.channel = std::stoi(channel); break; @@ -145,8 +145,8 @@ namespace provider_underwater_com std::getline(ss, packet_loss_count, ','); std::getline(ss, bit_error_rate, '*'); - tmp = link_up; - res.link = (uint8_t)link_up; + tmp = link_up.at(0); + res.link = (uint8_t)tmp; 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); From cfe7b249dca1899b95700786370dab90c33da75c Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 27 Sep 2021 21:59:20 +0000 Subject: [PATCH 075/140] change service init to match the proc --- src/provider_underwater_com_node.cc | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index d9791c9..593f1c8 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -38,10 +38,8 @@ namespace provider_underwater_com underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); - underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); - - char *auv; + char *auv; auv = getenv("AUV"); if(strcmp(auv, "AUV7") == 0) @@ -60,6 +58,8 @@ namespace provider_underwater_com read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); } manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); + + underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); } //Node Destructor From 230ab96e508f27a7fedd5bd2b234c15a16e0c6b2 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 27 Sep 2021 23:18:48 +0000 Subject: [PATCH 076/140] Add flush cmd to service --- src/provider_underwater_com_node.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 593f1c8..39c63f7 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -151,6 +151,10 @@ namespace provider_underwater_com res.packet_count_loss = std::stoi(packet_loss_count); res.bit_error_rate = std::stof(bit_error_rate); } + case CMD_FLUSH: + { + ROS_INFO_STREAM("Buffer of sensor flushed"); + } default: { ROS_ERROR("CMD received isn't working with the service."); From ffd83d5cb97522520ad73927985556b9e6d62cca Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 27 Sep 2021 19:27:27 -0400 Subject: [PATCH 077/140] fix switch case missing break --- src/provider_underwater_com_node.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 39c63f7..51a6b16 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -150,10 +150,12 @@ namespace provider_underwater_com 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_FLUSH: { ROS_INFO_STREAM("Buffer of sensor flushed"); + break; } default: { From fe48a836d66fc3968f95f5c0d672e232ef9a0957 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 27 Sep 2021 19:30:22 -0400 Subject: [PATCH 078/140] move define to sonia common --- src/provider_underwater_com_node.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 0e456c3..26e8d31 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -41,12 +41,9 @@ #include "Configuration.h" #include "drivers/serial.h" #include -//#include "ModemM64_definitions.h" #include #include "sharedQueue.h" -#define MALFORMED '!' - namespace provider_underwater_com { class ProviderUnderwaterComNode From 94d76af0e916aea438010ed72a6a7d37fbe67529 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 28 Sep 2021 01:23:28 +0000 Subject: [PATCH 079/140] add checkum for service --- src/provider_underwater_com_node.cc | 117 ++++++++++++++-------------- 1 file changed, 60 insertions(+), 57 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 51a6b16..930f846 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -101,70 +101,73 @@ namespace provider_underwater_com Read_for_Packet(buffer); writerQueue_mutex.unlock(); - - char cmd_rec = buffer[2]; - char tmp; - std::stringstream ss(buffer); - - switch (cmd_rec) + + if(ConfirmChecksum(buffer)) { - case CMD_GET_BUFFER_LENGTH: - { - std::string queue_length; - - std::getline(ss, queue_length, ','); - std::getline(ss, queue_length, '*'); + char cmd_rec = buffer[2]; + char tmp; + std::stringstream ss(buffer); - res.queue_length = std::stoi(queue_length); - break; - } - case CMD_GET_SETTINGS: + switch (cmd_rec) { - std::string role; - std::string channel; + case CMD_GET_BUFFER_LENGTH: + { + 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: + { + std::string role; + std::string channel; - std::getline(ss, role, ','); - std::getline(ss, role, ','); - std::getline(ss, channel, '*'); + std::getline(ss, role, ','); + std::getline(ss, role, ','); + std::getline(ss, channel, '*'); - tmp = role.at(0); - res.role = (uint8_t)tmp; - res.channel = std::stoi(channel); - break; - } - case CMD_GET_DIAGNOSTIC: - { - 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, '*'); - - tmp = link_up.at(0); - res.link = (uint8_t)tmp; - 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_FLUSH: - { - ROS_INFO_STREAM("Buffer of sensor flushed"); - break; - } - default: - { - ROS_ERROR("CMD received isn't working with the service."); - return false; + tmp = role.at(0); + res.role = (uint8_t)tmp; + res.channel = std::stoi(channel); + break; + } + case CMD_GET_DIAGNOSTIC: + { + 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, '*'); + + tmp = link_up.at(0); + res.link = (uint8_t)tmp; + 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_FLUSH: + { + ROS_INFO_STREAM("Buffer of sensor flushed"); + break; + } + default: + { + ROS_ERROR("CMD received isn't working with the service."); + return false; + } } + return true; } - - return true; + return false; } uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string &sentence, uint8_t length) From 2177a4b97ac9f8f2ab8031fb642ba0d18dad395f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 28 Sep 2021 18:23:24 +0000 Subject: [PATCH 080/140] change queue stream message --- src/provider_underwater_com_node.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 930f846..09b9b4e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -228,11 +228,11 @@ namespace provider_underwater_com AppendChecksum(sentence); writerQueue.push_back(sentence); - ROS_DEBUG_STREAM("Packet sent to Modem"); + ROS_INFO_STREAM("Packet sent to Modem"); } else { - ROS_INFO_STREAM("CMD unknow. Can't queue packet"); + ROS_WARN_STREAM("CMD unknow. Can't queue packet"); } } From 0b7f44df018286018cd2918ae87712a4d4e88113 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 28 Sep 2021 18:30:52 +0000 Subject: [PATCH 081/140] added extra sleep for slave read --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 09b9b4e..674a3c5 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -386,7 +386,7 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - ros::Duration(0.1).sleep(); + ros::Duration(2).sleep(); new_packet = Read_for_Packet(buffer); From 122843727e98329f229d7c2fe98bf275db6a69c8 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 28 Sep 2021 20:29:13 +0000 Subject: [PATCH 082/140] change sleep for the read packet --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 930f846..f23c33d 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -386,7 +386,7 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - ros::Duration(0.1).sleep(); + ros::Duration(1).sleep(); new_packet = Read_for_Packet(buffer); From 96729e091d5c066a6498427d7e8a20b5f469d380 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 16:15:24 +0000 Subject: [PATCH 083/140] add rate, slave cmd check & send_cmd functions --- src/provider_underwater_com_node.cc | 119 ++++++++++++++++------------ src/provider_underwater_com_node.h | 4 +- 2 files changed, 73 insertions(+), 50 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 32c3b8a..be71c1d 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -94,13 +94,7 @@ namespace provider_underwater_com { char buffer[BUFFER_SIZE]; - writerQueue_mutex.lock(); - - Queue_Packet(std::string(1, req.cmd)); - Transmit_Packet(true); - Read_for_Packet(buffer); - - writerQueue_mutex.unlock(); + Send_CMD_To_Sensor(buffer, (char)req.cmd); if(ConfirmChecksum(buffer)) { @@ -252,11 +246,11 @@ namespace provider_underwater_com } bool ProviderUnderwaterComNode::Read_for_Packet(char *buffer) - { - clock_t start = clock(); - clock_t now = clock(); - - while((now-start) / CLOCKS_PER_SEC <= timeout_) + { + ros::Rate r(2); // 2 Hz + uint8_t cycles = 0; + + while(cycles <= timeout_) { serialConnection_.readOnce(buffer, 0); @@ -272,13 +266,35 @@ namespace provider_underwater_com buffer[i] = 0; return true; } - - now = clock(); + ++cycles; + r.sleep(); } ROS_WARN_STREAM("No response after 10 secs."); return false; } + void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd) + { + writerQueue_mutex.lock(); + + Queue_Packet(std::string(1, cmd)); + Transmit_Packet(true); + Read_for_Packet(buffer); + + writerQueue_mutex.unlock(); + } + + void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet) + { + writerQueue_mutex.lock(); + + Queue_Packet(std::string(1, cmd), packet); + Transmit_Packet(true); + Read_for_Packet(buffer); + + writerQueue_mutex.unlock(); + } + bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) { const char *c_cmd = cmd.data(); @@ -298,13 +314,13 @@ namespace provider_underwater_com char buffer[BUFFER_SIZE]; bool new_packet; - if(resend_) Transmit_Packet(false); + if(!writerQueue.empty()) + { + if(resend_) Transmit_Packet(false); - new_packet = Read_for_Packet(buffer); + new_packet = Read_for_Packet(buffer); - if(new_packet) - { - if(ConfirmChecksum(buffer)) + if(new_packet && ConfirmChecksum(buffer)) { if(buffer[2] == RESP_GOT_PACKET) { @@ -324,42 +340,44 @@ namespace provider_underwater_com } } } - } void ProviderUnderwaterComNode::Manage_Packet_Slave() { - Export_To_ROS(readerQueue.get_n_pop_front()); - - while(writerQueue.empty()) + if(!readerQueue.empty()) { - ros::Duration(1).sleep(); - } + Export_To_ROS(readerQueue.get_n_pop_front()); - Transmit_Packet(true); + while(writerQueue.empty()) + { + ros::Duration(1).sleep(); + } + + Transmit_Packet(true); + } } void ProviderUnderwaterComNode::Manage_Packet() { + ros::Rate r(1); // 1 Hz + ROS_INFO_STREAM("Manage thread started"); while(!ros::isShuttingDown()) { - ros::Duration(1).sleep(); - writerQueue_mutex.lock(); - if(!writerQueue.empty() && role_ == ROLE_MASTER) + if(role_ == ROLE_MASTER) { Manage_Packet_Master(); } - - writerQueue_mutex.unlock(); - - if(!readerQueue.empty() && role_ == ROLE_SLAVE) + else if(role_ == ROLE_SLAVE) { Manage_Packet_Slave(); } + + writerQueue_mutex.unlock(); + r.sleep(); } } @@ -381,19 +399,30 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Read_for_Packet_Slave() { + ros::Rate r(1); // 1 Hz char buffer[BUFFER_SIZE]; bool new_packet = false; while(!ros::isShuttingDown()) - { - ros::Duration(1).sleep(); - + { new_packet = Read_for_Packet(buffer); if(new_packet && ConfirmChecksum(buffer)) { - readerQueue.push_back(buffer); + if(buffer[2] == RESP_GOT_PACKET) + { + readerQueue.push_back(buffer); + } + else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) + { + ROS_INFO_STREAM("Packet queue"); + } + else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) + { + ROS_ERROR_STREAM("Resquest not made properly"); + } } + r.sleep(); } } @@ -428,9 +457,7 @@ namespace provider_underwater_com std::string major_version = ""; char buffer[BUFFER_SIZE]; - Queue_Packet(std::string(1, CMD_GET_VERSION)); - Transmit_Packet(true); - Read_for_Packet(buffer); // TODO cmd and checksum check + Send_CMD_To_Sensor(buffer, CMD_GET_VERSION); std::stringstream ss(buffer); std::getline(ss, major_version, ','); @@ -448,9 +475,7 @@ namespace provider_underwater_com std::string payload = ""; char buffer[BUFFER_SIZE]; - Queue_Packet(std::string(1, CMD_GET_PAYLOAD_SIZE)); - Transmit_Packet(true); - Read_for_Packet(buffer); // TODO cmd check + Send_CMD_To_Sensor(buffer, CMD_GET_PAYLOAD_SIZE); std::stringstream ss(buffer); std::getline(ss, payload, ','); @@ -476,9 +501,7 @@ namespace provider_underwater_com sprintf(buffer, "%d", channel); std::string packet = "," + std::string(1, role) + "," + buffer; - Queue_Packet(std::string(1, CMD_SET_SETTINGS), packet); - Transmit_Packet(true); - Read_for_Packet(buffer); // TODO cmd check + Send_CMD_To_Sensor(buffer, CMD_SET_SETTINGS, packet); std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); @@ -496,9 +519,7 @@ namespace provider_underwater_com std::string acknowledge; char buffer[BUFFER_SIZE]; - Queue_Packet(std::string(1, CMD_FLUSH)); - Transmit_Packet(true); - Read_for_Packet(buffer); // TODO cmd and checksum check + Send_CMD_To_Sensor(buffer, CMD_FLUSH); std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 26e8d31..051f0be 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -67,6 +67,8 @@ class ProviderUnderwaterComNode void Queue_Packet(const std::string &cmd, const std::string &packet = ""); bool Transmit_Packet(bool pop_packet); bool Read_for_Packet(char *buffer); + void Send_CMD_To_Sensor(char *buffer, char cmd); + void Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet); bool Check_CMD(const std::string &cmd); void Manage_Packet_Master(); @@ -108,7 +110,7 @@ class ProviderUnderwaterComNode ros::Duration sleeptime; - float_t timeout_ = 10.0; + uint8_t timeout_ = 20; // 20 cycles }; } From f19d6521cf93cdf7303662567b746223f4c9a8ad Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 17:54:24 +0000 Subject: [PATCH 084/140] fix checksum with zero paddings --- src/provider_underwater_com_node.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index be71c1d..bc2978e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -182,7 +182,7 @@ namespace provider_underwater_com char buffer[2]; uint8_t checksum = CalculateChecksum(sentence, sentence.size()); - sprintf(buffer, "%x", checksum); + sprintf(buffer, "%02x", checksum); ss << sentence << std::string(1, CHECKSUM) << buffer << std::string(1, EOP); sentence = ss.str(); @@ -315,7 +315,7 @@ namespace provider_underwater_com bool new_packet; if(!writerQueue.empty()) - { + { if(resend_) Transmit_Packet(false); new_packet = Read_for_Packet(buffer); From 4aa4e7ad3cd30403998d395c8bbc878b15d90a7a Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 14:49:56 -0400 Subject: [PATCH 085/140] reduce ros_info quantity --- src/provider_underwater_com_node.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index bc2978e..49c59b2 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -150,12 +150,12 @@ namespace provider_underwater_com } case CMD_FLUSH: { - ROS_INFO_STREAM("Buffer of sensor flushed"); + ROS_DEBUG_STREAM("Flushed queue"); break; } default: { - ROS_ERROR("CMD received isn't working with the service."); + ROS_ERROR_STREAM("CMD received isn't working with the service. CMD received is %c", cmd_rec); return false; } } @@ -199,7 +199,7 @@ namespace provider_underwater_com } catch(...) { - ROS_INFO_STREAM("Underwater Com: bad checksum"); + ROS_WARN_STREAM("Underwater Com: bad checksum"); return false; } } @@ -222,7 +222,7 @@ namespace provider_underwater_com AppendChecksum(sentence); writerQueue.push_back(sentence); - ROS_INFO_STREAM("Packet sent to Modem"); + ROS_DEBUG_STREAM("Packet sent to Modem"); } else { @@ -269,7 +269,7 @@ namespace provider_underwater_com ++cycles; r.sleep(); } - ROS_WARN_STREAM("No response after 10 secs."); + ROS_DEBUG_STREAM("No response after 10 secs."); return false; } From 50ef42370fae0c26bfa04e9cec95452f26cbe18f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 14:53:05 -0400 Subject: [PATCH 086/140] fix error --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 49c59b2..08ec328 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -155,7 +155,7 @@ namespace provider_underwater_com } default: { - ROS_ERROR_STREAM("CMD received isn't working with the service. CMD received is %c", cmd_rec); + ROS_ERROR("CMD received isn't working with the service. CMD received is %c", cmd_rec); return false; } } From 6260d393c4036cfbae90bce8582d5397bb961612 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 20:41:02 +0000 Subject: [PATCH 087/140] add read mutex to stop the thread race --- src/provider_underwater_com_node.cc | 9 ++++++++- src/provider_underwater_com_node.h | 1 + 2 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 08ec328..6f50a4f 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -93,7 +93,7 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) { char buffer[BUFFER_SIZE]; - + Send_CMD_To_Sensor(buffer, (char)req.cmd); if(ConfirmChecksum(buffer)) @@ -276,23 +276,27 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd) { writerQueue_mutex.lock(); + readerQueue_mutex.lock(); Queue_Packet(std::string(1, cmd)); Transmit_Packet(true); Read_for_Packet(buffer); writerQueue_mutex.unlock(); + readerQueue_mutex.unlock(); } void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet) { writerQueue_mutex.lock(); + readerQueue_mutex.lock(); Queue_Packet(std::string(1, cmd), packet); Transmit_Packet(true); Read_for_Packet(buffer); writerQueue_mutex.unlock(); + readerQueue_mutex.unlock(); } bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) @@ -405,6 +409,8 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { + readerQueue_mutex.lock(); + new_packet = Read_for_Packet(buffer); if(new_packet && ConfirmChecksum(buffer)) @@ -422,6 +428,7 @@ namespace provider_underwater_com ROS_ERROR_STREAM("Resquest not made properly"); } } + readerQueue_mutex.unlock(); r.sleep(); } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 051f0be..217211b 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -97,6 +97,7 @@ class ProviderUnderwaterComNode std::thread read_for_packet_slave; std::mutex writerQueue_mutex; + std::mutex readerQueue_mutex; char role_; uint8_t channel_; From 3fa3962ca85103e99c2709fcac2ca892fa9b8082 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 22:40:14 +0000 Subject: [PATCH 088/140] change read thread --- src/provider_underwater_com_node.cc | 69 +++++++++++++++++++++++------ src/provider_underwater_com_node.h | 4 ++ 2 files changed, 59 insertions(+), 14 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 6f50a4f..805a6b5 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -39,6 +39,8 @@ namespace provider_underwater_com underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); + read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); + char *auv; auv = getenv("AUV"); @@ -53,10 +55,10 @@ namespace provider_underwater_com Set_Sensor(std::stoi(configuration_.getChannel())); - if(role_ == ROLE_SLAVE) + /*if(role_ == ROLE_SLAVE) { read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); - } + }*/ manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); @@ -250,7 +252,7 @@ namespace provider_underwater_com ros::Rate r(2); // 2 Hz uint8_t cycles = 0; - while(cycles <= timeout_) + while(cycles < timeout_) { serialConnection_.readOnce(buffer, 0); @@ -276,27 +278,51 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd) { writerQueue_mutex.lock(); - readerQueue_mutex.lock(); + //readerQueue_mutex.lock(); Queue_Packet(std::string(1, cmd)); Transmit_Packet(true); - Read_for_Packet(buffer); - + + std::unique_lock mlock(parseQueue_mutex); + parseQueue_cond.wait(mlock); + + if(!parseQueue.empty()) + { + std::string tmp = parseQueue.get_n_pop_front(); + + for(uint8_t i = 0; i < tmp.size(); ++i) + { + buffer[i] = tmp.at(i); + } + } + writerQueue_mutex.unlock(); - readerQueue_mutex.unlock(); + //readerQueue_mutex.unlock(); } void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet) { writerQueue_mutex.lock(); - readerQueue_mutex.lock(); + //readerQueue_mutex.lock(); Queue_Packet(std::string(1, cmd), packet); Transmit_Packet(true); - Read_for_Packet(buffer); + + std::unique_lock mlock(parseQueue_mutex); + parseQueue_cond.wait(mlock); + + if(!parseQueue.empty()) + { + std::string tmp = parseQueue.get_n_pop_front(); + + for(uint8_t i = 0; i < tmp.size(); ++i) + { + buffer[i] = tmp.at(i); + } + } writerQueue_mutex.unlock(); - readerQueue_mutex.unlock(); + //readerQueue_mutex.unlock(); } bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) @@ -322,10 +348,17 @@ namespace provider_underwater_com { if(resend_) Transmit_Packet(false); - new_packet = Read_for_Packet(buffer); + //new_packet = Read_for_Packet(buffer); - if(new_packet && ConfirmChecksum(buffer)) + if(!readerQueue.empty())) { + std::string tmp = readerQueue.get_n_pop_front(); + + for(uint8_t i = 0; i < tmp.size(); ++i) + { + buffer[i] = tmp.at(i); + } + if(buffer[2] == RESP_GOT_PACKET) { Export_To_ROS(buffer); @@ -409,7 +442,7 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - readerQueue_mutex.lock(); + //readerQueue_mutex.lock(); new_packet = Read_for_Packet(buffer); @@ -426,9 +459,17 @@ namespace provider_underwater_com else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) { ROS_ERROR_STREAM("Resquest not made properly"); + std::unique_lock mlock(parseQueue_mutex); + parseQueue_cond.notify_one(); + } + else + { + std::unique_lock mlock(parseQueue_mutex); + parseQueue.push_back(buffer); + parseQueue_cond.notify_one(); } } - readerQueue_mutex.unlock(); + //readerQueue_mutex.unlock(); r.sleep(); } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 217211b..4f3ce3d 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -98,6 +98,9 @@ class ProviderUnderwaterComNode std::mutex writerQueue_mutex; std::mutex readerQueue_mutex; + std::mutex parseQueue_mutex; + + std::condition_variable parseQueue_cond; char role_; uint8_t channel_; @@ -108,6 +111,7 @@ class ProviderUnderwaterComNode SharedQueue writerQueue; SharedQueue readerQueue; + SharedQueue parseQueue; ros::Duration sleeptime; From 91954da50494f6764de5385b9fd7b31f17a6433a Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 22:42:57 +0000 Subject: [PATCH 089/140] fix typo --- src/provider_underwater_com_node.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 805a6b5..74d0fdf 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -350,7 +350,7 @@ namespace provider_underwater_com //new_packet = Read_for_Packet(buffer); - if(!readerQueue.empty())) + if(!readerQueue.empty()) { std::string tmp = readerQueue.get_n_pop_front(); @@ -358,7 +358,7 @@ namespace provider_underwater_com { buffer[i] = tmp.at(i); } - + if(buffer[2] == RESP_GOT_PACKET) { Export_To_ROS(buffer); From c109fa813093aae152a295002185336ceb4ebecf Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 23:03:26 +0000 Subject: [PATCH 090/140] clean up the master send & receive --- src/provider_underwater_com_node.cc | 13 +------------ 1 file changed, 1 insertion(+), 12 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 74d0fdf..af4732e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -342,13 +342,12 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Manage_Packet_Master() { char buffer[BUFFER_SIZE]; - bool new_packet; if(!writerQueue.empty()) { if(resend_) Transmit_Packet(false); - //new_packet = Read_for_Packet(buffer); + resend_ = false; if(!readerQueue.empty()) { @@ -365,16 +364,6 @@ namespace provider_underwater_com writerQueue.pop_front(); resend_ = true; } - else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) - { - resend_ = false; - } - else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) - { - ROS_ERROR_STREAM("Resquest not made properly"); - writerQueue.pop_front(); - resend_ = true; - } } } } From 276701704e3dd1dc9353ee5e3db2cbbe1ccc80fe Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 23:27:17 +0000 Subject: [PATCH 091/140] test without mutex --- src/provider_underwater_com_node.cc | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index af4732e..1475871 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -277,7 +277,7 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd) { - writerQueue_mutex.lock(); + //writerQueue_mutex.lock(); //readerQueue_mutex.lock(); Queue_Packet(std::string(1, cmd)); @@ -296,13 +296,13 @@ namespace provider_underwater_com } } - writerQueue_mutex.unlock(); + //writerQueue_mutex.unlock(); //readerQueue_mutex.unlock(); } void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet) { - writerQueue_mutex.lock(); + //writerQueue_mutex.lock(); //readerQueue_mutex.lock(); Queue_Packet(std::string(1, cmd), packet); @@ -321,7 +321,7 @@ namespace provider_underwater_com } } - writerQueue_mutex.unlock(); + //writerQueue_mutex.unlock(); //readerQueue_mutex.unlock(); } @@ -391,7 +391,7 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - writerQueue_mutex.lock(); + //writerQueue_mutex.lock(); if(role_ == ROLE_MASTER) { @@ -402,7 +402,7 @@ namespace provider_underwater_com Manage_Packet_Slave(); } - writerQueue_mutex.unlock(); + //writerQueue_mutex.unlock(); r.sleep(); } } From 80fbe00b7702a16cd204e686bf2c7057591dc789 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 20:01:56 -0400 Subject: [PATCH 092/140] had sleep and redo for master --- src/provider_underwater_com_node.cc | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 1475871..b86a6fe 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -345,9 +345,7 @@ namespace provider_underwater_com if(!writerQueue.empty()) { - if(resend_) Transmit_Packet(false); - - resend_ = false; + Transmit_Packet(false); if(!readerQueue.empty()) { @@ -362,9 +360,9 @@ namespace provider_underwater_com { Export_To_ROS(buffer); writerQueue.pop_front(); - resend_ = true; } } + ros::Duration(5).sleep(); } } From 15833821db2ff49aab8faefcd206cc264da0453c Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 20:03:02 -0400 Subject: [PATCH 093/140] redo the command if no answer --- src/provider_underwater_com_node.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index b86a6fe..75318e5 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -362,7 +362,6 @@ namespace provider_underwater_com writerQueue.pop_front(); } } - ros::Duration(5).sleep(); } } From 4894eb7248296bf16f6550ec628741b47fe40693 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 29 Sep 2021 20:15:21 -0400 Subject: [PATCH 094/140] add send for master --- src/provider_underwater_com_node.cc | 8 ++++++-- src/provider_underwater_com_node.h | 2 +- 2 files changed, 7 insertions(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 75318e5..f104ec2 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -345,7 +345,9 @@ namespace provider_underwater_com if(!writerQueue.empty()) { - Transmit_Packet(false); + if(send_) Transmit_Packet(false); + + send_ = false; if(!readerQueue.empty()) { @@ -359,8 +361,10 @@ namespace provider_underwater_com if(buffer[2] == RESP_GOT_PACKET) { Export_To_ROS(buffer); - writerQueue.pop_front(); } + + writerQueue.pop_front(); + send_ = true; } } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 4f3ce3d..56393f8 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -107,7 +107,7 @@ class ProviderUnderwaterComNode uint8_t payload_; bool init_completed_ = true; bool init_error_ = true; - bool resend_ = true; + bool send_ = true; SharedQueue writerQueue; SharedQueue readerQueue; From db5250f1054cb128c5b1894d6415e512d7549c4b Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 1 Oct 2021 17:24:05 +0000 Subject: [PATCH 095/140] code cleanup --- src/provider_underwater_com_node.cc | 19 ------------------- 1 file changed, 19 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index f104ec2..62730aa 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -277,9 +277,6 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd) { - //writerQueue_mutex.lock(); - //readerQueue_mutex.lock(); - Queue_Packet(std::string(1, cmd)); Transmit_Packet(true); @@ -295,16 +292,10 @@ namespace provider_underwater_com buffer[i] = tmp.at(i); } } - - //writerQueue_mutex.unlock(); - //readerQueue_mutex.unlock(); } void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet) { - //writerQueue_mutex.lock(); - //readerQueue_mutex.lock(); - Queue_Packet(std::string(1, cmd), packet); Transmit_Packet(true); @@ -320,9 +311,6 @@ namespace provider_underwater_com buffer[i] = tmp.at(i); } } - - //writerQueue_mutex.unlock(); - //readerQueue_mutex.unlock(); } bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) @@ -392,8 +380,6 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - //writerQueue_mutex.lock(); - if(role_ == ROLE_MASTER) { Manage_Packet_Master(); @@ -402,8 +388,6 @@ namespace provider_underwater_com { Manage_Packet_Slave(); } - - //writerQueue_mutex.unlock(); r.sleep(); } } @@ -432,8 +416,6 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - //readerQueue_mutex.lock(); - new_packet = Read_for_Packet(buffer); if(new_packet && ConfirmChecksum(buffer)) @@ -459,7 +441,6 @@ namespace provider_underwater_com parseQueue_cond.notify_one(); } } - //readerQueue_mutex.unlock(); r.sleep(); } } From 659581997e81fb12d97fad82a75d89f127834aa7 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 1 Oct 2021 17:33:30 +0000 Subject: [PATCH 096/140] added debug and checks to avoid deadlocks --- src/provider_underwater_com_node.cc | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 62730aa..3406c27 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -334,6 +334,7 @@ namespace provider_underwater_com if(!writerQueue.empty()) { if(send_) Transmit_Packet(false); + if(send_) ROS_INFO_STREAM("Packet has been sent. Now waiting for a reply"); send_ = false; @@ -345,12 +346,10 @@ namespace provider_underwater_com { buffer[i] = tmp.at(i); } - - if(buffer[2] == RESP_GOT_PACKET) - { - Export_To_ROS(buffer); - } - + + if(buffer[2] != RESP_GOT_PACKET) ROS_INFO_STREAM("Packet received isn't a response"); + else Export_To_ROS(buffer); + writerQueue.pop_front(); send_ = true; } @@ -365,6 +364,7 @@ namespace provider_underwater_com while(writerQueue.empty()) { + ROS_INFO_STREAM("Waiting for packet to send back"); ros::Duration(1).sleep(); } @@ -420,6 +420,8 @@ namespace provider_underwater_com if(new_packet && ConfirmChecksum(buffer)) { + ROS_INFO("Cmd received is %c", buffer[2]); + if(buffer[2] == RESP_GOT_PACKET) { readerQueue.push_back(buffer); From c7baa32f6080f3f5aaa2af8466bbbec9404326f7 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 1 Oct 2021 18:32:34 +0000 Subject: [PATCH 097/140] added empty queues --- src/provider_underwater_com_node.cc | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 3406c27..dfe0fbf 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -55,10 +55,6 @@ namespace provider_underwater_com Set_Sensor(std::stoi(configuration_.getChannel())); - /*if(role_ == ROLE_SLAVE) - { - read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); - }*/ manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); @@ -148,6 +144,17 @@ namespace provider_underwater_com 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); + + if(tmp == LINK_DOWN) + { + Flush_Queue(); + while(!writerQueue.empty()) + { + writerQueue.pop_front(); + } + ROS_INFO_STREAM("Emptying the write queue"); + } + break; } case CMD_FLUSH: From e0b0c9ae79c60ef9ada436800de32a2a5f70dce1 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 1 Oct 2021 18:53:34 +0000 Subject: [PATCH 098/140] increase rate --- src/provider_underwater_com_node.cc | 12 +++++------- 1 file changed, 5 insertions(+), 7 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index dfe0fbf..20ed101 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -71,7 +71,7 @@ namespace provider_underwater_com //Node Spin void ProviderUnderwaterComNode::Spin() { - ros::Rate r(1); // 1 Hz + ros::Rate r(5); // 5 Hz while(ros::ok() && init_completed_ == true) { @@ -148,11 +148,9 @@ namespace provider_underwater_com if(tmp == LINK_DOWN) { Flush_Queue(); - while(!writerQueue.empty()) - { - writerQueue.pop_front(); - } - ROS_INFO_STREAM("Emptying the write queue"); + while(!writerQueue.empty()) writerQueue.pop_front(); + while(!readerQueue.empty()) readerQueue.pop_front(); + ROS_INFO_STREAM("Emptying the queues"); } break; @@ -435,7 +433,7 @@ namespace provider_underwater_com } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) { - ROS_INFO_STREAM("Packet queue"); + ROS_DEBUG_STREAM("Packet queue"); } else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) { From 347e6ef60e5a9677e577a9e542b91bc685303de9 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 1 Oct 2021 19:21:46 +0000 Subject: [PATCH 099/140] test with master sending message once --- src/provider_underwater_com_node.cc | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 20ed101..1224e99 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -338,10 +338,7 @@ namespace provider_underwater_com if(!writerQueue.empty()) { - if(send_) Transmit_Packet(false); - if(send_) ROS_INFO_STREAM("Packet has been sent. Now waiting for a reply"); - - send_ = false; + Transmit_Packet(true); if(!readerQueue.empty()) { @@ -354,9 +351,6 @@ namespace provider_underwater_com if(buffer[2] != RESP_GOT_PACKET) ROS_INFO_STREAM("Packet received isn't a response"); else Export_To_ROS(buffer); - - writerQueue.pop_front(); - send_ = true; } } } @@ -425,7 +419,7 @@ namespace provider_underwater_com if(new_packet && ConfirmChecksum(buffer)) { - ROS_INFO("Cmd received is %c", buffer[2]); + //ROS_INFO("Cmd received is %c", buffer[2]); if(buffer[2] == RESP_GOT_PACKET) { From 37d9e795b6b1d2dbe44310699b75d92e9a090afc Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 1 Oct 2021 19:26:22 +0000 Subject: [PATCH 100/140] nothing to say --- src/provider_underwater_com_node.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 1224e99..99d3a51 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -336,6 +336,8 @@ namespace provider_underwater_com { char buffer[BUFFER_SIZE]; + ROS_INFO("Write Queue Size : %d", writerQueue.size()); + if(!writerQueue.empty()) { Transmit_Packet(true); From dc3d408735f5665bec3f0346f92620362812f72a Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Fri, 1 Oct 2021 15:36:47 -0400 Subject: [PATCH 101/140] fix workflow feature --- .github/workflows/docker-image-perception-feature.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 From 65ecf0186548a3b6e881380adbba37868c91ee3c Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 23 Oct 2021 21:57:24 +0000 Subject: [PATCH 102/140] change to simple driver --- .devcontainer/devcontainer.json | 1 - config/AUV7.yaml | 7 - config/{AUV8.yaml => default.yaml} | 0 launch/provider_underwater_com.launch | 2 +- src/drivers/serial.cc | 2 +- src/provider_underwater_com_node.cc | 257 +++++++++++++------------- src/provider_underwater_com_node.h | 30 +-- 7 files changed, 152 insertions(+), 147 deletions(-) delete mode 100644 config/AUV7.yaml rename config/{AUV8.yaml => default.yaml} (100%) diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index 24ee7c0..09797ed 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -20,7 +20,6 @@ "seccomp=unconfined", "--rm", "--privileged", - "--env", "AUV=AUV8", "--network", "host" ], diff --git a/config/AUV7.yaml b/config/AUV7.yaml deleted file mode 100644 index 5a5c9f0..0000000 --- a/config/AUV7.yaml +++ /dev/null @@ -1,7 +0,0 @@ -#********** Driver Parameters ************ - -/provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" - -/provider_underwater_com/settings/role : "slave" - -/provider_underwater_com/settings/channel : "4" \ No newline at end of file diff --git a/config/AUV8.yaml b/config/default.yaml similarity index 100% rename from config/AUV8.yaml rename to config/default.yaml diff --git a/launch/provider_underwater_com.launch b/launch/provider_underwater_com.launch index df586ae..60ead1c 100644 --- a/launch/provider_underwater_com.launch +++ b/launch/provider_underwater_com.launch @@ -3,6 +3,6 @@ - + \ No newline at end of file diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index d600012..cadc633 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -30,7 +30,7 @@ Serial::Serial(std::string port) { - fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + fd = open(port.c_str(), O_RDWR | O_NOCTTY); if(fd == -1) { ROS_ERROR("unable to connect to %s", port.c_str()); diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 99d3a51..fec7b49 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -66,6 +66,8 @@ namespace provider_underwater_com manage_thread.~thread(); read_for_packet_slave.~thread(); underwaterComSubscriber_.shutdown(); + underwaterComPublisher_.shutdown(); + underwaterComService_.shutdown(); } //Node Spin @@ -94,81 +96,75 @@ namespace provider_underwater_com Send_CMD_To_Sensor(buffer, (char)req.cmd); - if(ConfirmChecksum(buffer)) + char cmd_rec = buffer[2]; + char tmp; + std::stringstream ss(buffer); + + switch (cmd_rec) { - char cmd_rec = buffer[2]; - char tmp; - std::stringstream ss(buffer); + case CMD_GET_BUFFER_LENGTH: + { + std::string queue_length; + + std::getline(ss, queue_length, ','); + std::getline(ss, queue_length, '*'); - switch (cmd_rec) + res.queue_length = std::stoi(queue_length); + break; + } + case CMD_GET_SETTINGS: { - case CMD_GET_BUFFER_LENGTH: - { - 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: - { - std::string role; - std::string channel; + std::string role; + std::string channel; - std::getline(ss, role, ','); - std::getline(ss, role, ','); - std::getline(ss, channel, '*'); + std::getline(ss, role, ','); + std::getline(ss, role, ','); + std::getline(ss, channel, '*'); - tmp = role.at(0); - res.role = (uint8_t)tmp; - res.channel = std::stoi(channel); - break; - } - case CMD_GET_DIAGNOSTIC: - { - 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, '*'); - - tmp = link_up.at(0); - res.link = (uint8_t)tmp; - 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); - - if(tmp == LINK_DOWN) - { - Flush_Queue(); - while(!writerQueue.empty()) writerQueue.pop_front(); - while(!readerQueue.empty()) readerQueue.pop_front(); - ROS_INFO_STREAM("Emptying the queues"); - } - - break; - } - case CMD_FLUSH: - { - ROS_DEBUG_STREAM("Flushed queue"); - break; - } - default: + tmp = role.at(0); + res.role = (uint8_t)tmp; + res.channel = std::stoi(channel); + break; + } + case CMD_GET_DIAGNOSTIC: + { + 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, '*'); + + tmp = link_up.at(0); + res.link = (uint8_t)tmp; + 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); + + if(tmp == LINK_DOWN) { - ROS_ERROR("CMD received isn't working with the service. CMD received is %c", cmd_rec); - return false; + Flush_Queue(); + ROS_INFO_STREAM("Emptying the queues"); } + + break; + } + case CMD_FLUSH: + { + ROS_DEBUG_STREAM("Flushed queue"); + break; + } + default: + { + ROS_ERROR("CMD received isn't working with the service. CMD received is %c", cmd_rec); + return false; } - return true; } - return false; + return true; } uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string &sentence, uint8_t length) @@ -227,7 +223,10 @@ namespace provider_underwater_com sentence = ss.str(); AppendChecksum(sentence); - writerQueue.push_back(sentence); + + std::unique_lock mlock(write_mutex); + write_string = sentence; + write_cond.notify_one(); ROS_DEBUG_STREAM("Packet sent to Modem"); } @@ -239,10 +238,10 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Transmit_Packet(bool pop_packet) { - if(!writerQueue.empty()) + if(!write_string.empty()) { - serialConnection_.transmit(writerQueue.front()); - if(pop_packet) writerQueue.pop_front(); + serialConnection_.transmit(write_string); + if(pop_packet) write_string.clear(); return true; } else @@ -252,7 +251,7 @@ namespace provider_underwater_com } } - bool ProviderUnderwaterComNode::Read_for_Packet(char *buffer) + /*void ProviderUnderwaterComNode::Read_for_Packet(char *buffer) { ros::Rate r(2); // 2 Hz uint8_t cycles = 0; @@ -278,43 +277,26 @@ namespace provider_underwater_com } ROS_DEBUG_STREAM("No response after 10 secs."); return false; - } - - void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd) - { - Queue_Packet(std::string(1, cmd)); - Transmit_Packet(true); - - std::unique_lock mlock(parseQueue_mutex); - parseQueue_cond.wait(mlock); - - if(!parseQueue.empty()) - { - std::string tmp = parseQueue.get_n_pop_front(); - - for(uint8_t i = 0; i < tmp.size(); ++i) - { - buffer[i] = tmp.at(i); - } - } - } + }*/ - void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet) + void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) { Queue_Packet(std::string(1, cmd), packet); Transmit_Packet(true); - std::unique_lock mlock(parseQueue_mutex); - parseQueue_cond.wait(mlock); + std::unique_lock mlock(parse_mutex); + parse_cond.wait(mlock); - if(!parseQueue.empty()) + if(!parse_string.empty()) { - std::string tmp = parseQueue.get_n_pop_front(); + std::string tmp = parse_string; for(uint8_t i = 0; i < tmp.size(); ++i) { buffer[i] = tmp.at(i); } + + parse_string.clear(); } } @@ -332,11 +314,9 @@ namespace provider_underwater_com return false; } - void ProviderUnderwaterComNode::Manage_Packet_Master() + /*void ProviderUnderwaterComNode::Manage_Packet_Master() { char buffer[BUFFER_SIZE]; - - ROS_INFO("Write Queue Size : %d", writerQueue.size()); if(!writerQueue.empty()) { @@ -371,7 +351,7 @@ namespace provider_underwater_com Transmit_Packet(true); } - } + }*/ void ProviderUnderwaterComNode::Manage_Packet() { @@ -381,18 +361,34 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - if(role_ == ROLE_MASTER) - { - Manage_Packet_Master(); - } - else if(role_ == ROLE_SLAVE) - { - Manage_Packet_Slave(); - } + Manage_Packet_2(); r.sleep(); } } + void ProviderUnderwaterComNode::Manage_Packet_2() // Faire des threads séparés pour les 2 + { + char buffer[BUFFER_SIZE]; + + if(!write_string.empty()) + { + Transmit_Packet(true); + } + if(!read_string.empty()) + { + std::string tmp = read_string; + read_string = ""; + + for(uint8_t i = 0; i < tmp.size(); ++i) + { + buffer[i] = tmp.at(i); + } + + if(buffer[2] != RESP_GOT_PACKET) ROS_WARN_STREAM("Packet received isn't a response"); + else Export_To_ROS(buffer); + } + } + void ProviderUnderwaterComNode::Export_To_ROS(std::string buffer) { std::string size; @@ -411,21 +407,33 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Read_for_Packet_Slave() { - ros::Rate r(1); // 1 Hz + uint8_t i; char buffer[BUFFER_SIZE]; - bool new_packet = false; while(!ros::isShuttingDown()) { - new_packet = Read_for_Packet(buffer); + do + { + serialConnection_.readOnce(buffer, 0); + } while(buffer[0] != SOP); - if(new_packet && ConfirmChecksum(buffer)) + for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) { - //ROS_INFO("Cmd received is %c", buffer[2]); - + serialConnection_. readOnce(buffer, i); + } + + buffer[i] = 0; + + if(i > BUFFER_SIZE) + { + continue; + } + + if(ConfirmChecksum(buffer)) + { if(buffer[2] == RESP_GOT_PACKET) { - readerQueue.push_back(buffer); + read_string = std::string(buffer); } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) { @@ -434,17 +442,16 @@ namespace provider_underwater_com else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) { ROS_ERROR_STREAM("Resquest not made properly"); - std::unique_lock mlock(parseQueue_mutex); - parseQueue_cond.notify_one(); + std::unique_lock mlock(parse_mutex); + parse_cond.notify_one(); } else { - std::unique_lock mlock(parseQueue_mutex); - parseQueue.push_back(buffer); - parseQueue_cond.notify_one(); + std::unique_lock mlock(parse_mutex); + parse_string = std::string(buffer); + parse_cond.notify_one(); } } - r.sleep(); } } @@ -485,7 +492,7 @@ namespace provider_underwater_com std::getline(ss, major_version, ','); std::getline(ss, major_version, ','); - if(major_version != "1" && !ConfirmChecksum(buffer)) + if(major_version != "1") { ROS_ERROR_STREAM("Major Version isn't 1. Restarting init"); init_error_ = true; @@ -503,7 +510,7 @@ namespace provider_underwater_com std::getline(ss, payload, ','); std::getline(ss, payload, '*'); - if((payload >= "0" || payload <= "9") && ConfirmChecksum(buffer)) + if((payload >= "0" || payload <= "9")) { payload_ = std::stoi(payload); ROS_DEBUG_STREAM("Payload set"); @@ -529,7 +536,7 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK) && !ConfirmChecksum(buffer)) + if(acknowledge == std::string(1, NAK)) { ROS_ERROR_STREAM("Could not set the configuration. Restarting init"); init_error_ = true; @@ -547,7 +554,7 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK) && !ConfirmChecksum(buffer)) + if(acknowledge == std::string(1, NAK)) { ROS_ERROR_STREAM("Couldn't flush the queue. Restarting init"); init_error_ = true; diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 56393f8..495579a 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -66,15 +66,15 @@ class ProviderUnderwaterComNode void Queue_Packet(const std::string &cmd, const std::string &packet = ""); bool Transmit_Packet(bool pop_packet); - bool Read_for_Packet(char *buffer); - void Send_CMD_To_Sensor(char *buffer, char cmd); - void Send_CMD_To_Sensor(char *buffer, char cmd, std::string &packet); + //bool Read_for_Packet(char *buffer); + void Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet = ""); bool Check_CMD(const std::string &cmd); - void Manage_Packet_Master(); - void Manage_Packet_Slave(); + //void Manage_Packet_Master(); + //void Manage_Packet_Slave(); void Manage_Packet(); + void Manage_Packet_2(); void Export_To_ROS(std::string buffer); void Read_for_Packet_Slave(); @@ -96,11 +96,17 @@ class ProviderUnderwaterComNode std::thread manage_thread; std::thread read_for_packet_slave; - std::mutex writerQueue_mutex; - std::mutex readerQueue_mutex; - std::mutex parseQueue_mutex; + std::mutex write_mutex; + std::mutex read_mutex; + std::mutex parse_mutex; - std::condition_variable parseQueue_cond; + std::condition_variable write_cond; + std::condition_variable read_cond; + std::condition_variable parse_cond; + + std::string write_string = ""; + std::string read_string = ""; + std::string parse_string = ""; char role_; uint8_t channel_; @@ -109,9 +115,9 @@ class ProviderUnderwaterComNode bool init_error_ = true; bool send_ = true; - SharedQueue writerQueue; - SharedQueue readerQueue; - SharedQueue parseQueue; + //SharedQueue writerQueue; + //SharedQueue readerQueue; + //SharedQueue parseQueue; ros::Duration sleeptime; From a6b7f2d2df4581bda899e4a6f442cf7bab5d3339 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sat, 23 Oct 2021 22:18:01 +0000 Subject: [PATCH 103/140] change init functions --- config/default.yaml | 2 +- src/provider_underwater_com_node.cc | 20 ++++---------------- src/provider_underwater_com_node.h | 4 ++-- 3 files changed, 7 insertions(+), 19 deletions(-) diff --git a/config/default.yaml b/config/default.yaml index c6795a7..28d07ce 100644 --- a/config/default.yaml +++ b/config/default.yaml @@ -2,6 +2,6 @@ /provider_underwater_com/connection/tty_port: "/dev/ttyUSB0" -/provider_underwater_com/settings/role : "master" +/provider_underwater_com/settings/role : "a" /provider_underwater_com/settings/channel : "4" \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index fec7b49..0fab236 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -41,19 +41,7 @@ namespace provider_underwater_com read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); - char *auv; - auv = getenv("AUV"); - - if(strcmp(auv, "AUV7") == 0) - { - role_ = ROLE_SLAVE; - } - else - { - role_ = ROLE_MASTER; - } - - Set_Sensor(std::stoi(configuration_.getChannel())); + Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); @@ -455,7 +443,7 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Set_Sensor(uint8_t channel) + void ProviderUnderwaterComNode::Set_Sensor(const char role, const uint8_t channel) { uint8_t i = 0; @@ -464,7 +452,7 @@ namespace provider_underwater_com init_error_ = false; Verify_Version(); Get_Payload_Load(); - Set_Configuration(role_, channel); + Set_Configuration(role, channel); Flush_Queue(); ++i; } @@ -522,7 +510,7 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Set_Configuration(const char &role, uint8_t channel) + void ProviderUnderwaterComNode::Set_Configuration(const char role, const uint8_t channel) { std::string acknowledge; char buffer[BUFFER_SIZE]; diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 495579a..9a2c2af 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -78,10 +78,10 @@ class ProviderUnderwaterComNode void Export_To_ROS(std::string buffer); void Read_for_Packet_Slave(); - void Set_Sensor(uint8_t channel = 4); + void Set_Sensor(const char role, const uint8_t channel = 4); void Verify_Version(); void Get_Payload_Load(); - void Set_Configuration(const char &role, uint8_t channel); + void Set_Configuration(const char role, const uint8_t channel); void Flush_Queue(); ros::NodeHandlePtr nh_; From 7d04e032980cc1effc7864d1547984cb0493db9e Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 24 Oct 2021 14:14:40 +0000 Subject: [PATCH 104/140] switch to string instead of shared queue --- src/provider_underwater_com_node.cc | 179 ++++++++-------------------- src/provider_underwater_com_node.h | 48 +++----- src/sharedQueue.h | 142 ---------------------- 3 files changed, 62 insertions(+), 307 deletions(-) delete mode 100644 src/sharedQueue.h diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 0fab236..7ba09cb 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -39,20 +39,21 @@ namespace provider_underwater_com underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); - read_for_packet_slave = std::thread(std::bind(&ProviderUnderwaterComNode::Read_for_Packet_Slave, this)); + manage_write_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Write, this)); + manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); + read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); - manage_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Packet, this)); - underwaterComService_ = nh_->advertiseService("/provider_underwater_com/request", &ProviderUnderwaterComNode::UnderwaterComService, this); } //Node Destructor ProviderUnderwaterComNode::~ProviderUnderwaterComNode() { - manage_thread.~thread(); - read_for_packet_slave.~thread(); + manage_write_thread.~thread(); + manage_response_thread.~thread(); + read_packet_thread.~thread(); underwaterComSubscriber_.shutdown(); underwaterComPublisher_.shutdown(); underwaterComService_.shutdown(); @@ -63,12 +64,11 @@ namespace provider_underwater_com { ros::Rate r(5); // 5 Hz - while(ros::ok() && init_completed_ == true) + while(ros::ok() && init_error_ == false) { ros::spinOnce(); r.sleep(); } - ros::shutdown(); } @@ -132,18 +132,12 @@ namespace provider_underwater_com 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); - - if(tmp == LINK_DOWN) - { - Flush_Queue(); - ROS_INFO_STREAM("Emptying the queues"); - } - break; } case CMD_FLUSH: { - ROS_DEBUG_STREAM("Flushed queue"); + ROS_INFO_STREAM("Flushed queue"); + Flush_Queue(); break; } default: @@ -163,7 +157,7 @@ namespace provider_underwater_com { check = crc_table[(uint8_t)sentence.at(i) ^ check]; } - + return check; } @@ -200,7 +194,7 @@ namespace provider_underwater_com std::stringstream ss; std::string sentence; - if(Check_CMD(cmd)) + if(Check_CMD(cmd.data())) { ss << SOP << DIR_CMD << cmd; @@ -239,38 +233,10 @@ namespace provider_underwater_com } } - /*void ProviderUnderwaterComNode::Read_for_Packet(char *buffer) - { - ros::Rate r(2); // 2 Hz - uint8_t cycles = 0; - - while(cycles < timeout_) - { - serialConnection_.readOnce(buffer, 0); - - if(buffer[0] == SOP) - { - uint8_t i; - - for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) - { - serialConnection_. readOnce(buffer, i); - } - - buffer[i] = 0; - return true; - } - ++cycles; - r.sleep(); - } - ROS_DEBUG_STREAM("No response after 10 secs."); - return false; - }*/ - void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) { Queue_Packet(std::string(1, cmd), packet); - Transmit_Packet(true); + //Transmit_Packet(true); std::unique_lock mlock(parse_mutex); parse_cond.wait(mlock); @@ -288,13 +254,11 @@ namespace provider_underwater_com } } - bool ProviderUnderwaterComNode::Check_CMD(const std::string &cmd) + bool ProviderUnderwaterComNode::Check_CMD(const char *cmd) { - const char *c_cmd = cmd.data(); - for(uint8_t i = 0; i < all_valid_size; ++i) { - if(c_cmd[0] == all_valid[i]) + if(cmd[0] == all_valid[i]) { return true; } @@ -302,78 +266,28 @@ namespace provider_underwater_com return false; } - /*void ProviderUnderwaterComNode::Manage_Packet_Master() - { - char buffer[BUFFER_SIZE]; - - if(!writerQueue.empty()) - { - Transmit_Packet(true); - - if(!readerQueue.empty()) - { - std::string tmp = readerQueue.get_n_pop_front(); - - for(uint8_t i = 0; i < tmp.size(); ++i) - { - buffer[i] = tmp.at(i); - } - - if(buffer[2] != RESP_GOT_PACKET) ROS_INFO_STREAM("Packet received isn't a response"); - else Export_To_ROS(buffer); - } - } - } - - void ProviderUnderwaterComNode::Manage_Packet_Slave() - { - if(!readerQueue.empty()) - { - Export_To_ROS(readerQueue.get_n_pop_front()); - - while(writerQueue.empty()) - { - ROS_INFO_STREAM("Waiting for packet to send back"); - ros::Duration(1).sleep(); - } - - Transmit_Packet(true); - } - }*/ - - void ProviderUnderwaterComNode::Manage_Packet() - { - ros::Rate r(1); // 1 Hz - - ROS_INFO_STREAM("Manage thread started"); + void ProviderUnderwaterComNode::Manage_Write() + { + ROS_INFO_STREAM("Manage write thread started"); while(!ros::isShuttingDown()) { - Manage_Packet_2(); - r.sleep(); + std::unique_lock mlock(write_mutex); + write_cond.wait(mlock); + Transmit_Packet(true); } } - void ProviderUnderwaterComNode::Manage_Packet_2() // Faire des threads séparés pour les 2 + void ProviderUnderwaterComNode::Manage_Response() { - char buffer[BUFFER_SIZE]; + ROS_INFO_STREAM("Manage response thread started"); - if(!write_string.empty()) - { - Transmit_Packet(true); - } - if(!read_string.empty()) + while (!ros::isShuttingDown()) { - std::string tmp = read_string; - read_string = ""; - - for(uint8_t i = 0; i < tmp.size(); ++i) - { - buffer[i] = tmp.at(i); - } - - if(buffer[2] != RESP_GOT_PACKET) ROS_WARN_STREAM("Packet received isn't a response"); - else Export_To_ROS(buffer); + std::unique_lock mlock(response_mutex); + response_cond.wait(mlock); + Export_To_ROS(response_string); + response_string.clear(); } } @@ -393,7 +307,7 @@ namespace provider_underwater_com underwaterComPublisher_.publish(msg_received); } - void ProviderUnderwaterComNode::Read_for_Packet_Slave() + void ProviderUnderwaterComNode::Read_Packet() { uint8_t i; char buffer[BUFFER_SIZE]; @@ -421,11 +335,13 @@ namespace provider_underwater_com { if(buffer[2] == RESP_GOT_PACKET) { - read_string = std::string(buffer); + std::unique_lock mlock(response_mutex); + response_string = std::string(buffer); + response_cond.notify_one(); } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) { - ROS_DEBUG_STREAM("Packet queue"); + ROS_INFO_STREAM("Packet queue"); } else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) { @@ -450,26 +366,21 @@ namespace provider_underwater_com while(i < 3 && init_error_ == true) { init_error_ = false; - Verify_Version(); - Get_Payload_Load(); - Set_Configuration(role, channel); - Flush_Queue(); + init_error_ = Verify_Version() | Get_Payload_Load() | Set_Configuration(role, channel) | Flush_Queue(); ++i; } - if(i == 3) + if(init_error_) { ROS_ERROR_STREAM("Problem with the init. Node shutting down."); - init_completed_ = false; } else { - init_completed_ = true; ROS_INFO_STREAM("Initialisation completed"); } } - void ProviderUnderwaterComNode::Verify_Version() + bool ProviderUnderwaterComNode::Verify_Version() { std::string major_version = ""; char buffer[BUFFER_SIZE]; @@ -483,11 +394,12 @@ namespace provider_underwater_com if(major_version != "1") { ROS_ERROR_STREAM("Major Version isn't 1. Restarting init"); - init_error_ = true; + return true; } + return false; } - void ProviderUnderwaterComNode::Get_Payload_Load() + bool ProviderUnderwaterComNode::Get_Payload_Load() { std::string payload = ""; char buffer[BUFFER_SIZE]; @@ -501,16 +413,17 @@ namespace provider_underwater_com if((payload >= "0" || payload <= "9")) { payload_ = std::stoi(payload); - ROS_DEBUG_STREAM("Payload set"); + ROS_INFO_STREAM("Payload set"); + return false; } else { ROS_ERROR_STREAM("Payload isn't a integer. Restarting init"); - init_error_ = true; + return true; } } - void ProviderUnderwaterComNode::Set_Configuration(const char role, const uint8_t channel) + bool ProviderUnderwaterComNode::Set_Configuration(const char role, const uint8_t channel) { std::string acknowledge; char buffer[BUFFER_SIZE]; @@ -527,11 +440,12 @@ namespace provider_underwater_com if(acknowledge == std::string(1, NAK)) { ROS_ERROR_STREAM("Could not set the configuration. Restarting init"); - init_error_ = true; + return true; } + return false; } - void ProviderUnderwaterComNode::Flush_Queue() + bool ProviderUnderwaterComNode::Flush_Queue() { std::string acknowledge; char buffer[BUFFER_SIZE]; @@ -545,7 +459,8 @@ namespace provider_underwater_com if(acknowledge == std::string(1, NAK)) { ROS_ERROR_STREAM("Couldn't flush the queue. Restarting init"); - init_error_ = true; + return true; } + return false; } } \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 9a2c2af..6e7bc92 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -35,14 +35,11 @@ #include #include #include -#include -#include #include "Configuration.h" #include "drivers/serial.h" #include #include -#include "sharedQueue.h" namespace provider_underwater_com { @@ -66,23 +63,19 @@ class ProviderUnderwaterComNode void Queue_Packet(const std::string &cmd, const std::string &packet = ""); bool Transmit_Packet(bool pop_packet); - //bool Read_for_Packet(char *buffer); void Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet = ""); - bool Check_CMD(const std::string &cmd); + bool Check_CMD(const char *cmd); - //void Manage_Packet_Master(); - //void Manage_Packet_Slave(); - - void Manage_Packet(); - void Manage_Packet_2(); + void Manage_Write(); + void Manage_Response(); void Export_To_ROS(std::string buffer); - void Read_for_Packet_Slave(); + void Read_Packet(); void Set_Sensor(const char role, const uint8_t channel = 4); - void Verify_Version(); - void Get_Payload_Load(); - void Set_Configuration(const char role, const uint8_t channel); - void Flush_Queue(); + 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_; @@ -93,35 +86,24 @@ class ProviderUnderwaterComNode ros::ServiceServer underwaterComService_; std_msgs::String msg_received; - std::thread manage_thread; - std::thread read_for_packet_slave; + std::thread manage_write_thread; + std::thread manage_response_thread; + std::thread read_packet_thread; std::mutex write_mutex; - std::mutex read_mutex; + std::mutex response_mutex; std::mutex parse_mutex; std::condition_variable write_cond; - std::condition_variable read_cond; + std::condition_variable response_cond; std::condition_variable parse_cond; std::string write_string = ""; - std::string read_string = ""; + std::string response_string = ""; std::string parse_string = ""; - - char role_; - uint8_t channel_; + uint8_t payload_; - bool init_completed_ = true; bool init_error_ = true; - bool send_ = true; - - //SharedQueue writerQueue; - //SharedQueue readerQueue; - //SharedQueue parseQueue; - - ros::Duration sleeptime; - - uint8_t timeout_ = 20; // 20 cycles }; } diff --git a/src/sharedQueue.h b/src/sharedQueue.h deleted file mode 100644 index 809e9d4..0000000 --- a/src/sharedQueue.h +++ /dev/null @@ -1,142 +0,0 @@ -/** - * \file serial.h - * \author user66875 (https://stackoverflow.com/a/36763257) - * \date 21/04/2016 - * - * \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 SHAREDQUEUE_H -#define SHAREDQUEUE_H - -#include -#include -#include - -template -class SharedQueue -{ -public: - SharedQueue(); - ~SharedQueue(); - - T& front(); - void pop_front(); - - T get_n_pop_front(); - - void push_back(const T& item); - void push_back(T&& item); - - - unsigned long size(); - bool empty(); - -private: - std::deque queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - -template -SharedQueue::SharedQueue(){} - -template -SharedQueue::~SharedQueue(){} - -template -T& SharedQueue::front() -{ - std::unique_lock mlock(mutex_); - while (queue_.empty()) - { - cond_.wait(mlock); - } - return queue_.front(); -} - -template -void SharedQueue::pop_front() -{ - std::unique_lock mlock(mutex_); - while (queue_.empty()) - { - cond_.wait(mlock); - } - queue_.pop_front(); -} - -template -T SharedQueue::get_n_pop_front() -{ - std::unique_lock mlock(mutex_); - while (queue_.empty()) - { - cond_.wait(mlock); - } - T temp = queue_.front(); - queue_.pop_front(); - mlock.unlock(); - cond_.notify_one(); - return temp; -} - -template -void SharedQueue::push_back(const T& item) -{ - std::unique_lock mlock(mutex_); - queue_.push_back(item); - mlock.unlock(); // unlock before notificiation to minimize mutex con - cond_.notify_one(); // notify one waiting thread - -} - -template -void SharedQueue::push_back(T&& item) -{ - std::unique_lock mlock(mutex_); - queue_.push_back(std::move(item)); - mlock.unlock(); // unlock before notificiation to minimize mutex con - cond_.notify_one(); // notify one waiting thread - -} - -template -unsigned long SharedQueue::size() -{ - std::unique_lock mlock(mutex_); - unsigned long size = queue_.size(); - mlock.unlock(); - cond_.notify_one(); - return size; -} - -template -bool SharedQueue::empty() -{ - std::unique_lock mlock(mutex_); - bool empty = queue_.empty(); - mlock.unlock(); - cond_.notify_one(); - return empty; -} - -#endif //SHAREDQUEUE_H From a3b16d5e4175f64d8580da6e55c80c4711756dab Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 24 Oct 2021 14:16:13 +0000 Subject: [PATCH 105/140] change in service to reduce code size --- src/provider_underwater_com_node.cc | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 7ba09cb..72f5385 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -85,7 +85,6 @@ namespace provider_underwater_com Send_CMD_To_Sensor(buffer, (char)req.cmd); char cmd_rec = buffer[2]; - char tmp; std::stringstream ss(buffer); switch (cmd_rec) @@ -109,8 +108,7 @@ namespace provider_underwater_com std::getline(ss, role, ','); std::getline(ss, channel, '*'); - tmp = role.at(0); - res.role = (uint8_t)tmp; + res.role = (uint8_t)role.at(0); res.channel = std::stoi(channel); break; } @@ -127,8 +125,7 @@ namespace provider_underwater_com std::getline(ss, packet_loss_count, ','); std::getline(ss, bit_error_rate, '*'); - tmp = link_up.at(0); - res.link = (uint8_t)tmp; + 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); From d8106c708db16d8c1c274c0dfd20581c833cbfe1 Mon Sep 17 00:00:00 2001 From: Francis Alonzo <44241055+supertoto29@users.noreply.github.com> Date: Sun, 24 Oct 2021 10:17:36 -0400 Subject: [PATCH 106/140] Update package.xml --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 9ab9b1f..04df372 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.4.0 + 0.4.1 The provider_underwater_com package Francis Alonzo From 5f3a513b40cdd14da21425ca83284a1d742c35ce Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 24 Oct 2021 14:44:17 +0000 Subject: [PATCH 107/140] add definitions for rapid developement --- src/Modem_Definitions.h | 104 ++++++++++++++++++++++++++++ src/provider_underwater_com_node.cc | 11 +-- src/provider_underwater_com_node.h | 3 +- 3 files changed, 113 insertions(+), 5 deletions(-) create mode 100644 src/Modem_Definitions.h diff --git a/src/Modem_Definitions.h b/src/Modem_Definitions.h new file mode 100644 index 0000000..3de1101 --- /dev/null +++ b/src/Modem_Definitions.h @@ -0,0 +1,104 @@ +/** + * \file Modem_defitions.h + * \author Francis Alonzo . + */ + +#ifndef MODEMM64_DEFINITIONS +#define MODEMM64_DEFINITIONS + +#define SOP 'w' +#define EOP '\n' +#define DIR_CMD 'c' +#define DIR_RESP 'r' +#define CHECKSUM '*' + +#define CMD_GET_VERSION 'v' +#define CMD_GET_PAYLOAD_SIZE 'n' +#define CMD_GET_BUFFER_LENGTH 'l' +#define CMD_GET_DIAGNOSTIC 'd' +#define CMD_GET_SETTINGS 'c' +#define CMD_SET_SETTINGS 's' +#define CMD_QUEUE_PACKET 'q' +#define CMD_FLUSH 'f' +#define RESP_GOT_PACKET 'p' +#define RETURN_ERROR '?' +#define MALFORMED '!' + +#define LINK_UP 'y' +#define LINK_DOWN 'n' + +#define ACK 'a' +#define NAK 'n' + +#define ROLE_MASTER 'a' +#define ROLE_SLAVE 'b' + +static const char all_valid [] = { + CMD_GET_VERSION, + CMD_GET_PAYLOAD_SIZE, + CMD_GET_BUFFER_LENGTH, + CMD_GET_DIAGNOSTIC, + CMD_GET_SETTINGS, + CMD_SET_SETTINGS, + CMD_QUEUE_PACKET, + CMD_FLUSH +}; + +static const uint8_t all_valid_size = 8; + +static const uint8_t crc_table[] = { + 0x00U,0x07U,0x0EU,0x09U,0x1CU,0x1BU,0x12U,0x15U, + 0x38U,0x3FU,0x36U,0x31U,0x24U,0x23U,0x2AU,0x2DU, + 0x70U,0x77U,0x7EU,0x79U,0x6CU,0x6BU,0x62U,0x65U, + 0x48U,0x4FU,0x46U,0x41U,0x54U,0x53U,0x5AU,0x5DU, + 0xE0U,0xE7U,0xEEU,0xE9U,0xFCU,0xFBU,0xF2U,0xF5U, + 0xD8U,0xDFU,0xD6U,0xD1U,0xC4U,0xC3U,0xCAU,0xCDU, + 0x90U,0x97U,0x9EU,0x99U,0x8CU,0x8BU,0x82U,0x85U, + 0xA8U,0xAFU,0xA6U,0xA1U,0xB4U,0xB3U,0xBAU,0xBDU, + 0xC7U,0xC0U,0xC9U,0xCEU,0xDBU,0xDCU,0xD5U,0xD2U, + 0xFFU,0xF8U,0xF1U,0xF6U,0xE3U,0xE4U,0xEDU,0xEAU, + 0xB7U,0xB0U,0xB9U,0xBEU,0xABU,0xACU,0xA5U,0xA2U, + 0x8FU,0x88U,0x81U,0x86U,0x93U,0x94U,0x9DU,0x9AU, + 0x27U,0x20U,0x29U,0x2EU,0x3BU,0x3CU,0x35U,0x32U, + 0x1FU,0x18U,0x11U,0x16U,0x03U,0x04U,0x0DU,0x0AU, + 0x57U,0x50U,0x59U,0x5EU,0x4BU,0x4CU,0x45U,0x42U, + 0x6FU,0x68U,0x61U,0x66U,0x73U,0x74U,0x7DU,0x7AU, + 0x89U,0x8EU,0x87U,0x80U,0x95U,0x92U,0x9BU,0x9CU, + 0xB1U,0xB6U,0xBFU,0xB8U,0xADU,0xAAU,0xA3U,0xA4U, + 0xF9U,0xFEU,0xF7U,0xF0U,0xE5U,0xE2U,0xEBU,0xECU, + 0xC1U,0xC6U,0xCFU,0xC8U,0xDDU,0xDAU,0xD3U,0xD4U, + 0x69U,0x6EU,0x67U,0x60U,0x75U,0x72U,0x7BU,0x7CU, + 0x51U,0x56U,0x5FU,0x58U,0x4DU,0x4AU,0x43U,0x44U, + 0x19U,0x1EU,0x17U,0x10U,0x05U,0x02U,0x0BU,0x0CU, + 0x21U,0x26U,0x2FU,0x28U,0x3DU,0x3AU,0x33U,0x34U, + 0x4EU,0x49U,0x40U,0x47U,0x52U,0x55U,0x5CU,0x5BU, + 0x76U,0x71U,0x78U,0x7FU,0x6AU,0x6DU,0x64U,0x63U, + 0x3EU,0x39U,0x30U,0x37U,0x22U,0x25U,0x2CU,0x2BU, + 0x06U,0x01U,0x08U,0x0FU,0x1AU,0x1DU,0x14U,0x13U, + 0xAEU,0xA9U,0xA0U,0xA7U,0xB2U,0xB5U,0xBCU,0xBBU, + 0x96U,0x91U,0x98U,0x9FU,0x8AU,0x8DU,0x84U,0x83U, + 0xDEU,0xD9U,0xD0U,0xD7U,0xC2U,0xC5U,0xCCU,0xCBU, + 0xE6U,0xE1U,0xE8U,0xEFU,0xFAU,0xFDU,0xF4U,0xF3U, +}; + +#endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 72f5385..0f3fc2f 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -131,6 +131,11 @@ namespace provider_underwater_com res.bit_error_rate = std::stof(bit_error_rate); break; } + case CMD_SET_SETTINGS: + { + // Modify sonia_common first for the proc + break; + } case CMD_FLUSH: { ROS_INFO_STREAM("Flushed queue"); @@ -139,7 +144,7 @@ namespace provider_underwater_com } default: { - ROS_ERROR("CMD received isn't working with the service. CMD received is %c", cmd_rec); + ROS_ERROR_STREAM("CMD received isn't working with this service"); return false; } } @@ -232,9 +237,7 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) { - Queue_Packet(std::string(1, cmd), packet); - //Transmit_Packet(true); - + Queue_Packet(std::string(1, cmd), packet); std::unique_lock mlock(parse_mutex); parse_cond.wait(mlock); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 6e7bc92..29c696b 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -38,7 +38,8 @@ #include "Configuration.h" #include "drivers/serial.h" -#include +#include "Modem_Definitions.h" +//#include #include namespace provider_underwater_com { From 34d254a6e0e669cc9cd8a9fd2fc73dcf445a5ba7 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Sun, 24 Oct 2021 15:04:29 +0000 Subject: [PATCH 108/140] add service for setting modem --- Dockerfile | 2 +- src/provider_underwater_com_node.cc | 32 +++++++++++++++++------------ src/provider_underwater_com_node.h | 1 - 3 files changed, 20 insertions(+), 15 deletions(-) diff --git a/Dockerfile b/Dockerfile index 5c22f73..4dc1b89 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -ARG BASE_IMAGE="ghcr.io/sonia-auv/sonia_common/sonia_common:x86-perception-latest" +ARG BASE_IMAGE="ghcr.io/sonia-auv/sonia_common/sonia_common:x86-perception-feature-modem-modif-service" FROM ${BASE_IMAGE} diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 0f3fc2f..1e0598d 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -34,7 +34,7 @@ namespace provider_underwater_com ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { - //serialConnection_.flush(); + serialConnection_.flush(); underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); @@ -82,15 +82,13 @@ namespace provider_underwater_com { char buffer[BUFFER_SIZE]; - Send_CMD_To_Sensor(buffer, (char)req.cmd); - - char cmd_rec = buffer[2]; - std::stringstream ss(buffer); - - switch (cmd_rec) + 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, ','); @@ -101,6 +99,9 @@ namespace provider_underwater_com } case CMD_GET_SETTINGS: { + Send_CMD_To_Sensor(buffer, CMD_GET_SETTINGS); + std::stringstream ss(buffer); + std::string role; std::string channel; @@ -114,6 +115,9 @@ namespace provider_underwater_com } 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; @@ -133,13 +137,15 @@ namespace provider_underwater_com } case CMD_SET_SETTINGS: { - // Modify sonia_common first for the proc + if(Set_Configuration((char)req.role, req.channel)) return false; + res.role = req.role; + res.channel = req.channel; break; } case CMD_FLUSH: { ROS_INFO_STREAM("Flushed queue"); - Flush_Queue(); + if(Flush_Queue()) return false; break; } default: @@ -393,7 +399,7 @@ namespace provider_underwater_com if(major_version != "1") { - ROS_ERROR_STREAM("Major Version isn't 1. Restarting init"); + ROS_ERROR_STREAM("Major Version isn't 1."); return true; } return false; @@ -418,7 +424,7 @@ namespace provider_underwater_com } else { - ROS_ERROR_STREAM("Payload isn't a integer. Restarting init"); + ROS_ERROR_STREAM("Payload isn't a integer."); return true; } } @@ -439,7 +445,7 @@ namespace provider_underwater_com if(acknowledge == std::string(1, NAK)) { - ROS_ERROR_STREAM("Could not set the configuration. Restarting init"); + ROS_ERROR_STREAM("Could not set the configuration."); return true; } return false; @@ -458,7 +464,7 @@ namespace provider_underwater_com if(acknowledge == std::string(1, NAK)) { - ROS_ERROR_STREAM("Couldn't flush the queue. Restarting init"); + ROS_ERROR_STREAM("Couldn't flush the queue."); return true; } return false; diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 29c696b..95ffa44 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -106,7 +106,6 @@ class ProviderUnderwaterComNode uint8_t payload_; bool init_error_ = true; }; - } #endif //PROVIDER_UNDERWATER_COM_NODE \ No newline at end of file From 7df8c75649bf1e5a20fb2764dbe20ecdc8bf45ec Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 13:17:54 -0400 Subject: [PATCH 109/140] change base image to latest --- Dockerfile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index 4dc1b89..5c22f73 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,4 +1,4 @@ -ARG BASE_IMAGE="ghcr.io/sonia-auv/sonia_common/sonia_common:x86-perception-feature-modem-modif-service" +ARG BASE_IMAGE="ghcr.io/sonia-auv/sonia_common/sonia_common:x86-perception-latest" FROM ${BASE_IMAGE} From a06e79e9978a99096e53c4133589ddfece7e66cb Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 17:30:55 +0000 Subject: [PATCH 110/140] add debug message --- src/provider_underwater_com_node.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 1e0598d..e303cd3 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -402,6 +402,7 @@ namespace provider_underwater_com ROS_ERROR_STREAM("Major Version isn't 1."); return true; } + ROS_INFO_STREAM("Major version is 1"); return false; } @@ -448,6 +449,7 @@ namespace provider_underwater_com ROS_ERROR_STREAM("Could not set the configuration."); return true; } + ROS_INFO_STREAM("Configuration set"); return false; } @@ -467,6 +469,7 @@ namespace provider_underwater_com ROS_ERROR_STREAM("Couldn't flush the queue."); return true; } + ROS_INFO_STREAM("Queue flushed"); return false; } } \ No newline at end of file From d590e6261727cefc5a64727ab9373b0fa40ea630 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 17:31:27 +0000 Subject: [PATCH 111/140] remove ros topic and service from destructor --- src/provider_underwater_com_node.cc | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index e303cd3..307c005 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -54,9 +54,6 @@ namespace provider_underwater_com manage_write_thread.~thread(); manage_response_thread.~thread(); read_packet_thread.~thread(); - underwaterComSubscriber_.shutdown(); - underwaterComPublisher_.shutdown(); - underwaterComService_.shutdown(); } //Node Spin From 4ed0e734584600c475e37a7cc62137c9ec7f8535 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 13:44:23 -0400 Subject: [PATCH 112/140] add more debug message --- src/provider_underwater_com_node.cc | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 307c005..8e0706e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -42,6 +42,8 @@ namespace provider_underwater_com manage_write_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Write, this)); manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); + + ROS_INFO_STREAM("Setting the sensor"); Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); @@ -278,6 +280,7 @@ namespace provider_underwater_com std::unique_lock mlock(write_mutex); write_cond.wait(mlock); Transmit_Packet(true); + ROS_INFO_STREAM("Packet is sent"); } } @@ -334,6 +337,8 @@ namespace provider_underwater_com continue; } + ROS_INFO_STREAM("New Data received"); + if(ConfirmChecksum(buffer)) { if(buffer[2] == RESP_GOT_PACKET) From 1058689de8422621f3b0f772bc0dd6f72b3cda1f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 17:50:18 +0000 Subject: [PATCH 113/140] remove serial flush at the start --- src/provider_underwater_com_node.cc | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 8e0706e..4787374 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -34,8 +34,6 @@ namespace provider_underwater_com ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) { - serialConnection_.flush(); - underwaterComSubscriber_ = nh_->subscribe("/proc_underwater_com/send_msgs", 100, &ProviderUnderwaterComNode::UnderwaterComCallback, this); underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); From 5488f56824845e2d888301e63223bd33bd095792 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 17:55:03 +0000 Subject: [PATCH 114/140] remove unned ros_info --- src/provider_underwater_com_node.cc | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 4787374..b442cd6 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -40,8 +40,6 @@ namespace provider_underwater_com manage_write_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Write, this)); manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); - - ROS_INFO_STREAM("Setting the sensor"); Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); @@ -278,7 +276,6 @@ namespace provider_underwater_com std::unique_lock mlock(write_mutex); write_cond.wait(mlock); Transmit_Packet(true); - ROS_INFO_STREAM("Packet is sent"); } } @@ -335,8 +332,6 @@ namespace provider_underwater_com continue; } - ROS_INFO_STREAM("New Data received"); - if(ConfirmChecksum(buffer)) { if(buffer[2] == RESP_GOT_PACKET) From 7363b9061f5232903bda1ddff0b799015a4ec6d6 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 18:26:39 +0000 Subject: [PATCH 115/140] remove string clear unneed --- src/provider_underwater_com_node.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index b442cd6..1733c9f 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -251,7 +251,7 @@ namespace provider_underwater_com buffer[i] = tmp.at(i); } - parse_string.clear(); + //parse_string.clear(); } } @@ -275,7 +275,7 @@ namespace provider_underwater_com { std::unique_lock mlock(write_mutex); write_cond.wait(mlock); - Transmit_Packet(true); + if(!write_string.empty()) Transmit_Packet(false); } } @@ -287,8 +287,8 @@ namespace provider_underwater_com { std::unique_lock mlock(response_mutex); response_cond.wait(mlock); - Export_To_ROS(response_string); - response_string.clear(); + if(!response_string.empty()) Export_To_ROS(response_string); + //response_string.clear(); } } From 6f1f74b059ac65689bf5645181860cff541a4fbc Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 18:27:12 +0000 Subject: [PATCH 116/140] bump package --- package.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/package.xml b/package.xml index 04df372..c46338a 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.4.1 + 0.5.0 The provider_underwater_com package Francis Alonzo From d8da3062c229554df6c130310b7b128e814b9718 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 14:35:06 -0400 Subject: [PATCH 117/140] retry with O_NDELAY --- src/drivers/serial.cc | 2 +- src/provider_underwater_com_node.cc | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index cadc633..d600012 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -30,7 +30,7 @@ Serial::Serial(std::string port) { - fd = open(port.c_str(), O_RDWR | O_NOCTTY); + fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); if(fd == -1) { ROS_ERROR("unable to connect to %s", port.c_str()); diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 1733c9f..a471478 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -310,14 +310,18 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Read_Packet() { + ros::Rate r(2); // 2 Hz uint8_t i; char buffer[BUFFER_SIZE]; + ROS_INFO_STREAM("Read Packet thread started"); + while(!ros::isShuttingDown()) { do { serialConnection_.readOnce(buffer, 0); + r.sleep(); } while(buffer[0] != SOP); for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) From a12fffc6f793faa98b8dc08c67bd410802704101 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 14:37:20 -0400 Subject: [PATCH 118/140] increase rate for read --- src/provider_underwater_com_node.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index a471478..154667d 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -310,7 +310,7 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Read_Packet() { - ros::Rate r(2); // 2 Hz + ros::Rate r(5); // 5 Hz uint8_t i; char buffer[BUFFER_SIZE]; From 8e6c3a8eb6e7ac1614c166d01b13e9919c9cff52 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 14:41:31 -0400 Subject: [PATCH 119/140] reverts changes --- src/provider_underwater_com_node.cc | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 154667d..ca3a0a9 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -41,6 +41,7 @@ namespace provider_underwater_com manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); + 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); @@ -276,6 +277,7 @@ namespace provider_underwater_com std::unique_lock mlock(write_mutex); write_cond.wait(mlock); if(!write_string.empty()) Transmit_Packet(false); + ROS_INFO_STREAM("Sent a packet"); } } @@ -289,6 +291,7 @@ namespace provider_underwater_com response_cond.wait(mlock); if(!response_string.empty()) Export_To_ROS(response_string); //response_string.clear(); + ROS_INFO_STREAM("Read a packet"); } } @@ -310,7 +313,6 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Read_Packet() { - ros::Rate r(5); // 5 Hz uint8_t i; char buffer[BUFFER_SIZE]; @@ -321,7 +323,6 @@ namespace provider_underwater_com do { serialConnection_.readOnce(buffer, 0); - r.sleep(); } while(buffer[0] != SOP); for(i = 1; buffer[i-1] != EOP && i < BUFFER_SIZE; ++i) From 39b4c5d8bb9b05a7437e8378b2151f4ceaf062c5 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 14:58:08 -0400 Subject: [PATCH 120/140] add rate and comment debug message --- src/provider_underwater_com_node.cc | 23 ++++++++++++----------- 1 file changed, 12 insertions(+), 11 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index ca3a0a9..7b0033f 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -251,8 +251,6 @@ namespace provider_underwater_com { buffer[i] = tmp.at(i); } - - //parse_string.clear(); } } @@ -270,19 +268,22 @@ namespace provider_underwater_com 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); - if(!write_string.empty()) Transmit_Packet(false); - ROS_INFO_STREAM("Sent a packet"); - } + while(!ros::isShuttingDown()) + { + std::unique_lock mlock(write_mutex); + write_cond.wait(mlock); + if(!write_string.empty()) Transmit_Packet(false); + //ROS_INFO_STREAM("Sent a packet"); + r.sleep(); + } } void ProviderUnderwaterComNode::Manage_Response() { + ros::Rate r(1); // 1 Hz ROS_INFO_STREAM("Manage response thread started"); while (!ros::isShuttingDown()) @@ -290,8 +291,8 @@ namespace provider_underwater_com std::unique_lock mlock(response_mutex); response_cond.wait(mlock); if(!response_string.empty()) Export_To_ROS(response_string); - //response_string.clear(); - ROS_INFO_STREAM("Read a packet"); + //ROS_INFO_STREAM("Read a packet"); + r.sleep(); } } From 870b7a9b1670a7d168ade1f0ba5a77eb9a9029dc Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 19:17:16 +0000 Subject: [PATCH 121/140] change position for setting up sensor --- src/provider_underwater_com_node.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 7b0033f..6c07247 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -41,9 +41,6 @@ namespace provider_underwater_com manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); - 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); } @@ -59,7 +56,10 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Spin() { ros::Rate r(5); // 5 Hz - + + ROS_INFO_STREAM("Setting the sensor"); + Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); + while(ros::ok() && init_error_ == false) { ros::spinOnce(); From 7e14047cf5f31e0535206b72547d9f2810a06092 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 15:24:52 -0400 Subject: [PATCH 122/140] give time for thread to start --- src/provider_underwater_com_node.cc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 6c07247..366ae36 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -40,6 +40,11 @@ namespace provider_underwater_com manage_write_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Write, this)); manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); + + ros::Duration(5).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); } @@ -57,9 +62,6 @@ namespace provider_underwater_com { ros::Rate r(5); // 5 Hz - ROS_INFO_STREAM("Setting the sensor"); - Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); - while(ros::ok() && init_error_ == false) { ros::spinOnce(); From 522307947c4fbcf967362f479038cc55560ba478 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 20:08:05 +0000 Subject: [PATCH 123/140] remove debug message --- src/provider_underwater_com_node.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 366ae36..9b0a4ef 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -142,7 +142,6 @@ namespace provider_underwater_com } case CMD_FLUSH: { - ROS_INFO_STREAM("Flushed queue"); if(Flush_Queue()) return false; break; } @@ -350,7 +349,7 @@ namespace provider_underwater_com } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) { - ROS_INFO_STREAM("Packet queue"); + ROS_DEBUG_STREAM("Packet queue"); } else if(buffer[2] == RETURN_ERROR || buffer[2] == MALFORMED) { From 02feda49d07eca0ac5a88056a91eb020a3aac404 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 20:42:47 +0000 Subject: [PATCH 124/140] back to shared Queue --- src/provider_underwater_com_node.cc | 51 +++++----- src/provider_underwater_com_node.h | 7 +- src/sharedQueue.h | 142 ++++++++++++++++++++++++++++ 3 files changed, 175 insertions(+), 25 deletions(-) create mode 100644 src/sharedQueue.h diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 9b0a4ef..4f7aa8d 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -41,7 +41,7 @@ namespace provider_underwater_com manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); - ros::Duration(5).sleep(); + ros::Duration(1).sleep(); ROS_INFO_STREAM("Setting the sensor"); Set_Sensor(configuration_.getRole().at(0), std::stoi(configuration_.getChannel())); @@ -210,10 +210,10 @@ namespace provider_underwater_com sentence = ss.str(); AppendChecksum(sentence); - - std::unique_lock mlock(write_mutex); - write_string = sentence; - write_cond.notify_one(); + writerQueue.push_back(sentence); + // std::unique_lock mlock(write_mutex); + // write_string = sentence; + // write_cond.notify_one(); ROS_DEBUG_STREAM("Packet sent to Modem"); } @@ -225,10 +225,10 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Transmit_Packet(bool pop_packet) { - if(!write_string.empty()) + if(!writerQueue.empty()) { - serialConnection_.transmit(write_string); - if(pop_packet) write_string.clear(); + serialConnection_.transmit(writerQueue.front()); + if(pop_packet) writerQueue.pop_front(); return true; } else @@ -238,13 +238,13 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) + bool ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) { Queue_Packet(std::string(1, cmd), packet); std::unique_lock mlock(parse_mutex); parse_cond.wait(mlock); - if(!parse_string.empty()) + if(!parseQueue.empty()) { std::string tmp = parse_string; @@ -252,7 +252,9 @@ namespace provider_underwater_com { buffer[i] = tmp.at(i); } + return false; } + return true; } bool ProviderUnderwaterComNode::Check_CMD(const char *cmd) @@ -274,10 +276,9 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - std::unique_lock mlock(write_mutex); - write_cond.wait(mlock); - if(!write_string.empty()) Transmit_Packet(false); - //ROS_INFO_STREAM("Sent a packet"); + // std::unique_lock mlock(write_mutex); + // write_cond.wait(mlock); + if(!writerQueue.empty()) Transmit_Packet(true); r.sleep(); } } @@ -289,9 +290,9 @@ namespace provider_underwater_com while (!ros::isShuttingDown()) { - std::unique_lock mlock(response_mutex); - response_cond.wait(mlock); - if(!response_string.empty()) Export_To_ROS(response_string); + // std::unique_lock mlock(response_mutex); + // response_cond.wait(mlock); + if(!responseQueue.empty()) Export_To_ROS(responseQueue.get_n_pop_front()); //ROS_INFO_STREAM("Read a packet"); r.sleep(); } @@ -343,9 +344,10 @@ namespace provider_underwater_com { if(buffer[2] == RESP_GOT_PACKET) { - std::unique_lock mlock(response_mutex); - response_string = std::string(buffer); - response_cond.notify_one(); + // std::unique_lock mlock(response_mutex); + // response_string = std::string(buffer); + // response_cond.notify_one(); + responseQueue.push_back(buffer); } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) { @@ -362,6 +364,7 @@ namespace provider_underwater_com std::unique_lock mlock(parse_mutex); parse_string = std::string(buffer); parse_cond.notify_one(); + parseQueue.push_back(buffer); } } } @@ -393,7 +396,7 @@ namespace provider_underwater_com std::string major_version = ""; char buffer[BUFFER_SIZE]; - Send_CMD_To_Sensor(buffer, CMD_GET_VERSION); + if(Send_CMD_To_Sensor(buffer, CMD_GET_VERSION)) return true; std::stringstream ss(buffer); std::getline(ss, major_version, ','); @@ -413,7 +416,7 @@ namespace provider_underwater_com std::string payload = ""; char buffer[BUFFER_SIZE]; - Send_CMD_To_Sensor(buffer, CMD_GET_PAYLOAD_SIZE); + if(Send_CMD_To_Sensor(buffer, CMD_GET_PAYLOAD_SIZE)) return true; std::stringstream ss(buffer); std::getline(ss, payload, ','); @@ -440,7 +443,7 @@ namespace provider_underwater_com sprintf(buffer, "%d", channel); std::string packet = "," + std::string(1, role) + "," + buffer; - Send_CMD_To_Sensor(buffer, CMD_SET_SETTINGS, packet); + if(Send_CMD_To_Sensor(buffer, CMD_SET_SETTINGS, packet)) return true; std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); @@ -460,7 +463,7 @@ namespace provider_underwater_com std::string acknowledge; char buffer[BUFFER_SIZE]; - Send_CMD_To_Sensor(buffer, CMD_FLUSH); + if(Send_CMD_To_Sensor(buffer, CMD_FLUSH)) return true; std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 95ffa44..fd5596a 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -37,6 +37,7 @@ #include #include "Configuration.h" +#include "sharedQueue.h" #include "drivers/serial.h" #include "Modem_Definitions.h" //#include @@ -64,7 +65,7 @@ class ProviderUnderwaterComNode void Queue_Packet(const std::string &cmd, const std::string &packet = ""); bool Transmit_Packet(bool pop_packet); - void Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet = ""); + bool Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet = ""); bool Check_CMD(const char *cmd); void Manage_Write(); @@ -102,6 +103,10 @@ class ProviderUnderwaterComNode std::string write_string = ""; std::string response_string = ""; std::string parse_string = ""; + + SharedQueue writerQueue; + SharedQueue responseQueue; + SharedQueue parseQueue; uint8_t payload_; bool init_error_ = true; diff --git a/src/sharedQueue.h b/src/sharedQueue.h new file mode 100644 index 0000000..65b09b5 --- /dev/null +++ b/src/sharedQueue.h @@ -0,0 +1,142 @@ +/** + * \file serial.h + * \author user66875 (https://stackoverflow.com/a/36763257) + * \date 21/04/2016 + * + * \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 SHAREDQUEUE_H +#define SHAREDQUEUE_H + +#include +#include +#include + +template +class SharedQueue +{ +public: + SharedQueue(); + ~SharedQueue(); + + T& front(); + void pop_front(); + + T get_n_pop_front(); + + void push_back(const T& item); + void push_back(T&& item); + + + unsigned long size(); + bool empty(); + +private: + std::deque queue_; + std::mutex mutex_; + std::condition_variable cond_; +}; + +template +SharedQueue::SharedQueue(){} + +template +SharedQueue::~SharedQueue(){} + +template +T& SharedQueue::front() +{ + std::unique_lock mlock(mutex_); + while (queue_.empty()) + { + cond_.wait(mlock); + } + return queue_.front(); +} + +template +void SharedQueue::pop_front() +{ + std::unique_lock mlock(mutex_); + while (queue_.empty()) + { + cond_.wait(mlock); + } + queue_.pop_front(); +} + +template +T SharedQueue::get_n_pop_front() +{ + std::unique_lock mlock(mutex_); + while (queue_.empty()) + { + cond_.wait(mlock); + } + T temp = queue_.front(); + queue_.pop_front(); + mlock.unlock(); + cond_.notify_one(); + return temp; +} + +template +void SharedQueue::push_back(const T& item) +{ + std::unique_lock mlock(mutex_); + queue_.push_back(item); + mlock.unlock(); // unlock before notificiation to minimize mutex con + cond_.notify_one(); // notify one waiting thread + +} + +template +void SharedQueue::push_back(T&& item) +{ + std::unique_lock mlock(mutex_); + queue_.push_back(std::move(item)); + mlock.unlock(); // unlock before notificiation to minimize mutex con + cond_.notify_one(); // notify one waiting thread + +} + +template +unsigned long SharedQueue::size() +{ + std::unique_lock mlock(mutex_); + unsigned long size = queue_.size(); + mlock.unlock(); + cond_.notify_one(); + return size; +} + +template +bool SharedQueue::empty() +{ + std::unique_lock mlock(mutex_); + bool empty = queue_.empty(); + mlock.unlock(); + cond_.notify_one(); + return empty; +} + +#endif //SHAREDQUEUE_H \ No newline at end of file From d15e23b18777a8726dc9a7a0cf46d0c07db05a29 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 25 Oct 2021 20:50:46 +0000 Subject: [PATCH 125/140] change serial constructor --- src/drivers/serial.cc | 4 ++-- src/drivers/serial.h | 2 +- src/provider_underwater_com_node.cc | 3 ++- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index d600012..49ff5f6 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -28,9 +28,9 @@ #include #include -Serial::Serial(std::string port) +Serial::Serial(std::string port, int flags) { - fd = open(port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY); + fd = open(port.c_str(), flags); if(fd == -1) { ROS_ERROR("unable to connect to %s", port.c_str()); diff --git a/src/drivers/serial.h b/src/drivers/serial.h index 8f68d43..4b4857d 100644 --- a/src/drivers/serial.h +++ b/src/drivers/serial.h @@ -34,7 +34,7 @@ class Serial{ public: - Serial(std::string port); + Serial(std::string port, int flags); ~Serial(); std::string receive(size_t count); diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 4f7aa8d..2dd15c2 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -24,6 +24,7 @@ */ #include "provider_underwater_com_node.h" +#include #define BUFFER_SIZE 256 @@ -32,7 +33,7 @@ namespace provider_underwater_com //Node Construtor ProviderUnderwaterComNode::ProviderUnderwaterComNode(const ros::NodeHandlePtr &_nh) - : nh_(_nh), configuration_(_nh), serialConnection_(configuration_.getTtyPort()) + : 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); From b312ab4a746736a7b5533c6f01661e1cd6efcf85 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Tue, 26 Oct 2021 15:28:44 -0400 Subject: [PATCH 126/140] fix workflow for package --- .github/workflows/docker-image-perception-develop.yml | 4 ++-- .github/workflows/docker-image-perception-master.yml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) 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-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 From 403eace39ef8f4f5ad0f17c246ebd880172fa893 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 28 Oct 2021 21:08:36 +0000 Subject: [PATCH 127/140] starting the custom packet. Problem with the array --- .vscode/settings.json | 64 ++++++++++++++++++++++++++++- package.xml | 2 +- src/modem_data.h | 31 ++++++++++++++ src/provider_underwater_com_node.cc | 60 +++++++++++++++++++++++++-- src/provider_underwater_com_node.h | 6 ++- 5 files changed, 157 insertions(+), 6 deletions(-) create mode 100644 src/modem_data.h diff --git a/.vscode/settings.json b/.vscode/settings.json index ede23ff..c9a9be3 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,5 +6,67 @@ "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/package.xml b/package.xml index c46338a..326de81 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.5.0 + 0.6.0 The provider_underwater_com package Francis Alonzo diff --git a/src/modem_data.h b/src/modem_data.h new file mode 100644 index 0000000..12f01f0 --- /dev/null +++ b/src/modem_data.h @@ -0,0 +1,31 @@ +#ifndef MODEM_DATA_H +#define MODEM_DATA_H + +#pragma pack(push, 1) + +typedef struct +{ + uint8_t packetNumber; + uint8_t packetId : 7; + uint8_t endOfPacket : 1; +} Header_t; + +typedef struct +{ + Header_t header; + uint8_t killSwitchState : 4, missionSwitchState : 4; + uint16_t depth; + uint8_t missionId; + uint8_t missionState; + uint8_t torpedosState : 4, droppersState : 4; +} Modem_M64_t; + +union bytes_8 +{ + Modem_M64_t raw_data; + uint64_t u64_data; +}; + +#pragma pack(pop) + +#endif //MODEM_DATA_H \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 2dd15c2..d47bb8c 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -71,10 +71,31 @@ namespace provider_underwater_com ros::shutdown(); } - void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::String &msg) + void ProviderUnderwaterComNode::UnderwaterComCallback(const sonia_common::IntersubCom &msg) { - std::string packet = "," + std::to_string(payload_) + "," + msg.data; // TODO add a size check before transmit - Queue_Packet(std::string(1, CMD_QUEUE_PACKET), packet); + char packet_array[8]= ""; + std::string packet = ""; + + bytes_8 data; + // Add function for multiple paquet support + data.raw_data.header.endOfPacket = 0b1; + data.raw_data.header.packetId = 0b0; + data.raw_data.header.packetNumber = 0b1; + + data.raw_data.killSwitchState = (msg.kill_state & 0x1); + data.raw_data.missionSwitchState = (msg.mission_State & 0x1); + data.raw_data.depth = msg.depth & 0xFFFF; + data.raw_data.missionId = msg.mission_id & 0xFF; + data.raw_data.missionState = msg.mission_state & 0xFF; + data.raw_data.torpedosState = 0x0; + data.raw_data.droppersState = 0x0; + + for(uint8_t i = 0; i < 8; ++i) + { + packet_array[i] = (char)(data.u64_data >> i*8); + } + + Queue_Packet(CMD_QUEUE_PACKET, payload_, packet_array); } bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) @@ -224,6 +245,39 @@ namespace provider_underwater_com } } + void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const uint8_t payload, const char packet[8]) + { + std::stringstream ss; + std::string sentence; + + if(Check_CMD(&cmd)) + { + ss << SOP << DIR_CMD << cmd; + + if(cmd == CMD_QUEUE_PACKET) + { + ss << ',' << std::to_string(payload_) << ',' << packet; + } + else if( cmd == CMD_SET_SETTINGS) + { + ss << packet; + } + + sentence = ss.str(); + AppendChecksum(sentence); + writerQueue.push_back(sentence); + // std::unique_lock mlock(write_mutex); + // write_string = sentence; + // 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(!writerQueue.empty()) diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index fd5596a..5d0731d 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -27,6 +27,7 @@ #define PROVIDER_UNDERWATER_COM_NODE #include +#include #include #include #include @@ -40,6 +41,8 @@ #include "sharedQueue.h" #include "drivers/serial.h" #include "Modem_Definitions.h" +#include "modem_data.h" +#include //#include #include @@ -56,7 +59,7 @@ class ProviderUnderwaterComNode private: - void UnderwaterComCallback(const std_msgs::String &msg); + void UnderwaterComCallback(const sonia_common::IntersubCom &msg); bool UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res); uint8_t CalculateChecksum(const std::string &sentence, uint8_t length); @@ -64,6 +67,7 @@ class ProviderUnderwaterComNode bool ConfirmChecksum(const std::string &sentence); void Queue_Packet(const std::string &cmd, const std::string &packet = ""); + void Queue_Packet(const char cmd, const uint8_t payload, const char packet[8]); bool Transmit_Packet(bool pop_packet); bool Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet = ""); bool Check_CMD(const char *cmd); From 514a8242ef3e4d645fb50d47a5272391fef4c127 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 8 Nov 2021 00:15:26 +0000 Subject: [PATCH 128/140] change to char array to test --- src/provider_underwater_com_node.cc | 166 ++++++++++++++++------------ src/provider_underwater_com_node.h | 19 ++-- 2 files changed, 105 insertions(+), 80 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index d47bb8c..6c3f430 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -28,7 +28,7 @@ #define BUFFER_SIZE 256 -namespace provider_underwater_com +namespace provider_underwater_com { //Node Construtor @@ -73,8 +73,7 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::UnderwaterComCallback(const sonia_common::IntersubCom &msg) { - char packet_array[8]= ""; - std::string packet = ""; + char packet_array[payload_]= ""; bytes_8 data; // Add function for multiple paquet support @@ -90,12 +89,12 @@ namespace provider_underwater_com data.raw_data.torpedosState = 0x0; data.raw_data.droppersState = 0x0; - for(uint8_t i = 0; i < 8; ++i) + for(uint8_t i = 0; i < payload_; ++i) { - packet_array[i] = (char)(data.u64_data >> i*8); + packet_array[i] = (char)(data.u64_data >> i*payload_); } - Queue_Packet(CMD_QUEUE_PACKET, payload_, packet_array); + Queue_Packet(CMD_QUEUE_PACKET, packet_array, payload_, payload_); } bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) @@ -176,37 +175,43 @@ namespace provider_underwater_com return true; } - uint8_t ProviderUnderwaterComNode::CalculateChecksum(const std::string &sentence, uint8_t length) + uint8_t ProviderUnderwaterComNode::Calculate_Checksum(const char *buffer, const size_t size) { uint8_t check = 0; - for(uint8_t i = 0; i < length; ++i) + for(uint8_t i = 0; i < size; ++i) { - check = crc_table[(uint8_t)sentence.at(i) ^ check]; + check = crc_table[(uint8_t)buffer[i] ^ check]; } return check; } - void ProviderUnderwaterComNode::AppendChecksum(std::string &sentence) + void ProviderUnderwaterComNode::Append_Checksum(char *buffer, const size_t size) { - std::stringstream ss; - char buffer[2]; + char checksum_buffer[2]; - uint8_t checksum = CalculateChecksum(sentence, sentence.size()); - sprintf(buffer, "%02x", checksum); + uint8_t checksum = Calculate_Checksum(buffer, size); + sprintf(checksum_buffer, "%02x", checksum); - ss << sentence << std::string(1, CHECKSUM) << buffer << std::string(1, EOP); - sentence = ss.str(); + buffer[size + 1] = CHECKSUM; + buffer[size + 2] = checksum_buffer[0]; + buffer[size + 3] = checksum_buffer[1]; + buffer[size + 4] = EOP; + buffer[size + 5] = '\0'; // À confirmer si utile } - bool ProviderUnderwaterComNode::ConfirmChecksum(const std::string &sentence) - { - try + bool ProviderUnderwaterComNode::Confirm_Checksum(char *buffer, const size_t size) + { + char checksumData[BUFFER_SIZE]; + + try // Calcul à vérifier { - std::string checksumData = sentence.substr(0, sentence.find("*", 0)); - uint8_t calculatedChecksum = CalculateChecksum(checksumData, checksumData.size()); - uint8_t originalChecksum = std::stoi(sentence.substr(sentence.find("*", 0)+1, 2), nullptr, 16); + uint8_t position = Find_Character(buffer, size, CHECKSUM); + std::strncpy(checksumData, buffer, position); + uint8_t calculatedChecksum = Calculate_Checksum(checksumData, position); + std::string checksumSentence = std::to_string(buffer[position + 1]) + std::to_string(buffer[position + 2]); + uint8_t originalChecksum = std::stoi(checksumSentence, nullptr, 16); return originalChecksum == calculatedChecksum; } catch(...) @@ -216,59 +221,62 @@ namespace provider_underwater_com } } - void ProviderUnderwaterComNode::Queue_Packet(const std::string &cmd, const std::string &packet) - { - std::stringstream ss; - std::string sentence; - - if(Check_CMD(cmd.data())) - { - ss << SOP << DIR_CMD << cmd; - - if(cmd == std::string(1, CMD_QUEUE_PACKET) || cmd == std::string(1, CMD_SET_SETTINGS)) - { - ss << packet; - } - - sentence = ss.str(); - AppendChecksum(sentence); - writerQueue.push_back(sentence); - // std::unique_lock mlock(write_mutex); - // write_string = sentence; - // write_cond.notify_one(); - - ROS_DEBUG_STREAM("Packet sent to Modem"); - } - else - { - ROS_WARN_STREAM("CMD unknow. Can't queue packet"); - } - } - - void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const uint8_t payload, const char packet[8]) + // void ProviderUnderwaterComNode::Queue_Packet(const std::string &cmd, const std::string &packet) + // { + // std::stringstream ss; + // std::string sentence; + + // if(Check_CMD(cmd.data())) + // { + // ss << SOP << DIR_CMD << cmd; + + // if(cmd == std::string(1, CMD_QUEUE_PACKET) || cmd == std::string(1, CMD_SET_SETTINGS)) + // { + // ss << packet; + // } + + // sentence = ss.str(); + // AppendChecksum(sentence); + // writerQueue.push_back(sentence); + // // std::unique_lock mlock(write_mutex); + // // write_string = sentence; + // // write_cond.notify_one(); + + // ROS_DEBUG_STREAM("Packet sent to Modem"); + // } + // else + // { + // ROS_WARN_STREAM("CMD unknow. Can't queue packet"); + // } + // } + + void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const char *packet, const uint8_t payload, const size_t size_packet) { - std::stringstream ss; - std::string sentence; + char send_msg[BUFFER_SIZE]; + uint8_t index = 0; if(Check_CMD(&cmd)) { - ss << SOP << DIR_CMD << cmd; + send_msg[0] = SOP; + send_msg[1] = DIR_CMD; + send_msg[2] = cmd; + send_msg[3] = ','; if(cmd == CMD_QUEUE_PACKET) { - ss << ',' << std::to_string(payload_) << ',' << packet; + send_msg[4] = payload_; + send_msg[5] = ','; + index = 6; + Append_Packet(send_msg, index, packet, size_packet); } else if( cmd == CMD_SET_SETTINGS) { - ss << packet; + index = 4; + Append_Packet(send_msg, index, packet, size_packet); } - sentence = ss.str(); - AppendChecksum(sentence); - writerQueue.push_back(sentence); - // std::unique_lock mlock(write_mutex); - // write_string = sentence; - // write_cond.notify_one(); + Append_Checksum(send_msg, index + size_packet); + //writerQueue.push_back(); ROS_DEBUG_STREAM("Packet sent to Modem"); } @@ -295,7 +303,7 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) { - Queue_Packet(std::string(1, cmd), packet); + Queue_Packet(cmd, packet.c_str()); std::unique_lock mlock(parse_mutex); parse_cond.wait(mlock); @@ -324,6 +332,26 @@ namespace provider_underwater_com return false; } + void ProviderUnderwaterComNode::Append_Packet(char *buffer, const size_t index, const char *packet, 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, 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::Manage_Write() { ros::Rate r(1); // 1 Hz @@ -331,8 +359,6 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - // std::unique_lock mlock(write_mutex); - // write_cond.wait(mlock); if(!writerQueue.empty()) Transmit_Packet(true); r.sleep(); } @@ -345,10 +371,7 @@ namespace provider_underwater_com while (!ros::isShuttingDown()) { - // std::unique_lock mlock(response_mutex); - // response_cond.wait(mlock); if(!responseQueue.empty()) Export_To_ROS(responseQueue.get_n_pop_front()); - //ROS_INFO_STREAM("Read a packet"); r.sleep(); } } @@ -395,13 +418,10 @@ namespace provider_underwater_com continue; } - if(ConfirmChecksum(buffer)) + if(Confirm_Checksum(buffer, i-1)) { if(buffer[2] == RESP_GOT_PACKET) { - // std::unique_lock mlock(response_mutex); - // response_string = std::string(buffer); - // response_cond.notify_one(); responseQueue.push_back(buffer); } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 5d0731d..4b433fd 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -40,10 +40,10 @@ #include "Configuration.h" #include "sharedQueue.h" #include "drivers/serial.h" -#include "Modem_Definitions.h" +//#include "Modem_Definitions.h" #include "modem_data.h" #include -//#include +#include #include namespace provider_underwater_com { @@ -62,15 +62,20 @@ class ProviderUnderwaterComNode void UnderwaterComCallback(const sonia_common::IntersubCom &msg); bool UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res); - uint8_t CalculateChecksum(const std::string &sentence, uint8_t length); - void AppendChecksum(std::string &sentence); - bool ConfirmChecksum(const std::string &sentence); + //uint8_t CalculateChecksum(const std::string &sentence, uint8_t length); + uint8_t Calculate_Checksum(const char *buffer, const size_t size); + //void AppendChecksum(std::string &sentence); + void Append_Checksum(char *buffer, const size_t size); + //bool ConfirmChecksum(const std::string &sentence); + bool Confirm_Checksum(char *buffer, const size_t size); - void Queue_Packet(const std::string &cmd, const std::string &packet = ""); - void Queue_Packet(const char cmd, const uint8_t payload, const char packet[8]); + //void Queue_Packet(const std::string &cmd, const std::string &packet = ""); + void Queue_Packet(const char cmd, const char *packet = "", const uint8_t payload = 0, const size_t size_packet = 0); bool Transmit_Packet(bool pop_packet); bool Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet = ""); bool Check_CMD(const char *cmd); + void Append_Packet(char *buffer, const size_t index, const char *packet, const size_t size_packet); + uint8_t Find_Character(const char *buffer, const char to_find, const size_t size); void Manage_Write(); void Manage_Response(); From e58ffe5c1391a8c4a4887333c3d57ddeff80a1f6 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 8 Nov 2021 19:42:06 +0000 Subject: [PATCH 129/140] conversion from string to char array complete --- src/drivers/serial.cc | 6 ++++ src/drivers/serial.h | 1 + src/provider_underwater_com_node.cc | 52 ++++++++++++++++------------- src/provider_underwater_com_node.h | 13 ++++---- 4 files changed, 42 insertions(+), 30 deletions(-) diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index 49ff5f6..fe9d065 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -101,3 +101,9 @@ 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 char *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 index 4b4857d..8593abb 100644 --- a/src/drivers/serial.h +++ b/src/drivers/serial.h @@ -41,6 +41,7 @@ class Serial{ void readOnce(char* data, int offset); void flush(); ssize_t transmit(const std::string data); + ssize_t transmit(const char *data, const ssize_t size); private: diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 6c3f430..8de02e4 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -26,8 +26,6 @@ #include "provider_underwater_com_node.h" #include -#define BUFFER_SIZE 256 - namespace provider_underwater_com { @@ -94,7 +92,7 @@ namespace provider_underwater_com packet_array[i] = (char)(data.u64_data >> i*payload_); } - Queue_Packet(CMD_QUEUE_PACKET, packet_array, payload_, payload_); + Queue_Packet(CMD_QUEUE_PACKET, packet_array, payload_); } bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) @@ -187,18 +185,19 @@ namespace provider_underwater_com return check; } - void ProviderUnderwaterComNode::Append_Checksum(char *buffer, const size_t size) + uint8_t ProviderUnderwaterComNode::Append_Checksum(char *buffer, const size_t size) { char checksum_buffer[2]; uint8_t checksum = Calculate_Checksum(buffer, size); sprintf(checksum_buffer, "%02x", checksum); - buffer[size + 1] = CHECKSUM; - buffer[size + 2] = checksum_buffer[0]; - buffer[size + 3] = checksum_buffer[1]; - buffer[size + 4] = EOP; - buffer[size + 5] = '\0'; // À confirmer si utile + 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, const size_t size) @@ -207,10 +206,10 @@ namespace provider_underwater_com try // Calcul à vérifier { - uint8_t position = Find_Character(buffer, size, CHECKSUM); + uint8_t position = Find_Character(buffer, CHECKSUM, size); std::strncpy(checksumData, buffer, position); uint8_t calculatedChecksum = Calculate_Checksum(checksumData, position); - std::string checksumSentence = std::to_string(buffer[position + 1]) + std::to_string(buffer[position + 2]); + std::string checksumSentence {buffer[position + 1], buffer[position + 2]}; uint8_t originalChecksum = std::stoi(checksumSentence, nullptr, 16); return originalChecksum == calculatedChecksum; } @@ -250,7 +249,7 @@ namespace provider_underwater_com // } // } - void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const char *packet, const uint8_t payload, const size_t size_packet) + void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const char *packet, const size_t size_packet) { char send_msg[BUFFER_SIZE]; uint8_t index = 0; @@ -260,10 +259,11 @@ namespace provider_underwater_com send_msg[0] = SOP; send_msg[1] = DIR_CMD; send_msg[2] = cmd; - send_msg[3] = ','; + index = 3; if(cmd == CMD_QUEUE_PACKET) { + send_msg[3] = ','; send_msg[4] = payload_; send_msg[5] = ','; index = 6; @@ -271,12 +271,14 @@ namespace provider_underwater_com } else if( cmd == CMD_SET_SETTINGS) { + send_msg[3] = ','; index = 4; Append_Packet(send_msg, index, packet, size_packet); } - Append_Checksum(send_msg, index + size_packet); - //writerQueue.push_back(); + index = Append_Checksum(send_msg, index + size_packet); + writerQueue.push_back(send_msg); + writerSizeQueue.push_back(index); ROS_DEBUG_STREAM("Packet sent to Modem"); } @@ -288,10 +290,14 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Transmit_Packet(bool pop_packet) { - if(!writerQueue.empty()) + if(!writerQueue.empty() && !writerSizeQueue.empty()) { - serialConnection_.transmit(writerQueue.front()); - if(pop_packet) writerQueue.pop_front(); + serialConnection_.transmit(writerQueue.front(), writerSizeQueue.front()); + if(pop_packet) + { + writerQueue.pop_front(); + writerSizeQueue.pop_front(); + } return true; } else @@ -303,7 +309,7 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) { - Queue_Packet(cmd, packet.c_str()); + Queue_Packet(cmd, packet.c_str(), packet.size()); std::unique_lock mlock(parse_mutex); parse_cond.wait(mlock); @@ -359,7 +365,7 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - if(!writerQueue.empty()) Transmit_Packet(true); + if(!writerQueue.empty() && !writerSizeQueue.empty()) Transmit_Packet(true); r.sleep(); } } @@ -516,7 +522,7 @@ namespace provider_underwater_com char buffer[BUFFER_SIZE]; sprintf(buffer, "%d", channel); - std::string packet = "," + std::string(1, role) + "," + buffer; + std::string packet = std::string(1, role) + "," + buffer; if(Send_CMD_To_Sensor(buffer, CMD_SET_SETTINGS, packet)) return true; @@ -524,7 +530,7 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK)) + if(acknowledge != std::string(1, ACK)) { ROS_ERROR_STREAM("Could not set the configuration."); return true; @@ -544,7 +550,7 @@ namespace provider_underwater_com std::getline(ss, acknowledge, ','); std::getline(ss, acknowledge, '*'); - if(acknowledge == std::string(1, NAK)) + if(acknowledge != std::string(1, ACK)) { ROS_ERROR_STREAM("Couldn't flush the queue."); return true; diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 4b433fd..755ba92 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -46,6 +46,8 @@ #include #include +#define BUFFER_SIZE 256 + namespace provider_underwater_com { class ProviderUnderwaterComNode @@ -62,15 +64,11 @@ class ProviderUnderwaterComNode void UnderwaterComCallback(const sonia_common::IntersubCom &msg); bool UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res); - //uint8_t CalculateChecksum(const std::string &sentence, uint8_t length); uint8_t Calculate_Checksum(const char *buffer, const size_t size); - //void AppendChecksum(std::string &sentence); - void Append_Checksum(char *buffer, const size_t size); - //bool ConfirmChecksum(const std::string &sentence); + uint8_t Append_Checksum(char *buffer, const size_t size); bool Confirm_Checksum(char *buffer, const size_t size); - //void Queue_Packet(const std::string &cmd, const std::string &packet = ""); - void Queue_Packet(const char cmd, const char *packet = "", const uint8_t payload = 0, const size_t size_packet = 0); + void Queue_Packet(const char cmd, const char *packet = "", const size_t size_packet = 0); bool Transmit_Packet(bool pop_packet); bool Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet = ""); bool Check_CMD(const char *cmd); @@ -113,7 +111,8 @@ class ProviderUnderwaterComNode std::string response_string = ""; std::string parse_string = ""; - SharedQueue writerQueue; + SharedQueue writerQueue; + SharedQueue writerSizeQueue; SharedQueue responseQueue; SharedQueue parseQueue; From 3431ffc255c5efbbc61e134052c490c986c7b623 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 8 Nov 2021 21:18:27 +0000 Subject: [PATCH 130/140] prevent decay of array --- src/drivers/serial.cc | 2 +- src/drivers/serial.h | 2 +- src/modem_data.h | 10 ++--- src/provider_underwater_com_node.cc | 66 +++++++++++++++-------------- src/provider_underwater_com_node.h | 12 +++--- 5 files changed, 48 insertions(+), 44 deletions(-) diff --git a/src/drivers/serial.cc b/src/drivers/serial.cc index fe9d065..f9c5725 100644 --- a/src/drivers/serial.cc +++ b/src/drivers/serial.cc @@ -102,7 +102,7 @@ ssize_t Serial::transmit(const std::string data) return write(fd, data.c_str(), data.size()); } -ssize_t Serial::transmit(const char *data, const ssize_t size) +ssize_t Serial::transmit(const void *data, const ssize_t size) { ROS_DEBUG("serial transmit data"); return write(fd, data, size); diff --git a/src/drivers/serial.h b/src/drivers/serial.h index 8593abb..c1e04e6 100644 --- a/src/drivers/serial.h +++ b/src/drivers/serial.h @@ -41,7 +41,7 @@ class Serial{ void readOnce(char* data, int offset); void flush(); ssize_t transmit(const std::string data); - ssize_t transmit(const char *data, const ssize_t size); + ssize_t transmit(const void *data, const ssize_t size); private: diff --git a/src/modem_data.h b/src/modem_data.h index 12f01f0..4ca46e7 100644 --- a/src/modem_data.h +++ b/src/modem_data.h @@ -20,11 +20,11 @@ typedef struct uint8_t torpedosState : 4, droppersState : 4; } Modem_M64_t; -union bytes_8 -{ - Modem_M64_t raw_data; - uint64_t u64_data; -}; +// union bytes_8 +// { +// Modem_M64_t raw_data; +// uint64_t u64_data; +// }; #pragma pack(pop) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 8de02e4..9616957 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -71,28 +71,30 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::UnderwaterComCallback(const sonia_common::IntersubCom &msg) { - char packet_array[payload_]= ""; + uint64_t packet = 0; + char packet_array[MODEM_M64_PAYLOAD]; - bytes_8 data; // Add function for multiple paquet support - data.raw_data.header.endOfPacket = 0b1; - data.raw_data.header.packetId = 0b0; - data.raw_data.header.packetNumber = 0b1; - - data.raw_data.killSwitchState = (msg.kill_state & 0x1); - data.raw_data.missionSwitchState = (msg.mission_State & 0x1); - data.raw_data.depth = msg.depth & 0xFFFF; - data.raw_data.missionId = msg.mission_id & 0xFF; - data.raw_data.missionState = msg.mission_state & 0xFF; - data.raw_data.torpedosState = 0x0; - data.raw_data.droppersState = 0x0; - - for(uint8_t i = 0; i < payload_; ++i) + modem_data.header.endOfPacket = 0b1; + modem_data.header.packetId = 0b0; + modem_data.header.packetNumber = 0b1; + + modem_data.killSwitchState = msg.kill_state; + modem_data.missionSwitchState = msg.mission_State; + modem_data.depth = msg.depth; + modem_data.missionId = msg.mission_id; + modem_data.missionState = msg.mission_state; + modem_data.torpedosState = 0x0; + modem_data.droppersState = 0x0; + + packet = *((uint64_t *)&modem_data); + + for(uint8_t i = 0; i < MODEM_M64_PAYLOAD; ++i) { - packet_array[i] = (char)(data.u64_data >> i*payload_); + packet_array[i] = (char) (packet >> i * MODEM_M64_PAYLOAD); } - Queue_Packet(CMD_QUEUE_PACKET, packet_array, payload_); + Queue_Packet(CMD_QUEUE_PACKET, packet_array, MODEM_M64_PAYLOAD); } bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) @@ -185,7 +187,7 @@ namespace provider_underwater_com return check; } - uint8_t ProviderUnderwaterComNode::Append_Checksum(char *buffer, const size_t size) + uint8_t ProviderUnderwaterComNode::Append_Checksum(char (&buffer)[BUFFER_SIZE], const size_t size) { char checksum_buffer[2]; @@ -204,7 +206,7 @@ namespace provider_underwater_com { char checksumData[BUFFER_SIZE]; - try // Calcul à vérifier + try { uint8_t position = Find_Character(buffer, CHECKSUM, size); std::strncpy(checksumData, buffer, position); @@ -249,7 +251,7 @@ namespace provider_underwater_com // } // } - void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const char *packet, const size_t size_packet) + 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; @@ -264,12 +266,12 @@ namespace provider_underwater_com if(cmd == CMD_QUEUE_PACKET) { send_msg[3] = ','; - send_msg[4] = payload_; + 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) + else if(cmd == CMD_SET_SETTINGS) { send_msg[3] = ','; index = 4; @@ -277,7 +279,7 @@ namespace provider_underwater_com } index = Append_Checksum(send_msg, index + size_packet); - writerQueue.push_back(send_msg); + std::copy(std::begin(send_msg), std::end(send_msg), std::begin(writerQueue)); writerSizeQueue.push_back(index); ROS_DEBUG_STREAM("Packet sent to Modem"); @@ -290,12 +292,11 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Transmit_Packet(bool pop_packet) { - if(!writerQueue.empty() && !writerSizeQueue.empty()) + if(!writerSizeQueue.empty()) { - serialConnection_.transmit(writerQueue.front(), writerSizeQueue.front()); + serialConnection_.transmit(writerQueue, writerSizeQueue.front()); if(pop_packet) { - writerQueue.pop_front(); writerSizeQueue.pop_front(); } return true; @@ -307,9 +308,9 @@ namespace provider_underwater_com } } - bool ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const std::string &packet) + bool ProviderUnderwaterComNode::Send_CMD_To_Sensor(char *buffer, char cmd, const char (&packet)[MODEM_M64_PAYLOAD], size_t size) { - Queue_Packet(cmd, packet.c_str(), packet.size()); + Queue_Packet(cmd, packet, size); std::unique_lock mlock(parse_mutex); parse_cond.wait(mlock); @@ -338,7 +339,7 @@ namespace provider_underwater_com return false; } - void ProviderUnderwaterComNode::Append_Packet(char *buffer, const size_t index, const char *packet, const size_t size_packet) + 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) { @@ -365,7 +366,7 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - if(!writerQueue.empty() && !writerSizeQueue.empty()) Transmit_Packet(true); + if(!writerSizeQueue.empty()) Transmit_Packet(true); r.sleep(); } } @@ -520,11 +521,12 @@ namespace provider_underwater_com { std::string acknowledge; char buffer[BUFFER_SIZE]; + char packet[MODEM_M64_PAYLOAD] = {role, ','}; sprintf(buffer, "%d", channel); - std::string packet = std::string(1, role) + "," + buffer; + packet[2] = buffer[0]; - if(Send_CMD_To_Sensor(buffer, CMD_SET_SETTINGS, packet)) return true; + if(Send_CMD_To_Sensor(buffer, CMD_SET_SETTINGS, packet, 3)) return true; std::stringstream ss(buffer); std::getline(ss, acknowledge, ','); diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 755ba92..f890ff4 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -47,6 +47,7 @@ #include #define BUFFER_SIZE 256 +#define MODEM_M64_PAYLOAD 8 namespace provider_underwater_com { @@ -65,14 +66,14 @@ class ProviderUnderwaterComNode bool UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res); uint8_t Calculate_Checksum(const char *buffer, const size_t size); - uint8_t Append_Checksum(char *buffer, const size_t size); + uint8_t Append_Checksum(char (&buffer)[BUFFER_SIZE], const size_t size); // Prevent decay bool Confirm_Checksum(char *buffer, const size_t size); - void Queue_Packet(const char cmd, const char *packet = "", const size_t size_packet = 0); + 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 std::string &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, const size_t index, const char *packet, const size_t size_packet); + 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, const char to_find, const size_t size); void Manage_Write(); @@ -111,13 +112,14 @@ class ProviderUnderwaterComNode std::string response_string = ""; std::string parse_string = ""; - SharedQueue writerQueue; + char writerQueue[BUFFER_SIZE] = {}; SharedQueue writerSizeQueue; SharedQueue responseQueue; SharedQueue parseQueue; uint8_t payload_; bool init_error_ = true; + Modem_M64_t modem_data; }; } From 2e9dd97d11b6e0144eddd1384c3536547ca2a104 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 8 Nov 2021 21:33:08 +0000 Subject: [PATCH 131/140] decay removed send and receive working --- src/provider_underwater_com_node.cc | 16 ++++++++++++---- src/provider_underwater_com_node.h | 7 ++++--- 2 files changed, 16 insertions(+), 7 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 9616957..ffd6960 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -175,7 +175,7 @@ namespace provider_underwater_com return true; } - uint8_t ProviderUnderwaterComNode::Calculate_Checksum(const char *buffer, const size_t size) + uint8_t ProviderUnderwaterComNode::Calculate_Checksum(const char (&buffer)[BUFFER_SIZE], const size_t size) { uint8_t check = 0; @@ -202,14 +202,14 @@ namespace provider_underwater_com return size + 4; } - bool ProviderUnderwaterComNode::Confirm_Checksum(char *buffer, const size_t size) + 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); - std::strncpy(checksumData, buffer, position); + 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); @@ -347,7 +347,7 @@ namespace provider_underwater_com } } - uint8_t ProviderUnderwaterComNode::Find_Character(const char *buffer, const char to_find, const size_t size) + uint8_t ProviderUnderwaterComNode::Find_Character(const char (&buffer)[BUFFER_SIZE], const char to_find, const size_t size) { uint8_t i; @@ -359,6 +359,14 @@ namespace provider_underwater_com return i; } + void ProviderUnderwaterComNode::Copy_Array(const char (&buffer)[BUFFER_SIZE], char (&checksum_data)[BUFFER_SIZE], const size_t size) + { + for(uint8_t i = 0; i < size; ++i) + { + checksum_data[i] = buffer[i]; + } + } + void ProviderUnderwaterComNode::Manage_Write() { ros::Rate r(1); // 1 Hz diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index f890ff4..12e46b3 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -65,16 +65,17 @@ class ProviderUnderwaterComNode void UnderwaterComCallback(const sonia_common::IntersubCom &msg); bool UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res); - uint8_t Calculate_Checksum(const char *buffer, const size_t size); + 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, const size_t size); + 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, const char to_find, const size_t size); + 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 (&checksum_data)[BUFFER_SIZE], const size_t size); // Prevent decay void Manage_Write(); void Manage_Response(); From a0b2af2be49adaf668e65d5bac9f31fea35059b9 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 8 Nov 2021 16:34:12 -0500 Subject: [PATCH 132/140] remove from local project definition --- src/Modem_Definitions.h | 104 ----------------------------- src/provider_underwater_com_node.h | 1 - 2 files changed, 105 deletions(-) delete mode 100644 src/Modem_Definitions.h diff --git a/src/Modem_Definitions.h b/src/Modem_Definitions.h deleted file mode 100644 index 3de1101..0000000 --- a/src/Modem_Definitions.h +++ /dev/null @@ -1,104 +0,0 @@ -/** - * \file Modem_defitions.h - * \author Francis Alonzo . - */ - -#ifndef MODEMM64_DEFINITIONS -#define MODEMM64_DEFINITIONS - -#define SOP 'w' -#define EOP '\n' -#define DIR_CMD 'c' -#define DIR_RESP 'r' -#define CHECKSUM '*' - -#define CMD_GET_VERSION 'v' -#define CMD_GET_PAYLOAD_SIZE 'n' -#define CMD_GET_BUFFER_LENGTH 'l' -#define CMD_GET_DIAGNOSTIC 'd' -#define CMD_GET_SETTINGS 'c' -#define CMD_SET_SETTINGS 's' -#define CMD_QUEUE_PACKET 'q' -#define CMD_FLUSH 'f' -#define RESP_GOT_PACKET 'p' -#define RETURN_ERROR '?' -#define MALFORMED '!' - -#define LINK_UP 'y' -#define LINK_DOWN 'n' - -#define ACK 'a' -#define NAK 'n' - -#define ROLE_MASTER 'a' -#define ROLE_SLAVE 'b' - -static const char all_valid [] = { - CMD_GET_VERSION, - CMD_GET_PAYLOAD_SIZE, - CMD_GET_BUFFER_LENGTH, - CMD_GET_DIAGNOSTIC, - CMD_GET_SETTINGS, - CMD_SET_SETTINGS, - CMD_QUEUE_PACKET, - CMD_FLUSH -}; - -static const uint8_t all_valid_size = 8; - -static const uint8_t crc_table[] = { - 0x00U,0x07U,0x0EU,0x09U,0x1CU,0x1BU,0x12U,0x15U, - 0x38U,0x3FU,0x36U,0x31U,0x24U,0x23U,0x2AU,0x2DU, - 0x70U,0x77U,0x7EU,0x79U,0x6CU,0x6BU,0x62U,0x65U, - 0x48U,0x4FU,0x46U,0x41U,0x54U,0x53U,0x5AU,0x5DU, - 0xE0U,0xE7U,0xEEU,0xE9U,0xFCU,0xFBU,0xF2U,0xF5U, - 0xD8U,0xDFU,0xD6U,0xD1U,0xC4U,0xC3U,0xCAU,0xCDU, - 0x90U,0x97U,0x9EU,0x99U,0x8CU,0x8BU,0x82U,0x85U, - 0xA8U,0xAFU,0xA6U,0xA1U,0xB4U,0xB3U,0xBAU,0xBDU, - 0xC7U,0xC0U,0xC9U,0xCEU,0xDBU,0xDCU,0xD5U,0xD2U, - 0xFFU,0xF8U,0xF1U,0xF6U,0xE3U,0xE4U,0xEDU,0xEAU, - 0xB7U,0xB0U,0xB9U,0xBEU,0xABU,0xACU,0xA5U,0xA2U, - 0x8FU,0x88U,0x81U,0x86U,0x93U,0x94U,0x9DU,0x9AU, - 0x27U,0x20U,0x29U,0x2EU,0x3BU,0x3CU,0x35U,0x32U, - 0x1FU,0x18U,0x11U,0x16U,0x03U,0x04U,0x0DU,0x0AU, - 0x57U,0x50U,0x59U,0x5EU,0x4BU,0x4CU,0x45U,0x42U, - 0x6FU,0x68U,0x61U,0x66U,0x73U,0x74U,0x7DU,0x7AU, - 0x89U,0x8EU,0x87U,0x80U,0x95U,0x92U,0x9BU,0x9CU, - 0xB1U,0xB6U,0xBFU,0xB8U,0xADU,0xAAU,0xA3U,0xA4U, - 0xF9U,0xFEU,0xF7U,0xF0U,0xE5U,0xE2U,0xEBU,0xECU, - 0xC1U,0xC6U,0xCFU,0xC8U,0xDDU,0xDAU,0xD3U,0xD4U, - 0x69U,0x6EU,0x67U,0x60U,0x75U,0x72U,0x7BU,0x7CU, - 0x51U,0x56U,0x5FU,0x58U,0x4DU,0x4AU,0x43U,0x44U, - 0x19U,0x1EU,0x17U,0x10U,0x05U,0x02U,0x0BU,0x0CU, - 0x21U,0x26U,0x2FU,0x28U,0x3DU,0x3AU,0x33U,0x34U, - 0x4EU,0x49U,0x40U,0x47U,0x52U,0x55U,0x5CU,0x5BU, - 0x76U,0x71U,0x78U,0x7FU,0x6AU,0x6DU,0x64U,0x63U, - 0x3EU,0x39U,0x30U,0x37U,0x22U,0x25U,0x2CU,0x2BU, - 0x06U,0x01U,0x08U,0x0FU,0x1AU,0x1DU,0x14U,0x13U, - 0xAEU,0xA9U,0xA0U,0xA7U,0xB2U,0xB5U,0xBCU,0xBBU, - 0x96U,0x91U,0x98U,0x9FU,0x8AU,0x8DU,0x84U,0x83U, - 0xDEU,0xD9U,0xD0U,0xD7U,0xC2U,0xC5U,0xCCU,0xCBU, - 0xE6U,0xE1U,0xE8U,0xEFU,0xFAU,0xFDU,0xF4U,0xF3U, -}; - -#endif // MODEMM64_DEFINITIONS \ No newline at end of file diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 12e46b3..2955d36 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -40,7 +40,6 @@ #include "Configuration.h" #include "sharedQueue.h" #include "drivers/serial.h" -//#include "Modem_Definitions.h" #include "modem_data.h" #include #include From d28faa23be2a0f578ba6baa78543cf2055ad3006 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 22 Nov 2021 20:48:59 +0000 Subject: [PATCH 133/140] add send to ROS. to test --- package.xml | 2 +- src/provider_underwater_com_node.cc | 100 ++++++++++------------------ src/provider_underwater_com_node.h | 13 ++-- 3 files changed, 43 insertions(+), 72 deletions(-) diff --git a/package.xml b/package.xml index 326de81..2726c75 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ provider_underwater_com - 0.6.0 + 0.6.1 The provider_underwater_com package Francis Alonzo diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index ffd6960..d3cbd69 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -34,10 +34,9 @@ namespace provider_underwater_com : 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); + underwaterComPublisher_ = nh_->advertise("/provider_underwater_com/receive_msgs", 100); manage_write_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Write, this)); - manage_response_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Manage_Response, this)); read_packet_thread = std::thread(std::bind(&ProviderUnderwaterComNode::Read_Packet, this)); ros::Duration(1).sleep(); @@ -52,7 +51,6 @@ namespace provider_underwater_com ProviderUnderwaterComNode::~ProviderUnderwaterComNode() { manage_write_thread.~thread(); - manage_response_thread.~thread(); read_packet_thread.~thread(); } @@ -222,35 +220,6 @@ namespace provider_underwater_com } } - // void ProviderUnderwaterComNode::Queue_Packet(const std::string &cmd, const std::string &packet) - // { - // std::stringstream ss; - // std::string sentence; - - // if(Check_CMD(cmd.data())) - // { - // ss << SOP << DIR_CMD << cmd; - - // if(cmd == std::string(1, CMD_QUEUE_PACKET) || cmd == std::string(1, CMD_SET_SETTINGS)) - // { - // ss << packet; - // } - - // sentence = ss.str(); - // AppendChecksum(sentence); - // writerQueue.push_back(sentence); - // // std::unique_lock mlock(write_mutex); - // // write_string = sentence; - // // write_cond.notify_one(); - - // ROS_DEBUG_STREAM("Packet sent to Modem"); - // } - // else - // { - // ROS_WARN_STREAM("CMD unknow. Can't queue packet"); - // } - // } - void ProviderUnderwaterComNode::Queue_Packet(const char cmd, const char (&packet)[MODEM_M64_PAYLOAD], const size_t size_packet) { char send_msg[BUFFER_SIZE]; @@ -278,9 +247,11 @@ namespace provider_underwater_com Append_Packet(send_msg, index, packet, size_packet); } - index = Append_Checksum(send_msg, index + size_packet); - std::copy(std::begin(send_msg), std::end(send_msg), std::begin(writerQueue)); - writerSizeQueue.push_back(index); + 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"); } @@ -292,12 +263,13 @@ namespace provider_underwater_com bool ProviderUnderwaterComNode::Transmit_Packet(bool pop_packet) { - if(!writerSizeQueue.empty()) + if(writeBuffer[0] != 0) { - serialConnection_.transmit(writerQueue, writerSizeQueue.front()); + serialConnection_.transmit(writeBuffer, writeSize); if(pop_packet) { - writerSizeQueue.pop_front(); + writeBuffer[0] = 0; + writeSize = 0; } return true; } @@ -355,15 +327,14 @@ namespace provider_underwater_com { if(buffer[i] == to_find) break; } - return i; } - void ProviderUnderwaterComNode::Copy_Array(const char (&buffer)[BUFFER_SIZE], char (&checksum_data)[BUFFER_SIZE], const size_t size) + 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) { - checksum_data[i] = buffer[i]; + buffer_returned[i+start] = buffer[i]; } } @@ -374,37 +345,36 @@ namespace provider_underwater_com while(!ros::isShuttingDown()) { - if(!writerSizeQueue.empty()) Transmit_Packet(true); - r.sleep(); + std::unique_lock mlock(write_mutex); + write_cond.wait(mlock); + Transmit_Packet(true); } } - void ProviderUnderwaterComNode::Manage_Response() + void ProviderUnderwaterComNode::Export_To_ROS(const char (&buffer)[BUFFER_SIZE], const ssize_t size) { - ros::Rate r(1); // 1 Hz - ROS_INFO_STREAM("Manage response thread started"); + Modem_M64_t packet; + uint64_t data = 0; + char tmp[BUFFER_SIZE]; + // TODO add a size check before publish and check if message good - while (!ros::isShuttingDown()) + uint8_t first = Find_Character(buffer, ',', size); + Copy_Array(buffer, tmp, MODEM_M64_PAYLOAD, first + 1); + + for(uint8_t i = 0; i < MODEM_M64_PAYLOAD; ++i) { - if(!responseQueue.empty()) Export_To_ROS(responseQueue.get_n_pop_front()); - r.sleep(); + if(i != 0) data += tmp[i] * (i * 8); + else data += tmp[i]; } - } - - void ProviderUnderwaterComNode::Export_To_ROS(std::string buffer) - { - std::string size; - std::string msg; + packet = *((Modem_M64_t *)&data); - msg_received.data.clear(); - std::stringstream ss(buffer); - std::getline(ss, size, ','); // TODO add a size check before publish and check if message good - std::getline(ss, size, ','); - std::getline(ss, msg, '*'); - - msg_received.data = msg; + msg.depth = packet.depth; + msg.kill_state = packet.killSwitchState; + msg.mission_state = packet.missionSwitchState; + msg.mission_id = packet.missionId; + msg.mission_State = packet.missionState; - underwaterComPublisher_.publish(msg_received); + underwaterComPublisher_.publish(msg); } void ProviderUnderwaterComNode::Read_Packet() @@ -434,10 +404,10 @@ namespace provider_underwater_com } if(Confirm_Checksum(buffer, i-1)) - { + { if(buffer[2] == RESP_GOT_PACKET) { - responseQueue.push_back(buffer); + Export_To_ROS(buffer, i-1); } else if(buffer[2] == CMD_QUEUE_PACKET && buffer[4] == ACK) { diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 2955d36..10138c5 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -74,11 +74,10 @@ class ProviderUnderwaterComNode 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 (&checksum_data)[BUFFER_SIZE], 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 Manage_Response(); - void Export_To_ROS(std::string buffer); + 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); @@ -94,7 +93,7 @@ class ProviderUnderwaterComNode ros::Subscriber underwaterComSubscriber_; ros::Publisher underwaterComPublisher_; ros::ServiceServer underwaterComService_; - std_msgs::String msg_received; + sonia_common::IntersubCom msg; std::thread manage_write_thread; std::thread manage_response_thread; @@ -112,8 +111,10 @@ class ProviderUnderwaterComNode std::string response_string = ""; std::string parse_string = ""; - char writerQueue[BUFFER_SIZE] = {}; - SharedQueue writerSizeQueue; + char writeBuffer[BUFFER_SIZE] = {}; + uint8_t writeSize = 0; + // char responseBuffer[BUFFER_SIZE] = {}; + // uint8_t responseSize = 0; SharedQueue responseQueue; SharedQueue parseQueue; From 765b9edd873696ec81abec856beb67871c4ce796 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Wed, 24 Nov 2021 19:29:12 +0000 Subject: [PATCH 134/140] test to change made to parse message --- src/provider_underwater_com_node.cc | 24 +++++++++++++----------- src/provider_underwater_com_node.h | 13 ++++--------- 2 files changed, 17 insertions(+), 20 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index d3cbd69..787b107 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -77,13 +77,13 @@ namespace provider_underwater_com modem_data.header.packetId = 0b0; modem_data.header.packetNumber = 0b1; - modem_data.killSwitchState = msg.kill_state; - modem_data.missionSwitchState = msg.mission_State; + 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 = msg.mission_state; - modem_data.torpedosState = 0x0; - modem_data.droppersState = 0x0; + 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); @@ -95,7 +95,7 @@ namespace provider_underwater_com Queue_Packet(CMD_QUEUE_PACKET, packet_array, MODEM_M64_PAYLOAD); } - bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res) + bool ProviderUnderwaterComNode::UnderwaterComService(sonia_common::ModemSendCmd::Request &req, sonia_common::ModemSendCmd::Response &res) { char buffer[BUFFER_SIZE]; @@ -286,9 +286,10 @@ namespace provider_underwater_com std::unique_lock mlock(parse_mutex); parse_cond.wait(mlock); - if(!parseQueue.empty()) + if(!parse_string.empty()) { std::string tmp = parse_string; + parse_string.erase(); for(uint8_t i = 0; i < tmp.size(); ++i) { @@ -369,10 +370,12 @@ namespace provider_underwater_com packet = *((Modem_M64_t *)&data); msg.depth = packet.depth; - msg.kill_state = packet.killSwitchState; - msg.mission_state = packet.missionSwitchState; + msg.kill_switch_state = packet.killSwitchState; + msg.mission_switch_state = packet.missionSwitchState; msg.mission_id = packet.missionId; - msg.mission_State = packet.missionState; + msg.mission_state = (int8_t) packet.missionState; + msg.torpedos_state = packet.torpedosState; + msg.droppers_state = packet.droppersState; underwaterComPublisher_.publish(msg); } @@ -424,7 +427,6 @@ namespace provider_underwater_com std::unique_lock mlock(parse_mutex); parse_string = std::string(buffer); parse_cond.notify_one(); - parseQueue.push_back(buffer); } } } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 10138c5..109c3a8 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -41,9 +41,9 @@ #include "sharedQueue.h" #include "drivers/serial.h" #include "modem_data.h" -#include #include -#include +#include +#include #define BUFFER_SIZE 256 #define MODEM_M64_PAYLOAD 8 @@ -62,7 +62,7 @@ class ProviderUnderwaterComNode private: void UnderwaterComCallback(const sonia_common::IntersubCom &msg); - bool UnderwaterComService(sonia_common::ModemPacket::Request &req, sonia_common::ModemPacket::Response &res); + 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 @@ -112,12 +112,7 @@ class ProviderUnderwaterComNode std::string parse_string = ""; char writeBuffer[BUFFER_SIZE] = {}; - uint8_t writeSize = 0; - // char responseBuffer[BUFFER_SIZE] = {}; - // uint8_t responseSize = 0; - SharedQueue responseQueue; - SharedQueue parseQueue; - + uint8_t writeSize = 0; uint8_t payload_; bool init_error_ = true; Modem_M64_t modem_data; From 47d2decac9e90a9d0c5a55feb3d68b95810d16aa Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 29 Nov 2021 17:40:32 +0000 Subject: [PATCH 135/140] fix export to ros hardcoded --- src/provider_underwater_com_node.cc | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 787b107..a232e54 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -359,13 +359,13 @@ namespace provider_underwater_com char tmp[BUFFER_SIZE]; // TODO add a size check before publish and check if message good - uint8_t first = Find_Character(buffer, ',', size); - Copy_Array(buffer, tmp, MODEM_M64_PAYLOAD, first + 1); + // uint8_t first = Find_Character(buffer, ',', size); + // Copy_Array(buffer, tmp, MODEM_M64_PAYLOAD, first + 1); - for(uint8_t i = 0; i < MODEM_M64_PAYLOAD; ++i) + for(uint8_t i = 6; i < MODEM_M64_PAYLOAD + 6; ++i) { - if(i != 0) data += tmp[i] * (i * 8); - else data += tmp[i]; + if((i-6) != 0) data += tmp[i-6] * ((i-6) * 8); + else data += tmp[i-6]; } packet = *((Modem_M64_t *)&data); From c2d4130eeb0e5f65a9905b3cd26d8cc8d290bb2f Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 29 Nov 2021 18:55:46 +0000 Subject: [PATCH 136/140] fix export to ros array to msg --- src/provider_underwater_com_node.cc | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index a232e54..8c1558e 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -356,17 +356,14 @@ namespace provider_underwater_com { Modem_M64_t packet; uint64_t data = 0; - char tmp[BUFFER_SIZE]; - // TODO add a size check before publish and check if message good + uint8_t tmp[MODEM_M64_PAYLOAD]; - // uint8_t first = Find_Character(buffer, ',', size); - // Copy_Array(buffer, tmp, MODEM_M64_PAYLOAD, first + 1); - - for(uint8_t i = 6; i < MODEM_M64_PAYLOAD + 6; ++i) + for(uint8_t i = 0; i < MODEM_M64_PAYLOAD; ++i) { - if((i-6) != 0) data += tmp[i-6] * ((i-6) * 8); - else data += tmp[i-6]; + tmp[i] = (uint8_t) buffer[i + 6]; } + + std::memcpy(&data, tmp, sizeof(data)); packet = *((Modem_M64_t *)&data); msg.depth = packet.depth; From 098879d26bb78ae4e2b3b679c9be47f9b38d2877 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Mon, 29 Nov 2021 18:59:32 +0000 Subject: [PATCH 137/140] fix send message --- src/provider_underwater_com_node.cc | 8 ++------ 1 file changed, 2 insertions(+), 6 deletions(-) diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 8c1558e..52b051a 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -86,12 +86,8 @@ namespace provider_underwater_com modem_data.droppersState = msg.droppers_state; packet = *((uint64_t *)&modem_data); - - for(uint8_t i = 0; i < MODEM_M64_PAYLOAD; ++i) - { - packet_array[i] = (char) (packet >> i * MODEM_M64_PAYLOAD); - } - + std::memcpy(packet_array, &packet, sizeof(packet_array)); + Queue_Packet(CMD_QUEUE_PACKET, packet_array, MODEM_M64_PAYLOAD); } From 96759dda3e4471ca604103c00c71f10bb8236420 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 16 Dec 2021 13:15:09 +0000 Subject: [PATCH 138/140] remove comments --- src/modem_data.h | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/modem_data.h b/src/modem_data.h index 4ca46e7..5ba3218 100644 --- a/src/modem_data.h +++ b/src/modem_data.h @@ -20,12 +20,6 @@ typedef struct uint8_t torpedosState : 4, droppersState : 4; } Modem_M64_t; -// union bytes_8 -// { -// Modem_M64_t raw_data; -// uint64_t u64_data; -// }; - #pragma pack(pop) #endif //MODEM_DATA_H \ No newline at end of file From 876e95f85692c71b97e72c9e4b09af860ccdd199 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 16 Dec 2021 13:17:24 +0000 Subject: [PATCH 139/140] remove sharedqueue not used --- src/provider_underwater_com_node.h | 1 - src/sharedQueue.h | 142 ----------------------------- 2 files changed, 143 deletions(-) delete mode 100644 src/sharedQueue.h diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index 109c3a8..e75933e 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -38,7 +38,6 @@ #include #include "Configuration.h" -#include "sharedQueue.h" #include "drivers/serial.h" #include "modem_data.h" #include diff --git a/src/sharedQueue.h b/src/sharedQueue.h deleted file mode 100644 index 65b09b5..0000000 --- a/src/sharedQueue.h +++ /dev/null @@ -1,142 +0,0 @@ -/** - * \file serial.h - * \author user66875 (https://stackoverflow.com/a/36763257) - * \date 21/04/2016 - * - * \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 SHAREDQUEUE_H -#define SHAREDQUEUE_H - -#include -#include -#include - -template -class SharedQueue -{ -public: - SharedQueue(); - ~SharedQueue(); - - T& front(); - void pop_front(); - - T get_n_pop_front(); - - void push_back(const T& item); - void push_back(T&& item); - - - unsigned long size(); - bool empty(); - -private: - std::deque queue_; - std::mutex mutex_; - std::condition_variable cond_; -}; - -template -SharedQueue::SharedQueue(){} - -template -SharedQueue::~SharedQueue(){} - -template -T& SharedQueue::front() -{ - std::unique_lock mlock(mutex_); - while (queue_.empty()) - { - cond_.wait(mlock); - } - return queue_.front(); -} - -template -void SharedQueue::pop_front() -{ - std::unique_lock mlock(mutex_); - while (queue_.empty()) - { - cond_.wait(mlock); - } - queue_.pop_front(); -} - -template -T SharedQueue::get_n_pop_front() -{ - std::unique_lock mlock(mutex_); - while (queue_.empty()) - { - cond_.wait(mlock); - } - T temp = queue_.front(); - queue_.pop_front(); - mlock.unlock(); - cond_.notify_one(); - return temp; -} - -template -void SharedQueue::push_back(const T& item) -{ - std::unique_lock mlock(mutex_); - queue_.push_back(item); - mlock.unlock(); // unlock before notificiation to minimize mutex con - cond_.notify_one(); // notify one waiting thread - -} - -template -void SharedQueue::push_back(T&& item) -{ - std::unique_lock mlock(mutex_); - queue_.push_back(std::move(item)); - mlock.unlock(); // unlock before notificiation to minimize mutex con - cond_.notify_one(); // notify one waiting thread - -} - -template -unsigned long SharedQueue::size() -{ - std::unique_lock mlock(mutex_); - unsigned long size = queue_.size(); - mlock.unlock(); - cond_.notify_one(); - return size; -} - -template -bool SharedQueue::empty() -{ - std::unique_lock mlock(mutex_); - bool empty = queue_.empty(); - mlock.unlock(); - cond_.notify_one(); - return empty; -} - -#endif //SHAREDQUEUE_H \ No newline at end of file From 6d09c59c5d65250baf2f6f75992bc4ce095fc124 Mon Sep 17 00:00:00 2001 From: supertoto29 Date: Thu, 16 Dec 2021 13:33:06 +0000 Subject: [PATCH 140/140] remove the custom packet from provider --- src/modem_data.h | 25 ------------ src/provider_underwater_com_node.cc | 62 ++++++++++++++--------------- src/provider_underwater_com_node.h | 12 ++---- 3 files changed, 34 insertions(+), 65 deletions(-) delete mode 100644 src/modem_data.h diff --git a/src/modem_data.h b/src/modem_data.h deleted file mode 100644 index 5ba3218..0000000 --- a/src/modem_data.h +++ /dev/null @@ -1,25 +0,0 @@ -#ifndef MODEM_DATA_H -#define MODEM_DATA_H - -#pragma pack(push, 1) - -typedef struct -{ - uint8_t packetNumber; - uint8_t packetId : 7; - uint8_t endOfPacket : 1; -} Header_t; - -typedef struct -{ - Header_t header; - uint8_t killSwitchState : 4, missionSwitchState : 4; - uint16_t depth; - uint8_t missionId; - uint8_t missionState; - uint8_t torpedosState : 4, droppersState : 4; -} Modem_M64_t; - -#pragma pack(pop) - -#endif //MODEM_DATA_H \ No newline at end of file diff --git a/src/provider_underwater_com_node.cc b/src/provider_underwater_com_node.cc index 52b051a..8e5179c 100644 --- a/src/provider_underwater_com_node.cc +++ b/src/provider_underwater_com_node.cc @@ -34,7 +34,7 @@ namespace provider_underwater_com : 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); + 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)); @@ -67,27 +67,26 @@ namespace provider_underwater_com ros::shutdown(); } - void ProviderUnderwaterComNode::UnderwaterComCallback(const sonia_common::IntersubCom &msg) + void ProviderUnderwaterComNode::UnderwaterComCallback(const std_msgs::UInt64 &msg) { - uint64_t packet = 0; + // 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, &packet, sizeof(packet_array)); - + // // 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); } @@ -350,8 +349,9 @@ namespace provider_underwater_com void ProviderUnderwaterComNode::Export_To_ROS(const char (&buffer)[BUFFER_SIZE], const ssize_t size) { - Modem_M64_t packet; - uint64_t data = 0; + // 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) @@ -359,16 +359,16 @@ namespace provider_underwater_com tmp[i] = (uint8_t) buffer[i + 6]; } - std::memcpy(&data, tmp, sizeof(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; + 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); } diff --git a/src/provider_underwater_com_node.h b/src/provider_underwater_com_node.h index e75933e..12e36d6 100644 --- a/src/provider_underwater_com_node.h +++ b/src/provider_underwater_com_node.h @@ -29,19 +29,14 @@ #include #include #include -#include -#include -#include -#include #include #include #include +#include #include "Configuration.h" #include "drivers/serial.h" -#include "modem_data.h" #include -#include #include #define BUFFER_SIZE 256 @@ -60,7 +55,7 @@ class ProviderUnderwaterComNode private: - void UnderwaterComCallback(const sonia_common::IntersubCom &msg); + 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 @@ -92,7 +87,6 @@ class ProviderUnderwaterComNode ros::Subscriber underwaterComSubscriber_; ros::Publisher underwaterComPublisher_; ros::ServiceServer underwaterComService_; - sonia_common::IntersubCom msg; std::thread manage_write_thread; std::thread manage_response_thread; @@ -114,7 +108,7 @@ class ProviderUnderwaterComNode uint8_t writeSize = 0; uint8_t payload_; bool init_error_ = true; - Modem_M64_t modem_data; + // Modem_M64_t modem_data; }; }