diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..e76a1de --- /dev/null +++ b/.clang-format @@ -0,0 +1,21 @@ +Language: Cpp +BasedOnStyle: Google + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +BraceWrapping: + AfterClass: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true +BreakBeforeBraces: Custom +ColumnLimit: 100 +ConstructorInitializerIndentWidth: 0 +ContinuationIndentWidth: 2 +DerivePointerAlignment: false +PointerAlignment: Middle +ReflowComments: true +IncludeBlocks: Preserve +AlignOperands: true +PenaltyBreakAssignment: 21 +PenaltyBreakBeforeFirstCallParameter: 1 diff --git a/.github/pull_request_template.md b/.github/pull_request_template.md new file mode 100644 index 0000000..0da4113 --- /dev/null +++ b/.github/pull_request_template.md @@ -0,0 +1,14 @@ +### Description + +- + +### Requirements + +- [ ] Code style guidelines followed +- [ ] Documentation updated + +### Tests 🧪 + +- [ ] Robot +- [ ] Container +- [ ] Simulation diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 0000000..08f7311 --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,11 @@ +--- +name: Pre-Commit + +on: + push: + +jobs: + pre-commit: + uses: ros-controls/ros2_control_ci/.github/workflows/reusable-pre-commit.yml@master + with: + ros_distro: humble diff --git a/.github/workflows/protect-default-branch.yaml b/.github/workflows/protect-default-branch.yaml new file mode 100644 index 0000000..678daeb --- /dev/null +++ b/.github/workflows/protect-default-branch.yaml @@ -0,0 +1,24 @@ +--- +name: Validate PR head branch +on: + pull_request: + branches: + - ros2 + +jobs: + check-head-branch: + runs-on: ubuntu-latest + steps: + - name: Check allowed branches + run: | + pattern="^[0-9]+\.[0-9]+\.[0-9]+-[0-9]{8}$" # This regex matches the X.X.X-YYYYMMDD pattern + if [[ "${{ github.head_ref }}" == *"hotfix"* ]]; then + echo "PR from a branch containing 'hotfix' is allowed." + exit 0 + elif [[ "${{ github.head_ref }}" =~ $pattern ]]; then + echo "PR from a branch matching X.X.X-YYYYMMDD pattern is allowed." + exit 0 + else + echo "PRs must come from branches containing 'hotfix' phrase or matching X.X.X-YYYYMMDD pattern." + exit 1 + fi diff --git a/.github/workflows/run-unit-tests.yaml b/.github/workflows/run-unit-tests.yaml new file mode 100644 index 0000000..7f23a7f --- /dev/null +++ b/.github/workflows/run-unit-tests.yaml @@ -0,0 +1,79 @@ +--- +name: Run unit tests + +on: + workflow_dispatch: + # TODO: ENABLE WHEN READY + # pull_request: + # branches: + # - ros2-devel + +jobs: + test: + name: Run unit tests + runs-on: self-hosted + env: + HUSARION_ROS_BUILD_TYPE: hardware + ROS_DISTRO: humble + TEST_RESULT_FILENAME: last_run_results.txt + COVERAGE_RESULT_FILENAME: coverage_results.log + steps: + - name: Prepare filesystem + working-directory: ${{ runner.temp }} + run: | + touch ${{ env.TEST_RESULT_FILENAME }} + touch ${{ env.COVERAGE_RESULT_FILENAME }} + + - name: Checkout repository + uses: actions/checkout@v3 + with: + ref: ${{ github.ref }} + path: ros2_ws/src/wibotic_ros + + - name: Resolve dependencies + working-directory: ros2_ws + run: | + sudo apt update + rosdep update --rosdistro $ROS_DISTRO + rosdep install -i --from-path src --rosdistro $ROS_DISTRO -y + + - name: Build + working-directory: ros2_ws + run: | + source /opt/ros/$ROS_DISTRO/setup.bash + if [ -f install/setup.bash ]; then source install/setup.bash; fi + colcon build --symlink-install --parallel-workers $(nproc) --packages-up-to panther --cmake-args -DCMAKE_CXX_FLAGS='-fprofile-arcs -ftest-coverage' + + - name: Test + working-directory: ros2_ws + run: | + source install/setup.bash + colcon test --packages-up-to panther --retest-until-pass 10 --event-handlers console_cohesion+ --return-code-on-test-failure + echo "result=$?" >> ${{ runner.temp }}/${{ env.TEST_RESULT_FILENAME }} + colcon lcov-result --packages-up-to panther --verbose >> ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} + lines_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} | grep -E 'lines' | head -1) + functions_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} | grep -E 'functions' | head -1) + branches_cov=$(cat ${{ runner.temp }}/${{ env.COVERAGE_RESULT_FILENAME }} | grep -E 'branches' | head -1) + echo "lines_cov=$lines_cov">> ${{ runner.temp }}/${{ env.TEST_RESULT_FILENAME }} + echo "functions_cov=$functions_cov" >> ${{ runner.temp }}/${{ env.TEST_RESULT_FILENAME }} + echo "branches_cov=$branches_cov" >> ${{ runner.temp }}/${{ env.TEST_RESULT_FILENAME }} + + - name: Collect unit tests output + working-directory: ${{ runner.temp }} + id: unit-tests-output + run: cat ${{ env.TEST_RESULT_FILENAME }} >> "$GITHUB_OUTPUT" + + - name: Validate tests result + uses: nick-fields/assert-action@v1 + with: + expected: 0 + actual: ${{ steps.unit-tests-output.outputs.result }} + + - name: Comment PR + uses: thollander/actions-comment-pull-request@v2 + with: + message: | + **Test coverage of modified packages:** + ${{ steps.unit-tests-output.outputs.lines_cov }} + ${{ steps.unit-tests-output.outputs.functions_cov }} + ${{ steps.unit-tests-output.outputs.branches_cov }} diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..bee8a64 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +__pycache__ diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..8f8266e --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,99 @@ +repos: + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v4.6.0 + hooks: + - id: check-added-large-files + # mesh files has to be taken into account + args: ["--maxkb=3000"] + - id: check-ast + - id: check-json + # vscode .json files do not follow the standard JSON format + exclude: ^.vscode/ + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: destroyed-symlinks + - id: detect-private-key + - id: end-of-file-fixer + - id: fix-byte-order-marker + - id: name-tests-test + - id: mixed-line-ending + - id: trailing-whitespace + + - repo: https://github.com/PyCQA/isort + rev: 5.13.2 + hooks: + - id: isort + args: ["--profile", "black"] + + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.13 + hooks: + - id: cmake-format + + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v18.1.8 + hooks: + - id: clang-format + + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + name: codespell + description: Checks for common misspellings in text files. + entry: codespell + args: + [ + "--ignore-words-list", + "ned" # north, east, down (NED) + ] + exclude_types: [rst, svg] + language: python + types: [text] + + - repo: https://github.com/jumanjihouse/pre-commit-hook-yamlfmt + rev: 0.2.3 + hooks: + - id: yamlfmt + files: ^.github|./\.yaml + args: [--mapping, '2', --sequence, '4', --offset, '2', --width, '100'] + + - repo: https://github.com/psf/black + rev: 24.8.0 + hooks: + - id: black + args: ["--line-length=99"] + + - repo: https://github.com/PyCQA/flake8 + rev: 7.1.1 + hooks: + - id: flake8 + args: + ["--ignore=E501,W503"] # ignore too long line and line break before binary operator, + # black checks it + + - repo: local + hooks: + - id: ament_copyright + name: ament_copyright + description: Check if copyright notice is available in all files. + stages: [commit] + entry: ament_copyright + language: system + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.1 + hooks: + - id: doc8 + args: ["--max-line-length=100", "--ignore=D001"] + exclude: ^.*\/CHANGELOG\.rst/.*$ + + - repo: https://github.com/tier4/pre-commit-hooks-ros + rev: v0.10.0 + hooks: + - id: prettier-package-xml + - id: sort-package-xml diff --git a/LICENSE b/LICENSE index 3f4d3f2..261eeb9 100644 --- a/LICENSE +++ b/LICENSE @@ -1,29 +1,201 @@ -BSD 3-Clause License - -Copyright © 2019, WiBotic Inc. -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -1. Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -2. Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -3. Neither the name of the copyright holder nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/README.md b/README.md index 8c4c607..5501ed8 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,33 @@ -# Overview +# wibotic_ros -WiBotic ROS CAN Connector is a set of ROS packages that supports interacting with a WiBotic Onboard Charger over a CAN bus. +The repository contains `wibotic_connector_can` and `wibotic_msgs` packages. It reads a CAN Bus thanks to the uavcan library and sends the measurements from [Wibotic Wireless Charger](https://husarion.com/manuals/panther/panther-wch/) to ROS 2. -The master branch of this repository remains synchronized with the latest WiBotic firmware release. Running firmware that matches with the equivalent WiBotic ROS CAN Connector version is strongly recommended. See the firmware update page on the WiBotic system GUI for more information about updating firmware. +## Quick start + +### Add CAN interface + +```bash +sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyACM0 +sudo ip link set up can0 type can bitrate 500000 +``` + +### Create workspace + +```bash +mkdir ~/husarion_ws +cd ~/husarion_ws +git clone -b ros2 https://github.com/husarion/wibotic_ros.git src/wibotic_ros +``` + +### Build + +```bash +sudo rosdep init +rosdep update --rosdistro $ROS_DISTRO +rosdep install --from-paths src -y -i + +source /opt/ros/$ROS_DISTRO/setup.bash +colcon build --symlink-install --packages-up-to wibotic_connector_can --cmake-args -DCMAKE_BUILD_TYPE=Release + +source install/setup.bash +``` diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index 7a6eaae..24fbd27 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -1,50 +1,83 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.8) project(wibotic_connector_can) +# Handle superbuild first +option(USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON) -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs - message_generation -) +if(USE_SUPERBUILD) + project(SUPERBUILD NONE) + include(cmake/SuperBuild.cmake) + return() +else() + project(wibotic_connector_can) +endif() -catkin_python_setup() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -################################### -## catkin specific configuration ## -################################### +set(PACKAGE_DEPENDENCIES ament_cmake rclcpp wibotic_msgs) -catkin_package( - LIBRARIES wibotic_connector_can - CATKIN_DEPENDS rospy std_msgs message_runtime wibotic_msg -) +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() -########### -## Build ## -########### +include_directories(${CMAKE_INSTALL_PREFIX}/include) -include_directories( - ${catkin_INCLUDE_DIRS} +add_library(wibotic_can_driver src/wibotic_can_driver.cpp) +target_compile_options(wibotic_can_driver PRIVATE -std=c++11) +target_include_directories( + wibotic_can_driver BEFORE + PUBLIC + ${CMAKE_CURRENT_BINARY_DIR}/wibotic_connector_can/ep_platform_specific_components/upstream/linux/libuavcan/uavcan_linux ) +target_include_directories( + wibotic_can_driver + PUBLIC $ + $) -############# -## Install ## -############# +ament_target_dependencies(wibotic_can_driver ${PACKAGE_DEPENDENCIES}) -catkin_install_python( - PROGRAMS src/wibotic_connector_can/wiboticros.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +set(UAVCAN_LIB ${CMAKE_INSTALL_PREFIX}/lib/libuavcan.a) -install( - DIRECTORY - launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch -) +target_link_libraries(wibotic_can_driver ${UAVCAN_LIB} rt) + +add_executable(wibotic_connector_can src/wibotic_can_driver_node.cpp + src/main.cpp) +target_compile_options(wibotic_connector_can PRIVATE -std=c++17) +target_include_directories( + wibotic_connector_can + PUBLIC $ + $) + +ament_target_dependencies(wibotic_connector_can ${PACKAGE_DEPENDENCIES}) + +target_link_libraries(wibotic_connector_can wibotic_can_driver) + +install(TARGETS wibotic_connector_can wibotic_can_driver + DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) + + # Unit tests + ament_add_gmock( + ${PROJECT_NAME}_test_wibotic_can_driver_node + test/unit/test_wibotic_can_driver_node.cpp src/wibotic_can_driver_node.cpp) + target_include_directories( + ${PROJECT_NAME}_test_wibotic_can_driver_node + PUBLIC $ + $) + target_link_libraries(${PROJECT_NAME}_test_wibotic_can_driver_node + wibotic_can_driver) + + # Integration tests + option(TEST_INTEGRATION "Run integration tests" ON) + if(TEST_INTEGRATION) + add_ros_test(test/integration/wibotic_connector_can.test.py) + endif() + +endif() -install( - DIRECTORY - src/wibotic_connector_can/uavcan_v0 - src/wibotic_connector_can/uavcan_v1 - src/wibotic_connector_can/uavcan_vendor_specific_types - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) \ No newline at end of file +ament_package() diff --git a/wibotic_connector_can/README.md b/wibotic_connector_can/README.md new file mode 100644 index 0000000..f78c181 --- /dev/null +++ b/wibotic_connector_can/README.md @@ -0,0 +1,20 @@ +# wibotic_connector_can + +It reads a CAN Bus thanks to the uavcan library and sends the measurements to ROS 2. + +## ROS Nodes + +### wibotic_connector_can + +It reads a CAN Bus thanks to the uavcan library and sends the measurements to ROS 2. + +#### Publishes + +- `wibotic_info` [*wibotic_msgs/WiboticInfo*]: Wibotic charger measurements. + +#### Parameters + +- `~can_iface_name` [*string*, default: **can0**]: CAN BUS interface used for Wibotic receiver. +- `~uavcan_node_id_` [*int*, default: **20**]: Uavcan node ID. +- `~uavcan_node_name_` [*string*, default: **can0**]: Uavcan node name. +- `~update_time_s_` [*string*, default: **can0**]: The period of reading WiboticInfo on a CAN BUS. diff --git a/wibotic_connector_can/cmake/SuperBuild.cmake b/wibotic_connector_can/cmake/SuperBuild.cmake new file mode 100644 index 0000000..3676904 --- /dev/null +++ b/wibotic_connector_can/cmake/SuperBuild.cmake @@ -0,0 +1,53 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# 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. + +include(ExternalProject) + +set(DEPENDENCIES ep_libuavcan ep_platform_specific_components) + +ExternalProject_Add( + ep_libuavcan + SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/ep_libuavcan/upstream + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} + GIT_REPOSITORY https://github.com/OpenCyphal-Garage/libcyphal/ + GIT_TAG dcc3a4de237b7482e04543d2393c3a9385685312 + PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_libuavcan + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + INSTALL_COMMAND make install INSTALL_PREFIX= + UPDATE_DISCONNECTED 1 + BUILD_IN_SOURCE 1 + STEP_TARGETS build) + +ExternalProject_Add( + ep_platform_specific_components + GIT_REPOSITORY + https://github.com/OpenCyphal-Garage/platform_specific_components/ + GIT_TAG 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 + PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components + CONFIGURE_COMMAND "" + BUILD_COMMAND "" + INSTALL_COMMAND + ${CMAKE_COMMAND} -E copy_directory + ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components/src/ep_platform_specific_components/linux/libuavcan/include + ${CMAKE_INSTALL_PREFIX}/include ${INSTALL_DIR}) + +install(DIRECTORY ${INSTALL_DIR} DESTINATION ${CMAKE_INSTALL_PREFIX}) + +ExternalProject_Add( + ep_wibotic_connector_can + DEPENDS ${DEPENDENCIES} + SOURCE_DIR ${PROJECT_SOURCE_DIR} + CMAKE_ARGS -DUSE_SUPERBUILD=OFF + INSTALL_COMMAND "" + BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}) diff --git a/wibotic_connector_can/include/wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp b/wibotic_connector_can/include/wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp new file mode 100644 index 0000000..cab9c2e --- /dev/null +++ b/wibotic_connector_can/include/wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp @@ -0,0 +1,437 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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. + +/* + * UAVCAN data structure definition for libuavcan. + * + * Autogenerated, do not edit. + * + * Source file: + * https://github.com/husarion/wibotic_ros/tree/ros2/wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan + */ + +#ifndef WIBOTIC_WIBOTICINFO_HPP_INCLUDED +#define WIBOTIC_WIBOTICINFO_HPP_INCLUDED + +#include +#include +#include + +/******************************* Source text ********************************** +# +# WiBotic periodic information. +# + +# +# Primary parameters. +# Some fields can be set to NAN if their values are unknown. +# +float16 VMonBatt +float16 IBattery +float16 VRect +float16 VMonCharger +float16 TBoard +float16 TargetIBatt +float16 ICharger +float16 ISingleCharger2 +float16 ISingleCharger3 +******************************************************************************/ + +/********************* DSDL signature source definition *********************** +wibotic.WiBoticInfo +saturated float16 VMonBatt +saturated float16 IBattery +saturated float16 VRect +saturated float16 VMonCharger +saturated float16 TBoard +saturated float16 TargetIBatt +saturated float16 ICharger +saturated float16 ISingleCharger2 +saturated float16 ISingleCharger3 +******************************************************************************/ + +#undef VMonBatt +#undef IBattery +#undef VRect +#undef VMonCharger +#undef TBoard +#undef TargetIBatt +#undef ICharger +#undef ISingleCharger2 +#undef ISingleCharger3 + +namespace wibotic +{ + +template +struct UAVCAN_EXPORT WiBoticInfo_ +{ + typedef const WiBoticInfo_<_tmpl> & ParameterType; + typedef WiBoticInfo_<_tmpl> & ReferenceType; + + struct ConstantTypes + { + }; + + struct FieldTypes + { + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> VMonBatt; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> IBattery; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> VRect; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> VMonCharger; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> TBoard; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> TargetIBatt; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> ICharger; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> ISingleCharger2; + typedef ::uavcan::FloatSpec<16, ::uavcan::CastModeSaturate> ISingleCharger3; + }; + + enum { + MinBitLen = FieldTypes::VMonBatt::MinBitLen + FieldTypes::IBattery::MinBitLen + + FieldTypes::VRect::MinBitLen + FieldTypes::VMonCharger::MinBitLen + + FieldTypes::TBoard::MinBitLen + FieldTypes::TargetIBatt::MinBitLen + + FieldTypes::ICharger::MinBitLen + FieldTypes::ISingleCharger2::MinBitLen + + FieldTypes::ISingleCharger3::MinBitLen + }; + + enum { + MaxBitLen = FieldTypes::VMonBatt::MaxBitLen + FieldTypes::IBattery::MaxBitLen + + FieldTypes::VRect::MaxBitLen + FieldTypes::VMonCharger::MaxBitLen + + FieldTypes::TBoard::MaxBitLen + FieldTypes::TargetIBatt::MaxBitLen + + FieldTypes::ICharger::MaxBitLen + FieldTypes::ISingleCharger2::MaxBitLen + + FieldTypes::ISingleCharger3::MaxBitLen + }; + + // Constants + + // Fields + typename ::uavcan::StorageType::Type VMonBatt; + typename ::uavcan::StorageType::Type IBattery; + typename ::uavcan::StorageType::Type VRect; + typename ::uavcan::StorageType::Type VMonCharger; + typename ::uavcan::StorageType::Type TBoard; + typename ::uavcan::StorageType::Type TargetIBatt; + typename ::uavcan::StorageType::Type ICharger; + typename ::uavcan::StorageType::Type ISingleCharger2; + typename ::uavcan::StorageType::Type ISingleCharger3; + + WiBoticInfo_() + : VMonBatt(), + IBattery(), + VRect(), + VMonCharger(), + TBoard(), + TargetIBatt(), + ICharger(), + ISingleCharger2(), + ISingleCharger3() + { + ::uavcan::StaticAssert<_tmpl == 0>::check(); // Usage check + +#if UAVCAN_DEBUG + /* + * Cross-checking MaxBitLen provided by the DSDL compiler. + * This check shall never be performed in user code because MaxBitLen value + * actually depends on the nested types, thus it is not invariant. + */ + ::uavcan::StaticAssert<144 == MaxBitLen>::check(); +#endif + } + + bool operator==(ParameterType rhs) const; + bool operator!=(ParameterType rhs) const { return !operator==(rhs); } + + /** + * This comparison is based on @ref uavcan::areClose(), which ensures proper comparison of + * floating point fields at any depth. + */ + bool isClose(ParameterType rhs) const; + + static int encode( + ParameterType self, ::uavcan::ScalarCodec & codec, + ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled); + + static int decode( + ReferenceType self, ::uavcan::ScalarCodec & codec, + ::uavcan::TailArrayOptimizationMode tao_mode = ::uavcan::TailArrayOptEnabled); + + /* + * Static type info + */ + enum { DataTypeKind = ::uavcan::DataTypeKindMessage }; + enum { DefaultDataTypeID = 20200 }; + + static const char * getDataTypeFullName() { return "wibotic.WiBoticInfo"; } + + static void extendDataTypeSignature(::uavcan::DataTypeSignature & signature) + { + signature.extend(getDataTypeSignature()); + } + + static ::uavcan::DataTypeSignature getDataTypeSignature(); +}; + +/* + * Out of line struct method definitions + */ + +template +bool WiBoticInfo_<_tmpl>::operator==(ParameterType rhs) const +{ + return VMonBatt == rhs.VMonBatt && IBattery == rhs.IBattery && VRect == rhs.VRect && + VMonCharger == rhs.VMonCharger && TBoard == rhs.TBoard && TargetIBatt == rhs.TargetIBatt && + ICharger == rhs.ICharger && ISingleCharger2 == rhs.ISingleCharger2 && + ISingleCharger3 == rhs.ISingleCharger3; +} + +template +bool WiBoticInfo_<_tmpl>::isClose(ParameterType rhs) const +{ + return ::uavcan::areClose(VMonBatt, rhs.VMonBatt) && ::uavcan::areClose(IBattery, rhs.IBattery) && + ::uavcan::areClose(VRect, rhs.VRect) && ::uavcan::areClose(VMonCharger, rhs.VMonCharger) && + ::uavcan::areClose(TBoard, rhs.TBoard) && + ::uavcan::areClose(TargetIBatt, rhs.TargetIBatt) && + ::uavcan::areClose(ICharger, rhs.ICharger) && + ::uavcan::areClose(ISingleCharger2, rhs.ISingleCharger2) && + ::uavcan::areClose(ISingleCharger3, rhs.ISingleCharger3); +} + +template +int WiBoticInfo_<_tmpl>::encode( + ParameterType self, ::uavcan::ScalarCodec & codec, ::uavcan::TailArrayOptimizationMode tao_mode) +{ + (void)self; + (void)codec; + (void)tao_mode; + int res = 1; + res = FieldTypes::VMonBatt::encode(self.VMonBatt, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::IBattery::encode(self.IBattery, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::VRect::encode(self.VRect, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::VMonCharger::encode(self.VMonCharger, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::TBoard::encode(self.TBoard, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::TargetIBatt::encode(self.TargetIBatt, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::ICharger::encode(self.ICharger, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::ISingleCharger2::encode( + self.ISingleCharger2, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::ISingleCharger3::encode(self.ISingleCharger3, codec, tao_mode); + return res; +} + +template +int WiBoticInfo_<_tmpl>::decode( + ReferenceType self, ::uavcan::ScalarCodec & codec, ::uavcan::TailArrayOptimizationMode tao_mode) +{ + (void)self; + (void)codec; + (void)tao_mode; + int res = 1; + res = FieldTypes::VMonBatt::decode(self.VMonBatt, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::IBattery::decode(self.IBattery, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::VRect::decode(self.VRect, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::VMonCharger::decode(self.VMonCharger, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::TBoard::decode(self.TBoard, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::TargetIBatt::decode(self.TargetIBatt, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::ICharger::decode(self.ICharger, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::ISingleCharger2::decode( + self.ISingleCharger2, codec, ::uavcan::TailArrayOptDisabled); + if (res <= 0) { + return res; + } + res = FieldTypes::ISingleCharger3::decode(self.ISingleCharger3, codec, tao_mode); + return res; +} + +/* + * Out of line type method definitions + */ +template +::uavcan::DataTypeSignature WiBoticInfo_<_tmpl>::getDataTypeSignature() +{ + ::uavcan::DataTypeSignature signature(0xD7EF2EACD772E948ULL); + + FieldTypes::VMonBatt::extendDataTypeSignature(signature); + FieldTypes::IBattery::extendDataTypeSignature(signature); + FieldTypes::VRect::extendDataTypeSignature(signature); + FieldTypes::VMonCharger::extendDataTypeSignature(signature); + FieldTypes::TBoard::extendDataTypeSignature(signature); + FieldTypes::TargetIBatt::extendDataTypeSignature(signature); + FieldTypes::ICharger::extendDataTypeSignature(signature); + FieldTypes::ISingleCharger2::extendDataTypeSignature(signature); + FieldTypes::ISingleCharger3::extendDataTypeSignature(signature); + + return signature; +} + +/* + * Out of line constant definitions + */ + +/* + * Final typedef + */ +typedef WiBoticInfo_<0> WiBoticInfo; + +namespace +{ + +const ::uavcan::DefaultDataTypeRegistrator< ::wibotic::WiBoticInfo> + _uavcan_gdtr_registrator_WiBoticInfo; + +} + +} // Namespace wibotic + +/* + * YAML streamer specialization + */ +namespace uavcan +{ + +template <> +class UAVCAN_EXPORT YamlStreamer< ::wibotic::WiBoticInfo> +{ +public: + template + static void stream(Stream & s, ::wibotic::WiBoticInfo::ParameterType obj, const int level); +}; + +template +void YamlStreamer< ::wibotic::WiBoticInfo>::stream( + Stream & s, ::wibotic::WiBoticInfo::ParameterType obj, const int level) +{ + (void)s; + (void)obj; + (void)level; + if (level > 0) { + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + } + s << "VMonBatt: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::VMonBatt>::stream(s, obj.VMonBatt, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "IBattery: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::IBattery>::stream(s, obj.IBattery, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "VRect: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::VRect>::stream(s, obj.VRect, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "VMonCharger: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::VMonCharger>::stream( + s, obj.VMonCharger, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "TBoard: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::TBoard>::stream(s, obj.TBoard, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "TargetIBatt: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::TargetIBatt>::stream( + s, obj.TargetIBatt, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "ICharger: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::ICharger>::stream(s, obj.ICharger, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "ISingleCharger2: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::ISingleCharger2>::stream( + s, obj.ISingleCharger2, level + 1); + s << '\n'; + for (int pos = 0; pos < level; pos++) { + s << " "; + } + s << "ISingleCharger3: "; + YamlStreamer< ::wibotic::WiBoticInfo::FieldTypes::ISingleCharger3>::stream( + s, obj.ISingleCharger3, level + 1); +} + +} // namespace uavcan + +namespace wibotic +{ + +template +inline Stream & operator<<(Stream & s, ::wibotic::WiBoticInfo::ParameterType obj) +{ + ::uavcan::YamlStreamer< ::wibotic::WiBoticInfo>::stream(s, obj, 0); + return s; +} + +} // Namespace wibotic + +#endif // WIBOTIC_WIBOTICINFO_HPP_INCLUDED diff --git a/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp new file mode 100644 index 0000000..192ecdc --- /dev/null +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp @@ -0,0 +1,173 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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. + +#ifndef WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_HPP_ +#define WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_HPP_ + +#include +#include + +#include +#include + +#include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" + +namespace wibotic_connector_can +{ + +/** + * @brief Abstract interface for the Wibotic CAN driver. + */ +class WiboticCanDriverInterface +{ +public: + /** + * @brief Virtual destructor for the WiboticCanDriverInterface class. + */ + virtual ~WiboticCanDriverInterface() = default; + + /** * + * @param can_iface_name The name of the CAN interface. + * @param node_id The ID of the node. + * @param node_name The name of the node. + * + * @exception std::runtime_error Thrown if can interface cannot be found. + * */ + virtual void ConfigureUavCan( + const std::string & can_iface_name, std::size_t node_id, const std::string & node_name) = 0; + + /** + * @brief Creates the UAVCAN node. + */ + virtual void CreateUavCanNode() = 0; + + /** + * @brief Creates the WiboticInfo subscriber. + */ + virtual void CreateWiboticInfoSubscriber() = 0; + + /** + * @brief Activates the Wibotic CAN driver. + * + */ + virtual void Activate() = 0; + + /** + * @brief Spins the Wibotic CAN driver. + * + * @param milliseconds The time to spin in milliseconds. + */ + virtual void Spin(std::size_t milliseconds) = 0; + + /** + * @brief Gets the WiboticInfo message. + * + * @return The WiboticInfo message. + */ + virtual wibotic::WiBoticInfo GetWiboticInfo() = 0; + + /** + * @brief Alias for a shared pointer to a WiboticCanDriverInterface object. + */ + using SharedPtr = std::shared_ptr; + + /** + * @brief Alias for a unique pointer to a WiboticCanDriverInterface object. + */ + using UniquePtr = std::unique_ptr; +}; + +/** + * @brief Class for the Wibotic CAN driver. + * + * This class inherits from the `WiboticCanDriverInterface` and implements its methods. + * Class communicates with CAN interface using libuavcan and gets WiboticInfo messages. + */ +class WiboticCanDriver : public WiboticCanDriverInterface +{ + typedef uavcan::MethodBinder< + WiboticCanDriver *, void (WiboticCanDriver::*)(const wibotic::WiBoticInfo &)> + WiBoticInfoCallbackBinder; + +public: + /** + * @param can_iface_name The name of the CAN interface. + * @param node_id The ID of the node. + * @param node_name The name of the node. + * + * @exception std::runtime_error Thrown if can interface cannot be found. + * */ + void ConfigureUavCan( + const std::string & can_iface_name, std::size_t node_id, + const std::string & node_name) override; + + /** + * @brief Creates the UAVCAN node. + */ + void CreateUavCanNode() override; + + void CreateWiboticInfoSubscriber() override; + + /** + * @brief Activates the Wibotic CAN driver. + * + * It starts the UAVCAN node, sets it to operational mode and starts Wibotic subscriber. + * + * @exception std::runtime_error Thrown if the node or subscriber does not exist and they does not + * start properly. + */ + void Activate() override; + + /** + * @brief Spins the Wibotic CAN driver. + * + * @param milliseconds The time to spin in milliseconds. + * + * @exception std::runtime_error Thrown if the node or subscriber does not spin properly. + */ + void Spin(std::size_t milliseconds) override; + + /** + * @brief Gets the WiboticInfo message. + * + * @return The WiboticInfo message. + * + * @exception std::runtime_error Thrown if the WiboticInfo message queue is empty. + */ + wibotic::WiBoticInfo GetWiboticInfo() override; + +protected: + /** + * @brief Callback for the WiboticInfo message. + * + * Adds uavcan messages to the queue. + * + * @param msg The WiboticInfo message. + */ + void WiboticInfoCallback(const wibotic::WiBoticInfo & msg); + + std::string can_iface_name_; + std::size_t node_id_; + std::string node_name_; + bool activated_ = false; + + uavcan_linux::NodePtr uavcan_node_; + std::shared_ptr> wibotic_info_uavcan_sub_; + + std::queue wibotic_info_queue_; +}; + +} // namespace wibotic_connector_can + +#endif // WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_HPP_ diff --git a/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver_node.hpp b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver_node.hpp new file mode 100644 index 0000000..061ca43 --- /dev/null +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver_node.hpp @@ -0,0 +1,62 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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. + +#ifndef WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_NODE_HPP_ +#define WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_NODE_HPP_ + +#include + +#include + +#include "wibotic_msgs/msg/wibotic_info.hpp" + +// RCLCPP is compiling with C++17, so we need to define UAVCAN_CPP_VERSION to UAVCAN_CPP11 +// to avoid compilation errors and silent them. +#define UAVCAN_CPP_VERSION UAVCAN_CPP11 +#include "wibotic_connector_can/wibotic_can_driver.hpp" + +namespace wibotic_connector_can +{ +class WiboticCanDriverNode : public rclcpp::Node +{ +public: + WiboticCanDriverNode( + const std::string & node_name, WiboticCanDriverInterface::SharedPtr wibotic_can_driver, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + void DeclareParameters(); + void GetParameters(); + + void CreateWiboticCanDriver(); + wibotic::WiBoticInfo GetWiboticInfo(); + + void WiboticInfoTimerCallback(); + + wibotic_msgs::msg::WiboticInfo ConvertWiboticInfoToMsg(const wibotic::WiBoticInfo & wibotic_info); + + std::string can_iface_name_; + std::size_t uavcan_node_id_; + std::string uavcan_node_name_; + float update_time_s_; + + WiboticCanDriverInterface::SharedPtr wibotic_can_driver_; + + rclcpp::TimerBase::SharedPtr wibotic_info_timer_; + rclcpp::Publisher::SharedPtr wibotic_info_pub_; +}; + +} // namespace wibotic_connector_can + +#endif // WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_NODE_HPP_ diff --git a/wibotic_connector_can/launch/wibotic_connector_can.launch b/wibotic_connector_can/launch/wibotic_connector_can.launch deleted file mode 100644 index edcc5c3..0000000 --- a/wibotic_connector_can/launch/wibotic_connector_can.launch +++ /dev/null @@ -1,7 +0,0 @@ - - - - - - - diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index 7636a84..f81abb3 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -1,25 +1,30 @@ - + + wibotic_connector_can - 0.0.1 - The wibotic_connector_can package - WiBotic Inc. - BSD 3-Clause - https://www.wibotic.com + 0.1.0 + Integration Husarion UGV with Wibotic charger - message_generation - message_runtime - catkin - rospy - std_msgs - rospy - std_msgs - rospy - std_msgs - python-future - uavcan-pip - wibotic_msg + Husarion + Apache License 2.0 + + https://husarion.com/ + https://github.com/husarion/wibotic_ros + https://github.com/husarion/wibotic_ros/issues + + Jakub Delicat + + ament_cmake + + rclcpp + wibotic_msgs + + ament_cmake_gmock + ament_cmake_gtest + google-mock + ros_testing + ament_cmake diff --git a/wibotic_connector_can/setup.py b/wibotic_connector_can/setup.py deleted file mode 100644 index ba98f78..0000000 --- a/wibotic_connector_can/setup.py +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env python - -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup - -setup_args = generate_distutils_setup( - packages=['wibotic_connector_can'], - package_dir={'': 'src'} -) - -setup(**setup_args) diff --git a/wibotic_connector_can/src/main.cpp b/wibotic_connector_can/src/main.cpp new file mode 100644 index 0000000..a09ea0b --- /dev/null +++ b/wibotic_connector_can/src/main.cpp @@ -0,0 +1,40 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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. + +#include +#include +#include + +#include + +#include "wibotic_connector_can/wibotic_can_driver_node.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + + auto wibotic_can_driver = std::make_shared(); + auto wibotic_can_driver_node = std::make_shared( + "wibotic_can_driver", wibotic_can_driver); + + try { + rclcpp::spin(wibotic_can_driver_node); + } catch (const std::runtime_error & e) { + std::cerr << "[wibotic_can_driver] Caught exception: " << e.what() << std::endl; + } + + std::cout << "[wibotic_can_driver] Shutting down" << std::endl; + rclcpp::shutdown(); + return 0; +} diff --git a/wibotic_connector_can/src/wibotic_can_driver.cpp b/wibotic_connector_can/src/wibotic_can_driver.cpp new file mode 100644 index 0000000..0e230f6 --- /dev/null +++ b/wibotic_connector_can/src/wibotic_can_driver.cpp @@ -0,0 +1,98 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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. + +#include "wibotic_connector_can/wibotic_can_driver.hpp" + +namespace wibotic_connector_can +{ +void WiboticCanDriver::ConfigureUavCan( + const std::string & can_iface_name, std::size_t node_id, const std::string & node_name) +{ + can_iface_name_ = can_iface_name; + node_id_ = node_id; + node_name_ = node_name; +} + +void WiboticCanDriver::CreateUavCanNode() +{ + uavcan_node_ = uavcan_linux::makeNode({can_iface_name_}); + uavcan_node_->setNodeID(node_id_); + uavcan_node_->setName(node_name_.c_str()); +} + +void WiboticCanDriver::CreateWiboticInfoSubscriber() +{ + wibotic_info_uavcan_sub_ = + std::make_shared>(*uavcan_node_); +} + +void WiboticCanDriver::Activate() +{ + if (!uavcan_node_) { + throw std::runtime_error("Trying to activate nonexisting node."); + } + + const int sub_res = wibotic_info_uavcan_sub_->start( + WiBoticInfoCallbackBinder(this, &WiboticCanDriver::WiboticInfoCallback)); + if (sub_res < 0) { + throw std::runtime_error("Failed to start the subscriber; error: " + std::to_string(sub_res)); + } + + const int node_start_res = uavcan_node_->start(); + if (node_start_res < 0) { + throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res)); + } + + uavcan_node_->setModeOperational(); + + activated_ = true; +} + +void WiboticCanDriver::Spin(std::size_t milliseconds) +{ + if (!uavcan_node_) { + throw std::runtime_error("Trying to spin nonexisting node."); + } + + if (!wibotic_info_uavcan_sub_) { + throw std::runtime_error("Trying to spin nonexisting subscriber."); + } + + if (!activated_) { + throw std::runtime_error("Trying to spin non-activated driver."); + } + + const int res = uavcan_node_->spin(uavcan::MonotonicDuration::fromMSec(milliseconds)); + if (res < 0) { + throw std::runtime_error("Failed to spin UAVCAN node, res: " + std::to_string(res)); + } +} + +wibotic::WiBoticInfo WiboticCanDriver::GetWiboticInfo() +{ + if (wibotic_info_queue_.empty()) { + throw std::runtime_error("WiBoticInfo queue is empty."); + } + + wibotic::WiBoticInfo wibotic_info = wibotic_info_queue_.front(); + wibotic_info_queue_.pop(); + return wibotic_info; +} + +void WiboticCanDriver::WiboticInfoCallback(const wibotic::WiBoticInfo & msg) +{ + wibotic_info_queue_.push(msg); +} + +} // namespace wibotic_connector_can diff --git a/wibotic_connector_can/src/wibotic_can_driver_node.cpp b/wibotic_connector_can/src/wibotic_can_driver_node.cpp new file mode 100644 index 0000000..67236e1 --- /dev/null +++ b/wibotic_connector_can/src/wibotic_can_driver_node.cpp @@ -0,0 +1,117 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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. + +#include "wibotic_connector_can/wibotic_can_driver_node.hpp" + +namespace wibotic_connector_can +{ +WiboticCanDriverNode::WiboticCanDriverNode( + const std::string & node_name, WiboticCanDriverInterface::SharedPtr wibotic_can_driver, + const rclcpp::NodeOptions & options) +: rclcpp::Node(node_name, options), wibotic_can_driver_(wibotic_can_driver) + +{ + RCLCPP_INFO(this->get_logger(), "Initializing node."); + if (!wibotic_can_driver_) { + throw std::runtime_error("Wibotic CAN driver is not initialized."); + } + + DeclareParameters(); + GetParameters(); + + CreateWiboticCanDriver(); + + wibotic_info_pub_ = this->create_publisher("wibotic_info", 10); + + wibotic_info_timer_ = this->create_wall_timer( + std::chrono::duration(update_time_s_), + std::bind(&WiboticCanDriverNode::WiboticInfoTimerCallback, this)); + + RCLCPP_INFO(this->get_logger(), "Node initialized successfully."); +} + +void WiboticCanDriverNode::DeclareParameters() +{ + this->declare_parameter("can_iface_name", "can0"); + this->declare_parameter("uavcan_node_id", 20); + this->declare_parameter("uavcan_node_name", "com.wibotic.ros_connector"); + this->declare_parameter("update_time_s", 0.2); +} + +void WiboticCanDriverNode::GetParameters() +{ + can_iface_name_ = this->get_parameter("can_iface_name").as_string(); + uavcan_node_id_ = this->get_parameter("uavcan_node_id").as_int(); + uavcan_node_name_ = this->get_parameter("uavcan_node_name").as_string(); + update_time_s_ = this->get_parameter("update_time_s").as_double(); +} + +void WiboticCanDriverNode::CreateWiboticCanDriver() +{ + wibotic_can_driver_->ConfigureUavCan(can_iface_name_, uavcan_node_id_, uavcan_node_name_); + wibotic_can_driver_->CreateUavCanNode(); + wibotic_can_driver_->CreateWiboticInfoSubscriber(); + wibotic_can_driver_->Activate(); +} + +wibotic::WiBoticInfo WiboticCanDriverNode::GetWiboticInfo() +{ + const auto update_time_ms = static_cast(update_time_s_ * 1000); + wibotic_can_driver_->Spin(update_time_ms); + + return wibotic_can_driver_->GetWiboticInfo(); +} + +void WiboticCanDriverNode::WiboticInfoTimerCallback() +{ + if (!wibotic_can_driver_) { + throw std::runtime_error("Trying to get WiboticInfo message from nonexisting driver."); + } + + wibotic::WiBoticInfo wibotic_info; + try { + wibotic_info = GetWiboticInfo(); + } catch (const std::runtime_error & e) { + // Skip if there is no messages. + return; + } + + try { + wibotic_info_pub_->publish(ConvertWiboticInfoToMsg(wibotic_info)); + } catch (const std::runtime_error & e) { + RCLCPP_WARN(this->get_logger(), e.what()); + } +} + +wibotic_msgs::msg::WiboticInfo WiboticCanDriverNode::ConvertWiboticInfoToMsg( + const wibotic::WiBoticInfo & wibotic_info) +{ + wibotic_msgs::msg::WiboticInfo wibotic_info_msg; + + wibotic_info_msg.header.stamp = this->now(); + wibotic_info_msg.header.frame_id = "wibotic_receiver"; + wibotic_info_msg.v_mon_batt = wibotic_info.VMonBatt; + wibotic_info_msg.i_battery = wibotic_info.IBattery; + wibotic_info_msg.v_rect = wibotic_info.VRect; + wibotic_info_msg.v_mon_charger = wibotic_info.VMonCharger; + wibotic_info_msg.t_board = wibotic_info.TBoard; + wibotic_info_msg.target_i_batt = wibotic_info.TargetIBatt; + wibotic_info_msg.i_charger = wibotic_info.ICharger; + wibotic_info_msg.i_single_charger2 = wibotic_info.ISingleCharger2; + wibotic_info_msg.i_single_charger3 = wibotic_info.ISingleCharger3; + + return wibotic_info_msg; +} + +} // namespace wibotic_connector_can diff --git a/wibotic_connector_can/src/wibotic_connector_can/__init__.py b/wibotic_connector_can/src/wibotic_connector_can/__init__.py deleted file mode 100755 index f662bb5..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/__init__.py +++ /dev/null @@ -1,3 +0,0 @@ -#!/usr/bin/env python - -import wiboticros \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan deleted file mode 100644 index b28c712..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan +++ /dev/null @@ -1,17 +0,0 @@ -# -# WiBotic periodic information. -# - -# -# Primary parameters. -# Some fields can be set to NAN if their values are unknown. -# -float16 VMonBatt -float16 IBattery -float16 VRect -float16 VMonCharger -float16 TBoard -float16 TargetIBatt -float16 ICharger -float16 ISingleCharger2 -float16 ISingleCharger3 \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan deleted file mode 100644 index e40691e..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan +++ /dev/null @@ -1,6 +0,0 @@ -# -# Describes a radio base station that has been seen -# - -uint64 unique_id -uint8 rssi \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.dsdl b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.dsdl deleted file mode 100644 index 10b3dd9..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.dsdl +++ /dev/null @@ -1,7 +0,0 @@ -# Node ID of the intended recipient. 255 to send to all nodes -uint8 destination - -uint12 CAPACITY_BYTES = 48 -uint8[<=CAPACITY_BYTES] payload - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.uavcan deleted file mode 100644 index 10b3dd9..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.uavcan +++ /dev/null @@ -1,7 +0,0 @@ -# Node ID of the intended recipient. 255 to send to all nodes -uint8 destination - -uint12 CAPACITY_BYTES = 48 -uint8[<=CAPACITY_BYTES] payload - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.dsdl b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.dsdl deleted file mode 100644 index 20dfee1..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.dsdl +++ /dev/null @@ -1,11 +0,0 @@ -# Outgoing serial packets are written as a service type to allow for RPC -# like behavior - -uint12 CAPACITY_BYTES = 48 -uint8[<=CAPACITY_BYTES] payload - -@sealed - ---- - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.uavcan deleted file mode 100644 index 20dfee1..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.uavcan +++ /dev/null @@ -1,11 +0,0 @@ -# Outgoing serial packets are written as a service type to allow for RPC -# like behavior - -uint12 CAPACITY_BYTES = 48 -uint8[<=CAPACITY_BYTES] payload - -@sealed - ---- - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.dsdl b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.dsdl deleted file mode 100644 index 2882b44..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.dsdl +++ /dev/null @@ -1,8 +0,0 @@ -# -# Describes a radio base station that has been seen -# - -uint64 unique_id -uint8 rssi - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.uavcan deleted file mode 100644 index 2882b44..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.uavcan +++ /dev/null @@ -1,8 +0,0 @@ -# -# Describes a radio base station that has been seen -# - -uint64 unique_id -uint8 rssi - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.dsdl b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.dsdl deleted file mode 100644 index c0e3277..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.dsdl +++ /dev/null @@ -1,31 +0,0 @@ -# -# WiBotic periodic information. -# - -# -# Primary parameters. -# Some fields can be set to NAN if their values are unknown. -# -uavcan.si.unit.voltage.Scalar.1.0 VMonBatt -uavcan.si.unit.electric_current.Scalar.1.0 IBattery -uavcan.si.unit.voltage.Scalar.1.0 VRect -uavcan.si.unit.voltage.Scalar.1.0 VMonCharger -uavcan.si.unit.electric_current.Scalar.1.0 TargetIBatt -uavcan.si.unit.electric_current.Scalar.1.0 ICharger -float32 TBoard - -uint8 CHARGER_STATE_IDLE = 0 -uint8 CHARGER_STATE_RAMP_UP = 1 -uint8 CHARGER_STATE_CC_CHARGE = 2 -uint8 CHARGER_STATE_CV_CHARGE = 3 -uint8 CHARGER_STATE_CHG_COMPLETE = 4 -uint8 CHARGER_STATE_STOPPING = 5 -uint8 CHARGER_STATE_ALARM = 6 -uint8 CHARGER_STATE_CHECK_VOLT = 7 -uint8 CHARGER_STATE_RECOVERY = 8 -uint8 CHARGER_STATE_FATAL = 9 -uint8 CHARGER_STATE_POWER_SUPPLY = 10 -uint8 CHARGER_STATE_NEGATIVE_DELTA_V = 11 -uint8 ChargerState - -@extent 64 * 8 \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.uavcan deleted file mode 100644 index c0e3277..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.uavcan +++ /dev/null @@ -1,31 +0,0 @@ -# -# WiBotic periodic information. -# - -# -# Primary parameters. -# Some fields can be set to NAN if their values are unknown. -# -uavcan.si.unit.voltage.Scalar.1.0 VMonBatt -uavcan.si.unit.electric_current.Scalar.1.0 IBattery -uavcan.si.unit.voltage.Scalar.1.0 VRect -uavcan.si.unit.voltage.Scalar.1.0 VMonCharger -uavcan.si.unit.electric_current.Scalar.1.0 TargetIBatt -uavcan.si.unit.electric_current.Scalar.1.0 ICharger -float32 TBoard - -uint8 CHARGER_STATE_IDLE = 0 -uint8 CHARGER_STATE_RAMP_UP = 1 -uint8 CHARGER_STATE_CC_CHARGE = 2 -uint8 CHARGER_STATE_CV_CHARGE = 3 -uint8 CHARGER_STATE_CHG_COMPLETE = 4 -uint8 CHARGER_STATE_STOPPING = 5 -uint8 CHARGER_STATE_ALARM = 6 -uint8 CHARGER_STATE_CHECK_VOLT = 7 -uint8 CHARGER_STATE_RECOVERY = 8 -uint8 CHARGER_STATE_FATAL = 9 -uint8 CHARGER_STATE_POWER_SUPPLY = 10 -uint8 CHARGER_STATE_NEGATIVE_DELTA_V = 11 -uint8 ChargerState - -@extent 64 * 8 \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/IncomingSerial.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/IncomingSerial.1.0.uavcan deleted file mode 100644 index 10b3dd9..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/IncomingSerial.1.0.uavcan +++ /dev/null @@ -1,7 +0,0 @@ -# Node ID of the intended recipient. 255 to send to all nodes -uint8 destination - -uint12 CAPACITY_BYTES = 48 -uint8[<=CAPACITY_BYTES] payload - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/OutgoingSerial.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/OutgoingSerial.1.0.uavcan deleted file mode 100644 index 20dfee1..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/OutgoingSerial.1.0.uavcan +++ /dev/null @@ -1,11 +0,0 @@ -# Outgoing serial packets are written as a service type to allow for RPC -# like behavior - -uint12 CAPACITY_BYTES = 48 -uint8[<=CAPACITY_BYTES] payload - -@sealed - ---- - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/RadioBaseStation.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/RadioBaseStation.1.0.uavcan deleted file mode 100644 index 2882b44..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/RadioBaseStation.1.0.uavcan +++ /dev/null @@ -1,8 +0,0 @@ -# -# Describes a radio base station that has been seen -# - -uint64 unique_id -uint8 rssi - -@sealed \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/WiBoticInfo.1.0.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/WiBoticInfo.1.0.uavcan deleted file mode 100644 index c0e3277..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/WiBoticInfo.1.0.uavcan +++ /dev/null @@ -1,31 +0,0 @@ -# -# WiBotic periodic information. -# - -# -# Primary parameters. -# Some fields can be set to NAN if their values are unknown. -# -uavcan.si.unit.voltage.Scalar.1.0 VMonBatt -uavcan.si.unit.electric_current.Scalar.1.0 IBattery -uavcan.si.unit.voltage.Scalar.1.0 VRect -uavcan.si.unit.voltage.Scalar.1.0 VMonCharger -uavcan.si.unit.electric_current.Scalar.1.0 TargetIBatt -uavcan.si.unit.electric_current.Scalar.1.0 ICharger -float32 TBoard - -uint8 CHARGER_STATE_IDLE = 0 -uint8 CHARGER_STATE_RAMP_UP = 1 -uint8 CHARGER_STATE_CC_CHARGE = 2 -uint8 CHARGER_STATE_CV_CHARGE = 3 -uint8 CHARGER_STATE_CHG_COMPLETE = 4 -uint8 CHARGER_STATE_STOPPING = 5 -uint8 CHARGER_STATE_ALARM = 6 -uint8 CHARGER_STATE_CHECK_VOLT = 7 -uint8 CHARGER_STATE_RECOVERY = 8 -uint8 CHARGER_STATE_FATAL = 9 -uint8 CHARGER_STATE_POWER_SUPPLY = 10 -uint8 CHARGER_STATE_NEGATIVE_DELTA_V = 11 -uint8 ChargerState - -@extent 64 * 8 \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20201.WiBoticCommand.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20201.WiBoticCommand.uavcan deleted file mode 100644 index 91803c7..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20201.WiBoticCommand.uavcan +++ /dev/null @@ -1,14 +0,0 @@ -# -# WiBotic Commands -# - -# -# Command -# This can be a code for a specific handler that will route the command properly for you. -# -uint16 command - -# -# Payload -# -uint32 payload \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20202.WiBoticRead.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20202.WiBoticRead.uavcan deleted file mode 100644 index 89144d2..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20202.WiBoticRead.uavcan +++ /dev/null @@ -1,9 +0,0 @@ -# -# WiBotic Read -# - -# -# Parameter Name -# This must be the code for a valid parameter to read -# -uint16 parameter_id \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan deleted file mode 100644 index e40691e..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan +++ /dev/null @@ -1,6 +0,0 @@ -# -# Describes a radio base station that has been seen -# - -uint64 unique_id -uint8 rssi \ No newline at end of file diff --git a/wibotic_connector_can/src/wibotic_connector_can/wiboticros.py b/wibotic_connector_can/src/wibotic_connector_can/wiboticros.py deleted file mode 100755 index a8d7858..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/wiboticros.py +++ /dev/null @@ -1,334 +0,0 @@ -#!/usr/bin/env python - -# use `pip install future pyuavcan_v0` before running on python 2 - -# ROS Imports -import rospy -from wibotic_msg import msg, srv - -# Other Imports -import pyuavcan_v0 as uavcan -import threading -import time -import queue -import signal -import sys -import os - -# Global state and constants shared between threads -DSDL_PATH = ( - os.path.dirname(__file__) - + "/uavcan_v0/uavcan_vendor_specific_types/wibotic" -) -WIBOTIC_NODE_NAME = "com.wibotic.charger" -PARAMETER_REQ_TIMEOUT = 3 -SUCCESS = 5 -FAILURE = 0 -_uav_incoming_info = queue.Queue() -_uav_incoming_param = queue.Queue(1) -_uav_outgoing = queue.Queue() -_threads = [] -_shutting_down = False - - -def ClearQueue(q): - while not q.empty(): - q.get() - - -def ascii_list_to_str(l): - return "".join(chr(i) for i in l) - - -class Shutdown(Exception): - pass - - -class ROSNodeThread(threading.Thread): - class Node: - def sender(self): - pub = rospy.Publisher( - "~wibotic_info", - msg.WiBoticInfo, - queue_size=10, - ) - while not rospy.is_shutdown(): - incoming_data = _uav_incoming_info.get() - uavcan_dsdl_type = uavcan.get_uavcan_data_type( - incoming_data - ) - unpacked_data = {} - for field in uavcan_dsdl_type.fields: - unpacked_data[field.name] = getattr( - incoming_data, field.name - ) - packaged_data = msg.WiBoticInfo( - **unpacked_data - ) - rospy.loginfo(packaged_data) - pub.publish(packaged_data) - - def handle_param_list(self, req): - params = [] - index = 0 - ClearQueue(_uav_incoming_param) - while True: - request = uavcan.protocol.param.GetSet.Request( - index=index - ) - _uav_outgoing.put(request) - try: - response = _uav_incoming_param.get( - block=True, - timeout=PARAMETER_REQ_TIMEOUT, - ) - if ( - uavcan.get_active_union_field( - response.value - ) - != "empty" - ): - params.append( - ascii_list_to_str(response.name) - ) - index += 1 - rospy.loginfo(index) - else: - return [SUCCESS, params] - except queue.Empty: - return [FAILURE, params] - - def handle_param_read(self, req): - request = uavcan.protocol.param.GetSet.Request( - name=req.name - ) - _uav_outgoing.put(request) - try: - response = _uav_incoming_param.get( - block=True, - timeout=PARAMETER_REQ_TIMEOUT, - ) - if ( - uavcan.get_active_union_field( - response.value - ) - != "empty" - ): - status = ( - SUCCESS - if response.name == req.name - else FAILURE - ) - value = response.value.integer_value - return [status, value] - except queue.Empty: - pass - - return [FAILURE, 0] - - def handle_param_write(self, req): - request = uavcan.protocol.param.GetSet.Request( - name=req.name, - value=uavcan.protocol.param.Value( - integer_value=req.value - ), - ) - _uav_outgoing.put(request) - try: - response = _uav_incoming_param.get( - block=True, - timeout=PARAMETER_REQ_TIMEOUT, - ) - if ( - uavcan.get_active_union_field( - response.value - ) - != "empty" - ): - return ( - SUCCESS - if response.value.integer_value - == req.value - else FAILURE - ) - except queue.Empty: - pass - - return FAILURE - - def handle_param_save(self, req): - request = uavcan.protocol.param.ExecuteOpcode.Request( - opcode=uavcan.protocol.param.ExecuteOpcode.Request().OPCODE_SAVE - ) - _uav_outgoing.put(request) - try: - response = _uav_incoming_param.get( - block=True, - timeout=PARAMETER_REQ_TIMEOUT, - ) - return SUCCESS if response.ok else FAILURE - except queue.Empty: - return FAILURE - - def __init__(self): - rospy.init_node("wibotic_connector_can") - super(ROSNodeThread, self).__init__() - self.daemon = True - - def run(self): - rospy.loginfo("ROS Thread Initialized") - node = ROSNodeThread.Node() - try: - rospy.Service( - "~read_parameter", - srv.ReadParameter, - node.handle_param_read, - ) - rospy.Service( - "~write_parameter", - srv.WriteParameter, - node.handle_param_write, - ) - rospy.Service( - "~list_parameters", - srv.ListParameters, - node.handle_param_list, - ) - rospy.Service( - "~save_parameters", - srv.SaveParameters, - node.handle_param_save, - ) - node.sender() - except rospy.ROSInterruptException: - pass - rospy.loginfo("ROS Thread Finished") - - -class UAVCanNodeThread(threading.Thread): - class Node: - outstanding_param_request = threading.Semaphore() - - def __init__(self, can_interface, node_id): - uavcan.load_dsdl(DSDL_PATH) - node_info = ( - uavcan.protocol.GetNodeInfo.Response() - ) - node_info.name = "com.wibotic.ros_connector" - node_info.software_version.major = 1 - try: - self.uavcan_node = uavcan.make_node( - can_interface, - node_id=node_id, - node_info=node_info, - mode=uavcan.protocol.NodeStatus().MODE_OPERATIONAL, - ) - except OSError: - rospy.logerr( - "ERROR: Device not found. " - "Please confirm the device name is correctly set!" - ) - else: - self.monitor = uavcan.app.node_monitor.NodeMonitor( - self.uavcan_node - ) - self.uavcan_node.add_handler( - uavcan.thirdparty.wibotic.WiBoticInfo, - self.wibotic_info_callback, - ) - - def get_wibotic_node_id(self): - online_nodes = self.monitor.get_all_node_id() - for node_id in online_nodes: - node_name = ascii_list_to_str( - self.monitor.get(node_id).info.name - ) - if node_name == WIBOTIC_NODE_NAME: - return node_id - return None - - def wibotic_info_callback(self, event): - _uav_incoming_info.put(event.transfer.payload) - - def wibotic_param_callback(self, event): - _uav_incoming_param.put(event.transfer.payload) - self.outstanding_param_request.release() - - def send_pending(self): - while True: - send_item = _uav_outgoing.get() - target_node_id = self.get_wibotic_node_id() - if target_node_id is not None: - self.outstanding_param_request.acquire() - self.uavcan_node.request( - send_item, - target_node_id, - self.wibotic_param_callback, - ) - else: - rospy.logwarn( - "No WiBotic device found on bus" - ) - - def check_shutdown(self): - if _shutting_down: - self.uavcan_node.close() - raise Shutdown("Node Shutdown") - - def __init__(self): - super(UAVCanNodeThread, self).__init__() - self.daemon = True - - def run(self): - rospy.loginfo("UAVCAN Thread Initialized") - - if not rospy.has_param("~can_interface"): - rospy.set_param("~can_interface", "can0") - if not rospy.has_param("~uavcan_node_id"): - rospy.set_param("~uavcan_node_id", 20) - can_interface = rospy.get_param("~can_interface") - node_id = rospy.get_param("~uavcan_node_id") - node = UAVCanNodeThread.Node(can_interface, node_id) - try: - # Thread implicitly daemonic since parent thread is daemonic - threading.Thread( - target=node.send_pending - ).start() - node.uavcan_node.periodic( - 1, node.check_shutdown - ) - node.uavcan_node.spin() - except uavcan.UAVCANException as e: - rospy.logerr(e) - except Shutdown as e: - pass - rospy.loginfo("UAVCAN Thread Finished") - - -def graceful_shutdown(signal, frame): - global _shutting_down - if not _shutting_down: - rospy.loginfo("Shutting Down") - - _shutting_down = True - rospy.signal_shutdown("Shutdown Requested") - for t in _threads: - t.join(timeout=3) - - sys.exit(0) - - -if __name__ == "__main__": - rospy.loginfo("WiBotic ROS Interface") - - _threads.append(UAVCanNodeThread()) - _threads.append(ROSNodeThread()) - - for t in _threads: - t.start() - - signal.signal(signal.SIGINT, graceful_shutdown) - signal.signal(signal.SIGTERM, graceful_shutdown) - rospy.loginfo("Press Ctrl+C to stop") - while True: - time.sleep(1) # allow SIGINT to be handled diff --git a/wibotic_connector_can/test/integration/wibotic_connector_can.test.py b/wibotic_connector_can/test/integration/wibotic_connector_can.test.py new file mode 100644 index 0000000..1b00c57 --- /dev/null +++ b/wibotic_connector_can/test/integration/wibotic_connector_can.test.py @@ -0,0 +1,69 @@ +# Copyright 2024 Husarion sp. z o.o. +# +# 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. + + +import unittest + +import launch +import launch_testing +import pytest +from launch import LaunchDescription +from launch_ros.actions import Node +from launch_testing_ros import WaitForTopics + +from wibotic_msgs.msg import WiboticInfo + + +@pytest.mark.launch_test +def generate_test_description(): + + wibotic_connector_can = Node( + package="wibotic_connector_can", + executable="wibotic_connector_can", + ) + + # Start test after 1s + delay_timer = launch.actions.TimerAction( + period=1.0, actions=[launch_testing.actions.ReadyToTest()] + ) + + actions = [wibotic_connector_can, delay_timer] + + context = {} + + return ( + LaunchDescription(actions), + context, + ) + + +class TestNodeInitialization(unittest.TestCase): + def test_initialization_log(self, proc_output): + proc_output.assertWaitFor("Node initialized successfully.") + + +class TestNode(unittest.TestCase): + def test_msg(self): + topic_list = [("/wibotic_info", WiboticInfo)] + + with WaitForTopics(topic_list, timeout=4.0) as wait_for_topics: + received_topics_str = ", ".join(wait_for_topics.topics_received()) + print("Received messages from the following topics: [" + received_topics_str + "]") + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that process exits with code 0: no error + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp b/wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp new file mode 100644 index 0000000..36d7749 --- /dev/null +++ b/wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp @@ -0,0 +1,143 @@ +// Copyright 2024 Husarion sp. z o.o. +// +// 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. + +#include +#include + +#include + +// RCLCPP is compiling with C++17, so we need to define UAVCAN_CPP_VERSION to UAVCAN_CPP11 +// to avoid compilation errors and silent them. +#define UAVCAN_CPP_VERSION UAVCAN_CPP11 +#include "wibotic_connector_can/wibotic_can_driver.hpp" +#include "wibotic_connector_can/wibotic_can_driver_node.hpp" + +namespace wibotic +{ + +// This is dummy stream overload because the real one is in the uavcan library what is built with +// C++11/ +std::ostream & operator<<(std::ostream & os, const WiBoticInfo &) { return os; } + +} // namespace wibotic + +class MockWiboticCanDriver : public wibotic_connector_can::WiboticCanDriverInterface +{ +public: + MOCK_METHOD( + void, ConfigureUavCan, (const std::string &, std::size_t, const std::string &), (override)); + MOCK_METHOD(void, CreateUavCanNode, (), (override)); + MOCK_METHOD(void, CreateWiboticInfoSubscriber, (), (override)); + MOCK_METHOD(void, Activate, (), (override)); + MOCK_METHOD(void, Spin, (std::size_t), (override)); + MOCK_METHOD(wibotic::WiBoticInfo, GetWiboticInfo, (), (override)); + + // Nice mock suppresses warnings about uninteresting calls + using NiceMock = testing::NiceMock; +}; + +class WiboticCanDriverNodeWrapper : public wibotic_connector_can::WiboticCanDriverNode +{ +public: + WiboticCanDriverNodeWrapper( + const std::string & node_name, + wibotic_connector_can::WiboticCanDriverInterface::SharedPtr wibotic_can_driver, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : wibotic_connector_can::WiboticCanDriverNode(node_name, wibotic_can_driver, options) + { + } + + wibotic::WiBoticInfo GetWiboticInfo() + { + return wibotic_connector_can::WiboticCanDriverNode::GetWiboticInfo(); + } + + wibotic_msgs::msg::WiboticInfo ConvertWiboticInfoToMsg(const wibotic::WiBoticInfo & wibotic_info) + { + return wibotic_connector_can::WiboticCanDriverNode::ConvertWiboticInfoToMsg(wibotic_info); + } +}; + +class TestWiboticCanDriverNode : public ::testing::Test +{ +public: + TestWiboticCanDriverNode(); + +protected: + std::shared_ptr wibotic_can_driver_; + std::unique_ptr wibotic_can_driver_node_; +}; + +TestWiboticCanDriverNode::TestWiboticCanDriverNode() +{ + wibotic_can_driver_ = std::make_shared(); + wibotic_can_driver_node_ = std::make_unique( + "wibotic_can_driver", wibotic_can_driver_); +} + +TEST_F(TestWiboticCanDriverNode, GetWiboticInfoEmptyQueue) +{ + ON_CALL(*wibotic_can_driver_, GetWiboticInfo()) + .WillByDefault(testing::Throw(std::runtime_error("Queue is empty!"))); + + EXPECT_THROW(wibotic_can_driver_node_->GetWiboticInfo(), std::runtime_error); +} + +TEST_F(TestWiboticCanDriverNode, GetWiboticInfo) +{ + wibotic::WiBoticInfo wibotic_info; + wibotic_info.VMonBatt = 1.0; + ON_CALL(*wibotic_can_driver_, GetWiboticInfo()).WillByDefault([wibotic_info]() { + return wibotic_info; + }); + + EXPECT_EQ(wibotic_can_driver_node_->GetWiboticInfo(), wibotic_info); +} + +TEST_F(TestWiboticCanDriverNode, ConvertWiboticInfoToMsg) +{ + wibotic::WiBoticInfo wibotic_info; + wibotic_info.VMonBatt = 1.0; + wibotic_info.IBattery = 2.0; + wibotic_info.VRect = 3.0; + wibotic_info.VMonCharger = 4.0; + wibotic_info.TBoard = 5.0; + wibotic_info.TargetIBatt = 6.0; + wibotic_info.ICharger = 7.0; + wibotic_info.ISingleCharger2 = 8.0; + wibotic_info.ISingleCharger3 = 9.0; + + auto wibotic_info_msg = wibotic_can_driver_node_->ConvertWiboticInfoToMsg(wibotic_info); + + EXPECT_FLOAT_EQ(wibotic_info_msg.v_mon_batt, 1.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.i_battery, 2.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.v_rect, 3.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.v_mon_charger, 4.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.t_board, 5.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.target_i_batt, 6.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.i_charger, 7.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.i_single_charger2, 8.0); + EXPECT_FLOAT_EQ(wibotic_info_msg.i_single_charger3, 9.0); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(0, nullptr); + + auto result = RUN_ALL_TESTS(); + + rclcpp::shutdown(); + return result; +} diff --git a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan b/wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan similarity index 91% rename from wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan rename to wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan index b28c712..88d5fbe 100644 --- a/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan +++ b/wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan @@ -14,4 +14,4 @@ float16 TBoard float16 TargetIBatt float16 ICharger float16 ISingleCharger2 -float16 ISingleCharger3 \ No newline at end of file +float16 ISingleCharger3 diff --git a/wibotic_msg/CMakeLists.txt b/wibotic_msg/CMakeLists.txt deleted file mode 100644 index 0f31590..0000000 --- a/wibotic_msg/CMakeLists.txt +++ /dev/null @@ -1,48 +0,0 @@ -cmake_minimum_required(VERSION 2.8.3) -project(wibotic_msg) - -find_package(catkin REQUIRED COMPONENTS - std_msgs - message_generation -) - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## Generate messages in the 'msg' folder -add_message_files( - FILES - WiBoticInfo.msg -) - -## Generate services in the 'srv' folder -add_service_files( - FILES - ReadParameter.srv - WriteParameter.srv - ListParameters.srv - SaveParameters.srv -) - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - CATKIN_DEPENDS std_msgs message_runtime -) - -########### -## Build ## -########### - -include_directories( - ${catkin_INCLUDE_DIRS} -) - diff --git a/wibotic_msg/msg/WiBoticInfo.msg b/wibotic_msg/msg/WiBoticInfo.msg deleted file mode 100644 index 92c220a..0000000 --- a/wibotic_msg/msg/WiBoticInfo.msg +++ /dev/null @@ -1,9 +0,0 @@ -float32 VMonBatt -float32 IBattery -float32 VRect -float32 VMonCharger -float32 TBoard -float32 TargetIBatt -float32 ICharger -float32 ISingleCharger2 -float32 ISingleCharger3 diff --git a/wibotic_msg/package.xml b/wibotic_msg/package.xml deleted file mode 100644 index 4b90301..0000000 --- a/wibotic_msg/package.xml +++ /dev/null @@ -1,19 +0,0 @@ - - - wibotic_msg - 0.0.1 - The wibotic_msg package - WiBotic Inc. - BSD 3-Clause - https://www.wibotic.com - - message_generation - message_runtime - catkin - std_msgs - std_msgs - std_msgs - - - - diff --git a/wibotic_msg/srv/ListParameters.srv b/wibotic_msg/srv/ListParameters.srv deleted file mode 100644 index 4c5e8b8..0000000 --- a/wibotic_msg/srv/ListParameters.srv +++ /dev/null @@ -1,3 +0,0 @@ ---- -uint8 status -string[] parameters diff --git a/wibotic_msg/srv/ReadParameter.srv b/wibotic_msg/srv/ReadParameter.srv deleted file mode 100644 index 428eae2..0000000 --- a/wibotic_msg/srv/ReadParameter.srv +++ /dev/null @@ -1,4 +0,0 @@ -string name ---- -uint8 status -int64 value diff --git a/wibotic_msg/srv/SaveParameters.srv b/wibotic_msg/srv/SaveParameters.srv deleted file mode 100644 index 197691a..0000000 --- a/wibotic_msg/srv/SaveParameters.srv +++ /dev/null @@ -1,2 +0,0 @@ ---- -uint8 status \ No newline at end of file diff --git a/wibotic_msg/srv/WriteParameter.srv b/wibotic_msg/srv/WriteParameter.srv deleted file mode 100644 index c3eccd1..0000000 --- a/wibotic_msg/srv/WriteParameter.srv +++ /dev/null @@ -1,4 +0,0 @@ -string name -int64 value ---- -uint8 status diff --git a/wibotic_msgs/CMakeLists.txt b/wibotic_msgs/CMakeLists.txt new file mode 100644 index 0000000..6132dc0 --- /dev/null +++ b/wibotic_msgs/CMakeLists.txt @@ -0,0 +1,18 @@ +cmake_minimum_required(VERSION 3.10.2) +project(wibotic_msgs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} "msg/WiboticInfo.msg" DEPENDENCIES + std_msgs) + +ament_export_dependencies(rosidl_default_runtime) + +ament_package() diff --git a/wibotic_msgs/msg/WiboticInfo.msg b/wibotic_msgs/msg/WiboticInfo.msg new file mode 100644 index 0000000..24dcc5f --- /dev/null +++ b/wibotic_msgs/msg/WiboticInfo.msg @@ -0,0 +1,10 @@ +std_msgs/Header header +float32 v_mon_batt +float32 i_battery +float32 v_rect +float32 v_mon_charger +float32 t_board +float32 target_i_batt +float32 i_charger +float32 i_single_charger2 +float32 i_single_charger3 diff --git a/wibotic_msgs/package.xml b/wibotic_msgs/package.xml new file mode 100644 index 0000000..5e036db --- /dev/null +++ b/wibotic_msgs/package.xml @@ -0,0 +1,28 @@ + + + + wibotic_msgs + 0.1.0 + Custom messages for Wibotic Charger robot. + Husarion + Apache License 2.0 + + https://husarion.com/ + https://github.com/husarion/panther_msgs/tree/ros2 + https://github.com/husarion/panther_msgs/issues + + Jakub Delicat + + ament_cmake + rosidl_default_generators + + builtin_interfaces + std_msgs + + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + +