From 5dece3d793ee709c6f84ba753b5413cc626b8d79 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 1 Oct 2024 08:09:20 +0000 Subject: [PATCH 01/16] Build Signed-off-by: Jakub Delicat --- wibotic_connector_can/CMakeLists.txt | 78 ++-- wibotic_connector_can/cmake/SuperBuild.cmake | 52 +++ wibotic_connector_can/lib/libcyphal | 1 + .../lib/platform_specific_components | 1 + wibotic_connector_can/package.xml | 2 +- wibotic_connector_can/src/main.cpp | 71 ++++ .../src/wibotic_connector_can/__init__.py | 3 - .../wibotic/20200.WiBoticInfo.uavcan | 17 - .../wibotic/20203.RadioBaseStation.uavcan | 6 - .../dsdl/dsdl_wibotic/IncomingSerial.1.0.dsdl | 7 - .../dsdl_wibotic/IncomingSerial.1.0.uavcan | 7 - .../dsdl/dsdl_wibotic/OutgoingSerial.1.0.dsdl | 11 - .../dsdl_wibotic/OutgoingSerial.1.0.uavcan | 11 - .../dsdl_wibotic/RadioBaseStation.1.0.dsdl | 8 - .../dsdl_wibotic/RadioBaseStation.1.0.uavcan | 8 - .../dsdl/dsdl_wibotic/WiBoticInfo.1.0.dsdl | 31 -- .../dsdl/dsdl_wibotic/WiBoticInfo.1.0.uavcan | 31 -- .../dsdl/wibotic/IncomingSerial.1.0.uavcan | 7 - .../dsdl/wibotic/OutgoingSerial.1.0.uavcan | 11 - .../dsdl/wibotic/RadioBaseStation.1.0.uavcan | 8 - .../dsdl/wibotic/WiBoticInfo.1.0.uavcan | 31 -- .../wibotic/20200.WiBoticInfo.uavcan | 17 - .../wibotic/20201.WiBoticCommand.uavcan | 14 - .../wibotic/20202.WiBoticRead.uavcan | 9 - .../wibotic/20203.RadioBaseStation.uavcan | 6 - .../src/wibotic_connector_can/wiboticros.py | 334 ------------------ 26 files changed, 169 insertions(+), 613 deletions(-) create mode 100644 wibotic_connector_can/cmake/SuperBuild.cmake create mode 160000 wibotic_connector_can/lib/libcyphal create mode 160000 wibotic_connector_can/lib/platform_specific_components create mode 100644 wibotic_connector_can/src/main.cpp delete mode 100755 wibotic_connector_can/src/wibotic_connector_can/__init__.py delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v0/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.dsdl delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/IncomingSerial.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.dsdl delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/OutgoingSerial.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.dsdl delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/RadioBaseStation.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.dsdl delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/dsdl_wibotic/WiBoticInfo.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/IncomingSerial.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/OutgoingSerial.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/RadioBaseStation.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_v1/dsdl/wibotic/WiBoticInfo.1.0.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20201.WiBoticCommand.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20202.WiBoticRead.uavcan delete mode 100644 wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20203.RadioBaseStation.uavcan delete mode 100755 wibotic_connector_can/src/wibotic_connector_can/wiboticros.py diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index 7a6eaae..a3baae6 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -1,50 +1,58 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10.2) project(wibotic_connector_can) - -find_package(catkin REQUIRED COMPONENTS - rospy - std_msgs - message_generation +# Handle superbuild first +# option(USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON) + +# if(USE_SUPERBUILD) +# project(SUPERBUILD NONE) +# include(cmake/SuperBuild.cmake) +# return() +# else() +# project(wibotic_connector_can) +# endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +set(PACKAGE_DEPENDENCIES + ament_cmake + PkgConfig ) -catkin_python_setup() +foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) + find_package(${PACKAGE} REQUIRED) +endforeach() -################################### -## catkin specific configuration ## -################################### +find_library(UAVCAN_LIB uavcan REQUIRED) +set(CMAKE_CXX_FLAGS "-Wall -Wextra -pedantic -Werror -std=c++11") -catkin_package( - LIBRARIES wibotic_connector_can - CATKIN_DEPENDS rospy std_msgs message_runtime wibotic_msg +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/include ) -########### -## Build ## -########### - include_directories( - ${catkin_INCLUDE_DIRS} + ${CMAKE_CURRENT_SOURCE_DIR}/lib/libcyphal/libuavcan/include ) -############# -## Install ## -############# +add_executable(wibotic_connector_can src/main.cpp) -catkin_install_python( - PROGRAMS src/wibotic_connector_can/wiboticros.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +target_include_directories(wibotic_connector_can + BEFORE + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/uavcan_linux ) +target_include_directories(wibotic_connector_can + PUBLIC + $ + $) -install( - DIRECTORY - launch/ - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch +ament_target_dependencies(wibotic_connector_can ${PACKAGE_DEPENDENCIES}) + +target_link_libraries(wibotic_connector_can + ${UAVCAN_LIB} ) -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 +install(TARGETS wibotic_connector_can DESTINATION lib/${PROJECT_NAME}) + + +ament_package() diff --git a/wibotic_connector_can/cmake/SuperBuild.cmake b/wibotic_connector_can/cmake/SuperBuild.cmake new file mode 100644 index 0000000..eea9125 --- /dev/null +++ b/wibotic_connector_can/cmake/SuperBuild.cmake @@ -0,0 +1,52 @@ +# 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 +) + +file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ep_libuavcan/include) +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 + INSTALL_COMMAND make install INSTALL_PREFIX= + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} + ) + +ExternalProject_Add( + ep_platform_specific_components + SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components/upstream + SOURCE_SUBDIR linux/libuavcan + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} + GIT_REPOSITORY https://github.com/OpenCyphal-Garage/platform_specific_components/ + GIT_TAG 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${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/lib/libcyphal b/wibotic_connector_can/lib/libcyphal new file mode 160000 index 0000000..dcc3a4d --- /dev/null +++ b/wibotic_connector_can/lib/libcyphal @@ -0,0 +1 @@ +Subproject commit dcc3a4de237b7482e04543d2393c3a9385685312 diff --git a/wibotic_connector_can/lib/platform_specific_components b/wibotic_connector_can/lib/platform_specific_components new file mode 160000 index 0000000..4745ef5 --- /dev/null +++ b/wibotic_connector_can/lib/platform_specific_components @@ -0,0 +1 @@ +Subproject commit 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index 7636a84..35b8b3f 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -18,7 +18,7 @@ std_msgs python-future uavcan-pip - wibotic_msg + diff --git a/wibotic_connector_can/src/main.cpp b/wibotic_connector_can/src/main.cpp new file mode 100644 index 0000000..8c541f8 --- /dev/null +++ b/wibotic_connector_can/src/main.cpp @@ -0,0 +1,71 @@ +/* + * This program subscribes to airspeed messages using libuavcan, and prints them into stdout in YAML format. + * It can be used to test alternative implementations of the UAVCAN stack against the reference implementation. + * + * GCC invocation command: + * g++ libuavcan_airspeed_subscriber.cpp -std=c++11 -lrt -luavcan + * + * Author: Pavel Kirienko + * License: CC0, no copyright reserved + * Language: C++11 + */ + +#include +#include +#include +#include + +uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) +{ + auto node = uavcan_linux::makeNode(ifaces); + node->setNodeID(nid); + node->setName(name.c_str()); + + if (node->start() < 0) + { + throw std::runtime_error("Failed to start UAVCAN node"); + } + + node->setModeOperational(); + return node; +} + +template +static void printMessage(const uavcan::ReceivedDataStructure& msg) +{ + std::cout << "[" << DataType::getDataTypeFullName() << "]\n" << msg << "\n---" << std::endl; +} + +template +static std::shared_ptr> makePrintingSubscriber(const uavcan_linux::NodePtr& node) +{ + return node->makeSubscriber(&printMessage); +} + +static void runForever(const uavcan_linux::NodePtr& node) +{ + auto sub_true_airspeed = makePrintingSubscriber(node); + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::getInfinite()); + if (res < 0) + { + node->logError("spin", "Error %*", res); + } + } +} + +int main(int argc, const char** argv) +{ + if (argc < 3) + { + std::cout << "Usage:\n\t" << argv[0] << " [can-iface-name-N...]" << std::endl; + return 1; + } + const int self_node_id = std::stoi(argv[1]); + std::vector iface_names(argv + 2, argv + argc); + uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.example.airspeed_subscriber"); + runForever(node); + return 0; +} 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/20200.WiBoticInfo.uavcan b/wibotic_connector_can/src/wibotic_connector_can/uavcan_vendor_specific_types/wibotic/20200.WiBoticInfo.uavcan deleted file mode 100644 index b28c712..0000000 --- a/wibotic_connector_can/src/wibotic_connector_can/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_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 From 5c9096586eb95dc17b6a6b5545efaa0283e4d1bf Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 2 Oct 2024 14:21:01 +0000 Subject: [PATCH 02/16] Working example Signed-off-by: Jakub Delicat --- README.md | 7 + wibotic_connector_can/CMakeLists.txt | 20 +- .../uavcan_types/wibotic/WiBoticInfo.hpp | 463 ++++++++++++++++++ .../wibotic_can_driver.hpp | 64 +++ wibotic_connector_can/src/main.cpp | 202 ++++++-- wibotic_connector_can/src/script.cpp | 140 ++++++ 6 files changed, 844 insertions(+), 52 deletions(-) create mode 100644 wibotic_connector_can/include/wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp create mode 100644 wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp create mode 100644 wibotic_connector_can/src/script.cpp diff --git a/README.md b/README.md index 8c4c607..19de2c8 100644 --- a/README.md +++ b/README.md @@ -3,3 +3,10 @@ WiBotic ROS CAN Connector is a set of ROS packages that supports interacting with a WiBotic Onboard Charger over a CAN bus. 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. + +## Add can interface + +```bash +sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyACM0 +sudo ip link set up can0 type can bitrate 500000 +``` diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index a3baae6..dd3fdca 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -49,10 +49,26 @@ target_include_directories(wibotic_connector_can ament_target_dependencies(wibotic_connector_can ${PACKAGE_DEPENDENCIES}) target_link_libraries(wibotic_connector_can - ${UAVCAN_LIB} + ${UAVCAN_LIB} rt ) -install(TARGETS wibotic_connector_can DESTINATION lib/${PROJECT_NAME}) +add_executable(script src/script.cpp) +target_include_directories(script + BEFORE + PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/uavcan_linux +) +target_include_directories(script + PUBLIC + $ + $) + +ament_target_dependencies(script ${PACKAGE_DEPENDENCIES}) + +target_link_libraries(script + ${UAVCAN_LIB} rt +) + +install(TARGETS script wibotic_connector_can DESTINATION lib/${PROJECT_NAME}) ament_package() 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..6242322 --- /dev/null +++ b/wibotic_connector_can/include/wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp @@ -0,0 +1,463 @@ +/* + * UAVCAN data structure definition for libuavcan. + * + * Autogenerated, do not edit. + * + * Source file: /home/husarion/ros2_ws/src/wibotic/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< typename FieldTypes::VMonBatt >::Type VMonBatt; + typename ::uavcan::StorageType< typename FieldTypes::IBattery >::Type IBattery; + typename ::uavcan::StorageType< typename FieldTypes::VRect >::Type VRect; + typename ::uavcan::StorageType< typename FieldTypes::VMonCharger >::Type VMonCharger; + typename ::uavcan::StorageType< typename FieldTypes::TBoard >::Type TBoard; + typename ::uavcan::StorageType< typename FieldTypes::TargetIBatt >::Type TargetIBatt; + typename ::uavcan::StorageType< typename FieldTypes::ICharger >::Type ICharger; + typename ::uavcan::StorageType< typename FieldTypes::ISingleCharger2 >::Type ISingleCharger2; + typename ::uavcan::StorageType< typename FieldTypes::ISingleCharger3 >::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 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 \ No newline at end of file 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..55b9276 --- /dev/null +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp @@ -0,0 +1,64 @@ +// 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_CONNECTOR_CAN_HPP_ +#define WIBOTIC_CONNECTOR_CAN_WIBOTIC_CONNECTOR_CAN_HPP_ + +#include + +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; + + virtual void Activate() = 0; + virtual void Deactivate() = 0; + + virtual void CreateUavCanNode() = 0; + virtual void DestroyUavCanNode() = 0; + + virtual void CreateWiboticInfoSubscriber() = 0; + + virtual void Spin(std::size_t microseconds) = 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; + +protected: + +}; + + + + + +} // namespace wibotic_connector_can + +#endif // WIBOTIC_CONNECTOR_CAN_WIBOTIC_CONNECTOR_CAN_HPP_ diff --git a/wibotic_connector_can/src/main.cpp b/wibotic_connector_can/src/main.cpp index 8c541f8..ef52382 100644 --- a/wibotic_connector_can/src/main.cpp +++ b/wibotic_connector_can/src/main.cpp @@ -1,71 +1,173 @@ -/* - * This program subscribes to airspeed messages using libuavcan, and prints them into stdout in YAML format. - * It can be used to test alternative implementations of the UAVCAN stack against the reference implementation. - * - * GCC invocation command: - * g++ libuavcan_airspeed_subscriber.cpp -std=c++11 -lrt -luavcan - * - * Author: Pavel Kirienko - * License: CC0, no copyright reserved - * Language: C++11 - */ +// 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 + #include #include +#include -uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) +#include "wibotic_connector_can/wibotic_can_driver.hpp" +#include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" + +class WiboticCanDriver : public wibotic_connector_can::WiboticCanDriverInterface { - auto node = uavcan_linux::makeNode(ifaces); - node->setNodeID(nid); - node->setName(name.c_str()); + typedef uavcan::MethodBinder + WiBoticInfoCallbackBinder; + +public: + WiboticCanDriver(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) + { + } + + virtual void CreateUavCanNode() + { + uavcan_node_ = uavcan_linux::makeNode({ can_iface_name_ }); + uavcan_node_->setNodeID(node_id_); + uavcan_node_->setName(node_name_.c_str()); - if (node->start() < 0) + uavcan::protocol::SoftwareVersion sw_version; + sw_version.major = 1; + uavcan_node_->setSoftwareVersion(sw_version); + + if (uavcan_node_->start() < 0) { - throw std::runtime_error("Failed to start UAVCAN node"); + throw std::runtime_error("Failed to start UAVCAN node"); } + } - node->setModeOperational(); - return node; -} + void Activate() override + { + if (!uavcan_node_) + { + throw std::runtime_error("Trying to activate nonexisting node."); + } -template -static void printMessage(const uavcan::ReceivedDataStructure& msg) -{ - std::cout << "[" << DataType::getDataTypeFullName() << "]\n" << msg << "\n---" << std::endl; -} + 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)); + } -template -static std::shared_ptr> makePrintingSubscriber(const uavcan_linux::NodePtr& node) -{ - return node->makeSubscriber(&printMessage); -} + 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)); + } -static void runForever(const uavcan_linux::NodePtr& node) -{ - auto sub_true_airspeed = makePrintingSubscriber(node); + uavcan_node_->setModeOperational(); + + activated_ = true; + } + + virtual void Deactivate() + { + DestroyUavCanNode(); + activated_ = false; + } - while (true) + virtual void DestroyUavCanNode() + { + if (!uavcan_node_) { - const int res = node->spin(uavcan::MonotonicDuration::getInfinite()); - if (res < 0) - { - node->logError("spin", "Error %*", res); - } + throw std::runtime_error("Trying to destroy nonexisting node."); } -} + uavcan_node_.reset(); + } + + virtual void CreateWiboticInfoSubscriber() + { + wibotic_info_uavcan_sub_ = std::make_shared>(*uavcan_node_); + } -int main(int argc, const char** argv) + virtual void Spin(std::size_t miliseconds) + { + 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(miliseconds)); + if (res < 0) + { + throw std::runtime_error("Failed to spin UAVCAN node, res: " + std::to_string(res)); + } + } + + wibotic::WiBoticInfo 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; + } + +protected: + 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_; + + void WiboticInfoCallback(const wibotic::WiBoticInfo& msg) + { + wibotic_info_queue_.push(msg); + } +}; + +int main() { - if (argc < 3) + std::cout << "Starting Wibotic CAN driver" << std::endl; + WiboticCanDriver wibotic_can_driver("can0", 20, "com.wibotic.ros_connector"); + wibotic_can_driver.CreateUavCanNode(); + wibotic_can_driver.CreateWiboticInfoSubscriber(); + wibotic_can_driver.Activate(); + + while (true) + { + wibotic_can_driver.Spin(10); + try + { + std::cout << wibotic_can_driver.GetWiboticInfo() << std::endl; + } + catch (const std::runtime_error& e) { - std::cout << "Usage:\n\t" << argv[0] << " [can-iface-name-N...]" << std::endl; - return 1; + std::cerr << e.what() << '\n'; } - const int self_node_id = std::stoi(argv[1]); - std::vector iface_names(argv + 2, argv + argc); - uavcan_linux::NodePtr node = initNode(iface_names, self_node_id, "org.uavcan.example.airspeed_subscriber"); - runForever(node); - return 0; + } + return 0; } diff --git a/wibotic_connector_can/src/script.cpp b/wibotic_connector_can/src/script.cpp new file mode 100644 index 0000000..4bed11d --- /dev/null +++ b/wibotic_connector_can/src/script.cpp @@ -0,0 +1,140 @@ +// 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 +#include +#include + +#include "wibotic_connector_can/wibotic_can_driver.hpp" +#include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" + +#include + +uavcan::ISystemClock& getSystemClock() +{ + static uavcan_linux::SystemClock clock; + return clock; +} + +uavcan::ICanDriver& getCanDriver() +{ + static uavcan_linux::SocketCanDriver driver(dynamic_cast(getSystemClock())); + if (driver.getNumIfaces() == 0) // Will be executed once + { + if (driver.addIface("can0") < 0) + { + throw std::runtime_error("Failed to add iface"); + } + } + return driver; +} + + +// constexpr unsigned NodeMemoryPoolSize = 16384; +// typedef uavcan::Node Node; + + +// static Node& getNode() +// { +// static Node node(getCanDriver(), getSystemClock()); +// return node; +// } +/** + * This class demonstrates how to use uavcan::MethodBinder with subscriber objects in C++03. + * In C++11 and newer standards it is recommended to use lambdas and std::function<> instead, as this approach + * would be much easier to implement and to understand. + */ +class Node +{ + static const unsigned NodeMemoryPoolSize = 16384; + + uavcan::Node node_; + + /* + * Instantiations of uavcan::MethodBinder<> + */ + typedef uavcan::MethodBinder + LogMessageCallbackBinder; + + + + uavcan::Subscriber log_sub_; + + + void logMessageCallback(const wibotic::WiBoticInfo& msg) const + { + std::cout << "Log message:\n" << msg << std::endl; + } + + + +public: + Node(uavcan::NodeID self_node_id, const std::string& self_node_name) : + node_(getCanDriver(), getSystemClock()), + log_sub_(node_) + { + node_.setNodeID(self_node_id); + node_.setName(self_node_name.c_str()); + } + + void run() + { + const int start_res = node_.start(); + if (start_res < 0) + { std::cout << "Starting Wibotic CAN driver" << std::endl; + + throw std::runtime_error("Failed to start the node: " + std::to_string(start_res)); + } + + const int log_sub_start_res = log_sub_.start(LogMessageCallbackBinder(this, &Node::logMessageCallback)); + if (log_sub_start_res < 0) + { + throw std::runtime_error("Failed to start the log subscriber; error: " + std::to_string(log_sub_start_res)); + } + node_.setModeOperational(); + + while (true) + { + const int res = node_.spin(uavcan::MonotonicDuration::fromMSec(1000)); + if (res < 0) + { + std::cerr << "Transient failure: " << res << std::endl; + } + + std::cout << "spinnging... "<< std::endl; + + } + } +}; + +int main(int argc, const char** argv) +{ + if (argc < 2) + { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + return 1; + } + + std::cout << "Starting Wibotic CAN driver SScript not class" << std::endl; + + const int self_node_id = std::stoi(argv[1]); + + Node node(self_node_id, "org.uavcan.tutorial.subscriber_cpp03"); + + node.run(); +} From ddd1af9bf5fe5ccb80c00ff6574f17b4082dfe55 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 2 Oct 2024 14:45:16 +0000 Subject: [PATCH 03/16] Added docs Signed-off-by: Jakub Delicat --- wibotic_connector_can/CMakeLists.txt | 18 +-- .../wibotic_can_driver.hpp | 140 +++++++++++++++++- wibotic_connector_can/src/main.cpp | 126 +--------------- wibotic_connector_can/src/script.cpp | 140 ------------------ .../src/wibotic_can_driver.cpp | 127 ++++++++++++++++ .../src/wibotic_can_driver_node.cpp | 15 ++ 6 files changed, 279 insertions(+), 287 deletions(-) delete mode 100644 wibotic_connector_can/src/script.cpp create mode 100644 wibotic_connector_can/src/wibotic_can_driver.cpp create mode 100644 wibotic_connector_can/src/wibotic_can_driver_node.cpp diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index dd3fdca..d52c996 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -35,7 +35,7 @@ include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/lib/libcyphal/libuavcan/include ) -add_executable(wibotic_connector_can src/main.cpp) +add_executable(wibotic_connector_can src/main.cpp src/wibotic_can_driver.cpp) target_include_directories(wibotic_connector_can BEFORE @@ -52,22 +52,6 @@ target_link_libraries(wibotic_connector_can ${UAVCAN_LIB} rt ) -add_executable(script src/script.cpp) -target_include_directories(script - BEFORE - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/uavcan_linux -) -target_include_directories(script - PUBLIC - $ - $) - -ament_target_dependencies(script ${PACKAGE_DEPENDENCIES}) - -target_link_libraries(script - ${UAVCAN_LIB} rt -) - install(TARGETS script wibotic_connector_can DESTINATION lib/${PROJECT_NAME}) 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 index 55b9276..3dc1a73 100644 --- a/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp @@ -16,6 +16,12 @@ #define WIBOTIC_CONNECTOR_CAN_WIBOTIC_CONNECTOR_CAN_HPP_ #include +#include + +#include +#include + +#include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" namespace wibotic_connector_can { @@ -31,16 +37,51 @@ class WiboticCanDriverInterface */ virtual ~WiboticCanDriverInterface() = default; - virtual void Activate() = 0; - virtual void Deactivate() = 0; - + /** + * @brief Creates the UAVCAN node. + */ virtual void CreateUavCanNode() = 0; + + /** + * @brief Destroys the UAVCAN node. + */ virtual void DestroyUavCanNode() = 0; + /** + * @brief Creates the WiboticInfo subscriber. + */ virtual void CreateWiboticInfoSubscriber() = 0; + /** + * @brief Destroys the WiboticInfo subscriber. + */ + virtual void DestroyWiboticInfoSubscriber() = 0; + + /** + * @brief Activates the Wibotic CAN driver. + * + */ + virtual void Activate() = 0; + + /** + * @brief Deactivates the Wibotic CAN driver. + */ + virtual void Deactivate() = 0; + + /** + * @brief Spins the Wibotic CAN driver. + * + * @param microseconds The time to spin in microseconds. + */ virtual void Spin(std::size_t microseconds) = 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. */ @@ -50,14 +91,103 @@ class WiboticCanDriverInterface * @brief Alias for a unique pointer to a WiboticCanDriverInterface object. */ using UniquePtr = std::unique_ptr; +}; -protected: -}; +/** + * @brief Class for the Wibotic CAN driver. + * + * This class inherits from the `WiboticCanDriverInterface` and implements its methods. + * Class communicates with CAN interface and gets WiboticInfo messages. + */ +class WiboticCanDriver : public WiboticCanDriverInterface +{ + typedef uavcan::MethodBinder + WiBoticInfoCallbackBinder; + +public: + /** + * @brief Constructor for the WiboticCanDriver class. + * + * @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. + */ + WiboticCanDriver(const std::string& can_iface_name, std::size_t node_id, const std::string& node_name); + + /** + * @brief Creates the UAVCAN node. + */ + void CreateUavCanNode() override; + + /** + * @brief Destroys the UAVCAN node. + * + * @exception Thrown if the UAVCAN node does not exist. + */ + void DestroyUavCanNode() override; + + void CreateWiboticInfoSubscriber() override; + void DestroyWiboticInfoSubscriber() 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 Deactivates the Wibotic CAN driver. + * + * It stops the UAVCAN node and the Wibotic subscriber. + * + * @exception std::runtime_error Thrown if not activated before. + */ + void Deactivate() override; + /** + * @brief Spins the Wibotic CAN driver. + * + * @param microseconds The time to spin in microseconds. + * + * @exception std::runtime_error Thrown if the node or subscriber does not spin properly. + */ + void Spin(std::size_t microseconds) 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: + 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_; + + /** + * @brief Callback for the WiboticInfo message. + * + * Adds uavcan messages to the queue. + * + * @param msg The WiboticInfo message. + */ + void WiboticInfoCallback(const wibotic::WiBoticInfo& msg); +}; } // namespace wibotic_connector_can diff --git a/wibotic_connector_can/src/main.cpp b/wibotic_connector_can/src/main.cpp index ef52382..4f39a2e 100644 --- a/wibotic_connector_can/src/main.cpp +++ b/wibotic_connector_can/src/main.cpp @@ -18,141 +18,17 @@ #include #include -#include #include #include "wibotic_connector_can/wibotic_can_driver.hpp" #include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" -class WiboticCanDriver : public wibotic_connector_can::WiboticCanDriverInterface -{ - typedef uavcan::MethodBinder - WiBoticInfoCallbackBinder; - -public: - WiboticCanDriver(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) - { - } - - virtual void CreateUavCanNode() - { - uavcan_node_ = uavcan_linux::makeNode({ can_iface_name_ }); - uavcan_node_->setNodeID(node_id_); - uavcan_node_->setName(node_name_.c_str()); - - uavcan::protocol::SoftwareVersion sw_version; - sw_version.major = 1; - uavcan_node_->setSoftwareVersion(sw_version); - - if (uavcan_node_->start() < 0) - { - throw std::runtime_error("Failed to start UAVCAN node"); - } - } - - void Activate() override - { - 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; - } - - virtual void Deactivate() - { - DestroyUavCanNode(); - activated_ = false; - } - - virtual void DestroyUavCanNode() - { - if (!uavcan_node_) - { - throw std::runtime_error("Trying to destroy nonexisting node."); - } - uavcan_node_.reset(); - } - - virtual void CreateWiboticInfoSubscriber() - { - wibotic_info_uavcan_sub_ = std::make_shared>(*uavcan_node_); - } - - virtual void Spin(std::size_t miliseconds) - { - 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(miliseconds)); - if (res < 0) - { - throw std::runtime_error("Failed to spin UAVCAN node, res: " + std::to_string(res)); - } - } - - wibotic::WiBoticInfo 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; - } - -protected: - 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_; - - void WiboticInfoCallback(const wibotic::WiBoticInfo& msg) - { - wibotic_info_queue_.push(msg); - } -}; int main() { std::cout << "Starting Wibotic CAN driver" << std::endl; - WiboticCanDriver wibotic_can_driver("can0", 20, "com.wibotic.ros_connector"); + wibotic_connector_can::WiboticCanDriver wibotic_can_driver("can0", 20, "com.wibotic.ros_connector"); wibotic_can_driver.CreateUavCanNode(); wibotic_can_driver.CreateWiboticInfoSubscriber(); wibotic_can_driver.Activate(); diff --git a/wibotic_connector_can/src/script.cpp b/wibotic_connector_can/src/script.cpp deleted file mode 100644 index 4bed11d..0000000 --- a/wibotic_connector_can/src/script.cpp +++ /dev/null @@ -1,140 +0,0 @@ -// 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 -#include -#include - -#include "wibotic_connector_can/wibotic_can_driver.hpp" -#include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" - -#include - -uavcan::ISystemClock& getSystemClock() -{ - static uavcan_linux::SystemClock clock; - return clock; -} - -uavcan::ICanDriver& getCanDriver() -{ - static uavcan_linux::SocketCanDriver driver(dynamic_cast(getSystemClock())); - if (driver.getNumIfaces() == 0) // Will be executed once - { - if (driver.addIface("can0") < 0) - { - throw std::runtime_error("Failed to add iface"); - } - } - return driver; -} - - -// constexpr unsigned NodeMemoryPoolSize = 16384; -// typedef uavcan::Node Node; - - -// static Node& getNode() -// { -// static Node node(getCanDriver(), getSystemClock()); -// return node; -// } -/** - * This class demonstrates how to use uavcan::MethodBinder with subscriber objects in C++03. - * In C++11 and newer standards it is recommended to use lambdas and std::function<> instead, as this approach - * would be much easier to implement and to understand. - */ -class Node -{ - static const unsigned NodeMemoryPoolSize = 16384; - - uavcan::Node node_; - - /* - * Instantiations of uavcan::MethodBinder<> - */ - typedef uavcan::MethodBinder - LogMessageCallbackBinder; - - - - uavcan::Subscriber log_sub_; - - - void logMessageCallback(const wibotic::WiBoticInfo& msg) const - { - std::cout << "Log message:\n" << msg << std::endl; - } - - - -public: - Node(uavcan::NodeID self_node_id, const std::string& self_node_name) : - node_(getCanDriver(), getSystemClock()), - log_sub_(node_) - { - node_.setNodeID(self_node_id); - node_.setName(self_node_name.c_str()); - } - - void run() - { - const int start_res = node_.start(); - if (start_res < 0) - { std::cout << "Starting Wibotic CAN driver" << std::endl; - - throw std::runtime_error("Failed to start the node: " + std::to_string(start_res)); - } - - const int log_sub_start_res = log_sub_.start(LogMessageCallbackBinder(this, &Node::logMessageCallback)); - if (log_sub_start_res < 0) - { - throw std::runtime_error("Failed to start the log subscriber; error: " + std::to_string(log_sub_start_res)); - } - node_.setModeOperational(); - - while (true) - { - const int res = node_.spin(uavcan::MonotonicDuration::fromMSec(1000)); - if (res < 0) - { - std::cerr << "Transient failure: " << res << std::endl; - } - - std::cout << "spinnging... "<< std::endl; - - } - } -}; - -int main(int argc, const char** argv) -{ - if (argc < 2) - { - std::cerr << "Usage: " << argv[0] << " " << std::endl; - return 1; - } - - std::cout << "Starting Wibotic CAN driver SScript not class" << std::endl; - - const int self_node_id = std::stoi(argv[1]); - - Node node(self_node_id, "org.uavcan.tutorial.subscriber_cpp03"); - - node.run(); -} 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..08790ff --- /dev/null +++ b/wibotic_connector_can/src/wibotic_can_driver.cpp @@ -0,0 +1,127 @@ +// 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 +{ +WiboticCanDriver::WiboticCanDriver(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::DestroyUavCanNode() +{ + if (!uavcan_node_) + { + throw std::runtime_error("Trying to destroy nonexisting node."); + } + uavcan_node_.reset(); +} + +void WiboticCanDriver::CreateWiboticInfoSubscriber() +{ + wibotic_info_uavcan_sub_ = std::make_shared>(*uavcan_node_); +} + +void WiboticCanDriver::DestroyWiboticInfoSubscriber() +{ + if (!wibotic_info_uavcan_sub_) + { + throw std::runtime_error("Trying to destroy nonexisting subscriber."); + } + wibotic_info_uavcan_sub_.reset(); +} + +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::Deactivate() +{ + DestroyUavCanNode(); + DestroyWiboticInfoSubscriber(); + activated_ = false; +} + +void WiboticCanDriver::Spin(std::size_t miliseconds) +{ + 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(miliseconds)); + 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..6e6d2b5 --- /dev/null +++ b/wibotic_connector_can/src/wibotic_can_driver_node.cpp @@ -0,0 +1,15 @@ +// 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 From f30409befcde1c19e85003754eae93888b0f6847 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 2 Oct 2024 16:47:40 +0000 Subject: [PATCH 04/16] Added node Signed-off-by: Jakub Delicat --- wibotic_connector_can/CMakeLists.txt | 60 ++++++++-------- .../wibotic_can_driver.hpp | 56 ++++----------- .../wibotic_can_driver_node.hpp | 53 +++++++++++++++ wibotic_connector_can/package.xml | 36 +++++----- wibotic_connector_can/src/main.cpp | 39 ++++------- .../src/wibotic_can_driver.cpp | 25 ------- .../src/wibotic_can_driver_node.cpp | 68 ++++++++++++++++++- 7 files changed, 192 insertions(+), 145 deletions(-) create mode 100644 wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver_node.hpp diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index d52c996..babbb7d 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -1,58 +1,58 @@ cmake_minimum_required(VERSION 3.10.2) project(wibotic_connector_can) -# Handle superbuild first -# option(USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON) +# Handle superbuild first option(USE_SUPERBUILD "Whether or not a superbuild +# should be invoked" ON) -# if(USE_SUPERBUILD) -# project(SUPERBUILD NONE) -# include(cmake/SuperBuild.cmake) -# return() -# else() -# project(wibotic_connector_can) -# endif() +# if(USE_SUPERBUILD) project(SUPERBUILD NONE) include(cmake/SuperBuild.cmake) +# return() else() project(wibotic_connector_can) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_DEPENDENCIES - ament_cmake - PkgConfig -) +set(PACKAGE_DEPENDENCIES ament_cmake rclcpp) +set(CMAKE_CXX_STANDARD_REQUIRED ON) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) endforeach() find_library(UAVCAN_LIB uavcan REQUIRED) -set(CMAKE_CXX_FLAGS "-Wall -Wextra -pedantic -Werror -std=c++11") include_directories( ${CMAKE_CURRENT_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/include ) -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR}/lib/libcyphal/libuavcan/include +include_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib/libcyphal/libuavcan/include) + +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_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/uavcan_linux ) +target_include_directories( + wibotic_can_driver + PUBLIC $ + $) -add_executable(wibotic_connector_can src/main.cpp src/wibotic_can_driver.cpp) +ament_target_dependencies(wibotic_can_driver ${PACKAGE_DEPENDENCIES}) -target_include_directories(wibotic_connector_can - BEFORE - PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/uavcan_linux -) -target_include_directories(wibotic_connector_can - PUBLIC - $ - $) +target_link_libraries(wibotic_can_driver ${UAVCAN_LIB} rt) -ament_target_dependencies(wibotic_connector_can ${PACKAGE_DEPENDENCIES}) +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 $ + $) -target_link_libraries(wibotic_connector_can - ${UAVCAN_LIB} rt -) +ament_target_dependencies(wibotic_connector_can ${PACKAGE_DEPENDENCIES}) -install(TARGETS script wibotic_connector_can DESTINATION lib/${PROJECT_NAME}) +target_link_libraries(wibotic_connector_can wibotic_can_driver) +install(TARGETS wibotic_connector_can wibotic_can_driver + DESTINATION lib/${PROJECT_NAME}) ament_package() 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 index 3dc1a73..673649a 100644 --- a/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef WIBOTIC_CONNECTOR_CAN_WIBOTIC_CONNECTOR_CAN_HPP_ -#define WIBOTIC_CONNECTOR_CAN_WIBOTIC_CONNECTOR_CAN_HPP_ +#ifndef WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_HPP_ +#define WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_HPP_ #include #include @@ -42,32 +42,17 @@ class WiboticCanDriverInterface */ virtual void CreateUavCanNode() = 0; - /** - * @brief Destroys the UAVCAN node. - */ - virtual void DestroyUavCanNode() = 0; - /** * @brief Creates the WiboticInfo subscriber. */ virtual void CreateWiboticInfoSubscriber() = 0; - /** - * @brief Destroys the WiboticInfo subscriber. - */ - virtual void DestroyWiboticInfoSubscriber() = 0; - /** * @brief Activates the Wibotic CAN driver. * */ virtual void Activate() = 0; - /** - * @brief Deactivates the Wibotic CAN driver. - */ - virtual void Deactivate() = 0; - /** * @brief Spins the Wibotic CAN driver. * @@ -93,7 +78,6 @@ class WiboticCanDriverInterface using UniquePtr = std::unique_ptr; }; - /** * @brief Class for the Wibotic CAN driver. * @@ -122,15 +106,8 @@ class WiboticCanDriver : public WiboticCanDriverInterface */ void CreateUavCanNode() override; - /** - * @brief Destroys the UAVCAN node. - * - * @exception Thrown if the UAVCAN node does not exist. - */ - void DestroyUavCanNode() override; void CreateWiboticInfoSubscriber() override; - void DestroyWiboticInfoSubscriber() override; /** * @brief Activates the Wibotic CAN driver. @@ -141,15 +118,6 @@ class WiboticCanDriver : public WiboticCanDriverInterface */ void Activate() override; - /** - * @brief Deactivates the Wibotic CAN driver. - * - * It stops the UAVCAN node and the Wibotic subscriber. - * - * @exception std::runtime_error Thrown if not activated before. - */ - void Deactivate() override; - /** * @brief Spins the Wibotic CAN driver. * @@ -169,6 +137,15 @@ class WiboticCanDriver : public WiboticCanDriverInterface 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_; @@ -178,17 +155,8 @@ class WiboticCanDriver : public WiboticCanDriverInterface std::shared_ptr> wibotic_info_uavcan_sub_; std::queue wibotic_info_queue_; - - /** - * @brief Callback for the WiboticInfo message. - * - * Adds uavcan messages to the queue. - * - * @param msg The WiboticInfo message. - */ - void WiboticInfoCallback(const wibotic::WiBoticInfo& msg); }; } // namespace wibotic_connector_can -#endif // WIBOTIC_CONNECTOR_CAN_WIBOTIC_CONNECTOR_CAN_HPP_ +#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..aa2dea8 --- /dev/null +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver_node.hpp @@ -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. + +#ifndef WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_NODE_HPP_ +#define WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_NODE_HPP_ + +#include + +#include + +#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, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +protected: + void DeclareParameters(); + void GetParameters(); + + void CreateUavCanNode(); + + std::string can_iface_name_; + std::size_t uavcan_node_id_; + std::string uavcan_node_name_; + float update_time_s_; + + std::unique_ptr wibotic_can_driver_; + + rclcpp::TimerBase::SharedPtr wibotic_info_timer_; + + void WiboticInfoTimerCallback(); +}; + +} // namespace wibotic_connector_can + +#endif // WIBOTIC_CONNECTOR_CAN_WIBOTIC_CAN_DRIVER_NODE_HPP_ diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index 35b8b3f..d6aa413 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -1,25 +1,21 @@ - + + wibotic_connector_can - 0.0.1 - The wibotic_connector_can package - WiBotic Inc. - BSD 3-Clause - https://www.wibotic.com + 2.1.0 + Integration Panther with Wibotic charger - message_generation - message_runtime - catkin - rospy - std_msgs - rospy - std_msgs - rospy - std_msgs - python-future - uavcan-pip - + Husarion + Apache License 2.0 - - + https://husarion.com/ + https://github.com/husarion/panther_ros + https://github.com/husarion/panther_ros/issues + + Jakub Delicat + + rclcpp + + ament_cmake + ament_cmake_gtest diff --git a/wibotic_connector_can/src/main.cpp b/wibotic_connector_can/src/main.cpp index 4f39a2e..015983a 100644 --- a/wibotic_connector_can/src/main.cpp +++ b/wibotic_connector_can/src/main.cpp @@ -12,38 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include #include -#include +#include -#include -#include +#include -#include "wibotic_connector_can/wibotic_can_driver.hpp" -#include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" +#include "wibotic_connector_can/wibotic_can_driver_node.hpp" - - -int main() +int main(int argc, char ** argv) { - std::cout << "Starting Wibotic CAN driver" << std::endl; - wibotic_connector_can::WiboticCanDriver wibotic_can_driver("can0", 20, "com.wibotic.ros_connector"); - wibotic_can_driver.CreateUavCanNode(); - wibotic_can_driver.CreateWiboticInfoSubscriber(); - wibotic_can_driver.Activate(); + rclcpp::init(argc, argv); - while (true) - { - wibotic_can_driver.Spin(10); - try - { - std::cout << wibotic_can_driver.GetWiboticInfo() << std::endl; - } - catch (const std::runtime_error& e) - { - std::cerr << e.what() << '\n'; - } + auto wibotic_can_driver_node = std::make_shared("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 index 08790ff..3c7e699 100644 --- a/wibotic_connector_can/src/wibotic_can_driver.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver.cpp @@ -28,29 +28,11 @@ void WiboticCanDriver::CreateUavCanNode() uavcan_node_->setName(node_name_.c_str()); } -void WiboticCanDriver::DestroyUavCanNode() -{ - if (!uavcan_node_) - { - throw std::runtime_error("Trying to destroy nonexisting node."); - } - uavcan_node_.reset(); -} - void WiboticCanDriver::CreateWiboticInfoSubscriber() { wibotic_info_uavcan_sub_ = std::make_shared>(*uavcan_node_); } -void WiboticCanDriver::DestroyWiboticInfoSubscriber() -{ - if (!wibotic_info_uavcan_sub_) - { - throw std::runtime_error("Trying to destroy nonexisting subscriber."); - } - wibotic_info_uavcan_sub_.reset(); -} - void WiboticCanDriver::Activate() { if (!uavcan_node_) @@ -76,13 +58,6 @@ void WiboticCanDriver::Activate() activated_ = true; } -void WiboticCanDriver::Deactivate() -{ - DestroyUavCanNode(); - DestroyWiboticInfoSubscriber(); - activated_ = false; -} - void WiboticCanDriver::Spin(std::size_t miliseconds) { if (!uavcan_node_) diff --git a/wibotic_connector_can/src/wibotic_can_driver_node.cpp b/wibotic_connector_can/src/wibotic_can_driver_node.cpp index 6e6d2b5..7827937 100644 --- a/wibotic_connector_can/src/wibotic_can_driver_node.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver_node.cpp @@ -12,4 +12,70 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "wibotic_connector_can/wibotic_can_driver_node.hpp" + +namespace wibotic_connector_can +{ +WiboticCanDriverNode::WiboticCanDriverNode(const std::string& node_name, const rclcpp::NodeOptions& options) + : rclcpp::Node(node_name, options) /*, diagnostic_updater_(std::make_shared(this))*/ +{ + RCLCPP_INFO(this->get_logger(), "Constructing node."); + DeclareParameters(); + GetParameters(); + + CreateUavCanNode(); + + wibotic_info_timer_ = this->create_wall_timer(std::chrono::duration(update_time_s_), + std::bind(&WiboticCanDriverNode::WiboticInfoTimerCallback, this)); + + // // diagnostic_updater_->setHardwareID("Battery"); + + RCLCPP_INFO(this->get_logger(), "Node constructed 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", 1.0); +} + +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::CreateUavCanNode() +{ + wibotic_can_driver_ = std::make_unique(can_iface_name_, uavcan_node_id_, uavcan_node_name_); + wibotic_can_driver_->CreateUavCanNode(); + wibotic_can_driver_->CreateWiboticInfoSubscriber(); + wibotic_can_driver_->Activate(); +} + +void WiboticCanDriverNode::WiboticInfoTimerCallback() +{ + if (!wibotic_can_driver_) + { + throw std::runtime_error("Trying to get WiboticInfo message from nonexisting driver."); + } + + try + { + RCLCPP_INFO(this->get_logger(), "Getting WiboticInfo message."); + wibotic_can_driver_->Spin(1000); + auto wibotic_info = wibotic_can_driver_->GetWiboticInfo(); + RCLCPP_INFO_STREAM(this->get_logger(), "Got WiboticInfo message: " << std::endl << wibotic_info); + } + catch (const std::runtime_error& e) + { + RCLCPP_WARN(this->get_logger(), e.what()); + } +} + +} // namespace wibotic_connector_can From 4ee08c79e23fa72ef1715a739219e7db9997ac75 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Thu, 3 Oct 2024 08:20:18 +0000 Subject: [PATCH 05/16] added node Signed-off-by: Jakub Delicat --- wibotic_connector_can/CMakeLists.txt | 3 +- .../wibotic_can_driver.hpp | 8 +-- .../wibotic_can_driver_node.hpp | 7 ++- wibotic_connector_can/package.xml | 1 + wibotic_connector_can/setup.py | 11 ---- .../src/wibotic_can_driver_node.cpp | 33 +++++++++-- wibotic_msg/CMakeLists.txt | 57 +++++-------------- wibotic_msg/msg/WiBoticInfo.msg | 9 --- wibotic_msg/msg/WiboticInfo.msg | 10 ++++ wibotic_msg/package.xml | 35 +++++++----- wibotic_msg/srv/ListParameters.srv | 3 - wibotic_msg/srv/ReadParameter.srv | 4 -- wibotic_msg/srv/SaveParameters.srv | 2 - wibotic_msg/srv/WriteParameter.srv | 4 -- 14 files changed, 86 insertions(+), 101 deletions(-) delete mode 100644 wibotic_connector_can/setup.py delete mode 100644 wibotic_msg/msg/WiBoticInfo.msg create mode 100644 wibotic_msg/msg/WiboticInfo.msg delete mode 100644 wibotic_msg/srv/ListParameters.srv delete mode 100644 wibotic_msg/srv/ReadParameter.srv delete mode 100644 wibotic_msg/srv/SaveParameters.srv delete mode 100644 wibotic_msg/srv/WriteParameter.srv diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index babbb7d..08efaa8 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -10,8 +10,7 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -set(PACKAGE_DEPENDENCIES ament_cmake rclcpp) -set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(PACKAGE_DEPENDENCIES ament_cmake rclcpp wibotic_msgs) foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) 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 index 673649a..da3c8cb 100644 --- a/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp @@ -56,9 +56,9 @@ class WiboticCanDriverInterface /** * @brief Spins the Wibotic CAN driver. * - * @param microseconds The time to spin in microseconds. + * @param miliseconds The time to spin in miliseconds. */ - virtual void Spin(std::size_t microseconds) = 0; + virtual void Spin(std::size_t miliseconds) = 0; /** * @brief Gets the WiboticInfo message. @@ -121,11 +121,11 @@ class WiboticCanDriver : public WiboticCanDriverInterface /** * @brief Spins the Wibotic CAN driver. * - * @param microseconds The time to spin in microseconds. + * @param miliseconds The time to spin in miliseconds. * * @exception std::runtime_error Thrown if the node or subscriber does not spin properly. */ - void Spin(std::size_t microseconds) override; + void Spin(std::size_t miliseconds) override; /** * @brief Gets the WiboticInfo message. 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 index aa2dea8..a34ddc4 100644 --- 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 @@ -19,6 +19,8 @@ #include +#include "wibotic_msgs/msg/wibotic_info.hpp" + #define UAVCAN_CPP_VERSION UAVCAN_CPP11 #include "wibotic_connector_can/wibotic_can_driver.hpp" @@ -34,7 +36,7 @@ class WiboticCanDriverNode : public rclcpp::Node void DeclareParameters(); void GetParameters(); - void CreateUavCanNode(); + void CreateWiboticCanDriver(); std::string can_iface_name_; std::size_t uavcan_node_id_; @@ -44,8 +46,11 @@ class WiboticCanDriverNode : public rclcpp::Node std::unique_ptr wibotic_can_driver_; rclcpp::TimerBase::SharedPtr wibotic_info_timer_; + rclcpp::Publisher::SharedPtr wibotic_info_pub_; void WiboticInfoTimerCallback(); + + wibotic_msgs::msg::WiboticInfo ConvertWiboticInfoToMsg(const wibotic::WiBoticInfo& wibotic_info); }; } // namespace wibotic_connector_can diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index d6aa413..47b6fb2 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -15,6 +15,7 @@ Jakub Delicat rclcpp + wibotic_msgs ament_cmake ament_cmake_gtest 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/wibotic_can_driver_node.cpp b/wibotic_connector_can/src/wibotic_can_driver_node.cpp index 7827937..360a7a1 100644 --- a/wibotic_connector_can/src/wibotic_can_driver_node.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver_node.cpp @@ -23,7 +23,9 @@ WiboticCanDriverNode::WiboticCanDriverNode(const std::string& node_name, const r DeclareParameters(); GetParameters(); - CreateUavCanNode(); + 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)); @@ -33,7 +35,6 @@ WiboticCanDriverNode::WiboticCanDriverNode(const std::string& node_name, const r RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); } - void WiboticCanDriverNode::DeclareParameters() { this->declare_parameter("can_iface_name", "can0"); @@ -50,7 +51,7 @@ void WiboticCanDriverNode::GetParameters() update_time_s_ = this->get_parameter("update_time_s").as_double(); } -void WiboticCanDriverNode::CreateUavCanNode() +void WiboticCanDriverNode::CreateWiboticCanDriver() { wibotic_can_driver_ = std::make_unique(can_iface_name_, uavcan_node_id_, uavcan_node_name_); wibotic_can_driver_->CreateUavCanNode(); @@ -67,10 +68,11 @@ void WiboticCanDriverNode::WiboticInfoTimerCallback() try { - RCLCPP_INFO(this->get_logger(), "Getting WiboticInfo message."); - wibotic_can_driver_->Spin(1000); + const auto update_time_ms = static_cast(update_time_s_ * 1000); + wibotic_can_driver_->Spin(update_time_ms); auto wibotic_info = wibotic_can_driver_->GetWiboticInfo(); - RCLCPP_INFO_STREAM(this->get_logger(), "Got WiboticInfo message: " << std::endl << wibotic_info); + + wibotic_info_pub_->publish(ConvertWiboticInfoToMsg(wibotic_info)); } catch (const std::runtime_error& e) { @@ -78,4 +80,23 @@ void WiboticCanDriverNode::WiboticInfoTimerCallback() } } +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_msg/CMakeLists.txt b/wibotic_msg/CMakeLists.txt index 0f31590..ca1dbe9 100644 --- a/wibotic_msg/CMakeLists.txt +++ b/wibotic_msg/CMakeLists.txt @@ -1,48 +1,21 @@ -cmake_minimum_required(VERSION 2.8.3) -project(wibotic_msg) +cmake_minimum_required(VERSION 3.10.2) +project(wibotic_msgs) -find_package(catkin REQUIRED COMPONENTS - std_msgs - message_generation -) +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() -################################################ -## Declare ROS messages, services and actions ## -################################################ +find_package(ament_cmake REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) -## 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( +rosidl_generate_interfaces( + ${PROJECT_NAME} + "msg/WiboticInfo.msg" DEPENDENCIES - std_msgs -) - -################################### -## catkin specific configuration ## -################################### -catkin_package( - CATKIN_DEPENDS std_msgs message_runtime -) - -########### -## Build ## -########### + std_msgs) -include_directories( - ${catkin_INCLUDE_DIRS} -) +ament_export_dependencies(rosidl_default_runtime) +ament_package() 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/msg/WiboticInfo.msg b/wibotic_msg/msg/WiboticInfo.msg new file mode 100644 index 0000000..24dcc5f --- /dev/null +++ b/wibotic_msg/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_msg/package.xml b/wibotic_msg/package.xml index 4b90301..c8b444a 100644 --- a/wibotic_msg/package.xml +++ b/wibotic_msg/package.xml @@ -1,19 +1,28 @@ - - wibotic_msg - 0.0.1 - The wibotic_msg package - WiBotic Inc. - BSD 3-Clause - https://www.wibotic.com + + + wibotic_msgs + 2.0.4 + Custom messages for Wibotic Charger robot. + Husarion + Apache License 2.0 - message_generation - message_runtime - catkin - std_msgs - std_msgs - std_msgs + 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 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 From 136eb96b4ca2a5ce651474d8b0e3a37e9558cb87 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 08:13:59 +0000 Subject: [PATCH 06/16] Added precommit | Added integration tests Signed-off-by: Jakub Delicat --- .clang-format | 21 ++++ .gitignore | 1 + .pre-commit-config.yaml | 99 +++++++++++++++++++ wibotic_connector_can/CMakeLists.txt | 17 +++- wibotic_connector_can/package.xml | 9 +- .../src/wibotic_can_driver_node.cpp | 32 +++--- .../integration/wibotic_connector_can.test.py | 68 +++++++++++++ 7 files changed, 227 insertions(+), 20 deletions(-) create mode 100644 .clang-format create mode 100644 .gitignore create mode 100644 .pre-commit-config.yaml create mode 100644 wibotic_connector_can/test/integration/wibotic_connector_can.test.py 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/.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/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index 08efaa8..383613a 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.10.2) +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) @@ -40,7 +40,8 @@ ament_target_dependencies(wibotic_can_driver ${PACKAGE_DEPENDENCIES}) target_link_libraries(wibotic_can_driver ${UAVCAN_LIB} rt) -add_executable(wibotic_connector_can src/wibotic_can_driver_node.cpp src/main.cpp ) +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 @@ -54,4 +55,16 @@ 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_gmock REQUIRED) + find_package(ros_testing REQUIRED) + + # Integration tests + option(TEST_INTEGRATION "Run integration tests" ON) + if(TEST_INTEGRATION) + add_ros_test(test/integration/wibotic_connector_can.test.py) + endif() + +endif() + ament_package() diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index 47b6fb2..b0f88cc 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -14,9 +14,16 @@ Jakub Delicat + ament_cmake + rclcpp wibotic_msgs - ament_cmake ament_cmake_gtest + google-mock + ros_testing + + + ament_cmake + diff --git a/wibotic_connector_can/src/wibotic_can_driver_node.cpp b/wibotic_connector_can/src/wibotic_can_driver_node.cpp index 360a7a1..cc67c9b 100644 --- a/wibotic_connector_can/src/wibotic_can_driver_node.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver_node.cpp @@ -16,10 +16,11 @@ namespace wibotic_connector_can { -WiboticCanDriverNode::WiboticCanDriverNode(const std::string& node_name, const rclcpp::NodeOptions& options) - : rclcpp::Node(node_name, options) /*, diagnostic_updater_(std::make_shared(this))*/ +WiboticCanDriverNode::WiboticCanDriverNode( + const std::string & node_name, const rclcpp::NodeOptions & options) +: rclcpp::Node(node_name, options) { - RCLCPP_INFO(this->get_logger(), "Constructing node."); + RCLCPP_INFO(this->get_logger(), "Initializing node."); DeclareParameters(); GetParameters(); @@ -27,12 +28,11 @@ WiboticCanDriverNode::WiboticCanDriverNode(const std::string& node_name, const r 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)); + wibotic_info_timer_ = this->create_wall_timer( + std::chrono::duration(update_time_s_), + std::bind(&WiboticCanDriverNode::WiboticInfoTimerCallback, this)); - // // diagnostic_updater_->setHardwareID("Battery"); - - RCLCPP_INFO(this->get_logger(), "Node constructed successfully."); + RCLCPP_INFO(this->get_logger(), "Node initialized successfully."); } void WiboticCanDriverNode::DeclareParameters() @@ -53,7 +53,8 @@ void WiboticCanDriverNode::GetParameters() void WiboticCanDriverNode::CreateWiboticCanDriver() { - wibotic_can_driver_ = std::make_unique(can_iface_name_, uavcan_node_id_, uavcan_node_name_); + wibotic_can_driver_ = std::make_unique( + can_iface_name_, uavcan_node_id_, uavcan_node_name_); wibotic_can_driver_->CreateUavCanNode(); wibotic_can_driver_->CreateWiboticInfoSubscriber(); wibotic_can_driver_->Activate(); @@ -61,26 +62,23 @@ void WiboticCanDriverNode::CreateWiboticCanDriver() void WiboticCanDriverNode::WiboticInfoTimerCallback() { - if (!wibotic_can_driver_) - { + if (!wibotic_can_driver_) { throw std::runtime_error("Trying to get WiboticInfo message from nonexisting driver."); } - try - { + try { const auto update_time_ms = static_cast(update_time_s_ * 1000); wibotic_can_driver_->Spin(update_time_ms); auto wibotic_info = wibotic_can_driver_->GetWiboticInfo(); wibotic_info_pub_->publish(ConvertWiboticInfoToMsg(wibotic_info)); - } - catch (const std::runtime_error& e) - { + } 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 WiboticCanDriverNode::ConvertWiboticInfoToMsg( + const wibotic::WiBoticInfo & wibotic_info) { wibotic_msgs::msg::WiboticInfo wibotic_info_msg; 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..566a0b0 --- /dev/null +++ b/wibotic_connector_can/test/integration/wibotic_connector_can.test.py @@ -0,0 +1,68 @@ +# 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) From b37bb1157205e89fe23db73cf6392a9d7d68ad90 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 08:16:43 +0000 Subject: [PATCH 07/16] Changed Licence | Added CI/CD Signed-off-by: Jakub Delicat --- .github/pull_request_template.md | 14 ++ .github/workflows/pre-commit.yaml | 11 + .github/workflows/protect-default-branch.yaml | 24 ++ .github/workflows/run-unit-tests.yaml | 79 ++++++ LICENSE | 230 +++++++++++++++--- 5 files changed, 329 insertions(+), 29 deletions(-) create mode 100644 .github/pull_request_template.md create mode 100644 .github/workflows/pre-commit.yaml create mode 100644 .github/workflows/protect-default-branch.yaml create mode 100644 .github/workflows/run-unit-tests.yaml 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..ca65b19 --- /dev/null +++ b/.github/workflows/run-unit-tests.yaml @@ -0,0 +1,79 @@ +--- +name: Run panther 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/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. From b176c7d0fc79ed5f40e24a8b03b02fae78becf6e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 11:24:07 +0000 Subject: [PATCH 08/16] Added node unit tests Signed-off-by: Jakub Delicat --- .github/workflows/run-unit-tests.yaml | 2 +- wibotic_connector_can/CMakeLists.txt | 12 ++ .../wibotic_can_driver.hpp | 39 +++-- .../wibotic_can_driver_node.hpp | 18 ++- wibotic_connector_can/package.xml | 1 + wibotic_connector_can/src/main.cpp | 4 +- .../src/wibotic_can_driver.cpp | 46 +++--- .../src/wibotic_can_driver_node.cpp | 25 ++- .../unit/test_wibotic_can_driver_node.cpp | 143 ++++++++++++++++++ 9 files changed, 234 insertions(+), 56 deletions(-) create mode 100644 wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp diff --git a/.github/workflows/run-unit-tests.yaml b/.github/workflows/run-unit-tests.yaml index ca65b19..7f23a7f 100644 --- a/.github/workflows/run-unit-tests.yaml +++ b/.github/workflows/run-unit-tests.yaml @@ -1,5 +1,5 @@ --- -name: Run panther unit tests +name: Run unit tests on: workflow_dispatch: diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index 383613a..41cab95 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -56,9 +56,21 @@ 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) 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 index da3c8cb..0107635 100644 --- a/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp @@ -18,8 +18,8 @@ #include #include -#include #include +#include #include "wibotic_connector_can/uavcan_types/wibotic/WiBoticInfo.hpp" @@ -37,6 +37,16 @@ class WiboticCanDriverInterface */ 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 SetUavCanSettings( + const std::string & can_iface_name, std::size_t node_id, const std::string & node_name) = 0; + /** * @brief Creates the UAVCAN node. */ @@ -56,9 +66,9 @@ class WiboticCanDriverInterface /** * @brief Spins the Wibotic CAN driver. * - * @param miliseconds The time to spin in miliseconds. + * @param milliseconds The time to spin in milliseconds. */ - virtual void Spin(std::size_t miliseconds) = 0; + virtual void Spin(std::size_t milliseconds) = 0; /** * @brief Gets the WiboticInfo message. @@ -86,27 +96,27 @@ class WiboticCanDriverInterface */ class WiboticCanDriver : public WiboticCanDriverInterface { - typedef uavcan::MethodBinder - WiBoticInfoCallbackBinder; + typedef uavcan::MethodBinder< + WiboticCanDriver *, void (WiboticCanDriver::*)(const wibotic::WiBoticInfo &)> + WiBoticInfoCallbackBinder; public: /** - * @brief Constructor for the WiboticCanDriver class. - * * @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. - */ - WiboticCanDriver(const std::string& can_iface_name, std::size_t node_id, const std::string& node_name); + * */ + void SetUavCanSettings( + 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; /** @@ -114,18 +124,19 @@ class WiboticCanDriver : public WiboticCanDriverInterface * * 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. + * @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 miliseconds The time to spin in miliseconds. + * @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 miliseconds) override; + void Spin(std::size_t milliseconds) override; /** * @brief Gets the WiboticInfo message. @@ -144,7 +155,7 @@ class WiboticCanDriver : public WiboticCanDriverInterface * * @param msg The WiboticInfo message. */ - void WiboticInfoCallback(const wibotic::WiBoticInfo& msg); + void WiboticInfoCallback(const wibotic::WiBoticInfo & msg); std::string can_iface_name_; std::size_t node_id_; 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 index a34ddc4..061ca43 100644 --- 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 @@ -21,6 +21,8 @@ #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" @@ -29,28 +31,30 @@ namespace wibotic_connector_can class WiboticCanDriverNode : public rclcpp::Node { public: - WiboticCanDriverNode(const std::string& node_name, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + 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_; - std::unique_ptr wibotic_can_driver_; + WiboticCanDriverInterface::SharedPtr wibotic_can_driver_; rclcpp::TimerBase::SharedPtr wibotic_info_timer_; rclcpp::Publisher::SharedPtr wibotic_info_pub_; - - void WiboticInfoTimerCallback(); - - wibotic_msgs::msg::WiboticInfo ConvertWiboticInfoToMsg(const wibotic::WiBoticInfo& wibotic_info); }; } // namespace wibotic_connector_can diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index b0f88cc..6c6cf57 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -19,6 +19,7 @@ rclcpp wibotic_msgs + ament_cmake_gmock ament_cmake_gtest google-mock ros_testing diff --git a/wibotic_connector_can/src/main.cpp b/wibotic_connector_can/src/main.cpp index 015983a..a09ea0b 100644 --- a/wibotic_connector_can/src/main.cpp +++ b/wibotic_connector_can/src/main.cpp @@ -24,7 +24,9 @@ int main(int argc, char ** argv) { rclcpp::init(argc, argv); - auto wibotic_can_driver_node = std::make_shared("wibotic_can_driver"); + 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); diff --git a/wibotic_connector_can/src/wibotic_can_driver.cpp b/wibotic_connector_can/src/wibotic_can_driver.cpp index 3c7e699..c1cffb4 100644 --- a/wibotic_connector_can/src/wibotic_can_driver.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver.cpp @@ -16,40 +16,41 @@ namespace wibotic_connector_can { -WiboticCanDriver::WiboticCanDriver(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::SetUavCanSettings( + 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_ = 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_); + wibotic_info_uavcan_sub_ = + std::make_shared>(*uavcan_node_); } void WiboticCanDriver::Activate() { - if (!uavcan_node_) - { + 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) - { + 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) - { + if (node_start_res < 0) { throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res)); } @@ -58,34 +59,29 @@ void WiboticCanDriver::Activate() activated_ = true; } -void WiboticCanDriver::Spin(std::size_t miliseconds) +void WiboticCanDriver::Spin(std::size_t milliseconds) { - if (!uavcan_node_) - { + if (!uavcan_node_) { throw std::runtime_error("Trying to spin nonexisting node."); } - if (!wibotic_info_uavcan_sub_) - { + if (!wibotic_info_uavcan_sub_) { throw std::runtime_error("Trying to spin nonexisting subscriber."); } - if (!activated_) - { + if (!activated_) { throw std::runtime_error("Trying to spin non-activated driver."); } - const int res = uavcan_node_->spin(uavcan::MonotonicDuration::fromMSec(miliseconds)); - if (res < 0) - { + 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()) - { + if (wibotic_info_queue_.empty()) { throw std::runtime_error("WiBoticInfo queue is empty."); } @@ -94,7 +90,7 @@ wibotic::WiBoticInfo WiboticCanDriver::GetWiboticInfo() return wibotic_info; } -void WiboticCanDriver::WiboticInfoCallback(const wibotic::WiBoticInfo& msg) +void WiboticCanDriver::WiboticInfoCallback(const wibotic::WiBoticInfo & msg) { wibotic_info_queue_.push(msg); } diff --git a/wibotic_connector_can/src/wibotic_can_driver_node.cpp b/wibotic_connector_can/src/wibotic_can_driver_node.cpp index cc67c9b..37df40c 100644 --- a/wibotic_connector_can/src/wibotic_can_driver_node.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver_node.cpp @@ -17,10 +17,16 @@ namespace wibotic_connector_can { WiboticCanDriverNode::WiboticCanDriverNode( - const std::string & node_name, const rclcpp::NodeOptions & options) -: rclcpp::Node(node_name, options) + 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(); @@ -53,13 +59,19 @@ void WiboticCanDriverNode::GetParameters() void WiboticCanDriverNode::CreateWiboticCanDriver() { - wibotic_can_driver_ = std::make_unique( - can_iface_name_, uavcan_node_id_, uavcan_node_name_); + wibotic_can_driver_->SetUavCanSettings(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_) { @@ -67,10 +79,7 @@ void WiboticCanDriverNode::WiboticInfoTimerCallback() } try { - const auto update_time_ms = static_cast(update_time_s_ * 1000); - wibotic_can_driver_->Spin(update_time_ms); - auto wibotic_info = wibotic_can_driver_->GetWiboticInfo(); - + auto wibotic_info = GetWiboticInfo(); wibotic_info_pub_->publish(ConvertWiboticInfoToMsg(wibotic_info)); } catch (const std::runtime_error & e) { RCLCPP_WARN(this->get_logger(), e.what()); 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..0fb1cfe --- /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, SetUavCanSettings, (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; +} From 8ca2abaeae69d426836ba1c8522b36c085804fc1 Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 11:25:02 +0000 Subject: [PATCH 09/16] Run precommit Signed-off-by: Jakub Delicat --- wibotic_connector_can/cmake/SuperBuild.cmake | 25 +- .../uavcan_types/wibotic/WiBoticInfo.hpp | 599 +++++++++--------- .../launch/wibotic_connector_can.launch | 1 - wibotic_msg/CMakeLists.txt | 7 +- 4 files changed, 298 insertions(+), 334 deletions(-) diff --git a/wibotic_connector_can/cmake/SuperBuild.cmake b/wibotic_connector_can/cmake/SuperBuild.cmake index eea9125..b981e61 100644 --- a/wibotic_connector_can/cmake/SuperBuild.cmake +++ b/wibotic_connector_can/cmake/SuperBuild.cmake @@ -14,10 +14,7 @@ include(ExternalProject) -set(DEPENDENCIES - ep_libuavcan - ep_platform_specific_components -) +set(DEPENDENCIES ep_libuavcan ep_platform_specific_components) file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ep_libuavcan/include) ExternalProject_Add( @@ -28,20 +25,18 @@ ExternalProject_Add( GIT_TAG dcc3a4de237b7482e04543d2393c3a9385685312 PREFIX ${CMAKE_CURRENT_BINARY_DIR}/ep_libuavcan INSTALL_COMMAND make install INSTALL_PREFIX= - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} - ) + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) ExternalProject_Add( ep_platform_specific_components - SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components/upstream - SOURCE_SUBDIR linux/libuavcan - INSTALL_DIR ${CMAKE_INSTALL_PREFIX} - GIT_REPOSITORY https://github.com/OpenCyphal-Garage/platform_specific_components/ - GIT_TAG 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX} - - ) - + SOURCE_DIR + ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components/upstream + SOURCE_SUBDIR linux/libuavcan + INSTALL_DIR ${CMAKE_INSTALL_PREFIX} + GIT_REPOSITORY + https://github.com/OpenCyphal-Garage/platform_specific_components/ + GIT_TAG 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) ExternalProject_Add( ep_wibotic_connector_can 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 index 6242322..f5d8f20 100644 --- 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 @@ -1,3 +1,17 @@ +// 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. * @@ -10,8 +24,8 @@ #define WIBOTIC_WIBOTICINFO_HPP_INCLUDED #include -#include #include +#include /******************************* Source text ********************************** # @@ -62,123 +76,109 @@ 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< typename FieldTypes::VMonBatt >::Type VMonBatt; - typename ::uavcan::StorageType< typename FieldTypes::IBattery >::Type IBattery; - typename ::uavcan::StorageType< typename FieldTypes::VRect >::Type VRect; - typename ::uavcan::StorageType< typename FieldTypes::VMonCharger >::Type VMonCharger; - typename ::uavcan::StorageType< typename FieldTypes::TBoard >::Type TBoard; - typename ::uavcan::StorageType< typename FieldTypes::TargetIBatt >::Type TargetIBatt; - typename ::uavcan::StorageType< typename FieldTypes::ICharger >::Type ICharger; - typename ::uavcan::StorageType< typename FieldTypes::ISingleCharger2 >::Type ISingleCharger2; - typename ::uavcan::StorageType< typename FieldTypes::ISingleCharger3 >::Type ISingleCharger3; - - WiBoticInfo_() - : VMonBatt() - , IBattery() - , VRect() - , VMonCharger() - , TBoard() - , TargetIBatt() - , ICharger() - , ISingleCharger2() - , ISingleCharger3() - { - ::uavcan::StaticAssert<_tmpl == 0>::check(); // Usage check + 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(); + /* + * 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); } + 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; + /** + * 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 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 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 type info + */ + enum { DataTypeKind = ::uavcan::DataTypeKindMessage }; + enum { DefaultDataTypeID = 20200 }; - static void extendDataTypeSignature(::uavcan::DataTypeSignature& signature) - { - signature.extend(getDataTypeSignature()); - } + static const char * getDataTypeFullName() { return "wibotic.WiBoticInfo"; } - static ::uavcan::DataTypeSignature getDataTypeSignature(); + static void extendDataTypeSignature(::uavcan::DataTypeSignature & signature) + { + signature.extend(getDataTypeSignature()); + } + static ::uavcan::DataTypeSignature getDataTypeSignature(); }; /* @@ -188,135 +188,112 @@ struct UAVCAN_EXPORT WiBoticInfo_ 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; + 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); + 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) +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); + (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) +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); + (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; } /* @@ -325,19 +302,19 @@ int WiBoticInfo_<_tmpl>::decode(ReferenceType self, ::uavcan::ScalarCodec& codec 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; + ::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; } /* @@ -352,11 +329,12 @@ typedef WiBoticInfo_<0> WiBoticInfo; namespace { -const ::uavcan::DefaultDataTypeRegistrator< ::wibotic::WiBoticInfo > _uavcan_gdtr_registrator_WiBoticInfo; +const ::uavcan::DefaultDataTypeRegistrator< ::wibotic::WiBoticInfo> + _uavcan_gdtr_registrator_WiBoticInfo; } -} // Namespace wibotic +} // Namespace wibotic /* * YAML streamer specialization @@ -365,99 +343,94 @@ namespace uavcan { template <> -class UAVCAN_EXPORT YamlStreamer< ::wibotic::WiBoticInfo > +class UAVCAN_EXPORT YamlStreamer< ::wibotic::WiBoticInfo> { public: - template - static void stream(Stream& s, ::wibotic::WiBoticInfo::ParameterType obj, const int level); + 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 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); + (void)s; + (void)obj; + (void)level; + if (level > 0) { s << '\n'; - for (int pos = 0; pos < level; pos++) - { - s << " "; + 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); + } + 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) +inline Stream & operator<<(Stream & s, ::wibotic::WiBoticInfo::ParameterType obj) { - ::uavcan::YamlStreamer< ::wibotic::WiBoticInfo >::stream(s, obj, 0); - return s; + ::uavcan::YamlStreamer< ::wibotic::WiBoticInfo>::stream(s, obj, 0); + return s; } -} // Namespace wibotic +} // Namespace wibotic -#endif // WIBOTIC_WIBOTICINFO_HPP_INCLUDED \ No newline at end of file +#endif // WIBOTIC_WIBOTICINFO_HPP_INCLUDED diff --git a/wibotic_connector_can/launch/wibotic_connector_can.launch b/wibotic_connector_can/launch/wibotic_connector_can.launch index edcc5c3..d960b7d 100644 --- a/wibotic_connector_can/launch/wibotic_connector_can.launch +++ b/wibotic_connector_can/launch/wibotic_connector_can.launch @@ -4,4 +4,3 @@ - diff --git a/wibotic_msg/CMakeLists.txt b/wibotic_msg/CMakeLists.txt index ca1dbe9..6132dc0 100644 --- a/wibotic_msg/CMakeLists.txt +++ b/wibotic_msg/CMakeLists.txt @@ -10,11 +10,8 @@ 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) +rosidl_generate_interfaces(${PROJECT_NAME} "msg/WiboticInfo.msg" DEPENDENCIES + std_msgs) ament_export_dependencies(rosidl_default_runtime) From 66bd67b3b1adddbb64459613225c72eab4a8a53d Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 11:42:05 +0000 Subject: [PATCH 10/16] removed launch | added readme --- README.md | 21 ++++++++++++++++--- .../launch/wibotic_connector_can.launch | 6 ------ .../unit/test_wibotic_can_driver_node.cpp | 2 +- 3 files changed, 19 insertions(+), 10 deletions(-) delete mode 100644 wibotic_connector_can/launch/wibotic_connector_can.launch diff --git a/README.md b/README.md index 19de2c8..db63b3e 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,23 @@ -# 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 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. +## 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. ## Add can interface 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 d960b7d..0000000 --- a/wibotic_connector_can/launch/wibotic_connector_can.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - 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 index 0fb1cfe..550b64b 100644 --- a/wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp +++ b/wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp @@ -44,7 +44,7 @@ class MockWiboticCanDriver : public wibotic_connector_can::WiboticCanDriverInter MOCK_METHOD(wibotic::WiBoticInfo, GetWiboticInfo, (), (override)); // Nice mock suppresses warnings about uninteresting calls - // using NiceMock = testing::NiceMock; + using NiceMock = testing::NiceMock; }; class WiboticCanDriverNodeWrapper : public wibotic_connector_can::WiboticCanDriverNode From 408babe298abfa976a348cc7b5a2e4e0f4d2b3be Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 13:39:36 +0000 Subject: [PATCH 11/16] changed wibotic_msgs | fixed cmake Signed-off-by: Jakub Delicat --- wibotic_connector_can/CMakeLists.txt | 27 ++++++++-------- wibotic_connector_can/cmake/SuperBuild.cmake | 32 +++++++++++++------ wibotic_connector_can/lib/libcyphal | 1 - .../lib/platform_specific_components | 1 - {wibotic_msg => wibotic_msgs}/CMakeLists.txt | 0 .../msg/WiboticInfo.msg | 0 {wibotic_msg => wibotic_msgs}/package.xml | 0 7 files changed, 36 insertions(+), 25 deletions(-) delete mode 160000 wibotic_connector_can/lib/libcyphal delete mode 160000 wibotic_connector_can/lib/platform_specific_components rename {wibotic_msg => wibotic_msgs}/CMakeLists.txt (100%) rename {wibotic_msg => wibotic_msgs}/msg/WiboticInfo.msg (100%) rename {wibotic_msg => wibotic_msgs}/package.xml (100%) diff --git a/wibotic_connector_can/CMakeLists.txt b/wibotic_connector_can/CMakeLists.txt index 41cab95..24fbd27 100644 --- a/wibotic_connector_can/CMakeLists.txt +++ b/wibotic_connector_can/CMakeLists.txt @@ -1,10 +1,15 @@ 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) - -# if(USE_SUPERBUILD) project(SUPERBUILD NONE) include(cmake/SuperBuild.cmake) -# return() else() project(wibotic_connector_can) endif() +# Handle superbuild first +option(USE_SUPERBUILD "Whether or not a superbuild should be invoked" ON) + +if(USE_SUPERBUILD) + project(SUPERBUILD NONE) + include(cmake/SuperBuild.cmake) + return() +else() + project(wibotic_connector_can) +endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) @@ -16,20 +21,14 @@ foreach(PACKAGE IN ITEMS ${PACKAGE_DEPENDENCIES}) find_package(${PACKAGE} REQUIRED) endforeach() -find_library(UAVCAN_LIB uavcan REQUIRED) - -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/include -) - -include_directories(${CMAKE_CURRENT_SOURCE_DIR}/lib/libcyphal/libuavcan/include) +include_directories(${CMAKE_INSTALL_PREFIX}/include) 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_SOURCE_DIR}/lib/platform_specific_components/linux/libuavcan/uavcan_linux + ${CMAKE_CURRENT_BINARY_DIR}/wibotic_connector_can/ep_platform_specific_components/upstream/linux/libuavcan/uavcan_linux ) target_include_directories( wibotic_can_driver @@ -38,6 +37,8 @@ target_include_directories( ament_target_dependencies(wibotic_can_driver ${PACKAGE_DEPENDENCIES}) +set(UAVCAN_LIB ${CMAKE_INSTALL_PREFIX}/lib/libuavcan.a) + target_link_libraries(wibotic_can_driver ${UAVCAN_LIB} rt) add_executable(wibotic_connector_can src/wibotic_can_driver_node.cpp diff --git a/wibotic_connector_can/cmake/SuperBuild.cmake b/wibotic_connector_can/cmake/SuperBuild.cmake index b981e61..565b837 100644 --- a/wibotic_connector_can/cmake/SuperBuild.cmake +++ b/wibotic_connector_can/cmake/SuperBuild.cmake @@ -16,7 +16,6 @@ include(ExternalProject) set(DEPENDENCIES ep_libuavcan ep_platform_specific_components) -file(MAKE_DIRECTORY ${CMAKE_CURRENT_BINARY_DIR}/ep_libuavcan/include) ExternalProject_Add( ep_libuavcan SOURCE_DIR ${CMAKE_CURRENT_BINARY_DIR}/ep_libuavcan/upstream @@ -24,19 +23,32 @@ ExternalProject_Add( 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= - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + UPDATE_DISCONNECTED 1 + BUILD_IN_SOURCE 1 + STEP_TARGETS build) ExternalProject_Add( - ep_platform_specific_components - SOURCE_DIR - ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components/upstream - SOURCE_SUBDIR linux/libuavcan - INSTALL_DIR ${CMAKE_INSTALL_PREFIX} + ep_platform_specific_components # Name of the external project GIT_REPOSITORY - https://github.com/OpenCyphal-Garage/platform_specific_components/ - GIT_TAG 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 - CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}) + https://github.com/OpenCyphal-Garage/platform_specific_components/ # Repository + # URL + GIT_TAG 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 # Specific commit + PREFIX + ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components # Directory + # where external + # project will + # be + 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}) + +# Make sure that the install directory is created +install(DIRECTORY ${INSTALL_DIR} DESTINATION ${CMAKE_INSTALL_PREFIX}) ExternalProject_Add( ep_wibotic_connector_can diff --git a/wibotic_connector_can/lib/libcyphal b/wibotic_connector_can/lib/libcyphal deleted file mode 160000 index dcc3a4d..0000000 --- a/wibotic_connector_can/lib/libcyphal +++ /dev/null @@ -1 +0,0 @@ -Subproject commit dcc3a4de237b7482e04543d2393c3a9385685312 diff --git a/wibotic_connector_can/lib/platform_specific_components b/wibotic_connector_can/lib/platform_specific_components deleted file mode 160000 index 4745ef5..0000000 --- a/wibotic_connector_can/lib/platform_specific_components +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 diff --git a/wibotic_msg/CMakeLists.txt b/wibotic_msgs/CMakeLists.txt similarity index 100% rename from wibotic_msg/CMakeLists.txt rename to wibotic_msgs/CMakeLists.txt diff --git a/wibotic_msg/msg/WiboticInfo.msg b/wibotic_msgs/msg/WiboticInfo.msg similarity index 100% rename from wibotic_msg/msg/WiboticInfo.msg rename to wibotic_msgs/msg/WiboticInfo.msg diff --git a/wibotic_msg/package.xml b/wibotic_msgs/package.xml similarity index 100% rename from wibotic_msg/package.xml rename to wibotic_msgs/package.xml From 1620487adef056974ff7ac3a2ba8912e913b751e Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 13:40:00 +0000 Subject: [PATCH 12/16] run precommit Signed-off-by: Jakub Delicat --- .../test/integration/wibotic_connector_can.test.py | 1 + 1 file changed, 1 insertion(+) diff --git a/wibotic_connector_can/test/integration/wibotic_connector_can.test.py b/wibotic_connector_can/test/integration/wibotic_connector_can.test.py index 566a0b0..1b00c57 100644 --- a/wibotic_connector_can/test/integration/wibotic_connector_can.test.py +++ b/wibotic_connector_can/test/integration/wibotic_connector_can.test.py @@ -21,6 +21,7 @@ from launch import LaunchDescription from launch_ros.actions import Node from launch_testing_ros import WaitForTopics + from wibotic_msgs.msg import WiboticInfo From a9700c854e2e6be967a8b673ddab97537c2eeb0f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Fri, 4 Oct 2024 14:19:08 +0000 Subject: [PATCH 13/16] Set version to 0.1.0 Signed-off-by: Jakub Delicat --- wibotic_connector_can/package.xml | 2 +- wibotic_msgs/package.xml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index 6c6cf57..6a4be9e 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -2,7 +2,7 @@ wibotic_connector_can - 2.1.0 + 0.1.0 Integration Panther with Wibotic charger Husarion diff --git a/wibotic_msgs/package.xml b/wibotic_msgs/package.xml index c8b444a..5e036db 100644 --- a/wibotic_msgs/package.xml +++ b/wibotic_msgs/package.xml @@ -2,7 +2,7 @@ wibotic_msgs - 2.0.4 + 0.1.0 Custom messages for Wibotic Charger robot. Husarion Apache License 2.0 From cc66f0ada96321793b630408b343cc595fd2b13d Mon Sep 17 00:00:00 2001 From: Jakub Delicat <109142865+delihus@users.noreply.github.com> Date: Tue, 15 Oct 2024 17:19:51 +0200 Subject: [PATCH 14/16] Update README.md Co-authored-by: rafal-gorecki <126687345+rafal-gorecki@users.noreply.github.com> --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index db63b3e..8bfb6fd 100644 --- a/README.md +++ b/README.md @@ -19,7 +19,7 @@ It reads a CAN Bus thanks to the uavcan library and sends the measurements to RO - `uavcan_node_name_` [*string*, default: **can0**]: Uavcan node name. - `update_time_s_` [*string*, default: **can0**]: The period of reading WiboticInfo on a CAN BUS. -## Add can interface +## Add CAN interface ```bash sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyACM0 From 3e0efb90a4c8af458e16a7cb8c3df3e4338f334f Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Tue, 15 Oct 2024 15:56:42 +0000 Subject: [PATCH 15/16] review suggestions Signed-off-by: Jakub Delicat --- .github/workflows/run-unit-tests.yaml | 1 + README.md | 36 +++++++++++-------- wibotic_connector_can/README.md | 20 +++++++++++ wibotic_connector_can/cmake/SuperBuild.cmake | 14 +++----- .../uavcan_types/wibotic/WiBoticInfo.hpp | 3 +- .../wibotic_can_driver.hpp | 6 ++-- wibotic_connector_can/package.xml | 6 ++-- .../src/wibotic_can_driver.cpp | 2 +- .../src/wibotic_can_driver_node.cpp | 14 ++++++-- .../unit/test_wibotic_can_driver_node.cpp | 2 +- .../uavcan/20200.WiBoticInfo.uavcan | 17 +++++++++ 11 files changed, 84 insertions(+), 37 deletions(-) create mode 100644 wibotic_connector_can/README.md create mode 100644 wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan diff --git a/.github/workflows/run-unit-tests.yaml b/.github/workflows/run-unit-tests.yaml index 7f23a7f..4281cd2 100644 --- a/.github/workflows/run-unit-tests.yaml +++ b/.github/workflows/run-unit-tests.yaml @@ -2,6 +2,7 @@ name: Run unit tests on: + push: workflow_dispatch: # TODO: ENABLE WHEN READY # pull_request: diff --git a/README.md b/README.md index db63b3e..46dab28 100644 --- a/README.md +++ b/README.md @@ -1,27 +1,33 @@ # wibotic_ros -The repository contains `wibotic_connector_can` and `wibotic_msgs` packages. It reads a CAN Bus thanks to the uavcan library and sends the measurements to ROS 2. +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. -## ROS Nodes +## Quick start -### wibotic_connector_can +### Add can interface -It reads a CAN Bus thanks to the uavcan library and sends the measurements to ROS 2. +```bash +sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyACM0 +sudo ip link set up can0 type can bitrate 500000 +``` -#### Publishes +### Create workspace -- `wibotic_info` [*wibotic_msgs/WiboticInfo*]: Wibotic charger measurements. +```bash +mkdir ~/husarion_ws +cd ~/husarion_ws +git clone -b ros2 https://github.com/husarion/wibotic_ros.git src/wibotic_ros +``` -#### Parameters +### Build -- `~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. +```bash +sudo rosdep init +rosdep update --rosdistro $ROS_DISTRO +rosdep install --from-paths src -y -i -## Add can interface +source /opt/ros/$ROS_DISTRO/setup.bash +colcon build --symlink-install --packages-up-to wibotic_connector_can --cmake-args -DCMAKE_BUILD_TYPE=Release -```bash -sudo slcand -o -s6 -t hw -S 3000000 /dev/ttyACM0 -sudo ip link set up can0 type can bitrate 500000 +source install/setup.bash ``` 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 index 565b837..3676904 100644 --- a/wibotic_connector_can/cmake/SuperBuild.cmake +++ b/wibotic_connector_can/cmake/SuperBuild.cmake @@ -30,16 +30,11 @@ ExternalProject_Add( STEP_TARGETS build) ExternalProject_Add( - ep_platform_specific_components # Name of the external project + ep_platform_specific_components GIT_REPOSITORY - https://github.com/OpenCyphal-Garage/platform_specific_components/ # Repository - # URL - GIT_TAG 4745ef59f57b7e1c34705b127ea8c7a35e3874c1 # Specific commit - PREFIX - ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components # Directory - # where external - # project will - # be + 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 @@ -47,7 +42,6 @@ ExternalProject_Add( ${CMAKE_CURRENT_BINARY_DIR}/ep_platform_specific_components/src/ep_platform_specific_components/linux/libuavcan/include ${CMAKE_INSTALL_PREFIX}/include ${INSTALL_DIR}) -# Make sure that the install directory is created install(DIRECTORY ${INSTALL_DIR} DESTINATION ${CMAKE_INSTALL_PREFIX}) ExternalProject_Add( 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 index f5d8f20..cab9c2e 100644 --- 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 @@ -17,7 +17,8 @@ * * Autogenerated, do not edit. * - * Source file: /home/husarion/ros2_ws/src/wibotic/20200.WiBoticInfo.uavcan + * Source file: + * https://github.com/husarion/wibotic_ros/tree/ros2/wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan */ #ifndef 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 index 0107635..192ecdc 100644 --- a/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp +++ b/wibotic_connector_can/include/wibotic_connector_can/wibotic_can_driver.hpp @@ -44,7 +44,7 @@ class WiboticCanDriverInterface * * @exception std::runtime_error Thrown if can interface cannot be found. * */ - virtual void SetUavCanSettings( + virtual void ConfigureUavCan( const std::string & can_iface_name, std::size_t node_id, const std::string & node_name) = 0; /** @@ -92,7 +92,7 @@ class WiboticCanDriverInterface * @brief Class for the Wibotic CAN driver. * * This class inherits from the `WiboticCanDriverInterface` and implements its methods. - * Class communicates with CAN interface and gets WiboticInfo messages. + * Class communicates with CAN interface using libuavcan and gets WiboticInfo messages. */ class WiboticCanDriver : public WiboticCanDriverInterface { @@ -108,7 +108,7 @@ class WiboticCanDriver : public WiboticCanDriverInterface * * @exception std::runtime_error Thrown if can interface cannot be found. * */ - void SetUavCanSettings( + void ConfigureUavCan( const std::string & can_iface_name, std::size_t node_id, const std::string & node_name) override; diff --git a/wibotic_connector_can/package.xml b/wibotic_connector_can/package.xml index 6a4be9e..f81abb3 100644 --- a/wibotic_connector_can/package.xml +++ b/wibotic_connector_can/package.xml @@ -3,14 +3,14 @@ wibotic_connector_can 0.1.0 - Integration Panther with Wibotic charger + Integration Husarion UGV with Wibotic charger Husarion Apache License 2.0 https://husarion.com/ - https://github.com/husarion/panther_ros - https://github.com/husarion/panther_ros/issues + https://github.com/husarion/wibotic_ros + https://github.com/husarion/wibotic_ros/issues Jakub Delicat diff --git a/wibotic_connector_can/src/wibotic_can_driver.cpp b/wibotic_connector_can/src/wibotic_can_driver.cpp index c1cffb4..0e230f6 100644 --- a/wibotic_connector_can/src/wibotic_can_driver.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver.cpp @@ -16,7 +16,7 @@ namespace wibotic_connector_can { -void WiboticCanDriver::SetUavCanSettings( +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; diff --git a/wibotic_connector_can/src/wibotic_can_driver_node.cpp b/wibotic_connector_can/src/wibotic_can_driver_node.cpp index 37df40c..67236e1 100644 --- a/wibotic_connector_can/src/wibotic_can_driver_node.cpp +++ b/wibotic_connector_can/src/wibotic_can_driver_node.cpp @@ -46,7 +46,7 @@ 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", 1.0); + this->declare_parameter("update_time_s", 0.2); } void WiboticCanDriverNode::GetParameters() @@ -59,7 +59,7 @@ void WiboticCanDriverNode::GetParameters() void WiboticCanDriverNode::CreateWiboticCanDriver() { - wibotic_can_driver_->SetUavCanSettings(can_iface_name_, uavcan_node_id_, uavcan_node_name_); + 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(); @@ -69,6 +69,7 @@ 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(); } @@ -78,8 +79,15 @@ void WiboticCanDriverNode::WiboticInfoTimerCallback() 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 { - auto wibotic_info = GetWiboticInfo(); wibotic_info_pub_->publish(ConvertWiboticInfoToMsg(wibotic_info)); } catch (const std::runtime_error & e) { RCLCPP_WARN(this->get_logger(), e.what()); 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 index 550b64b..36d7749 100644 --- a/wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp +++ b/wibotic_connector_can/test/unit/test_wibotic_can_driver_node.cpp @@ -36,7 +36,7 @@ class MockWiboticCanDriver : public wibotic_connector_can::WiboticCanDriverInter { public: MOCK_METHOD( - void, SetUavCanSettings, (const std::string &, std::size_t, const std::string &), (override)); + 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)); diff --git a/wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan b/wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan new file mode 100644 index 0000000..88d5fbe --- /dev/null +++ b/wibotic_connector_can/uavcan/20200.WiBoticInfo.uavcan @@ -0,0 +1,17 @@ +# +# 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 From fc5d44f4e999f5bf0aab568c48eccc75d8b33bee Mon Sep 17 00:00:00 2001 From: Jakub Delicat Date: Wed, 16 Oct 2024 19:48:37 +0000 Subject: [PATCH 16/16] Removed unit tests Signed-off-by: Jakub Delicat --- .github/workflows/run-unit-tests.yaml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/run-unit-tests.yaml b/.github/workflows/run-unit-tests.yaml index 4281cd2..7f23a7f 100644 --- a/.github/workflows/run-unit-tests.yaml +++ b/.github/workflows/run-unit-tests.yaml @@ -2,7 +2,6 @@ name: Run unit tests on: - push: workflow_dispatch: # TODO: ENABLE WHEN READY # pull_request: