From a296551cbe3d5e293e1ac36ef2f1c8611ec0c0e6 Mon Sep 17 00:00:00 2001 From: ad-daniel <44834743+ad-daniel@users.noreply.github.com> Date: Thu, 19 Jan 2023 08:53:20 +0100 Subject: [PATCH] Enforce clang format (#599) * Apply clangformat * Add test sources * Test pep8 * Fix workflow * Fix workflow * Fix script * pep8 fixes * Cpp fixes * Remove ament pep257 & flake8 * Update urdf2webots submodule * Revert "Update urdf2webots submodule" This reverts commit fa45543bd5d0e94de462342e9b192262baa6aab5. * Revert "Remove ament pep257 & flake8" This reverts commit 92df8a316d916d1e260b482c87dfcc3bebd35839. * Revert "Cpp fixes" This reverts commit c3a497cff51c6dd7e4f7da791e2515e9f6b578b2. * Revert "pep8 fixes" This reverts commit 141b0c85aaa55f942734cd616a8b636d5a7dd963. * Revert "Revert "pep8 fixes"" This reverts commit ad007a456bc8739f48b14dc2220f1716f909d74d. * Test * Revert "Test" This reverts commit 25934ef9d581bdecebc3eb4635d2c5cb571b01a3. * Revert "Revert "Cpp fixes"" This reverts commit 8462d81851bf866b3b742a4313e4c4e52b9deeb5. * Revert "Revert "Remove ament pep257 & flake8"" This reverts commit c689352da0ba037b7ab794434c7428259628a862. * Revert "Revert "Update urdf2webots submodule"" This reverts commit e2ed2b3efb03de99594277bc0c4c58782112697f. * Order fixes * Skip submodule tests * Finalize --- .clang-format | 88 +++++++++ .github/ISSUE_TEMPLATE/bug_report.md | 2 +- .github/workflows/tests_sources.yml | 45 +++++ tests/.gitignore | 1 + tests/sources/requirements.txt | 3 + tests/sources/test_clang_format.py | 152 ++++++++++++++ tests/sources/test_cppcheck.py | 116 +++++++++++ tests/sources/test_pep8.py | 187 ++++++++++++++++++ .../test_pep257.py => tests/test_sources.py | 20 +- webots_ros2/package.xml | 2 - webots_ros2/test/test_flake8.py | 25 --- webots_ros2/test/test_pep257.py | 25 --- .../webots_ros2_control/Ros2Control.hpp | 37 ++-- .../webots_ros2_control/Ros2ControlSystem.hpp | 42 ++-- .../Ros2ControlSystemInterface.hpp | 39 ++-- webots_ros2_control/src/Ros2Control.cpp | 70 +++---- webots_ros2_control/src/Ros2ControlSystem.cpp | 101 +++++----- .../webots_ros2_driver/PluginInterface.hpp | 39 ++-- .../webots_ros2_driver/PythonPlugin.hpp | 28 ++- .../include/webots_ros2_driver/WebotsNode.hpp | 14 +- .../plugins/Ros2SensorPlugin.hpp | 15 +- .../plugins/dynamic/Ros2IMU.hpp | 14 +- .../plugins/static/Ros2Camera.hpp | 21 +- .../plugins/static/Ros2DistanceSensor.hpp | 15 +- .../plugins/static/Ros2GPS.hpp | 10 +- .../plugins/static/Ros2LED.hpp | 16 +- .../plugins/static/Ros2Lidar.hpp | 15 +- .../plugins/static/Ros2LightSensor.hpp | 15 +- .../plugins/static/Ros2RangeFinder.hpp | 15 +- .../include/webots_ros2_driver/utils/Math.hpp | 7 +- .../webots_ros2_driver/utils/Utils.hpp | 9 +- .../scripts/webots_tcp_client.py | 3 +- webots_ros2_driver/src/Driver.cpp | 7 +- webots_ros2_driver/src/PythonPlugin.cpp | 114 ++++++----- webots_ros2_driver/src/WebotsNode.cpp | 145 ++++++-------- .../src/plugins/Ros2SensorPlugin.cpp | 11 +- .../src/plugins/dynamic/Ros2IMU.cpp | 47 ++--- .../src/plugins/static/Ros2Camera.cpp | 88 ++++----- .../src/plugins/static/Ros2DistanceSensor.cpp | 28 +-- .../src/plugins/static/Ros2GPS.cpp | 46 ++--- .../src/plugins/static/Ros2LED.cpp | 23 +-- .../src/plugins/static/Ros2Lidar.cpp | 41 ++-- .../src/plugins/static/Ros2LightSensor.cpp | 28 ++- .../src/plugins/static/Ros2RangeFinder.cpp | 53 ++--- webots_ros2_driver/src/utils/Math.cpp | 69 ++----- webots_ros2_driver/src/utils/Utils.cpp | 37 ++-- .../webots_ros2_driver/ros2_supervisor.py | 54 ++--- .../webots_ros2_driver/urdf_spawner.py | 14 +- .../webots_ros2_driver/utils.py | 16 +- .../webots_ros2_driver/webots_launcher.py | 18 +- webots_ros2_epuck/package.xml | 2 - webots_ros2_epuck/test/test_flake8.py | 25 --- webots_ros2_epuck/test/test_pep257.py | 25 --- webots_ros2_importer/package.xml | 2 - webots_ros2_importer/pytest.ini | 4 +- webots_ros2_importer/test/test_flake8.py | 25 --- .../webots_ros2_importer/urdf2webots | 2 +- webots_ros2_mavic/package.xml | 2 - webots_ros2_mavic/test/test_flake8.py | 25 --- webots_ros2_mavic/test/test_pep257.py | 25 --- .../msg/CameraRecognitionObject.msg | 2 +- webots_ros2_tesla/package.xml | 2 - webots_ros2_tesla/test/test_flake8.py | 25 --- webots_ros2_tesla/test/test_pep257.py | 25 --- webots_ros2_tests/package.xml | 2 - webots_ros2_tests/test/test_flake8.py | 23 --- webots_ros2_tests/test/test_pep257.py | 23 --- webots_ros2_tiago/package.xml | 2 - webots_ros2_tiago/test/test_flake8.py | 25 --- webots_ros2_tiago/test/test_pep257.py | 25 --- webots_ros2_turtlebot/package.xml | 2 - webots_ros2_turtlebot/test/test_flake8.py | 25 --- webots_ros2_turtlebot/test/test_pep257.py | 25 --- webots_ros2_universal_robot/package.xml | 2 - .../resource/moveit_movegroup.yaml | 2 +- .../test/test_flake8.py | 25 --- .../test/test_pep257.py | 25 --- 77 files changed, 1213 insertions(+), 1214 deletions(-) create mode 100644 .clang-format create mode 100644 .github/workflows/tests_sources.yml create mode 100644 tests/.gitignore create mode 100644 tests/sources/requirements.txt create mode 100755 tests/sources/test_clang_format.py create mode 100755 tests/sources/test_cppcheck.py create mode 100755 tests/sources/test_pep8.py rename webots_ros2_importer/test/test_pep257.py => tests/test_sources.py (64%) mode change 100644 => 100755 delete mode 100644 webots_ros2/test/test_flake8.py delete mode 100644 webots_ros2/test/test_pep257.py delete mode 100644 webots_ros2_epuck/test/test_flake8.py delete mode 100644 webots_ros2_epuck/test/test_pep257.py delete mode 100644 webots_ros2_importer/test/test_flake8.py delete mode 100644 webots_ros2_mavic/test/test_flake8.py delete mode 100644 webots_ros2_mavic/test/test_pep257.py delete mode 100644 webots_ros2_tesla/test/test_flake8.py delete mode 100644 webots_ros2_tesla/test/test_pep257.py delete mode 100644 webots_ros2_tests/test/test_flake8.py delete mode 100644 webots_ros2_tests/test/test_pep257.py delete mode 100644 webots_ros2_tiago/test/test_flake8.py delete mode 100644 webots_ros2_tiago/test/test_pep257.py delete mode 100644 webots_ros2_turtlebot/test/test_flake8.py delete mode 100644 webots_ros2_turtlebot/test/test_pep257.py delete mode 100644 webots_ros2_universal_robot/test/test_flake8.py delete mode 100644 webots_ros2_universal_robot/test/test_pep257.py diff --git a/.clang-format b/.clang-format new file mode 100644 index 000000000..bc49dbcb2 --- /dev/null +++ b/.clang-format @@ -0,0 +1,88 @@ +--- +Language: Cpp +AccessModifierOffset: -2 +AlignAfterOpenBracket: Align +AlignEscapedNewlines: Left +AlignOperands: true +AllowAllConstructorInitializersOnNextLine: false +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: InlineOnly +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: false +AlwaysBreakTemplateDeclarations: false +BinPackArguments: true +BinPackParameters: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: AfterColon +BreakStringLiterals: true +ColumnLimit: 128 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 2 +ContinuationIndentWidth: 2 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeCategories: + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentWidth: 2 +IndentWrappedFunctionNames: true +JavaScriptQuotes: Single +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: All +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Right +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: false +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: false +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Cpp11 +TabWidth: 8 +UseTab: Never +... diff --git a/.github/ISSUE_TEMPLATE/bug_report.md b/.github/ISSUE_TEMPLATE/bug_report.md index 5723c44df..e713da515 100644 --- a/.github/ISSUE_TEMPLATE/bug_report.md +++ b/.github/ISSUE_TEMPLATE/bug_report.md @@ -26,7 +26,7 @@ If applicable, add screenshots to help explain your problem. **System** - Webots Version: [e.g., R2019b, R2020a revision 1] - - ROS Version: [e.g., Dashing, Eloquent] + - ROS Version: [e.g., Dashing, Eloquent] - Operating System: [e.g., Windows 10, Linux Ubuntu 18.04, macOS Mojave] - Graphics Card: [e.g., NVIDIA GeForce RTX 2080 11 GB, AMD Radeon RX 580 8GB, etc.] diff --git a/.github/workflows/tests_sources.yml b/.github/workflows/tests_sources.yml new file mode 100644 index 000000000..239e5a2f9 --- /dev/null +++ b/.github/workflows/tests_sources.yml @@ -0,0 +1,45 @@ +name: Test Sources + +on: + pull_request: + types: [opened, synchronize, reopened, labeled, unlabeled] + branches-ignore: + - 'released' + schedule: + - cron: '0 22 * * *' + +defaults: + run: + shell: bash + +concurrency: + group: ${{ github.workflow }}-${{ github.ref }} + cancel-in-progress: true + +jobs: + test-sources: + strategy: + fail-fast: false + matrix: + os: [ubuntu-20.04] + python: ['3.10'] + include: + - os: ubuntu-20.04 + DEPENDENCIES_INSTALLATION: "sudo apt -y install clang-format-10 cppcheck" + runs-on: ${{ matrix.os }} + steps: + - uses: actions/checkout@v3 + if: github.event_name == 'schedule' || github.event.pull_request.draft == false || contains(github.event.pull_request.labels.*.name, 'test sources') + with: + submodules: true + - name: Set up Python 3.10 + if: github.event_name == 'schedule' || github.event.pull_request.draft == false || contains(github.event.pull_request.labels.*.name, 'test sources') + uses: actions/setup-python@v4 + with: + python-version: '3.10' + - name: Test Sources + if: github.event_name == 'schedule' || github.event.pull_request.draft == false || contains(github.event.pull_request.labels.*.name, 'test sources') + run: | + ${{ matrix.DEPENDENCIES_INSTALLATION }} + pip install -r tests/sources/requirements.txt + python3 -m unittest discover -s tests/sources/ diff --git a/tests/.gitignore b/tests/.gitignore new file mode 100644 index 000000000..174b880f6 --- /dev/null +++ b/tests/.gitignore @@ -0,0 +1 @@ +/cppcheck_report.txt diff --git a/tests/sources/requirements.txt b/tests/sources/requirements.txt new file mode 100644 index 000000000..faecd21e9 --- /dev/null +++ b/tests/sources/requirements.txt @@ -0,0 +1,3 @@ +pycodestyle +pyflakes +image diff --git a/tests/sources/test_clang_format.py b/tests/sources/test_clang_format.py new file mode 100755 index 000000000..9d21390c2 --- /dev/null +++ b/tests/sources/test_clang_format.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python + +# Copyright 1996-2023 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Test that the C, C++ and shader source code is compliant with ClangFormat.""" +import unittest + +import difflib +import os +import shutil +import subprocess + +from io import open + + +class TestClangFormat(unittest.TestCase): + """Unit test for ClangFormat compliance.""" + + def setUp(self): + """Set up called before each test.""" + self.ROOT_FOLDER = os.path.dirname(os.path.dirname(os.path.dirname(__file__))) + print(self.ROOT_FOLDER) + + def _runClangFormat(self, f): + """Run clang format on 'f' file.""" + return subprocess.check_output(['clang-format', '-style=file', f]) + + def test_clang_format_is_correctly_installed(self): + """Test ClangFormat is correctly installed.""" + self.assertTrue( + shutil.which('clang-format') is not None, + msg='ClangFormat is not installed on this computer.' + ) + clangFormatConfigFile = os.path.join(self.ROOT_FOLDER, '.clang-format') + self.assertTrue( + os.path.exists(clangFormatConfigFile), + msg=clangFormatConfigFile + ' not found.' + ) + + def test_sources_are_clang_format_compliant(self): + """Test that sources are ClangFormat compliant.""" + directories = [ + 'webots_ros2', + 'webots_ros2_control', + 'webots_ros2_driver', + 'webots_ros2_epuck', + 'webots_ros2_importer', + 'webots_ros2_mavic', + 'webots_ros2_msgs', + 'webots_ros2_tesla', + 'webots_ros2_tests', + 'webots_ros2_tiago', + 'webots_ros2_turtlebot', + 'webots_ros2_universal_robot' + ] + skippedPaths = [ + 'webots_ros2_driver/webots/', + 'webots_ros2_importer/webots_ros2_importer/urdf2webots/tests' + ] + skippedFiles = [ + ] + skippedDirectories = [ + ] + skippedPathsFull = [os.path.join(self.ROOT_FOLDER, os.path.normpath(path)) for path in skippedPaths] + skippedFilesFull = [os.path.join(self.ROOT_FOLDER, os.path.normpath(file)) for file in skippedFiles] + + extensions = ['c', 'h', 'cpp', 'hpp', 'cc', 'hh', 'c++', 'h++', 'vert', 'frag'] + modified_files = os.path.join(self.ROOT_FOLDER, 'tests', 'sources', 'modified_files.txt') + sources = [] + if os.path.isfile(modified_files): + with open(modified_files, 'r') as file: + for line in file: + line = line.strip() + extension = os.path.splitext(line)[1][1:].lower() + if extension not in extensions: + continue + found = False + for directory in directories: + if line.startswith(directory): + found = True + break + if not found: + continue + found = False + for directory in skippedPaths + skippedFiles: + if line.startswith(directory): + found = True + break + if found: + continue + for directory in skippedDirectories: + currentDirectories = line.split(os.sep) + if directory in currentDirectories: + found = True + if found: + continue + sources.append(os.path.normpath(line)) + else: + for directory in directories: + path = os.path.join(self.ROOT_FOLDER, os.path.normpath(directory)) + for rootPath, dirNames, fileNames in os.walk(path): + shouldContinue = False + for skippedPath in skippedPathsFull: + if rootPath.startswith(skippedPath): + shouldContinue = True + break + for directory in skippedDirectories: + currentDirectories = rootPath.replace(self.ROOT_FOLDER + os.sep, '').split(os.sep) + if directory in currentDirectories: + shouldContinue = True + break + if shouldContinue: + continue + for fileName in fileNames: + extension = os.path.splitext(fileName)[1][1:].lower() + if extension not in extensions: + continue + path = os.path.normpath(os.path.join(rootPath, fileName)) + if path not in skippedFilesFull: + sources.append(path) + curdir = os.getcwd() + os.chdir(self.ROOT_FOLDER) + for source in sources: + diff = '' + with open(source, encoding='utf8') as file: + try: + for line in difflib.context_diff(self._runClangFormat(source).decode('utf-8').splitlines(), + file.read().splitlines()): + diff += line + '\n' + except UnicodeDecodeError: + self.assertTrue(False, msg='utf-8 decode problem in %s' % source) + self.assertTrue( + len(diff) == 0, + msg='Source file "%s" is not compliant with ClangFormat:\n\nDIFF:%s' % (source, diff) + ) + os.chdir(curdir) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/sources/test_cppcheck.py b/tests/sources/test_cppcheck.py new file mode 100755 index 000000000..48d88029d --- /dev/null +++ b/tests/sources/test_cppcheck.py @@ -0,0 +1,116 @@ +#!/usr/bin/env python + +# Copyright 1996-2023 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Test quality of the source code using Cppcheck.""" +import unittest +import os +import multiprocessing +import shutil + + +class TestCppCheck(unittest.TestCase): + """Unit test for CppCheck errors.""" + + def setUp(self): + """Set up called before each test.""" + self.ROOT_FOLDER = os.path.dirname(os.path.dirname(os.path.dirname(__file__))) + self.reportFilename = os.path.join(self.ROOT_FOLDER, 'tests', 'cppcheck_report.txt') + self.extensions = ['c', 'h', 'cpp', 'hpp', 'cc', 'hh', 'c++', 'h++'] + + def test_cppcheck_is_correctly_installed(self): + """Test Cppcheck is correctly installed.""" + self.assertTrue( + shutil.which('cppcheck') is not None, + msg='Cppcheck is not installed on this computer.' + ) + + def run_cppcheck(self, command): + """Run Cppcheck command and check for errors.""" + curdir = os.getcwd() + os.chdir(self.ROOT_FOLDER) + if os.path.isfile(self.reportFilename): + os.remove(self.reportFilename) + os.system(command) # warning: on Windows, the length of command is limited to 8192 characters + if os.path.isfile(self.reportFilename): + with open(self.reportFilename, 'r') as reportFile: + reportText = reportFile.read() + self.assertTrue( + not reportText, + msg='Cppcheck detected some errors:\n\n%s' % reportText + ) + os.remove(self.reportFilename) + os.chdir(curdir) + + def add_source_files(self, sourceDirs, skippedDirs, skippedfiles=[]): + command = '' + modified_files = os.path.join(self.ROOT_FOLDER, 'tests', 'sources', 'modified_files.txt') + if os.path.isfile(modified_files): + with open(modified_files, 'r') as file: + for line in file: + line = line.strip() + extension = os.path.splitext(line)[1][1:].lower() + if extension not in self.extensions: + continue + for sourceDir in sourceDirs: + if line.startswith(sourceDir): + shouldSkip = False + for skipped in skippedDirs + skippedfiles: + if line.startswith(skipped): + shouldSkip = True + break + if not shouldSkip: + command += ' \"' + line + '\"' + continue + for source in skippedfiles: + command += ' --suppress=\"*:' + source + '\"' + else: + for source in skippedfiles: + command += ' --suppress=\"*:' + source + '\"' + for source in skippedDirs: + command += ' -i\"' + source + '\"' + for source in sourceDirs: + command += ' \"' + source + '\"' + return command + + def test_sources_with_cppcheck(self): + """Test with Cppcheck.""" + sourceDirs = [ + 'webots_ros2_control/src', + 'webots_ros2_driver/src', + ] + skippedDirs = [ + + ] + includeDirs = [ + 'webots_ros2_control/include', + ] + command = 'cppcheck --enable=warning,style,performance,portability --inconclusive --force -q' + command += ' -j %s' % str(multiprocessing.cpu_count()) + command += ' --inline-suppr --suppress=invalidPointerCast --suppress=useStlAlgorithm --suppress=uninitMemberVar ' + command += ' --suppress=noCopyConstructor --suppress=noOperatorEq --suppress=strdupCalled --suppress=unknownMacro' + # command += ' --xml ' # Uncomment this line to get more information on the errors + command += ' --output-file=\"' + self.reportFilename + '\"' + for include in includeDirs: + command += ' -I\"' + include + '\"' + sources = self.add_source_files(sourceDirs, skippedDirs) + if not sources: + return + command += sources + self.run_cppcheck(command) + + +if __name__ == '__main__': + unittest.main() diff --git a/tests/sources/test_pep8.py b/tests/sources/test_pep8.py new file mode 100755 index 000000000..71b0dabf3 --- /dev/null +++ b/tests/sources/test_pep8.py @@ -0,0 +1,187 @@ +#!/usr/bin/env python + +# Copyright 1996-2023 Cyberbotics Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +"""Test module of the PEP8 format in the tests.""" +import unittest + +import _ast +import fnmatch +import pycodestyle +import os +import sys +from io import open # needed for compatibility with Python 2.7 for open(file, encoding='utf-8') + +from pyflakes import checker +from pyflakes.reporter import Reporter + +ROOT_FOLDER = os.path.dirname(os.path.dirname(os.path.dirname(__file__))) +skippedDirectories = [ + '.git', + 'webots_ros2_importer/webots_ros2_importer/urdf2webots/', + 'webots_ros2_driver/webots/lib' +] +skippedDirectoriesFull = [os.path.join(ROOT_FOLDER, os.path.normpath(path)) for path in skippedDirectories] + + +class FlakesReporter(Reporter): + """Flakes reporter.""" + + def __init__(self): + self.error = '' + + def unexpectedError(self, filename, msg): + """Handle unexpected errors.""" + self.error += '%s: %s\n' % (filename, msg) + + def syntaxError(self, filename, msg, lineno, offset, text): + """Handle syntax errors.""" + line = text.splitlines()[-1] + if offset is not None: + offset = offset - (len(text) - len(line)) + self.error += '%s:%d: %s\n' % (filename, lineno, msg) + self.error += line + '\n' + if offset is not None: + self.error += ' ' * (offset + 1) + '^\n' + + def flake(self, message): + """Add message to error string.""" + self.error += str(message) + '\n' + + +def checkFlakes(codeString, filename, reporter): + """Check the Python source given by codeString} for flakes.""" + # First, compile into an AST and handle syntax errors. + try: + tree = compile(codeString, filename, "exec", _ast.PyCF_ONLY_AST) + except SyntaxError: + value = sys.exc_info()[1] + msg = value.args[0] + (lineno, offset, text) = value.lineno, value.offset, value.text + # If there's an encoding problem with the file, the text is None. + if text is None: + # Avoid using msg, since for the only known case, it contains a + # bogus message that claims the encoding the file declared was + # unknown. + reporter.unexpectedError(filename, 'problem decoding source') + else: + reporter.syntaxError(filename, msg, lineno, offset, text) + return + except Exception: + reporter.unexpectedError(filename, 'problem decoding source') + return + # Okay, it's syntactically valid. Now check it. + w = checker.Checker(tree, filename) + w.messages.sort(key=lambda m: m.lineno) + for warning in w.messages: + reporter.flake(warning) + + +def checkFlakesPath(filename, reporter): + """Check the given path, printing out any warnings detected.""" + try: + with open(filename, encoding='utf-8') as f: + codestr = f.read() + '\n' + except UnicodeError: + reporter.unexpectedError(filename, 'problem decoding source') + return + except IOError: + msg = sys.exc_info()[1] + reporter.unexpectedError(filename, msg.args[1]) + return + checkFlakes(codestr.encode('utf-8'), filename, reporter) + + +class CustomReport(pycodestyle.StandardReport): + """Collect report, and overload the string operator.""" + + results = [] + + def get_file_results(self): + """Overload this function to collect the report.""" + if self._deferred_print: + self._deferred_print.sort() + for line_number, offset, code, text, _ in self._deferred_print: + self.results.append({ + 'path': self.filename, + 'row': self.line_offset + line_number, + 'col': offset + 1, + 'code': code, + 'text': text, + }) + return self.file_errors + + def __str__(self): + """Overload the string operator.""" + output = 'Report:\n' + for result in self.results: + output += '%s:%d:%d: %s: %s\n' % ( + result['path'], + result['row'], + result['col'], + result['code'], + result['text'] + ) + return output + + +class TestCodeFormat(unittest.TestCase): + """Unit test of the PEP8 format in the tests.""" + + def setUp(self): + """Get all the python files.""" + self.files = [] + for rootPath, dirNames, fileNames in os.walk(ROOT_FOLDER): + for fileName in fnmatch.filter(fileNames, '*.py'): + shouldContinue = False + for skippedDirectory in skippedDirectoriesFull: + if rootPath.startswith(skippedDirectory): + shouldContinue = True + break + if shouldContinue: + continue + filePath = os.path.join(rootPath, fileName) + if sys.version_info[0] < 3: + with open(filePath) as file: + if file.readline().startswith('#!/usr/bin/env python3'): + continue + self.files.append(filePath) + + def test_pep8_conformance(self): + """Test that the tests are PEP8 compliant.""" + # Use pep8 module to detect 'W' and 'E' errors + checker = pycodestyle.StyleGuide( + quiet=True, + paths=self.files, + reporter=CustomReport, + ) + checker.options.max_line_length = 128 + report = checker.check_files() + self.assertEqual( + report.total_errors, 0, + msg='PEP8 errors:\n%s' % (report) + ) + # Use flakes module to detect 'F' errors + reporter = FlakesReporter() + for fileName in self.files: + checkFlakesPath(fileName, reporter) + self.assertTrue( + not reporter.error, + msg='PEP8 errors:\n%s' % (reporter.error) + ) + + +if __name__ == '__main__': + unittest.main() diff --git a/webots_ros2_importer/test/test_pep257.py b/tests/test_sources.py old mode 100644 new mode 100755 similarity index 64% rename from webots_ros2_importer/test/test_pep257.py rename to tests/test_sources.py index 06a350a3f..3f247627b --- a/webots_ros2_importer/test/test_pep257.py +++ b/tests/test_sources.py @@ -1,3 +1,5 @@ +#!/usr/bin/env python + # Copyright 1996-2023 Cyberbotics Ltd. # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -12,14 +14,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - +"""Unit test.""" +import sys +import unittest -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', '--exclude', 'webots_ros2_importer/urdf2webots/']) - assert rc == 0, 'Found code style errors / warnings' +# Run the sources tests +test_suite = unittest.defaultTestLoader.discover('sources', '*.py') +test_runner = unittest.TextTestRunner(verbosity=2, resultclass=unittest.TextTestResult) +result = test_runner.run(test_suite) +sys.exit(not result.wasSuccessful()) diff --git a/webots_ros2/package.xml b/webots_ros2/package.xml index 0a3dd8903..d46ee8a86 100644 --- a/webots_ros2/package.xml +++ b/webots_ros2/package.xml @@ -26,8 +26,6 @@ webots_ros2_universal_robot ament_copyright - ament_flake8 - ament_pep257 python3-pytest webots_ros2_tests diff --git a/webots_ros2/test/test_flake8.py b/webots_ros2/test/test_flake8.py deleted file mode 100644 index c79ba0d9c..000000000 --- a/webots_ros2/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=[]) - assert rc == 0, 'Found errors' diff --git a/webots_ros2/test/test_pep257.py b/webots_ros2/test/test_pep257.py deleted file mode 100644 index fac857efd..000000000 --- a/webots_ros2/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp index 13988a3b8..6dc1262e5 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2Control.hpp @@ -17,42 +17,41 @@ #include #include -#include #include +#include #if FOXY - #include "hardware_interface/base_interface.hpp" - #include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" #endif -#include "hardware_interface/system_interface.hpp" +#include "controller_manager/controller_manager.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "controller_manager/controller_manager.hpp" #include "rclcpp/macros.hpp" +#include "webots_ros2_control/Ros2ControlSystemInterface.hpp" #include "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/WebotsNode.hpp" -#include "webots_ros2_control/Ros2ControlSystemInterface.hpp" -namespace webots_ros2_control -{ - class Ros2Control : public webots_ros2_driver::PluginInterface - { +namespace webots_ros2_control { + class Ros2Control : public webots_ros2_driver::PluginInterface { public: + Ros2Control(); void step() override; void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; - private: - webots_ros2_driver::WebotsNode *mNode; - std::shared_ptr> mHardwareLoader; - std::shared_ptr mControllerManager; - int mControlPeriodMs; - int mLastControlUpdateMs; + private: + webots_ros2_driver::WebotsNode *mNode; + std::shared_ptr> mHardwareLoader; + std::shared_ptr mControllerManager; + int mControlPeriodMs; + int mLastControlUpdateMs; - std::thread mThreadExecutor; - rclcpp::executors::MultiThreadedExecutor::SharedPtr mExecutor; + std::thread mThreadExecutor; + rclcpp::executors::MultiThreadedExecutor::SharedPtr mExecutor; }; -} +} // namespace webots_ros2_control #endif diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp index e924d94da..3a38af0a6 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystem.hpp @@ -20,24 +20,23 @@ #include #if FOXY - #include "hardware_interface/base_interface.hpp" - #include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" #endif -#include "hardware_interface/system_interface.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/macros.hpp" #include "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/WebotsNode.hpp" -#include "webots_ros2_control/Ros2ControlSystemInterface.hpp" #include "webots/motor.h" #include "webots/position_sensor.h" +#include "webots_ros2_control/Ros2ControlSystemInterface.hpp" -namespace webots_ros2_control -{ +namespace webots_ros2_control { struct Joint { double positionCommand; double position; @@ -53,20 +52,23 @@ namespace webots_ros2_control WbDeviceTag sensor; }; - class Ros2ControlSystem : public Ros2ControlSystemInterface - { + class Ros2ControlSystem : public Ros2ControlSystemInterface { public: + Ros2ControlSystem(); void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) override; - #if FOXY - hardware_interface::return_type configure(const hardware_interface::HardwareInfo &info) override; - hardware_interface::return_type start() override; - hardware_interface::return_type stop() override; - #else - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State & /*previous_state*/) override; - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) override; - #endif +#if FOXY + hardware_interface::return_type configure(const hardware_interface::HardwareInfo &info) override; + hardware_interface::return_type start() override; + hardware_interface::return_type stop() override; +#else + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init( + const hardware_interface::HardwareInfo &info) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) override; +#endif std::vector export_state_interfaces() override; std::vector export_command_interfaces() override; @@ -74,14 +76,14 @@ namespace webots_ros2_control hardware_interface::return_type read() override; hardware_interface::return_type write() override; #else // HUMBLE, ROLLING - hardware_interface::return_type read(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) override; - hardware_interface::return_type write(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) override; + hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; #endif private: webots_ros2_driver::WebotsNode *mNode; std::vector mJoints; }; -} +} // namespace webots_ros2_control #endif diff --git a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp index d25be9128..f9cbac55e 100644 --- a/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp +++ b/webots_ros2_control/include/webots_ros2_control/Ros2ControlSystemInterface.hpp @@ -20,34 +20,31 @@ #include #if FOXY - #include "hardware_interface/base_interface.hpp" - #include "hardware_interface/types/hardware_interface_status_values.hpp" +#include "hardware_interface/base_interface.hpp" +#include "hardware_interface/types/hardware_interface_status_values.hpp" #endif -#include "hardware_interface/system_interface.hpp" +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp" #include "webots_ros2_driver/PluginInterface.hpp" -#include #include "webots_ros2_driver/WebotsNode.hpp" -namespace webots_ros2_control -{ - #if FOXY - class Ros2ControlSystemInterface : public hardware_interface::BaseInterface - { - public: - virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0; - }; - #else - class Ros2ControlSystemInterface : public hardware_interface::SystemInterface - { - public: - virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0; - }; - #endif - -} +namespace webots_ros2_control { +#if FOXY + class Ros2ControlSystemInterface : public hardware_interface::BaseInterface { + public: + virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0; + }; +#else + class Ros2ControlSystemInterface : public hardware_interface::SystemInterface { + public: + virtual void init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) = 0; + }; +#endif + +} // namespace webots_ros2_control #endif diff --git a/webots_ros2_control/src/Ros2Control.cpp b/webots_ros2_control/src/Ros2Control.cpp index 466627d8f..a79d805fe 100644 --- a/webots_ros2_control/src/Ros2Control.cpp +++ b/webots_ros2_control/src/Ros2Control.cpp @@ -20,11 +20,11 @@ #include #include -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "hardware_interface/resource_manager.hpp" #include "hardware_interface/component_parser.hpp" -#include "rclcpp/rclcpp.hpp" +#include "hardware_interface/resource_manager.hpp" +#include "hardware_interface/types/hardware_interface_type_values.hpp" #include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" #include @@ -32,19 +32,18 @@ const double CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS = 1.0; -namespace webots_ros2_control -{ +namespace webots_ros2_control { - void Ros2Control::step() - { + Ros2Control::Ros2Control() { mNode = NULL; } + + void Ros2Control::step() { const int nowMs = wb_robot_get_time() * 1000.0; const int periodMs = nowMs - mLastControlUpdateMs; - const rclcpp::Duration dt = rclcpp::Duration::from_seconds(mControlPeriodMs / 1000.0); - if (periodMs >= mControlPeriodMs) - { + if (periodMs >= mControlPeriodMs) { #if FOXY mControllerManager->read(); #else + const rclcpp::Duration dt = rclcpp::Duration::from_seconds(mControlPeriodMs / 1000.0); mControllerManager->read(mNode->get_clock()->now(), dt); #endif @@ -56,55 +55,46 @@ namespace webots_ros2_control #endif #if FOXY - mControllerManager->write(); -#else // HUMBLE, ROLLING - mControllerManager->write(mNode->get_clock()->now(), dt); + mControllerManager->write(); +#else // HUMBLE, ROLLING + mControllerManager->write(mNode->get_clock()->now(), dt); #endif } } - void Ros2Control::init(webots_ros2_driver::WebotsNode *node, std::unordered_map &) - { + void Ros2Control::init(webots_ros2_driver::WebotsNode *node, std::unordered_map &) { mNode = node; mLastControlUpdateMs = 0; // Load hardware - try - { + try { mHardwareLoader.reset(new pluginlib::ClassLoader( - "webots_ros2_control", - "webots_ros2_control::Ros2ControlSystemInterface")); - } - catch (pluginlib::LibraryLoadException &ex) - { + "webots_ros2_control", "webots_ros2_control::Ros2ControlSystemInterface")); + } catch (pluginlib::LibraryLoadException &ex) { throw std::runtime_error("Hardware loader cannot be created: " + atoi(ex.what())); } // Control Hardware std::string urdfString; std::vector controlHardware; - std::unique_ptr resourceManager = std::make_unique(); - try - { + std::unique_ptr resourceManager = + std::make_unique(); + try { urdfString = mNode->urdf(); controlHardware = hardware_interface::parse_control_resources_from_urdf(urdfString); - } - catch (const std::runtime_error &ex) - { + } catch (const std::runtime_error &ex) { throw std::runtime_error("URDF cannot be parsed by a `ros2_control` component parser: " + atoi(ex.what())); } - for (unsigned int i = 0; i < controlHardware.size(); i++) - { - + for (unsigned int i = 0; i < controlHardware.size(); i++) { // Necessary hotfix for renamed variables present in "hardware_interface" package for versions above 3.5 (#590) #if HARDWARE_INTERFACE_VERSION_MAJOR >= 3 && HARDWARE_INTERFACE_VERSION_MINOR >= 5 const std::string pluginName = controlHardware[i].hardware_plugin_name; - auto webotsSystem = std::unique_ptr( - mHardwareLoader->createUnmanagedInstance(pluginName)); + auto webotsSystem = + std::unique_ptr(mHardwareLoader->createUnmanagedInstance(pluginName)); #else const std::string hardwareType = controlHardware[i].hardware_class_type; auto webotsSystem = std::unique_ptr( - mHardwareLoader->createUnmanagedInstance(hardwareType)); + mHardwareLoader->createUnmanagedInstance(hardwareType)); #endif webotsSystem->init(mNode, controlHardware[i]); #if FOXY @@ -131,17 +121,21 @@ namespace webots_ros2_control controlPeriodProductMs += wb_robot_get_basic_time_step(); if (abs(controlPeriodProductMs - mControlPeriodMs) > CONTROLLER_MANAGER_ALLOWED_SAMPLE_ERROR_MS) - RCLCPP_WARN_STREAM(node->get_logger(), "Desired controller update period (" << mControlPeriodMs << "ms / " << updateRate << "Hz) is different from the Webots timestep (" << wb_robot_get_basic_time_step() << "ms). Please adjust the `update_rate` parameter in the `controller_manager` or the `basicTimeStep` parameter in the Webots `WorldInfo` node."); + RCLCPP_WARN_STREAM(node->get_logger(), "Desired controller update period (" + << mControlPeriodMs << "ms / " << updateRate + << "Hz) is different from the Webots timestep (" + << wb_robot_get_basic_time_step() + << "ms). Please adjust the `update_rate` parameter in the `controller_manager` " + "or the `basicTimeStep` parameter in the Webots `WorldInfo` node."); // Spin mExecutor->add_node(mControllerManager); - auto spin = [this]() - { + auto spin = [this]() { while (rclcpp::ok()) mExecutor->spin_once(); }; mThreadExecutor = std::thread(spin); } -} +} // namespace webots_ros2_control PLUGINLIB_EXPORT_CLASS(webots_ros2_control::Ros2Control, webots_ros2_driver::PluginInterface) diff --git a/webots_ros2_control/src/Ros2ControlSystem.cpp b/webots_ros2_control/src/Ros2ControlSystem.cpp index 415466066..3dd151fe2 100644 --- a/webots_ros2_control/src/Ros2ControlSystem.cpp +++ b/webots_ros2_control/src/Ros2ControlSystem.cpp @@ -21,28 +21,26 @@ #include #include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" #include "pluginlib/class_list_macros.hpp" +#include "rclcpp/rclcpp.hpp" -#include #include +#include -namespace webots_ros2_control -{ - void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) - { +namespace webots_ros2_control { + Ros2ControlSystem::Ros2ControlSystem() { mNode = NULL; } + void Ros2ControlSystem::init(webots_ros2_driver::WebotsNode *node, const hardware_interface::HardwareInfo &info) { mNode = node; - for (hardware_interface::ComponentInfo component : info.joints) - { + for (hardware_interface::ComponentInfo component : info.joints) { Joint joint; joint.name = component.name; WbDeviceTag device = wb_robot_get_device(joint.name.c_str()); WbNodeType type = wb_device_get_node_type(device); - joint.motor = (type == WB_NODE_LINEAR_MOTOR || type == WB_NODE_ROTATIONAL_MOTOR) ? device : wb_position_sensor_get_motor(device); - device = (component.parameters.count("sensor") == 0) ? - wb_robot_get_device(joint.name.c_str()) : - wb_robot_get_device(component.parameters.at("sensor").c_str()); + joint.motor = + (type == WB_NODE_LINEAR_MOTOR || type == WB_NODE_ROTATIONAL_MOTOR) ? device : wb_position_sensor_get_motor(device); + device = (component.parameters.count("sensor") == 0) ? wb_robot_get_device(joint.name.c_str()) : + wb_robot_get_device(component.parameters.at("sensor").c_str()); type = wb_device_get_node_type(device); joint.sensor = (type == WB_NODE_POSITION_SENSOR) ? device : wb_motor_get_position_sensor(device); @@ -63,8 +61,7 @@ namespace webots_ros2_control joint.acceleration = NAN; // Configure the command interface - for (hardware_interface::InterfaceInfo commandInterface : component.command_interfaces) - { + for (hardware_interface::InterfaceInfo commandInterface : component.command_interfaces) { if (commandInterface.name == "position") joint.controlPosition = true; else if (commandInterface.name == "velocity") @@ -74,8 +71,7 @@ namespace webots_ros2_control else throw std::runtime_error("Invalid hardware info name `" + commandInterface.name + "`"); } - if (joint.motor && joint.controlVelocity && !joint.controlPosition) - { + if (joint.motor && joint.controlVelocity && !joint.controlPosition) { wb_motor_set_position(joint.motor, INFINITY); wb_motor_set_velocity(joint.motor, 0.0); } @@ -85,75 +81,74 @@ namespace webots_ros2_control } #if FOXY - hardware_interface::return_type Ros2ControlSystem::configure(const hardware_interface::HardwareInfo &info) - { - if (configure_default(info) != hardware_interface::return_type::OK) - { + hardware_interface::return_type Ros2ControlSystem::configure(const hardware_interface::HardwareInfo &info) { + if (configure_default(info) != hardware_interface::return_type::OK) { return hardware_interface::return_type::ERROR; } status_ = hardware_interface::status::CONFIGURED; return hardware_interface::return_type::OK; } #else - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_init(const hardware_interface::HardwareInfo &info) - { - if (hardware_interface::SystemInterface::on_init(info) != rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) - { + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_init( + const hardware_interface::HardwareInfo &info) { + if (hardware_interface::SystemInterface::on_init(info) != + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } #endif - std::vector Ros2ControlSystem::export_state_interfaces() - { + std::vector Ros2ControlSystem::export_state_interfaces() { std::vector interfaces; for (Joint &joint : mJoints) if (joint.sensor) { - interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &(joint.position))); - interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_VELOCITY, &(joint.velocity))); - interfaces.emplace_back(hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_ACCELERATION, &(joint.acceleration))); + interfaces.emplace_back( + hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_POSITION, &(joint.position))); + interfaces.emplace_back( + hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_VELOCITY, &(joint.velocity))); + interfaces.emplace_back( + hardware_interface::StateInterface(joint.name, hardware_interface::HW_IF_ACCELERATION, &(joint.acceleration))); } return interfaces; } - std::vector Ros2ControlSystem::export_command_interfaces() - { + std::vector Ros2ControlSystem::export_command_interfaces() { std::vector interfaces; for (Joint &joint : mJoints) - if (joint.motor) - { + if (joint.motor) { if (joint.controlPosition) - interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &(joint.positionCommand))); + interfaces.emplace_back( + hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &(joint.positionCommand))); if (joint.controlEffort) - interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_EFFORT, &(joint.effortCommand))); + interfaces.emplace_back( + hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_EFFORT, &(joint.effortCommand))); if (joint.controlVelocity) - interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_VELOCITY, &(joint.velocityCommand))); + interfaces.emplace_back( + hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_VELOCITY, &(joint.velocityCommand))); } return interfaces; } #if FOXY - hardware_interface::return_type Ros2ControlSystem::start() - { + hardware_interface::return_type Ros2ControlSystem::start() { status_ = hardware_interface::status::STARTED; return hardware_interface::return_type::OK; } - hardware_interface::return_type Ros2ControlSystem::stop() - { + hardware_interface::return_type Ros2ControlSystem::stop() { status_ = hardware_interface::status::STOPPED; return hardware_interface::return_type::OK; } #else - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) - { + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) - { + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Ros2ControlSystem::on_deactivate( + const rclcpp_lifecycle::State & /*previous_state*/) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } #endif @@ -161,7 +156,7 @@ namespace webots_ros2_control #if FOXY hardware_interface::return_type Ros2ControlSystem::read() #else // HUMBLE, ROLLING - hardware_interface::return_type Ros2ControlSystem::read(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) + hardware_interface::return_type Ros2ControlSystem::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) #endif { static double lastReadTime = 0; @@ -169,8 +164,7 @@ namespace webots_ros2_control const double deltaTime = wb_robot_get_time() - lastReadTime; lastReadTime = wb_robot_get_time(); - for (Joint &joint : mJoints) - { + for (Joint &joint : mJoints) { if (joint.sensor) { const double position = wb_position_sensor_get_value(joint.sensor); const double velocity = std::isnan(joint.position) ? NAN : (position - joint.position) / deltaTime; @@ -188,17 +182,14 @@ namespace webots_ros2_control #if FOXY hardware_interface::return_type Ros2ControlSystem::write() #else // HUMBLE, ROLLING - hardware_interface::return_type Ros2ControlSystem::write(const rclcpp::Time &/*time*/, const rclcpp::Duration &/*period*/) + hardware_interface::return_type Ros2ControlSystem::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) #endif { - for (Joint &joint : mJoints) - { - if (joint.motor) - { + for (Joint &joint : mJoints) { + if (joint.motor) { if (joint.controlPosition && !std::isnan(joint.positionCommand)) wb_motor_set_position(joint.motor, joint.positionCommand); - if (joint.controlVelocity && !std::isnan(joint.velocityCommand)) - { + if (joint.controlVelocity && !std::isnan(joint.velocityCommand)) { // In the position control mode the velocity cannot be negative. const double velocityCommand = joint.controlPosition ? abs(joint.velocityCommand) : joint.velocityCommand; wb_motor_set_velocity(joint.motor, velocityCommand); @@ -209,6 +200,6 @@ namespace webots_ros2_control } return hardware_interface::return_type::OK; } -} +} // namespace webots_ros2_control PLUGINLIB_EXPORT_CLASS(webots_ros2_control::Ros2ControlSystem, webots_ros2_control::Ros2ControlSystemInterface) diff --git a/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp b/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp index e0a36124b..8aca6140b 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/PluginInterface.hpp @@ -15,29 +15,28 @@ #ifndef PLUGIN_INTERFACE #define PLUGIN_INTERFACE -#include #include +#include -namespace webots_ros2_driver -{ - class WebotsNode; +namespace webots_ros2_driver { + class WebotsNode; - class PluginInterface - { - public: - /// Prepare your plugin in this method. - /// Fired before the node is spinned. - /// Parameters are passed from the WebotsNode and/or from URDF. - /** - * \param[in] node WebotsNode inherited from `rclcpp::Node` with a few extra methods related - * \param[in] parameters Parameters (key-value pairs) located under a (dynamically loaded plugins) or (statically loaded plugins). - */ - virtual void init(WebotsNode *node, std::unordered_map ¶meters) = 0; + class PluginInterface { + public: + /// Prepare your plugin in this method. + /// Fired before the node is spinned. + /// Parameters are passed from the WebotsNode and/or from URDF. + /** + * \param[in] node WebotsNode inherited from `rclcpp::Node` with a few extra methods related + * \param[in] parameters Parameters (key-value pairs) located under a (dynamically loaded plugins) or + * (statically loaded plugins). + */ + virtual void init(WebotsNode *node, std::unordered_map ¶meters) = 0; - /// This method is called on each timestep. - /// You should not call `robot.step()` in this method as it is automatically called. - virtual void step() = 0; - }; -} + /// This method is called on each timestep. + /// You should not call `robot.step()` in this method as it is automatically called. + virtual void step() = 0; + }; +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp b/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp index 848914d54..b5f20a358 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/PythonPlugin.hpp @@ -15,29 +15,27 @@ #ifndef PYTHON_PLUGIN_HPP #define PYTHON_PLUGIN_HPP -#include #include #include +#include #include #include -namespace webots_ros2_driver -{ - class PythonPlugin : public PluginInterface - { - public: - void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; - void step() override; +namespace webots_ros2_driver { + class PythonPlugin : public PluginInterface { + public: + void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; + void step() override; - static std::shared_ptr createFromType(const std::string &type); + static std::shared_ptr createFromType(const std::string &type); - private: - PythonPlugin(PyObject *pyPlugin); + private: + PythonPlugin(PyObject *pyPlugin); - PyObject *mPyPlugin; - PyObject *getPyWebotsNodeInstance(); - }; -} + PyObject *mPyPlugin; + PyObject *getPyWebotsNodeInstance(); + }; +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp b/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp index 8d88b1a14..3e6fac187 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/WebotsNode.hpp @@ -15,27 +15,25 @@ #ifndef WEBOTS_NODE_HPP #define WEBOTS_NODE_HPP -#include #include +#include #include #include #include -#include +#include #include +#include #include -#include #include "webots_ros2_driver/PluginInterface.hpp" -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { class PluginInterface; - class WebotsNode : public rclcpp::Node - { + class WebotsNode : public rclcpp::Node { public: WebotsNode(std::string name); void init(); @@ -65,6 +63,6 @@ namespace webots_ros2_driver std::shared_ptr loadPlugin(const std::string &type); }; -} // end namespace webots_ros2_driver +} // end namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp index 1a17d269e..ba80b4fac 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/Ros2SensorPlugin.hpp @@ -15,26 +15,23 @@ #ifndef ROS2_SENSOR_PLUGIN_HPP #define ROS2_SENSOR_PLUGIN_HPP -#include #include +#include #include #include - -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { /// Utility class for common Webots sensors - class Ros2SensorPlugin : public PluginInterface - { + class Ros2SensorPlugin : public PluginInterface { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; protected: /// Checks if a sensor has data ready and updates the last update time /** - * \return Should publishing from the sensor be considered - */ + * \return Should publishing from the sensor be considered + */ bool preStep(); webots_ros2_driver::WebotsNode *mNode; @@ -47,6 +44,6 @@ namespace webots_ros2_driver double mLastUpdate; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp index f0135f4e3..2b0668946 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/dynamic/Ros2IMU.hpp @@ -17,20 +17,18 @@ #include -#include -#include #include +#include +#include #include -#include #include +#include -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { - class Ros2IMU : public Ros2SensorPlugin - { + class Ros2IMU : public Ros2SensorPlugin { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -50,6 +48,6 @@ namespace webots_ros2_driver bool mIsEnabled; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp index 478f7313f..9f7e7ace1 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Camera.hpp @@ -19,14 +19,14 @@ #include -#include #include +#include #include -#if defined (HUMBLE) || defined (ROLLING) +#if defined(HUMBLE) || defined(ROLLING) #include #else // Deprecated in Humble and Rolling -#include +#include #endif #include #include @@ -36,18 +36,15 @@ #include #include +#include +#include +#include #include #include -#include -#include -#include - -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { - class Ros2Camera : public Ros2SensorPlugin - { + class Ros2Camera : public Ros2SensorPlugin { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -73,6 +70,6 @@ namespace webots_ros2_driver bool mRecognitionIsEnabled; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp index 6e69af39e..ea64882f8 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2DistanceSensor.hpp @@ -15,18 +15,15 @@ #ifndef ROS2_DISTANCE_SENSOR_HPP #define ROS2_DISTANCE_SENSOR_HPP -#include -#include #include -#include +#include +#include #include +#include +namespace webots_ros2_driver { -namespace webots_ros2_driver -{ - - class Ros2DistanceSensor : public Ros2SensorPlugin - { + class Ros2DistanceSensor : public Ros2SensorPlugin { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -43,6 +40,6 @@ namespace webots_ros2_driver bool mIsEnabled; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp index 1c075d490..e43681f85 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2GPS.hpp @@ -21,13 +21,11 @@ #include #include #include -#include #include +#include -namespace webots_ros2_driver -{ - class Ros2GPS : public Ros2SensorPlugin - { +namespace webots_ros2_driver { + class Ros2GPS : public Ros2SensorPlugin { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -55,5 +53,5 @@ namespace webots_ros2_driver bool mIsEnabled; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp index b285def97..1eab45e29 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LED.hpp @@ -15,18 +15,16 @@ #ifndef ROS2_LED_HPP #define ROS2_LED_HPP -#include -#include #include -#include -#include +#include +#include #include +#include +#include -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { - class Ros2LED : public PluginInterface - { + class Ros2LED : public PluginInterface { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -39,6 +37,6 @@ namespace webots_ros2_driver rclcpp::Subscription::SharedPtr mSubscriber; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp index c941b3e01..09331776b 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2Lidar.hpp @@ -15,19 +15,16 @@ #ifndef ROS2_LIDAR_HPP #define ROS2_LIDAR_HPP -#include +#include #include #include #include -#include +#include #include -#include - +#include -namespace webots_ros2_driver -{ - class Ros2Lidar : public Ros2SensorPlugin - { +namespace webots_ros2_driver { + class Ros2Lidar : public Ros2SensorPlugin { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -49,6 +46,6 @@ namespace webots_ros2_driver bool mIsPointCloudEnabled; }; -} // end namespace webots_ros2_driver +} // end namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp index 94a531afb..4bc4cbbc0 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2LightSensor.hpp @@ -15,18 +15,15 @@ #ifndef ROS2_LIGHT_SENSOR_HPP #define ROS2_LIGHT_SENSOR_HPP -#include -#include #include -#include +#include +#include #include +#include +namespace webots_ros2_driver { -namespace webots_ros2_driver -{ - - class Ros2LightSensor : public Ros2SensorPlugin - { + class Ros2LightSensor : public Ros2SensorPlugin { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -44,6 +41,6 @@ namespace webots_ros2_driver bool mIsEnabled; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp index c99074d98..3f2821436 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/plugins/static/Ros2RangeFinder.hpp @@ -15,20 +15,17 @@ #ifndef ROS2_RANGE_FINDER_HPP #define ROS2_RANGE_FINDER_HPP -#include +#include #include #include #include -#include -#include +#include #include +#include +namespace webots_ros2_driver { -namespace webots_ros2_driver -{ - - class Ros2RangeFinder : public Ros2SensorPlugin - { + class Ros2RangeFinder : public Ros2SensorPlugin { public: void init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) override; void step() override; @@ -49,6 +46,6 @@ namespace webots_ros2_driver bool mIsEnabled; }; -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/utils/Math.hpp b/webots_ros2_driver/include/webots_ros2_driver/utils/Math.hpp index 205fa7291..33edcb4a3 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/utils/Math.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/utils/Math.hpp @@ -15,16 +15,15 @@ #ifndef MATH_HPP #define MATH_HPP +#include #include #include -#include -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { void matrixToQuaternion(const double *matrix, geometry_msgs::msg::Quaternion &q); void axisAngleToQuaternion(const double *axisAngle, geometry_msgs::msg::Quaternion &q); void quaternionToAxisAngle(const geometry_msgs::msg::Quaternion &q, double *axisAngle); double interpolateLookupTable(double value, std::vector &table); -} +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/include/webots_ros2_driver/utils/Utils.hpp b/webots_ros2_driver/include/webots_ros2_driver/utils/Utils.hpp index 06396778e..7479ac402 100644 --- a/webots_ros2_driver/include/webots_ros2_driver/utils/Utils.hpp +++ b/webots_ros2_driver/include/webots_ros2_driver/utils/Utils.hpp @@ -17,10 +17,9 @@ #include -namespace webots_ros2_driver -{ - int getDeviceTimestepMsFromPublishTimestep(double publishTimestep, int basicTimestepMs); - std::string getFixedNameString(const std::string &name); -} +namespace webots_ros2_driver { + int getDeviceTimestepMsFromPublishTimestep(double publishTimestep, int basicTimestepMs); + std::string getFixedNameString(const std::string &name); +} // namespace webots_ros2_driver #endif diff --git a/webots_ros2_driver/scripts/webots_tcp_client.py b/webots_ros2_driver/scripts/webots_tcp_client.py index bbe64c01e..411aa430a 100644 --- a/webots_ros2_driver/scripts/webots_tcp_client.py +++ b/webots_ros2_driver/scripts/webots_tcp_client.py @@ -55,7 +55,8 @@ def host_shared_folder(): while tcp_socket.connect_ex((HOST, PORT)) != 0: - print('WARNING: Unable to start Webots. Please start the local simulation server on your host machine. Next connection attempt in 1 second.', file=sys.stderr) + print('WARNING: Unable to start Webots. Please start the local simulation server on your host machine. Next connection ' + 'attempt in 1 second.', file=sys.stderr) time.sleep(1) command = 'webots ' + launch_arguments + ' ' + os.path.join(host_shared_folder(), world_name) tcp_socket.sendall(command.encode('utf-8')) diff --git a/webots_ros2_driver/src/Driver.cpp b/webots_ros2_driver/src/Driver.cpp index 65f367ed5..6f8d116ad 100644 --- a/webots_ros2_driver/src/Driver.cpp +++ b/webots_ros2_driver/src/Driver.cpp @@ -12,10 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include #include #include -#include -#include #include int main(int argc, char **argv) { @@ -40,8 +40,7 @@ int main(int argc, char **argv) { for (char notAllowedChar : " -.)(") std::replace(robotName.begin(), robotName.end(), notAllowedChar, '_'); - std::shared_ptr node = - std::make_shared(robotName); + std::shared_ptr node = std::make_shared(robotName); node->init(); while (true) { if (node->step() == -1) diff --git a/webots_ros2_driver/src/PythonPlugin.cpp b/webots_ros2_driver/src/PythonPlugin.cpp index af83742e4..39dbf3df4 100644 --- a/webots_ros2_driver/src/PythonPlugin.cpp +++ b/webots_ros2_driver/src/PythonPlugin.cpp @@ -2,39 +2,36 @@ static PyObject *gPyWebotsNode = NULL; -namespace webots_ros2_driver -{ - PythonPlugin::PythonPlugin(PyObject *pyPlugin) : mPyPlugin(pyPlugin){}; - - void PythonPlugin::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { - PyObject *pyParameters = PyDict_New(); - for (const std::pair ¶meter : parameters) - PyDict_SetItem(pyParameters, PyUnicode_FromString(parameter.first.c_str()), PyUnicode_FromString(parameter.second.c_str())); - - PyObject_CallMethod(mPyPlugin, "init", "OO", getPyWebotsNodeInstance(), pyParameters); - PyErr_Print(); - } - - void PythonPlugin::step() - { - PyObject_CallMethod(mPyPlugin, "step", ""); - PyErr_Print(); - } - - PyObject *PythonPlugin::getPyWebotsNodeInstance() - { - if (gPyWebotsNode) - return gPyWebotsNode; - - PyObject *pyWebotsExtraModuleSource = Py_CompileString( - R"EOT( +namespace webots_ros2_driver { + PythonPlugin::PythonPlugin(PyObject *pyPlugin) : mPyPlugin(pyPlugin){}; + + void PythonPlugin::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { + PyObject *pyParameters = PyDict_New(); + for (const std::pair ¶meter : parameters) + PyDict_SetItem(pyParameters, PyUnicode_FromString(parameter.first.c_str()), + PyUnicode_FromString(parameter.second.c_str())); + + PyObject_CallMethod(mPyPlugin, "init", "OO", getPyWebotsNodeInstance(), pyParameters); + PyErr_Print(); + } + + void PythonPlugin::step() { + PyObject_CallMethod(mPyPlugin, "step", ""); + PyErr_Print(); + } + + PyObject *PythonPlugin::getPyWebotsNodeInstance() { + if (gPyWebotsNode) + return gPyWebotsNode; + + PyObject *pyWebotsExtraModuleSource = Py_CompileString( + R"EOT( import os import sys # Set WEBOTS_HOME to the webots_ros2_driver installation folder # to load the correct libController libraries from the Python API -from ament_index_python.packages import get_package_prefix +from ament_index_python.packages import get_package_prefix os.environ['WEBOTS_HOME'] = get_package_prefix('webots_ros2_driver') import controller @@ -52,44 +49,43 @@ class WebotsNode: else: self.robot = Supervisor() )EOT", - "webots_extra", Py_file_input); + "webots_extra", Py_file_input); - if (!pyWebotsExtraModuleSource) - throw std::runtime_error("Error: The Python module with the WebotsNode class cannot be compiled."); + if (!pyWebotsExtraModuleSource) + throw std::runtime_error("Error: The Python module with the WebotsNode class cannot be compiled."); - PyObject *pyWebotsExtraModule = PyImport_ExecCodeModule("webots_extra", pyWebotsExtraModuleSource); + PyObject *pyWebotsExtraModule = PyImport_ExecCodeModule("webots_extra", pyWebotsExtraModuleSource); - if (!pyWebotsExtraModule) - throw std::runtime_error("Error: The Python module with the WebotsNode class cannot be executed."); + if (!pyWebotsExtraModule) + throw std::runtime_error("Error: The Python module with the WebotsNode class cannot be executed."); - PyObject *pyDict = PyModule_GetDict(pyWebotsExtraModule); - PyObject *pyClass = PyDict_GetItemString(pyDict, "WebotsNode"); - gPyWebotsNode = PyObject_CallObject(pyClass, nullptr); - return gPyWebotsNode; - } + PyObject *pyDict = PyModule_GetDict(pyWebotsExtraModule); + PyObject *pyClass = PyDict_GetItemString(pyDict, "WebotsNode"); + gPyWebotsNode = PyObject_CallObject(pyClass, nullptr); + return gPyWebotsNode; + } - std::shared_ptr PythonPlugin::createFromType(const std::string &type) - { - const std::string moduleName = type.substr(0, type.find_last_of(".")); - const std::string className = type.substr(type.find_last_of(".") + 1); + std::shared_ptr PythonPlugin::createFromType(const std::string &type) { + const std::string moduleName = type.substr(0, type.find_last_of(".")); + const std::string className = type.substr(type.find_last_of(".") + 1); - Py_Initialize(); + Py_Initialize(); - PyObject *pyName = PyUnicode_FromString(moduleName.c_str()); - PyObject *pyModule = PyImport_Import(pyName); - PyErr_Print(); + PyObject *pyName = PyUnicode_FromString(moduleName.c_str()); + PyObject *pyModule = PyImport_Import(pyName); + PyErr_Print(); - // If the module cannot be found the error should be handled in the upper level (e.g. try loading C++ plugin) - if (!pyModule) - return NULL; + // If the module cannot be found the error should be handled in the upper level (e.g. try loading C++ plugin) + if (!pyModule) + return NULL; - PyObject *pyDict = PyModule_GetDict(pyModule); - PyObject *pyClass = PyDict_GetItemString(pyDict, className.c_str()); - PyErr_Print(); - if (!pyClass) - throw std::runtime_error("Error in plugin " + type + ": The class " + className + " cannot be found."); + PyObject *pyDict = PyModule_GetDict(pyModule); + PyObject *pyClass = PyDict_GetItemString(pyDict, className.c_str()); + PyErr_Print(); + if (!pyClass) + throw std::runtime_error("Error in plugin " + type + ": The class " + className + " cannot be found."); - PyObject *pyPlugin = PyObject_CallObject(pyClass, nullptr); - return std::shared_ptr(new PythonPlugin(pyPlugin)); - } -} + PyObject *pyPlugin = PyObject_CallObject(pyClass, nullptr); + return std::shared_ptr(new PythonPlugin(pyPlugin)); + } +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/WebotsNode.cpp b/webots_ros2_driver/src/WebotsNode.cpp index de86aca53..db7854910 100644 --- a/webots_ros2_driver/src/WebotsNode.cpp +++ b/webots_ros2_driver/src/WebotsNode.cpp @@ -18,23 +18,22 @@ #include #include -#include #include +#include -#include "webots_ros2_driver/PluginInterface.hpp" -#include #include -#include -#include #include -#include +#include #include +#include +#include +#include +#include "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/PluginInterface.hpp" #include "webots_ros2_driver/PythonPlugin.hpp" -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { const char *gDeviceReferenceAttribute = "reference"; const char *gDeviceRosTag = "ros"; const char *gPluginInterface = "webots_ros2_driver::PluginInterface"; @@ -42,17 +41,12 @@ namespace webots_ros2_driver bool gShutdownSignalReceived = false; - void handleSigint(int sig) - { - gShutdownSignalReceived = true; - } + void handleSigint(int sig) { gShutdownSignalReceived = true; } - WebotsNode::WebotsNode(std::string name) : Node(name), mPluginLoader(gPluginInterfaceName, gPluginInterface) - { + WebotsNode::WebotsNode(std::string name) : Node(name), mPluginLoader(gPluginInterfaceName, gPluginInterface) { mRobotDescription = this->declare_parameter("robot_description", ""); mSetRobotStatePublisher = this->declare_parameter("set_robot_state_publisher", false); - if (mRobotDescription != "") - { + if (mRobotDescription != "") { mRobotDescriptionDocument = std::make_shared(); mRobotDescriptionDocument->Parse(mRobotDescription.c_str()); if (!mRobotDescriptionDocument) @@ -61,9 +55,7 @@ namespace webots_ros2_driver if (!robotXMLElement) throw std::runtime_error("Invalid URDF, it doesn't contain a tag"); mWebotsXMLElement = robotXMLElement->FirstChildElement("webots"); - } - else - { + } else { mWebotsXMLElement = NULL; RCLCPP_INFO(get_logger(), "Robot description is not passed, using default parameters."); } @@ -72,15 +64,12 @@ namespace webots_ros2_driver mRemoveUrdfRobotMessage.data = name; } - std::unordered_map WebotsNode::getPluginProperties(tinyxml2::XMLElement *pluginElement) const - { + std::unordered_map WebotsNode::getPluginProperties(tinyxml2::XMLElement *pluginElement) const { std::unordered_map properties; - if (pluginElement) - { + if (pluginElement) { tinyxml2::XMLElement *deviceChild = pluginElement->FirstChildElement(); - while (deviceChild) - { + while (deviceChild) { properties[deviceChild->Name()] = deviceChild->GetText(); deviceChild = deviceChild->NextSiblingElement(); } @@ -89,8 +78,7 @@ namespace webots_ros2_driver return properties; } - std::unordered_map WebotsNode::getDeviceRosProperties(const std::string &name) const - { + std::unordered_map WebotsNode::getDeviceRosProperties(const std::string &name) const { std::unordered_map properties({{"enabled", "true"}}); // No URDF file specified @@ -98,8 +86,7 @@ namespace webots_ros2_driver return properties; tinyxml2::XMLElement *deviceChild = mWebotsXMLElement->FirstChildElement(); - while (deviceChild) - { + while (deviceChild) { if (deviceChild->Attribute(gDeviceReferenceAttribute) && deviceChild->Attribute(gDeviceReferenceAttribute) == name) break; deviceChild = deviceChild->NextSiblingElement(); @@ -111,8 +98,7 @@ namespace webots_ros2_driver // Store ROS properties tinyxml2::XMLElement *propertyChild = deviceChild->FirstChildElement(gDeviceRosTag)->FirstChildElement(); - while (propertyChild) - { + while (propertyChild) { properties[propertyChild->Name()] = propertyChild->GetText(); propertyChild = propertyChild->NextSiblingElement(); } @@ -120,13 +106,12 @@ namespace webots_ros2_driver return properties; } - void WebotsNode::init() - { - if (mSetRobotStatePublisher){ + void WebotsNode::init() { + if (mSetRobotStatePublisher) { std::string prefix = ""; setAnotherNodeParameter("robot_state_publisher", "robot_description", wb_robot_get_urdf(prefix.c_str())); } - + mStep = wb_robot_get_basic_time_step(); // Load static plugins @@ -134,8 +119,7 @@ namespace webots_ros2_driver // The static plugins will try to guess parameter based on the robot model, // but one can overwrite the default behavior in the section. // Typical static plugins are ROS 2 interfaces for Webots devices. - for (int i = 0; i < wb_robot_get_number_of_devices(); i++) - { + for (int i = 0; i < wb_robot_get_number_of_devices(); i++) { WbDeviceTag device = wb_robot_get_device_by_index(i); // Prepare parameters @@ -145,32 +129,30 @@ namespace webots_ros2_driver parameters["name"] = wb_device_get_name(device); std::shared_ptr plugin = nullptr; - switch (wb_device_get_node_type(device)) - { - case WB_NODE_LIDAR: - plugin = std::make_shared(); - break; - case WB_NODE_CAMERA: - plugin = std::make_shared(); - break; - case WB_NODE_GPS: - plugin = std::make_shared(); - break; - case WB_NODE_RANGE_FINDER: - plugin = std::make_shared(); - break; - case WB_NODE_DISTANCE_SENSOR: - plugin = std::make_shared(); - break; - case WB_NODE_LIGHT_SENSOR: - plugin = std::make_shared(); - break; - case WB_NODE_LED: - plugin = std::make_shared(); - break; + switch (wb_device_get_node_type(device)) { + case WB_NODE_LIDAR: + plugin = std::make_shared(); + break; + case WB_NODE_CAMERA: + plugin = std::make_shared(); + break; + case WB_NODE_GPS: + plugin = std::make_shared(); + break; + case WB_NODE_RANGE_FINDER: + plugin = std::make_shared(); + break; + case WB_NODE_DISTANCE_SENSOR: + plugin = std::make_shared(); + break; + case WB_NODE_LIGHT_SENSOR: + plugin = std::make_shared(); + break; + case WB_NODE_LED: + plugin = std::make_shared(); + break; } - if (plugin) - { + if (plugin) { plugin->init(this, parameters); mPlugins.push_back(plugin); } @@ -184,10 +166,10 @@ namespace webots_ros2_driver return; tinyxml2::XMLElement *pluginElement = mWebotsXMLElement->FirstChildElement("plugin"); - while (pluginElement) - { + while (pluginElement) { if (!pluginElement->Attribute("type")) - throw std::runtime_error("Invalid URDF, a plugin is missing a `type` property at line " + std::to_string(pluginElement->GetLineNum())); + throw std::runtime_error("Invalid URDF, a plugin is missing a `type` property at line " + + std::to_string(pluginElement->GetLineNum())); const std::string type = pluginElement->Attribute("type"); @@ -200,20 +182,14 @@ namespace webots_ros2_driver } } - std::shared_ptr WebotsNode::loadPlugin(const std::string &type) - { + std::shared_ptr WebotsNode::loadPlugin(const std::string &type) { // First, we assume the plugin is C++ - try - { + try { std::shared_ptr plugin(mPluginLoader.createUnmanagedInstance(type)); return plugin; - } - catch (const pluginlib::LibraryLoadException &e) - { + } catch (const pluginlib::LibraryLoadException &e) { // It may be a Python plugin - } - catch (const pluginlib::CreateClassException &e) - { + } catch (const pluginlib::CreateClassException &e) { throw std::runtime_error("The " + type + " class cannot be initialized."); } @@ -224,10 +200,8 @@ namespace webots_ros2_driver return plugin; } - int WebotsNode::step() - { - if (gShutdownSignalReceived && !mWaitingForUrdfRobotToBeRemoved) - { + int WebotsNode::step() { + if (gShutdownSignalReceived && !mWaitingForUrdfRobotToBeRemoved) { mRemoveUrdfRobotPublisher->publish(mRemoveUrdfRobotMessage); mWaitingForUrdfRobotToBeRemoved = true; } @@ -241,11 +215,11 @@ namespace webots_ros2_driver return result; } - void WebotsNode::setAnotherNodeParameter(std::string anotherNodeName, std::string parameterName, std::string parameterValue) - { + void WebotsNode::setAnotherNodeParameter(std::string anotherNodeName, std::string parameterName, std::string parameterValue) { mClient = create_client(get_namespace() + anotherNodeName + "/set_parameters"); mClient->wait_for_service(std::chrono::seconds(1)); - rcl_interfaces::srv::SetParameters::Request::SharedPtr request = std::make_shared(); + rcl_interfaces::srv::SetParameters::Request::SharedPtr request = + std::make_shared(); rcl_interfaces::msg::Parameter parameter; parameter.name = parameterName; parameter.value.string_value = parameterValue; @@ -254,8 +228,5 @@ namespace webots_ros2_driver mClient->async_send_request(request); } - void WebotsNode::handleSignals() - { - signal(SIGINT, handleSigint); - } -} // end namespace webots_ros2_driver + void WebotsNode::handleSignals() { signal(SIGINT, handleSigint); } +} // end namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp b/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp index cf28147ce..3e0a412a8 100644 --- a/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp +++ b/webots_ros2_driver/src/plugins/Ros2SensorPlugin.cpp @@ -17,10 +17,8 @@ #include -namespace webots_ros2_driver -{ - void Ros2SensorPlugin::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { +namespace webots_ros2_driver { + void Ros2SensorPlugin::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { mNode = node; mLastUpdate = 0; @@ -33,12 +31,11 @@ namespace webots_ros2_driver mPublishTimestepSyncedMs = getDeviceTimestepMsFromPublishTimestep(mPublishTimestep, wb_robot_get_basic_time_step()); } - bool Ros2SensorPlugin::preStep() - { + bool Ros2SensorPlugin::preStep() { // Update only if needed if (wb_robot_get_time() - mLastUpdate < mPublishTimestep) return false; mLastUpdate = wb_robot_get_time(); return true; } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp b/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp index fa214300e..1e0ea352a 100644 --- a/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp +++ b/webots_ros2_driver/src/plugins/dynamic/Ros2IMU.cpp @@ -17,14 +17,11 @@ #include #include "pluginlib/class_list_macros.hpp" -#include #include +#include - -namespace webots_ros2_driver -{ - void Ros2IMU::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { +namespace webots_ros2_driver { + void Ros2IMU::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; mInertialUnit = 0; @@ -32,24 +29,22 @@ namespace webots_ros2_driver mAccelerometer = 0; if (!parameters.count("inertialUnitName") && !parameters.count("gyroName") && !parameters.count("accelerometerName")) - throw std::runtime_error("The IMU plugins has to contain at least of: , , or "); + throw std::runtime_error( + "The IMU plugins has to contain at least of: , , or "); - if (parameters.count("inertialUnitName")) - { + if (parameters.count("inertialUnitName")) { mInertialUnit = wb_robot_get_device(parameters["inertialUnitName"].c_str()); if (mInertialUnit == 0 || wb_device_get_node_type(mInertialUnit) != WB_NODE_INERTIAL_UNIT) throw std::runtime_error("Cannot find InertialUnit with name " + parameters["inertialUnitName"]); } - if (parameters.count("gyroName")) - { + if (parameters.count("gyroName")) { mGyro = wb_robot_get_device(parameters["gyroName"].c_str()); if (mGyro == 0 || wb_device_get_node_type(mGyro) != WB_NODE_GYRO) throw std::runtime_error("Cannot find Gyro with name " + parameters["gyroName"]); } - if (parameters.count("accelerometerName")) - { + if (parameters.count("accelerometerName")) { mAccelerometer = wb_robot_get_device(parameters["accelerometerName"].c_str()); if (mAccelerometer == 0 || wb_device_get_node_type(mAccelerometer) != WB_NODE_ACCELEROMETER) throw std::runtime_error("Cannot find Accelerometer with name " + parameters["accelerometerName"]); @@ -64,8 +59,7 @@ namespace webots_ros2_driver } } - void Ros2IMU::enable() - { + void Ros2IMU::enable() { if (mInertialUnit) wb_inertial_unit_enable(mInertialUnit, mPublishTimestepSyncedMs); if (mAccelerometer) @@ -74,8 +68,7 @@ namespace webots_ros2_driver wb_gyro_enable(mGyro, mPublishTimestepSyncedMs); } - void Ros2IMU::disable() - { + void Ros2IMU::disable() { if (mInertialUnit) wb_inertial_unit_disable(mInertialUnit); if (mAccelerometer) @@ -84,8 +77,7 @@ namespace webots_ros2_driver wb_gyro_disable(mGyro); } - void Ros2IMU::step() - { + void Ros2IMU::step() { if (!preStep()) return; @@ -97,8 +89,7 @@ namespace webots_ros2_driver // Enable/Disable sensor const bool shouldBeEnabled = mPublisher->get_subscription_count() > 0; - if (shouldBeEnabled != mIsEnabled) - { + if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) enable(); else @@ -107,25 +98,21 @@ namespace webots_ros2_driver } } - void Ros2IMU::publishData() - { + void Ros2IMU::publishData() { mMessage.header.stamp = mNode->get_clock()->now(); - if (mAccelerometer) - { + if (mAccelerometer) { const double *values = wb_accelerometer_get_values(mAccelerometer); mMessage.linear_acceleration.x = values[0]; mMessage.linear_acceleration.y = values[1]; mMessage.linear_acceleration.z = values[2]; } - if (mGyro) - { + if (mGyro) { const double *values = wb_gyro_get_values(mGyro); mMessage.angular_velocity.x = values[0]; mMessage.angular_velocity.y = values[1]; mMessage.angular_velocity.z = values[2]; } - if (mInertialUnit) - { + if (mInertialUnit) { const double *values = wb_inertial_unit_get_quaternion(mInertialUnit); mMessage.orientation.x = values[0]; mMessage.orientation.y = values[1]; @@ -134,6 +121,6 @@ namespace webots_ros2_driver } mPublisher->publish(mMessage); } -} +} // namespace webots_ros2_driver PLUGINLIB_EXPORT_CLASS(webots_ros2_driver::Ros2IMU, webots_ros2_driver::PluginInterface) diff --git a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp index b136899d0..9b131996e 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Camera.cpp @@ -16,10 +16,8 @@ #include -namespace webots_ros2_driver -{ - void Ros2Camera::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { +namespace webots_ros2_driver { + void Ros2Camera::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; mRecognitionIsEnabled = false; @@ -38,7 +36,8 @@ namespace webots_ros2_driver mImageMessage.encoding = sensor_msgs::image_encodings::BGRA8; // CameraInfo publisher - mCameraInfoPublisher = mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); + mCameraInfoPublisher = + mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); mCameraInfoMessage.header.stamp = mNode->get_clock()->now(); mCameraInfoMessage.header.frame_id = mFrameName; mCameraInfoMessage.height = wb_camera_get_height(mCamera); @@ -50,44 +49,45 @@ namespace webots_ros2_driver mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0}; mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; - mCameraInfoMessage.k = { - focalLength, 0.0, (double)wb_camera_get_width(mCamera) / 2, - 0.0, focalLength, (double)wb_camera_get_height(mCamera) / 2, - 0.0, 0.0, 1.0}; - mCameraInfoMessage.p = { - focalLength, 0.0, (double)wb_camera_get_width(mCamera) / 2, 0.0, - 0.0, focalLength, (double)wb_camera_get_height(mCamera) / 2, 0.0, - 0.0, 0.0, 1.0, 0.0}; + mCameraInfoMessage.k = {focalLength, 0.0, (double)wb_camera_get_width(mCamera) / 2, + 0.0, focalLength, (double)wb_camera_get_height(mCamera) / 2, + 0.0, 0.0, 1.0}; + mCameraInfoMessage.p = {focalLength, + 0.0, + (double)wb_camera_get_width(mCamera) / 2, + 0.0, + 0.0, + focalLength, + (double)wb_camera_get_height(mCamera) / 2, + 0.0, + 0.0, + 0.0, + 1.0, + 0.0}; // Recognition publisher - if (wb_camera_has_recognition(mCamera)) - { - mRecognitionPublisher = mNode->create_publisher( - mTopicName + "/recognitions", - rclcpp::SensorDataQoS().reliable()); - mWebotsRecognitionPublisher = mNode->create_publisher< - webots_ros2_msgs::msg::CameraRecognitionObjects>( - mTopicName + "/recognitions/webots", - rclcpp::SensorDataQoS().reliable()); + if (wb_camera_has_recognition(mCamera)) { + mRecognitionPublisher = mNode->create_publisher(mTopicName + "/recognitions", + rclcpp::SensorDataQoS().reliable()); + mWebotsRecognitionPublisher = mNode->create_publisher( + mTopicName + "/recognitions/webots", rclcpp::SensorDataQoS().reliable()); mRecognitionMessage.header.frame_id = mFrameName; mWebotsRecognitionMessage.header.frame_id = mFrameName; } } - void Ros2Camera::step() - { + void Ros2Camera::step() { if (!preStep()) return; // Enable/Disable sensor const bool imageSubscriptionsExist = mImagePublisher->get_subscription_count() > 0; const bool recognitionSubscriptionsExist = - (mRecognitionPublisher != nullptr && mRecognitionPublisher->get_subscription_count() > 0) || - (mWebotsRecognitionPublisher != nullptr && mWebotsRecognitionPublisher->get_subscription_count() > 0); + (mRecognitionPublisher != nullptr && mRecognitionPublisher->get_subscription_count() > 0) || + (mWebotsRecognitionPublisher != nullptr && mWebotsRecognitionPublisher->get_subscription_count() > 0); const bool shouldBeEnabled = mAlwaysOn || imageSubscriptionsExist || recognitionSubscriptionsExist; - if (shouldBeEnabled != mIsEnabled) - { + if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) wb_camera_enable(mCamera, mPublishTimestepSyncedMs); else @@ -95,8 +95,7 @@ namespace webots_ros2_driver mIsEnabled = shouldBeEnabled; } - if (recognitionSubscriptionsExist != mRecognitionIsEnabled) - { + if (recognitionSubscriptionsExist != mRecognitionIsEnabled) { if (recognitionSubscriptionsExist) wb_camera_recognition_enable(mCamera, mPublishTimestepSyncedMs); else @@ -113,19 +112,16 @@ namespace webots_ros2_driver mCameraInfoPublisher->publish(mCameraInfoMessage); } - void Ros2Camera::publishImage() - { + void Ros2Camera::publishImage() { auto image = wb_camera_get_image(mCamera); - if (image) - { + if (image) { mImageMessage.header.stamp = mNode->get_clock()->now(); memcpy(mImageMessage.data.data(), image, mImageMessage.data.size()); mImagePublisher->publish(mImageMessage); } } - void Ros2Camera::publishRecognition() - { + void Ros2Camera::publishRecognition() { if (wb_camera_recognition_get_number_of_objects(mCamera) == 0) return; @@ -135,8 +131,7 @@ namespace webots_ros2_driver mRecognitionMessage.detections.clear(); mWebotsRecognitionMessage.objects.clear(); - for (size_t i = 0; i < wb_camera_recognition_get_number_of_objects(mCamera); i++) - { + for (size_t i = 0; i < wb_camera_recognition_get_number_of_objects(mCamera); i++) { // Getting Object Info geometry_msgs::msg::PoseStamped pose; pose.pose.position.x = objects[i].position[0]; @@ -158,13 +153,13 @@ namespace webots_ros2_driver vision_msgs::msg::ObjectHypothesisWithPose hypothesis; hypothesis.pose.pose = pose.pose; detection.results.push_back(hypothesis); - #if FOXY +#if FOXY detection.bbox.center.x = objects[i].position_on_image[0]; detection.bbox.center.y = objects[i].position_on_image[1]; - #else +#else detection.bbox.center.position.x = objects[i].position_on_image[0]; detection.bbox.center.position.y = objects[i].position_on_image[1]; - #endif +#endif detection.bbox.size_x = objects[i].size_on_image[0]; detection.bbox.size_y = objects[i].size_on_image[1]; mRecognitionMessage.detections.push_back(detection); @@ -174,17 +169,16 @@ namespace webots_ros2_driver recognitionWebotsObject.id = objects[i].id; recognitionWebotsObject.model = std::string(objects[i].model); recognitionWebotsObject.pose = pose; - #if FOXY +#if FOXY recognitionWebotsObject.bbox.center.x = objects[i].position_on_image[0]; recognitionWebotsObject.bbox.center.y = objects[i].position_on_image[1]; - #else +#else recognitionWebotsObject.bbox.center.position.x = objects[i].position_on_image[0]; recognitionWebotsObject.bbox.center.position.y = objects[i].position_on_image[1]; - #endif +#endif recognitionWebotsObject.bbox.size_x = objects[i].size_on_image[0]; recognitionWebotsObject.bbox.size_y = objects[i].size_on_image[1]; - for (size_t j = 0; j < objects[i].number_of_colors; j++) - { + for (size_t j = 0; j < objects[i].number_of_colors; j++) { std_msgs::msg::ColorRGBA color; color.r = objects[i].colors[3 * j]; color.g = objects[i].colors[3 * j + 1]; @@ -196,4 +190,4 @@ namespace webots_ros2_driver mWebotsRecognitionPublisher->publish(mWebotsRecognitionMessage); mRecognitionPublisher->publish(mRecognitionMessage); } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp b/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp index a9466ae63..bb0e2aab0 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2DistanceSensor.cpp @@ -18,23 +18,26 @@ #include -namespace webots_ros2_driver -{ - void Ros2DistanceSensor::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { +namespace webots_ros2_driver { + void Ros2DistanceSensor::init(webots_ros2_driver::WebotsNode *node, + std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; mDistanceSensor = wb_robot_get_device(parameters["name"].c_str()); assert(mDistanceSensor != 0); - mLookupTable.assign(wb_distance_sensor_get_lookup_table(mDistanceSensor), wb_distance_sensor_get_lookup_table(mDistanceSensor) + wb_distance_sensor_get_lookup_table_size(mDistanceSensor) * 3); + mLookupTable.assign( + wb_distance_sensor_get_lookup_table(mDistanceSensor), + wb_distance_sensor_get_lookup_table(mDistanceSensor) + wb_distance_sensor_get_lookup_table_size(mDistanceSensor) * 3); const int size = mLookupTable.size(); const double maxValue = std::max(mLookupTable[0], mLookupTable[size - 3]); const double minValue = std::min(mLookupTable[0], mLookupTable[size - 3]); - const double lowerStd = (mLookupTable[0] < mLookupTable[size - 3]) ? mLookupTable[2] * mLookupTable[0] : mLookupTable[size - 1] * mLookupTable[size - 3]; - const double upperStd = (mLookupTable[0] > mLookupTable[size - 3]) ? mLookupTable[2] * mLookupTable[0] : mLookupTable[size - 1] * mLookupTable[size - 3]; + const double lowerStd = (mLookupTable[0] < mLookupTable[size - 3]) ? mLookupTable[2] * mLookupTable[0] : + mLookupTable[size - 1] * mLookupTable[size - 3]; + const double upperStd = (mLookupTable[0] > mLookupTable[size - 3]) ? mLookupTable[2] * mLookupTable[0] : + mLookupTable[size - 1] * mLookupTable[size - 3]; const double minRange = minValue + lowerStd; const double maxRange = maxValue - upperStd; @@ -51,8 +54,7 @@ namespace webots_ros2_driver } } - void Ros2DistanceSensor::step() - { + void Ros2DistanceSensor::step() { if (!preStep()) return; @@ -64,8 +66,7 @@ namespace webots_ros2_driver // Enable/Disable sensor const bool shouldBeEnabled = mPublisher->get_subscription_count() > 0; - if (shouldBeEnabled != mIsEnabled) - { + if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) wb_distance_sensor_enable(mDistanceSensor, mPublishTimestepSyncedMs); else @@ -74,8 +75,7 @@ namespace webots_ros2_driver } } - void Ros2DistanceSensor::publishRange() - { + void Ros2DistanceSensor::publishRange() { const double value = wb_distance_sensor_get_value(mDistanceSensor); if (std::isnan(value)) return; @@ -83,4 +83,4 @@ namespace webots_ros2_driver mMessage.range = interpolateLookupTable(value, mLookupTable); mPublisher->publish(mMessage); } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp b/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp index 9b0101541..d59ceeaf6 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2GPS.cpp @@ -16,31 +16,29 @@ #include -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { - void Ros2GPS::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { + void Ros2GPS::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; mGPS = wb_robot_get_device(parameters["name"].c_str()); assert(mGPS != 0); - if (wb_gps_get_coordinate_system(mGPS) == WB_GPS_WGS84_COORDINATE) - { + if (wb_gps_get_coordinate_system(mGPS) == WB_GPS_WGS84_COORDINATE) { mGPSPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); mGPSMessage.header.frame_id = mFrameName; mGPSMessage.position_covariance_type = sensor_msgs::msg::NavSatFix::COVARIANCE_TYPE_UNKNOWN; mGPSMessage.status.service = sensor_msgs::msg::NavSatStatus::SERVICE_GPS; - } - else - { - mPointPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); + } else { + mPointPublisher = + mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); mPointMessage.header.frame_id = mFrameName; } - mSpeedPublisher = mNode->create_publisher(mTopicName + "/speed", rclcpp::SensorDataQoS().reliable()); - mSpeedVectorPublisher = mNode->create_publisher(mTopicName + "/speed_vector", rclcpp::SensorDataQoS().reliable()); + mSpeedPublisher = + mNode->create_publisher(mTopicName + "/speed", rclcpp::SensorDataQoS().reliable()); + mSpeedVectorPublisher = + mNode->create_publisher(mTopicName + "/speed_vector", rclcpp::SensorDataQoS().reliable()); if (mAlwaysOn) { wb_gps_enable(mGPS, mPublishTimestepSyncedMs); @@ -48,8 +46,7 @@ namespace webots_ros2_driver } } - void Ros2GPS::step() - { + void Ros2GPS::step() { if (!preStep()) return; @@ -73,10 +70,9 @@ namespace webots_ros2_driver const bool gpsSubscriptionsExist = mGPSPublisher != nullptr && mGPSPublisher->get_subscription_count() > 0; const bool speedSubscriptionsExist = mSpeedPublisher->get_subscription_count() > 0; const bool speedVectorSubscriptionsExist = mSpeedVectorPublisher->get_subscription_count() > 0; - const bool shouldBeEnabled = pointSubscriptionsExist || gpsSubscriptionsExist || speedSubscriptionsExist - || speedVectorSubscriptionsExist; - if (shouldBeEnabled != mIsEnabled) - { + const bool shouldBeEnabled = + pointSubscriptionsExist || gpsSubscriptionsExist || speedSubscriptionsExist || speedVectorSubscriptionsExist; + if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) wb_gps_enable(mGPS, mPublishTimestepSyncedMs); else @@ -85,8 +81,7 @@ namespace webots_ros2_driver } } - void Ros2GPS::publishPoint() - { + void Ros2GPS::publishPoint() { mPointMessage.header.stamp = mNode->get_clock()->now(); const double *values = wb_gps_get_values(mGPS); mPointMessage.point.x = values[0]; @@ -95,8 +90,7 @@ namespace webots_ros2_driver mPointPublisher->publish(mPointMessage); } - void Ros2GPS::publishGPS() - { + void Ros2GPS::publishGPS() { mGPSMessage.header.stamp = mNode->get_clock()->now(); const double *values = wb_gps_get_values(mGPS); mGPSMessage.latitude = values[0]; @@ -105,18 +99,16 @@ namespace webots_ros2_driver mGPSPublisher->publish(mGPSMessage); } - void Ros2GPS::publishSpeed() - { + void Ros2GPS::publishSpeed() { mSpeedMessage.data = wb_gps_get_speed(mGPS); mSpeedPublisher->publish(mSpeedMessage); } - void Ros2GPS::publishSpeedVector() - { + void Ros2GPS::publishSpeedVector() { const double *values = wb_gps_get_speed_vector(mGPS); mSpeedVectorMessage.x = values[0]; mSpeedVectorMessage.y = values[1]; mSpeedVectorMessage.z = values[2]; mSpeedVectorPublisher->publish(mSpeedVectorMessage); } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/static/Ros2LED.cpp b/webots_ros2_driver/src/plugins/static/Ros2LED.cpp index 17450fac3..69bb9d8d8 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2LED.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2LED.cpp @@ -18,25 +18,20 @@ #include -namespace webots_ros2_driver -{ - void Ros2LED::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { +namespace webots_ros2_driver { + void Ros2LED::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { mNode = node; mLED = wb_robot_get_device(parameters["name"].c_str()); assert(mLED != 0); - const std::string topicName = parameters.count("topicName") ? parameters["topicName"] : "~/" + getFixedNameString(parameters["name"]); - mSubscriber = mNode->create_subscription(topicName, rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2LED::onMessageReceived, this, std::placeholders::_1)); + const std::string topicName = + parameters.count("topicName") ? parameters["topicName"] : "~/" + getFixedNameString(parameters["name"]); + mSubscriber = mNode->create_subscription( + topicName, rclcpp::SensorDataQoS().reliable(), std::bind(&Ros2LED::onMessageReceived, this, std::placeholders::_1)); } - void Ros2LED::step() - { - } + void Ros2LED::step() {} - void Ros2LED::onMessageReceived(const std_msgs::msg::Int32::SharedPtr message) - { - wb_led_set(mLED, message->data); - } -} + void Ros2LED::onMessageReceived(const std_msgs::msg::Int32::SharedPtr message) { wb_led_set(mLED, message->data); } +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp index abd152812..4a4313d54 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2Lidar.cpp @@ -14,15 +14,13 @@ #include -#include #include +#include #include -namespace webots_ros2_driver -{ - void Ros2Lidar::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { +namespace webots_ros2_driver { + void Ros2Lidar::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsSensorEnabled = false; mIsPointCloudEnabled = false; @@ -31,8 +29,7 @@ namespace webots_ros2_driver assert(mLidar != 0); // Laser publisher - if (wb_lidar_get_number_of_layers(mLidar) == 1) - { + if (wb_lidar_get_number_of_layers(mLidar) == 1) { mLaserPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); const int resolution = wb_lidar_get_horizontal_resolution(mLidar); mLaserMessage.header.frame_id = mFrameName; @@ -47,7 +44,8 @@ namespace webots_ros2_driver } // Point cloud publisher - mPointCloudPublisher = mNode->create_publisher(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable()); + mPointCloudPublisher = + mNode->create_publisher(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable()); mPointCloudMessage.header.frame_id = mFrameName; mPointCloudMessage.height = 1; mPointCloudMessage.point_step = 20; @@ -75,8 +73,7 @@ namespace webots_ros2_driver } } - void Ros2Lidar::step() - { + void Ros2Lidar::step() { if (!preStep()) return; @@ -90,12 +87,11 @@ namespace webots_ros2_driver return; const bool shouldPointCloudBeEnabled = mPointCloudPublisher->get_subscription_count() > 0; - const bool shouldSensorBeEnabled = shouldPointCloudBeEnabled || - (mLaserPublisher != nullptr && mLaserPublisher->get_subscription_count() > 0); + const bool shouldSensorBeEnabled = + shouldPointCloudBeEnabled || (mLaserPublisher != nullptr && mLaserPublisher->get_subscription_count() > 0); // Enable/Disable sensor - if (shouldSensorBeEnabled != mIsSensorEnabled) - { + if (shouldSensorBeEnabled != mIsSensorEnabled) { if (shouldSensorBeEnabled) wb_lidar_enable(mLidar, mPublishTimestepSyncedMs); else @@ -104,8 +100,7 @@ namespace webots_ros2_driver } // Enable/Disable point cloud - if (shouldPointCloudBeEnabled != mIsPointCloudEnabled) - { + if (shouldPointCloudBeEnabled != mIsPointCloudEnabled) { if (shouldPointCloudBeEnabled) wb_lidar_enable_point_cloud(mLidar); else @@ -114,11 +109,9 @@ namespace webots_ros2_driver } } - void Ros2Lidar::publishPointCloud() - { + void Ros2Lidar::publishPointCloud() { auto data = wb_lidar_get_point_cloud(mLidar); - if (data) - { + if (data) { mPointCloudMessage.header.stamp = mNode->get_clock()->now(); mPointCloudMessage.width = wb_lidar_get_number_of_points(mLidar); @@ -131,15 +124,13 @@ namespace webots_ros2_driver } } - void Ros2Lidar::publishLaserScan() - { + void Ros2Lidar::publishLaserScan() { auto rangeImage = wb_lidar_get_layer_range_image(mLidar, 0); - if (rangeImage) - { + if (rangeImage) { memcpy(mLaserMessage.ranges.data(), rangeImage, mLaserMessage.ranges.size() * sizeof(float)); mLaserMessage.header.stamp = mNode->get_clock()->now(); mLaserPublisher->publish(mLaserMessage); } } -} // end namespace webots_ros2_driver +} // end namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp b/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp index f44998acc..63a5e4bdc 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2LightSensor.cpp @@ -18,12 +18,10 @@ #include -namespace webots_ros2_driver -{ +namespace webots_ros2_driver { const double gIrradianceToIlluminance = 120.0; - void Ros2LightSensor::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { + void Ros2LightSensor::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; mLightSensor = wb_robot_get_device(parameters["name"].c_str()); @@ -33,7 +31,9 @@ namespace webots_ros2_driver mPublisher = mNode->create_publisher(mTopicName, rclcpp::SensorDataQoS().reliable()); mMessage.header.frame_id = mFrameName; - mLookupTable.assign(wb_light_sensor_get_lookup_table(mLightSensor), wb_light_sensor_get_lookup_table(mLightSensor) + wb_light_sensor_get_lookup_table_size(mLightSensor) * 3); + mLookupTable.assign( + wb_light_sensor_get_lookup_table(mLightSensor), + wb_light_sensor_get_lookup_table(mLightSensor) + wb_light_sensor_get_lookup_table_size(mLightSensor) * 3); if (mAlwaysOn) { wb_light_sensor_enable(mLightSensor, mPublishTimestepSyncedMs); @@ -41,8 +41,7 @@ namespace webots_ros2_driver } } - void Ros2LightSensor::step() - { + void Ros2LightSensor::step() { if (!preStep()) return; @@ -54,8 +53,7 @@ namespace webots_ros2_driver // Enable/Disable sensor const bool shouldBeEnabled = mPublisher->get_subscription_count() > 0; - if (shouldBeEnabled != mIsEnabled) - { + if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) wb_light_sensor_enable(mLightSensor, mPublishTimestepSyncedMs); else @@ -64,8 +62,7 @@ namespace webots_ros2_driver } } - void Ros2LightSensor::publishValue() - { + void Ros2LightSensor::publishValue() { const double value = wb_light_sensor_get_value(mLightSensor); mMessage.header.stamp = mNode->get_clock()->now(); mMessage.illuminance = interpolateLookupTable(value, mLookupTable); @@ -73,13 +70,12 @@ namespace webots_ros2_driver mPublisher->publish(mMessage); } - double Ros2LightSensor::findVariance(double rawValue) - { + double Ros2LightSensor::findVariance(double rawValue) { // Find relative standard deviation in lookup table double relativeStd = NAN; for (int i = 0; i < mLookupTable.size() - 3; i += 3) - if ((mLookupTable[i + 1] < rawValue < mLookupTable[i + 3 + 1]) || (mLookupTable[i + 1] > rawValue > mLookupTable[i + 3 + 1])) - { + if ((mLookupTable[i + 1] < rawValue < mLookupTable[i + 3 + 1]) || + (mLookupTable[i + 1] > rawValue > mLookupTable[i + 3 + 1])) { relativeStd = mLookupTable[i + 2]; break; } @@ -93,4 +89,4 @@ namespace webots_ros2_driver double std = interpolateLookupTable(rawValue, mLookupTable) * gIrradianceToIlluminance * relativeStd; return std * std; } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp index 2011abc2d..ca2d564b8 100644 --- a/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp +++ b/webots_ros2_driver/src/plugins/static/Ros2RangeFinder.cpp @@ -19,10 +19,8 @@ #include -namespace webots_ros2_driver -{ - void Ros2RangeFinder::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) - { +namespace webots_ros2_driver { + void Ros2RangeFinder::init(webots_ros2_driver::WebotsNode *node, std::unordered_map ¶meters) { Ros2SensorPlugin::init(node, parameters); mIsEnabled = false; mRangeFinder = wb_robot_get_device(parameters["name"].c_str()); @@ -43,7 +41,8 @@ namespace webots_ros2_driver mImageMessage.encoding = sensor_msgs::image_encodings::TYPE_32FC1; // CameraInfo publisher - mCameraInfoPublisher = mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); + mCameraInfoPublisher = + mNode->create_publisher(mTopicName + "/camera_info", rclcpp::SensorDataQoS().reliable()); mCameraInfoMessage.header.stamp = mNode->get_clock()->now(); mCameraInfoMessage.header.frame_id = mFrameName; mCameraInfoMessage.height = height; @@ -53,17 +52,13 @@ namespace webots_ros2_driver const double focalLengthY = 0.5 * height * (1 / tan(0.5 * wb_range_finder_get_fov(mRangeFinder))); mCameraInfoMessage.d = {0.0, 0.0, 0.0, 0.0, 0.0}; mCameraInfoMessage.r = {1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0}; - mCameraInfoMessage.k = { - focalLengthX, 0.0, (double)width / 2, - 0.0, focalLengthY, (double)height / 2, - 0.0, 0.0, 1.0}; - mCameraInfoMessage.p = { - focalLengthX, 0.0, (double)width / 2, 0.0, - 0.0, focalLengthY, (double)height / 2, 0.0, - 0.0, 0.0, 1.0, 0.0}; + mCameraInfoMessage.k = {focalLengthX, 0.0, (double)width / 2, 0.0, focalLengthY, (double)height / 2, 0.0, 0.0, 1.0}; + mCameraInfoMessage.p = {focalLengthX, 0.0, (double)width / 2, 0.0, 0.0, focalLengthY, (double)height / 2, 0.0, 0.0, 0.0, + 1.0, 0.0}; // Point cloud publisher - mPointCloudPublisher = mNode->create_publisher(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable()); + mPointCloudPublisher = + mNode->create_publisher(mTopicName + "/point_cloud", rclcpp::SensorDataQoS().reliable()); mPointCloudMessage.header.frame_id = mFrameName; mPointCloudMessage.fields.resize(3); mPointCloudMessage.fields[0].name = "x"; @@ -91,8 +86,7 @@ namespace webots_ros2_driver } } - void Ros2RangeFinder::step() - { + void Ros2RangeFinder::step() { if (!preStep()) return; @@ -109,8 +103,7 @@ namespace webots_ros2_driver // Enable/Disable sensor const bool shouldBeEnabled = mImagePublisher->get_subscription_count() > 0; - if (shouldBeEnabled != mIsEnabled) - { + if (shouldBeEnabled != mIsEnabled) { if (shouldBeEnabled) wb_range_finder_enable(mRangeFinder, mPublishTimestepSyncedMs); else @@ -119,11 +112,9 @@ namespace webots_ros2_driver } } - void Ros2RangeFinder::publishImage() - { + void Ros2RangeFinder::publishImage() { auto image = wb_range_finder_get_range_image(mRangeFinder); - if (image) - { + if (image) { mImageMessage.header.stamp = mNode->get_clock()->now(); memcpy(mImageMessage.data.data(), image, mImageMessage.data.size()); mImagePublisher->publish(mImageMessage); @@ -131,11 +122,9 @@ namespace webots_ros2_driver } // To be redesigned when mRangeFinder->getPointCloud() will be implemented on Webots side. - void Ros2RangeFinder::publishPointCloud() - { + void Ros2RangeFinder::publishPointCloud() { auto image = wb_range_finder_get_range_image(mRangeFinder); - if (image) - { + if (image) { mPointCloudMessage.header.stamp = mNode->get_clock()->now(); const int width = mCameraInfoMessage.width; @@ -148,16 +137,14 @@ namespace webots_ros2_driver int idx; float x, y, z; - float* data = (float*)mPointCloudMessage.data.data(); - for (int j = 0; j < height; j++) - { - for (int i = 0; i < width; i++) - { + float *data = (float *)mPointCloudMessage.data.data(); + for (int j = 0; j < height; j++) { + for (int i = 0; i < width; i++) { idx = i + j * width; x = image[idx]; y = -(i - cx) * x / fx; z = -(j - cy) * x / fy; - memcpy(data + idx * 3 , &x, sizeof(float)); + memcpy(data + idx * 3, &x, sizeof(float)); memcpy(data + idx * 3 + 1, &y, sizeof(float)); memcpy(data + idx * 3 + 2, &z, sizeof(float)); } @@ -165,4 +152,4 @@ namespace webots_ros2_driver mPointCloudPublisher->publish(mPointCloudMessage); } } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/utils/Math.cpp b/webots_ros2_driver/src/utils/Math.cpp index 1ded99bd0..72b612234 100644 --- a/webots_ros2_driver/src/utils/Math.cpp +++ b/webots_ros2_driver/src/utils/Math.cpp @@ -19,14 +19,12 @@ #include -namespace webots_ros2_driver -{ - static double interpolateFunction(double value, double startX, double startY, double endX, double endY, bool ascending = false); +namespace webots_ros2_driver { + static double interpolateFunction(double value, double startX, double startY, double endX, double endY, + bool ascending = false); - void matrixToQuaternion(const double *matrix, geometry_msgs::msg::Quaternion &q) - { - if (matrix[0] == matrix[4] && matrix[0] == matrix[8] && matrix[0] == 1.0) - { + void matrixToQuaternion(const double *matrix, geometry_msgs::msg::Quaternion &q) { + if (matrix[0] == matrix[4] && matrix[0] == matrix[8] && matrix[0] == 1.0) { // exception q.w = 1.0; q.x = 0.0; @@ -38,8 +36,7 @@ namespace webots_ros2_driver double s = 2.0; double invS = 1.0; const double trace = matrix[0] + matrix[4] + matrix[8]; - if (trace >= 0.0) - { + if (trace >= 0.0) { s *= sqrt(trace + 1); invS = 1.0 / s; q.w = 0.25 * s; @@ -49,8 +46,7 @@ namespace webots_ros2_driver return; } - if (matrix[0] > matrix[4] && matrix[0] > matrix[8]) - { + if (matrix[0] > matrix[4] && matrix[0] > matrix[8]) { // matrix[0] is larger than max(M(4), M(8)) s *= sqrt(1.0 + matrix[0] - matrix[4] - matrix[8]); invS = 1.0 / s; @@ -61,10 +57,9 @@ namespace webots_ros2_driver return; } - if (matrix[4] > matrix[8]) - { + if (matrix[4] > matrix[8]) { // matrix[4] is the largest - s *= sqrt(1.0 + matrix[4] - matrix[8] - matrix[0]); // s = 4y + s *= sqrt(1.0 + matrix[4] - matrix[8] - matrix[0]); // s = 4y invS = 1.0 / s; q.w = (matrix[2] - matrix[6]) * invS; q.x = (matrix[1] + matrix[3]) * invS; @@ -74,7 +69,7 @@ namespace webots_ros2_driver } // else matrix[8] is the largest - s *= sqrt(1.0 + matrix[8] - matrix[0] - matrix[4]); // s = 4z + s *= sqrt(1.0 + matrix[8] - matrix[0] - matrix[4]); // s = 4z invS = 1.0 / s; q.w = (matrix[3] - matrix[1]) * invS; q.x = (matrix[6] + matrix[2]) * invS; @@ -82,8 +77,7 @@ namespace webots_ros2_driver q.z = 0.25 * s; } - void axisAngleToQuaternion(const double *axisAngle, geometry_msgs::msg::Quaternion &q) - { + void axisAngleToQuaternion(const double *axisAngle, geometry_msgs::msg::Quaternion &q) { const double halfAngle = 0.5 * axisAngle[3]; const double sinusHalfAngle = sin(halfAngle); q.w = cos(halfAngle); @@ -92,13 +86,11 @@ namespace webots_ros2_driver q.z = axisAngle[2] * sinusHalfAngle; } - void quaternionToAxisAngle(const geometry_msgs::msg::Quaternion &q, double *axisAngle) - { + void quaternionToAxisAngle(const geometry_msgs::msg::Quaternion &q, double *axisAngle) { // if q.w > 1, acos will return nan // if this actually happens we should normalize the quaternion here axisAngle[3] = 2.0 * acos(q.w); - if (axisAngle[3] < 0.0001) - { + if (axisAngle[3] < 0.0001) { axisAngle[0] = 0.0; axisAngle[1] = 1.0; axisAngle[2] = 0.0; @@ -111,8 +103,7 @@ namespace webots_ros2_driver axisAngle[2] = q.z * inv; } - double interpolateFunction(double value, double startX, double startY, double endX, double endY, bool ascending) - { + double interpolateFunction(double value, double startX, double startY, double endX, double endY, bool ascending) { if (endX - startX == 0) if ((ascending && value < startX) || (!ascending && value > startX)) return startY; @@ -124,42 +115,24 @@ namespace webots_ros2_driver return slope * (value - startX) + startY; } - double interpolateLookupTable(double value, std::vector &table) - { + double interpolateLookupTable(double value, std::vector &table) { if (!table.size()) return value; const int size = table.size(); // Interpolate - for (int i = 0; i < size / 3 - 1; i++) - { + for (int i = 0; i < size / 3 - 1; i++) { if ((value < table[i * 3 + 1] && value >= table[(i + 1) * 3 + 1]) || (value >= table[i * 3 + 1] && value < table[(i + 1) * 3 + 1])) - return interpolateFunction( - value, - table[i * 3 + 1], - table[i * 3], - table[(i + 1) * 3 + 1], - table[(i + 1) * 3]); + return interpolateFunction(value, table[i * 3 + 1], table[i * 3], table[(i + 1) * 3 + 1], table[(i + 1) * 3]); } // Extrapolate (we assume that the table is sorted, order is irrelevant) const bool ascending = (table[1] < table[size - 1 * 3 + 1]); if ((ascending && value >= table[1]) || (!ascending && value < table[1])) - return interpolateFunction( - value, - table[size - 2 * 3 + 1], - table[size - 2 * 3 + 0], - table[size - 1 * 3 + 1], - table[size - 1 * 3 + 0], - ascending); + return interpolateFunction(value, table[size - 2 * 3 + 1], table[size - 2 * 3 + 0], table[size - 1 * 3 + 1], + table[size - 1 * 3 + 0], ascending); else - return interpolateFunction( - value, - table[1], - table[0], - table[3 + 1], - table[3], - ascending); + return interpolateFunction(value, table[1], table[0], table[3 + 1], table[3], ascending); } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/src/utils/Utils.cpp b/webots_ros2_driver/src/utils/Utils.cpp index c8ad12926..3e4569f81 100644 --- a/webots_ros2_driver/src/utils/Utils.cpp +++ b/webots_ros2_driver/src/utils/Utils.cpp @@ -16,25 +16,22 @@ #include -namespace webots_ros2_driver -{ - int getDeviceTimestepMsFromPublishTimestep(double publishTimestep, int basicTimestepMs) - { - int result = basicTimestepMs; - while (result / 1000.0 < publishTimestep - basicTimestepMs / 2000.0) - result += basicTimestepMs; - return result; - } +namespace webots_ros2_driver { + int getDeviceTimestepMsFromPublishTimestep(double publishTimestep, int basicTimestepMs) { + int result = basicTimestepMs; + while (result / 1000.0 < publishTimestep - basicTimestepMs / 2000.0) + result += basicTimestepMs; + return result; + } - std::string getFixedNameString(const std::string &name) - { - std::string fixedName = name; - std::replace(fixedName.begin(), fixedName.end(), '-', '_'); - std::replace(fixedName.begin(), fixedName.end(), '.', '_'); - std::replace(fixedName.begin(), fixedName.end(), ' ', '_'); - std::replace(fixedName.begin(), fixedName.end(), ')', '_'); - std::replace(fixedName.begin(), fixedName.end(), '(', '_'); - return fixedName; - } + std::string getFixedNameString(const std::string &name) { + std::string fixedName = name; + std::replace(fixedName.begin(), fixedName.end(), '-', '_'); + std::replace(fixedName.begin(), fixedName.end(), '.', '_'); + std::replace(fixedName.begin(), fixedName.end(), ' ', '_'); + std::replace(fixedName.begin(), fixedName.end(), ')', '_'); + std::replace(fixedName.begin(), fixedName.end(), '(', '_'); + return fixedName; + } -} +} // namespace webots_ros2_driver diff --git a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py index 2cb140105..94be9397c 100755 --- a/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py +++ b/webots_ros2_driver/webots_ros2_driver/ros2_supervisor.py @@ -20,26 +20,27 @@ import os import sys +import re import rclpy import vehicle import controller import webots_ros2_importer + from rclpy.time import Time from rclpy.node import Node from rclpy.qos import qos_profile_services_default from rosgraph_msgs.msg import Clock from std_msgs.msg import String sys.path.insert(1, os.path.join(os.path.dirname(webots_ros2_importer.__file__), 'urdf2webots')) -from urdf2webots.importer import convertUrdfFile, convertUrdfContent -from webots_ros2_msgs.srv import SpawnUrdfRobot, SpawnNodeFromString -import re +from urdf2webots.importer import convertUrdfFile, convertUrdfContent # noqa +from webots_ros2_msgs.srv import SpawnUrdfRobot, SpawnNodeFromString # noqa # As Ros2Supervisor needs the controller library, we extend the path here # to avoid to load another library named "controller" or "vehicle". sys.path.insert(1, os.path.dirname(vehicle.__file__)) sys.path.insert(1, os.path.dirname(controller.__file__)) -from controller import Supervisor +from controller import Supervisor # noqa class Ros2Supervisor(Node): @@ -56,16 +57,13 @@ def __init__(self): # Spawn Nodes (URDF robots or Webots objects) root_node = self.__robot.getRoot() self.__insertion_node_place = root_node.getField('children') - self.__node_list=[] - - + self.__node_list = [] + # Services self.create_service(SpawnUrdfRobot, 'spawn_urdf_robot', self.__spawn_urdf_robot_callback) - self.create_service(SpawnNodeFromString, 'spawn_node_from_string', self.__spawn_node_from_string_callback) - # Subscriptions + self.create_service(SpawnNodeFromString, 'spawn_node_from_string', self.__spawn_node_from_string_callback) + # Subscriptions self.create_subscription(String, 'remove_node', self.__remove_imported_node_callback, qos_profile_services_default) - - def __spawn_urdf_robot_callback(self, request, response): robot = request.robot @@ -73,11 +71,13 @@ def __spawn_urdf_robot_callback(self, request, response): robot_name = robot.name if robot.name else '' if robot_name == '': - self.get_logger().info('Ros2Supervisor cannot import an unnamed URDF robot. Please specifiy it with name="" in the URDFSpawner object.') + self.get_logger().info('Ros2Supervisor cannot import an unnamed URDF robot. Please specifiy it with name="" in the ' + 'URDFSpawner object.') response.success = False return response if robot_name in self.__node_list: - self.get_logger().info('The URDF robot name "' + str(robot_name) + '" is already used by another robot! Please specifiy a unique name.') + self.get_logger().info('The URDF robot name "' + str(robot_name) + '" is already used by another robot! Please ' + 'specifiy a unique name.') response.success = False return response @@ -90,15 +90,17 @@ def __spawn_urdf_robot_callback(self, request, response): # Choose the conversion according to the input if robot.urdf_path: robot_string = convertUrdfFile(input=robot.urdf_path, robotName=robot_name, normal=normal, - boxCollision=box_collision, initTranslation=robot_translation, initRotation=robot_rotation, - initPos=init_pos) + boxCollision=box_collision, initTranslation=robot_translation, + initRotation=robot_rotation, initPos=init_pos) elif robot.robot_description: relative_path_prefix = robot.relative_path_prefix if robot.relative_path_prefix else None robot_string = convertUrdfContent(input=robot.robot_description, robotName=robot_name, normal=normal, - boxCollision=box_collision, initTranslation=robot_translation, initRotation=robot_rotation, - initPos=init_pos, relativePathPrefix=relative_path_prefix) + boxCollision=box_collision, initTranslation=robot_translation, + initRotation=robot_rotation, initPos=init_pos, + relativePathPrefix=relative_path_prefix) else: - self.get_logger().info('Ros2Supervisor can not import a URDF file without a specified "urdf_path" or "robot_description" in the URDFSpawner object.') + self.get_logger().info('Ros2Supervisor can not import a URDF file without a specified "urdf_path" or ' + '"robot_description" in the URDFSpawner object.') response.success = False return response self.__insertion_node_place.importMFNodeFromString(-1, robot_string) @@ -106,11 +108,10 @@ def __spawn_urdf_robot_callback(self, request, response): self.__node_list.append(robot_name) response.success = True return response - - - def __spawn_node_from_string_callback(self, request, response): + + def __spawn_node_from_string_callback(self, request, response): object_string = request.data - if(object_string == ''): + if object_string == '': self.get_logger().info('Ros2Supervisor cannot import an empty string.') response.success = False return response @@ -120,18 +121,19 @@ def __spawn_node_from_string_callback(self, request, response): object_name = object_name.replace('"', '') # Check that the name is not an empty string. if object_name == '': - self.get_logger().info('Ros2Supervisor cannot import an unnamed node.') + self.get_logger().info('Ros2Supervisor cannot import an unnamed node.') response.success = False return response # Check that the name is unique. if object_name in self.__node_list: - self.get_logger().info('Ros2Supervisor has found a duplicate node in the world named "' + str(object_name) + '". Please specifiy a unique name.') + self.get_logger().info('Ros2Supervisor has found a duplicate node in the world named "' + str(object_name) + '". ' + 'Please specifiy a unique name.') response.success = False return response # Insert the object. self.__node_list.append(object_name) self.__insertion_node_place.importMFNodeFromString(-1, object_string) - + # Check if the object has been imported into the world node = None node_imported_successfully = False @@ -171,7 +173,7 @@ def __remove_imported_node_callback(self, message): self.get_logger().info('Ros2Supervisor has removed the node named "' + str(name) + '".') else: self.get_logger().info('Ros2Supervisor wanted to remove the node named "' + str(name) + - '" but this node has not been found in the simulation world.') + '" but this node has not been found in the simulation world.') def __supervisor_step_callback(self): if self.__robot.step(self.__timestep) < 0: diff --git a/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py b/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py index e7ad477c9..7e42f0dcc 100644 --- a/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py +++ b/webots_ros2_driver/webots_ros2_driver/urdf_spawner.py @@ -23,6 +23,7 @@ from launch.actions import ExecuteProcess from webots_ros2_driver.utils import is_wsl, has_shared_folder, container_shared_folder, host_shared_folder + def get_webots_driver_node(event, driver_node): """Return the driver node in case the service response is successful.""" if 'success=True' in event.text.decode().strip(): @@ -30,12 +31,17 @@ def get_webots_driver_node(event, driver_node): print('WARNING: the Ros2Supervisor was not able to spawn this URDF robot.') return + class URDFSpawner(ExecuteProcess): - def __init__(self, output='log', name=None, urdf_path=None, robot_description=None, relative_path_prefix=None, translation='0 0 0', rotation='0 0 1 0', normal=False, box_collision=False, init_pos=None, **kwargs): + def __init__(self, output='log', name=None, urdf_path=None, robot_description=None, relative_path_prefix=None, + translation='0 0 0', rotation='0 0 1 0', normal=False, box_collision=False, init_pos=None, **kwargs): if is_wsl() and relative_path_prefix: - relative_path_prefix = subprocess.check_output(['wslpath', '-w', relative_path_prefix]).strip().decode('utf-8').replace('\\', '/') - if has_shared_folder() and relative_path_prefix and not os.path.isdir(os.path.join(container_shared_folder(), os.path.basename(relative_path_prefix))): - shutil.copytree(relative_path_prefix, os.path.join(container_shared_folder(), os.path.basename(relative_path_prefix))) + command = ['wslpath', '-w', relative_path_prefix] + relative_path_prefix = subprocess.check_output(command).strip().decode('utf-8').replace('\\', '/') + if has_shared_folder() and relative_path_prefix and not os.path.isdir( + os.path.join(container_shared_folder(), os.path.basename(relative_path_prefix))): + shutil.copytree(relative_path_prefix, os.path.join(container_shared_folder(), + os.path.basename(relative_path_prefix))) relative_path_prefix = os.path.join(host_shared_folder(), os.path.basename(relative_path_prefix)) message = '{robot: {' diff --git a/webots_ros2_driver/webots_ros2_driver/utils.py b/webots_ros2_driver/webots_ros2_driver/utils.py index ebcbf02da..94844db7b 100644 --- a/webots_ros2_driver/webots_ros2_driver/utils.py +++ b/webots_ros2_driver/webots_ros2_driver/utils.py @@ -20,11 +20,9 @@ import re import sys import shutil -import socket import tarfile import functools import subprocess -import time import urllib.request from pathlib import Path from platform import uname @@ -158,8 +156,10 @@ def version_min(found, minimum): if variable in os.environ and os.path.isdir(os.environ[variable]) and WebotsVersion.from_path(os.environ[variable]): os.environ['WEBOTS_HOME'] = os.environ[variable] return os.environ[variable] - elif variable in os.environ and (not os.path.isdir(os.environ[variable]) or WebotsVersion.from_path(os.environ[variable]) is None): - print(f'WARNING: Webots directory `{os.environ[variable]}` specified in `{variable}` is not a valid Webots directory or is not found.') + elif variable in os.environ and \ + (not os.path.isdir(os.environ[variable]) or WebotsVersion.from_path(os.environ[variable]) is None): + print(f'WARNING: Webots directory `{os.environ[variable]}` specified in `{variable}` is not a valid Webots ' + 'directory or is not found.') # Normalize Webots version minimum_version = WebotsVersion.minimum() @@ -190,7 +190,8 @@ def version_min(found, minimum): if os.path.isdir(path) and version_min(WebotsVersion.from_path(path), minimum_version): os.environ['WEBOTS_HOME'] = path if show_warning: - print(f'WARNING: No valid Webots directory specified in `ROS2_WEBOTS_HOME` and `WEBOTS_HOME`, fallback to default installation folder {path}.') + print('WARNING: No valid Webots directory specified in `ROS2_WEBOTS_HOME` and `WEBOTS_HOME`, fallback to ' + f'default installation folder {path}.') return path return None @@ -245,7 +246,10 @@ def on_download_progress_changed(count, block_size, total_size): def handle_webots_installation(): minimum_version = WebotsVersion.minimum() - installation_directory = f'C:\\Program Files\\Webots' if (is_wsl() or sys.platform == 'win32') else os.path.join(str(Path.home()), '.ros') + if is_wsl() or sys.platform == 'win32': + installation_directory = 'C:\\Program Files\\Webots' + else: + installation_directory = os.path.join(str(Path.home()), '.ros') webots_release_url = f'https://github.com/cyberbotics/webots/releases/tag/{minimum_version.short()}' print( diff --git a/webots_ros2_driver/webots_ros2_driver/webots_launcher.py b/webots_ros2_driver/webots_ros2_driver/webots_launcher.py index 3e7b1accc..e77a72a9c 100644 --- a/webots_ros2_driver/webots_ros2_driver/webots_launcher.py +++ b/webots_ros2_driver/webots_ros2_driver/webots_launcher.py @@ -54,8 +54,10 @@ def perform(self, context): class WebotsLauncher(ExecuteProcess): def __init__(self, output='screen', world=None, gui=True, mode='realtime', stream=False, **kwargs): if sys.platform == 'win32': - print('WARNING: Native webots_ros2 compatibility with Windows is deprecated and will be removed soon. Please use a WSL (Windows Subsystem for Linux) environment instead.', file=sys.stderr) - print('WARNING: Check https://github.com/cyberbotics/webots_ros2/wiki/Complete-Installation-Guide for more information.', file=sys.stderr) + print('WARNING: Native webots_ros2 compatibility with Windows is deprecated and will be removed soon. Please use a ' + 'WSL (Windows Subsystem for Linux) environment instead.', file=sys.stderr) + print('WARNING: Check https://github.com/cyberbotics/webots_ros2/wiki/Complete-Installation-Guide for more ' + 'information.', file=sys.stderr) self.__is_wsl = is_wsl() self.__has_shared_folder = has_shared_folder() @@ -97,7 +99,8 @@ def __init__(self, output='screen', world=None, gui=True, mode='realtime', strea # Initialize command to start Webots remotely through TCP if self.__has_shared_folder: - webots_tcp_client = (os.path.join(get_package_share_directory('webots_ros2_driver'), 'scripts', 'webots_tcp_client.py')) + webots_tcp_client = (os.path.join(get_package_share_directory('webots_ros2_driver'), 'scripts', + 'webots_tcp_client.py')) super().__init__( output=output, cmd=[ @@ -153,12 +156,14 @@ def execute(self, context: LaunchContext): url_path = match.group(1) # Absolute path or Webots relative path or Web paths - if os.path.isabs(url_path) or url_path.startswith('webots://') or url_path.startswith('http://') or url_path.startswith('https://'): + if os.path.isabs(url_path) or url_path.startswith('webots://') or url_path.startswith('http://') \ + or url_path.startswith('https://'): continue new_url_path = os.path.split(world_path)[0] + '/' + url_path if self.__is_wsl: - new_url_path = subprocess.check_output(['wslpath', '-w', new_url_path]).strip().decode('utf-8').replace('\\', '/') + command = ['wslpath', '-w', new_url_path] + new_url_path = subprocess.check_output(command).strip().decode('utf-8').replace('\\', '/') new_url_path = '"' + new_url_path + '"' url_path = '"' + url_path + '"' content = content.replace(url_path, new_url_path) @@ -178,7 +183,8 @@ def execute(self, context: LaunchContext): # Copy world file to shared folder if self.__has_shared_folder: - shutil.copy(self.__world_copy.name, os.path.join(container_shared_folder(), os.path.basename(self.__world_copy.name))) + shutil.copy(self.__world_copy.name, os.path.join(container_shared_folder(), + os.path.basename(self.__world_copy.name))) # Execute process return super().execute(context) diff --git a/webots_ros2_epuck/package.xml b/webots_ros2_epuck/package.xml index e4ee096e1..22373c73c 100644 --- a/webots_ros2_epuck/package.xml +++ b/webots_ros2_epuck/package.xml @@ -28,8 +28,6 @@ webots_ros2_control ament_copyright - ament_flake8 - ament_pep257 python3-pytest diff --git a/webots_ros2_epuck/test/test_flake8.py b/webots_ros2_epuck/test/test_flake8.py deleted file mode 100644 index ffa6a773c..000000000 --- a/webots_ros2_epuck/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_epuck/test/test_pep257.py b/webots_ros2_epuck/test/test_pep257.py deleted file mode 100644 index fac857efd..000000000 --- a/webots_ros2_epuck/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_importer/package.xml b/webots_ros2_importer/package.xml index f22b80d56..fa97b7117 100644 --- a/webots_ros2_importer/package.xml +++ b/webots_ros2_importer/package.xml @@ -19,8 +19,6 @@ ament_copyright - ament_flake8 - ament_pep257 python3-pycodestyle python3-pil python3-numpy diff --git a/webots_ros2_importer/pytest.ini b/webots_ros2_importer/pytest.ini index 467f0d75c..c4eb0fd7c 100644 --- a/webots_ros2_importer/pytest.ini +++ b/webots_ros2_importer/pytest.ini @@ -1,3 +1,3 @@ [pytest] -# Set testpaths, otherwise pytest finds 'tests' in the urdf2webots directory -testpaths = test \ No newline at end of file +# Ignore tests inside urdf2webots folder +norecursedirs = webots_ros2_importer/urdf2webots/* diff --git a/webots_ros2_importer/test/test_flake8.py b/webots_ros2_importer/test/test_flake8.py deleted file mode 100644 index a78207364..000000000 --- a/webots_ros2_importer/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128', '--exclude', 'webots_ros2_importer/urdf2webots/']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_importer/webots_ros2_importer/urdf2webots b/webots_ros2_importer/webots_ros2_importer/urdf2webots index 4dc4613a2..58f6dc30c 160000 --- a/webots_ros2_importer/webots_ros2_importer/urdf2webots +++ b/webots_ros2_importer/webots_ros2_importer/urdf2webots @@ -1 +1 @@ -Subproject commit 4dc4613a276995d211b0377a52c4a23068f3a890 +Subproject commit 58f6dc30c6b4d25f6e2a36f1689a3de0ccf23fd4 diff --git a/webots_ros2_mavic/package.xml b/webots_ros2_mavic/package.xml index 4e579af52..2a3840306 100644 --- a/webots_ros2_mavic/package.xml +++ b/webots_ros2_mavic/package.xml @@ -18,8 +18,6 @@ ament_copyright - ament_flake8 - ament_pep257 python3-pytest diff --git a/webots_ros2_mavic/test/test_flake8.py b/webots_ros2_mavic/test/test_flake8.py deleted file mode 100644 index ffa6a773c..000000000 --- a/webots_ros2_mavic/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_mavic/test/test_pep257.py b/webots_ros2_mavic/test/test_pep257.py deleted file mode 100644 index fac857efd..000000000 --- a/webots_ros2_mavic/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_msgs/msg/CameraRecognitionObject.msg b/webots_ros2_msgs/msg/CameraRecognitionObject.msg index dd684485e..c995aea73 100644 --- a/webots_ros2_msgs/msg/CameraRecognitionObject.msg +++ b/webots_ros2_msgs/msg/CameraRecognitionObject.msg @@ -1,5 +1,5 @@ int32 id geometry_msgs/PoseStamped pose -vision_msgs/BoundingBox2D bbox +vision_msgs/BoundingBox2D bbox std_msgs/ColorRGBA[] colors string model diff --git a/webots_ros2_tesla/package.xml b/webots_ros2_tesla/package.xml index d9b5ac1e6..3026fc88a 100644 --- a/webots_ros2_tesla/package.xml +++ b/webots_ros2_tesla/package.xml @@ -21,8 +21,6 @@ ament_copyright - ament_flake8 - ament_pep257 python3-pytest diff --git a/webots_ros2_tesla/test/test_flake8.py b/webots_ros2_tesla/test/test_flake8.py deleted file mode 100644 index ffa6a773c..000000000 --- a/webots_ros2_tesla/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_tesla/test/test_pep257.py b/webots_ros2_tesla/test/test_pep257.py deleted file mode 100644 index fac857efd..000000000 --- a/webots_ros2_tesla/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_tests/package.xml b/webots_ros2_tests/package.xml index 9ef5d3e85..87eae083c 100644 --- a/webots_ros2_tests/package.xml +++ b/webots_ros2_tests/package.xml @@ -28,8 +28,6 @@ std_srvs geometry_msgs ament_copyright - ament_flake8 - ament_pep257 python3-pytest launch launch_testing diff --git a/webots_ros2_tests/test/test_flake8.py b/webots_ros2_tests/test/test_flake8.py deleted file mode 100644 index 86fc569c6..000000000 --- a/webots_ros2_tests/test/test_flake8.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_tests/test/test_pep257.py b/webots_ros2_tests/test/test_pep257.py deleted file mode 100644 index 0bd725218..000000000 --- a/webots_ros2_tests/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_tiago/package.xml b/webots_ros2_tiago/package.xml index b09f1f284..a30a8eead 100644 --- a/webots_ros2_tiago/package.xml +++ b/webots_ros2_tiago/package.xml @@ -28,8 +28,6 @@ ament_copyright - ament_flake8 - ament_pep257 python3-pytest diff --git a/webots_ros2_tiago/test/test_flake8.py b/webots_ros2_tiago/test/test_flake8.py deleted file mode 100644 index ffa6a773c..000000000 --- a/webots_ros2_tiago/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_tiago/test/test_pep257.py b/webots_ros2_tiago/test/test_pep257.py deleted file mode 100644 index fac857efd..000000000 --- a/webots_ros2_tiago/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_turtlebot/package.xml b/webots_ros2_turtlebot/package.xml index 221f88458..28a7c6cc0 100644 --- a/webots_ros2_turtlebot/package.xml +++ b/webots_ros2_turtlebot/package.xml @@ -23,8 +23,6 @@ rviz2 ament_copyright - ament_flake8 - ament_pep257 python3-pytest diff --git a/webots_ros2_turtlebot/test/test_flake8.py b/webots_ros2_turtlebot/test/test_flake8.py deleted file mode 100644 index e59de015c..000000000 --- a/webots_ros2_turtlebot/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_turtlebot/test/test_pep257.py b/webots_ros2_turtlebot/test/test_pep257.py deleted file mode 100644 index fac857efd..000000000 --- a/webots_ros2_turtlebot/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/webots_ros2_universal_robot/package.xml b/webots_ros2_universal_robot/package.xml index 8ecbe3cbe..c64a866a7 100644 --- a/webots_ros2_universal_robot/package.xml +++ b/webots_ros2_universal_robot/package.xml @@ -27,8 +27,6 @@ ament_copyright - ament_flake8 - ament_pep257 python3-pytest diff --git a/webots_ros2_universal_robot/resource/moveit_movegroup.yaml b/webots_ros2_universal_robot/resource/moveit_movegroup.yaml index 9ab970686..3b98e36ac 100644 --- a/webots_ros2_universal_robot/resource/moveit_movegroup.yaml +++ b/webots_ros2_universal_robot/resource/moveit_movegroup.yaml @@ -1,5 +1,5 @@ planning_plugin: ompl_interface/OMPLPlanner -request_adapters: +request_adapters: default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds diff --git a/webots_ros2_universal_robot/test/test_flake8.py b/webots_ros2_universal_robot/test/test_flake8.py deleted file mode 100644 index ffa6a773c..000000000 --- a/webots_ros2_universal_robot/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, Inc. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect flake8.""" - -from ament_flake8.main import main -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc = main(argv=['--linelength', '128']) - assert rc == 0, 'Found errors' diff --git a/webots_ros2_universal_robot/test/test_pep257.py b/webots_ros2_universal_robot/test/test_pep257.py deleted file mode 100644 index fac857efd..000000000 --- a/webots_ros2_universal_robot/test/test_pep257.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 1996-2023 Cyberbotics Ltd. -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -"""Test that python files respect pep257.""" - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.']) - assert rc == 0, 'Found code style errors / warnings'