From 64900e4e9bce17f450807b4c398face0ae130472 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Fri, 18 Feb 2022 15:47:40 +0100 Subject: [PATCH 01/71] Release/v1.1.0 (#40) * Update version and changelog --- CHANGELOG.md | 16 +++++++++++++++- source/modulo_core/CMakeLists.txt | 4 ++-- source/modulo_core/package.xml | 4 ++-- 3 files changed, 19 insertions(+), 5 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index e74e2fac9..cd877868b 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,4 +1,18 @@ -## 1.0.0 (September 22, 2020) +## 1.1.0 (February 18, 2022) + +This version contains some general improvements and fixes, including support +for the breaking changes introduced by control libraries 5.0 + +- Update docker paradigm (#30) +- Refactor the shutdown logic (#31) +- Modify build script for production by default (#32) +- Change the underlying message type to UInt8MultiArray (#33) +- Support int and int array parameters (#34) +- Remove the pure virtual step function (#35) +- Fix the examples due to breaking changes in DS (#36) +- Correct the include due to changes in control-libraries (#38) + +## 1.0.0 (September 22, 2021) Modulo core library to provide ROS2 Cell and Component classes built around state_representation and clproto libraries. diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index 85d1450aa..9d3249976 100644 --- a/source/modulo_core/CMakeLists.txt +++ b/source/modulo_core/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.9) -project(modulo_core) +project(modulo_core VERSION 1.1.0) # Default to C++17 if(NOT CMAKE_CXX_STANDARD) @@ -86,4 +86,4 @@ ament_auto_package() install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/ -) \ No newline at end of file +) diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 2c4edf7e0..ea98d3c09 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -2,7 +2,7 @@ modulo_core - 0.0.0 + 1.1.0 TODO: Package description buschbapti TODO: License declaration @@ -19,7 +19,7 @@ lifecycle_msgs tf2_msgs tf2_ros - + ament_lint_auto ament_lint_common From 3e860b49ddfde4aae80a73b39bbd651b77f5101a Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 13 Apr 2022 18:21:39 +0200 Subject: [PATCH 02/71] Fix develop after control libraries StateType refactor (#55) * Fix switch cases due to StateType refactor * Use galactic-devel image for develop branch --- build-server.sh | 2 +- source/modulo_core/src/Cell.cpp | 210 ++++++++++++++-------------- source/modulo_core/src/Recorder.cpp | 4 +- 3 files changed, 110 insertions(+), 106 deletions(-) diff --git a/build-server.sh b/build-server.sh index e84f75012..e62d56afb 100755 --- a/build-server.sh +++ b/build-server.sh @@ -1,5 +1,5 @@ #!/bin/bash -ROS_VERSION=galactic +ROS_VERSION=galactic-devel IMAGE_NAME=epfl-lasa/modulo IMAGE_TAG=latest diff --git a/source/modulo_core/src/Cell.cpp b/source/modulo_core/src/Cell.cpp index 1a4ece412..dc8d3eeaf 100644 --- a/source/modulo_core/src/Cell.cpp +++ b/source/modulo_core/src/Cell.cpp @@ -159,74 +159,73 @@ void Cell::add_parameters( ) { using namespace state_representation; using namespace state_representation::exceptions; - for (auto& param : parameters) { - switch (param->get_type()) { - case StateType::PARAMETER_INT: { + for (auto& param: parameters) { + switch (param->get_parameter_type()) { + case ParameterType::INT: { this->add_parameter(std::static_pointer_cast>(param), prefix); break; } - case StateType::PARAMETER_INT_ARRAY: { + case ParameterType::INT_ARRAY: { this->add_parameter(std::static_pointer_cast>>(param), prefix); break; } - case StateType::PARAMETER_DOUBLE: { + case ParameterType::DOUBLE: { this->add_parameter(std::static_pointer_cast>(param), prefix); break; } - case StateType::PARAMETER_DOUBLE_ARRAY: { + case ParameterType::DOUBLE_ARRAY: { this->add_parameter(std::static_pointer_cast>>(param), prefix); break; } - case StateType::PARAMETER_BOOL: { + case ParameterType::BOOL: { this->add_parameter(std::static_pointer_cast>(param), prefix); break; } - case StateType::PARAMETER_BOOL_ARRAY: { + case ParameterType::BOOL_ARRAY: { this->add_parameter(std::static_pointer_cast>>(param), prefix); break; } - case StateType::PARAMETER_STRING: { + case ParameterType::STRING: { this->add_parameter(std::static_pointer_cast>(param), prefix); break; } - case StateType::PARAMETER_STRING_ARRAY: { + case ParameterType::STRING_ARRAY: { this->add_parameter(std::static_pointer_cast>>(param), prefix); break; } - case StateType::PARAMETER_CARTESIANSTATE: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case StateType::PARAMETER_CARTESIANPOSE: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case StateType::PARAMETER_JOINTSTATE: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case StateType::PARAMETER_JOINTPOSITIONS: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case StateType::PARAMETER_ELLIPSOID: { - this->add_parameter(std::static_pointer_cast>(param), prefix); + case ParameterType::STATE: { + switch (param->get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + this->add_parameter(std::static_pointer_cast>(param), prefix); + break; + case StateType::CARTESIAN_POSE: + this->add_parameter(std::static_pointer_cast>(param), prefix); + break; + case StateType::JOINT_STATE: + this->add_parameter(std::static_pointer_cast>(param), prefix); + break; + case StateType::JOINT_POSITIONS: + this->add_parameter(std::static_pointer_cast>(param), prefix); + break; + case StateType::GEOMETRY_ELLIPSOID: + this->add_parameter(std::static_pointer_cast>(param), prefix); + break; + default: { + throw UnrecognizedParameterTypeException("The Parameter state type is not available"); + } + } break; } - case StateType::PARAMETER_MATRIX: { + case ParameterType::MATRIX: { this->add_parameter(std::static_pointer_cast>(param), prefix); break; } @@ -300,73 +299,72 @@ void Cell::set_parameter_value(const std::string& parameter_name, const Eigen::M void Cell::set_parameter_value(const std::shared_ptr& parameter) { using namespace state_representation; using namespace state_representation::exceptions; - switch (parameter->get_type()) { - case StateType::PARAMETER_INT: { + switch (parameter->get_parameter_type()) { + case ParameterType::INT: { this->set_parameter_value(std::static_pointer_cast>(parameter)); break; } - case StateType::PARAMETER_INT_ARRAY: { + case ParameterType::INT_ARRAY: { this->set_parameter_value(std::static_pointer_cast>>(parameter)); break; } - case StateType::PARAMETER_DOUBLE: { + case ParameterType::DOUBLE: { this->set_parameter_value(std::static_pointer_cast>(parameter)); break; } - case StateType::PARAMETER_DOUBLE_ARRAY: { + case ParameterType::DOUBLE_ARRAY: { this->set_parameter_value(std::static_pointer_cast>>(parameter)); break; } - case StateType::PARAMETER_BOOL: { + case ParameterType::BOOL: { this->set_parameter_value(std::static_pointer_cast>(parameter)); break; } - case StateType::PARAMETER_BOOL_ARRAY: { + case ParameterType::BOOL_ARRAY: { this->set_parameter_value(std::static_pointer_cast>>(parameter)); break; } - case StateType::PARAMETER_STRING: { + case ParameterType::STRING: { this->set_parameter_value(std::static_pointer_cast>(parameter)); break; } - case StateType::PARAMETER_STRING_ARRAY: { + case ParameterType::STRING_ARRAY: { this->set_parameter_value(std::static_pointer_cast>>(parameter)); break; } - case StateType::PARAMETER_CARTESIANSTATE: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case StateType::PARAMETER_CARTESIANPOSE: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case StateType::PARAMETER_JOINTSTATE: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case StateType::PARAMETER_JOINTPOSITIONS: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case StateType::PARAMETER_ELLIPSOID: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); + case ParameterType::STATE: { + switch (parameter->get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + this->set_parameter_value(std::static_pointer_cast>(parameter)); + break; + case StateType::CARTESIAN_POSE: + this->set_parameter_value(std::static_pointer_cast>(parameter)); + break; + case StateType::JOINT_STATE: + this->set_parameter_value(std::static_pointer_cast>(parameter)); + break; + case StateType::JOINT_POSITIONS: + this->set_parameter_value(std::static_pointer_cast>(parameter)); + break; + case StateType::GEOMETRY_ELLIPSOID: + this->set_parameter_value(std::static_pointer_cast>(parameter)); + break; + default: { + throw UnrecognizedParameterTypeException("The Parameter state type is not available"); + } + } break; } - case StateType::PARAMETER_MATRIX: { + case ParameterType::MATRIX: { this->set_parameter_value(std::static_pointer_cast>(parameter)); break; } @@ -572,88 +570,94 @@ void Cell::update_parameters() { using namespace state_representation; using namespace state_representation::exceptions; try { - for (auto& [key, param] : this->parameters_) { - switch (param->get_type()) { - case StateType::PARAMETER_INT: { + for (auto&[key, param]: this->parameters_) { + switch (param->get_parameter_type()) { + case ParameterType::INT: { int value = static_cast(this->get_parameter(param->get_name()).as_int()); std::static_pointer_cast>(param)->set_value(value); break; } - case StateType::PARAMETER_INT_ARRAY: { + case ParameterType::INT_ARRAY: { std::vector tmp = this->get_parameter(param->get_name()).as_integer_array(); std::vector value(tmp.begin(), tmp.end()); std::static_pointer_cast>>(param)->set_value(value); break; } - case StateType::PARAMETER_DOUBLE: { + case ParameterType::DOUBLE: { double value = this->get_parameter(param->get_name()).as_double(); std::static_pointer_cast>(param)->set_value(value); break; } - case StateType::PARAMETER_DOUBLE_ARRAY: { + case ParameterType::DOUBLE_ARRAY: { std::vector value = this->get_parameter(param->get_name()).as_double_array(); std::static_pointer_cast>>(param)->set_value(value); break; } - case StateType::PARAMETER_BOOL: { + case ParameterType::BOOL: { bool value = this->get_parameter(param->get_name()).as_bool(); std::static_pointer_cast>(param)->set_value(value); break; } - case StateType::PARAMETER_BOOL_ARRAY: { + case ParameterType::BOOL_ARRAY: { std::vector value = this->get_parameter(param->get_name()).as_bool_array(); std::static_pointer_cast>>(param)->set_value(value); break; } - case StateType::PARAMETER_STRING: { + case ParameterType::STRING: { std::string value = this->get_parameter(param->get_name()).as_string(); std::static_pointer_cast>(param)->set_value(value); break; } - case StateType::PARAMETER_STRING_ARRAY: { + case ParameterType::STRING_ARRAY: { std::vector value = this->get_parameter(param->get_name()).as_string_array(); std::static_pointer_cast>>(param)->set_value(value); break; } - case StateType::PARAMETER_CARTESIANSTATE: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().set_data(value); - break; - } - - case StateType::PARAMETER_CARTESIANPOSE: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().CartesianPose::set_data(value); - break; - } - - case StateType::PARAMETER_JOINTSTATE: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().set_data(value); - break; - } - - case StateType::PARAMETER_JOINTPOSITIONS: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().JointPositions::set_data(value); - break; - } - - case StateType::PARAMETER_ELLIPSOID: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().set_data(value); + case ParameterType::STATE: { + switch (param->get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: { + std::vector value = this->get_parameter(param->get_name()).as_double_array(); + std::static_pointer_cast>(param)->get_value().set_data(value); + break; + } + case StateType::CARTESIAN_POSE: { + std::vector value = this->get_parameter(param->get_name()).as_double_array(); + std::static_pointer_cast>(param)->get_value().CartesianPose::set_data(value); + break; + } + case StateType::JOINT_STATE: { + std::vector value = this->get_parameter(param->get_name()).as_double_array(); + std::static_pointer_cast>(param)->get_value().set_data(value); + break; + } + case StateType::JOINT_POSITIONS: { + std::vector value = this->get_parameter(param->get_name()).as_double_array(); + std::static_pointer_cast>(param)->get_value().JointPositions::set_data( + value + ); + break; + } + case StateType::GEOMETRY_ELLIPSOID: { + std::vector value = this->get_parameter(param->get_name()).as_double_array(); + std::static_pointer_cast>(param)->get_value().set_data(value); + break; + } + default: { + throw UnrecognizedParameterTypeException("The Parameter state type is not available"); + } + } break; } - case StateType::PARAMETER_MATRIX: { + case ParameterType::MATRIX: { std::vector value = this->get_parameter(param->get_name()).as_double_array(); size_t rows = std::static_pointer_cast>(param)->get_value().rows(); size_t cols = std::static_pointer_cast>(param)->get_value().cols(); diff --git a/source/modulo_core/src/Recorder.cpp b/source/modulo_core/src/Recorder.cpp index a87846e18..87df3cc68 100644 --- a/source/modulo_core/src/Recorder.cpp +++ b/source/modulo_core/src/Recorder.cpp @@ -38,10 +38,10 @@ void Recorder::step() { bool Recorder::record(const state_representation::State& state) const { switch (state.get_type()) { - case state_representation::StateType::CARTESIANSTATE: + case state_representation::StateType::CARTESIAN_STATE: return record(static_cast(state)); - case state_representation::StateType::JOINTSTATE: + case state_representation::StateType::JOINT_STATE: return record(static_cast(state)); default: From c10e1226ea88dac767f9f4df013e226a42aad75d Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 14 Apr 2022 23:05:52 +0200 Subject: [PATCH 03/71] Fix examples after DS factory refactor (#56) --- .../examples/cartesianCircular.cpp | 5 +---- .../modulo_core/examples/cartesianLinear.cpp | 4 +--- source/modulo_core/examples/helicoidal.cpp | 9 ++------ source/modulo_core/examples/joints.cpp | 5 +---- .../examples/movingReferenceFrame.cpp | 22 +++++++++---------- 5 files changed, 16 insertions(+), 29 deletions(-) diff --git a/source/modulo_core/examples/cartesianCircular.cpp b/source/modulo_core/examples/cartesianCircular.cpp index a0e4dccbb..5e8bc512f 100644 --- a/source/modulo_core/examples/cartesianCircular.cpp +++ b/source/modulo_core/examples/cartesianCircular.cpp @@ -24,10 +24,7 @@ class MotionGenerator : public modulo::core::Cell { Cell(node_name, period), current_pose(std::make_shared("robot_test")), desired_twist(std::make_shared("robot_test")), - motion_generator( - DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::CIRCULAR - )) { + motion_generator(CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::CIRCULAR)) { motion_generator->set_parameter_value("limit_cycle", Ellipsoid("attractor")); } diff --git a/source/modulo_core/examples/cartesianLinear.cpp b/source/modulo_core/examples/cartesianLinear.cpp index 2050c866a..07d7c8a29 100644 --- a/source/modulo_core/examples/cartesianLinear.cpp +++ b/source/modulo_core/examples/cartesianLinear.cpp @@ -26,9 +26,7 @@ class LinearMotionGenerator : public modulo::core::Cell { current_pose(std::make_shared("robot_test")), desired_twist(std::make_shared("robot_test")), motion_generator( - DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - )) { + CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)) { this->motion_generator->set_parameter_value("attractor", CartesianPose::Random("robot_test")); } diff --git a/source/modulo_core/examples/helicoidal.cpp b/source/modulo_core/examples/helicoidal.cpp index 601c3d604..ce3f88b23 100644 --- a/source/modulo_core/examples/helicoidal.cpp +++ b/source/modulo_core/examples/helicoidal.cpp @@ -28,14 +28,9 @@ class MotionGenerator : public Cell { Cell(node_name, period), current_pose(std::make_shared("robot_test")), desired_twist(std::make_shared("robot_test")), - ring_motion_generator( - DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::RING - )), + ring_motion_generator(CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::RING)), linear_motion_generator( - DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - )), + CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)), radius(1.0), radius_decay(0.99) { this->linear_motion_generator->set_parameter_value("attractor", CartesianPose::Identity("robot_test")); diff --git a/source/modulo_core/examples/joints.cpp b/source/modulo_core/examples/joints.cpp index 809af7f15..dcccae5e2 100644 --- a/source/modulo_core/examples/joints.cpp +++ b/source/modulo_core/examples/joints.cpp @@ -21,10 +21,7 @@ class LinearMotionGenerator : public modulo::core::Cell { Cell(node_name, period), current_positions(std::make_shared("robot", 6)), desired_velocities(std::make_shared("robot", 6)), - motion_generator( - DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - )) { + motion_generator(JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)) { motion_generator->set_parameter_value("attractor", JointState::Random("robot", 6)); } diff --git a/source/modulo_core/examples/movingReferenceFrame.cpp b/source/modulo_core/examples/movingReferenceFrame.cpp index afaf4dd2f..daf964d32 100644 --- a/source/modulo_core/examples/movingReferenceFrame.cpp +++ b/source/modulo_core/examples/movingReferenceFrame.cpp @@ -26,10 +26,7 @@ class MotionGenerator : public modulo::core::Cell { object_state(std::make_shared("object", "world")), current_pose(std::make_shared("robot_test", "world")), desired_twist(std::make_shared("robot_test", "world")), - motion_generator( - DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::CIRCULAR - )) { + motion_generator(CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::CIRCULAR)) { motion_generator->set_parameter_value("limit_cycle", Ellipsoid("attractor", "object")); } @@ -91,13 +88,16 @@ class SimulatedObject : public modulo::core::Cell { public: explicit SimulatedObject(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), object_pose( - std::make_shared( - CartesianPose::Identity("object"))), object_twist( - std::make_shared(CartesianTwist("object"))), motion_generator( - DynamicalSystemFactory::create_dynamical_system( - DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR - )), dt(period), sign(-1) { + Cell(node_name, period), + object_pose( + std::make_shared( + CartesianPose::Identity("object"))), + object_twist( + std::make_shared(CartesianTwist("object"))), + motion_generator( + CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)), + dt(period), + sign(-1) { motion_generator->set_parameter_value("attractor", CartesianPose("object_attractor", 2., 0., 0.)); } From 349c801e35e822eea5e526a5c8927c35669149f8 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 25 Mar 2022 11:14:21 +0100 Subject: [PATCH 04/71] Create modulo_components package (#42) * Add modulo components * Build modulo_core in development stage * Remove code * Upgrade ament cmake python * Rename to NodeT and move header * Remove python code and trim code * Cleaner structure for Dockerfile --- Dockerfile | 23 +++--- build-server.sh | 6 +- source/modulo_components/CMakeLists.txt | 33 +++++++++ source/modulo_components/README.md | 1 + .../include/modulo_components/Component.h | 12 ++++ .../modulo_components/ComponentInterface.h | 17 +++++ .../modulo_components/LifecycleComponent.h | 13 ++++ .../modulo_components/utilities/utilities.h | 71 +++++++++++++++++++ source/modulo_components/package.xml | 29 ++++++++ source/modulo_components/setup.cfg | 0 source/modulo_components/src/Component.cpp | 1 + .../src/ComponentInterface.cpp | 7 ++ .../src/LifecycleComponent.cpp | 1 + .../test/test_cpp_components.cpp | 7 ++ 14 files changed, 209 insertions(+), 12 deletions(-) create mode 100644 source/modulo_components/CMakeLists.txt create mode 100644 source/modulo_components/README.md create mode 100644 source/modulo_components/include/modulo_components/Component.h create mode 100644 source/modulo_components/include/modulo_components/ComponentInterface.h create mode 100644 source/modulo_components/include/modulo_components/LifecycleComponent.h create mode 100644 source/modulo_components/include/modulo_components/utilities/utilities.h create mode 100644 source/modulo_components/package.xml create mode 100644 source/modulo_components/setup.cfg create mode 100644 source/modulo_components/src/Component.cpp create mode 100644 source/modulo_components/src/ComponentInterface.cpp create mode 100644 source/modulo_components/src/LifecycleComponent.cpp create mode 100644 source/modulo_components/test/test_cpp_components.cpp diff --git a/Dockerfile b/Dockerfile index 6c99f2dd9..96b5c511f 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,16 +1,21 @@ ARG ROS_VERSION=galactic -FROM ghcr.io/aica-technology/ros2-control-libraries:${ROS_VERSION} as development +FROM ghcr.io/aica-technology/ros2-control-libraries:${ROS_VERSION} as dependencies +# upgrade ament_cmake_python +RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-ament-cmake-python && sudo rm -rf /var/lib/apt/lists/* WORKDIR ${HOME}/ros2_ws -# copy sources and build ROS workspace with user permissions -WORKDIR ${HOME}/ros2_ws/ -COPY --chown=${USER} ./source/ ./src/ -# clean image -RUN sudo apt-get clean && sudo rm -rf /var/lib/apt/lists/* + +FROM dependencies as modulo-core + +COPY --chown=${USER} ./source/modulo_core ./src/modulo_core +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build --packages-select modulo_core" -FROM development as production +FROM modulo-core as modulo-components -# build modulo -RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build" +COPY --chown=${USER} ./source/modulo_components ./src/modulo_components +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build --packages-select modulo_components" + +# clean image +RUN sudo apt-get clean && sudo rm -rf /var/lib/apt/lists/* diff --git a/build-server.sh b/build-server.sh index e62d56afb..c5ed2baa7 100755 --- a/build-server.sh +++ b/build-server.sh @@ -9,8 +9,8 @@ SERVE_REMOTE=false HELP_MESSAGE="Usage: build.sh [-p] [-r] Options: - -d, --development Only target the development layer to prevent - sources from being built or tested + -d, --development Only target the modulo-core layer to prevent + modulo_component from being built or tested -r, --rebuild Rebuild the image(s) using the docker --no-cache option @@ -25,7 +25,7 @@ BUILD_FLAGS=(--build-arg ROS_VERSION="${ROS_VERSION}") while [[ $# -gt 0 ]]; do opt="$1" case $opt in - -d|--development) BUILD_FLAGS+=(--target development) ; IMAGE_TAG=development ; shift ;; + -d|--development) BUILD_FLAGS+=(--target modulo-core) ; IMAGE_TAG=development ; shift ;; -r|--rebuild) BUILD_FLAGS+=(--no-cache) ; shift ;; -v|--verbose) BUILD_FLAGS+=(--progress=plain) ; shift ;; -s|--serve) SERVE_REMOTE=true ; shift ;; diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt new file mode 100644 index 000000000..c0e7b5255 --- /dev/null +++ b/source/modulo_components/CMakeLists.txt @@ -0,0 +1,33 @@ +cmake_minimum_required(VERSION 3.15) +project(modulo_components) + +# default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_auto_find_build_dependencies() + +find_package(control_libraries 5.0.0 REQUIRED COMPONENTS state_representation) + +include_directories(include) + +ament_auto_add_library(${PROJECT_NAME} SHARED + ${PROJECT_SOURCE_DIR}/src/ComponentInterface.cpp + ${PROJECT_SOURCE_DIR}/src/Component.cpp + ${PROJECT_SOURCE_DIR}/src/LifecycleComponent.cpp) + +ament_auto_package() diff --git a/source/modulo_components/README.md b/source/modulo_components/README.md new file mode 100644 index 000000000..87d02d367 --- /dev/null +++ b/source/modulo_components/README.md @@ -0,0 +1 @@ +# modulo_components \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/Component.h b/source/modulo_components/include/modulo_components/Component.h new file mode 100644 index 000000000..6a694f52b --- /dev/null +++ b/source/modulo_components/include/modulo_components/Component.h @@ -0,0 +1,12 @@ +#pragma once + +#include "modulo_components/ComponentInterface.h" +#include "modulo_core/communication/EncodedState.hpp" + +namespace modulo_components { + +class Component : public ComponentInterface> { + +}; + +} \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.h b/source/modulo_components/include/modulo_components/ComponentInterface.h new file mode 100644 index 000000000..69da18612 --- /dev/null +++ b/source/modulo_components/include/modulo_components/ComponentInterface.h @@ -0,0 +1,17 @@ +#pragma once + +#include +#include +#include + +namespace modulo_components { + +template +class ComponentInterface : public state_representation::ParameterMap, public NodeT { + +public: + explicit ComponentInterface(const rclcpp::NodeOptions& node_options); + +}; + +}// namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.h b/source/modulo_components/include/modulo_components/LifecycleComponent.h new file mode 100644 index 000000000..cc1e85107 --- /dev/null +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.h @@ -0,0 +1,13 @@ +#pragma once + +#include "modulo_components/ComponentInterface.h" +#include "modulo_core/communication/EncodedState.hpp" + +namespace modulo_components { + +class LifecycleComponent : public ComponentInterface> { + +}; + +} \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.h b/source/modulo_components/include/modulo_components/utilities/utilities.h new file mode 100644 index 000000000..9542020a8 --- /dev/null +++ b/source/modulo_components/include/modulo_components/utilities/utilities.h @@ -0,0 +1,71 @@ +#pragma once + +#include + +#include + +namespace modulo_components::utilities { + +/** + * @brief Parse a string argument value from an argument list given a pattern prefix. + * @param args a vector of string arguments + * @param pattern the prefix pattern to find and strip + * @param result the default argument value that is overwritten by reference if the given pattern is found + * @return the value of the resultant string + */ +static std::string parse_string_argument(const std::vector& args, const std::string& pattern, std::string& result) { + for (const auto& arg : args) { + std::string::size_type index = arg.find(pattern); + if (index != std::string::npos) { + result = arg; + result.erase(index, pattern.length()); + break; + } + } + return result; +} + +/** + * @brief Parse a string node name from NodeOptions arguments. + * @param options the NodeOptions structure to parse + * @param fallback the default name if the NodeOptions structure cannot be parsed + * @return the parsed node name or the fallback name + */ +static std::string parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback="") { + std::string node_name(fallback); + const std::string pattern("__node:="); + return parse_string_argument(options.arguments(), pattern, node_name); +} + +/** + * @brief Parse a string node namespace from NodeOptions arguments. + * @param options the NodeOptions structure to parse + * @param fallback the default namespace if the NodeOptions structure cannot be parsed + * @return the parsed node namespace or the fallback namespace + */ +static std::string parse_node_namespace(const rclcpp::NodeOptions& options, const std::string& fallback="") { + std::string node_namespace(fallback); + const std::string pattern("__ns:="); + return parse_string_argument(options.arguments(), pattern, node_namespace); +} + +/** + * @brief Parse a period in seconds from NodeOptions parameters. + * @details The parameter must have the name "period" + * and be interpretable as a value in seconds of type double + * @param options the NodeOptions structure to parse + * @param default_period the default period in seconds + * @return the parsed period or the default period as a chrono duration + */ +static std::chrono::nanoseconds parse_period(const rclcpp::NodeOptions& options, double default_period = 0.001) { + double period = default_period; + for (auto& param : options.parameter_overrides()) { + if (param.get_name() == "period") { + period = param.as_double(); + break; + } + } + return std::chrono::nanoseconds(static_cast(period * 1e9)); +} + +}// namespace modulo_components::utilities \ No newline at end of file diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml new file mode 100644 index 000000000..109a3d970 --- /dev/null +++ b/source/modulo_components/package.xml @@ -0,0 +1,29 @@ + + + + modulo_components + 0.0.1 + TODO + TODO + TODO + TODO + TODO: License declaration + + ament_cmake_auto + ament_cmake_python + + rclcpp + rclpy + rclcpp_components + rcutils + modulo_core + + launch_ros + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/source/modulo_components/setup.cfg b/source/modulo_components/setup.cfg new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp new file mode 100644 index 000000000..3843f4ec5 --- /dev/null +++ b/source/modulo_components/src/Component.cpp @@ -0,0 +1 @@ +#include "modulo_components/Component.h" \ No newline at end of file diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp new file mode 100644 index 000000000..bbfde9bb3 --- /dev/null +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -0,0 +1,7 @@ +#include "modulo_components/ComponentInterface.h" + + +namespace modulo_components { + + +}// namespace modulo_components diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp new file mode 100644 index 000000000..f342be47a --- /dev/null +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -0,0 +1 @@ +#include "modulo_components/LifecycleComponent.h" \ No newline at end of file diff --git a/source/modulo_components/test/test_cpp_components.cpp b/source/modulo_components/test/test_cpp_components.cpp new file mode 100644 index 000000000..e7bb1d13c --- /dev/null +++ b/source/modulo_components/test/test_cpp_components.cpp @@ -0,0 +1,7 @@ +#include + + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From edde5143a1be3855cba2cf4e65282fb0c442ff11 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Sun, 27 Mar 2022 21:23:16 +0200 Subject: [PATCH 05/71] Modulo translators (#41) * Add modulo_new_core package with translators in cpp and python * Update ament cmake python * Use modulo_new_core for translations * Fix includes * Move EncodedState to modulo_new_core * Fix formatting of if statements and check for emptiness early * Update test suite name and add expect_throw for empty state * Use namespace differently * Use base tag in Docker and fix CMakeLists * Add EncodedState to python * Throw error if state is empty * Improve python tests * Update build script --- Dockerfile | 7 +- build-server.sh | 16 +- .../include/modulo_components/Component.h | 4 +- .../modulo_components/LifecycleComponent.h | 4 +- source/modulo_components/package.xml | 1 + source/modulo_core/CMakeLists.txt | 2 - .../examples/cartesianCircular.cpp | 1 + source/modulo_core/examples/helicoidal.cpp | 12 +- .../examples/movingReferenceFrame.cpp | 1 + .../modulo_core/include/modulo_core/Cell.hpp | 9 +- .../message_passing/MessagePassingHandler.hpp | 4 +- .../message_passing/PublisherHandler.hpp | 2 +- .../message_passing/SubscriptionHandler.hpp | 2 +- source/modulo_core/package.xml | 1 + source/modulo_core/src/Cell.cpp | 1 + source/modulo_core/src/Component.cpp | 2 + .../message_passing/ReadStateConversion.cpp | 116 ---------- .../TransformListenerHandler.cpp | 2 +- .../message_passing/WriteStateConversion.cpp | 216 ------------------ source/modulo_new_core/CMakeLists.txt | 60 +++++ source/modulo_new_core/README.md | 1 + .../include/modulo_new_core}/EncodedState.hpp | 2 +- .../translators}/ReadStateConversion.hpp | 50 +--- .../translators}/WriteStateConversion.hpp | 117 ++-------- .../modulo_new_core/__init__.py | 0 .../modulo_new_core/encoded_state.py | 3 + .../modulo_new_core/translators/__init__.py | 0 .../translators/read_state_conversion.py | 72 ++++++ .../translators/write_state_conversion.py | 94 ++++++++ source/modulo_new_core/package.xml | 26 +++ source/modulo_new_core/setup.cfg | 0 .../src/translators/ReadStateConversion.cpp | 80 +++++++ .../src/translators/WriteStateConversion.cpp | 174 ++++++++++++++ .../translators/test_translators.cpp | 181 +++++++++++++++ .../test/python_tests/conftest.py | 18 ++ .../test/python_tests/test_translators.py | 171 ++++++++++++++ .../test/test_modulo_new_core.cpp | 7 + 37 files changed, 957 insertions(+), 502 deletions(-) delete mode 100644 source/modulo_core/src/communication/message_passing/ReadStateConversion.cpp delete mode 100644 source/modulo_core/src/communication/message_passing/WriteStateConversion.cpp create mode 100644 source/modulo_new_core/CMakeLists.txt create mode 100644 source/modulo_new_core/README.md rename source/{modulo_core/include/modulo_core/communication => modulo_new_core/include/modulo_new_core}/EncodedState.hpp (80%) rename source/{modulo_core/include/modulo_core/communication/message_passing => modulo_new_core/include/modulo_new_core/translators}/ReadStateConversion.hpp (71%) rename source/{modulo_core/include/modulo_core/communication/message_passing => modulo_new_core/include/modulo_new_core/translators}/WriteStateConversion.hpp (63%) create mode 100644 source/modulo_new_core/modulo_new_core/__init__.py create mode 100644 source/modulo_new_core/modulo_new_core/encoded_state.py create mode 100644 source/modulo_new_core/modulo_new_core/translators/__init__.py create mode 100644 source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py create mode 100644 source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py create mode 100644 source/modulo_new_core/package.xml create mode 100644 source/modulo_new_core/setup.cfg create mode 100644 source/modulo_new_core/src/translators/ReadStateConversion.cpp create mode 100644 source/modulo_new_core/src/translators/WriteStateConversion.cpp create mode 100644 source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp create mode 100644 source/modulo_new_core/test/python_tests/conftest.py create mode 100644 source/modulo_new_core/test/python_tests/test_translators.py create mode 100644 source/modulo_new_core/test/test_modulo_new_core.cpp diff --git a/Dockerfile b/Dockerfile index 96b5c511f..1da2173c1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,5 +1,5 @@ -ARG ROS_VERSION=galactic -FROM ghcr.io/aica-technology/ros2-control-libraries:${ROS_VERSION} as dependencies +ARG BASE_TAG=galactic +FROM ghcr.io/aica-technology/ros2-control-libraries:${BASE_TAG} as dependencies # upgrade ament_cmake_python RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-ament-cmake-python && sudo rm -rf /var/lib/apt/lists/* @@ -9,7 +9,8 @@ WORKDIR ${HOME}/ros2_ws FROM dependencies as modulo-core COPY --chown=${USER} ./source/modulo_core ./src/modulo_core -RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build --packages-select modulo_core" +COPY --chown=${USER} ./source/modulo_new_core ./src/modulo_new_core +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build" FROM modulo-core as modulo-components diff --git a/build-server.sh b/build-server.sh index c5ed2baa7..ed9a726d8 100755 --- a/build-server.sh +++ b/build-server.sh @@ -1,5 +1,5 @@ #!/bin/bash -ROS_VERSION=galactic-devel +BASE_TAG=galactic-devel IMAGE_NAME=epfl-lasa/modulo IMAGE_TAG=latest @@ -9,8 +9,11 @@ SERVE_REMOTE=false HELP_MESSAGE="Usage: build.sh [-p] [-r] Options: - -d, --development Only target the modulo-core layer to prevent - modulo_component from being built or tested + -d, --development Only target the dependencies layer to prevent + sources from being built or tested + + -c, --core Only target the modulo core layer to prevent + modulo components from being built or tested -r, --rebuild Rebuild the image(s) using the docker --no-cache option @@ -21,11 +24,12 @@ Options: -s, --serve Start the remove development server " -BUILD_FLAGS=(--build-arg ROS_VERSION="${ROS_VERSION}") +BUILD_FLAGS=(--build-arg BASE_TAG="${BASE_TAG}") while [[ $# -gt 0 ]]; do opt="$1" case $opt in - -d|--development) BUILD_FLAGS+=(--target modulo-core) ; IMAGE_TAG=development ; shift ;; + -d|--development) BUILD_FLAGS+=(--target dependencies) ; IMAGE_TAG=development ; shift ;; + -c|--core) BUILD_FLAGS+=(--target modulo-core) ; IMAGE_TAG=development ; shift ;; -r|--rebuild) BUILD_FLAGS+=(--no-cache) ; shift ;; -v|--verbose) BUILD_FLAGS+=(--progress=plain) ; shift ;; -s|--serve) SERVE_REMOTE=true ; shift ;; @@ -36,7 +40,7 @@ while [[ $# -gt 0 ]]; do esac done -docker pull ghcr.io/aica-technology/ros2-control-libraries:"${ROS_VERSION}" +docker pull ghcr.io/aica-technology/ros2-control-libraries:"${BASE_TAG}" DOCKER_BUILDKIT=1 docker build -t "${IMAGE_NAME}:${IMAGE_TAG}" "${BUILD_FLAGS[@]}" . || exit 1 if [ "${SERVE_REMOTE}" = true ]; then diff --git a/source/modulo_components/include/modulo_components/Component.h b/source/modulo_components/include/modulo_components/Component.h index 6a694f52b..3fdb58673 100644 --- a/source/modulo_components/include/modulo_components/Component.h +++ b/source/modulo_components/include/modulo_components/Component.h @@ -1,11 +1,11 @@ #pragma once #include "modulo_components/ComponentInterface.h" -#include "modulo_core/communication/EncodedState.hpp" +#include "modulo_new_core/EncodedState.hpp" namespace modulo_components { -class Component : public ComponentInterface> { +class Component : public ComponentInterface> { }; diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.h b/source/modulo_components/include/modulo_components/LifecycleComponent.h index cc1e85107..25facb0e8 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.h +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.h @@ -1,12 +1,12 @@ #pragma once #include "modulo_components/ComponentInterface.h" -#include "modulo_core/communication/EncodedState.hpp" +#include "modulo_new_core/EncodedState.hpp" namespace modulo_components { class LifecycleComponent : public ComponentInterface> { + rclcpp_lifecycle::LifecyclePublisher> { }; diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index 109a3d970..da223ef97 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -17,6 +17,7 @@ rclcpp_components rcutils modulo_core + modulo_new_core launch_ros diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index 9d3249976..ccd364b91 100644 --- a/source/modulo_core/CMakeLists.txt +++ b/source/modulo_core/CMakeLists.txt @@ -28,8 +28,6 @@ find_library(controllers REQUIRED) find_library(robot_model REQUIRED) set(CORE_SOURCES - src/communication/message_passing/ReadStateConversion.cpp - src/communication/message_passing/WriteStateConversion.cpp src/communication/message_passing/TransformListenerHandler.cpp src/communication/message_passing/TransformBroadcasterHandler.cpp src/communication/message_passing/MessagePassingHandler.cpp diff --git a/source/modulo_core/examples/cartesianCircular.cpp b/source/modulo_core/examples/cartesianCircular.cpp index 5e8bc512f..0641e81d6 100644 --- a/source/modulo_core/examples/cartesianCircular.cpp +++ b/source/modulo_core/examples/cartesianCircular.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include diff --git a/source/modulo_core/examples/helicoidal.cpp b/source/modulo_core/examples/helicoidal.cpp index ce3f88b23..a9d9cc5b5 100644 --- a/source/modulo_core/examples/helicoidal.cpp +++ b/source/modulo_core/examples/helicoidal.cpp @@ -38,8 +38,8 @@ class MotionGenerator : public Cell { } bool on_configure() { - this->add_subscription("/robot_test/pose", this->current_pose); - this->add_publisher("/ds/desired_twist", this->desired_twist); + this->add_subscription("/robot_test/pose", this->current_pose); + this->add_publisher("/ds/desired_twist", this->desired_twist); return true; } @@ -74,8 +74,8 @@ class ConsoleVisualizer : public Cell { desired_twist(std::make_shared("robot_test")) {} bool on_configure() { - this->add_subscription("/robot_test/pose", this->robot_pose); - this->add_subscription("/ds/desired_twist", this->desired_twist); + this->add_subscription("/robot_test/pose", this->robot_pose); + this->add_subscription("/ds/desired_twist", this->desired_twist); return true; } @@ -106,8 +106,8 @@ class SimulatedRobotInterface : public Cell { dt(period) {} bool on_configure() { - this->add_subscription("/ds/desired_twist", this->desired_twist); - this->add_publisher("/robot_test/pose", this->robot_pose); + this->add_subscription("/ds/desired_twist", this->desired_twist); + this->add_publisher("/robot_test/pose", this->robot_pose); return true; } diff --git a/source/modulo_core/examples/movingReferenceFrame.cpp b/source/modulo_core/examples/movingReferenceFrame.cpp index daf964d32..d9131dc3b 100644 --- a/source/modulo_core/examples/movingReferenceFrame.cpp +++ b/source/modulo_core/examples/movingReferenceFrame.cpp @@ -2,6 +2,7 @@ #include #include +#include #include #include #include diff --git a/source/modulo_core/include/modulo_core/Cell.hpp b/source/modulo_core/include/modulo_core/Cell.hpp index ad37c0353..b52b7f523 100644 --- a/source/modulo_core/include/modulo_core/Cell.hpp +++ b/source/modulo_core/include/modulo_core/Cell.hpp @@ -16,6 +16,7 @@ #include #include +#include "modulo_new_core/EncodedState.hpp" #include "modulo_core/communication/message_passing/PublisherHandler.hpp" #include "modulo_core/communication/message_passing/SubscriptionHandler.hpp" #include "modulo_core/communication/message_passing/TransformBroadcasterHandler.hpp" @@ -622,7 +623,7 @@ void Cell::add_state_publisher(const std::string& channel, const std::chrono::duration& period, bool always_active, int queue_size) { - this->add_publisher(channel, recipient, period, always_active, queue_size); + this->add_publisher(channel, recipient, period, always_active, queue_size); } template @@ -630,7 +631,7 @@ void Cell::add_state_publisher(const std::string& channel, const std::shared_ptr& recipient, bool always_active, int queue_size) { - this->add_publisher(channel, recipient, always_active, queue_size); + this->add_publisher(channel, recipient, always_active, queue_size); } template @@ -662,7 +663,7 @@ void Cell::add_state_subscription(const std::string& channel, const std::chrono::duration& timeout, bool always_active, int queue_size) { - this->add_subscription(channel, recipient, timeout, always_active, queue_size); + this->add_subscription(channel, recipient, timeout, always_active, queue_size); } template @@ -671,7 +672,7 @@ void Cell::add_state_subscription(const std::string& channel, bool always_active, unsigned int nb_period_to_timeout, int queue_size) { - this->add_subscription(channel, recipient, nb_period_to_timeout, always_active, queue_size); + this->add_subscription(channel, recipient, nb_period_to_timeout, always_active, queue_size); } template diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp index 760de08dc..eb1788612 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp @@ -6,8 +6,8 @@ #include #include "modulo_core/communication/CommunicationHandler.hpp" -#include "modulo_core/communication/message_passing/ReadStateConversion.hpp" -#include "modulo_core/communication/message_passing/WriteStateConversion.hpp" +#include "modulo_new_core/translators/ReadStateConversion.hpp" +#include "modulo_new_core/translators/WriteStateConversion.hpp" namespace modulo::core::communication { /** diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp index f08618ca3..1ace75818 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp @@ -99,7 +99,7 @@ PublisherHandler::PublisherHandler(const std::shared_ptr void PublisherHandler::publish(const RecT& recipient) { auto out_msg = std::make_unique(); - state_conversion::write_msg(*out_msg, recipient, this->get_clock().now()); + modulo_new_core::translators::write_msg(*out_msg, recipient, this->get_clock().now()); this->publisher_->publish(std::move(out_msg)); } diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp index 1c2893576..c2b7091fc 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp @@ -62,7 +62,7 @@ SubscriptionHandler::SubscriptionHandler(const std::shared_ptr void SubscriptionHandler::subscription_callback(const std::shared_ptr msg) { - state_conversion::read_msg(static_cast(this->get_recipient()), *msg); + modulo_new_core::translators::read_msg(static_cast(this->get_recipient()), *msg); } template diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index ea98d3c09..110a3d28e 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -19,6 +19,7 @@ lifecycle_msgs tf2_msgs tf2_ros + modulo_new_core ament_lint_auto ament_lint_common diff --git a/source/modulo_core/src/Cell.cpp b/source/modulo_core/src/Cell.cpp index dc8d3eeaf..3d12b0fea 100644 --- a/source/modulo_core/src/Cell.cpp +++ b/source/modulo_core/src/Cell.cpp @@ -2,6 +2,7 @@ #include +#include #include #include #include diff --git a/source/modulo_core/src/Component.cpp b/source/modulo_core/src/Component.cpp index 02acff68b..3c549f65e 100644 --- a/source/modulo_core/src/Component.cpp +++ b/source/modulo_core/src/Component.cpp @@ -3,6 +3,8 @@ #include "modulo_core/exceptions/PredicateAlreadyRegisteredException.hpp" #include "modulo_core/exceptions/PredicateNotFoundException.hpp" +#include + namespace modulo::core { void Component::on_init() { diff --git a/source/modulo_core/src/communication/message_passing/ReadStateConversion.cpp b/source/modulo_core/src/communication/message_passing/ReadStateConversion.cpp deleted file mode 100644 index 09071d00f..000000000 --- a/source/modulo_core/src/communication/message_passing/ReadStateConversion.cpp +++ /dev/null @@ -1,116 +0,0 @@ -#include "modulo_core/communication/message_passing/ReadStateConversion.hpp" - -#include -#include - -using namespace state_representation::exceptions; - -namespace modulo::core::communication::state_conversion { -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& msg) { - // transform messages - Eigen::Vector3d position(msg.position.x, msg.position.y, msg.position.z); - Eigen::Quaterniond orientation(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); - // read_msg the state - state.set_position(position); - state.set_orientation(orientation); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.pose); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& msg) { - // transform messages - Eigen::Vector3d position(msg.translation.x, msg.translation.y, msg.translation.z); - Eigen::Quaterniond orientation(msg.rotation.w, msg.rotation.x, msg.rotation.y, msg.rotation.z); - // read_msg the state - state.set_position(position); - state.set_orientation(orientation); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - state.set_name(msg.child_frame_id); - read_msg(state, msg.transform); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& msg) { - // transform messages - Eigen::Vector3d linear_velocity(msg.linear.x, msg.linear.y, msg.linear.z); - Eigen::Vector3d angular_velocity(msg.angular.x, msg.angular.y, msg.angular.z); - // read_msg the state - state.set_linear_velocity(linear_velocity); - state.set_angular_velocity(angular_velocity); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.twist); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& msg) { - // transform messages - Eigen::Vector3d linear_acceleration(msg.linear.x, msg.linear.y, msg.linear.z); - Eigen::Vector3d angular_acceleration(msg.angular.x, msg.angular.y, msg.angular.z); - // read_msg the state - state.set_linear_acceleration(linear_acceleration); - state.set_angular_acceleration(angular_acceleration); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.accel); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& msg) { - // transform messages - Eigen::Vector3d force(msg.force.x, msg.force.y, msg.force.z); - Eigen::Vector3d torque(msg.torque.x, msg.torque.y, msg.torque.z); - // read_msg the state - state.set_force(force); - state.set_torque(torque); -} - -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.wrench); -} - -void read_msg(state_representation::CartesianState& state, const nav_msgs::msg::Odometry& msg) { - state.set_reference_frame(msg.header.frame_id); - // and odometry message contains a pose with uncertainty - read_msg(state, msg.pose.pose); - //and a twist with uncertainty - read_msg(state, msg.twist.twist); -} - -void read_msg(state_representation::JointState& state, const sensor_msgs::msg::JointState& msg) { - state.set_names(msg.name); - if (!msg.position.empty()) state.set_positions(Eigen::VectorXd::Map(msg.position.data(), msg.position.size())); - if (!msg.velocity.empty()) state.set_velocities(Eigen::VectorXd::Map(msg.velocity.data(), msg.velocity.size())); - if (!msg.effort.empty()) state.set_torques(Eigen::VectorXd::Map(msg.effort.data(), msg.effort.size())); -} - -void read_msg(state_representation::DualQuaternionPose& state, const geometry_msgs::msg::Pose& msg) { - Eigen::Quaterniond orientation(msg.orientation.w, msg.orientation.x, msg.orientation.y, msg.orientation.z); - Eigen::Vector3d position(msg.position.x, msg.position.y, msg.position.z); - state.set_orientation(orientation); - state.set_position(position); -} - -void read_msg(state_representation::DualQuaternionPose& state, const geometry_msgs::msg::PoseStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.pose); -} - -void read_msg(state_representation::DualQuaternionTwist& state, const geometry_msgs::msg::Twist& msg) { - state.set_linear_velocity(Eigen::Vector3d(msg.linear.x, msg.linear.y, msg.linear.z)); - state.set_angular_velocity(Eigen::Vector3d(msg.angular.x, msg.angular.y, msg.angular.z)); -} - -void read_msg(state_representation::DualQuaternionTwist& state, const geometry_msgs::msg::TwistStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.twist); -} -}// namespace modulo::core::communication::state_conversion diff --git a/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp b/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp index 971fc67d5..d4f71c1c1 100644 --- a/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp +++ b/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp @@ -9,7 +9,7 @@ const state_representation::CartesianPose TransformListenerHandler::lookup_trans frame_name, tf2::TimePoint(std::chrono::milliseconds(0)), tf2::Duration(this->get_timeout())); - state_conversion::read_msg(result, transformStamped); + modulo_new_core::translators::read_msg(result, transformStamped); return result; } }// namespace modulo::core::communication diff --git a/source/modulo_core/src/communication/message_passing/WriteStateConversion.cpp b/source/modulo_core/src/communication/message_passing/WriteStateConversion.cpp deleted file mode 100644 index 2ab49e88c..000000000 --- a/source/modulo_core/src/communication/message_passing/WriteStateConversion.cpp +++ /dev/null @@ -1,216 +0,0 @@ -#include "modulo_core/communication/message_passing/WriteStateConversion.hpp" - -#include - -using namespace state_representation::exceptions; - -namespace modulo::core::communication::state_conversion { -void write_msg(geometry_msgs::msg::Quaternion& msg, const state_representation::CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - // orientation - msg.w = state.get_orientation().w(); - msg.x = state.get_orientation().x(); - msg.y = state.get_orientation().y(); - msg.z = state.get_orientation().z(); -} - -void write_msg(geometry_msgs::msg::Pose& msg, const state_representation::CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - // position - msg.position.x = state.get_position()(0); - msg.position.y = state.get_position()(1); - msg.position.z = state.get_position()(2); - // orientation - msg.orientation.w = state.get_orientation().w(); - msg.orientation.x = state.get_orientation().x(); - msg.orientation.y = state.get_orientation().y(); - msg.orientation.z = state.get_orientation().z(); -} - -void write_msg(geometry_msgs::msg::PoseStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time) { - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - write_msg(msg.pose, state, time); -} - -void write_msg(geometry_msgs::msg::Transform& msg, const state_representation::CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - // position - msg.translation.x = state.get_position()(0); - msg.translation.y = state.get_position()(1); - msg.translation.z = state.get_position()(2); - // orientation - msg.rotation.w = state.get_orientation().w(); - msg.rotation.x = state.get_orientation().x(); - msg.rotation.y = state.get_orientation().y(); - msg.rotation.z = state.get_orientation().z(); -} - -void write_msg(geometry_msgs::msg::TransformStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time) { - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - msg.child_frame_id = state.get_name(); - write_msg(msg.transform, state, time); -} - -void write_msg(geometry_msgs::msg::Twist& msg, const state_representation::CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - // linear velocity - msg.linear.x = state.get_linear_velocity()(0); - msg.linear.y = state.get_linear_velocity()(1); - msg.linear.z = state.get_linear_velocity()(2); - // angular velocity - msg.angular.x = state.get_angular_velocity()(0); - msg.angular.y = state.get_angular_velocity()(1); - msg.angular.z = state.get_angular_velocity()(2); -} - -void write_msg(geometry_msgs::msg::TwistStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time) { - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - write_msg(msg.twist, state, time); -} - -void write_msg(geometry_msgs::msg::Accel& msg, const state_representation::CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - // linear acceleration - msg.linear.x = state.get_linear_acceleration()(0); - msg.linear.y = state.get_linear_acceleration()(1); - msg.linear.z = state.get_linear_acceleration()(2); - // angular acceleration - msg.angular.x = state.get_angular_acceleration()(0); - msg.angular.y = state.get_angular_acceleration()(1); - msg.angular.z = state.get_angular_acceleration()(2); -} - -void write_msg(geometry_msgs::msg::AccelStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time) { - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - write_msg(msg.accel, state, time); -} - -void write_msg(geometry_msgs::msg::Wrench& msg, const state_representation::CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - // force - msg.force.x = state.get_force()(0); - msg.force.y = state.get_force()(1); - msg.force.z = state.get_force()(2); - // torque - msg.torque.x = state.get_torque()(0); - msg.torque.y = state.get_torque()(1); - msg.torque.z = state.get_torque()(2); -} - -void write_msg(geometry_msgs::msg::WrenchStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time) { - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - write_msg(msg.wrench, state, time); -} - -void write_msg(sensor_msgs::msg::JointState& msg, const state_representation::JointState& state, const rclcpp::Time& time) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - msg.header.stamp = time; - msg.name = state.get_names(); - msg.position = std::vector(state.get_positions().data(), state.get_positions().data() + state.get_positions().size()); - msg.velocity = std::vector(state.get_velocities().data(), state.get_velocities().data() + state.get_velocities().size()); - msg.effort = std::vector(state.get_torques().data(), state.get_torques().data() + state.get_torques().size()); -} - -void write_msg(geometry_msgs::msg::Pose& msg, const state_representation::DualQuaternionPose& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - // position - msg.position.x = state.get_position()(0); - msg.position.y = state.get_position()(1); - msg.position.z = state.get_position()(2); - // orientation - msg.orientation.w = state.get_orientation().w(); - msg.orientation.x = state.get_orientation().x(); - msg.orientation.y = state.get_orientation().y(); - msg.orientation.z = state.get_orientation().z(); -} - -void write_msg(geometry_msgs::msg::PoseStamped& msg, const state_representation::DualQuaternionPose& state, const rclcpp::Time& time) { - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - write_msg(msg.pose, state, time); -} - -void write_msg(geometry_msgs::msg::Twist& msg, const state_representation::DualQuaternionTwist& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - Eigen::Vector3d linear = state.get_linear_velocity(); - Eigen::Vector3d angular = state.get_angular_velocity(); - - msg.linear.x = linear(0); - msg.linear.y = linear(1); - msg.linear.z = linear(2); - - msg.angular.x = angular(0); - msg.angular.y = angular(1); - msg.angular.z = angular(2); -} - -void write_msg(geometry_msgs::msg::TwistStamped& msg, const state_representation::DualQuaternionTwist& state, const rclcpp::Time& time) { - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - write_msg(msg.twist, state, time); -} - -void write_msg(tf2_msgs::msg::TFMessage& msg, const state_representation::CartesianState& state, const rclcpp::Time& time) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - geometry_msgs::msg::TransformStamped transform; - write_msg(transform, state, time); - msg.transforms.push_back(transform); -} - -void write_msg(trajectory_msgs::msg::JointTrajectoryPoint& msg, const state_representation::JointState& state, const rclcpp::Time&) { - msg.positions = std::vector(state.get_positions().data(), state.get_positions().data() + state.get_positions().size()); - msg.velocities = std::vector(state.get_velocities().data(), state.get_velocities().data() + state.get_velocities().size()); - msg.accelerations = std::vector(state.get_accelerations().data(), state.get_accelerations().data() + state.get_accelerations().size()); - msg.effort = std::vector(state.get_torques().data(), state.get_torques().data() + state.get_torques().size()); -} - -void write_msg(trajectory_msgs::msg::JointTrajectory& msg, const state_representation::Trajectory& state, const rclcpp::Time& time) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - msg.header.frame_id = state.get_name(); - msg.joint_names = state.get_joint_names(); - - int trajectory_size = state.get_size(); - msg.points.resize(trajectory_size); - - for (int i = 0; i < trajectory_size; i++) { - auto point = state[i]; - write_msg(msg.points[i], point.first, time); - msg.points[i].time_from_start = rclcpp::Duration(point.second); - } -} - -template -void write_msg(U& msg, const state_representation::Parameter& state, const rclcpp::Time&) { - using namespace state_representation::exceptions; - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - msg.data = state.get_value(); -} - -template void write_msg(std_msgs::msg::Float64& msg, const state_representation::Parameter& state, const rclcpp::Time&); - -template void write_msg>(std_msgs::msg::Float64MultiArray& msg, const state_representation::Parameter>& state, const rclcpp::Time&); - -template void write_msg(std_msgs::msg::Bool& msg, const state_representation::Parameter& state, const rclcpp::Time&); - -template void write_msg(std_msgs::msg::String& msg, const state_representation::Parameter& state, const rclcpp::Time&); - -template <> -void write_msg(geometry_msgs::msg::Transform& msg, const state_representation::Parameter& state, const rclcpp::Time& time) { - write_msg(msg, state.get_value(), time); -} - -template <> -void write_msg(geometry_msgs::msg::TransformStamped& msg, const state_representation::Parameter& state, const rclcpp::Time& time) { - write_msg(msg, state.get_value(), time); -} - -template <> -void write_msg(tf2_msgs::msg::TFMessage& msg, const state_representation::Parameter& state, const rclcpp::Time& time) { - write_msg(msg, state.get_value(), time); -} -}// namespace modulo::core::communication::state_conversion diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt new file mode 100644 index 000000000..28537c8f6 --- /dev/null +++ b/source/modulo_new_core/CMakeLists.txt @@ -0,0 +1,60 @@ +cmake_minimum_required(VERSION 3.15) +project(modulo_new_core) + +# default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake_python REQUIRED) + +ament_auto_find_build_dependencies() + +find_package(control_libraries 5.0.0 REQUIRED COMPONENTS state_representation) +find_library(clproto REQUIRED) + +include_directories(include) + +ament_auto_add_library(modulo_new_core SHARED + ${PROJECT_SOURCE_DIR}/src/translators/ReadStateConversion.cpp + ${PROJECT_SOURCE_DIR}/src/translators/WriteStateConversion.cpp) + +# install Python modules +ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + # run cpp tests + file(GLOB_RECURSE TEST_CPP_SOURCES ${PROJECT_SOURCE_DIR}/test/test_*.cpp) + + ament_add_gtest(test_modulo_new_core ${TEST_CPP_SOURCES}) + target_include_directories(test_modulo_new_core PRIVATE include) + target_link_libraries(test_modulo_new_core modulo_new_core state_representation clproto) + + # prevent pluginlib from using boost + target_compile_definitions(test_modulo_new_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + + # run python tests + file(GLOB_RECURSE TEST_PYTHON_SOURCES ${PROJECT_SOURCE_DIR}/test/test_*.py) + + foreach(TEST_PYTHON IN LISTS TEST_PYTHON_SOURCES) + get_filename_component(TEST_FILENAME ${TEST_PYTHON} NAME_WE) + ament_add_pytest_test(${TEST_FILENAME} ${TEST_PYTHON}) + endforeach() + +endif() + +ament_auto_package() diff --git a/source/modulo_new_core/README.md b/source/modulo_new_core/README.md new file mode 100644 index 000000000..31183b9a5 --- /dev/null +++ b/source/modulo_new_core/README.md @@ -0,0 +1 @@ +# aica_msgs \ No newline at end of file diff --git a/source/modulo_core/include/modulo_core/communication/EncodedState.hpp b/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp similarity index 80% rename from source/modulo_core/include/modulo_core/communication/EncodedState.hpp rename to source/modulo_new_core/include/modulo_new_core/EncodedState.hpp index 8f44f9b6f..a3b3709f7 100644 --- a/source/modulo_core/include/modulo_core/communication/EncodedState.hpp +++ b/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp @@ -2,6 +2,6 @@ #include -namespace modulo::core { +namespace modulo_new_core { typedef std_msgs::msg::UInt8MultiArray EncodedState; } \ No newline at end of file diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/ReadStateConversion.hpp b/source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp similarity index 71% rename from source/modulo_core/include/modulo_core/communication/message_passing/ReadStateConversion.hpp rename to source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp index 213ed245d..421520a42 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/ReadStateConversion.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp @@ -1,8 +1,5 @@ #pragma once -#include -#include -#include #include #include #include @@ -10,26 +7,19 @@ #include #include #include -#include -#include -#include #include -#include -#include +#include #include -#include #include #include #include #include -#include -#include -#include +#include -#include "modulo_core/communication/EncodedState.hpp" +#include "modulo_new_core/EncodedState.hpp" -namespace modulo::core::communication::state_conversion { +namespace modulo_new_core::translators { /** * @brief Convert a ROS geometry_msgs::msg::Pose to a CartesianState * @param state The CartesianState to populate @@ -114,34 +104,6 @@ void read_msg(state_representation::CartesianState& state, const nav_msgs::msg:: */ void read_msg(state_representation::JointState& state, const sensor_msgs::msg::JointState& msg); -/** - * @brief Convert a ROS geometry_msgs::msg::Pose to a DualQuaternionPose - * @param state The DualQuaternionPose to populate - * @param msg The ROS msg to read from - */ -void read_msg(state_representation::DualQuaternionPose& state, const geometry_msgs::msg::Pose& msg); - -/** - * @brief Convert a ROS geometry_msgs::msg::PoseStamped to a DualQuaternionPose - * @param state The DualQuaternionPose to populate - * @param msg The ROS msg to read from - */ -void read_msg(state_representation::DualQuaternionPose& state, const geometry_msgs::msg::PoseStamped& msg); - -/** - * @brief Convert a ROS geometry_msgs::msg::Twist to a DualQuaternionTwist - * @param state The DualQuaternionPose to populate - * @param msg The ROS msg to read from - */ -void read_msg(state_representation::DualQuaternionTwist& state, const geometry_msgs::msg::Twist& msg); - -/** - * @brief Convert a ROS geometry_msgs::msg::TwistStamped to a DualQuaternionTwist - * @param state The DualQuaternionPose to populate - * @param msg The ROS msg to read from - */ -void read_msg(state_representation::DualQuaternionTwist& state, const geometry_msgs::msg::TwistStamped& msg); - /** * @brief Template function to convert a ROS std_msgs::msg::T to a Parameter * @tparam T all types of parameters supported in ROS std messages @@ -155,7 +117,7 @@ void read_msg(state_representation::Parameter& state, const U& msg) { } /** - * @brief Convert an EncodedState type message to a State using protobuf decoding + * @brief Convert a ROS std_msgs::msg::UInt8MultiArray message to a State using protobuf decoding * @tparam a state_representation::State type object * @param state The state to populate * @param msg The ROS msg to read from @@ -165,4 +127,4 @@ void read_msg(T& state, const EncodedState& msg) { std::string tmp(msg.data.begin(), msg.data.end()); state = clproto::decode(tmp); } -}// namespace modulo::core::communication::state_conversion \ No newline at end of file +}// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/WriteStateConversion.hpp b/source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp similarity index 63% rename from source/modulo_core/include/modulo_core/communication/message_passing/WriteStateConversion.hpp rename to source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp index 4856efc25..034558188 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/WriteStateConversion.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp @@ -1,38 +1,42 @@ #pragma once -#include #include -#include #include #include #include #include #include -#include #include -#include -#include -#include -#include #include -#include -#include -#include -#include +#include #include +#include #include #include #include #include #include -#include -#include -#include -#include "modulo_core/communication/EncodedState.hpp" +#include "modulo_new_core/EncodedState.hpp" + +namespace modulo_new_core::translators { +/** + * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Accel + * @param msg The ROS msg to populate + * @param state The state to read from + * @param time The time of the message + */ +void write_msg(geometry_msgs::msg::Accel& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); + +/** + * @brief Convert a CartesianState to a ROS geometry_msgs::msg::AccelStamped + * @param msg The ROS msg to populate + * @param state The state to read from + * @param time The time of the message + */ +void write_msg(geometry_msgs::msg::AccelStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); -namespace modulo::core::communication::state_conversion { /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Quaternion * @param msg The ROS msg to populate @@ -89,22 +93,6 @@ void write_msg(geometry_msgs::msg::Twist& msg, const state_representation::Carte */ void write_msg(geometry_msgs::msg::TwistStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); -/** - * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Accel - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(geometry_msgs::msg::Accel& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); - -/** - * @brief Convert a CartesianState to a ROS geometry_msgs::msg::AccelStamped - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(geometry_msgs::msg::AccelStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); - /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Wrench * @param msg The ROS msg to populate @@ -129,38 +117,6 @@ void write_msg(geometry_msgs::msg::WrenchStamped& msg, const state_representatio */ void write_msg(sensor_msgs::msg::JointState& msg, const state_representation::JointState& state, const rclcpp::Time& time); -/** - * @brief Convert a DualQuaternionPose to a ROS geometry_msgs::msg::Pose - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(geometry_msgs::msg::Pose& msg, const state_representation::DualQuaternionPose& state, const rclcpp::Time& time); - -/** - * @brief Convert a DualQuaternionPose to a ROS geometry_msgs::msg::PoseStamped - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(geometry_msgs::msg::PoseStamped& msg, const state_representation::DualQuaternionPose& state, const rclcpp::Time& time); - -/** - * @brief Convert a DualQuaternionPose to a ROS geometry_msgs::msg::Twist - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(geometry_msgs::msg::Twist& msg, const state_representation::DualQuaternionTwist& state, const rclcpp::Time& time); - -/** - * @brief Convert a DualQuaternionPose to a ROS geometry_msgs::msg::TwistStamped - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(geometry_msgs::msg::TwistStamped& msg, const state_representation::DualQuaternionTwist& state, const rclcpp::Time& time); - /** * @brief Convert a CartesianState to a ROS tf2_msgs::msg::TFMessage * @param msg The ROS msg to populate @@ -169,22 +125,6 @@ void write_msg(geometry_msgs::msg::TwistStamped& msg, const state_representation */ void write_msg(tf2_msgs::msg::TFMessage& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); -/** - * @brief Convert a JointState to a ROS trajectory_msgs::msg::JointTrajectoryPoint - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(trajectory_msgs::msg::JointTrajectoryPoint& msg, const state_representation::JointState& state, const rclcpp::Time&); - -/** - * @brief Convert a JointState to a ROS trajectory_msgs::msg::JointTrajectory - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(trajectory_msgs::msg::JointTrajectory& msg, const state_representation::Trajectory& state, const rclcpp::Time& time); - /** * @brief Convert a Parameter to a ROS equivalent representation * @tparam T all types of parameters supported in ROS std messages @@ -197,20 +137,7 @@ template void write_msg(U& msg, const state_representation::Parameter& state, const rclcpp::Time&); /** - * @brief Convert a state to a ROS std_msgs::msg::Float64MultiArray - * @tparam a state_representation::State type object - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -template -void write_msg(std_msgs::msg::Float64MultiArray& msg, const T& state, const rclcpp::Time&) { - if (state.is_empty()) throw EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - msg.data = state.to_std_vector(); -} - -/** - * @brief Convert a state to an EncodedState type message using protobuf encoding + * @brief Convert a state to a ROS std_msgs::msg::UInt8MultiArray message using protobuf encoding * @tparam a state_representation::State type object * @param msg The ROS msg to populate * @param state The state to read from @@ -221,4 +148,4 @@ void write_msg(EncodedState& msg, const T& state, const rclcpp::Time&) { std::string tmp = clproto::encode(state); msg.data = std::vector(tmp.begin(), tmp.end()); } -}// namespace modulo::core::communication::state_conversion +}// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/modulo_new_core/__init__.py b/source/modulo_new_core/modulo_new_core/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_new_core/modulo_new_core/encoded_state.py b/source/modulo_new_core/modulo_new_core/encoded_state.py new file mode 100644 index 000000000..9b3e8fe4b --- /dev/null +++ b/source/modulo_new_core/modulo_new_core/encoded_state.py @@ -0,0 +1,3 @@ +from std_msgs.msg import UInt8MultiArray + +EncodedState = UInt8MultiArray diff --git a/source/modulo_new_core/modulo_new_core/translators/__init__.py b/source/modulo_new_core/modulo_new_core/translators/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py b/source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py new file mode 100644 index 000000000..2d39687ce --- /dev/null +++ b/source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py @@ -0,0 +1,72 @@ +from typing import List + +import clproto +import geometry_msgs.msg as geometry +import sensor_msgs.msg +from modulo_new_core.encoded_state import EncodedState + + +def __read_xyz(msg) -> List[float]: + return [msg.x, msg.y, msg.z] + + +def __read_quaternion(msg: geometry.Quaternion) -> List[float]: + return [msg.w, msg.x, msg.y, msg.z] + + +def read_msg(state, msg): + if isinstance(msg, geometry.Accel): + state.set_linear_acceleration(__read_xyz(msg.linear)) + state.set_angular_acceleration(__read_xyz(msg.angular)) + elif isinstance(msg, geometry.Pose): + state.set_position(__read_xyz(msg.position)) + state.set_orientation(__read_quaternion(msg.orientation)) + elif isinstance(msg, geometry.Transform): + state.set_position(__read_xyz(msg.translation)) + state.set_orientation(__read_quaternion(msg.rotation)) + elif isinstance(msg, geometry.Twist): + state.set_linear_velocity(__read_xyz(msg.linear)) + state.set_angular_velocity(__read_xyz(msg.angular)) + elif isinstance(msg, geometry.Wrench): + state.set_force(__read_xyz(msg.force)) + state.set_torque(__read_xyz(msg.torque)) + elif isinstance(msg, sensor_msgs.msg.JointState): + state.set_names(msg.name) + if len(msg.position): + state.set_positions(msg.position) + if len(msg.velocity): + state.set_velocities(msg.velocity) + if len(msg.effort): + state.set_torques(msg.effort) + else: + raise RuntimeError("This message type is not supported") + return state + + +def read_msg_stamped(state, msg): + if isinstance(msg, geometry.AccelStamped): + read_msg(state, msg.accel) + state.set_reference_frame(msg.header.frame_id) + elif isinstance(msg, geometry.PoseStamped): + read_msg(state, msg.pose) + state.set_reference_frame(msg.header.frame_id) + elif isinstance(msg, geometry.TransformStamped): + read_msg(state, msg.transform) + state.set_reference_frame(msg.header.frame_id) + state.set_name(msg.child_frame_id) + elif isinstance(msg, geometry.TwistStamped): + read_msg(state, msg.twist) + state.set_reference_frame(msg.header.frame_id) + elif isinstance(msg, geometry.WrenchStamped): + read_msg(state, msg.wrench) + state.set_reference_frame(msg.header.frame_id) + else: + raise RuntimeError("This message type is not supported") + return state + + +def read_clproto_msg(msg: EncodedState): + if isinstance(msg, EncodedState): + return clproto.decode(msg.data.tobytes()) + else: + raise RuntimeError("Your input does not have the correct type") diff --git a/source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py b/source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py new file mode 100644 index 000000000..be3b22a1d --- /dev/null +++ b/source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py @@ -0,0 +1,94 @@ +import clproto +import geometry_msgs.msg as geometry +import numpy as np +import rclpy.time +import sensor_msgs.msg +import state_representation as sr +from modulo_new_core.encoded_state import EncodedState + + +def __write_xyz(msg, vector: np.array): + msg.x = vector[0] + msg.y = vector[1] + msg.z = vector[2] + return msg + + +def __write_quaternion(msg: geometry.Quaternion, quat: np.array) -> geometry.Quaternion: + msg.w = quat[0] + msg.x = quat[1] + msg.y = quat[2] + msg.z = quat[3] + return msg + + +def write_msg(msg, state): + if not isinstance(state, sr.State): + raise RuntimeError("This state type is not supported") + if state.is_empty(): + raise RuntimeError(f"{state.get_name()} state is empty while attempting to write it to message") + if isinstance(msg, geometry.Point) and isinstance(state, sr.CartesianState): + __write_xyz(msg, state) + elif isinstance(msg, geometry.Vector3) and isinstance(state, sr.CartesianState): + __write_xyz(msg, state) + elif isinstance(msg, geometry.Quaternion) and isinstance(state, sr.CartesianState): + __write_quaternion(msg, state) + elif isinstance(msg, geometry.Accel) and isinstance(state, sr.CartesianState): + __write_xyz(msg.linear, state.get_linear_acceleration()) + __write_xyz(msg.angular, state.get_angular_acceleration()) + elif isinstance(msg, geometry.Pose) and isinstance(state, sr.CartesianState): + __write_xyz(msg.position, state.get_position()) + __write_quaternion(msg.orientation, state.get_orientation()) + elif isinstance(msg, geometry.Transform) and isinstance(state, sr.CartesianState): + __write_xyz(msg.translation, state.get_position()) + __write_quaternion(msg.rotation, state.get_orientation()) + elif isinstance(msg, geometry.Twist) and isinstance(state, sr.CartesianState): + __write_xyz(msg.linear, state.get_linear_velocity()) + __write_xyz(msg.angular, state.get_angular_velocity()) + elif isinstance(msg, geometry.Wrench) and isinstance(state, sr.CartesianState): + __write_xyz(msg.force, state.get_force()) + __write_xyz(msg.torque, state.get_torque()) + elif isinstance(msg, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): + msg.name = state.get_names() + msg.position = state.get_positions().tolist() + msg.velocity = state.get_velocities().tolist() + msg.effort = state.get_torques().tolist() + else: + raise RuntimeError("This message type is not supported") + return msg + + +def write_msg_stamped(msg, state, time: rclpy.time.Time): + if isinstance(msg, geometry.AccelStamped): + write_msg(msg.accel, state) + msg.header.stamp = time + msg.header.frame_id = state.get_reference_frame() + elif isinstance(msg, geometry.PoseStamped): + write_msg(msg.pose, state) + msg.header.stamp = time + msg.header.frame_id = state.get_reference_frame() + elif isinstance(msg, geometry.TransformStamped): + write_msg(msg.transform, state) + msg.header.stamp = time + msg.header.frame_id = state.get_reference_frame() + msg.child_frame_id = state.get_name() + elif isinstance(msg, geometry.TwistStamped): + write_msg(msg.twist, state) + msg.header.stamp = time + msg.header.frame_id = state.get_reference_frame() + elif isinstance(msg, geometry.WrenchStamped): + write_msg(msg.wrench, state) + msg.header.stamp = time + msg.header.frame_id = state.get_reference_frame() + else: + raise RuntimeError("This message type is not supported") + return state + + +def write_clproto_msg(state: sr.State, msg_type: clproto.MessageType) -> EncodedState(): + msg = EncodedState() + if isinstance(state, sr.State) and isinstance(msg_type, clproto.MessageType): + msg.data = clproto.encode(state, msg_type) + else: + raise RuntimeError("Your inputs do not have the correct types") + return msg diff --git a/source/modulo_new_core/package.xml b/source/modulo_new_core/package.xml new file mode 100644 index 000000000..6f78cd4a3 --- /dev/null +++ b/source/modulo_new_core/package.xml @@ -0,0 +1,26 @@ + + + + modulo_new_core + 0.0.1 + TODO + Dominic Reber + TODO: License declaration + + ament_cmake_auto + ament_cmake_python + + rclcpp + rclpy + std_msgs + geometry_msgs + sensor_msgs + tf2_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/source/modulo_new_core/setup.cfg b/source/modulo_new_core/setup.cfg new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_new_core/src/translators/ReadStateConversion.cpp b/source/modulo_new_core/src/translators/ReadStateConversion.cpp new file mode 100644 index 000000000..1138c5d9f --- /dev/null +++ b/source/modulo_new_core/src/translators/ReadStateConversion.cpp @@ -0,0 +1,80 @@ +#include "modulo_new_core/translators/ReadStateConversion.hpp" + +namespace modulo_new_core::translators { + +static Eigen::Vector3d read_point(const geometry_msgs::msg::Point& msg) { + return {msg.x, msg.y, msg.z}; +} + +static Eigen::Vector3d read_vector3(const geometry_msgs::msg::Vector3& msg) { + return {msg.x, msg.y, msg.z}; +} + +static Eigen::Quaterniond read_quaternion(const geometry_msgs::msg::Quaternion& msg) { + return {msg.w, msg.x, msg.y, msg.z}; +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& msg) { + state.set_linear_acceleration(read_vector3(msg.linear)); + state.set_angular_acceleration(read_vector3(msg.angular)); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& msg) { + state.set_reference_frame(msg.header.frame_id); + read_msg(state, msg.accel); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& msg) { + state.set_position(read_point(msg.position)); + state.set_orientation(read_quaternion(msg.orientation)); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& msg) { + state.set_reference_frame(msg.header.frame_id); + read_msg(state, msg.pose); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& msg) { + state.set_position(read_vector3(msg.translation)); + state.set_orientation(read_quaternion(msg.rotation)); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& msg) { + state.set_reference_frame(msg.header.frame_id); + state.set_name(msg.child_frame_id); + read_msg(state, msg.transform); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& msg) { + state.set_linear_velocity(read_vector3(msg.linear)); + state.set_angular_velocity(read_vector3(msg.angular)); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& msg) { + state.set_reference_frame(msg.header.frame_id); + read_msg(state, msg.twist); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& msg) { + state.set_force(read_vector3(msg.force)); + state.set_torque(read_vector3(msg.torque)); +} + +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& msg) { + state.set_reference_frame(msg.header.frame_id); + read_msg(state, msg.wrench); +} + +void read_msg(state_representation::JointState& state, const sensor_msgs::msg::JointState& msg) { + state.set_names(msg.name); + if (!msg.position.empty()) { + state.set_positions(Eigen::VectorXd::Map(msg.position.data(), msg.position.size())); + } + if (!msg.velocity.empty()) { + state.set_velocities(Eigen::VectorXd::Map(msg.velocity.data(), msg.velocity.size())); + } + if (!msg.effort.empty()) { + state.set_torques(Eigen::VectorXd::Map(msg.effort.data(), msg.effort.size())); + } +} +}// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/src/translators/WriteStateConversion.cpp b/source/modulo_new_core/src/translators/WriteStateConversion.cpp new file mode 100644 index 000000000..ee312653c --- /dev/null +++ b/source/modulo_new_core/src/translators/WriteStateConversion.cpp @@ -0,0 +1,174 @@ +#include "modulo_new_core/translators/WriteStateConversion.hpp" + +#include +#include +#include +#include + +using namespace state_representation; + +namespace modulo_new_core::translators { + +static void write_point(geometry_msgs::msg::Point& msg, const Eigen::Vector3d& vector) { + msg.x = vector.x(); + msg.y = vector.y(); + msg.z = vector.z(); +} + +static void write_vector3(geometry_msgs::msg::Vector3& msg, const Eigen::Vector3d& vector) { + msg.x = vector.x(); + msg.y = vector.y(); + msg.z = vector.z(); +} + +static void write_quaternion(geometry_msgs::msg::Quaternion& msg, const Eigen::Quaterniond& quat) { + msg.w = quat.w(); + msg.x = quat.x(); + msg.y = quat.y(); + msg.z = quat.z(); +} + +void write_msg(geometry_msgs::msg::Point& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_point(msg, state.get_position()); +} + +void write_msg(geometry_msgs::msg::Vector3& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_vector3(msg, state.get_position()); +} + +void write_msg(geometry_msgs::msg::Quaternion& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_quaternion(msg, state.get_orientation()); +} + +void write_msg(geometry_msgs::msg::Accel& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_vector3(msg.linear, state.get_linear_acceleration()); + write_vector3(msg.angular, state.get_angular_acceleration()); +} + +void write_msg(geometry_msgs::msg::AccelStamped& msg, const CartesianState& state, const rclcpp::Time& time) { + write_msg(msg.accel, state, time); + msg.header.stamp = time; + msg.header.frame_id = state.get_reference_frame(); +} + + +void write_msg(geometry_msgs::msg::Pose& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_point(msg.position, state.get_position()); + write_quaternion(msg.orientation, state.get_orientation()); +} + +void write_msg(geometry_msgs::msg::PoseStamped& msg, const CartesianState& state, const rclcpp::Time& time) { + write_msg(msg.pose, state, time); + msg.header.stamp = time; + msg.header.frame_id = state.get_reference_frame(); +} + +void write_msg(geometry_msgs::msg::Transform& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_vector3(msg.translation, state.get_position()); + write_quaternion(msg.rotation, state.get_orientation()); +} + +void write_msg(geometry_msgs::msg::TransformStamped& msg, const CartesianState& state, const rclcpp::Time& time) { + write_msg(msg.transform, state, time); + msg.header.stamp = time; + msg.header.frame_id = state.get_reference_frame(); + msg.child_frame_id = state.get_name(); +} + +void write_msg(geometry_msgs::msg::Twist& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_vector3(msg.linear, state.get_linear_velocity()); + write_vector3(msg.angular, state.get_angular_velocity()); +} + +void write_msg(geometry_msgs::msg::TwistStamped& msg, const CartesianState& state, const rclcpp::Time& time) { + write_msg(msg.twist, state, time); + msg.header.stamp = time; + msg.header.frame_id = state.get_reference_frame(); +} + +void write_msg(geometry_msgs::msg::Wrench& msg, const CartesianState& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + write_vector3(msg.force, state.get_force()); + write_vector3(msg.torque, state.get_torque()); +} + +void write_msg(geometry_msgs::msg::WrenchStamped& msg, const CartesianState& state, const rclcpp::Time& time) { + write_msg(msg.wrench, state, time); + msg.header.stamp = time; + msg.header.frame_id = state.get_reference_frame(); +} + +void write_msg(sensor_msgs::msg::JointState& msg, const JointState& state, const rclcpp::Time& time) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + msg.header.stamp = time; + msg.name = state.get_names(); + msg.position = std::vector(state.get_positions().data(), state.get_positions().data() + state.get_positions().size()); + msg.velocity = std::vector(state.get_velocities().data(), state.get_velocities().data() + state.get_velocities().size()); + msg.effort = std::vector(state.get_torques().data(), state.get_torques().data() + state.get_torques().size()); +} + +void write_msg(tf2_msgs::msg::TFMessage& msg, const CartesianState& state, const rclcpp::Time& time) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + geometry_msgs::msg::TransformStamped transform; + write_msg(transform, state, time); + msg.transforms.push_back(transform); +} + +template +void write_msg(U& msg, const Parameter& state, const rclcpp::Time&) { + if (state.is_empty()) { + throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + } + msg.data = state.get_value(); +} + +template void write_msg(std_msgs::msg::Float64& msg, const Parameter& state, const rclcpp::Time&); + +template void write_msg>(std_msgs::msg::Float64MultiArray& msg, const Parameter>& state, const rclcpp::Time&); + +template void write_msg(std_msgs::msg::Bool& msg, const Parameter& state, const rclcpp::Time&); + +template void write_msg(std_msgs::msg::String& msg, const Parameter& state, const rclcpp::Time&); + +template <> +void write_msg(geometry_msgs::msg::Transform& msg, const Parameter& state, const rclcpp::Time& time) { + write_msg(msg, state.get_value(), time); +} + +template <> +void write_msg(geometry_msgs::msg::TransformStamped& msg, const Parameter& state, const rclcpp::Time& time) { + write_msg(msg, state.get_value(), time); +} + +template <> +void write_msg(tf2_msgs::msg::TFMessage& msg, const Parameter& state, const rclcpp::Time& time) { + write_msg(msg, state.get_value(), time); +} +}// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp new file mode 100644 index 000000000..c0ed3040d --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp @@ -0,0 +1,181 @@ +#include + +#include "modulo_new_core/translators/ReadStateConversion.hpp" +#include "modulo_new_core/translators/WriteStateConversion.hpp" + +#include + +using namespace modulo_new_core::translators; + +static void expect_vector_equal(const Eigen::Vector3d& v1, const geometry_msgs::msg::Vector3& v2) { + EXPECT_FLOAT_EQ(v1.x(), v2.x); + EXPECT_FLOAT_EQ(v1.y(), v2.y); + EXPECT_FLOAT_EQ(v1.z(), v2.z); +} + +static void expect_point_equal(const Eigen::Vector3d& vector, const geometry_msgs::msg::Point& point) { + EXPECT_FLOAT_EQ(vector.x(), point.x); + EXPECT_FLOAT_EQ(vector.y(), point.y); + EXPECT_FLOAT_EQ(vector.z(), point.z); +} + +static void expect_quaternion_equal(const Eigen::Quaterniond& q1, const geometry_msgs::msg::Quaternion& q2) { + EXPECT_FLOAT_EQ(q1.x(), q2.x); + EXPECT_FLOAT_EQ(q1.y(), q2.y); + EXPECT_FLOAT_EQ(q1.z(), q2.z); + EXPECT_FLOAT_EQ(q1.w(), q2.w); +} + +class TranslatorsTest : public ::testing::Test { +protected: + void SetUp() override { + state_ = state_representation::CartesianState::Random("test", "reference"); + joint_state_ = state_representation::JointState::Random("robot", 3); + } + state_representation::CartesianState state_; + state_representation::JointState joint_state_; + rclcpp::Clock clock_; +}; + +TEST_F(TranslatorsTest, TestAccel) { + auto accel = geometry_msgs::msg::Accel(); + write_msg(accel, state_, clock_.now()); + expect_vector_equal(state_.get_linear_acceleration(), accel.linear); + expect_vector_equal(state_.get_angular_acceleration(), accel.angular); + + state_representation::CartesianState new_state("new"); + EXPECT_THROW(write_msg(accel, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_msg(new_state, accel); + EXPECT_TRUE(new_state.get_acceleration().isApprox(state_.get_acceleration())); + + auto accel_stamped = geometry_msgs::msg::AccelStamped(); + write_msg(accel_stamped, state_, clock_.now()); + EXPECT_EQ(state_.get_reference_frame(), accel_stamped.header.frame_id); + expect_vector_equal(state_.get_linear_acceleration(), accel_stamped.accel.linear); + expect_vector_equal(state_.get_angular_acceleration(), accel_stamped.accel.angular); + new_state = state_representation::CartesianState("new"); + read_msg(new_state, accel_stamped); + EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); + EXPECT_TRUE(new_state.get_acceleration().isApprox(state_.get_acceleration())); +} + +TEST_F(TranslatorsTest, TestPose) { + auto pose = geometry_msgs::msg::Pose(); + write_msg(pose, state_, clock_.now()); + expect_point_equal(state_.get_position(), pose.position); + expect_quaternion_equal(state_.get_orientation(), pose.orientation); + + state_representation::CartesianState new_state("new"); + EXPECT_THROW(write_msg(pose, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_msg(new_state, pose); + EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); + + auto pose_stamped = geometry_msgs::msg::PoseStamped(); + write_msg(pose_stamped, state_, clock_.now()); + EXPECT_EQ(state_.get_reference_frame(), pose_stamped.header.frame_id); + expect_point_equal(state_.get_position(), pose_stamped.pose.position); + expect_quaternion_equal(state_.get_orientation(), pose_stamped.pose.orientation); + new_state = state_representation::CartesianState("new"); + read_msg(new_state, pose_stamped); + EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); + EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); +} + +TEST_F(TranslatorsTest, TestTransform) { + auto tf = geometry_msgs::msg::Transform(); + write_msg(tf, state_, clock_.now()); + expect_vector_equal(state_.get_position(), tf.translation); + expect_quaternion_equal(state_.get_orientation(), tf.rotation); + + state_representation::CartesianState new_state("new"); + EXPECT_THROW(write_msg(tf, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_msg(new_state, tf); + EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); + + auto tf_stamped = geometry_msgs::msg::TransformStamped(); + write_msg(tf_stamped, state_, clock_.now()); + EXPECT_EQ(state_.get_name(), tf_stamped.child_frame_id); + EXPECT_EQ(state_.get_reference_frame(), tf_stamped.header.frame_id); + expect_vector_equal(state_.get_position(), tf_stamped.transform.translation); + expect_quaternion_equal(state_.get_orientation(), tf_stamped.transform.rotation); + new_state = state_representation::CartesianState("new"); + read_msg(new_state, tf_stamped); + EXPECT_EQ(new_state.get_name(), state_.get_name()); + EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); + EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); +} + +TEST_F(TranslatorsTest, TestTwist) { + auto twist = geometry_msgs::msg::Twist(); + write_msg(twist, state_, clock_.now()); + expect_vector_equal(state_.get_linear_velocity(), twist.linear); + expect_vector_equal(state_.get_angular_velocity(), twist.angular); + + state_representation::CartesianState new_state("new"); + EXPECT_THROW(write_msg(twist, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_msg(new_state, twist); + EXPECT_TRUE(new_state.get_twist().isApprox(state_.get_twist())); + + auto twist_stamped = geometry_msgs::msg::TwistStamped(); + write_msg(twist_stamped, state_, clock_.now()); + EXPECT_EQ(state_.get_reference_frame(), twist_stamped.header.frame_id); + expect_vector_equal(state_.get_linear_velocity(), twist_stamped.twist.linear); + expect_vector_equal(state_.get_angular_velocity(), twist_stamped.twist.angular); + new_state = state_representation::CartesianState("new"); + read_msg(new_state, twist_stamped); + EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); + EXPECT_TRUE(new_state.get_twist().isApprox(state_.get_twist())); +} + +TEST_F(TranslatorsTest, TestWrench) { + auto wrench = geometry_msgs::msg::Wrench(); + write_msg(wrench, state_, clock_.now()); + expect_vector_equal(state_.get_force(), wrench.force); + expect_vector_equal(state_.get_torque(), wrench.torque); + + state_representation::CartesianState new_state("new"); + EXPECT_THROW(write_msg(wrench, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_msg(new_state, wrench); + EXPECT_TRUE(new_state.get_wrench().isApprox(state_.get_wrench())); + + auto wrench_stamped = geometry_msgs::msg::WrenchStamped(); + write_msg(wrench_stamped, state_, clock_.now()); + EXPECT_EQ(state_.get_reference_frame(), wrench_stamped.header.frame_id); + expect_vector_equal(state_.get_force(), wrench_stamped.wrench.force); + expect_vector_equal(state_.get_torque(), wrench_stamped.wrench.torque); + new_state = state_representation::CartesianState("new"); + read_msg(new_state, wrench_stamped); + EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); + EXPECT_TRUE(new_state.get_wrench().isApprox(state_.get_wrench())); +} + +TEST_F(TranslatorsTest, TestJointState) { + auto msg = sensor_msgs::msg::JointState(); + write_msg(msg, joint_state_, clock_.now()); + for (unsigned int i = 0; i < joint_state_.get_size(); ++i) { + EXPECT_EQ(msg.name.at(i), joint_state_.get_names().at(i)); + EXPECT_NEAR(msg.position.at(i), joint_state_.get_position(i), 1e-5); + EXPECT_NEAR(msg.velocity.at(i), joint_state_.get_velocity(i), 1e-5); + EXPECT_NEAR(msg.effort.at(i), joint_state_.get_torque(i), 1e-5); + } + + auto new_state = state_representation::JointState("test", {"1", "2", "3"}); + EXPECT_THROW(write_msg(msg, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_msg(new_state, msg); + EXPECT_TRUE(new_state.get_positions().isApprox(joint_state_.get_positions())); + EXPECT_TRUE(new_state.get_velocities().isApprox(joint_state_.get_velocities())); + EXPECT_TRUE(new_state.get_torques().isApprox(joint_state_.get_torques())); + for (unsigned int i = 0; i < joint_state_.get_size(); ++i) { + EXPECT_EQ(new_state.get_names().at(i), joint_state_.get_names().at(i)); + } +} + +TEST_F(TranslatorsTest, TestEncodedState) { + auto msg = modulo_new_core::EncodedState(); + write_msg(msg, state_, clock_.now()); + state_representation::CartesianState new_state; + read_msg(new_state, msg); + EXPECT_TRUE(state_.data().isApprox(new_state.data())); + EXPECT_EQ(state_.get_name(), new_state.get_name()); + EXPECT_EQ(state_.get_reference_frame(), new_state.get_reference_frame()); +} \ No newline at end of file diff --git a/source/modulo_new_core/test/python_tests/conftest.py b/source/modulo_new_core/test/python_tests/conftest.py new file mode 100644 index 000000000..f2f923958 --- /dev/null +++ b/source/modulo_new_core/test/python_tests/conftest.py @@ -0,0 +1,18 @@ +import pytest +import rclpy.clock +from state_representation import CartesianState, JointState + + +@pytest.fixture +def cart_state(): + return CartesianState().Random("test", "ref") + + +@pytest.fixture +def joint_state(): + return JointState().Random("robot", 3) + + +@pytest.fixture +def clock(): + return rclpy.clock.Clock() diff --git a/source/modulo_new_core/test/python_tests/test_translators.py b/source/modulo_new_core/test/python_tests/test_translators.py new file mode 100644 index 000000000..9ab280a0a --- /dev/null +++ b/source/modulo_new_core/test/python_tests/test_translators.py @@ -0,0 +1,171 @@ +import clproto +import geometry_msgs.msg +import numpy as np +import pytest +import rclpy +import sensor_msgs.msg +import state_representation as sr + +import modulo_new_core.translators.read_state_conversion as modulo_readers +import modulo_new_core.translators.write_state_conversion as modulo_writers + + +def read_xyz(msg): + return [msg.x, msg.y, msg.z] + + +def read_quaternion(msg): + return [msg.w, msg.x, msg.y, msg.z] + + +def assert_np_array_equal(a: np.array, b: np.array, places=3): + try: + np.testing.assert_almost_equal(a, b, decimal=places) + except AssertionError as e: + pytest.fail(f'{e}') + + +def test_accel(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): + msg = geometry_msgs.msg.Accel() + modulo_writers.write_msg(msg, cart_state) + assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_acceleration()) + assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_acceleration()) + + new_state = sr.CartesianState("new") + with pytest.raises(RuntimeError): + modulo_writers.write_msg(new_state, msg) + modulo_readers.read_msg(new_state, msg) + assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) + + msg_stamped = geometry_msgs.msg.AccelStamped() + modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + assert msg_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_acceleration()) + assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_acceleration()) + new_state = sr.CartesianState("new") + modulo_readers.read_msg_stamped(new_state, msg_stamped) + assert cart_state.get_reference_frame() == new_state.get_reference_frame() + assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) + + +def test_pose(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): + msg = geometry_msgs.msg.Pose() + modulo_writers.write_msg(msg, cart_state) + assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) + assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation()) + + new_state = sr.CartesianState("new") + with pytest.raises(RuntimeError): + modulo_writers.write_msg(new_state, msg) + modulo_readers.read_msg(new_state, msg) + assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) + + msg_stamped = geometry_msgs.msg.PoseStamped() + modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + assert msg_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) + assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation()) + new_state = sr.CartesianState("new") + modulo_readers.read_msg_stamped(new_state, msg_stamped) + assert cart_state.get_reference_frame() == new_state.get_reference_frame() + assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) + + +def test_transform(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): + msg = geometry_msgs.msg.Transform() + modulo_writers.write_msg(msg, cart_state) + assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) + assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation()) + + new_state = sr.CartesianState("new") + with pytest.raises(RuntimeError): + modulo_writers.write_msg(new_state, msg) + modulo_readers.read_msg(new_state, msg) + assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) + + msg_stamped = geometry_msgs.msg.TransformStamped() + modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + assert msg_stamped.header.frame_id == cart_state.get_reference_frame() + assert msg_stamped.child_frame_id == cart_state.get_name() + assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) + assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation()) + new_state = sr.CartesianState("new") + modulo_readers.read_msg_stamped(new_state, msg_stamped) + assert cart_state.get_reference_frame() == new_state.get_reference_frame() + assert cart_state.get_name() == new_state.get_name() + assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) + + +def test_twist(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): + msg = geometry_msgs.msg.Twist() + modulo_writers.write_msg(msg, cart_state) + assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_velocity()) + assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_velocity()) + + new_state = sr.CartesianState("new") + with pytest.raises(RuntimeError): + modulo_writers.write_msg(new_state, msg) + modulo_readers.read_msg(new_state, msg) + assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) + + msg_stamped = geometry_msgs.msg.TwistStamped() + modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + assert msg_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_velocity()) + assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_velocity()) + new_state = sr.CartesianState("new") + modulo_readers.read_msg_stamped(new_state, msg_stamped) + assert cart_state.get_reference_frame() == new_state.get_reference_frame() + assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) + + +def test_wrench(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): + msg = geometry_msgs.msg.Wrench() + modulo_writers.write_msg(msg, cart_state) + assert_np_array_equal(read_xyz(msg.force), cart_state.get_force()) + assert_np_array_equal(read_xyz(msg.torque), cart_state.get_torque()) + + new_state = sr.CartesianState("new") + with pytest.raises(RuntimeError): + modulo_writers.write_msg(new_state, msg) + modulo_readers.read_msg(new_state, msg) + assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) + + msg_stamped = geometry_msgs.msg.WrenchStamped() + modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + assert msg_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(msg.force), cart_state.get_force()) + assert_np_array_equal(read_xyz(msg.torque), cart_state.get_torque()) + new_state = sr.CartesianState("new") + modulo_readers.read_msg_stamped(new_state, msg_stamped) + assert cart_state.get_reference_frame() == new_state.get_reference_frame() + assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) + + +def test_joint_state(joint_state: sr.JointState): + msg = sensor_msgs.msg.JointState() + modulo_writers.write_msg(msg, joint_state) + for i in range(joint_state.get_size()): + assert msg.name[i] == joint_state.get_names()[i] + assert_np_array_equal(msg.position, joint_state.get_positions()) + assert_np_array_equal(msg.velocity, joint_state.get_velocities()) + assert_np_array_equal(msg.effort, joint_state.get_torques()) + + new_state = sr.JointState("test", ["1", "2", "3"]) + with pytest.raises(RuntimeError): + modulo_writers.write_msg(new_state, msg) + modulo_readers.read_msg(new_state, msg) + for i in range(joint_state.get_size()): + assert new_state.get_names()[i] == joint_state.get_names()[i] + assert_np_array_equal(new_state.get_positions(), joint_state.get_positions()) + assert_np_array_equal(new_state.get_velocities(), joint_state.get_velocities()) + assert_np_array_equal(new_state.get_torques(), joint_state.get_torques()) + + +def test_encoded_state(cart_state: sr.CartesianState): + msg = modulo_writers.write_clproto_msg(cart_state, clproto.MessageType.CARTESIAN_STATE_MESSAGE) + + new_state = modulo_readers.read_clproto_msg(msg) + assert_np_array_equal(new_state.data(), cart_state.data()) + assert new_state.get_name() == cart_state.get_name() + assert new_state.get_reference_frame() == cart_state.get_reference_frame() diff --git a/source/modulo_new_core/test/test_modulo_new_core.cpp b/source/modulo_new_core/test/test_modulo_new_core.cpp new file mode 100644 index 000000000..e7bb1d13c --- /dev/null +++ b/source/modulo_new_core/test/test_modulo_new_core.cpp @@ -0,0 +1,7 @@ +#include + + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} From aa4f51cebee66134c7232505595e156804e17fe2 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 1 Apr 2022 17:01:00 +0200 Subject: [PATCH 06/71] Add MessagePairInterface and MessagePair including tests (#43) * Add MessagePairInterface and MessagePair including tests * Minor improvements * Rename functions --- source/modulo_new_core/CMakeLists.txt | 2 + .../communication/MessagePair.hpp | 40 +++++++++++++ .../communication/MessagePairInterface.hpp | 55 ++++++++++++++++++ .../communication/MessageType.hpp | 9 +++ .../src/communication/MessagePair.cpp | 27 +++++++++ .../communication/MessagePairInterface.cpp | 11 ++++ .../communication/test_message_pair.cpp | 56 +++++++++++++++++++ 7 files changed, 200 insertions(+) create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp create mode 100644 source/modulo_new_core/src/communication/MessagePair.cpp create mode 100644 source/modulo_new_core/src/communication/MessagePairInterface.cpp create mode 100644 source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index 28537c8f6..4d711c451 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -27,6 +27,8 @@ find_library(clproto REQUIRED) include_directories(include) ament_auto_add_library(modulo_new_core SHARED + ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp + ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp ${PROJECT_SOURCE_DIR}/src/translators/ReadStateConversion.cpp ${PROJECT_SOURCE_DIR}/src/translators/WriteStateConversion.cpp) diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp new file mode 100644 index 000000000..a060bd3da --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -0,0 +1,40 @@ +#pragma once + +#include "modulo_new_core/communication/MessagePairInterface.hpp" +#include "modulo_new_core/translators/WriteStateConversion.hpp" + +#include +#include +#include +#include +#include + +namespace modulo_new_core::communication { + +template +class MessagePair : public MessagePairInterface { +private: + std::shared_ptr data_; + +public: + explicit MessagePair(std::shared_ptr data); + + [[nodiscard]] MsgT write_message() const; + + [[nodiscard]] std::shared_ptr get_data() const; +}; + +template +MsgT MessagePair::write_message() const { + auto msg = MsgT(); + // TODO use translators + // translators::write_msg(msg, *this->data_); + msg.data = *this->data_; + return msg; +} + +template +std::shared_ptr MessagePair::get_data() const { + return this->data_; +} +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp new file mode 100644 index 000000000..eb9ae6353 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp @@ -0,0 +1,55 @@ +#pragma once + +#include +#include "modulo_new_core/communication/MessageType.hpp" + +namespace modulo_new_core::communication { + +// forward declaration of derived MessagePair class +template +class MessagePair; + +class MessagePairInterface : public std::enable_shared_from_this { +public: + explicit MessagePairInterface(MessageType type); + + virtual ~MessagePairInterface() = default; + + MessagePairInterface(const MessagePairInterface& message_pair) = default; + + template + std::shared_ptr> get_message_pair(bool validate_pointer = true); + + template + MsgT write_message(); + + MessageType get_type() const; + +private: + MessageType type_; +}; + +template +inline std::shared_ptr> MessagePairInterface::get_message_pair(bool validate_pointer) { + std::shared_ptr> message_pair_ptr; + try { + message_pair_ptr = std::dynamic_pointer_cast>(this->shared_from_this()); + } catch (const std::exception& ex) { + if (validate_pointer) { + // TODO proper exception + throw std::runtime_error("MessagePair interface is not managed by a valid pointer"); + } + } + if (message_pair_ptr == nullptr && validate_pointer) { + // TODO proper exception + throw std::runtime_error("Unable to cast MessagePair interface"); + } + return message_pair_ptr; +} + +template +inline MsgT MessagePairInterface::write_message() { + return this->template get_message_pair()->write_message(); +} + +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp new file mode 100644 index 000000000..009299858 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp @@ -0,0 +1,9 @@ +#pragma once + +namespace modulo_new_core::communication { + +enum class MessageType { + BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT64, STRING +}; + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_new_core/src/communication/MessagePair.cpp new file mode 100644 index 000000000..6469b8549 --- /dev/null +++ b/source/modulo_new_core/src/communication/MessagePair.cpp @@ -0,0 +1,27 @@ +#include "modulo_new_core/communication/MessagePair.hpp" + +#include + +namespace modulo_new_core::communication { + +template<> +MessagePair::MessagePair(std::shared_ptr data) : + MessagePairInterface(MessageType::BOOL), data_(std::move(data)) {} + +template<> +MessagePair::MessagePair(std::shared_ptr data) : + MessagePairInterface(MessageType::FLOAT64), data_(std::move(data)) {} + +template<> +MessagePair>::MessagePair(std::shared_ptr> data) : + MessagePairInterface(MessageType::FLOAT64_MULTI_ARRAY), data_(std::move(data)) {} + +template<> +MessagePair::MessagePair(std::shared_ptr data) : + MessagePairInterface(MessageType::INT64), data_(std::move(data)) {} + +template<> +MessagePair::MessagePair(std::shared_ptr data) : + MessagePairInterface(MessageType::STRING), data_(std::move(data)) {} + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/src/communication/MessagePairInterface.cpp b/source/modulo_new_core/src/communication/MessagePairInterface.cpp new file mode 100644 index 000000000..2b4c5a097 --- /dev/null +++ b/source/modulo_new_core/src/communication/MessagePairInterface.cpp @@ -0,0 +1,11 @@ +#include "modulo_new_core/communication/MessagePairInterface.hpp" + +namespace modulo_new_core::communication { + +MessagePairInterface::MessagePairInterface(MessageType type) : type_(type) {} + +MessageType MessagePairInterface::get_type() const { + return this->type_; +} + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp new file mode 100644 index 000000000..bd7afe47d --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -0,0 +1,56 @@ +#include + +#include "modulo_new_core/communication/MessagePair.hpp" + +using namespace modulo_new_core::communication; + +template +static void test_message_interface(const DataT& initial_value, const DataT& new_value) { + auto data = std::make_shared(initial_value); + auto msg_pair = std::make_shared>(data); + EXPECT_EQ(initial_value, *msg_pair->get_data()); + EXPECT_EQ(initial_value, msg_pair->write_message().data); + + std::shared_ptr msg_pair_interface(msg_pair); + auto msg = msg_pair_interface->template write_message(); + EXPECT_EQ(initial_value, msg.data); + + *data = new_value; + EXPECT_EQ(new_value, *msg_pair->get_data()); + EXPECT_EQ(new_value, msg_pair->write_message().data); + msg = msg_pair_interface->template write_message(); + EXPECT_EQ(new_value, msg.data); +} + +TEST(MessagePairTest, TestBasicTypes) { + test_message_interface(false, true); + test_message_interface(0.1, 0.2); + test_message_interface(1, 2); + test_message_interface("this", "that"); +} + +TEST(MessagePairTest, TestDoubleArray) { + std::vector initial_value = {0.1, 0.2, 0.3}; + auto data = std::make_shared>(initial_value); + auto msg_pair = std::make_shared>>(data); + for (std::size_t i = 0; i < initial_value.size(); ++i) { + EXPECT_EQ(initial_value.at(i), msg_pair->get_data()->at(i)); + EXPECT_EQ(initial_value.at(i), msg_pair->write_message().data.at(i)); + } + + std::shared_ptr msg_pair_interface(msg_pair); + auto msg = msg_pair_interface->write_message>(); + for (std::size_t i = 0; i < initial_value.size(); ++i) { + EXPECT_EQ(initial_value.at(i),msg.data.at(i)); + } + EXPECT_EQ(initial_value, msg.data); + + std::vector new_value = {0.4, 0.5}; + *data = new_value; + msg = msg_pair_interface->write_message>(); + for (std::size_t i = 0; i < new_value.size(); ++i) { + EXPECT_EQ(new_value.at(i), msg_pair->get_data()->at(i)); + EXPECT_EQ(new_value.at(i), msg_pair->write_message().data.at(i)); + EXPECT_EQ(new_value.at(i),msg.data.at(i)); + } +} From 0880ed6bb26fcd05c20c3af28d1703a19bbf8522 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 5 Apr 2022 13:44:24 +0200 Subject: [PATCH 07/71] Use translators in MessagePair (#44) * Add translators for std msgs * Fix includes * Update MessagePair with clock * Add exceptions --- .../modulo_core/include/modulo_core/Cell.hpp | 1 + .../TransformListenerHandler.hpp | 1 + .../communication/MessagePair.hpp | 10 +-- .../communication/MessagePairInterface.hpp | 11 ++- .../communication/MessageType.hpp | 2 +- .../InvalidMessagePairCastException.hpp | 11 +++ .../exceptions/InvalidPointerException.hpp | 11 +++ .../translators/ReadStateConversion.hpp | 83 +++++++++++++------ .../translators/WriteStateConversion.hpp | 59 +++++++++---- source/modulo_new_core/package.xml | 2 +- .../src/communication/MessagePair.cpp | 26 +++--- .../src/translators/ReadStateConversion.cpp | 20 +++++ .../src/translators/WriteStateConversion.cpp | 48 +++++------ .../communication/test_message_pair.cpp | 32 ++++--- .../translators/test_translators.cpp | 28 +++++++ 15 files changed, 245 insertions(+), 100 deletions(-) create mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp diff --git a/source/modulo_core/include/modulo_core/Cell.hpp b/source/modulo_core/include/modulo_core/Cell.hpp index b52b7f523..065cc35ff 100644 --- a/source/modulo_core/include/modulo_core/Cell.hpp +++ b/source/modulo_core/include/modulo_core/Cell.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp index 146adef59..99db8377d 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index a060bd3da..c5d7cda96 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -3,10 +3,11 @@ #include "modulo_new_core/communication/MessagePairInterface.hpp" #include "modulo_new_core/translators/WriteStateConversion.hpp" +#include #include #include #include -#include +#include #include namespace modulo_new_core::communication { @@ -15,9 +16,10 @@ template class MessagePair : public MessagePairInterface { private: std::shared_ptr data_; + std::shared_ptr clock_; public: - explicit MessagePair(std::shared_ptr data); + MessagePair(std::shared_ptr data, std::shared_ptr clock); [[nodiscard]] MsgT write_message() const; @@ -27,9 +29,7 @@ class MessagePair : public MessagePairInterface { template MsgT MessagePair::write_message() const { auto msg = MsgT(); - // TODO use translators - // translators::write_msg(msg, *this->data_); - msg.data = *this->data_; + translators::write_msg(msg, *this->data_, clock_->now()); return msg; } diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp index eb9ae6353..dd36796ef 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp @@ -1,7 +1,10 @@ #pragma once #include + #include "modulo_new_core/communication/MessageType.hpp" +#include "modulo_new_core/exceptions/InvalidMessagePairCastException.hpp" +#include "modulo_new_core/exceptions/InvalidPointerException.hpp" namespace modulo_new_core::communication { @@ -36,13 +39,13 @@ inline std::shared_ptr> MessagePairInterface::get_messa message_pair_ptr = std::dynamic_pointer_cast>(this->shared_from_this()); } catch (const std::exception& ex) { if (validate_pointer) { - // TODO proper exception - throw std::runtime_error("MessagePair interface is not managed by a valid pointer"); + throw exceptions::InvalidPointerException("Message pair interface is not managed by a valid pointer"); } } if (message_pair_ptr == nullptr && validate_pointer) { - // TODO proper exception - throw std::runtime_error("Unable to cast MessagePair interface"); + throw exceptions::InvalidMessagePairCastException( + "Unable to case message pair interface to a message pair pointer of requested type" + ); } return message_pair_ptr; } diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp index 009299858..00f5c717b 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp @@ -3,7 +3,7 @@ namespace modulo_new_core::communication { enum class MessageType { - BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT64, STRING + BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT32, STRING }; }// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp new file mode 100644 index 000000000..c93eb679f --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace modulo_new_core::exceptions { +class InvalidMessagePairCastException : public std::runtime_error { +public: + explicit InvalidMessagePairCastException(const std::string& msg) : std::runtime_error(msg) {}; +}; +} \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp new file mode 100644 index 000000000..7d27dfb27 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace modulo_new_core::exceptions { +class InvalidPointerException : public std::runtime_error { +public: + explicit InvalidPointerException(const std::string& msg) : std::runtime_error(msg) {}; +}; +} \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp b/source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp index 421520a42..cadeab27e 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp @@ -5,21 +5,36 @@ #include #include #include -#include +#include +#include +#include +#include +#include #include -#include #include #include -#include #include -#include -#include #include #include "modulo_new_core/EncodedState.hpp" namespace modulo_new_core::translators { + +/** + * @brief Convert a ROS geometry_msgs::msg::Accel to a CartesianState + * @param state The CartesianState to populate + * @param msg The ROS msg to read from + */ +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& msg); + +/** + * @brief Convert a ROS geometry_msgs::msg::AccelStamped to a CartesianState + * @param state The CartesianState to populate + * @param msg The ROS msg to read from + */ +void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& msg); + /** * @brief Convert a ROS geometry_msgs::msg::Pose to a CartesianState * @param state The CartesianState to populate @@ -62,20 +77,6 @@ void read_msg(state_representation::CartesianState& state, const geometry_msgs:: */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& msg); -/** - * @brief Convert a ROS geometry_msgs::msg::Accel to a CartesianState - * @param state The CartesianState to populate - * @param msg The ROS msg to read from - */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& msg); - -/** - * @brief Convert a ROS geometry_msgs::msg::AccelStamped to a CartesianState - * @param state The CartesianState to populate - * @param msg The ROS msg to read from - */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& msg); - /** * @brief Convert a ROS geometry_msgs::msg::Wrench to a CartesianState * @param state The CartesianState to populate @@ -90,13 +91,6 @@ void read_msg(state_representation::CartesianState& state, const geometry_msgs:: */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& msg); -/** - * @brief Convert a ROS geometry_msgs::msg::WrenchStamped to a CartesianState - * @param state The CartesianState to populate - * @param msg The ROS msg to read from - */ -void read_msg(state_representation::CartesianState& state, const nav_msgs::msg::Odometry& msg); - /** * @brief Convert a ROS sensor_msgs::msg::JointState to a JointState * @param state The JointState to populate @@ -116,6 +110,41 @@ void read_msg(state_representation::Parameter& state, const U& msg) { state.set_value(msg.data); } +/** + * @brief Convert a ROS std_msgs::msg::Bool to a boolean + * @param state The state to populate + * @param msg The ROS msg to read from + */ +void read_msg(bool& state, const std_msgs::msg::Bool& msg); + +/** + * @brief Convert a ROS std_msgs::msg::Float64 to a double + * @param state The state to populate + * @param msg The ROS msg to read from + */ +void read_msg(double& state, const std_msgs::msg::Float64& msg); + +/** + * @brief Convert a ROS std_msgs::msg::Float64MultiArray to a vector of double + * @param state The state to populate + * @param msg The ROS msg to read from + */ +void read_msg(std::vector& state, const std_msgs::msg::Float64MultiArray& msg); + +/** + * @brief Convert a ROS std_msgs::msg::Int32 to an integer + * @param state The state to populate + * @param msg The ROS msg to read from + */ +void read_msg(int& state, const std_msgs::msg::Int32& msg); + +/** + * @brief Convert a ROS std_msgs::msg::String to a string + * @param state The state to populate + * @param msg The ROS msg to read from + */ +void read_msg(std::string& state, const std_msgs::msg::String& msg); + /** * @brief Convert a ROS std_msgs::msg::UInt8MultiArray message to a State using protobuf decoding * @tparam a state_representation::State type object @@ -123,7 +152,7 @@ void read_msg(state_representation::Parameter& state, const U& msg) { * @param msg The ROS msg to read from */ template -void read_msg(T& state, const EncodedState& msg) { +inline void read_msg(T& state, const EncodedState& msg) { std::string tmp(msg.data.begin(), msg.data.end()); state = clproto::decode(tmp); } diff --git a/source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp b/source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp index 034558188..96dfcc182 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp @@ -6,21 +6,23 @@ #include #include #include +#include +#include +#include +#include +#include #include #include #include -#include -#include -#include -#include #include -#include -#include +#include +#include #include "modulo_new_core/EncodedState.hpp" namespace modulo_new_core::translators { + /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Accel * @param msg The ROS msg to populate @@ -37,14 +39,6 @@ void write_msg(geometry_msgs::msg::Accel& msg, const state_representation::Carte */ void write_msg(geometry_msgs::msg::AccelStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); -/** - * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Quaternion - * @param msg The ROS msg to populate - * @param state The state to read from - * @param time The time of the message - */ -void write_msg(geometry_msgs::msg::Quaternion& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); - /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Pose * @param msg The ROS msg to populate @@ -136,6 +130,41 @@ void write_msg(tf2_msgs::msg::TFMessage& msg, const state_representation::Cartes template void write_msg(U& msg, const state_representation::Parameter& state, const rclcpp::Time&); +/** + * @brief Convert a boolean to a ROS std_msgs::msg::Bool + * @param msg The ROS msg to populate + * @param state The state to read from + */ +void write_msg(std_msgs::msg::Bool& msg, const bool& state, const rclcpp::Time& time); + +/** + * @brief Convert a double to a ROS std_msgs::msg::Float64 + * @param msg The ROS msg to populate + * @param state The state to read from + */ +void write_msg(std_msgs::msg::Float64& msg, const double& state, const rclcpp::Time& time); + +/** + * @brief Convert a vector of double to a ROS std_msgs::msg::Float64MultiArray + * @param msg The ROS msg to populate + * @param state The state to read from + */ +void write_msg(std_msgs::msg::Float64MultiArray& msg, const std::vector& state, const rclcpp::Time& time); + +/** + * @brief Convert an integer to a ROS std_msgs::msg::Int32 + * @param msg The ROS msg to populate + * @param state The state to read from + */ +void write_msg(std_msgs::msg::Int32& msg, const int& state, const rclcpp::Time& time); + +/** + * @brief Convert a string to a ROS std_msgs::msg::String + * @param msg The ROS msg to populate + * @param state The state to read from + */ +void write_msg(std_msgs::msg::String& msg, const std::string& state, const rclcpp::Time& time); + /** * @brief Convert a state to a ROS std_msgs::msg::UInt8MultiArray message using protobuf encoding * @tparam a state_representation::State type object @@ -144,7 +173,7 @@ void write_msg(U& msg, const state_representation::Parameter& state, const rc * @param time The time of the message */ template -void write_msg(EncodedState& msg, const T& state, const rclcpp::Time&) { +inline void write_msg(EncodedState& msg, const T& state, const rclcpp::Time&) { std::string tmp = clproto::encode(state); msg.data = std::vector(tmp.begin(), tmp.end()); } diff --git a/source/modulo_new_core/package.xml b/source/modulo_new_core/package.xml index 6f78cd4a3..385d8a412 100644 --- a/source/modulo_new_core/package.xml +++ b/source/modulo_new_core/package.xml @@ -12,9 +12,9 @@ rclcpp rclpy - std_msgs geometry_msgs sensor_msgs + std_msgs tf2_msgs ament_lint_auto diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_new_core/src/communication/MessagePair.cpp index 6469b8549..cfe2e900c 100644 --- a/source/modulo_new_core/src/communication/MessagePair.cpp +++ b/source/modulo_new_core/src/communication/MessagePair.cpp @@ -5,23 +5,29 @@ namespace modulo_new_core::communication { template<> -MessagePair::MessagePair(std::shared_ptr data) : - MessagePairInterface(MessageType::BOOL), data_(std::move(data)) {} +MessagePair::MessagePair(std::shared_ptr data, std::shared_ptr clock) : + MessagePairInterface(MessageType::BOOL), data_(std::move(data)), clock_(std::move(clock)) {} template<> -MessagePair::MessagePair(std::shared_ptr data) : - MessagePairInterface(MessageType::FLOAT64), data_(std::move(data)) {} +MessagePair::MessagePair( + std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::FLOAT64), data_(std::move(data)), clock_(std::move(clock)) {} template<> -MessagePair>::MessagePair(std::shared_ptr> data) : - MessagePairInterface(MessageType::FLOAT64_MULTI_ARRAY), data_(std::move(data)) {} +MessagePair>::MessagePair( + std::shared_ptr> data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::FLOAT64_MULTI_ARRAY), data_(std::move(data)), clock_(std::move(clock)) {} template<> -MessagePair::MessagePair(std::shared_ptr data) : - MessagePairInterface(MessageType::INT64), data_(std::move(data)) {} +MessagePair::MessagePair(std::shared_ptr data, std::shared_ptr clock) : + MessagePairInterface(MessageType::INT32), data_(std::move(data)), clock_(std::move(clock)) {} template<> -MessagePair::MessagePair(std::shared_ptr data) : - MessagePairInterface(MessageType::STRING), data_(std::move(data)) {} +MessagePair::MessagePair( + std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::STRING), data_(std::move(data)), clock_(std::move(clock)) {} }// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/src/translators/ReadStateConversion.cpp b/source/modulo_new_core/src/translators/ReadStateConversion.cpp index 1138c5d9f..a4fe873c4 100644 --- a/source/modulo_new_core/src/translators/ReadStateConversion.cpp +++ b/source/modulo_new_core/src/translators/ReadStateConversion.cpp @@ -77,4 +77,24 @@ void read_msg(state_representation::JointState& state, const sensor_msgs::msg::J state.set_torques(Eigen::VectorXd::Map(msg.effort.data(), msg.effort.size())); } } + +void read_msg(bool& state, const std_msgs::msg::Bool& msg) { + state = msg.data; +} + +void read_msg(double& state, const std_msgs::msg::Float64& msg) { + state = msg.data; +} + +void read_msg(std::vector& state, const std_msgs::msg::Float64MultiArray& msg) { + state = msg.data; +} + +void read_msg(int& state, const std_msgs::msg::Int32& msg) { + state = msg.data; +} + +void read_msg(std::string& state, const std_msgs::msg::String& msg) { + state = msg.data; +} }// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/src/translators/WriteStateConversion.cpp b/source/modulo_new_core/src/translators/WriteStateConversion.cpp index ee312653c..70bb17844 100644 --- a/source/modulo_new_core/src/translators/WriteStateConversion.cpp +++ b/source/modulo_new_core/src/translators/WriteStateConversion.cpp @@ -1,9 +1,7 @@ #include "modulo_new_core/translators/WriteStateConversion.hpp" -#include -#include -#include -#include +#include +#include using namespace state_representation; @@ -28,27 +26,6 @@ static void write_quaternion(geometry_msgs::msg::Quaternion& msg, const Eigen::Q msg.z = quat.z(); } -void write_msg(geometry_msgs::msg::Point& msg, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - } - write_point(msg, state.get_position()); -} - -void write_msg(geometry_msgs::msg::Vector3& msg, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - } - write_vector3(msg, state.get_position()); -} - -void write_msg(geometry_msgs::msg::Quaternion& msg, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); - } - write_quaternion(msg, state.get_orientation()); -} - void write_msg(geometry_msgs::msg::Accel& msg, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); @@ -63,7 +40,6 @@ void write_msg(geometry_msgs::msg::AccelStamped& msg, const CartesianState& stat msg.header.frame_id = state.get_reference_frame(); } - void write_msg(geometry_msgs::msg::Pose& msg, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); @@ -171,4 +147,24 @@ template <> void write_msg(tf2_msgs::msg::TFMessage& msg, const Parameter& state, const rclcpp::Time& time) { write_msg(msg, state.get_value(), time); } + +void write_msg(std_msgs::msg::Bool& msg, const bool& state, const rclcpp::Time&) { + msg.data = state; +} + +void write_msg(std_msgs::msg::Float64& msg, const double& state, const rclcpp::Time&) { + msg.data = state; +} + +void write_msg(std_msgs::msg::Float64MultiArray& msg, const std::vector& state, const rclcpp::Time&) { + msg.data = state; +} + +void write_msg(std_msgs::msg::Int32& msg, const int& state, const rclcpp::Time&) { + msg.data = state; +} + +void write_msg(std_msgs::msg::String& msg, const std::string& state, const rclcpp::Time&) { + msg.data = state; +} }// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp index bd7afe47d..a9fbaa9bf 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -5,9 +5,10 @@ using namespace modulo_new_core::communication; template -static void test_message_interface(const DataT& initial_value, const DataT& new_value) { +static void +test_message_interface(const DataT& initial_value, const DataT& new_value, const std::shared_ptr clock) { auto data = std::make_shared(initial_value); - auto msg_pair = std::make_shared>(data); + auto msg_pair = std::make_shared>(data, clock); EXPECT_EQ(initial_value, *msg_pair->get_data()); EXPECT_EQ(initial_value, msg_pair->write_message().data); @@ -22,17 +23,26 @@ static void test_message_interface(const DataT& initial_value, const DataT& new_ EXPECT_EQ(new_value, msg.data); } -TEST(MessagePairTest, TestBasicTypes) { - test_message_interface(false, true); - test_message_interface(0.1, 0.2); - test_message_interface(1, 2); - test_message_interface("this", "that"); +class MessagePairTest : public ::testing::Test { +protected: + void SetUp() override { + clock_ = std::make_shared(); + } + + std::shared_ptr clock_; +}; + +TEST_F(MessagePairTest, TestBasicTypes) { + test_message_interface(false, true, clock_); + test_message_interface(0.1, 0.2, clock_); + test_message_interface(1, 2, clock_); + test_message_interface("this", "that", clock_); } -TEST(MessagePairTest, TestDoubleArray) { +TEST_F(MessagePairTest, TestDoubleArray) { std::vector initial_value = {0.1, 0.2, 0.3}; auto data = std::make_shared>(initial_value); - auto msg_pair = std::make_shared>>(data); + auto msg_pair = std::make_shared>>(data, clock_); for (std::size_t i = 0; i < initial_value.size(); ++i) { EXPECT_EQ(initial_value.at(i), msg_pair->get_data()->at(i)); EXPECT_EQ(initial_value.at(i), msg_pair->write_message().data.at(i)); @@ -41,7 +51,7 @@ TEST(MessagePairTest, TestDoubleArray) { std::shared_ptr msg_pair_interface(msg_pair); auto msg = msg_pair_interface->write_message>(); for (std::size_t i = 0; i < initial_value.size(); ++i) { - EXPECT_EQ(initial_value.at(i),msg.data.at(i)); + EXPECT_EQ(initial_value.at(i), msg.data.at(i)); } EXPECT_EQ(initial_value, msg.data); @@ -51,6 +61,6 @@ TEST(MessagePairTest, TestDoubleArray) { for (std::size_t i = 0; i < new_value.size(); ++i) { EXPECT_EQ(new_value.at(i), msg_pair->get_data()->at(i)); EXPECT_EQ(new_value.at(i), msg_pair->write_message().data.at(i)); - EXPECT_EQ(new_value.at(i),msg.data.at(i)); + EXPECT_EQ(new_value.at(i), msg.data.at(i)); } } diff --git a/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp index c0ed3040d..fc3edc0d4 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp @@ -4,9 +4,20 @@ #include "modulo_new_core/translators/WriteStateConversion.hpp" #include +#include using namespace modulo_new_core::translators; +template +static void test_std_messages(const DataT& state, const rclcpp::Time& time) { + auto msg = MsgT(); + write_msg(msg, state, time); + EXPECT_EQ(msg.data, state); + DataT new_state; + read_msg(new_state, msg); + EXPECT_EQ(state, new_state); +} + static void expect_vector_equal(const Eigen::Vector3d& v1, const geometry_msgs::msg::Vector3& v2) { EXPECT_FLOAT_EQ(v1.x(), v2.x); EXPECT_FLOAT_EQ(v1.y(), v2.y); @@ -170,6 +181,23 @@ TEST_F(TranslatorsTest, TestJointState) { } } +TEST_F(TranslatorsTest, TestStdMsgs) { + test_std_messages(true, clock_.now()); + test_std_messages(0.3, clock_.now()); + test_std_messages(2, clock_.now()); + test_std_messages("test", clock_.now()); + + auto msg = std_msgs::msg::Float64MultiArray(); + std::vector state = {0.3, 0.6}; + write_msg(msg, state, clock_.now()); + std::vector new_state; + read_msg(new_state, msg); + for (std::size_t i = 0; i < state.size(); ++i) { + EXPECT_EQ(msg.data.at(i), state.at(i)); + EXPECT_EQ(state.at(i), new_state.at(i)); + } +} + TEST_F(TranslatorsTest, TestEncodedState) { auto msg = modulo_new_core::EncodedState(); write_msg(msg, state_, clock_.now()); From d8d232ab06d22b013b76288a1229b30b53bafe59 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 7 Apr 2022 10:36:04 +0200 Subject: [PATCH 08/71] Add EncodedState support (#45) * Template MessagePair constructor to allow state representation types * Add EncodedState to MessageType and update unittests --- source/modulo_new_core/CMakeLists.txt | 1 - .../communication/MessagePair.hpp | 8 ++- .../communication/MessageType.hpp | 2 +- .../src/communication/MessagePair.cpp | 33 ------------ .../communication/test_message_pair.cpp | 54 +++++++++---------- 5 files changed, 35 insertions(+), 63 deletions(-) delete mode 100644 source/modulo_new_core/src/communication/MessagePair.cpp diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index 4d711c451..73cbb0d98 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -27,7 +27,6 @@ find_library(clproto REQUIRED) include_directories(include) ament_auto_add_library(modulo_new_core SHARED - ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp ${PROJECT_SOURCE_DIR}/src/translators/ReadStateConversion.cpp ${PROJECT_SOURCE_DIR}/src/translators/WriteStateConversion.cpp) diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index c5d7cda96..67a1b3e39 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -19,13 +19,19 @@ class MessagePair : public MessagePairInterface { std::shared_ptr clock_; public: - MessagePair(std::shared_ptr data, std::shared_ptr clock); + MessagePair(MessageType type, std::shared_ptr data, std::shared_ptr clock); [[nodiscard]] MsgT write_message() const; [[nodiscard]] std::shared_ptr get_data() const; }; +template +MessagePair::MessagePair( + MessageType type, std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(type), data_(std::move(data)), clock_(std::move(clock)) {} + template MsgT MessagePair::write_message() const { auto msg = MsgT(); diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp index 00f5c717b..d8c07773b 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp @@ -3,7 +3,7 @@ namespace modulo_new_core::communication { enum class MessageType { - BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT32, STRING + BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT32, STRING, ENCODED_STATE }; }// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_new_core/src/communication/MessagePair.cpp deleted file mode 100644 index cfe2e900c..000000000 --- a/source/modulo_new_core/src/communication/MessagePair.cpp +++ /dev/null @@ -1,33 +0,0 @@ -#include "modulo_new_core/communication/MessagePair.hpp" - -#include - -namespace modulo_new_core::communication { - -template<> -MessagePair::MessagePair(std::shared_ptr data, std::shared_ptr clock) : - MessagePairInterface(MessageType::BOOL), data_(std::move(data)), clock_(std::move(clock)) {} - -template<> -MessagePair::MessagePair( - std::shared_ptr data, std::shared_ptr clock -) : - MessagePairInterface(MessageType::FLOAT64), data_(std::move(data)), clock_(std::move(clock)) {} - -template<> -MessagePair>::MessagePair( - std::shared_ptr> data, std::shared_ptr clock -) : - MessagePairInterface(MessageType::FLOAT64_MULTI_ARRAY), data_(std::move(data)), clock_(std::move(clock)) {} - -template<> -MessagePair::MessagePair(std::shared_ptr data, std::shared_ptr clock) : - MessagePairInterface(MessageType::INT32), data_(std::move(data)), clock_(std::move(clock)) {} - -template<> -MessagePair::MessagePair( - std::shared_ptr data, std::shared_ptr clock -) : - MessagePairInterface(MessageType::STRING), data_(std::move(data)), clock_(std::move(clock)) {} - -}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp index a9fbaa9bf..7e159d2f1 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -5,10 +5,11 @@ using namespace modulo_new_core::communication; template -static void -test_message_interface(const DataT& initial_value, const DataT& new_value, const std::shared_ptr clock) { +static void test_message_interface( + MessageType type, const DataT& initial_value, const DataT& new_value, const std::shared_ptr clock +) { auto data = std::make_shared(initial_value); - auto msg_pair = std::make_shared>(data, clock); + auto msg_pair = std::make_shared>(type, data, clock); EXPECT_EQ(initial_value, *msg_pair->get_data()); EXPECT_EQ(initial_value, msg_pair->write_message().data); @@ -33,34 +34,33 @@ class MessagePairTest : public ::testing::Test { }; TEST_F(MessagePairTest, TestBasicTypes) { - test_message_interface(false, true, clock_); - test_message_interface(0.1, 0.2, clock_); - test_message_interface(1, 2, clock_); - test_message_interface("this", "that", clock_); + test_message_interface(MessageType::BOOL, false, true, clock_); + test_message_interface(MessageType::FLOAT64, 0.1, 0.2, clock_); + test_message_interface>( + MessageType::FLOAT64_MULTI_ARRAY, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, clock_ + ); + test_message_interface(MessageType::INT32, 1, 2, clock_); + test_message_interface(MessageType::STRING, "this", "that", clock_); } -TEST_F(MessagePairTest, TestDoubleArray) { - std::vector initial_value = {0.1, 0.2, 0.3}; - auto data = std::make_shared>(initial_value); - auto msg_pair = std::make_shared>>(data, clock_); - for (std::size_t i = 0; i < initial_value.size(); ++i) { - EXPECT_EQ(initial_value.at(i), msg_pair->get_data()->at(i)); - EXPECT_EQ(initial_value.at(i), msg_pair->write_message().data.at(i)); - } +TEST_F(MessagePairTest, TestCartesianState) { + auto initial_value = state_representation::CartesianState::Random("test"); + auto data = std::make_shared(initial_value); + auto msg_pair = std::make_shared>( + MessageType::ENCODED_STATE, data, clock_ + ); + EXPECT_TRUE(initial_value.data().isApprox(msg_pair->get_data()->data())); std::shared_ptr msg_pair_interface(msg_pair); - auto msg = msg_pair_interface->write_message>(); - for (std::size_t i = 0; i < initial_value.size(); ++i) { - EXPECT_EQ(initial_value.at(i), msg.data.at(i)); - } - EXPECT_EQ(initial_value, msg.data); + auto msg = msg_pair_interface->write_message(); + std::string tmp(msg.data.begin(), msg.data.end()); + auto decoded = clproto::decode(tmp); + EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); - std::vector new_value = {0.4, 0.5}; + auto new_value = state_representation::CartesianState::Identity("world"); *data = new_value; - msg = msg_pair_interface->write_message>(); - for (std::size_t i = 0; i < new_value.size(); ++i) { - EXPECT_EQ(new_value.at(i), msg_pair->get_data()->at(i)); - EXPECT_EQ(new_value.at(i), msg_pair->write_message().data.at(i)); - EXPECT_EQ(new_value.at(i), msg.data.at(i)); - } + msg = msg_pair_interface->write_message(); + tmp = std::string(msg.data.begin(), msg.data.end()); + decoded = clproto::decode(tmp); + EXPECT_TRUE(new_value.data().isApprox(decoded.data())); } From f45da7ff5ff277f6c3f06704f25ade28a3f397e5 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 7 Apr 2022 13:29:47 +0200 Subject: [PATCH 09/71] Add basic methods and attributes to component interface (#46) * Try to do code related to predicates * A few fixes * Rename predicate type and fixes --- .../modulo_components/ComponentInterface.h | 177 +++++++++++++++++- .../modulo_components/LifecycleComponent.h | 3 + .../exceptions/PredicateNotFoundException.hpp | 12 ++ .../utilities/predicate_variant.h | 9 + 4 files changed, 199 insertions(+), 2 deletions(-) create mode 100644 source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp create mode 100644 source/modulo_components/include/modulo_components/utilities/predicate_variant.h diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.h b/source/modulo_components/include/modulo_components/ComponentInterface.h index 69da18612..601de6265 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.h +++ b/source/modulo_components/include/modulo_components/ComponentInterface.h @@ -1,8 +1,20 @@ #pragma once #include -#include -#include +#include +#include +#include + +#include +#include +#include + +#include +#include + +#include "modulo_components/exceptions/PredicateNotFoundException.hpp" +#include "modulo_components/utilities/utilities.h" +#include "modulo_components/utilities/predicate_variant.h" namespace modulo_components { @@ -12,6 +24,167 @@ class ComponentInterface : public state_representation::ParameterMap, public Nod public: explicit ComponentInterface(const rclcpp::NodeOptions& node_options); +protected: + void add_predicate(const std::string& name, const std::function& predicate); + + void add_predicate(const std::string& name, bool predicate); + + void set_predicate(const std::string& name, const std::function& predicate); + + void set_predicate(const std::string& name, bool predicate); + + void send_transform(const state_representation::CartesianPose& transform); + + [[nodiscard]] state_representation::CartesianPose + lookup_transform(const std::string& frame_name, const std::string& reference_frame_name = "world") const; + +private: + [[nodiscard]] std::string generate_predicate_topic(const std::string& predicate_name) const; + + void add_predicate(const std::string& name, const utilities::PredicateVariant& predicate); + + void set_predicate(const std::string& name, const utilities::PredicateVariant& predicate); + + void step(); + + std::map predicates_; + std::map>> predicate_publishers_; + + std::shared_ptr step_timer_; + rclcpp::Parameter period_; + rclcpp::Parameter has_tf_listener_; + rclcpp::Parameter has_tf_broadcaster_; + std::shared_ptr tf_buffer_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_broadcaster_; }; +template +ComponentInterface::ComponentInterface(const rclcpp::NodeOptions& options) : + rclcpp::Node(utilities::parse_node_name(options, "ComponentInterface")) { + this->declare_parameter("period", 1.0); + this->declare_parameter("has_tf_listener", false); + this->declare_parameter("has_tf_broadcaster", false); + + // here I need to do typename NodeT:: because ParameterMap also has get_parameter + period_ = typename NodeT::get_parameter("has_tf_listener"); + has_tf_listener_ = typename NodeT::get_parameter("has_tf_listener"); + has_tf_broadcaster_ = typename NodeT::get_parameter("has_tf_broadcaster"); + if (has_tf_listener_.as_bool()) { + this->tf_buffer_ = std::make_shared(this->get_clock()); + this->tf_listener_ = std::make_shared(*this->tf_buffer_); + } + if (has_tf_broadcaster_.as_bool()) { + this->tf_broadcaster_ = std::make_shared(*this); + } + + this->step_timer_ = this->create_wall_timer( + std::chrono::nanoseconds(static_cast(this->period_.as_double() * 1e9)), [this] { this->step(); } + ); +} + +template +void ComponentInterface::step() { + for (const auto& predicate: this->predicates_) { + std_msgs::msg::Bool msg; + switch (predicate.second.index()) { + case 0: + msg.data = std::get<0>(predicate.second); + case 1: + msg.data = std::get<1>(predicate.second)(); + } + auto predicate_iterator = this->predicate_publishers_.find(predicate.first); + if (predicate_iterator == this->predicate_publishers_.end()) { + // TODO throw here + RCLCPP_FATAL(this->get_logger(), "no publisher for predicate found"); + return; + } + predicate_publishers_.at(predicate.first)->publish(msg); + } +} + +template +std::string ComponentInterface::generate_predicate_topic(const std::string& predicate_name) const { + return "/predicates/" + std::string(this->get_name()) + "/" + predicate_name; +} + +template +void +ComponentInterface::add_predicate(const std::string& name, const utilities::PredicateVariant& predicate) { + if (this->predicates_.find(name) != this->predicates_.end()) { + RCLCPP_INFO(this->get_logger(), "Predicate already exists, overwriting"); + this->predicates_.at(name) = predicate; + } else { + this->predicates_.insert(std::make_pair(name, predicate)); + // TODO add publisher with message interface + this->predicate_publishers_.insert( + std::make_pair( + name, this->template create_publisher(this->generate_predicate_topic(name), 10))); + } +} + +template +void ComponentInterface::add_predicate(const std::string& name, bool predicate) { + this->add_predicate(name, utilities::PredicateVariant(predicate)); +} + +template +void ComponentInterface::add_predicate( + const std::string& name, const std::function& predicate +) { + this->add_predicate(name, utilities::PredicateVariant(predicate)); +} + +template +void ComponentInterface::set_predicate( + const std::string& name, const utilities::PredicateVariant& predicate +) { + auto predicate_iterator = this->predicates_.find(name); + if (predicate_iterator == this->predicates_.end()) { + throw exceptions::PredicateNotFoundException(name); + } + this->predicates_.at(name) = predicate; +} + +template +void ComponentInterface::set_predicate(const std::string& name, bool predicate) { + this->set_predicate(name, utilities::PredicateVariant(predicate)); +} + +template +void ComponentInterface::set_predicate( + const std::string& name, const std::function& predicate +) { + this->set_predicate(name, utilities::PredicateVariant(predicate)); +} + +template +void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { + // TODO: throw here? + if (this->tf_broadcaster_ == nullptr) { + RCLCPP_FATAL(this->get_logger(), "No tf broadcaster"); + } + geometry_msgs::msg::TransformStamped tf_message; + modulo_new_core::translators::write_msg(tf_message, transform, this->get_clock()->now()); + this->tf_broadcaster_->sendTransform(tf_message); +} + +template +state_representation::CartesianPose ComponentInterface::lookup_transform( + const std::string& frame_name, const std::string& reference_frame_name +) const { + // TODO: throw here? + if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { + RCLCPP_FATAL(this->get_logger(), "No tf buffer / listener"); + } + geometry_msgs::msg::TransformStamped transform; + state_representation::CartesianPose result(frame_name, reference_frame_name); + // TODO: timeout + transform = this->tf_buffer_->lookupTransform( + reference_frame_name, frame_name, tf2::TimePoint(std::chrono::microseconds(0)), + tf2::Duration(std::chrono::microseconds(10))); + modulo_new_core::translators::read_msg(result, transform); + return result; +} + }// namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.h b/source/modulo_components/include/modulo_components/LifecycleComponent.h index 25facb0e8..ca9d0e220 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.h +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.h @@ -1,7 +1,10 @@ #pragma once #include "modulo_components/ComponentInterface.h" + #include "modulo_new_core/EncodedState.hpp" +#include +#include namespace modulo_components { diff --git a/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp b/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp new file mode 100644 index 000000000..e5c79e0fc --- /dev/null +++ b/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include +#include + +namespace modulo_components::exceptions { +class PredicateNotFoundException : public std::runtime_error { +public: + explicit PredicateNotFoundException(const std::string& predicate_name) : + runtime_error("Predicate " + predicate_name + " not found in the list of predicates") {}; +}; +}// namespace modulo::core::exceptions diff --git a/source/modulo_components/include/modulo_components/utilities/predicate_variant.h b/source/modulo_components/include/modulo_components/utilities/predicate_variant.h new file mode 100644 index 000000000..5ea249320 --- /dev/null +++ b/source/modulo_components/include/modulo_components/utilities/predicate_variant.h @@ -0,0 +1,9 @@ +#pragma once + +#include + +namespace modulo_components::utilities { + +typedef std::variant> PredicateVariant; + +}// namespace modulo_components::utilities \ No newline at end of file From ea8011db52ce4b889ddcf5baeecc124745a70854 Mon Sep 17 00:00:00 2001 From: Baptiste Busch Date: Fri, 8 Apr 2022 13:24:44 +0200 Subject: [PATCH 10/71] Add tests for the predicate interface (#48) * refactor: add the predicates as variant * fix: change for more verbose name Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> * fix: remove uneeded exception * fix: correct test naming convention * fix: correct typo * fix: remove periodic mention as it is called on need * fix: recreate the node on SetUp * fix: correct tests to use proxy functions over protected/private methods * fix: remove uneeded gtest include * fix: use shared_ptr instead of unique_ptr Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- source/modulo_components/CMakeLists.txt | 27 ++++- .../include/modulo_components/Component.h | 2 + .../modulo_components/ComponentInterface.h | 107 +++++++++++++----- .../modulo_components/LifecycleComponent.h | 2 + .../exceptions/PredicateNotFoundException.hpp | 2 +- .../src/ComponentInterface.cpp | 1 - .../test/cpp/test_component_interface.cpp | 79 +++++++++++++ 7 files changed, 187 insertions(+), 33 deletions(-) create mode 100644 source/modulo_components/test/cpp/test_component_interface.cpp diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index c0e7b5255..fe72eed7a 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 5.0.0 REQUIRED COMPONENTS state_representation) +find_package(control_libraries 5.1.0 REQUIRED COMPONENTS state_representation) include_directories(include) @@ -30,4 +30,29 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${PROJECT_SOURCE_DIR}/src/Component.cpp ${PROJECT_SOURCE_DIR}/src/LifecycleComponent.cpp) +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + # add cpp tests + file(GLOB_RECURSE TEST_CPP_SOURCES ${PROJECT_SOURCE_DIR}/test/ test_*.cpp) + + ament_add_gtest(test_cpp_components ${TEST_CPP_SOURCES}) + target_include_directories(test_cpp_components PRIVATE include) + target_link_libraries(test_cpp_components ${PROJECT_NAME} state_representation) + target_compile_definitions(test_cpp_components PRIVATE TEST_FIXTURES="${CMAKE_CURRENT_SOURCE_DIR}/test/fixtures/") + + # prevent pluginlib from using boost + target_compile_definitions(test_cpp_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + + # add python tests + file(GLOB_RECURSE TEST_PYTHON_SOURCES ${PROJECT_SOURCE_DIR}/test/ test_*.py) + + foreach(TEST_PYTHON IN LISTS TEST_PYTHON_SOURCES) + get_filename_component(TEST_FILENAME ${TEST_PYTHON} NAME_WE) + ament_add_pytest_test(${TEST_FILENAME} ${TEST_PYTHON}) + endforeach() + +endif() + ament_auto_package() diff --git a/source/modulo_components/include/modulo_components/Component.h b/source/modulo_components/include/modulo_components/Component.h index 3fdb58673..67ffbc096 100644 --- a/source/modulo_components/include/modulo_components/Component.h +++ b/source/modulo_components/include/modulo_components/Component.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "modulo_components/ComponentInterface.h" #include "modulo_new_core/EncodedState.hpp" diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.h b/source/modulo_components/include/modulo_components/ComponentInterface.h index 601de6265..97d71a04e 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.h +++ b/source/modulo_components/include/modulo_components/ComponentInterface.h @@ -1,9 +1,9 @@ #pragma once -#include #include #include #include +#include #include #include @@ -19,19 +19,54 @@ namespace modulo_components { template -class ComponentInterface : public state_representation::ParameterMap, public NodeT { +class ComponentInterface : NodeT { + friend class ComponentInterfaceTest; public: + /** + * @brief Constructor from node options + * @param node_options node options as used in ROS2 Node + */ explicit ComponentInterface(const rclcpp::NodeOptions& node_options); protected: - void add_predicate(const std::string& name, const std::function& predicate); - - void add_predicate(const std::string& name, bool predicate); - - void set_predicate(const std::string& name, const std::function& predicate); - - void set_predicate(const std::string& name, bool predicate); + /** + * @brief Add a predicate to the map of predicates + * @param predicate_name the name of the associated predicate + * @param predicate_value the boolean value of the predicate + */ + void add_predicate(const std::string& predicate_name, bool predicate_value); + + /** + * @brief Add a predicate to the map of predicates based on a function to periodically call + * @param predicate_name the name of the associated predicate + * @param predicate_function the function to call that returns the value of the predicate + */ + void add_predicate(const std::string& predicate_name, const std::function& predicate_function); + + /** + * @brief Get the value of the predicate given as parameter + * @param predicate_name the name of the predicate to retrieve from the + * map of predicates + * @return the value of the predicate as a boolean + */ + [[nodiscard]] bool get_predicate(const std::string& predicate_name) const; + + /** + * @brief Set the value of the predicate given as parameter + * @param predicate_name the name of the predicate to retrieve from the + * map of predicates + * @param predicate_value the new value of the predicate + */ + void set_predicate(const std::string& predicate_name, bool predicate_value); + + /** + * @brief Set the value of the predicate given as parameter + * @param predicate_name the name of the predicate to retrieve from the + * map of predicates + * @param predicate_function the function to periodically call that returns the value of the predicate + */ + void set_predicate(const std::string& predicate_name, const std::function& predicate_function); void send_transform(const state_representation::CartesianPose& transform); @@ -41,9 +76,9 @@ class ComponentInterface : public state_representation::ParameterMap, public Nod private: [[nodiscard]] std::string generate_predicate_topic(const std::string& predicate_name) const; - void add_predicate(const std::string& name, const utilities::PredicateVariant& predicate); + void add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); - void set_predicate(const std::string& name, const utilities::PredicateVariant& predicate); + void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); void step(); @@ -61,21 +96,22 @@ class ComponentInterface : public state_representation::ParameterMap, public Nod template ComponentInterface::ComponentInterface(const rclcpp::NodeOptions& options) : - rclcpp::Node(utilities::parse_node_name(options, "ComponentInterface")) { - this->declare_parameter("period", 1.0); + rclcpp::Node(utilities::parse_node_name(options, "ComponentInterface"), options) { + this->declare_parameter("period", 2.0); this->declare_parameter("has_tf_listener", false); this->declare_parameter("has_tf_broadcaster", false); // here I need to do typename NodeT:: because ParameterMap also has get_parameter - period_ = typename NodeT::get_parameter("has_tf_listener"); - has_tf_listener_ = typename NodeT::get_parameter("has_tf_listener"); - has_tf_broadcaster_ = typename NodeT::get_parameter("has_tf_broadcaster"); + period_ = NodeT::get_parameter("period"); + has_tf_listener_ = NodeT::get_parameter("has_tf_listener"); + has_tf_broadcaster_ = NodeT::get_parameter("has_tf_broadcaster"); + if (has_tf_listener_.as_bool()) { this->tf_buffer_ = std::make_shared(this->get_clock()); this->tf_listener_ = std::make_shared(*this->tf_buffer_); } if (has_tf_broadcaster_.as_bool()) { - this->tf_broadcaster_ = std::make_shared(*this); + this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); } this->step_timer_ = this->create_wall_timer( @@ -87,12 +123,7 @@ template void ComponentInterface::step() { for (const auto& predicate: this->predicates_) { std_msgs::msg::Bool msg; - switch (predicate.second.index()) { - case 0: - msg.data = std::get<0>(predicate.second); - case 1: - msg.data = std::get<1>(predicate.second)(); - } + msg.data = this->get_predicate(predicate.first); auto predicate_iterator = this->predicate_publishers_.find(predicate.first); if (predicate_iterator == this->predicate_publishers_.end()) { // TODO throw here @@ -110,7 +141,7 @@ std::string ComponentInterface::generate_predicate_topic(const std: template void -ComponentInterface::add_predicate(const std::string& name, const utilities::PredicateVariant& predicate) { +ComponentInterface::add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate) { if (this->predicates_.find(name) != this->predicates_.end()) { RCLCPP_INFO(this->get_logger(), "Predicate already exists, overwriting"); this->predicates_.at(name) = predicate; @@ -125,18 +156,34 @@ ComponentInterface::add_predicate(const std::string& name, const ut template void ComponentInterface::add_predicate(const std::string& name, bool predicate) { - this->add_predicate(name, utilities::PredicateVariant(predicate)); + this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } template void ComponentInterface::add_predicate( const std::string& name, const std::function& predicate ) { - this->add_predicate(name, utilities::PredicateVariant(predicate)); + this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } template -void ComponentInterface::set_predicate( +bool ComponentInterface::get_predicate(const std::string& predicate_name) const { + auto predicate_iterator = this->predicates_.find(predicate_name); + if (predicate_iterator == this->predicates_.end()) { + throw exceptions::PredicateNotFoundException(predicate_name); + } + // try to get the value from the variant as a bool + auto* ptr_value = std::get_if(&predicate_iterator->second); + if (ptr_value) { + return *ptr_value; + } + // if previous check failed, it means the variant is actually a callback function + auto callback_function = std::get>(predicate_iterator->second); + return (callback_function)(); +} + +template +void ComponentInterface::set_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate ) { auto predicate_iterator = this->predicates_.find(name); @@ -148,14 +195,14 @@ void ComponentInterface::set_predicate( template void ComponentInterface::set_predicate(const std::string& name, bool predicate) { - this->set_predicate(name, utilities::PredicateVariant(predicate)); + this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } template void ComponentInterface::set_predicate( const std::string& name, const std::function& predicate ) { - this->set_predicate(name, utilities::PredicateVariant(predicate)); + this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } template @@ -187,4 +234,4 @@ state_representation::CartesianPose ComponentInterface::lookup_tran return result; } -}// namespace modulo_components \ No newline at end of file +}// namespace modulo_components diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.h b/source/modulo_components/include/modulo_components/LifecycleComponent.h index ca9d0e220..ad4cc0b21 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.h +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.h @@ -1,5 +1,7 @@ #pragma once +#include + #include "modulo_components/ComponentInterface.h" #include "modulo_new_core/EncodedState.hpp" diff --git a/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp b/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp index e5c79e0fc..b489c8caa 100644 --- a/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp @@ -9,4 +9,4 @@ class PredicateNotFoundException : public std::runtime_error { explicit PredicateNotFoundException(const std::string& predicate_name) : runtime_error("Predicate " + predicate_name + " not found in the list of predicates") {}; }; -}// namespace modulo::core::exceptions +}// namespace modulo_components::exceptions diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index bbfde9bb3..04d4a87a1 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -3,5 +3,4 @@ namespace modulo_components { - }// namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp new file mode 100644 index 000000000..ff3016c2b --- /dev/null +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -0,0 +1,79 @@ +#include +#include +#include +#include + +#include "modulo_components/ComponentInterface.h" +#include "modulo_new_core/EncodedState.hpp" + +namespace modulo_components { +class ComponentInterfaceTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + void SetUp() override { + component_ = std::make_shared>(rclcpp::NodeOptions()); + } + + std::map get_predicate_map() { + return component_->predicates_; + } + + void add_predicate(const std::string& name, bool value) { + component_->add_predicate(name, value); + } + + void add_predicate(const std::string& predicate_name, const std::function& predicate_function) { + component_->add_predicate(predicate_name, predicate_function); + } + + bool get_predicate(const std::string& predicate_name) const { + return component_->get_predicate(predicate_name); + } + + void set_predicate(const std::string& predicate_name, bool predicate_value) { + component_->set_predicate(predicate_name, predicate_value); + } + + std::shared_ptr> component_; +}; + +TEST_F(ComponentInterfaceTest, AddBoolPredicate) { + add_predicate("foo", true); + auto predicates = get_predicate_map(); + auto predicate_iterator = predicates.find("foo"); + EXPECT_TRUE(predicate_iterator != predicates.end()); + auto value = std::get(predicate_iterator->second); + EXPECT_TRUE(value); +} + +TEST_F(ComponentInterfaceTest, AddFunctionPredicate) { + add_predicate("bar", [&]() { return false; }); + auto predicates = get_predicate_map(); + auto predicate_iterator = predicates.find("bar"); + EXPECT_TRUE(predicate_iterator != predicates.end()); + auto value_callback = std::get>(predicate_iterator->second); + EXPECT_FALSE((value_callback)()); +} + +TEST_F(ComponentInterfaceTest, GetPredicateValue) { + add_predicate("foo", true); + EXPECT_TRUE(get_predicate("foo")); + add_predicate("bar", [&]() { return false; }); + EXPECT_FALSE(get_predicate("bar")); + EXPECT_THROW(get_predicate("test"), exceptions::PredicateNotFoundException); +} + +TEST_F(ComponentInterfaceTest, SetPredicateValue) { + add_predicate("foo", true); + set_predicate("foo", false); + EXPECT_FALSE(get_predicate("foo")); + EXPECT_THROW(set_predicate("bar", true), exceptions::PredicateNotFoundException); + add_predicate("bar", [&]() { return false; }); + set_predicate("bar", true); + EXPECT_TRUE(get_predicate("bar")); +} + +} // namespace modulo_components \ No newline at end of file From fbdb48b69a1b9e1f8a10a7b11ee47404ba23121d Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 8 Apr 2022 15:02:43 +0200 Subject: [PATCH 11/71] Add publisher interface and handler (#47) * Improve message interface * Add publisher interface * Add publisher handler * Remove PubT template from ComponentInterface * Fixes * Rename all h to hpp files --- .../include/modulo_components/Component.h | 14 ---- .../include/modulo_components/Component.hpp | 14 ++++ ...nentInterface.h => ComponentInterface.hpp} | 68 +++++++++++-------- ...ycleComponent.h => LifecycleComponent.hpp} | 5 +- ...dicate_variant.h => predicate_variant.hpp} | 0 .../utilities/{utilities.h => utilities.hpp} | 0 source/modulo_components/src/Component.cpp | 2 +- .../src/ComponentInterface.cpp | 2 +- .../src/LifecycleComponent.cpp | 2 +- .../test/cpp/test_component_interface.cpp | 8 ++- source/modulo_new_core/CMakeLists.txt | 1 + .../communication/MessagePair.hpp | 17 +++-- .../communication/MessagePairInterface.hpp | 12 ++-- .../communication/PublisherHandler.hpp | 27 ++++++++ .../communication/PublisherInterface.hpp | 41 +++++++++++ .../communication/PublisherType.hpp | 9 +++ .../InvalidMessagePairCastException.hpp | 11 --- .../InvalidPointerCastException.hpp | 11 +++ .../exceptions/NullPointerException.hpp | 11 +++ .../src/communication/PublisherInterface.cpp | 22 ++++++ .../communication/test_message_pair.cpp | 8 +-- 21 files changed, 202 insertions(+), 83 deletions(-) delete mode 100644 source/modulo_components/include/modulo_components/Component.h create mode 100644 source/modulo_components/include/modulo_components/Component.hpp rename source/modulo_components/include/modulo_components/{ComponentInterface.h => ComponentInterface.hpp} (81%) rename source/modulo_components/include/modulo_components/{LifecycleComponent.h => LifecycleComponent.hpp} (61%) rename source/modulo_components/include/modulo_components/utilities/{predicate_variant.h => predicate_variant.hpp} (100%) rename source/modulo_components/include/modulo_components/utilities/{utilities.h => utilities.hpp} (100%) create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp delete mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp create mode 100644 source/modulo_new_core/src/communication/PublisherInterface.cpp diff --git a/source/modulo_components/include/modulo_components/Component.h b/source/modulo_components/include/modulo_components/Component.h deleted file mode 100644 index 67ffbc096..000000000 --- a/source/modulo_components/include/modulo_components/Component.h +++ /dev/null @@ -1,14 +0,0 @@ -#pragma once - -#include - -#include "modulo_components/ComponentInterface.h" -#include "modulo_new_core/EncodedState.hpp" - -namespace modulo_components { - -class Component : public ComponentInterface> { - -}; - -} \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp new file mode 100644 index 000000000..218471732 --- /dev/null +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -0,0 +1,14 @@ +#pragma once + +#include + +#include "modulo_components/ComponentInterface.hpp" +#include "modulo_new_core/EncodedState.hpp" + +namespace modulo_components { + +class Component : public ComponentInterface { + +}; + +} \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.h b/source/modulo_components/include/modulo_components/ComponentInterface.hpp similarity index 81% rename from source/modulo_components/include/modulo_components/ComponentInterface.h rename to source/modulo_components/include/modulo_components/ComponentInterface.hpp index 97d71a04e..60fec2ff4 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.h +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -9,16 +9,17 @@ #include #include +#include #include #include #include "modulo_components/exceptions/PredicateNotFoundException.hpp" -#include "modulo_components/utilities/utilities.h" -#include "modulo_components/utilities/predicate_variant.h" +#include "modulo_components/utilities/utilities.hpp" +#include "modulo_components/utilities/predicate_variant.hpp" namespace modulo_components { -template +template class ComponentInterface : NodeT { friend class ComponentInterfaceTest; @@ -27,7 +28,9 @@ class ComponentInterface : NodeT { * @brief Constructor from node options * @param node_options node options as used in ROS2 Node */ - explicit ComponentInterface(const rclcpp::NodeOptions& node_options); + explicit ComponentInterface( + const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type + ); protected: /** @@ -82,6 +85,8 @@ class ComponentInterface : NodeT { void step(); + modulo_new_core::communication::PublisherType publisher_type_; + std::map predicates_; std::map>> predicate_publishers_; @@ -94,9 +99,11 @@ class ComponentInterface : NodeT { std::shared_ptr tf_broadcaster_; }; -template -ComponentInterface::ComponentInterface(const rclcpp::NodeOptions& options) : - rclcpp::Node(utilities::parse_node_name(options, "ComponentInterface"), options) { +template +ComponentInterface::ComponentInterface( + const rclcpp::NodeOptions& options, modulo_new_core::communication::PublisherType publisher_type +) : + rclcpp::Node(utilities::parse_node_name(options, "ComponentInterface"), options), publisher_type_(publisher_type) { this->declare_parameter("period", 2.0); this->declare_parameter("has_tf_listener", false); this->declare_parameter("has_tf_broadcaster", false); @@ -119,8 +126,8 @@ ComponentInterface::ComponentInterface(const rclcpp::NodeOptions& o ); } -template -void ComponentInterface::step() { +template +void ComponentInterface::step() { for (const auto& predicate: this->predicates_) { std_msgs::msg::Bool msg; msg.data = this->get_predicate(predicate.first); @@ -134,14 +141,15 @@ void ComponentInterface::step() { } } -template -std::string ComponentInterface::generate_predicate_topic(const std::string& predicate_name) const { +template +std::string ComponentInterface::generate_predicate_topic(const std::string& predicate_name) const { return "/predicates/" + std::string(this->get_name()) + "/" + predicate_name; } -template -void -ComponentInterface::add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate) { +template +void ComponentInterface::add_variant_predicate( + const std::string& name, const utilities::PredicateVariant& predicate +) { if (this->predicates_.find(name) != this->predicates_.end()) { RCLCPP_INFO(this->get_logger(), "Predicate already exists, overwriting"); this->predicates_.at(name) = predicate; @@ -154,20 +162,20 @@ ComponentInterface::add_variant_predicate(const std::string& name, } } -template -void ComponentInterface::add_predicate(const std::string& name, bool predicate) { +template +void ComponentInterface::add_predicate(const std::string& name, bool predicate) { this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template -void ComponentInterface::add_predicate( +template +void ComponentInterface::add_predicate( const std::string& name, const std::function& predicate ) { this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template -bool ComponentInterface::get_predicate(const std::string& predicate_name) const { +template +bool ComponentInterface::get_predicate(const std::string& predicate_name) const { auto predicate_iterator = this->predicates_.find(predicate_name); if (predicate_iterator == this->predicates_.end()) { throw exceptions::PredicateNotFoundException(predicate_name); @@ -182,8 +190,8 @@ bool ComponentInterface::get_predicate(const std::string& predicate return (callback_function)(); } -template -void ComponentInterface::set_variant_predicate( +template +void ComponentInterface::set_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate ) { auto predicate_iterator = this->predicates_.find(name); @@ -193,20 +201,20 @@ void ComponentInterface::set_variant_predicate( this->predicates_.at(name) = predicate; } -template -void ComponentInterface::set_predicate(const std::string& name, bool predicate) { +template +void ComponentInterface::set_predicate(const std::string& name, bool predicate) { this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template -void ComponentInterface::set_predicate( +template +void ComponentInterface::set_predicate( const std::string& name, const std::function& predicate ) { this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } -template -void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { +template +void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { // TODO: throw here? if (this->tf_broadcaster_ == nullptr) { RCLCPP_FATAL(this->get_logger(), "No tf broadcaster"); @@ -216,8 +224,8 @@ void ComponentInterface::send_transform(const state_representation: this->tf_broadcaster_->sendTransform(tf_message); } -template -state_representation::CartesianPose ComponentInterface::lookup_transform( +template +state_representation::CartesianPose ComponentInterface::lookup_transform( const std::string& frame_name, const std::string& reference_frame_name ) const { // TODO: throw here? diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.h b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp similarity index 61% rename from source/modulo_components/include/modulo_components/LifecycleComponent.h rename to source/modulo_components/include/modulo_components/LifecycleComponent.hpp index ad4cc0b21..eeda138be 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.h +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -2,7 +2,7 @@ #include -#include "modulo_components/ComponentInterface.h" +#include "modulo_components/ComponentInterface.hpp" #include "modulo_new_core/EncodedState.hpp" #include @@ -10,8 +10,7 @@ namespace modulo_components { -class LifecycleComponent : public ComponentInterface> { +class LifecycleComponent : public ComponentInterface { }; diff --git a/source/modulo_components/include/modulo_components/utilities/predicate_variant.h b/source/modulo_components/include/modulo_components/utilities/predicate_variant.hpp similarity index 100% rename from source/modulo_components/include/modulo_components/utilities/predicate_variant.h rename to source/modulo_components/include/modulo_components/utilities/predicate_variant.hpp diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.h b/source/modulo_components/include/modulo_components/utilities/utilities.hpp similarity index 100% rename from source/modulo_components/include/modulo_components/utilities/utilities.h rename to source/modulo_components/include/modulo_components/utilities/utilities.hpp diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 3843f4ec5..5c3128de4 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -1 +1 @@ -#include "modulo_components/Component.h" \ No newline at end of file +#include "modulo_components/Component.hpp" \ No newline at end of file diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 04d4a87a1..d7dcf1aed 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -1,4 +1,4 @@ -#include "modulo_components/ComponentInterface.h" +#include "modulo_components/ComponentInterface.hpp" namespace modulo_components { diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index f342be47a..8788cd275 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -1 +1 @@ -#include "modulo_components/LifecycleComponent.h" \ No newline at end of file +#include "modulo_components/LifecycleComponent.hpp" \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index ff3016c2b..ccf1bd69f 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -3,7 +3,7 @@ #include #include -#include "modulo_components/ComponentInterface.h" +#include "modulo_components/ComponentInterface.hpp" #include "modulo_new_core/EncodedState.hpp" namespace modulo_components { @@ -14,7 +14,9 @@ class ComponentInterfaceTest : public ::testing::Test { } void SetUp() override { - component_ = std::make_shared>(rclcpp::NodeOptions()); + component_ = std::make_shared>( + rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::PUBLISHER + ); } std::map get_predicate_map() { @@ -37,7 +39,7 @@ class ComponentInterfaceTest : public ::testing::Test { component_->set_predicate(predicate_name, predicate_value); } - std::shared_ptr> component_; + std::shared_ptr> component_; }; TEST_F(ComponentInterfaceTest, AddBoolPredicate) { diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index 73cbb0d98..fc0ef2f8e 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -28,6 +28,7 @@ include_directories(include) ament_auto_add_library(modulo_new_core SHARED ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp + ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp ${PROJECT_SOURCE_DIR}/src/translators/ReadStateConversion.cpp ${PROJECT_SOURCE_DIR}/src/translators/WriteStateConversion.cpp) diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index 67a1b3e39..1da958dd5 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -1,29 +1,25 @@ #pragma once #include "modulo_new_core/communication/MessagePairInterface.hpp" +#include "modulo_new_core/exceptions/NullPointerException.hpp" #include "modulo_new_core/translators/WriteStateConversion.hpp" #include -#include -#include -#include -#include -#include namespace modulo_new_core::communication { template class MessagePair : public MessagePairInterface { -private: - std::shared_ptr data_; - std::shared_ptr clock_; - public: MessagePair(MessageType type, std::shared_ptr data, std::shared_ptr clock); [[nodiscard]] MsgT write_message() const; [[nodiscard]] std::shared_ptr get_data() const; + +private: + std::shared_ptr data_; + std::shared_ptr clock_; }; template @@ -34,6 +30,9 @@ MessagePair::MessagePair( template MsgT MessagePair::write_message() const { + if (this->data_ == nullptr) { + throw exceptions::NullPointerException("The message pair data is not set, nothing to write"); + } auto msg = MsgT(); translators::write_msg(msg, *this->data_, clock_->now()); return msg; diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp index dd36796ef..a40b330ed 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp @@ -3,7 +3,7 @@ #include #include "modulo_new_core/communication/MessageType.hpp" -#include "modulo_new_core/exceptions/InvalidMessagePairCastException.hpp" +#include "modulo_new_core/exceptions/InvalidPointerCastException.hpp" #include "modulo_new_core/exceptions/InvalidPointerException.hpp" namespace modulo_new_core::communication { @@ -21,10 +21,10 @@ class MessagePairInterface : public std::enable_shared_from_this - std::shared_ptr> get_message_pair(bool validate_pointer = true); + [[nodiscard]] std::shared_ptr> get_message_pair(bool validate_pointer = true); template - MsgT write_message(); + [[nodiscard]] MsgT write(); MessageType get_type() const; @@ -43,15 +43,15 @@ inline std::shared_ptr> MessagePairInterface::get_messa } } if (message_pair_ptr == nullptr && validate_pointer) { - throw exceptions::InvalidMessagePairCastException( - "Unable to case message pair interface to a message pair pointer of requested type" + throw exceptions::InvalidPointerCastException( + "Unable to cast message pair interface to a message pair pointer of requested type" ); } return message_pair_ptr; } template -inline MsgT MessagePairInterface::write_message() { +inline MsgT MessagePairInterface::write() { return this->template get_message_pair()->write_message(); } diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp new file mode 100644 index 000000000..5b0d9e583 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp @@ -0,0 +1,27 @@ +#pragma once + +#include "modulo_new_core/communication/PublisherInterface.hpp" + +namespace modulo_new_core::communication { + +template +class PublisherHandler : public PublisherInterface { +public: + PublisherHandler(PublisherType type, std::shared_ptr publisher); + + void publish(const MsgT& message) const; + +private: + std::shared_ptr publisher_; +}; + +template +PublisherHandler::PublisherHandler(PublisherType type, std::shared_ptr publisher) : + PublisherInterface(type), publisher_(std::move(publisher)) {} + +template +void PublisherHandler::publish(const MsgT& message) const { + this->publisher_->publish(message); +} + +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp new file mode 100644 index 000000000..c21e54a01 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp @@ -0,0 +1,41 @@ +#pragma once + +#include + +#include "modulo_new_core/communication/MessagePair.hpp" +#include "modulo_new_core/communication/PublisherType.hpp" +#include "modulo_new_core/exceptions/InvalidPointerCastException.hpp" +#include "modulo_new_core/exceptions/InvalidPointerException.hpp" + +namespace modulo_new_core::communication { + +// forward declaration of derived Publisher class +template +class PublisherHandler; + +class PublisherInterface : public std::enable_shared_from_this { +public: + explicit PublisherInterface(PublisherType type); + + PublisherInterface(const PublisherInterface& publisher) = default; + + virtual ~PublisherInterface() = default; + + template + std::shared_ptr> get_publisher(bool validate_pointer = true); + + void publish(); + + void set_message_pair(const std::shared_ptr& message_pair); + + PublisherType get_type() const; + +private: + template + void publish(const MsgT& message); + + PublisherType type_; + std::shared_ptr message_pair_; +}; + +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp new file mode 100644 index 000000000..5d756312a --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp @@ -0,0 +1,9 @@ +#pragma once + +namespace modulo_new_core::communication { + +enum class PublisherType { + PUBLISHER, LIFECYCLE_PUBLISHER +}; + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp deleted file mode 100644 index c93eb679f..000000000 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidMessagePairCastException.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo_new_core::exceptions { -class InvalidMessagePairCastException : public std::runtime_error { -public: - explicit InvalidMessagePairCastException(const std::string& msg) : std::runtime_error(msg) {}; -}; -} \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp new file mode 100644 index 000000000..0c5584b8c --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace modulo_new_core::exceptions { +class InvalidPointerCastException : public std::runtime_error { +public: + explicit InvalidPointerCastException(const std::string& msg) : std::runtime_error(msg) {}; +}; +} \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp new file mode 100644 index 000000000..27fbaa247 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace modulo_new_core::exceptions { +class NullPointerException : public std::runtime_error { +public: + explicit NullPointerException(const std::string& msg) : std::runtime_error(msg) {}; +}; +} \ No newline at end of file diff --git a/source/modulo_new_core/src/communication/PublisherInterface.cpp b/source/modulo_new_core/src/communication/PublisherInterface.cpp new file mode 100644 index 000000000..7508363ed --- /dev/null +++ b/source/modulo_new_core/src/communication/PublisherInterface.cpp @@ -0,0 +1,22 @@ +#include "modulo_new_core/communication/PublisherInterface.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include "modulo_new_core/communication/PublisherHandler.hpp" +#include "modulo_new_core/exceptions/NullPointerException.hpp" + +namespace modulo_new_core::communication { + +PublisherInterface::PublisherInterface(PublisherType type) : type_(type) {} + +PublisherType PublisherInterface::get_type() const { + return this->type_; +} + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp index 7e159d2f1..d14827015 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -14,13 +14,13 @@ static void test_message_interface( EXPECT_EQ(initial_value, msg_pair->write_message().data); std::shared_ptr msg_pair_interface(msg_pair); - auto msg = msg_pair_interface->template write_message(); + auto msg = msg_pair_interface->template write(); EXPECT_EQ(initial_value, msg.data); *data = new_value; EXPECT_EQ(new_value, *msg_pair->get_data()); EXPECT_EQ(new_value, msg_pair->write_message().data); - msg = msg_pair_interface->template write_message(); + msg = msg_pair_interface->template write(); EXPECT_EQ(new_value, msg.data); } @@ -52,14 +52,14 @@ TEST_F(MessagePairTest, TestCartesianState) { EXPECT_TRUE(initial_value.data().isApprox(msg_pair->get_data()->data())); std::shared_ptr msg_pair_interface(msg_pair); - auto msg = msg_pair_interface->write_message(); + auto msg = msg_pair_interface->write(); std::string tmp(msg.data.begin(), msg.data.end()); auto decoded = clproto::decode(tmp); EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); auto new_value = state_representation::CartesianState::Identity("world"); *data = new_value; - msg = msg_pair_interface->write_message(); + msg = msg_pair_interface->write(); tmp = std::string(msg.data.begin(), msg.data.end()); decoded = clproto::decode(tmp); EXPECT_TRUE(new_value.data().isApprox(decoded.data())); From 0516f08ad16fcc6c0de0796584a01d4e020ea523 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 8 Apr 2022 17:22:43 +0200 Subject: [PATCH 12/71] Fix modulo_core after StateType refactor (#49) * Use devel image * Fix modulo_core --- build-server.sh | 2 +- source/modulo_core/src/Recorder.cpp | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/build-server.sh b/build-server.sh index ed9a726d8..edd02599b 100755 --- a/build-server.sh +++ b/build-server.sh @@ -1,5 +1,5 @@ #!/bin/bash -BASE_TAG=galactic-devel +BASE_TAG=galactic-devel-state-type IMAGE_NAME=epfl-lasa/modulo IMAGE_TAG=latest diff --git a/source/modulo_core/src/Recorder.cpp b/source/modulo_core/src/Recorder.cpp index 87df3cc68..1629efd9d 100644 --- a/source/modulo_core/src/Recorder.cpp +++ b/source/modulo_core/src/Recorder.cpp @@ -1,5 +1,7 @@ #include "modulo_core/Recorder.hpp" +#include + namespace modulo::core { Recorder::~Recorder() { this->on_shutdown(); From 9f148c686346f8d9bcaf9b7526a3b7460e90aae1 Mon Sep 17 00:00:00 2001 From: Baptiste Busch Date: Sat, 9 Apr 2022 15:29:30 +0200 Subject: [PATCH 13/71] Add the base ComponentInterface class in python (#51) * feat: add the base ComponentInterface class in python * fix: add and correct docstring * fix: remove TODO about publishers as we use normal publishers for predicate * fix: change for predicate_publishers * fix: correct docstrings Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- source/modulo_components/CMakeLists.txt | 3 + .../modulo_components/ComponentInterface.hpp | 10 +- .../modulo_components/__init__.py | 0 .../modulo_components/component_interface.py | 120 ++++++++++++++++++ .../modulo_components/test/python/conftest.py | 9 ++ .../test/python/test_component_interface.py | 48 +++++++ 6 files changed, 185 insertions(+), 5 deletions(-) create mode 100644 source/modulo_components/modulo_components/__init__.py create mode 100644 source/modulo_components/modulo_components/component_interface.py create mode 100644 source/modulo_components/test/python/conftest.py create mode 100644 source/modulo_components/test/python/test_component_interface.py diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index fe72eed7a..74fb209fe 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -30,6 +30,9 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${PROJECT_SOURCE_DIR}/src/Component.cpp ${PROJECT_SOURCE_DIR}/src/LifecycleComponent.cpp) +# Install Python modules +ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) + if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 60fec2ff4..1faa37503 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -48,7 +48,8 @@ class ComponentInterface : NodeT { void add_predicate(const std::string& predicate_name, const std::function& predicate_function); /** - * @brief Get the value of the predicate given as parameter + * @brief Get the logical value of a predicate. + * @details If the predicate is not found or the callable function fails, the return value is false. * @param predicate_name the name of the predicate to retrieve from the * map of predicates * @return the value of the predicate as a boolean @@ -56,7 +57,7 @@ class ComponentInterface : NodeT { [[nodiscard]] bool get_predicate(const std::string& predicate_name) const; /** - * @brief Set the value of the predicate given as parameter + * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything * @param predicate_name the name of the predicate to retrieve from the * map of predicates * @param predicate_value the new value of the predicate @@ -64,10 +65,10 @@ class ComponentInterface : NodeT { void set_predicate(const std::string& predicate_name, bool predicate_value); /** - * @brief Set the value of the predicate given as parameter + * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything * @param predicate_name the name of the predicate to retrieve from the * map of predicates - * @param predicate_function the function to periodically call that returns the value of the predicate + * @param predicate_function the function to call that returns the value of the predicate */ void set_predicate(const std::string& predicate_name, const std::function& predicate_function); @@ -155,7 +156,6 @@ void ComponentInterface::add_variant_predicate( this->predicates_.at(name) = predicate; } else { this->predicates_.insert(std::make_pair(name, predicate)); - // TODO add publisher with message interface this->predicate_publishers_.insert( std::make_pair( name, this->template create_publisher(this->generate_predicate_topic(name), 10))); diff --git a/source/modulo_components/modulo_components/__init__.py b/source/modulo_components/modulo_components/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py new file mode 100644 index 000000000..79f344f45 --- /dev/null +++ b/source/modulo_components/modulo_components/component_interface.py @@ -0,0 +1,120 @@ +from rclpy.node import Node +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener +from tf2_ros import TransformBroadcaster +from rcl_interfaces.msg import ParameterDescriptor +from std_msgs.msg import Bool + +from typing import Union +from typing import Callable + + +class ComponentInterface(Node): + """ + Abstract class to represent a Component in python, following the same logic pattern + as the c++ modulo_component::ComponentInterface class. + ... + Attributes: + predicates (dict(Parameters(bool))): map of predicates added to the Component. + predicate_publishers (dict(rclpy.Publisher(Bool))): map of publishers associated to each predicate. + tf_buffer (tf2_ros.Buffer): the buffer to lookup transforms published on tf2. + tf_listener (tf2_ros.TransformListener): the listener to lookup transforms published on tf2. + tf_broadcaster (tf2_ros.TransformBroadcaster): the broadcaster to publish transforms on tf2 + Parameters: + period (double): period (in s) between step function calls. + has_tf_listener (bool): if true, add a tf listener to enable calls to lookup transforms. + has_tf_broadcaster (bool): if true, add a tf broadcaster to enable the publishing of transforms. + """ + + def __init__(self, node_name, *kargs, **kwargs): + """ + Constructs all the necessary attributes and declare all the parameters. + Parameters: + node_name (str): name of the node to be passed to the base Node class + """ + super().__init__(node_name, *kargs, **kwargs) + self.declare_parameter('period', 0.1, + ParameterDescriptor(description="Period (in s) between step function calls.")) + self.declare_parameter('has_tf_listener', False, + ParameterDescriptor( + description="If true, add a tf listener to enable calls to lookup transforms.")) + self.declare_parameter('has_tf_broadcaster', False, + ParameterDescriptor( + description="If true, add a tf broadcaster to enable the publishing of transforms.")) + + self._period = self.get_parameter('period').get_parameter_value().double_value + self._has_tf_listener = self.get_parameter('has_tf_listener').get_parameter_value().double_value + self._has_tf_broadcaster = self.get_parameter('has_tf_broadcaster').get_parameter_value().double_value + self._predicates = {} + self._predicate_publishers = {} + + if self._has_tf_listener: + self.__tf_buffer = Buffer() + self.__tf_listener = TransformListener(self.__tf_buffer, self) + if self._has_tf_broadcaster: + self.__tf_broadcaster = TransformBroadcaster(self) + + self.create_timer(self._period, self.__step) + + def __step(self): + for predicate_name in self._predicates.keys(): + msg = Bool() + msg.data = self.get_predicate(predicate_name) + if predicate_name not in self._predicate_publishers.keys(): + self.get_logger().error(f"No publisher for predicate {predicate_name} found.", + throttle_duration_sec=1.0) + return + self._predicate_publishers[predicate_name].publish(msg) + + def __generate_predicate_topic(self, predicate_name: str) -> str: + return f'/predicates/{self.get_name()}/{predicate_name}' + + def add_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable]): + """ + Add a predicate to the map of predicates. + + :param predicate_name: the name of the associated predicate + :param predicate_value: the value of the predicate as a bool or a callable function + """ + if predicate_name in self._predicates.keys(): + self.get_logger().debug(f"Predicate {predicate_name} already exists, overwriting.") + else: + self._predicate_publishers[predicate_name] = self.create_publisher(Bool, self.__generate_predicate_topic( + predicate_name), 10) + self._predicates[predicate_name] = predicate_value + + def get_predicate(self, predicate_name: str) -> bool: + """ + Get the value of the predicate given as parameter. If the predicate is not found or the callable function fails, + return false. + + :param predicate_name: the name of the predicate to retrieve from the map of predicates + :return: the value of the predicate as a boolean + """ + if predicate_name not in self._predicates.keys(): + self.get_logger().error(f"Predicate {predicate_name} does not exist, returning false.", + throttle_duration_sec=1.0) + return False + value = self._predicates[predicate_name] + if callable(value): + bool_value = False + try: + bool_value = value() + except Exception as e: + self.get_logger().error(f"Error while evaluating the callback function: {e}", + throttle_duration_sec=1.0) + return bool_value + return value + + def set_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable]): + """ + Set the value of the predicate given as parameter, if the predicate is not found does not do anything. + + :param predicate_name: the name of the predicate to retrieve from the map of predicates + :param predicate_value: the new value of the predicate as a bool or a callable function + """ + if predicate_name not in self._predicates.keys(): + self.get_logger().error(f"Cannot set predicate {predicate_name} with a new value because it does not exist.", + throttle_duration_sec=1.0) + return + self._predicates[predicate_name] = predicate_value diff --git a/source/modulo_components/test/python/conftest.py b/source/modulo_components/test/python/conftest.py new file mode 100644 index 000000000..807010268 --- /dev/null +++ b/source/modulo_components/test/python/conftest.py @@ -0,0 +1,9 @@ +import pytest +import rclpy + + +@pytest.fixture +def ros_context(): + rclpy.init() + yield + rclpy.shutdown() \ No newline at end of file diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py new file mode 100644 index 000000000..1e965f179 --- /dev/null +++ b/source/modulo_components/test/python/test_component_interface.py @@ -0,0 +1,48 @@ +import pytest + +from modulo_components.component_interface import ComponentInterface + + +def raise_(ex): + raise ex + + +@pytest.fixture() +def component_interface(ros_context): + yield ComponentInterface('component_interface') + + +def test_add_bool_predicate(component_interface): + component_interface.add_predicate('foo', True) + assert 'foo' in component_interface._predicates.keys() + assert component_interface._predicates['foo'] + + +def test_add_function_predicate(component_interface): + component_interface.add_predicate('foo', lambda: False) + assert 'foo' in component_interface._predicates.keys() + assert not component_interface._predicates['foo']() + + +def test_get_predicate(component_interface): + component_interface.add_predicate('foo', True) + assert component_interface.get_predicate('foo') + component_interface.add_predicate('bar', lambda: True) + assert component_interface.get_predicate('bar') + # predicate does not exist, expect false + assert not component_interface.get_predicate('test') + # error in callback function except false + component_interface.add_predicate('error', lambda: raise_(RuntimeError("An error occurred"))) + assert not component_interface.get_predicate('error') + + +def test_set_predicate(component_interface): + component_interface.add_predicate('foo', True) + component_interface.set_predicate('foo', False) + assert not component_interface.get_predicate('foo') + # predicate does not exist so setting won't do anything + component_interface.set_predicate('bar', True) + assert not component_interface.get_predicate('bar') + component_interface.add_predicate('bar', True) + component_interface.set_predicate('bar', lambda: False) + assert not component_interface.get_predicate('bar') From 1059ab454a43a38a3e9adb11dd7213819ca5f65e Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 12 Apr 2022 14:12:08 +0200 Subject: [PATCH 14/71] Specialize all available MessagePairs and add read capability (#52) * Specialize all available MessagePairs * Update tests * Add read capabilities --- source/modulo_new_core/CMakeLists.txt | 1 + .../communication/MessagePair.hpp | 44 ++++++++++--- .../communication/MessagePairInterface.hpp | 8 +++ .../src/communication/MessagePair.cpp | 41 ++++++++++++ .../communication/test_message_pair.cpp | 64 ++++++++++--------- 5 files changed, 120 insertions(+), 38 deletions(-) create mode 100644 source/modulo_new_core/src/communication/MessagePair.cpp diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index fc0ef2f8e..91990f0db 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -27,6 +27,7 @@ find_library(clproto REQUIRED) include_directories(include) ament_auto_add_library(modulo_new_core SHARED + ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp ${PROJECT_SOURCE_DIR}/src/translators/ReadStateConversion.cpp diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index 1da958dd5..02eb300ac 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -2,6 +2,7 @@ #include "modulo_new_core/communication/MessagePairInterface.hpp" #include "modulo_new_core/exceptions/NullPointerException.hpp" +#include "modulo_new_core/translators/ReadStateConversion.hpp" #include "modulo_new_core/translators/WriteStateConversion.hpp" #include @@ -11,25 +12,23 @@ namespace modulo_new_core::communication { template class MessagePair : public MessagePairInterface { public: - MessagePair(MessageType type, std::shared_ptr data, std::shared_ptr clock); + MessagePair(std::shared_ptr data, std::shared_ptr clock); [[nodiscard]] MsgT write_message() const; + void read_message(const MsgT& message); + [[nodiscard]] std::shared_ptr get_data() const; + void set_data(const std::shared_ptr& data); + private: std::shared_ptr data_; std::shared_ptr clock_; }; template -MessagePair::MessagePair( - MessageType type, std::shared_ptr data, std::shared_ptr clock -) : - MessagePairInterface(type), data_(std::move(data)), clock_(std::move(clock)) {} - -template -MsgT MessagePair::write_message() const { +inline MsgT MessagePair::write_message() const { if (this->data_ == nullptr) { throw exceptions::NullPointerException("The message pair data is not set, nothing to write"); } @@ -38,8 +37,35 @@ MsgT MessagePair::write_message() const { return msg; } +template<> +inline EncodedState MessagePair::write_message() const { + auto msg = EncodedState(); + // TODO write the translators +// translators::write_msg(msg, this->data_, clock_->now()); + return msg; +} + +template +inline void MessagePair::read_message(const MsgT& message) { + translators::read_msg(*this->data_, message); +} + +template<> +inline void MessagePair::read_message(const EncodedState& message) { + // TODO write the translators +// translators::read_msg(this->data_, message, clock_->now()); +} + template -std::shared_ptr MessagePair::get_data() const { +inline std::shared_ptr MessagePair::get_data() const { return this->data_; } + +template +inline void MessagePair::set_data(const std::shared_ptr& data) { + if (data == nullptr) { + throw exceptions::NullPointerException("Provide a valid pointer"); + } + this->data_ = data; +} }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp index a40b330ed..38e61a346 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp @@ -26,6 +26,9 @@ class MessagePairInterface : public std::enable_shared_from_this [[nodiscard]] MsgT write(); + template + void read(const MsgT& message); + MessageType get_type() const; private: @@ -55,4 +58,9 @@ inline MsgT MessagePairInterface::write() { return this->template get_message_pair()->write_message(); } +template +inline void MessagePairInterface::read(const MsgT& message) { + this->template get_message_pair()->read_message(message); +} + }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_new_core/src/communication/MessagePair.cpp new file mode 100644 index 000000000..8fe5ee511 --- /dev/null +++ b/source/modulo_new_core/src/communication/MessagePair.cpp @@ -0,0 +1,41 @@ +#include "modulo_new_core/communication/MessagePair.hpp" + +namespace modulo_new_core::communication { + +template<> +MessagePair::MessagePair( + std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::BOOL), data_(std::move(data)), clock_(std::move(clock)) {} + +template<> +MessagePair::MessagePair( + std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::FLOAT64), data_(std::move(data)), clock_(std::move(clock)) {} + +template<> +MessagePair>::MessagePair( + std::shared_ptr> data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::FLOAT64_MULTI_ARRAY), data_(std::move(data)), clock_(std::move(clock)) {} + +template<> +MessagePair::MessagePair( + std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::INT32), data_(std::move(data)), clock_(std::move(clock)) {} + +template<> +MessagePair::MessagePair( + std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::STRING), data_(std::move(data)), clock_(std::move(clock)) {} + +template<> +MessagePair::MessagePair( + std::shared_ptr data, std::shared_ptr clock +) : + MessagePairInterface(MessageType::ENCODED_STATE), data_(std::move(data)), clock_(std::move(clock)) {} + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp index d14827015..9931cda43 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -6,10 +6,10 @@ using namespace modulo_new_core::communication; template static void test_message_interface( - MessageType type, const DataT& initial_value, const DataT& new_value, const std::shared_ptr clock + const DataT& initial_value, const DataT& new_value, const std::shared_ptr clock ) { auto data = std::make_shared(initial_value); - auto msg_pair = std::make_shared>(type, data, clock); + auto msg_pair = std::make_shared>(data, clock); EXPECT_EQ(initial_value, *msg_pair->get_data()); EXPECT_EQ(initial_value, msg_pair->write_message().data); @@ -22,6 +22,11 @@ static void test_message_interface( EXPECT_EQ(new_value, msg_pair->write_message().data); msg = msg_pair_interface->template write(); EXPECT_EQ(new_value, msg.data); + + msg = MsgT(); + msg.data = initial_value; + msg_pair_interface->template read(msg); + EXPECT_EQ(initial_value, *msg_pair->get_data()); } class MessagePairTest : public ::testing::Test { @@ -33,34 +38,35 @@ class MessagePairTest : public ::testing::Test { std::shared_ptr clock_; }; -TEST_F(MessagePairTest, TestBasicTypes) { - test_message_interface(MessageType::BOOL, false, true, clock_); - test_message_interface(MessageType::FLOAT64, 0.1, 0.2, clock_); +TEST_F(MessagePairTest, BasicTypes) { + test_message_interface(false, true, clock_); + test_message_interface(0.1, 0.2, clock_); test_message_interface>( - MessageType::FLOAT64_MULTI_ARRAY, {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, clock_ + {0.1, 0.2, 0.3}, {0.4, 0.5, 0.6}, clock_ ); - test_message_interface(MessageType::INT32, 1, 2, clock_); - test_message_interface(MessageType::STRING, "this", "that", clock_); + test_message_interface(1, 2, clock_); + test_message_interface("this", "that", clock_); } -TEST_F(MessagePairTest, TestCartesianState) { - auto initial_value = state_representation::CartesianState::Random("test"); - auto data = std::make_shared(initial_value); - auto msg_pair = std::make_shared>( - MessageType::ENCODED_STATE, data, clock_ - ); - EXPECT_TRUE(initial_value.data().isApprox(msg_pair->get_data()->data())); - - std::shared_ptr msg_pair_interface(msg_pair); - auto msg = msg_pair_interface->write(); - std::string tmp(msg.data.begin(), msg.data.end()); - auto decoded = clproto::decode(tmp); - EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); - - auto new_value = state_representation::CartesianState::Identity("world"); - *data = new_value; - msg = msg_pair_interface->write(); - tmp = std::string(msg.data.begin(), msg.data.end()); - decoded = clproto::decode(tmp); - EXPECT_TRUE(new_value.data().isApprox(decoded.data())); -} +// FIXME after translators are finished +//TEST_F(MessagePairTest, TestCartesianState) { +// auto initial_value = state_representation::CartesianState::Random("test"); +// auto data = std::make_shared(initial_value); +// auto msg_pair = std::make_shared>( +// MessageType::ENCODED_STATE, data, clock_ +// ); +// EXPECT_TRUE(initial_value.data().isApprox(msg_pair->get_data()->data())); +// +// std::shared_ptr msg_pair_interface(msg_pair); +// auto msg = msg_pair_interface->write(); +// std::string tmp(msg.data.begin(), msg.data.end()); +// auto decoded = clproto::decode(tmp); +// EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); +// +// auto new_value = state_representation::CartesianState::Identity("world"); +// *data = new_value; +// msg = msg_pair_interface->write(); +// tmp = std::string(msg.data.begin(), msg.data.end()); +// decoded = clproto::decode(tmp); +// EXPECT_TRUE(new_value.data().isApprox(decoded.data())); +//} From 6e790fc9a3fcdaa94f535e8ec8d18661f8bd8373 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 14 Apr 2022 09:59:12 +0200 Subject: [PATCH 15/71] Implementation of publisher interface (#53) * Add implementation of functions * Add tests * Apply suggestions from code review * Implement PublisherInterface helper in derived class --- .../communication/PublisherHandler.hpp | 12 +++ .../communication/PublisherInterface.hpp | 18 +++++ .../src/communication/PublisherInterface.cpp | 45 +++++++++++ .../communication/test_publisher_handler.cpp | 81 +++++++++++++++++++ 4 files changed, 156 insertions(+) create mode 100644 source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp index 5b0d9e583..cba7b9950 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp @@ -11,6 +11,9 @@ class PublisherHandler : public PublisherInterface { void publish(const MsgT& message) const; + std::shared_ptr + create_publisher_interface(const std::shared_ptr& message_pair); + private: std::shared_ptr publisher_; }; @@ -24,4 +27,13 @@ void PublisherHandler::publish(const MsgT& message) const { this->publisher_->publish(message); } +template +std::shared_ptr PublisherHandler::create_publisher_interface( + const std::shared_ptr& message_pair +) { + std::shared_ptr publisher_interface(this->shared_from_this()); + publisher_interface->set_message_pair(message_pair); + return publisher_interface; +} + }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp index c21e54a01..a99af0228 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp @@ -38,4 +38,22 @@ class PublisherInterface : public std::enable_shared_from_this message_pair_; }; +template +inline std::shared_ptr> PublisherInterface::get_publisher(bool validate_pointer) { + std::shared_ptr> publisher_ptr; + try { + publisher_ptr = std::dynamic_pointer_cast>(this->shared_from_this()); + } catch (const std::exception& ex) { + if (validate_pointer) { + throw exceptions::InvalidPointerException("Publisher interface is not managed by a valid pointer"); + } + } + if (publisher_ptr == nullptr && validate_pointer) { + throw exceptions::InvalidPointerCastException( + "Unable to cast publisher interface to a publisher pointer of requested type" + ); + } + return publisher_ptr; +} + }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/PublisherInterface.cpp b/source/modulo_new_core/src/communication/PublisherInterface.cpp index 7508363ed..3876e3e94 100644 --- a/source/modulo_new_core/src/communication/PublisherInterface.cpp +++ b/source/modulo_new_core/src/communication/PublisherInterface.cpp @@ -15,6 +15,51 @@ namespace modulo_new_core::communication { PublisherInterface::PublisherInterface(PublisherType type) : type_(type) {} +void PublisherInterface::publish() { + if (this->message_pair_ == nullptr) { + throw exceptions::NullPointerException("Message pair is not set, nothing to publish"); + } + switch (this->message_pair_->get_type()) { + case MessageType::BOOL: + this->publish(this->message_pair_->write()); + break; + case MessageType::FLOAT64: + this->publish(this->message_pair_->write()); + break; + case MessageType::FLOAT64_MULTI_ARRAY: + this->publish(this->message_pair_->write>()); + break; + case MessageType::INT32: + this->publish(this->message_pair_->write()); + break; + case MessageType::STRING: + this->publish(this->message_pair_->write()); + break; + case MessageType::ENCODED_STATE: + this->publish(this->message_pair_->write()); + break; + } +} + +template +void PublisherInterface::publish(const MsgT& message) { + switch (this->get_type()) { + case PublisherType::PUBLISHER: + this->template get_publisher, MsgT>()->publish(message); + break; + case PublisherType::LIFECYCLE_PUBLISHER: + this->template get_publisher, MsgT>()->publish(message); + break; + } +} + +void PublisherInterface::set_message_pair(const std::shared_ptr& message_pair) { + if (message_pair == nullptr) { + throw exceptions::NullPointerException("Provide a valid pointer"); + } + this->message_pair_ = message_pair; +} + PublisherType PublisherInterface::get_type() const { return this->type_; } diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp new file mode 100644 index 000000000..91b54794d --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp @@ -0,0 +1,81 @@ +#include + +#include + +#include "modulo_new_core/communication/PublisherHandler.hpp" +#include "modulo_new_core/communication/MessagePair.hpp" +#include "modulo_new_core/exceptions/NullPointerException.hpp" + +using namespace modulo_new_core::communication; + +template +static void test_publisher_interface(const std::shared_ptr& node, const DataT& value) { + // create message pair + auto data = std::make_shared(value); + auto msg_pair = std::make_shared>(data, node->get_clock()); + + // create publisher handler + auto publisher = node->create_publisher("topic", 10); + auto publisher_handler = + std::make_shared, MsgT>>(PublisherType::PUBLISHER, publisher); + + // use in publisher interface + std::shared_ptr publisher_interface(publisher_handler); + EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); + publisher_interface->set_message_pair(msg_pair); + EXPECT_NO_THROW(publisher_interface->publish()); + + auto publisher_interface_ptr = publisher_handler->create_publisher_interface(msg_pair); + EXPECT_NO_THROW(publisher_interface->publish()); +} + +class PublisherTest : public ::testing::Test { +public: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + +protected: + void SetUp() { + node = std::make_shared("test_node"); + } + + std::shared_ptr node; +}; + +TEST_F(PublisherTest, BasicTypes) { + test_publisher_interface(node, true); + test_publisher_interface(node, 0.1); + test_publisher_interface>(node, {0.1, 0.2, 0.3}); + test_publisher_interface(node, 1); + test_publisher_interface(node, "this"); +} + +// FIXME after translators are done +//TEST_F(PublisherTest, CartesianState) { +// // create message pair +// auto data = +// std::make_shared(state_representation::CartesianState::Random("test")); +// auto msg_pair = std::make_shared>( +// data, node->get_clock()); +// +// // create publisher handler +// auto publisher = node->create_publisher("topic", 10); +// auto publisher_handler = std::make_shared, +// modulo_new_core::EncodedState>>( +// PublisherType::PUBLISHER, publisher +// ); +// +// // use in publisher interface +// std::shared_ptr publisher_interface(publisher_handler); +// EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); +// publisher_interface->set_message_pair(msg_pair); +// EXPECT_NO_THROW(publisher_interface->publish()); +// +// publisher_interface = create_publisher_interface(publisher_handler, msg_pair); +// EXPECT_NO_THROW(publisher_interface->publish()); +//} \ No newline at end of file From c778bbbaea296cdd49d71a753566bbd1067f7934 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Thu, 14 Apr 2022 20:18:31 +0200 Subject: [PATCH 16/71] Add parameter translators (#54) * Implement parameter translators to read and write between ROS parameters and state representation ParameterInterface pointers * Add parameterized test fixtures to test reading and rewriting a ROS parameter object --- source/modulo_new_core/CMakeLists.txt | 1 + .../translators/parameter_translators.hpp | 32 +++ .../src/translators/parameter_translators.cpp | 225 ++++++++++++++++++ .../parameters/test_parameter_readers.cpp | 158 ++++++++++++ 4 files changed, 416 insertions(+) create mode 100644 source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp create mode 100644 source/modulo_new_core/src/translators/parameter_translators.cpp create mode 100644 source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index 91990f0db..ab83a0358 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -30,6 +30,7 @@ ament_auto_add_library(modulo_new_core SHARED ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp + ${PROJECT_SOURCE_DIR}/src/translators/parameter_translators.cpp ${PROJECT_SOURCE_DIR}/src/translators/ReadStateConversion.cpp ${PROJECT_SOURCE_DIR}/src/translators/WriteStateConversion.cpp) diff --git a/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp b/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp new file mode 100644 index 000000000..2996b8538 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp @@ -0,0 +1,32 @@ +#pragma once + +#include +#include + +namespace modulo_new_core::translators { + +/** + * @brief Write a ROS Parameter from a ParameterInterface pointer. + * @param parameter the ParameterInterface pointer with a name and value + * @return A new ROS Parameter object + */ +rclcpp::Parameter write_parameter(const std::shared_ptr& parameter); + +/** + * @brief Create a new ParameterInterface from a ROS Parameter object. + * @param ros_parameter the ROS parameter object to read + * @return A new ParameterInterface pointer + */ +std::shared_ptr read_parameter(const rclcpp::Parameter& ros_parameter); + +/** + * @brief Update the parameter value of a ParameterInterface from a ROS Parameter object. + * @details The destination ParameterInterface must have a compatible parameter name and type. + * @param ros_parameter the ROS parameter object to read + * @param parameter An existing ParameterInterface pointer which will have + */ +void read_parameter( + const rclcpp::Parameter& ros_parameter, std::shared_ptr& parameter +); + +} \ No newline at end of file diff --git a/source/modulo_new_core/src/translators/parameter_translators.cpp b/source/modulo_new_core/src/translators/parameter_translators.cpp new file mode 100644 index 000000000..4b3d1432d --- /dev/null +++ b/source/modulo_new_core/src/translators/parameter_translators.cpp @@ -0,0 +1,225 @@ +#include "modulo_new_core/translators/parameter_translators.hpp" + +#include +#include +#include +#include + +#include + +using namespace state_representation; + +namespace modulo_new_core::translators { + +rclcpp::Parameter write_parameter(const std::shared_ptr& parameter) { + switch (parameter->get_parameter_type()) { + case ParameterType::BOOL: + return {parameter->get_name(), parameter->get_parameter_value()}; + case ParameterType::BOOL_ARRAY: + return {parameter->get_name(), parameter->get_parameter_value>()}; + case ParameterType::INT: + return {parameter->get_name(), parameter->get_parameter_value()}; + case ParameterType::INT_ARRAY: + return {parameter->get_name(), parameter->get_parameter_value>()}; + case ParameterType::DOUBLE: + return {parameter->get_name(), parameter->get_parameter_value()}; + case ParameterType::DOUBLE_ARRAY: + return {parameter->get_name(), parameter->get_parameter_value>()}; + case ParameterType::STRING: + return {parameter->get_name(), parameter->get_parameter_value()}; + case ParameterType::STRING_ARRAY: + return {parameter->get_name(), parameter->get_parameter_value>()}; + case ParameterType::STATE: { + switch (parameter->get_parameter_state_type()) { + case StateType::CARTESIAN_STATE: + return {parameter->get_name(), clproto::to_json(parameter->get_parameter_value())}; + case StateType::CARTESIAN_POSE: + return {parameter->get_name(), clproto::to_json(parameter->get_parameter_value())}; + case StateType::JOINT_STATE: + return {parameter->get_name(), clproto::to_json(parameter->get_parameter_value())}; + case StateType::JOINT_POSITIONS: + return {parameter->get_name(), clproto::to_json(parameter->get_parameter_value())}; + default: + break; + } + break; + } + case ParameterType::VECTOR: { + auto eigen_vector = parameter->get_parameter_value(); + std::vector vec(eigen_vector.data(), eigen_vector.data() + eigen_vector.size()); + return {parameter->get_name(), vec}; + } + case ParameterType::MATRIX: { + auto eigen_matrix = parameter->get_parameter_value(); + std::vector vec(eigen_matrix.data(), eigen_matrix.data() + eigen_matrix.size()); + return {parameter->get_name(), vec}; + } + default: + break; + } + throw InvalidParameterException("Parameter " + parameter->get_name() + " could not be written!"); +} + +std::shared_ptr read_parameter(const rclcpp::Parameter& parameter) { + switch (parameter.get_type()) { + case rclcpp::PARAMETER_BOOL: + return make_shared_parameter(parameter.get_name(), parameter.as_bool()); + case rclcpp::PARAMETER_BOOL_ARRAY: + return make_shared_parameter(parameter.get_name(), parameter.as_bool_array()); + case rclcpp::PARAMETER_INTEGER: + return make_shared_parameter(parameter.get_name(), static_cast(parameter.as_int())); + case rclcpp::PARAMETER_INTEGER_ARRAY: { + auto array = parameter.as_integer_array(); + std::vector int_array(array.begin(), array.end()); + return make_shared_parameter(parameter.get_name(), int_array); + } + case rclcpp::PARAMETER_DOUBLE: + return make_shared_parameter(parameter.get_name(), parameter.as_double()); + case rclcpp::PARAMETER_DOUBLE_ARRAY: + return make_shared_parameter(parameter.get_name(), parameter.as_double_array()); + case rclcpp::PARAMETER_STRING_ARRAY: + return make_shared_parameter>(parameter.get_name(), parameter.as_string_array()); + case rclcpp::PARAMETER_STRING: { + // TODO: consider dedicated clproto::decode>(msg) specialization in library + std::string encoding; + try { + encoding = clproto::from_json(parameter.as_string()); + } catch (const clproto::JsonParsingException&) {} + if (!clproto::is_valid(encoding)) { + return make_shared_parameter(parameter.get_name(), parameter.as_string()); + } + switch (clproto::check_message_type(encoding)) { + case clproto::CARTESIAN_STATE_MESSAGE: + return make_shared_parameter(parameter.get_name(), clproto::decode(encoding)); + case clproto::CARTESIAN_POSE_MESSAGE: + return make_shared_parameter(parameter.get_name(), clproto::decode(encoding)); + case clproto::JOINT_STATE_MESSAGE: + return make_shared_parameter(parameter.get_name(), clproto::decode(encoding)); + case clproto::JOINT_POSITIONS_MESSAGE: + return make_shared_parameter(parameter.get_name(), clproto::decode(encoding)); + default: + throw InvalidParameterException( + "Parameter " + parameter.get_name() + " has an unsupported encoded message type"); + } + } + case rclcpp::PARAMETER_BYTE_ARRAY: + // TODO: try clproto decode, re-use logic from above + throw InvalidParameterException("Parameter byte arrays are not currently supported."); + default: + // TODO: handle default + break; + } + throw InvalidParameterException("Parameter " + parameter.get_name() + " could not be read!"); +} + +void read_parameter( + const rclcpp::Parameter& ros_parameter, std::shared_ptr& parameter +) { + if (ros_parameter.get_name() != parameter->get_name()) { + // TODO: throw mismatch parameter exception + } + auto new_parameter = read_parameter(ros_parameter); + if (new_parameter->get_parameter_type() != parameter->get_parameter_type() + && new_parameter->get_parameter_type() != ParameterType::STRING + && new_parameter->get_parameter_type() != ParameterType::DOUBLE_ARRAY) { + // TODO: throw mismatch parameter exception + } + switch (new_parameter->get_parameter_type()) { + case ParameterType::BOOL: + parameter->set_parameter_value(new_parameter->get_parameter_value()); + break; + case ParameterType::BOOL_ARRAY: + parameter->set_parameter_value>(new_parameter->get_parameter_value>()); + break; + case ParameterType::INT: + parameter->set_parameter_value(new_parameter->get_parameter_value()); + break; + case ParameterType::INT_ARRAY: + parameter->set_parameter_value>(new_parameter->get_parameter_value>()); + break; + case ParameterType::DOUBLE: + parameter->set_parameter_value(new_parameter->get_parameter_value()); + break; + case ParameterType::DOUBLE_ARRAY: { + auto value = new_parameter->get_parameter_value>(); + switch (parameter->get_parameter_type()) { + case ParameterType::DOUBLE_ARRAY: + parameter->set_parameter_value>(value); + break; + case ParameterType::VECTOR: { + Eigen::VectorXd vector = Eigen::Map(value.data(), static_cast(value.size())); + parameter->set_parameter_value(vector); + break; + } + case ParameterType::MATRIX: { + auto matrix = parameter->get_parameter_value(); + if (static_cast(matrix.size()) != value.size()) { + // TODO: throw mismatch matrix size + } + matrix = Eigen::Map(value.data(), matrix.rows(), matrix.cols()); + parameter->set_parameter_value(matrix); + break; + } + default: + // TODO: throw mismatch parameter exception + break; + } + break; + } + case ParameterType::STRING_ARRAY: + parameter->set_parameter_value>( + new_parameter->get_parameter_value>()); + break; + case ParameterType::STRING: { + auto value = new_parameter->get_parameter_value(); + switch (parameter->get_parameter_type()) { + case ParameterType::STRING: + parameter->set_parameter_value(value); + break; + case ParameterType::STATE: { + std::string encoding; + try { + encoding = clproto::from_json(value); + } catch (const clproto::JsonParsingException&) { + // TODO: optional handling, otherwise just rethrow + throw; + } + try { + switch (clproto::check_message_type(encoding)) { + case clproto::CARTESIAN_POSE_MESSAGE: + parameter->set_parameter_value(clproto::decode(encoding)); + break; + case clproto::JOINT_STATE_MESSAGE: + parameter->set_parameter_value(clproto::decode(encoding)); + break; + case clproto::JOINT_POSITIONS_MESSAGE: + parameter->set_parameter_value(clproto::decode(encoding)); + break; + default: + // TODO: throw unsupported parameter state type + break; + } + } catch (const clproto::DecodingException&) { + // TODO: optional handling, otherwise just rethrow + throw; + } + } + default: + // TODO: throw mismatch parameter exception + break; + } + break; + } + case ParameterType::VECTOR: + parameter->set_parameter_value(new_parameter->get_parameter_value()); + break; + case ParameterType::MATRIX: + parameter->set_parameter_value(new_parameter->get_parameter_value()); + break; + default: + break; + } + throw InvalidParameterException("Parameter " + ros_parameter.get_name() + " could not be translated!"); +} + +} \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp new file mode 100644 index 000000000..b97adf9bd --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp @@ -0,0 +1,158 @@ +#include + +#include +#include +#include +#include +#include + +#include "modulo_new_core/translators/parameter_translators.hpp" + +using namespace modulo_new_core::translators; + +// Parameterized test fixture by type and expected value +// See also: http://www.ashermancinelli.com/gtest-type-val-param + +template using RParamT = std::vector>; + +static std::tuple< + RParamT, + RParamT>, + RParamT, + RParamT>, + RParamT, + RParamT>, + RParamT, + RParamT> +> read_test_params{{ + std::make_tuple(true, state_representation::ParameterType::BOOL), + std::make_tuple(false, state_representation::ParameterType::BOOL), + }, { + std::make_tuple( + std::vector({true, false, true}), state_representation::ParameterType::BOOL_ARRAY), + }, { + std::make_tuple(0, state_representation::ParameterType::INT), + std::make_tuple(1, state_representation::ParameterType::INT), + }, { + std::make_tuple(std::vector({1, 2, 3}), state_representation::ParameterType::INT_ARRAY), + }, { + std::make_tuple(1.0, state_representation::ParameterType::DOUBLE), + }, { + std::make_tuple( + std::vector({true, false, true}), state_representation::ParameterType::DOUBLE_ARRAY), + }, { + std::make_tuple("test", state_representation::ParameterType::STRING), + }, { + std::make_tuple( + std::vector({"1", "2", "3"}), + state_representation::ParameterType::STRING_ARRAY), + }, +}; + +template +class ReadParameterTest : public testing::Test { +public: + ReadParameterTest() : test_params_{std::get>(read_test_params)} {} +protected: + void test_read_parameter(T value, state_representation::ParameterType expected_type) { + ros_param_ = rclcpp::Parameter("test", value); + param_ = read_parameter(ros_param_); + ASSERT_NO_THROW(param_ = read_parameter(ros_param_)); + EXPECT_EQ(param_->get_name(), ros_param_.get_name()); + EXPECT_EQ(param_->get_parameter_type(), expected_type); + EXPECT_EQ(param_->get_parameter_value(), value); + } + + void test_rewrite_parameter() { + rclcpp::Parameter new_ros_param; + ASSERT_NO_THROW(new_ros_param = write_parameter(param_)); + EXPECT_EQ(new_ros_param, ros_param_); + } + + std::shared_ptr param_; + rclcpp::Parameter ros_param_; + RParamT test_params_; +}; +TYPED_TEST_SUITE_P(ReadParameterTest); + +TYPED_TEST_P(ReadParameterTest, ReadAndReWrite) { + for (auto const& [value, type]: this->test_params_) { + this->test_read_parameter(value, type); + this->test_rewrite_parameter(); + } +} + +REGISTER_TYPED_TEST_SUITE_P(ReadParameterTest, ReadAndReWrite); + +using ReadTestTypes = testing::Types< + bool, std::vector, int, std::vector, double, std::vector, std::string, std::vector>; +INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ReadParameterTest, ReadTestTypes); + +/* + * Test reading and rewriting parameters containing states + */ + +static std::tuple< + state_representation::CartesianState, + state_representation::CartesianPose, + state_representation::JointState, + state_representation::JointPositions +> read_test_states{ + state_representation::CartesianState::Random("frame", "reference"), + state_representation::CartesianPose::Random("frame", "reference"), + state_representation::JointState::Random("robot", 3), + state_representation::JointPositions::Random("robot", 3), +}; + +template +class ReadStateParameterTest : public testing::Test { +public: + ReadStateParameterTest() : test_state_{std::get(read_test_states)} {} +protected: + + void check_state_equality(const T& new_state) { + EXPECT_STREQ(new_state.get_name().c_str(), this->test_state_.get_name().c_str()); + EXPECT_EQ(new_state.get_type(), this->test_state_.get_type()); + EXPECT_NEAR(state_representation::dist(new_state, this->test_state_), 0.0, 1e-9); + } + + void test_read_parameter() { + std::string json = clproto::to_json(this->test_state_); + ros_param_ = rclcpp::Parameter("test", json); + param_ = read_parameter(ros_param_); + ASSERT_NO_THROW(param_ = read_parameter(ros_param_)); + EXPECT_EQ(param_->get_name(), ros_param_.get_name()); + EXPECT_EQ(param_->get_parameter_type(), state_representation::ParameterType::STATE); + EXPECT_EQ(param_->get_parameter_state_type(), this->test_state_.get_type()); + + ASSERT_NO_THROW(check_state_equality(param_->get_parameter_value())); + } + + void test_rewrite_parameter() { + rclcpp::Parameter new_ros_param; + ASSERT_NO_THROW(new_ros_param = write_parameter(param_)); + EXPECT_STREQ(new_ros_param.get_name().c_str(), ros_param_.get_name().c_str()); + EXPECT_EQ(new_ros_param.get_type(), ros_param_.get_type()); + EXPECT_NO_THROW(check_state_equality(clproto::from_json(new_ros_param.as_string()))); + } + + std::shared_ptr param_; + rclcpp::Parameter ros_param_; + T test_state_; +}; +TYPED_TEST_SUITE_P(ReadStateParameterTest); + +TYPED_TEST_P(ReadStateParameterTest, ReadAndReWriteState) { + this->test_read_parameter(); + this->test_rewrite_parameter(); +} + +REGISTER_TYPED_TEST_SUITE_P(ReadStateParameterTest, ReadAndReWriteState); + +using ReadStateTestTypes = testing::Types< + state_representation::CartesianState, + state_representation::CartesianPose, + state_representation::JointState, + state_representation::JointPositions +>; +INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ReadStateParameterTest, ReadStateTestTypes); \ No newline at end of file From 28894547c75656f0d1a260fcba8e2a86607209dc Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 19 Apr 2022 16:01:16 +0200 Subject: [PATCH 17/71] Use galactic-devel base tag (#59) --- build-server.sh | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build-server.sh b/build-server.sh index edd02599b..ed9a726d8 100755 --- a/build-server.sh +++ b/build-server.sh @@ -1,5 +1,5 @@ #!/bin/bash -BASE_TAG=galactic-devel-state-type +BASE_TAG=galactic-devel IMAGE_NAME=epfl-lasa/modulo IMAGE_TAG=latest From 79cd9d1be90bd1515cd2f05ef219e6ca625d093c Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Tue, 19 Apr 2022 17:24:45 +0200 Subject: [PATCH 18/71] Parameter translation improvements (#57) * Add const parameter reader * Add read parameter translator that takes a reference to a const ParameterInterface. This is useful for validating parameters without modifying them (in case of invalidity), where an original parameter object is needed as a reference (e.g. for matrix size, state compatibility or other) * Refactor and split tests by parameter type * Rename message translators * Rename ReadStateConversion and WriteStateConversion to read_message and write_message respectively. For consistency in translators, lower_snake_case is used for source files that have no specific class. * Rename test file and update references * Remove unused variable to suppress warning * Touchup test case for constness * Ensure that the const reader makes a new parameter interface and cannot modify the original parameter through the "copy" * Copy parameter helper * Implement a parameter copy helper for ParameterInterface pointers so that a referenced value can be updated without breaking the reference * Add IncompatibleParameterException to resolve some TODOs * Extend tests * Update constness * Mark copy method source parameter as const * Enable matrix translation and test --- .../modulo_components/ComponentInterface.hpp | 4 +- .../message_passing/MessagePassingHandler.hpp | 4 +- source/modulo_new_core/CMakeLists.txt | 4 +- .../communication/MessagePair.hpp | 6 +- .../IncompatibleParameterException.hpp | 11 ++ ...tateConversion.hpp => message_readers.hpp} | 0 ...tateConversion.hpp => message_writers.hpp} | 0 .../translators/parameter_translators.hpp | 24 ++- ...tateConversion.cpp => message_readers.cpp} | 2 +- ...tateConversion.cpp => message_writers.cpp} | 2 +- .../src/translators/parameter_translators.cpp | 183 ++++++++++-------- .../test_parameter_eigen_translators.cpp | 66 +++++++ .../parameters/test_parameter_readers.cpp | 158 --------------- .../test_parameter_state_translators.cpp | 95 +++++++++ .../parameters/test_parameter_translators.cpp | 120 ++++++++++++ ...test_translators.cpp => test_messages.cpp} | 22 +-- 16 files changed, 438 insertions(+), 263 deletions(-) create mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp rename source/modulo_new_core/include/modulo_new_core/translators/{ReadStateConversion.hpp => message_readers.hpp} (100%) rename source/modulo_new_core/include/modulo_new_core/translators/{WriteStateConversion.hpp => message_writers.hpp} (100%) rename source/modulo_new_core/src/translators/{ReadStateConversion.cpp => message_readers.cpp} (98%) rename source/modulo_new_core/src/translators/{WriteStateConversion.cpp => message_writers.cpp} (99%) create mode 100644 source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp delete mode 100644 source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp create mode 100644 source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp create mode 100644 source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp rename source/modulo_new_core/test/cpp_tests/translators/{test_translators.cpp => test_messages.cpp} (94%) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 1faa37503..1174e928d 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -10,8 +10,8 @@ #include #include -#include -#include +#include +#include #include "modulo_components/exceptions/PredicateNotFoundException.hpp" #include "modulo_components/utilities/utilities.hpp" diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp index eb1788612..cb115d157 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp @@ -6,8 +6,8 @@ #include #include "modulo_core/communication/CommunicationHandler.hpp" -#include "modulo_new_core/translators/ReadStateConversion.hpp" -#include "modulo_new_core/translators/WriteStateConversion.hpp" +#include "modulo_new_core/translators/message_readers.hpp" +#include "modulo_new_core/translators/message_writers.hpp" namespace modulo::core::communication { /** diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index ab83a0358..742add05e 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -31,8 +31,8 @@ ament_auto_add_library(modulo_new_core SHARED ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp ${PROJECT_SOURCE_DIR}/src/translators/parameter_translators.cpp - ${PROJECT_SOURCE_DIR}/src/translators/ReadStateConversion.cpp - ${PROJECT_SOURCE_DIR}/src/translators/WriteStateConversion.cpp) + ${PROJECT_SOURCE_DIR}/src/translators/message_readers.cpp + ${PROJECT_SOURCE_DIR}/src/translators/message_writers.cpp) # install Python modules ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index 02eb300ac..92684a5b3 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -2,8 +2,8 @@ #include "modulo_new_core/communication/MessagePairInterface.hpp" #include "modulo_new_core/exceptions/NullPointerException.hpp" -#include "modulo_new_core/translators/ReadStateConversion.hpp" -#include "modulo_new_core/translators/WriteStateConversion.hpp" +#include "modulo_new_core/translators/message_readers.hpp" +#include "modulo_new_core/translators/message_writers.hpp" #include @@ -51,7 +51,7 @@ inline void MessagePair::read_message(const MsgT& message) { } template<> -inline void MessagePair::read_message(const EncodedState& message) { +inline void MessagePair::read_message(const EncodedState&) { // TODO write the translators // translators::read_msg(this->data_, message, clock_->now()); } diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp new file mode 100644 index 000000000..aae2aa72d --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include + +namespace modulo_new_core::exceptions { +class IncompatibleParameterException : public state_representation::exceptions::InvalidParameterException { +public: + explicit IncompatibleParameterException(const std::string& msg) : + state_representation::exceptions::InvalidParameterException(msg) {}; +}; +} \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp similarity index 100% rename from source/modulo_new_core/include/modulo_new_core/translators/ReadStateConversion.hpp rename to source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp diff --git a/source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp similarity index 100% rename from source/modulo_new_core/include/modulo_new_core/translators/WriteStateConversion.hpp rename to source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp diff --git a/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp b/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp index 2996b8538..a849286be 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp @@ -5,6 +5,21 @@ namespace modulo_new_core::translators { +/** + * @brief Copy the value of one parameter interface into another. + * @details When referencing a Parameter instance through a ParameterInterface pointer, it is necessary + * to know the parameter type to get or set the parameter value. Sometimes it is desirable to update the + * parameter value from another compatible ParameterInterface, while still preserving the reference + * to the original instance. This helper function calls the getters and setters of the appropriate parameter type + * to modify the value of the parameter instance while preserving the reference of the original pointer. + * @param source_parameter The ParameterInterface with a value to copy + * @param parameter The destination ParameterInterface to be updated + */ +void copy_parameter_value( + const std::shared_ptr& source_parameter, + const std::shared_ptr& parameter +); + /** * @brief Write a ROS Parameter from a ParameterInterface pointer. * @param parameter the ParameterInterface pointer with a name and value @@ -19,14 +34,19 @@ rclcpp::Parameter write_parameter(const std::shared_ptr read_parameter(const rclcpp::Parameter& ros_parameter); +std::shared_ptr read_parameter_const( + const rclcpp::Parameter& ros_parameter, + const std::shared_ptr& parameter +); + /** * @brief Update the parameter value of a ParameterInterface from a ROS Parameter object. * @details The destination ParameterInterface must have a compatible parameter name and type. * @param ros_parameter the ROS parameter object to read - * @param parameter An existing ParameterInterface pointer which will have + * @param parameter An existing ParameterInterface pointer */ void read_parameter( - const rclcpp::Parameter& ros_parameter, std::shared_ptr& parameter + const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter ); } \ No newline at end of file diff --git a/source/modulo_new_core/src/translators/ReadStateConversion.cpp b/source/modulo_new_core/src/translators/message_readers.cpp similarity index 98% rename from source/modulo_new_core/src/translators/ReadStateConversion.cpp rename to source/modulo_new_core/src/translators/message_readers.cpp index a4fe873c4..bf16ae8ec 100644 --- a/source/modulo_new_core/src/translators/ReadStateConversion.cpp +++ b/source/modulo_new_core/src/translators/message_readers.cpp @@ -1,4 +1,4 @@ -#include "modulo_new_core/translators/ReadStateConversion.hpp" +#include "modulo_new_core/translators/message_readers.hpp" namespace modulo_new_core::translators { diff --git a/source/modulo_new_core/src/translators/WriteStateConversion.cpp b/source/modulo_new_core/src/translators/message_writers.cpp similarity index 99% rename from source/modulo_new_core/src/translators/WriteStateConversion.cpp rename to source/modulo_new_core/src/translators/message_writers.cpp index 70bb17844..31dcbae40 100644 --- a/source/modulo_new_core/src/translators/WriteStateConversion.cpp +++ b/source/modulo_new_core/src/translators/message_writers.cpp @@ -1,4 +1,4 @@ -#include "modulo_new_core/translators/WriteStateConversion.hpp" +#include "modulo_new_core/translators/message_writers.hpp" #include #include diff --git a/source/modulo_new_core/src/translators/parameter_translators.cpp b/source/modulo_new_core/src/translators/parameter_translators.cpp index 4b3d1432d..c75c6ceaa 100644 --- a/source/modulo_new_core/src/translators/parameter_translators.cpp +++ b/source/modulo_new_core/src/translators/parameter_translators.cpp @@ -5,12 +5,86 @@ #include #include +#include "modulo_new_core/exceptions/IncompatibleParameterException.hpp" + #include using namespace state_representation; namespace modulo_new_core::translators { +void copy_parameter_value( + const std::shared_ptr& source_parameter, + const std::shared_ptr& parameter +) { + if (source_parameter->get_parameter_type() != parameter->get_parameter_type()) { + throw exceptions::IncompatibleParameterException( + "Source parameter " + source_parameter->get_name() + + " to be copied does not have the same type as destination parameter " + parameter->get_name()); + } + switch (source_parameter->get_parameter_type()) { + case ParameterType::BOOL: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case ParameterType::BOOL_ARRAY: + parameter->set_parameter_value(source_parameter->get_parameter_value>()); + return; + case ParameterType::INT: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case ParameterType::INT_ARRAY: + parameter->set_parameter_value(source_parameter->get_parameter_value>()); + return; + case ParameterType::DOUBLE: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case ParameterType::DOUBLE_ARRAY: + parameter->set_parameter_value(source_parameter->get_parameter_value>()); + return; + case ParameterType::STRING: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case ParameterType::STRING_ARRAY: + parameter->set_parameter_value(source_parameter->get_parameter_value>()); + return; + case ParameterType::VECTOR: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case ParameterType::MATRIX: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case ParameterType::STATE: + if (source_parameter->get_parameter_state_type() != parameter->get_parameter_state_type()) { + throw exceptions::IncompatibleParameterException( + "Source parameter " + source_parameter->get_name() + + " to be copied does not have the same parameter state type as destination parameter " + + parameter->get_name()); + } + switch (source_parameter->get_parameter_state_type()) { + case state_representation::StateType::CARTESIAN_STATE: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case state_representation::StateType::CARTESIAN_POSE: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case state_representation::StateType::JOINT_STATE: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + case state_representation::StateType::JOINT_POSITIONS: + parameter->set_parameter_value(source_parameter->get_parameter_value()); + return; + default: + break; + } + break; + default: + break; + } + throw InvalidParameterException( + "Could not copy the value from source parameter " + source_parameter->get_name() + " into parameter " + + parameter->get_name()); +} + rclcpp::Parameter write_parameter(const std::shared_ptr& parameter) { switch (parameter->get_parameter_type()) { case ParameterType::BOOL: @@ -106,120 +180,67 @@ std::shared_ptr read_parameter(const r // TODO: try clproto decode, re-use logic from above throw InvalidParameterException("Parameter byte arrays are not currently supported."); default: - // TODO: handle default break; } throw InvalidParameterException("Parameter " + parameter.get_name() + " could not be read!"); } -void read_parameter( - const rclcpp::Parameter& ros_parameter, std::shared_ptr& parameter +std::shared_ptr read_parameter_const( + const rclcpp::Parameter& ros_parameter, + const std::shared_ptr& parameter ) { if (ros_parameter.get_name() != parameter->get_name()) { - // TODO: throw mismatch parameter exception + throw exceptions::IncompatibleParameterException( + "The ROS parameter " + ros_parameter.get_name() + + " to be read does not have the same name as the reference parameter " + parameter->get_name()); } auto new_parameter = read_parameter(ros_parameter); - if (new_parameter->get_parameter_type() != parameter->get_parameter_type() - && new_parameter->get_parameter_type() != ParameterType::STRING - && new_parameter->get_parameter_type() != ParameterType::DOUBLE_ARRAY) { - // TODO: throw mismatch parameter exception + if (new_parameter->get_parameter_type() == parameter->get_parameter_type()) { + return new_parameter; } switch (new_parameter->get_parameter_type()) { - case ParameterType::BOOL: - parameter->set_parameter_value(new_parameter->get_parameter_value()); - break; - case ParameterType::BOOL_ARRAY: - parameter->set_parameter_value>(new_parameter->get_parameter_value>()); - break; - case ParameterType::INT: - parameter->set_parameter_value(new_parameter->get_parameter_value()); - break; - case ParameterType::INT_ARRAY: - parameter->set_parameter_value>(new_parameter->get_parameter_value>()); - break; - case ParameterType::DOUBLE: - parameter->set_parameter_value(new_parameter->get_parameter_value()); - break; case ParameterType::DOUBLE_ARRAY: { auto value = new_parameter->get_parameter_value>(); switch (parameter->get_parameter_type()) { - case ParameterType::DOUBLE_ARRAY: - parameter->set_parameter_value>(value); - break; case ParameterType::VECTOR: { Eigen::VectorXd vector = Eigen::Map(value.data(), static_cast(value.size())); - parameter->set_parameter_value(vector); + new_parameter = make_shared_parameter(parameter->get_name(), vector); break; } case ParameterType::MATRIX: { auto matrix = parameter->get_parameter_value(); if (static_cast(matrix.size()) != value.size()) { - // TODO: throw mismatch matrix size + throw exceptions::IncompatibleParameterException( + "The ROS parameter " + ros_parameter.get_name() + " with type double array has size " + + std::to_string(value.size()) + " while the reference parameter matrix " + parameter->get_name() + + " has size " + std::to_string(matrix.size())); } matrix = Eigen::Map(value.data(), matrix.rows(), matrix.cols()); - parameter->set_parameter_value(matrix); + new_parameter = make_shared_parameter(parameter->get_name(), matrix); break; } default: - // TODO: throw mismatch parameter exception + throw exceptions::IncompatibleParameterException( + "The ROS parameter " + ros_parameter.get_name() + + " with type double array cannot be interpreted by reference parameter " + parameter->get_name() + + " (type code " + std::to_string(static_cast(parameter->get_parameter_type())) + ")"); break; } break; } - case ParameterType::STRING_ARRAY: - parameter->set_parameter_value>( - new_parameter->get_parameter_value>()); - break; - case ParameterType::STRING: { - auto value = new_parameter->get_parameter_value(); - switch (parameter->get_parameter_type()) { - case ParameterType::STRING: - parameter->set_parameter_value(value); - break; - case ParameterType::STATE: { - std::string encoding; - try { - encoding = clproto::from_json(value); - } catch (const clproto::JsonParsingException&) { - // TODO: optional handling, otherwise just rethrow - throw; - } - try { - switch (clproto::check_message_type(encoding)) { - case clproto::CARTESIAN_POSE_MESSAGE: - parameter->set_parameter_value(clproto::decode(encoding)); - break; - case clproto::JOINT_STATE_MESSAGE: - parameter->set_parameter_value(clproto::decode(encoding)); - break; - case clproto::JOINT_POSITIONS_MESSAGE: - parameter->set_parameter_value(clproto::decode(encoding)); - break; - default: - // TODO: throw unsupported parameter state type - break; - } - } catch (const clproto::DecodingException&) { - // TODO: optional handling, otherwise just rethrow - throw; - } - } - default: - // TODO: throw mismatch parameter exception - break; - } - break; - } - case ParameterType::VECTOR: - parameter->set_parameter_value(new_parameter->get_parameter_value()); - break; - case ParameterType::MATRIX: - parameter->set_parameter_value(new_parameter->get_parameter_value()); - break; default: + throw state_representation::exceptions::InvalidParameterException( + "Something went wrong while reading parameter " + parameter->get_name()); break; } - throw InvalidParameterException("Parameter " + ros_parameter.get_name() + " could not be translated!"); + return new_parameter; +} + +void read_parameter( + const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter +) { + auto new_parameter = read_parameter_const(ros_parameter, parameter); + copy_parameter_value(new_parameter, parameter); } } \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp new file mode 100644 index 000000000..3f96e39b0 --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp @@ -0,0 +1,66 @@ +#include + +#include "modulo_new_core/translators/parameter_translators.hpp" + +using namespace modulo_new_core::translators; + +TEST(ParameterEigenTranslationTest, EigenVector) { + Eigen::VectorXd vec = Eigen::VectorXd::Random(5); + auto param = state_representation::make_shared_parameter("vector", vec); + + // writing the eigen parameter converts it to a double array + auto ros_param = write_parameter(param); + EXPECT_EQ(ros_param.get_name(), param->get_name()); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); + std::vector ros_vec; + EXPECT_NO_THROW(ros_vec = ros_param.as_double_array()); + EXPECT_EQ(ros_vec.size(), static_cast(vec.size())); + for (std::size_t ind = 0; ind < ros_vec.size(); ++ind) { + EXPECT_FLOAT_EQ(ros_vec.at(ind), vec(ind)); + } + + // reading the parameter with no context does not retain the eigen type + auto new_param = read_parameter(ros_param); + EXPECT_EQ(new_param->get_name(), ros_param.get_name()); + EXPECT_EQ(new_param->get_parameter_type(), state_representation::ParameterType::DOUBLE_ARRAY); + EXPECT_EQ(new_param->get_parameter_value>(), ros_vec); + + // reading the parameter with a const reference to an existing parameter preserves the eigen type + new_param = read_parameter_const(ros_param, param); + EXPECT_EQ(new_param->get_name(), ros_param.get_name()); + EXPECT_EQ(new_param->get_parameter_type(), state_representation::ParameterType::VECTOR); + Eigen::VectorXd new_vec; + EXPECT_NO_THROW(new_vec = new_param->get_parameter_value()); + EXPECT_EQ(new_vec, vec); +} + + +TEST(ParameterEigenTranslationTest, EigenMatrix) { + Eigen::MatrixXd mat = Eigen::MatrixXd::Random(3,4); + auto param = state_representation::make_shared_parameter("matrix", mat); + + // writing the eigen parameter converts it to a double array + auto ros_param = write_parameter(param); + EXPECT_EQ(ros_param.get_name(), param->get_name()); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY); + std::vector ros_vec; + EXPECT_NO_THROW(ros_vec = ros_param.as_double_array()); + EXPECT_EQ(ros_vec.size(), static_cast(mat.size())); + for (std::size_t ind = 0; ind < ros_vec.size(); ++ind) { + EXPECT_FLOAT_EQ(ros_vec.at(ind), mat(ind)); + } + + // reading the parameter with no context does not retain the eigen type + auto new_param = read_parameter(ros_param); + EXPECT_EQ(new_param->get_name(), ros_param.get_name()); + EXPECT_EQ(new_param->get_parameter_type(), state_representation::ParameterType::DOUBLE_ARRAY); + EXPECT_EQ(new_param->get_parameter_value>(), ros_vec); + + // reading the parameter with a const reference to an existing parameter preserves the eigen type + new_param = read_parameter_const(ros_param, param); + EXPECT_EQ(new_param->get_name(), ros_param.get_name()); + EXPECT_EQ(new_param->get_parameter_type(), state_representation::ParameterType::MATRIX); + Eigen::MatrixXd new_mat; + EXPECT_NO_THROW(new_mat = new_param->get_parameter_value()); + EXPECT_EQ(new_mat, mat); +} \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp deleted file mode 100644 index b97adf9bd..000000000 --- a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_readers.cpp +++ /dev/null @@ -1,158 +0,0 @@ -#include - -#include -#include -#include -#include -#include - -#include "modulo_new_core/translators/parameter_translators.hpp" - -using namespace modulo_new_core::translators; - -// Parameterized test fixture by type and expected value -// See also: http://www.ashermancinelli.com/gtest-type-val-param - -template using RParamT = std::vector>; - -static std::tuple< - RParamT, - RParamT>, - RParamT, - RParamT>, - RParamT, - RParamT>, - RParamT, - RParamT> -> read_test_params{{ - std::make_tuple(true, state_representation::ParameterType::BOOL), - std::make_tuple(false, state_representation::ParameterType::BOOL), - }, { - std::make_tuple( - std::vector({true, false, true}), state_representation::ParameterType::BOOL_ARRAY), - }, { - std::make_tuple(0, state_representation::ParameterType::INT), - std::make_tuple(1, state_representation::ParameterType::INT), - }, { - std::make_tuple(std::vector({1, 2, 3}), state_representation::ParameterType::INT_ARRAY), - }, { - std::make_tuple(1.0, state_representation::ParameterType::DOUBLE), - }, { - std::make_tuple( - std::vector({true, false, true}), state_representation::ParameterType::DOUBLE_ARRAY), - }, { - std::make_tuple("test", state_representation::ParameterType::STRING), - }, { - std::make_tuple( - std::vector({"1", "2", "3"}), - state_representation::ParameterType::STRING_ARRAY), - }, -}; - -template -class ReadParameterTest : public testing::Test { -public: - ReadParameterTest() : test_params_{std::get>(read_test_params)} {} -protected: - void test_read_parameter(T value, state_representation::ParameterType expected_type) { - ros_param_ = rclcpp::Parameter("test", value); - param_ = read_parameter(ros_param_); - ASSERT_NO_THROW(param_ = read_parameter(ros_param_)); - EXPECT_EQ(param_->get_name(), ros_param_.get_name()); - EXPECT_EQ(param_->get_parameter_type(), expected_type); - EXPECT_EQ(param_->get_parameter_value(), value); - } - - void test_rewrite_parameter() { - rclcpp::Parameter new_ros_param; - ASSERT_NO_THROW(new_ros_param = write_parameter(param_)); - EXPECT_EQ(new_ros_param, ros_param_); - } - - std::shared_ptr param_; - rclcpp::Parameter ros_param_; - RParamT test_params_; -}; -TYPED_TEST_SUITE_P(ReadParameterTest); - -TYPED_TEST_P(ReadParameterTest, ReadAndReWrite) { - for (auto const& [value, type]: this->test_params_) { - this->test_read_parameter(value, type); - this->test_rewrite_parameter(); - } -} - -REGISTER_TYPED_TEST_SUITE_P(ReadParameterTest, ReadAndReWrite); - -using ReadTestTypes = testing::Types< - bool, std::vector, int, std::vector, double, std::vector, std::string, std::vector>; -INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ReadParameterTest, ReadTestTypes); - -/* - * Test reading and rewriting parameters containing states - */ - -static std::tuple< - state_representation::CartesianState, - state_representation::CartesianPose, - state_representation::JointState, - state_representation::JointPositions -> read_test_states{ - state_representation::CartesianState::Random("frame", "reference"), - state_representation::CartesianPose::Random("frame", "reference"), - state_representation::JointState::Random("robot", 3), - state_representation::JointPositions::Random("robot", 3), -}; - -template -class ReadStateParameterTest : public testing::Test { -public: - ReadStateParameterTest() : test_state_{std::get(read_test_states)} {} -protected: - - void check_state_equality(const T& new_state) { - EXPECT_STREQ(new_state.get_name().c_str(), this->test_state_.get_name().c_str()); - EXPECT_EQ(new_state.get_type(), this->test_state_.get_type()); - EXPECT_NEAR(state_representation::dist(new_state, this->test_state_), 0.0, 1e-9); - } - - void test_read_parameter() { - std::string json = clproto::to_json(this->test_state_); - ros_param_ = rclcpp::Parameter("test", json); - param_ = read_parameter(ros_param_); - ASSERT_NO_THROW(param_ = read_parameter(ros_param_)); - EXPECT_EQ(param_->get_name(), ros_param_.get_name()); - EXPECT_EQ(param_->get_parameter_type(), state_representation::ParameterType::STATE); - EXPECT_EQ(param_->get_parameter_state_type(), this->test_state_.get_type()); - - ASSERT_NO_THROW(check_state_equality(param_->get_parameter_value())); - } - - void test_rewrite_parameter() { - rclcpp::Parameter new_ros_param; - ASSERT_NO_THROW(new_ros_param = write_parameter(param_)); - EXPECT_STREQ(new_ros_param.get_name().c_str(), ros_param_.get_name().c_str()); - EXPECT_EQ(new_ros_param.get_type(), ros_param_.get_type()); - EXPECT_NO_THROW(check_state_equality(clproto::from_json(new_ros_param.as_string()))); - } - - std::shared_ptr param_; - rclcpp::Parameter ros_param_; - T test_state_; -}; -TYPED_TEST_SUITE_P(ReadStateParameterTest); - -TYPED_TEST_P(ReadStateParameterTest, ReadAndReWriteState) { - this->test_read_parameter(); - this->test_rewrite_parameter(); -} - -REGISTER_TYPED_TEST_SUITE_P(ReadStateParameterTest, ReadAndReWriteState); - -using ReadStateTestTypes = testing::Types< - state_representation::CartesianState, - state_representation::CartesianPose, - state_representation::JointState, - state_representation::JointPositions ->; -INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ReadStateParameterTest, ReadStateTestTypes); \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp new file mode 100644 index 000000000..2d12e1cc9 --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp @@ -0,0 +1,95 @@ +#include + +#include +#include +#include +#include +#include + +#include "modulo_new_core/translators/parameter_translators.hpp" + +using namespace modulo_new_core::translators; + +// Parameterized test fixture by type and expected value +// See also: http://www.ashermancinelli.com/gtest-type-val-param + +static std::tuple< + state_representation::CartesianState, + state_representation::CartesianPose, + state_representation::JointState, + state_representation::JointPositions +> parameter_state_test_cases { + state_representation::CartesianState::Random("frame", "reference"), + state_representation::CartesianPose::Random("frame", "reference"), + state_representation::JointState::Random("robot", 3), + state_representation::JointPositions::Random("robot", 3), +}; + +template +class ParameterStateTranslationTest : public testing::Test { +public: + ParameterStateTranslationTest() : test_state_{std::get(parameter_state_test_cases)} {} +protected: + + void check_state_equality(const T& new_state) { + EXPECT_STREQ(new_state.get_name().c_str(), this->test_state_.get_name().c_str()); + EXPECT_EQ(new_state.get_type(), this->test_state_.get_type()); + EXPECT_NEAR(state_representation::dist(new_state, this->test_state_), 0.0, 1e-9); + } + + T test_state_; +}; +TYPED_TEST_SUITE_P(ParameterStateTranslationTest); + +TYPED_TEST_P(ParameterStateTranslationTest, Write) { + // ROS parameter of a state parameter + auto param = state_representation::make_shared_parameter("test", this->test_state_); + auto ros_param = write_parameter(param); + EXPECT_STREQ(ros_param.get_name().c_str(), param->get_name().c_str()); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_STRING); + // string equality of encoding cannot be checked because the timestamp is not preserved + // EXPECT_STREQ(ros_param_.as_string().c_str(), clproto::to_json(this->test_state_).c_str()) + this->check_state_equality(clproto::from_json(ros_param.as_string())); +} + +TYPED_TEST_P(ParameterStateTranslationTest, ReadAndReWrite) { + std::string json = clproto::to_json(this->test_state_); + auto ros_param = rclcpp::Parameter("test", json); + std::shared_ptr param; + ASSERT_NO_THROW(param = read_parameter(ros_param)); + EXPECT_EQ(param->get_name(), ros_param.get_name()); + EXPECT_EQ(param->get_parameter_type(), state_representation::ParameterType::STATE); + EXPECT_EQ(param->get_parameter_state_type(), this->test_state_.get_type()); + + ASSERT_NO_THROW(this->check_state_equality(param->get_parameter_value())); + + rclcpp::Parameter new_ros_param; + ASSERT_NO_THROW(new_ros_param = write_parameter(param)); + EXPECT_STREQ(new_ros_param.get_name().c_str(), ros_param.get_name().c_str()); + EXPECT_EQ(new_ros_param.get_type(), ros_param.get_type()); + EXPECT_NO_THROW(this->check_state_equality(clproto::from_json(new_ros_param.as_string()))); +} + +TYPED_TEST_P(ParameterStateTranslationTest, ConstRead) { + auto param = state_representation::make_shared_parameter("test", this->test_state_); + auto ros_param = write_parameter(param); + + std::shared_ptr new_param; + EXPECT_NO_THROW(new_param = read_parameter_const(ros_param, param)); + EXPECT_EQ(new_param->get_name(), param->get_name()); + EXPECT_EQ(new_param->get_parameter_type(), state_representation::ParameterType::STATE); + EXPECT_EQ(new_param->get_parameter_type(), param->get_parameter_type()); + EXPECT_EQ(new_param->get_parameter_state_type(), this->test_state_.get_type()); + + this->check_state_equality(new_param->get_parameter_value()); +} + +REGISTER_TYPED_TEST_SUITE_P(ParameterStateTranslationTest, Write, ReadAndReWrite, ConstRead); + +using ParameterStateTestTypes = testing::Types< + state_representation::CartesianState, + state_representation::CartesianPose, + state_representation::JointState, + state_representation::JointPositions +>; +INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ParameterStateTranslationTest, ParameterStateTestTypes); \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp new file mode 100644 index 000000000..a49c64d51 --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp @@ -0,0 +1,120 @@ +#include + +#include "modulo_new_core/translators/parameter_translators.hpp" + +using namespace modulo_new_core::translators; + +// Parameterized test fixture by type and expected value +// See also: http://www.ashermancinelli.com/gtest-type-val-param + +template using ParamT = std::vector>; + +static std::tuple< + ParamT, + ParamT>, + ParamT, + ParamT>, + ParamT, + ParamT>, + ParamT, + ParamT> +> parameter_test_cases{{ + std::make_tuple(true, state_representation::ParameterType::BOOL), + std::make_tuple(false, state_representation::ParameterType::BOOL), + }, { + std::make_tuple( + std::vector({true, false, true}), state_representation::ParameterType::BOOL_ARRAY), + }, { + std::make_tuple(0, state_representation::ParameterType::INT), + std::make_tuple(1, state_representation::ParameterType::INT), + }, { + std::make_tuple(std::vector({1, 2, 3}), state_representation::ParameterType::INT_ARRAY), + }, { + std::make_tuple(1.0, state_representation::ParameterType::DOUBLE), + }, { + std::make_tuple( + std::vector({true, false, true}), state_representation::ParameterType::DOUBLE_ARRAY), + }, { + std::make_tuple("test", state_representation::ParameterType::STRING), + }, { + std::make_tuple( + std::vector({"1", "2", "3"}), + state_representation::ParameterType::STRING_ARRAY), + }, +}; + +template +class ParameterTranslationTest : public testing::Test { +public: + ParameterTranslationTest() : test_cases_{std::get>(parameter_test_cases)} {} +protected: + ParamT test_cases_; +}; +TYPED_TEST_SUITE_P(ParameterTranslationTest); + +TYPED_TEST_P(ParameterTranslationTest, Write) { + for (auto const& test_case : this->test_cases_) { + auto param = state_representation::make_shared_parameter("test", std::get<0>(test_case)); + rclcpp::Parameter ros_param; + EXPECT_NO_THROW(ros_param = write_parameter(param)); + EXPECT_EQ(ros_param, rclcpp::Parameter("test", std::get<0>(test_case))); + } +} + +TYPED_TEST_P(ParameterTranslationTest, ReadAndReWrite) { + for (auto const& [value, type]: this->test_cases_) { + auto ros_param = rclcpp::Parameter("test", value); + std::shared_ptr param; + ASSERT_NO_THROW(param = read_parameter(ros_param)); + EXPECT_EQ(param->get_name(), ros_param.get_name()); + EXPECT_EQ(param->get_parameter_type(), type); + EXPECT_EQ(param->get_parameter_value(), value); + + rclcpp::Parameter new_ros_param; + ASSERT_NO_THROW(new_ros_param = write_parameter(param)); + EXPECT_EQ(new_ros_param, ros_param); + } +} + +TYPED_TEST_P(ParameterTranslationTest, ConstRead) { + for (auto const& [value, type]: this->test_cases_) { + auto param = state_representation::make_shared_parameter("test", value); + rclcpp::Parameter ros_param("test", value); + + std::shared_ptr new_param; + EXPECT_NO_THROW(new_param = read_parameter_const(ros_param, param)); + EXPECT_EQ(new_param->get_name(), param->get_name()); + EXPECT_EQ(new_param->get_parameter_type(), type); + EXPECT_EQ(new_param->get_parameter_type(), param->get_parameter_type()); + EXPECT_EQ(new_param->get_parameter_value(), value); + + // the const reader constructs a new parameter, so it should not be possible to reference or modify the original + new_param->set_name("other"); + EXPECT_NE(new_param->get_name(), param->get_name()); + } +} + +TYPED_TEST_P(ParameterTranslationTest, NonConstRead) { + for (auto const& [value, type]: this->test_cases_) { + auto param = std::make_shared>("test"); + rclcpp::Parameter ros_param("test", value); + + // make a copy of the pointer referencing the parameter + auto param_ref = param; + ASSERT_NO_THROW(read_parameter(ros_param, param)); + EXPECT_EQ(param->get_name(), ros_param.get_name()); + EXPECT_EQ(param->get_parameter_type(), type); + EXPECT_EQ(param->get_value(), value); + + // The reference should be preserved + EXPECT_EQ(param_ref->get_name(), ros_param.get_name()); + EXPECT_EQ(param_ref->get_parameter_type(), type); + EXPECT_EQ(param_ref->get_value(), value); + } +} + +REGISTER_TYPED_TEST_SUITE_P(ParameterTranslationTest, Write, ReadAndReWrite, ConstRead, NonConstRead); + +using ParameterTestTypes = testing::Types< + bool, std::vector, int, std::vector, double, std::vector, std::string, std::vector>; +INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ParameterTranslationTest, ParameterTestTypes); diff --git a/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp similarity index 94% rename from source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp rename to source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp index fc3edc0d4..f3eb48f6f 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/test_translators.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp @@ -1,7 +1,7 @@ #include -#include "modulo_new_core/translators/ReadStateConversion.hpp" -#include "modulo_new_core/translators/WriteStateConversion.hpp" +#include "modulo_new_core/translators/message_readers.hpp" +#include "modulo_new_core/translators/message_writers.hpp" #include #include @@ -37,7 +37,7 @@ static void expect_quaternion_equal(const Eigen::Quaterniond& q1, const geometry EXPECT_FLOAT_EQ(q1.w(), q2.w); } -class TranslatorsTest : public ::testing::Test { +class MessageTranslatorsTest : public ::testing::Test { protected: void SetUp() override { state_ = state_representation::CartesianState::Random("test", "reference"); @@ -48,7 +48,7 @@ class TranslatorsTest : public ::testing::Test { rclcpp::Clock clock_; }; -TEST_F(TranslatorsTest, TestAccel) { +TEST_F(MessageTranslatorsTest, TestAccel) { auto accel = geometry_msgs::msg::Accel(); write_msg(accel, state_, clock_.now()); expect_vector_equal(state_.get_linear_acceleration(), accel.linear); @@ -70,7 +70,7 @@ TEST_F(TranslatorsTest, TestAccel) { EXPECT_TRUE(new_state.get_acceleration().isApprox(state_.get_acceleration())); } -TEST_F(TranslatorsTest, TestPose) { +TEST_F(MessageTranslatorsTest, TestPose) { auto pose = geometry_msgs::msg::Pose(); write_msg(pose, state_, clock_.now()); expect_point_equal(state_.get_position(), pose.position); @@ -92,7 +92,7 @@ TEST_F(TranslatorsTest, TestPose) { EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); } -TEST_F(TranslatorsTest, TestTransform) { +TEST_F(MessageTranslatorsTest, TestTransform) { auto tf = geometry_msgs::msg::Transform(); write_msg(tf, state_, clock_.now()); expect_vector_equal(state_.get_position(), tf.translation); @@ -116,7 +116,7 @@ TEST_F(TranslatorsTest, TestTransform) { EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); } -TEST_F(TranslatorsTest, TestTwist) { +TEST_F(MessageTranslatorsTest, TestTwist) { auto twist = geometry_msgs::msg::Twist(); write_msg(twist, state_, clock_.now()); expect_vector_equal(state_.get_linear_velocity(), twist.linear); @@ -138,7 +138,7 @@ TEST_F(TranslatorsTest, TestTwist) { EXPECT_TRUE(new_state.get_twist().isApprox(state_.get_twist())); } -TEST_F(TranslatorsTest, TestWrench) { +TEST_F(MessageTranslatorsTest, TestWrench) { auto wrench = geometry_msgs::msg::Wrench(); write_msg(wrench, state_, clock_.now()); expect_vector_equal(state_.get_force(), wrench.force); @@ -160,7 +160,7 @@ TEST_F(TranslatorsTest, TestWrench) { EXPECT_TRUE(new_state.get_wrench().isApprox(state_.get_wrench())); } -TEST_F(TranslatorsTest, TestJointState) { +TEST_F(MessageTranslatorsTest, TestJointState) { auto msg = sensor_msgs::msg::JointState(); write_msg(msg, joint_state_, clock_.now()); for (unsigned int i = 0; i < joint_state_.get_size(); ++i) { @@ -181,7 +181,7 @@ TEST_F(TranslatorsTest, TestJointState) { } } -TEST_F(TranslatorsTest, TestStdMsgs) { +TEST_F(MessageTranslatorsTest, TestStdMsgs) { test_std_messages(true, clock_.now()); test_std_messages(0.3, clock_.now()); test_std_messages(2, clock_.now()); @@ -198,7 +198,7 @@ TEST_F(TranslatorsTest, TestStdMsgs) { } } -TEST_F(TranslatorsTest, TestEncodedState) { +TEST_F(MessageTranslatorsTest, TestEncodedState) { auto msg = modulo_new_core::EncodedState(); write_msg(msg, state_, clock_.now()); state_representation::CartesianState new_state; From 9043bdc01fce5740ad0b34cca046c76751637249 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 19 Apr 2022 21:13:58 +0200 Subject: [PATCH 19/71] Support write/read for MessagePair (#60) * Update formatting and tests in translators * Add and test read/write for MessagePair * Revive PublisherInterface tests for EncodedState * Fix test case --- .../communication/MessagePair.hpp | 8 ++- .../translators/message_writers.hpp | 7 ++- .../src/translators/message_writers.cpp | 8 +-- .../communication/test_message_pair.cpp | 51 +++++++++++-------- .../communication/test_publisher_handler.cpp | 47 +++++++++-------- .../cpp_tests/translators/test_messages.cpp | 12 +++++ 6 files changed, 77 insertions(+), 56 deletions(-) diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index 92684a5b3..c2933f68d 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -40,8 +40,7 @@ inline MsgT MessagePair::write_message() const { template<> inline EncodedState MessagePair::write_message() const { auto msg = EncodedState(); - // TODO write the translators -// translators::write_msg(msg, this->data_, clock_->now()); + translators::write_msg(msg, this->data_, clock_->now()); return msg; } @@ -51,9 +50,8 @@ inline void MessagePair::read_message(const MsgT& message) { } template<> -inline void MessagePair::read_message(const EncodedState&) { - // TODO write the translators -// translators::read_msg(this->data_, message, clock_->now()); +inline void MessagePair::read_message(const EncodedState& message) { + translators::read_msg(this->data_, message); } template diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp index 96dfcc182..472792566 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp @@ -134,6 +134,7 @@ void write_msg(U& msg, const state_representation::Parameter& state, const rc * @brief Convert a boolean to a ROS std_msgs::msg::Bool * @param msg The ROS msg to populate * @param state The state to read from + * @param time The time of the message */ void write_msg(std_msgs::msg::Bool& msg, const bool& state, const rclcpp::Time& time); @@ -141,6 +142,7 @@ void write_msg(std_msgs::msg::Bool& msg, const bool& state, const rclcpp::Time& * @brief Convert a double to a ROS std_msgs::msg::Float64 * @param msg The ROS msg to populate * @param state The state to read from + * @param time The time of the message */ void write_msg(std_msgs::msg::Float64& msg, const double& state, const rclcpp::Time& time); @@ -148,6 +150,7 @@ void write_msg(std_msgs::msg::Float64& msg, const double& state, const rclcpp::T * @brief Convert a vector of double to a ROS std_msgs::msg::Float64MultiArray * @param msg The ROS msg to populate * @param state The state to read from + * @param time The time of the message */ void write_msg(std_msgs::msg::Float64MultiArray& msg, const std::vector& state, const rclcpp::Time& time); @@ -155,6 +158,7 @@ void write_msg(std_msgs::msg::Float64MultiArray& msg, const std::vector& * @brief Convert an integer to a ROS std_msgs::msg::Int32 * @param msg The ROS msg to populate * @param state The state to read from + * @param time The time of the message */ void write_msg(std_msgs::msg::Int32& msg, const int& state, const rclcpp::Time& time); @@ -162,6 +166,7 @@ void write_msg(std_msgs::msg::Int32& msg, const int& state, const rclcpp::Time& * @brief Convert a string to a ROS std_msgs::msg::String * @param msg The ROS msg to populate * @param state The state to read from + * @param time The time of the message */ void write_msg(std_msgs::msg::String& msg, const std::string& state, const rclcpp::Time& time); @@ -172,7 +177,7 @@ void write_msg(std_msgs::msg::String& msg, const std::string& state, const rclcp * @param state The state to read from * @param time The time of the message */ -template +template inline void write_msg(EncodedState& msg, const T& state, const rclcpp::Time&) { std::string tmp = clproto::encode(state); msg.data = std::vector(tmp.begin(), tmp.end()); diff --git a/source/modulo_new_core/src/translators/message_writers.cpp b/source/modulo_new_core/src/translators/message_writers.cpp index 31dcbae40..ba582e437 100644 --- a/source/modulo_new_core/src/translators/message_writers.cpp +++ b/source/modulo_new_core/src/translators/message_writers.cpp @@ -117,7 +117,7 @@ void write_msg(tf2_msgs::msg::TFMessage& msg, const CartesianState& state, const msg.transforms.push_back(transform); } -template +template void write_msg(U& msg, const Parameter& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); @@ -133,17 +133,17 @@ template void write_msg(std_msgs::msg::Bool& msg, con template void write_msg(std_msgs::msg::String& msg, const Parameter& state, const rclcpp::Time&); -template <> +template<> void write_msg(geometry_msgs::msg::Transform& msg, const Parameter& state, const rclcpp::Time& time) { write_msg(msg, state.get_value(), time); } -template <> +template<> void write_msg(geometry_msgs::msg::TransformStamped& msg, const Parameter& state, const rclcpp::Time& time) { write_msg(msg, state.get_value(), time); } -template <> +template<> void write_msg(tf2_msgs::msg::TFMessage& msg, const Parameter& state, const rclcpp::Time& time) { write_msg(msg, state.get_value(), time); } diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp index 9931cda43..a627563de 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -48,25 +48,32 @@ TEST_F(MessagePairTest, BasicTypes) { test_message_interface("this", "that", clock_); } -// FIXME after translators are finished -//TEST_F(MessagePairTest, TestCartesianState) { -// auto initial_value = state_representation::CartesianState::Random("test"); -// auto data = std::make_shared(initial_value); -// auto msg_pair = std::make_shared>( -// MessageType::ENCODED_STATE, data, clock_ -// ); -// EXPECT_TRUE(initial_value.data().isApprox(msg_pair->get_data()->data())); -// -// std::shared_ptr msg_pair_interface(msg_pair); -// auto msg = msg_pair_interface->write(); -// std::string tmp(msg.data.begin(), msg.data.end()); -// auto decoded = clproto::decode(tmp); -// EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); -// -// auto new_value = state_representation::CartesianState::Identity("world"); -// *data = new_value; -// msg = msg_pair_interface->write(); -// tmp = std::string(msg.data.begin(), msg.data.end()); -// decoded = clproto::decode(tmp); -// EXPECT_TRUE(new_value.data().isApprox(decoded.data())); -//} +TEST_F(MessagePairTest, TestCartesianState) { + auto initial_value = state_representation::CartesianState::Random("test"); + auto data = state_representation::make_shared_state(initial_value); + auto msg_pair = + std::make_shared>(data, clock_); + EXPECT_TRUE(initial_value.data().isApprox( + std::dynamic_pointer_cast(msg_pair->get_data())->data())); + + std::shared_ptr msg_pair_interface(msg_pair); + auto msg = msg_pair_interface->write(); + std::string tmp(msg.data.begin(), msg.data.end()); + auto decoded = clproto::decode(tmp); + EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); + + auto new_value = state_representation::CartesianState::Identity("world"); + msg_pair->set_data(state_representation::make_shared_state(new_value)); + msg = msg_pair_interface->write(); + tmp = std::string(msg.data.begin(), msg.data.end()); + decoded = clproto::decode(tmp); + EXPECT_TRUE(new_value.data().isApprox(decoded.data())); + + data = state_representation::make_shared_state(initial_value); + msg_pair->set_data(data); + msg = modulo_new_core::EncodedState(); + modulo_new_core::translators::write_msg(msg, data, clock_->now()); + msg_pair_interface->read(msg); + EXPECT_TRUE(initial_value.data().isApprox( + std::dynamic_pointer_cast(msg_pair->get_data())->data())); +} diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp index 91b54794d..8932c4dd7 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp @@ -55,27 +55,26 @@ TEST_F(PublisherTest, BasicTypes) { test_publisher_interface(node, "this"); } -// FIXME after translators are done -//TEST_F(PublisherTest, CartesianState) { -// // create message pair -// auto data = -// std::make_shared(state_representation::CartesianState::Random("test")); -// auto msg_pair = std::make_shared>( -// data, node->get_clock()); -// -// // create publisher handler -// auto publisher = node->create_publisher("topic", 10); -// auto publisher_handler = std::make_shared, -// modulo_new_core::EncodedState>>( -// PublisherType::PUBLISHER, publisher -// ); -// -// // use in publisher interface -// std::shared_ptr publisher_interface(publisher_handler); -// EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); -// publisher_interface->set_message_pair(msg_pair); -// EXPECT_NO_THROW(publisher_interface->publish()); -// -// publisher_interface = create_publisher_interface(publisher_handler, msg_pair); -// EXPECT_NO_THROW(publisher_interface->publish()); -//} \ No newline at end of file +TEST_F(PublisherTest, CartesianState) { + // create message pair + auto data = + std::make_shared(state_representation::CartesianState::Random("test")); + auto msg_pair = std::make_shared>( + data, node->get_clock()); + + // create publisher handler + auto publisher = node->create_publisher("topic", 10); + auto publisher_handler = std::make_shared, + modulo_new_core::EncodedState>>( + PublisherType::PUBLISHER, publisher + ); + + // use in publisher interface + std::shared_ptr publisher_interface(publisher_handler); + EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); + publisher_interface->set_message_pair(msg_pair); + EXPECT_NO_THROW(publisher_interface->publish()); + + publisher_interface = publisher_handler->create_publisher_interface(msg_pair); + EXPECT_NO_THROW(publisher_interface->publish()); +} \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp index f3eb48f6f..546e6bc24 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp @@ -206,4 +206,16 @@ TEST_F(MessageTranslatorsTest, TestEncodedState) { EXPECT_TRUE(state_.data().isApprox(new_state.data())); EXPECT_EQ(state_.get_name(), new_state.get_name()); EXPECT_EQ(state_.get_reference_frame(), new_state.get_reference_frame()); +} + +TEST_F(MessageTranslatorsTest, TestEncodedStatePointer) { + auto state_ptr = state_representation::make_shared_state(state_); + auto msg = modulo_new_core::EncodedState(); + write_msg(msg, state_ptr, clock_.now()); + auto new_state_ptr = state_representation::make_shared_state(state_representation::CartesianState()); + read_msg(new_state_ptr, msg); + auto new_state = *std::dynamic_pointer_cast(new_state_ptr); + EXPECT_TRUE(state_.data().isApprox(new_state.data())); + EXPECT_EQ(state_.get_name(), new_state.get_name()); + EXPECT_EQ(state_.get_reference_frame(), new_state.get_reference_frame()); } \ No newline at end of file From 7d635a53066cbf3ec6d92bfc604b42b7bbb6778f Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Tue, 19 Apr 2022 23:15:41 +0200 Subject: [PATCH 20/71] Component Parameters (#58) * Component Parameters * Implement add_ and get_ methods for component parameters, using the modulo parameter translators. * Link clproto in CMakeLists because of parameter translators * Add tests for component parameter behaviour * Mark parameter getters const * Fix build warnings * Remove unused utilities and arguments --- source/modulo_components/CMakeLists.txt | 5 +- .../modulo_components/ComponentInterface.hpp | 154 ++++++++++++++++-- .../modulo_components/utilities/utilities.hpp | 31 ---- .../test/cpp/test_component_interface.cpp | 4 + .../test_component_interface_parameters.cpp | 92 +++++++++++ 5 files changed, 237 insertions(+), 49 deletions(-) create mode 100644 source/modulo_components/test/cpp/test_component_interface_parameters.cpp diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 74fb209fe..1c0b8407e 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() find_package(control_libraries 5.1.0 REQUIRED COMPONENTS state_representation) +find_library(clproto REQUIRED) include_directories(include) @@ -30,6 +31,8 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${PROJECT_SOURCE_DIR}/src/Component.cpp ${PROJECT_SOURCE_DIR}/src/LifecycleComponent.cpp) +target_link_libraries(${PROJECT_NAME} clproto) + # Install Python modules ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) @@ -42,7 +45,7 @@ if(BUILD_TESTING) ament_add_gtest(test_cpp_components ${TEST_CPP_SOURCES}) target_include_directories(test_cpp_components PRIVATE include) - target_link_libraries(test_cpp_components ${PROJECT_NAME} state_representation) + target_link_libraries(test_cpp_components ${PROJECT_NAME} state_representation clproto) target_compile_definitions(test_cpp_components PRIVATE TEST_FIXTURES="${CMAKE_CURRENT_SOURCE_DIR}/test/fixtures/") # prevent pluginlib from using boost diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 1174e928d..0ba1fb65b 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -1,17 +1,21 @@ #pragma once -#include #include #include #include +#include #include #include #include +#include +#include + #include #include #include +#include #include "modulo_components/exceptions/PredicateNotFoundException.hpp" #include "modulo_components/utilities/utilities.hpp" @@ -24,6 +28,8 @@ class ComponentInterface : NodeT { friend class ComponentInterfaceTest; public: + friend class ComponentInterfacePublicInterface; + /** * @brief Constructor from node options * @param node_options node options as used in ROS2 Node @@ -33,6 +39,53 @@ class ComponentInterface : NodeT { ); protected: + /** + * @brief Add a parameter. + * @details This method stores a pointer reference to an existing Parameter object in the local parameter map + * and declares the equivalent ROS parameter on the ROS interface. + * @param parameter A ParameterInterface pointer to a Parameter instance. + */ + void add_parameter(const std::shared_ptr& parameter); + + /** + * @brief Add a parameter. + * @details This method creates a new Parameter object instance to reference in the local parameter map + * and declares the equivalent ROS parameter on the ROS interface. + * @tparam T The type of the parameter + * @param name The name of the parameter + * @param value The value of the parameter + */ + template + void add_parameter(const std::string& name, const T& value); + + /** + * @brief Get a parameter by name. + * @param name The name of the parameter + * @return The ParameterInterface pointer to a Parameter instance + */ + std::shared_ptr get_parameter(const std::string& name) const; + + /** + * @brief Get a parameter value by name. + * @tparam T The type of the parameter + * @param name The name of the parameter + * @return The value of the parameter + */ + template + T get_parameter_value(const std::string& name) const; + + /** + * @brief Parameter validation function to be redefined by derived Component classes. + * @details This method is automatically invoked whenever the ROS interface tried to modify a parameter. + * Validation and sanitization can be performed by reading or writing the value of the parameter through the + * ParameterInterface pointer, depending on the parameter name and desired component behaviour. If the validation + * returns true, the updated parameter value (including any modifications) is applied. If the validation returns + * false, any changes to the parameter are discarded and the parameter value is not changed. + * @param parameter A ParameterInterface pointer to a Parameter instance + * @return The validation result + */ + virtual bool validate_parameter(const std::shared_ptr& parameter); + /** * @brief Add a predicate to the map of predicates * @param predicate_name the name of the associated predicate @@ -78,6 +131,13 @@ class ComponentInterface : NodeT { lookup_transform(const std::string& frame_name, const std::string& reference_frame_name = "world") const; private: + /** + * @brief Callback function to validate and update parameters on change. + * @param parameters The new parameter objects provided by the ROS interface + * @return The result of the validation + */ + rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector& parameters); + [[nodiscard]] std::string generate_predicate_topic(const std::string& predicate_name) const; void add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); @@ -91,10 +151,10 @@ class ComponentInterface : NodeT { std::map predicates_; std::map>> predicate_publishers_; + state_representation::ParameterMap parameter_map_; + std::shared_ptr parameter_cb_handle_; + std::shared_ptr step_timer_; - rclcpp::Parameter period_; - rclcpp::Parameter has_tf_listener_; - rclcpp::Parameter has_tf_broadcaster_; std::shared_ptr tf_buffer_; std::shared_ptr tf_listener_; std::shared_ptr tf_broadcaster_; @@ -105,26 +165,26 @@ ComponentInterface::ComponentInterface( const rclcpp::NodeOptions& options, modulo_new_core::communication::PublisherType publisher_type ) : rclcpp::Node(utilities::parse_node_name(options, "ComponentInterface"), options), publisher_type_(publisher_type) { - this->declare_parameter("period", 2.0); - this->declare_parameter("has_tf_listener", false); - this->declare_parameter("has_tf_broadcaster", false); - - // here I need to do typename NodeT:: because ParameterMap also has get_parameter - period_ = NodeT::get_parameter("period"); - has_tf_listener_ = NodeT::get_parameter("has_tf_listener"); - has_tf_broadcaster_ = NodeT::get_parameter("has_tf_broadcaster"); - - if (has_tf_listener_.as_bool()) { + // register the parameter change callback handler + parameter_cb_handle_ = NodeT::add_on_set_parameters_callback( + [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { + return this->on_set_parameters_callback(parameters); + }); + this->add_parameter("period", 1.0); + this->add_parameter("has_tf_listener", false); + this->add_parameter("has_tf_broadcaster", false); + + if (this->get_parameter_value("has_tf_listener")) { this->tf_buffer_ = std::make_shared(this->get_clock()); this->tf_listener_ = std::make_shared(*this->tf_buffer_); } - if (has_tf_broadcaster_.as_bool()) { + if (this->get_parameter_value("has_tf_broadcaster")) { this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); } this->step_timer_ = this->create_wall_timer( - std::chrono::nanoseconds(static_cast(this->period_.as_double() * 1e9)), [this] { this->step(); } - ); + std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), + [this] { this->step(); }); } template @@ -162,6 +222,66 @@ void ComponentInterface::add_variant_predicate( } } +template +template +void ComponentInterface::add_parameter(const std::string& name, const T& value) { + this->add_parameter(state_representation::make_shared_parameter(name, value)); +} + +template +template +T ComponentInterface::get_parameter_value(const std::string& name) const { + return this->parameter_map_.template get_parameter_value(name); +} + +template +void +ComponentInterface::add_parameter(const std::shared_ptr& parameter) { + parameter_map_.set_parameter(parameter); + auto ros_param = modulo_new_core::translators::write_parameter(parameter); + NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value()); +} + +template +std::shared_ptr +ComponentInterface::get_parameter(const std::string& name) const { + return this->parameter_map_.get_parameter(name); +} + +template +bool ComponentInterface::validate_parameter( + const std::shared_ptr& +) { + return true; +} + +template +rcl_interfaces::msg::SetParametersResult +ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + for (const auto& ros_parameter: parameters) { + try { + // get the associated parameter interface by name + auto parameter = parameter_map_.get_parameter(ros_parameter.get_name()); + + // convert the ROS parameter into a ParameterInterface without modifying the original + auto new_parameter = modulo_new_core::translators::read_parameter_const(ros_parameter, parameter); + if (!validate_parameter(new_parameter)) { + result.successful = false; + result.reason += "Parameter " + ros_parameter.get_name() + " could not be set! "; + } else { + // update the value of the parameter in the map + modulo_new_core::translators::copy_parameter_value(new_parameter, parameter); + } + } catch (const std::exception& ex) { + result.successful = false; + result.reason += ex.what(); + } + } + return result; +} + template void ComponentInterface::add_predicate(const std::string& name, bool predicate) { this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index 9542020a8..0d8bf902d 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/source/modulo_components/include/modulo_components/utilities/utilities.hpp @@ -37,35 +37,4 @@ static std::string parse_node_name(const rclcpp::NodeOptions& options, const std return parse_string_argument(options.arguments(), pattern, node_name); } -/** - * @brief Parse a string node namespace from NodeOptions arguments. - * @param options the NodeOptions structure to parse - * @param fallback the default namespace if the NodeOptions structure cannot be parsed - * @return the parsed node namespace or the fallback namespace - */ -static std::string parse_node_namespace(const rclcpp::NodeOptions& options, const std::string& fallback="") { - std::string node_namespace(fallback); - const std::string pattern("__ns:="); - return parse_string_argument(options.arguments(), pattern, node_namespace); -} - -/** - * @brief Parse a period in seconds from NodeOptions parameters. - * @details The parameter must have the name "period" - * and be interpretable as a value in seconds of type double - * @param options the NodeOptions structure to parse - * @param default_period the default period in seconds - * @return the parsed period or the default period as a chrono duration - */ -static std::chrono::nanoseconds parse_period(const rclcpp::NodeOptions& options, double default_period = 0.001) { - double period = default_period; - for (auto& param : options.parameter_overrides()) { - if (param.get_name() == "period") { - period = param.as_double(); - break; - } - } - return std::chrono::nanoseconds(static_cast(period * 1e9)); -} - }// namespace modulo_components::utilities \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index ccf1bd69f..ff1b9ff1f 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -13,6 +13,10 @@ class ComponentInterfaceTest : public ::testing::Test { rclcpp::init(0, nullptr); } + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + void SetUp() override { component_ = std::make_shared>( rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::PUBLISHER diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp new file mode 100644 index 000000000..9e8e834c9 --- /dev/null +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -0,0 +1,92 @@ +#include +#include +#include +#include + +#include + +#include "modulo_components/ComponentInterface.hpp" +#include "modulo_new_core/EncodedState.hpp" + +namespace modulo_components { + +class ComponentInterfacePublicInterface : public ComponentInterface { +public: + ComponentInterfacePublicInterface(const rclcpp::NodeOptions& node_options) : + ComponentInterface(node_options, modulo_new_core::communication::PublisherType::PUBLISHER) {} + using ComponentInterface::add_parameter; + using ComponentInterface::get_parameter; + using ComponentInterface::get_parameter_value; + using ComponentInterface::parameter_map_; + + bool validate_parameter(const std::shared_ptr&) override { + validate_was_called = true; + return validation_return_value; + } + + rclcpp::Parameter get_ros_parameter(const std::string& name) { + return rclcpp::Node::get_parameter(name); + } + + rcl_interfaces::msg::SetParametersResult set_ros_parameter(const rclcpp::Parameter& parameter) { + return rclcpp::Node::set_parameter(parameter); + } + + bool validate_was_called = false; + bool validation_return_value = true; +}; + +class ComponentInterfaceParameterTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + component_ = std::make_shared(rclcpp::NodeOptions()); + } + + template + void expect_parameter_value(const T& value) { + EXPECT_STREQ(component_->get_ros_parameter("test").value_to_string().c_str(), std::to_string(value).c_str()); + EXPECT_EQ(component_->get_parameter_value("test"), value); + EXPECT_EQ(component_->parameter_map_.get_parameter_value("test"), value); + } + + std::shared_ptr component_; +}; + + +TEST_F(ComponentInterfaceParameterTest, AddParameter) { + EXPECT_THROW(component_->get_parameter("test"), state_representation::exceptions::InvalidParameterException); + EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + auto param = state_representation::make_shared_parameter("test", 1); + component_->add_parameter(param); + + // Adding the parameter should declare and set the value and call the validation function + EXPECT_TRUE(component_->validate_was_called); + EXPECT_NO_THROW(component_->get_parameter("test")); + EXPECT_NO_THROW(component_->get_ros_parameter("test")); + expect_parameter_value(1); + + // Setting the parameter value should call the validation function and update the referenced value + component_->validate_was_called = false; + component_->set_ros_parameter({"test", 2}); + EXPECT_TRUE(component_->validate_was_called); + expect_parameter_value(2); + EXPECT_EQ(param->get_value(), 2); + + // If the validation function returns false, setting the parameter value should _not_ update the referenced value + component_->validate_was_called = false; + component_->validation_return_value = false; + component_->set_ros_parameter({"test", 3}); + EXPECT_TRUE(component_->validate_was_called); + expect_parameter_value(2); + EXPECT_EQ(param->get_value(), 2); +} + +} // namespace modulo_components \ No newline at end of file From 966ed1e39cc40e8483c480d37c0ffef53d609eb7 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Fri, 22 Apr 2022 17:00:37 +0200 Subject: [PATCH 21/71] Component parameter description and improvements (#61) * Add parameter description * Require a parameter description string when adding parameters * Check if a parameter has been declared when adding, and if it has, just set the value instead * Add local parameter value setter * For convenience, add a local parameter value setter that invokes the Node-level parameter setter (including validation) * Revise transform parameters as functions * Implement TF broadcaster and listener initializations as setup functions rather than parameters. The TF broadcaster and listener were initialized on construction based on a parameter. This meant that derived classes could not override the behaviour unless they explicitly manipulated the NodeOptions structure before passing it to the base constructor, since base classes are constructed first. * Extend tests * Test read-only parameter --- .../modulo_components/ComponentInterface.hpp | 86 +++++++++--- .../test_component_interface_parameters.cpp | 122 ++++++++++++++++-- 2 files changed, 179 insertions(+), 29 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 0ba1fb65b..e4e488abd 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -45,7 +45,10 @@ class ComponentInterface : NodeT { * and declares the equivalent ROS parameter on the ROS interface. * @param parameter A ParameterInterface pointer to a Parameter instance. */ - void add_parameter(const std::shared_ptr& parameter); + void add_parameter( + const std::shared_ptr& parameter, const std::string& description, + bool read_only = false + ); /** * @brief Add a parameter. @@ -56,14 +59,14 @@ class ComponentInterface : NodeT { * @param value The value of the parameter */ template - void add_parameter(const std::string& name, const T& value); + void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false); /** * @brief Get a parameter by name. * @param name The name of the parameter * @return The ParameterInterface pointer to a Parameter instance */ - std::shared_ptr get_parameter(const std::string& name) const; + [[nodiscard]] std::shared_ptr get_parameter(const std::string& name) const; /** * @brief Get a parameter value by name. @@ -74,6 +77,17 @@ class ComponentInterface : NodeT { template T get_parameter_value(const std::string& name) const; + /** + * @brief Set the value of a parameter. + * @details The parameter must have been previously declared. This method preserves the reference + * to the original Parameter instance + * @tparam T The type of the parameter + * @param name The name of the parameter + * @return The value of the parameter + */ + template + void set_parameter_value(const std::string& name, const T& value); + /** * @brief Parameter validation function to be redefined by derived Component classes. * @details This method is automatically invoked whenever the ROS interface tried to modify a parameter. @@ -125,6 +139,16 @@ class ComponentInterface : NodeT { */ void set_predicate(const std::string& predicate_name, const std::function& predicate_function); + /** + * @brief Configure a transform broadcaster. + */ + void add_tf_broadcaster(); + + /** + * @brief Configure a transform buffer and listener. + */ + void add_tf_listener(); + void send_transform(const state_representation::CartesianPose& transform); [[nodiscard]] state_representation::CartesianPose @@ -170,17 +194,7 @@ ComponentInterface::ComponentInterface( [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { return this->on_set_parameters_callback(parameters); }); - this->add_parameter("period", 1.0); - this->add_parameter("has_tf_listener", false); - this->add_parameter("has_tf_broadcaster", false); - - if (this->get_parameter_value("has_tf_listener")) { - this->tf_buffer_ = std::make_shared(this->get_clock()); - this->tf_listener_ = std::make_shared(*this->tf_buffer_); - } - if (this->get_parameter_value("has_tf_broadcaster")) { - this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); - } + this->add_parameter("period", 1.0, "The time interval in seconds for all periodic callbacks", true); this->step_timer_ = this->create_wall_timer( std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), @@ -224,8 +238,10 @@ void ComponentInterface::add_variant_predicate( template template -void ComponentInterface::add_parameter(const std::string& name, const T& value) { - this->add_parameter(state_representation::make_shared_parameter(name, value)); +void ComponentInterface::add_parameter( + const std::string& name, const T& value, const std::string& description, bool read_only +) { + this->add_parameter(state_representation::make_shared_parameter(name, value), description, read_only); } template @@ -235,11 +251,20 @@ T ComponentInterface::get_parameter_value(const std::string& name) const } template -void -ComponentInterface::add_parameter(const std::shared_ptr& parameter) { - parameter_map_.set_parameter(parameter); +void ComponentInterface::add_parameter( + const std::shared_ptr& parameter, const std::string& description, + bool read_only +) { auto ros_param = modulo_new_core::translators::write_parameter(parameter); - NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value()); + if (!NodeT::has_parameter(parameter->get_name())) { + parameter_map_.set_parameter(parameter); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = description; + descriptor.read_only = read_only; + NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); + } else { + NodeT::set_parameter(ros_param); + } } template @@ -248,6 +273,16 @@ ComponentInterface::get_parameter(const std::string& name) const { return this->parameter_map_.get_parameter(name); } +template +template +void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { + rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter( + modulo_new_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); + if (!result.successful) { + throw state_representation::exceptions::InvalidParameterException(result.reason); + } +} + template bool ComponentInterface::validate_parameter( const std::shared_ptr& @@ -333,6 +368,17 @@ void ComponentInterface::set_predicate( this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } +template +void ComponentInterface::add_tf_broadcaster() { + this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); +} + +template +void ComponentInterface::add_tf_listener() { + this->tf_buffer_ = std::make_shared(this->get_clock()); + this->tf_listener_ = std::make_shared(*this->tf_buffer_); +} + template void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { // TODO: throw here? diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index 9e8e834c9..043761905 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -17,6 +17,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface::add_parameter; using ComponentInterface::get_parameter; using ComponentInterface::get_parameter_value; + using ComponentInterface::set_parameter_value; using ComponentInterface::parameter_map_; bool validate_parameter(const std::shared_ptr&) override { @@ -32,6 +33,10 @@ class ComponentInterfacePublicInterface : public ComponentInterface(rclcpp::NodeOptions()); + param_ = state_representation::make_shared_parameter("test", 1); } template @@ -58,35 +64,133 @@ class ComponentInterfaceParameterTest : public ::testing::Test { } std::shared_ptr component_; + std::shared_ptr> param_; }; - TEST_F(ComponentInterfaceParameterTest, AddParameter) { - EXPECT_THROW(component_->get_parameter("test"), state_representation::exceptions::InvalidParameterException); + EXPECT_THROW(auto discard = component_->get_parameter("test"), + state_representation::exceptions::InvalidParameterException); + EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + component_->add_parameter(param_, "Test parameter"); + + // Adding the parameter should declare and set the value and call the validation function + EXPECT_TRUE(component_->validate_was_called); + EXPECT_NO_THROW(auto discard = component_->get_parameter("test")); + EXPECT_NO_THROW(component_->get_ros_parameter("test")); + expect_parameter_value(1); +} + +TEST_F(ComponentInterfaceParameterTest, AddNameValueParameter) { + EXPECT_THROW(auto discard = component_->get_parameter("test"), + state_representation::exceptions::InvalidParameterException); EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); - auto param = state_representation::make_shared_parameter("test", 1); - component_->add_parameter(param); + component_->add_parameter("test", 1, "Test parameter"); // Adding the parameter should declare and set the value and call the validation function EXPECT_TRUE(component_->validate_was_called); - EXPECT_NO_THROW(component_->get_parameter("test")); + EXPECT_NO_THROW(auto discard = component_->get_parameter("test")); EXPECT_NO_THROW(component_->get_ros_parameter("test")); expect_parameter_value(1); +} + +TEST_F(ComponentInterfaceParameterTest, AddParameterAgain) { + EXPECT_THROW(auto + discard = component_->get_parameter("test"), + state_representation::exceptions::InvalidParameterException); + EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + component_->add_parameter(param_, "Test parameter"); + + // Adding an existing parameter again should just set the value + component_->validate_was_called = false; + EXPECT_NO_THROW(component_->add_parameter("test", 2, "foo")); + EXPECT_TRUE(component_->validate_was_called); + expect_parameter_value(2); + EXPECT_EQ(param_->get_value(), 2); +} + +TEST_F(ComponentInterfaceParameterTest, SetParameter) { + // setting before adding should not work + EXPECT_THROW(component_->set_parameter_value("test", 1), rclcpp::exceptions::ParameterNotDeclaredException); + + // validation should not be invoked as the parameter did not exist + EXPECT_FALSE(component_->validate_was_called); + EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + + component_->add_parameter(param_, "Test parameter"); // Setting the parameter value should call the validation function and update the referenced value component_->validate_was_called = false; - component_->set_ros_parameter({"test", 2}); + EXPECT_NO_THROW(component_->set_parameter_value("test", 2)); EXPECT_TRUE(component_->validate_was_called); expect_parameter_value(2); - EXPECT_EQ(param->get_value(), 2); + EXPECT_EQ(param_->get_value(), 2); // If the validation function returns false, setting the parameter value should _not_ update the referenced value component_->validate_was_called = false; component_->validation_return_value = false; - component_->set_ros_parameter({"test", 3}); + EXPECT_THROW(component_->set_parameter_value("test", 3), state_representation::exceptions::InvalidParameterException); + EXPECT_TRUE(component_->validate_was_called); + expect_parameter_value(2); + EXPECT_EQ(param_->get_value(), 2); + + // Setting a value with an incompatible type should not update the parameter + EXPECT_THROW(component_->set_parameter_value("test", "foo"), + state_representation::exceptions::InvalidParameterException); EXPECT_TRUE(component_->validate_was_called); expect_parameter_value(2); - EXPECT_EQ(param->get_value(), 2); + EXPECT_EQ(param_->get_value(), 2); +} + +TEST_F(ComponentInterfaceParameterTest, SetParameterROS) { + component_->add_parameter(param_, "Test parameter"); + + // Setting the parameter value should call the validation function and update the referenced value + component_->validate_was_called = false; + auto result = component_->set_ros_parameter({"test", 2}); + EXPECT_TRUE(component_->validate_was_called); + EXPECT_TRUE(result.successful); + expect_parameter_value(2); + EXPECT_EQ(param_->get_value(), 2); + + // If the validation function returns false, setting the parameter value should _not_ update the referenced value + component_->validate_was_called = false; + component_->validation_return_value = false; + result = component_->set_ros_parameter({"test", 3}); + EXPECT_TRUE(component_->validate_was_called); + EXPECT_FALSE(result.successful); + expect_parameter_value(2); + EXPECT_EQ(param_->get_value(), 2); +} + +TEST_F(ComponentInterfaceParameterTest, GetParameterDescription) { + component_->add_parameter(param_, "Test parameter"); + EXPECT_STREQ(component_->get_parameter_description("test").c_str(), "Test parameter"); + + EXPECT_THROW(component_->get_parameter_description("foo"), rclcpp::exceptions::ParameterNotDeclaredException); +} + +TEST_F(ComponentInterfaceParameterTest, ReadOnlyParameter) { + // Adding a read-only parameter should behave normally + component_->add_parameter(param_, "Test parameter", true); + EXPECT_TRUE(component_->validate_was_called); + EXPECT_NO_THROW(auto discard = component_->get_parameter("test")); + EXPECT_NO_THROW(component_->get_ros_parameter("test")); + expect_parameter_value(1); + + // Trying to set the value of the read-only parameter should fail before the validation step + component_->validate_was_called = false; + EXPECT_THROW(component_->set_parameter_value("test", 2), state_representation::exceptions::InvalidParameterException); + EXPECT_FALSE(component_->validate_was_called); + expect_parameter_value(1); + EXPECT_EQ(param_->get_value(), 1); + + // Setting the value on the ROS interface should also fail + component_->validate_was_called = false; + auto result = component_->set_ros_parameter({"test", 2}); + EXPECT_FALSE(component_->validate_was_called); + EXPECT_FALSE(result.successful); + expect_parameter_value(1); + EXPECT_EQ(param_->get_value(), 1); } } // namespace modulo_components \ No newline at end of file From e75439d604c289615aa245874074cd238786c2c9 Mon Sep 17 00:00:00 2001 From: Baptiste Busch Date: Tue, 26 Apr 2022 15:14:19 +0200 Subject: [PATCH 22/71] Update predicate logic in ComponentInterface (#50) * fix: modify the behavior with predicates not found to avoid raising unhandled exceptions * fix: add comment * fix: add more tests to showcase the predicate error handling * fix: set does not add if not found anymore * fix: change to true for more clarity * fix: correct error message to match python version * fix: use the new add_parameter functions * fix: remove part that should not be here --- .../modulo_components/ComponentInterface.hpp | 31 +++++++++++++------ .../exceptions/PredicateNotFoundException.hpp | 12 ------- .../test/cpp/test_component_interface.cpp | 27 ++++++++++++---- 3 files changed, 42 insertions(+), 28 deletions(-) delete mode 100644 source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index e4e488abd..d3ca7f7b9 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -17,7 +17,6 @@ #include #include -#include "modulo_components/exceptions/PredicateNotFoundException.hpp" #include "modulo_components/utilities/utilities.hpp" #include "modulo_components/utilities/predicate_variant.hpp" @@ -121,7 +120,7 @@ class ComponentInterface : NodeT { * map of predicates * @return the value of the predicate as a boolean */ - [[nodiscard]] bool get_predicate(const std::string& predicate_name) const; + [[nodiscard]] bool get_predicate(const std::string& predicate_name); /** * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything @@ -208,8 +207,8 @@ void ComponentInterface::step() { msg.data = this->get_predicate(predicate.first); auto predicate_iterator = this->predicate_publishers_.find(predicate.first); if (predicate_iterator == this->predicate_publishers_.end()) { - // TODO throw here - RCLCPP_FATAL(this->get_logger(), "no publisher for predicate found"); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, + "No publisher for predicate " << predicate.first << " found."); return; } predicate_publishers_.at(predicate.first)->publish(msg); @@ -226,7 +225,7 @@ void ComponentInterface::add_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate ) { if (this->predicates_.find(name) != this->predicates_.end()) { - RCLCPP_INFO(this->get_logger(), "Predicate already exists, overwriting"); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate " << name << " already exists, overwriting."); this->predicates_.at(name) = predicate; } else { this->predicates_.insert(std::make_pair(name, predicate)); @@ -330,10 +329,13 @@ void ComponentInterface::add_predicate( } template -bool ComponentInterface::get_predicate(const std::string& predicate_name) const { +bool ComponentInterface::get_predicate(const std::string& predicate_name) { auto predicate_iterator = this->predicates_.find(predicate_name); + // if there is no predicate with that name simply return false with an error message if (predicate_iterator == this->predicates_.end()) { - throw exceptions::PredicateNotFoundException(predicate_name); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, + "Predicate " << predicate_name << " does not exists, returning false."); + return false; } // try to get the value from the variant as a bool auto* ptr_value = std::get_if(&predicate_iterator->second); @@ -342,7 +344,14 @@ bool ComponentInterface::get_predicate(const std::string& predicate_name) } // if previous check failed, it means the variant is actually a callback function auto callback_function = std::get>(predicate_iterator->second); - return (callback_function)(); + bool value = false; + try { + value = (callback_function)(); + } catch (const std::exception& e) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, + "Error while evaluating the callback function: " << e.what()); + } + return value; } template @@ -351,9 +360,11 @@ void ComponentInterface::set_variant_predicate( ) { auto predicate_iterator = this->predicates_.find(name); if (predicate_iterator == this->predicates_.end()) { - throw exceptions::PredicateNotFoundException(name); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, + "Cannot set predicate " << name << " with a new value because it does not exist."); + return; } - this->predicates_.at(name) = predicate; + predicate_iterator->second = predicate; } template diff --git a/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp b/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp deleted file mode 100644 index b489c8caa..000000000 --- a/source/modulo_components/include/modulo_components/exceptions/PredicateNotFoundException.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo_components::exceptions { -class PredicateNotFoundException : public std::runtime_error { -public: - explicit PredicateNotFoundException(const std::string& predicate_name) : - runtime_error("Predicate " + predicate_name + " not found in the list of predicates") {}; -}; -}// namespace modulo_components::exceptions diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index ff1b9ff1f..98183d5b2 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -43,6 +43,10 @@ class ComponentInterfaceTest : public ::testing::Test { component_->set_predicate(predicate_name, predicate_value); } + void set_predicate(const std::string& predicate_name, const std::function& predicate_function) { + component_->set_predicate(predicate_name, predicate_function); + } + std::shared_ptr> component_; }; @@ -67,19 +71,30 @@ TEST_F(ComponentInterfaceTest, AddFunctionPredicate) { TEST_F(ComponentInterfaceTest, GetPredicateValue) { add_predicate("foo", true); EXPECT_TRUE(get_predicate("foo")); - add_predicate("bar", [&]() { return false; }); - EXPECT_FALSE(get_predicate("bar")); - EXPECT_THROW(get_predicate("test"), exceptions::PredicateNotFoundException); + add_predicate("bar", [&]() { return true; }); + EXPECT_TRUE(get_predicate("bar")); + // predicate does not exist, expect false + EXPECT_FALSE(get_predicate("test")); + // error in callback function except false + add_predicate( + "error", [&]() { + throw std::runtime_error("An error occurred"); + return false; + } + ); + EXPECT_FALSE(get_predicate("error")); } TEST_F(ComponentInterfaceTest, SetPredicateValue) { add_predicate("foo", true); set_predicate("foo", false); EXPECT_FALSE(get_predicate("foo")); - EXPECT_THROW(set_predicate("bar", true), exceptions::PredicateNotFoundException); - add_predicate("bar", [&]() { return false; }); + // predicate does not exist so setting won't do anything set_predicate("bar", true); - EXPECT_TRUE(get_predicate("bar")); + EXPECT_FALSE(get_predicate("bar")); + add_predicate("bar", true); + set_predicate("bar", [&]() { return false; }); + EXPECT_FALSE(get_predicate("bar")); } } // namespace modulo_components \ No newline at end of file From 345d6692ebee6b8432bf5474616991f0472c4b49 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 27 Apr 2022 10:19:11 +0200 Subject: [PATCH 23/71] Add methods to add output/publishers (#62) * Add helper for making shared message pair interface * Add activate/deactivate for publisher interface * Rename tests and add helper function to component interface * Add constructor and add_output to Component * Add constructor and add_output to LifecycleComponent * Fix exception namespace * Hide functions and qos from interface class * Try to be more consistent with exceptions in modulo core * Catch exceptions * Add default_topic argument and parse_signal_name utility * Implement parse_signal_name * Don't throw on activate and deactivate of non lifecycle publishers --- source/modulo_components/CMakeLists.txt | 10 +- .../include/modulo_components/Component.hpp | 87 +++++++++++++++ .../modulo_components/ComponentInterface.hpp | 33 +++++- .../modulo_components/LifecycleComponent.hpp | 32 ++++++ .../modulo_components/utilities/utilities.hpp | 21 +++- source/modulo_components/src/Component.cpp | 11 +- .../src/LifecycleComponent.cpp | 105 +++++++++++++++++- .../test/cpp/test_component.cpp | 45 ++++++++ .../test/cpp/test_lifecycle_component.cpp | 49 ++++++++ ...ponents.cpp => test_modulo_components.cpp} | 0 .../communication/MessagePair.hpp | 14 +++ .../communication/PublisherHandler.hpp | 34 ++++++ .../communication/PublisherInterface.hpp | 12 +- .../NotLifecyclePublisherException.hpp | 11 ++ .../src/communication/MessagePair.cpp | 38 +++++++ .../src/communication/PublisherInterface.cpp | 75 ++++++++++++- 16 files changed, 561 insertions(+), 16 deletions(-) create mode 100644 source/modulo_components/test/cpp/test_component.cpp create mode 100644 source/modulo_components/test/cpp/test_lifecycle_component.cpp rename source/modulo_components/test/{test_cpp_components.cpp => test_modulo_components.cpp} (100%) create mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 1c0b8407e..00e4a0d3a 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -43,13 +43,13 @@ if(BUILD_TESTING) # add cpp tests file(GLOB_RECURSE TEST_CPP_SOURCES ${PROJECT_SOURCE_DIR}/test/ test_*.cpp) - ament_add_gtest(test_cpp_components ${TEST_CPP_SOURCES}) - target_include_directories(test_cpp_components PRIVATE include) - target_link_libraries(test_cpp_components ${PROJECT_NAME} state_representation clproto) - target_compile_definitions(test_cpp_components PRIVATE TEST_FIXTURES="${CMAKE_CURRENT_SOURCE_DIR}/test/fixtures/") + ament_add_gtest(test_modulo_components ${TEST_CPP_SOURCES}) + target_include_directories(test_modulo_components PRIVATE include) + target_link_libraries(test_modulo_components ${PROJECT_NAME} state_representation clproto) + target_compile_definitions(test_modulo_components PRIVATE TEST_FIXTURES="${CMAKE_CURRENT_SOURCE_DIR}/test/fixtures/") # prevent pluginlib from using boost - target_compile_definitions(test_cpp_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + target_compile_definitions(test_modulo_components PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") # add python tests file(GLOB_RECURSE TEST_PYTHON_SOURCES ${PROJECT_SOURCE_DIR}/test/ test_*.py) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 218471732..501ab571d 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -3,12 +3,99 @@ #include #include "modulo_components/ComponentInterface.hpp" +#include "modulo_components/utilities/utilities.hpp" #include "modulo_new_core/EncodedState.hpp" namespace modulo_components { class Component : public ComponentInterface { +public: + friend class ComponentPublicInterface; + /** + * @brief Constructor from node options + * @param node_options node options as used in ROS2 Node + */ + explicit Component(const rclcpp::NodeOptions& node_options); + +protected: + template + void add_output( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, + const std::string& default_topic = "" + ); + +private: + using ComponentInterface::create_output; + using ComponentInterface::qos_; }; +template +void Component::add_output( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, + const std::string& default_topic +) { + using namespace modulo_new_core::communication; + try { + std::string parsed_signal_name = utilities::parse_signal_name(signal_name); + this->create_output(parsed_signal_name, data, fixed_topic, default_topic); + auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair(); + switch (message_pair->get_type()) { + case MessageType::BOOL: { + auto publisher = this->create_publisher(topic_name, this->qos_); + this->outputs_.at(parsed_signal_name) = + std::make_shared, std_msgs::msg::Bool>>( + PublisherType::PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::FLOAT64: { + auto publisher = this->create_publisher(topic_name, this->qos_); + this->outputs_.at(parsed_signal_name) = + std::make_shared, std_msgs::msg::Float64>>( + PublisherType::PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::FLOAT64_MULTI_ARRAY: { + auto publisher = this->create_publisher(topic_name, this->qos_); + this->outputs_.at(parsed_signal_name) = + std::make_shared, + std_msgs::msg::Float64MultiArray>>( + PublisherType::PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::INT32: { + auto publisher = this->create_publisher(topic_name, this->qos_); + this->outputs_.at(parsed_signal_name) = + std::make_shared, std_msgs::msg::Int32>>( + PublisherType::PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::STRING: { + auto publisher = this->create_publisher(topic_name, this->qos_); + this->outputs_.at(parsed_signal_name) = + std::make_shared, std_msgs::msg::String>>( + PublisherType::PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::ENCODED_STATE: { + auto publisher = this->create_publisher(topic_name, this->qos_); + this->outputs_.at(parsed_signal_name) = + std::make_shared, + modulo_new_core::EncodedState>>( + PublisherType::PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + } + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); + } +} + } \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index d3ca7f7b9..c3bab0386 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -12,6 +12,8 @@ #include #include +#include +#include #include #include #include @@ -23,7 +25,7 @@ namespace modulo_components { template -class ComponentInterface : NodeT { +class ComponentInterface : public NodeT { friend class ComponentInterfaceTest; public: @@ -148,11 +150,21 @@ class ComponentInterface : NodeT { */ void add_tf_listener(); + template + void create_output( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, + const std::string& default_topic = "" + ); + void send_transform(const state_representation::CartesianPose& transform); [[nodiscard]] state_representation::CartesianPose lookup_transform(const std::string& frame_name, const std::string& reference_frame_name = "world") const; + std::map> outputs_; + + rclcpp::QoS qos_ = rclcpp::QoS(10); + private: /** * @brief Callback function to validate and update parameters on change. @@ -187,7 +199,7 @@ template ComponentInterface::ComponentInterface( const rclcpp::NodeOptions& options, modulo_new_core::communication::PublisherType publisher_type ) : - rclcpp::Node(utilities::parse_node_name(options, "ComponentInterface"), options), publisher_type_(publisher_type) { + NodeT(utilities::parse_node_name(options, "ComponentInterface"), options), publisher_type_(publisher_type) { // register the parameter change callback handler parameter_cb_handle_ = NodeT::add_on_set_parameters_callback( [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { @@ -419,4 +431,21 @@ state_representation::CartesianPose ComponentInterface::lookup_transform( return result; } +template +template +inline void ComponentInterface::create_output( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, + const std::string& default_topic +) { + using namespace modulo_new_core::communication; + auto message_pair = make_shared_message_pair(data, this->get_clock()); + this->outputs_.insert( + std::make_pair( + signal_name, std::make_shared(this->publisher_type_, message_pair))); + std::string topic_name = default_topic.empty() ? "~/" + signal_name : default_topic; + this->add_parameter( + signal_name + "_topic", topic_name, "Output topic name of signal '" + signal_name + "'", fixed_topic + ); +} + }// namespace modulo_components diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index eeda138be..1798a74dc 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -11,7 +11,39 @@ namespace modulo_components { class LifecycleComponent : public ComponentInterface { +public: + friend class LifecycleComponentPublicInterface; + /** + * @brief Constructor from node options + * @param node_options node options as used in ROS2 Node + */ + explicit LifecycleComponent(const rclcpp::NodeOptions& node_options); + +protected: + template + void add_output(const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false); + +private: + bool configure_outputs(); + + bool activate_outputs(); + + bool deactivate_outputs(); + + using ComponentInterface::create_output; + using ComponentInterface::qos_; }; +template +void +LifecycleComponent::add_output(const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic) { + try { + std::string parsed_signal_name = utilities::parse_signal_name(signal_name); + this->create_output(parsed_signal_name, data, fixed_topic); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); + } +} + } \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index 0d8bf902d..067c82511 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/source/modulo_components/include/modulo_components/utilities/utilities.hpp @@ -31,10 +31,29 @@ static std::string parse_string_argument(const std::vector& args, c * @param fallback the default name if the NodeOptions structure cannot be parsed * @return the parsed node name or the fallback name */ -static std::string parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback="") { +static std::string parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback = "") { std::string node_name(fallback); const std::string pattern("__node:="); return parse_string_argument(options.arguments(), pattern, node_name); } +/** + * @brief Parse a string signal name from a user-provided input. + * @details This functions removes all characters different from + * a-z, A-Z, 0-9, and _ from a string. + * @param signal_name The input string + * @return The sanitized string + */ +static std::string parse_signal_name(const std::string& signal_name) { + std::string output; + for (char c: signal_name) { + if ((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9') || c == '_') { + if (!(c == '_' && output.empty())) { + output.insert(output.end(), std::tolower(c)); + } + } + } + return output; +} + }// namespace modulo_components::utilities \ No newline at end of file diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 5c3128de4..0f69cbfe3 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -1 +1,10 @@ -#include "modulo_components/Component.hpp" \ No newline at end of file +#include "modulo_components/Component.hpp" + +using namespace modulo_new_core::communication; + +namespace modulo_components { + +Component::Component(const rclcpp::NodeOptions& node_options) : + ComponentInterface(node_options, PublisherType::PUBLISHER) {} + +} \ No newline at end of file diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 8788cd275..30ea9d23e 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -1 +1,104 @@ -#include "modulo_components/LifecycleComponent.hpp" \ No newline at end of file +#include "modulo_components/LifecycleComponent.hpp" + +using namespace modulo_new_core::communication; + +namespace modulo_components { + +LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options) : + ComponentInterface(node_options, PublisherType::LIFECYCLE_PUBLISHER) {} + +bool LifecycleComponent::configure_outputs() { + bool success = true; + for (auto& [name, interface]: this->outputs_) { + try { + auto topic_name = this->get_parameter_value(name + "_topic"); + auto message_pair = interface->get_message_pair(); + switch (message_pair->get_type()) { + case MessageType::BOOL: { + auto publisher = this->create_publisher(topic_name, this->qos_); + interface = std::make_shared, + std_msgs::msg::Bool>>( + PublisherType::LIFECYCLE_PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::FLOAT64: { + auto publisher = this->create_publisher(topic_name, this->qos_); + interface = std::make_shared, + std_msgs::msg::Float64>>( + PublisherType::LIFECYCLE_PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::FLOAT64_MULTI_ARRAY: { + auto publisher = this->create_publisher( + topic_name, this->qos_ + ); + interface = + std::make_shared, + std_msgs::msg::Float64MultiArray>>( + PublisherType::LIFECYCLE_PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::INT32: { + auto publisher = this->create_publisher(topic_name, this->qos_); + interface = std::make_shared, + std_msgs::msg::Int32>>( + PublisherType::LIFECYCLE_PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::STRING: { + auto publisher = this->create_publisher(topic_name, this->qos_); + interface = std::make_shared, + std_msgs::msg::String>>( + PublisherType::LIFECYCLE_PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + case MessageType::ENCODED_STATE: { + auto publisher = this->create_publisher(topic_name, this->qos_); + interface = + std::make_shared, + modulo_new_core::EncodedState>>( + PublisherType::LIFECYCLE_PUBLISHER, publisher + )->create_publisher_interface(message_pair); + break; + } + } + } catch (const std::exception& ex) { + success = false; + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to configure output '" << name << "': " << ex.what()); + } + } + return success; +} + +bool LifecycleComponent::activate_outputs() { + bool success = true; + for (auto const& [name, interface]: this->outputs_) { + try { + interface->activate(); + } catch (const std::exception& ex) { + success = false; + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to activate output '" << name << "': " << ex.what()); + } + } + return success; +} + +bool LifecycleComponent::deactivate_outputs() { + bool success = true; + for (auto const& [name, interface]: this->outputs_) { + try { + interface->deactivate(); + } catch (const std::exception& ex) { + success = false; + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to deactivate output '" << name << "': " << ex.what()); + } + } + return success; +} + +} \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp new file mode 100644 index 000000000..c7f22345d --- /dev/null +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -0,0 +1,45 @@ +#include +#include +#include + +#include "modulo_components/Component.hpp" +#include "modulo_new_core/EncodedState.hpp" + +using namespace state_representation; + +namespace modulo_components { + +class ComponentPublicInterface : public Component { +public: + explicit ComponentPublicInterface(const rclcpp::NodeOptions& node_options) : Component(node_options) {} + using Component::add_output; + using Component::outputs_; + using Component::get_clock; +}; + +class ComponentTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + component_ = std::make_shared(rclcpp::NodeOptions()); + } + + std::shared_ptr component_; +}; + +TEST_F(ComponentTest, AddOutput) { + std::shared_ptr data = make_shared_state(CartesianState::Random("test")); + component_->add_output("_tEsT_#1@3", data, true); + auto outputs_iterator = component_->outputs_.find("test_13"); + EXPECT_TRUE(outputs_iterator != component_->outputs_.end()); + EXPECT_NO_THROW(component_->outputs_.at("test_13")->publish()); +} + +} // namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_lifecycle_component.cpp b/source/modulo_components/test/cpp/test_lifecycle_component.cpp new file mode 100644 index 000000000..b3f37da0f --- /dev/null +++ b/source/modulo_components/test/cpp/test_lifecycle_component.cpp @@ -0,0 +1,49 @@ +#include +#include +#include + +#include "modulo_components/LifecycleComponent.hpp" +#include "modulo_new_core/EncodedState.hpp" + +using namespace state_representation; + +namespace modulo_components { + +class LifecycleComponentPublicInterface : public LifecycleComponent { +public: + explicit LifecycleComponentPublicInterface(const rclcpp::NodeOptions& node_options) : LifecycleComponent(node_options) {} + using LifecycleComponent::add_output; + using LifecycleComponent::configure_outputs; + using LifecycleComponent::activate_outputs; + using LifecycleComponent::outputs_; + using LifecycleComponent::get_clock; +}; + +class LifecycleComponentTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + component_ = std::make_shared(rclcpp::NodeOptions()); + } + + std::shared_ptr component_; +}; + +TEST_F(LifecycleComponentTest, AddOutput) { + std::shared_ptr data = make_shared_state(CartesianState::Random("test")); + component_->add_output("test", data, true); + auto outputs_iterator = component_->outputs_.find("test"); + EXPECT_TRUE(outputs_iterator != component_->outputs_.end()); + EXPECT_NO_THROW(component_->configure_outputs()); + EXPECT_NO_THROW(component_->activate_outputs()); + EXPECT_NO_THROW(component_->outputs_.at("test")->publish()); +} + +} // namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/test/test_cpp_components.cpp b/source/modulo_components/test/test_modulo_components.cpp similarity index 100% rename from source/modulo_components/test/test_cpp_components.cpp rename to source/modulo_components/test/test_modulo_components.cpp diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index c2933f68d..f0e60289a 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -39,6 +39,9 @@ inline MsgT MessagePair::write_message() const { template<> inline EncodedState MessagePair::write_message() const { + if (this->data_ == nullptr) { + throw exceptions::NullPointerException("The message pair data is not set, nothing to write"); + } auto msg = EncodedState(); translators::write_msg(msg, this->data_, clock_->now()); return msg; @@ -46,11 +49,17 @@ inline EncodedState MessagePair::writ template inline void MessagePair::read_message(const MsgT& message) { + if (this->data_ == nullptr) { + throw exceptions::NullPointerException("The message pair data is not set, nothing to read"); + } translators::read_msg(*this->data_, message); } template<> inline void MessagePair::read_message(const EncodedState& message) { + if (this->data_ == nullptr) { + throw exceptions::NullPointerException("The message pair data is not set, nothing to read"); + } translators::read_msg(this->data_, message); } @@ -66,4 +75,9 @@ inline void MessagePair::set_data(const std::shared_ptr& dat } this->data_ = data; } + +template +std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock); + }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp index cba7b9950..efdbd6ae5 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp @@ -1,6 +1,9 @@ #pragma once +#include + #include "modulo_new_core/communication/PublisherInterface.hpp" +#include "modulo_new_core/exceptions/NotLifecyclePublisherException.hpp" namespace modulo_new_core::communication { @@ -9,6 +12,10 @@ class PublisherHandler : public PublisherInterface { public: PublisherHandler(PublisherType type, std::shared_ptr publisher); + void on_activate(); + + void on_deactivate(); + void publish(const MsgT& message) const; std::shared_ptr @@ -22,8 +29,35 @@ template PublisherHandler::PublisherHandler(PublisherType type, std::shared_ptr publisher) : PublisherInterface(type), publisher_(std::move(publisher)) {} +template +void PublisherHandler::on_activate() { + if (this->publisher_ == nullptr) { + throw exceptions::NullPointerException("Publisher not set"); + } + if (this->get_type() == PublisherType::LIFECYCLE_PUBLISHER) { + this->publisher_->on_activate(); + } else { + RCLCPP_WARN(rclcpp::get_logger("PublisherHandler"), "Only LifecyclePublishers can be deactivated"); + } +} + +template +void PublisherHandler::on_deactivate() { + if (this->publisher_ == nullptr) { + throw exceptions::NullPointerException("Publisher not set"); + } + if (this->get_type() == PublisherType::LIFECYCLE_PUBLISHER) { + this->publisher_->on_deactivate(); + } else { + RCLCPP_WARN(rclcpp::get_logger("PublisherHandler"), "Only LifecyclePublishers can be deactivated"); + } +} + template void PublisherHandler::publish(const MsgT& message) const { + if (this->publisher_ == nullptr) { + throw exceptions::NullPointerException("Publisher not set"); + } this->publisher_->publish(message); } diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp index a99af0228..228781067 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp @@ -15,17 +15,23 @@ class PublisherHandler; class PublisherInterface : public std::enable_shared_from_this { public: - explicit PublisherInterface(PublisherType type); + explicit PublisherInterface(PublisherType type, std::shared_ptr message_pair = nullptr); PublisherInterface(const PublisherInterface& publisher) = default; virtual ~PublisherInterface() = default; template - std::shared_ptr> get_publisher(bool validate_pointer = true); + std::shared_ptr> get_handler(bool validate_pointer = true); + + void activate(); + + void deactivate(); void publish(); + [[nodiscard]] std::shared_ptr get_message_pair() const; + void set_message_pair(const std::shared_ptr& message_pair); PublisherType get_type() const; @@ -39,7 +45,7 @@ class PublisherInterface : public std::enable_shared_from_this -inline std::shared_ptr> PublisherInterface::get_publisher(bool validate_pointer) { +inline std::shared_ptr> PublisherInterface::get_handler(bool validate_pointer) { std::shared_ptr> publisher_ptr; try { publisher_ptr = std::dynamic_pointer_cast>(this->shared_from_this()); diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp new file mode 100644 index 000000000..aa364d97d --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace modulo_new_core::exceptions { +class NotLifecyclePublisherException : public std::runtime_error { +public: + explicit NotLifecyclePublisherException(const std::string& msg) : std::runtime_error(msg) {}; +}; +} \ No newline at end of file diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_new_core/src/communication/MessagePair.cpp index 8fe5ee511..a28ba42fc 100644 --- a/source/modulo_new_core/src/communication/MessagePair.cpp +++ b/source/modulo_new_core/src/communication/MessagePair.cpp @@ -38,4 +38,42 @@ MessagePair::MessagePair( ) : MessagePairInterface(MessageType::ENCODED_STATE), data_(std::move(data)), clock_(std::move(clock)) {} +template<> +std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} + +template<> +std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} + +template<> +std::shared_ptr make_shared_message_pair( + const std::shared_ptr>& data, const std::shared_ptr& clock +) { + return std::make_shared>>(data, clock); +} + +template<> +std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} + +template<> +std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} + +template<> +std::shared_ptr make_shared_message_pair( + const std::shared_ptr& data, const std::shared_ptr& clock +) { + return std::make_shared>(data, clock); +} + }// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/src/communication/PublisherInterface.cpp b/source/modulo_new_core/src/communication/PublisherInterface.cpp index 3876e3e94..2da1cf06a 100644 --- a/source/modulo_new_core/src/communication/PublisherInterface.cpp +++ b/source/modulo_new_core/src/communication/PublisherInterface.cpp @@ -1,5 +1,7 @@ #include "modulo_new_core/communication/PublisherInterface.hpp" +#include + #include #include #include @@ -13,7 +15,70 @@ namespace modulo_new_core::communication { -PublisherInterface::PublisherInterface(PublisherType type) : type_(type) {} +PublisherInterface::PublisherInterface(PublisherType type, std::shared_ptr message_pair) : + type_(type), message_pair_(std::move(message_pair)) {} + +void PublisherInterface::activate() { + if (this->message_pair_ == nullptr) { + throw exceptions::NullPointerException("Message pair is not set, cannot deduce message type"); + } + switch (this->message_pair_->get_type()) { + case MessageType::BOOL: + this->template get_handler, + std_msgs::msg::Bool>()->on_activate(); + break; + case MessageType::FLOAT64: + this->template get_handler, + std_msgs::msg::Float64>()->on_activate(); + break; + case MessageType::FLOAT64_MULTI_ARRAY: + this->template get_handler, + std_msgs::msg::Float64MultiArray>()->on_activate(); + break; + case MessageType::INT32: + this->template get_handler, + std_msgs::msg::Int32>()->on_activate(); + break; + case MessageType::STRING: + this->template get_handler, + std_msgs::msg::String>()->on_activate(); + break; + case MessageType::ENCODED_STATE: + this->template get_handler, EncodedState>()->on_activate(); + break; + } +} + +void PublisherInterface::deactivate() { + if (this->message_pair_ == nullptr) { + throw exceptions::NullPointerException("Message pair is not set, cannot deduce message type"); + } + switch (this->message_pair_->get_type()) { + case MessageType::BOOL: + this->template get_handler, + std_msgs::msg::Bool>()->on_deactivate(); + break; + case MessageType::FLOAT64: + this->template get_handler, + std_msgs::msg::Float64>()->on_deactivate(); + break; + case MessageType::FLOAT64_MULTI_ARRAY: + this->template get_handler, + std_msgs::msg::Float64MultiArray>()->on_deactivate(); + break; + case MessageType::INT32: + this->template get_handler, + std_msgs::msg::Int32>()->on_deactivate(); + break; + case MessageType::STRING: + this->template get_handler, + std_msgs::msg::String>()->on_deactivate(); + break; + case MessageType::ENCODED_STATE: + this->template get_handler, EncodedState>()->on_deactivate(); + break; + } +} void PublisherInterface::publish() { if (this->message_pair_ == nullptr) { @@ -45,14 +110,18 @@ template void PublisherInterface::publish(const MsgT& message) { switch (this->get_type()) { case PublisherType::PUBLISHER: - this->template get_publisher, MsgT>()->publish(message); + this->template get_handler, MsgT>()->publish(message); break; case PublisherType::LIFECYCLE_PUBLISHER: - this->template get_publisher, MsgT>()->publish(message); + this->template get_handler, MsgT>()->publish(message); break; } } +std::shared_ptr PublisherInterface::get_message_pair() const { + return this->message_pair_; +} + void PublisherInterface::set_message_pair(const std::shared_ptr& message_pair) { if (message_pair == nullptr) { throw exceptions::NullPointerException("Provide a valid pointer"); From d104bde10a93dc5c9aa3cfc3885686d7ecca3d35 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 27 Apr 2022 15:52:13 +0200 Subject: [PATCH 24/71] Add docstrings and QoS getter/setter (#63) * Add docstrings in modulo_new_core * Add docstrings in modulo_components * Fix missing character Co-authored-by: Baptiste Busch Co-authored-by: Baptiste Busch --- .../include/modulo_components/Component.hpp | 7 ++ .../modulo_components/ComponentInterface.hpp | 56 +++++++++++--- .../modulo_components/LifecycleComponent.hpp | 19 +++++ .../src/ComponentInterface.cpp | 11 ++- .../include/modulo_new_core/EncodedState.hpp | 8 +- .../communication/MessagePair.hpp | 31 +++++++- .../communication/MessagePairInterface.hpp | 54 ++++++++++++- .../communication/MessageType.hpp | 4 + .../communication/PublisherHandler.hpp | 28 ++++++- .../communication/PublisherInterface.hpp | 76 ++++++++++++++++++- .../communication/PublisherType.hpp | 4 + 11 files changed, 279 insertions(+), 19 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 501ab571d..5fef83f66 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -19,6 +19,13 @@ class Component : public ComponentInterface { explicit Component(const rclcpp::NodeOptions& node_options); protected: + /** + * @brief Add and configure an output signal of the component. + * @tparam DataT Type of the data pointer + * @param signal_name Name of the output signal + * @param data Data to transmit on the output signal + * @param fixed_topic If true, the topic name of the output signal is fixed + */ template void add_output( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index c3bab0386..50ca487dc 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -150,20 +150,51 @@ class ComponentInterface : public NodeT { */ void add_tf_listener(); + /** + * @brief Helper function to parse the signal name and add an unconfigured + * PublisherInterface to the map of outputs. + * @tparam DataT Type of the data pointer + * @param signal_name Name of the output signal + * @param data Data to transmit on the output signal + * @param fixed_topic If true, the topic name of the output signal is fixed + */ template void create_output( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, const std::string& default_topic = "" ); + /** + * @brief Getter of the Quality of Service attribute. + * @return The Quality of Service attribute + */ + [[nodiscard]] rclcpp::QoS get_qos() const; + + /** + * @brief Set the Quality of Service for ROS publishers and subscribers. + * @param qos The desired Quality of Service + */ + void set_qos(const rclcpp::QoS& qos); + + /** + * @brief Send a transform to TF. + * @param transform The transform to send + */ void send_transform(const state_representation::CartesianPose& transform); + /** + * @brief Look up a transform from TF. + * @param frame_name The desired frame of the transform + * @param reference_frame_name The desired reference frame of the transform + * @return If it exists, the requested transform + */ [[nodiscard]] state_representation::CartesianPose lookup_transform(const std::string& frame_name, const std::string& reference_frame_name = "world") const; - std::map> outputs_; + std::map> + outputs_; ///< Map of outputs - rclcpp::QoS qos_ = rclcpp::QoS(10); + rclcpp::QoS qos_ = rclcpp::QoS(10); ///< Quality of Service for ROS publishers and subscribers private: /** @@ -181,18 +212,21 @@ class ComponentInterface : public NodeT { void step(); - modulo_new_core::communication::PublisherType publisher_type_; + modulo_new_core::communication::PublisherType + publisher_type_; ///< Type of the output publishers (one of PUBLISHER, LIFECYCLE_PUBLISHER) - std::map predicates_; - std::map>> predicate_publishers_; + std::map predicates_; ///< Map of predicates + std::map>> + predicate_publishers_; ///< Map of predicate publishers - state_representation::ParameterMap parameter_map_; - std::shared_ptr parameter_cb_handle_; + state_representation::ParameterMap parameter_map_; ///< ParameterMap for handling parameters + std::shared_ptr + parameter_cb_handle_; ///< ROS callback function handle for setting parameters - std::shared_ptr step_timer_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - std::shared_ptr tf_broadcaster_; + std::shared_ptr step_timer_; ///< Timer for the step function + std::shared_ptr tf_buffer_; ///< TF buffer + std::shared_ptr tf_listener_; ///< TF listener + std::shared_ptr tf_broadcaster_; ///< TF broadcaster }; template diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 1798a74dc..4d866031a 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -21,14 +21,33 @@ class LifecycleComponent : public ComponentInterface void add_output(const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false); private: + /** + * @brief Configure all outputs. + * @return True configuration was successful + */ bool configure_outputs(); + /** + * @brief Activate all outputs. + * @return True activation was successful + */ bool activate_outputs(); + /** + * @brief Deactivate all outputs. + * @return True deactivation was successful + */ bool deactivate_outputs(); using ComponentInterface::create_output; diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index d7dcf1aed..107d25ab2 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -1,6 +1,15 @@ #include "modulo_components/ComponentInterface.hpp" - namespace modulo_components { +template +rclcpp::QoS ComponentInterface::get_qos() const { + return this->qos_; +} + +template +void ComponentInterface::set_qos(const rclcpp::QoS& qos) { + this->qos_ = qos; +} + }// namespace modulo_components diff --git a/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp b/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp index a3b3709f7..79c37eaa4 100644 --- a/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp +++ b/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp @@ -3,5 +3,11 @@ #include namespace modulo_new_core { - typedef std_msgs::msg::UInt8MultiArray EncodedState; + +/** + * @typedef EncodedState + * @brief Define the EncodedState as UInt8MultiArray message type. + */ +typedef std_msgs::msg::UInt8MultiArray EncodedState; + } \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index f0e60289a..ed3a962c3 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -9,22 +9,49 @@ namespace modulo_new_core::communication { +/** + * @class MessagePair + * @brief The MessagePair stores a pointer to a variable and + * translates the value of this pointer back and forth between + * the corresponding ROS messages. + * @tparam MsgT ROS message type of the MessagePair + * @tparam DataT Data type corresponding to the ROS message type + */ template class MessagePair : public MessagePairInterface { public: + /** + * @brief Constructor of the MessagePair. + * @param data The pointer referring to the data stored in the MessagePair + * @param clock The ROS clock for translating messages + */ MessagePair(std::shared_ptr data, std::shared_ptr clock); + /** + * @brief Write the value of the data pointer to a ROS message. + * @return The value of the data pointer as a ROS message + */ [[nodiscard]] MsgT write_message() const; + /** + * @brief Read a ROS message and store the value in the data pointer. + * @param message The ROS message to read + */ void read_message(const MsgT& message); + /** + * @brief Get the data pointer. + */ [[nodiscard]] std::shared_ptr get_data() const; + /** + * @brief Set the data pointer. + */ void set_data(const std::shared_ptr& data); private: - std::shared_ptr data_; - std::shared_ptr clock_; + std::shared_ptr data_; ///< Pointer referring to the data stored in the MessagePair + std::shared_ptr clock_; ///< ROS clock for translating messages }; template diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp index 38e61a346..86d22a2b6 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp @@ -12,27 +12,79 @@ namespace modulo_new_core::communication { template class MessagePair; +/** + * @class MessagePairInterface + * @brief Interface class to enable non-templated writing and reading ROS messages + * from derived MessagePair instances through dynamic downcasting. + */ class MessagePairInterface : public std::enable_shared_from_this { public: + /** + * @brief Constructor with the message type + * @param type The message type of the message pair + */ explicit MessagePairInterface(MessageType type); + /** + * @brief Default virtual destructor. + */ virtual ~MessagePairInterface() = default; + /** + * @brief Copy constructor from another MessagePairInterface. + */ MessagePairInterface(const MessagePairInterface& message_pair) = default; + /** + * @brief Get a pointer to a derived MessagePair instance from a MessagePairInterface pointer. + * @details If a MessagePairInterface pointer is used to address a derived MessagePair instance, + * this method will return a pointer to that derived instance through dynamic down-casting. + * The downcast will fail if the base MessagePairInterface object has no reference count + * (if the object is not owned by any pointer), or if the derived object is not a correctly + * typed instance of a MessagePair. By default, an InvalidPointerCastException is thrown when + * the downcast fails. If this validation is disabled by setting the validate_pointer flag to false, + * it will not throw an exception and instead return a null pointer. + * @tparam MsgT The ROS message type of the MessagePair + * @tparam DataT The data type of the MessagePair + * @param validate_pointer If true, throw an exception when downcasting fails + * @return A pointer to a derived MessagePair instance of the desired type, or a null pointer + * if downcasting failed and validate_pointer was set to false. + */ template [[nodiscard]] std::shared_ptr> get_message_pair(bool validate_pointer = true); + /** + * @brief Get the ROS message of a derived MessagePair instance through the MessagePairInterface pointer. + * @details This throws an InvalidPointerCastException if the MessagePairInterface does not point to a + * valid MessagePair instance or if the specified types does not match the type of the MessagePair instance. + * @see MessagePairInterface::get_message_pair() + * @tparam MsgT The ROS message type of the MessagePair + * @tparam DataT The data type of the MessagePair + * @return The ROS message containing the data from the underlying MessagePair instance + */ template [[nodiscard]] MsgT write(); + /** + * @brief Read a ROS message and set the data of the derived MessagePair instance through the + * MessagePairInterface pointer. + * @details This throws an InvalidPointerCastException if the MessagePairInterface does not point to + * a valid MessagePair instance or if the specified types does not match the type of the MessagePair instance. + * @tparam MsgT The ROS message type of the MessagePair + * @tparam DataT The data type of the MessagePair + * @param message The ROS message to read from + */ template void read(const MsgT& message); + /** + * @brief Get the MessageType of the MessagePairInterface. + * @see MessageType + */ MessageType get_type() const; private: - MessageType type_; + MessageType type_; ///< The message type of the MessagePairInterface }; template diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp index d8c07773b..3987f669b 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp @@ -2,6 +2,10 @@ namespace modulo_new_core::communication { +/** + * @brief Enum of all supported ROS message types for the MessagePairInterface + * @see MessagePairInterface + */ enum class MessageType { BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT32, STRING, ENCODED_STATE }; diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp index efdbd6ae5..c3b39d2c7 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp @@ -7,22 +7,48 @@ namespace modulo_new_core::communication { +/** + * @class PublisherHandler + * @brief The PublisherHandler handles different types of ROS publishers to activate, deactivate and publish + * data with those publishers. + * @tparam PubT The ROS publisher type + * @tparam MsgT The ROS message type of the ROS publisher + */ template class PublisherHandler : public PublisherInterface { public: + /** + * @brief Constructor with the publisher type and the pointer to the ROS publisher. + * @param type The publisher type + * @param publisher The pointer to the ROS publisher + */ PublisherHandler(PublisherType type, std::shared_ptr publisher); + /** + * @brief Activate the ROS publisher if applicable. + */ void on_activate(); + /** + * @brief Deactivate the ROS publisher if applicable. + */ void on_deactivate(); + /** + * @brief Publish the ROS message through the ROS publisher. + * @param message The ROS message to publish + */ void publish(const MsgT& message) const; + /** + * @brief Create a PublisherInterface instance from the current PublisherHandler. + * @param message_pair The message pair of the PublisherInterface + */ std::shared_ptr create_publisher_interface(const std::shared_ptr& message_pair); private: - std::shared_ptr publisher_; + std::shared_ptr publisher_; ///< The ROS publisher }; template diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp index 228781067..f45783ab3 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp @@ -13,35 +13,107 @@ namespace modulo_new_core::communication { template class PublisherHandler; +/** + * @class PublisherInterface + * @brief Interface class to enable non-templated activating/deactivating/publishing of ROS publishers + * from derived PublisherHandler instances through dynamic downcasting. + */ class PublisherInterface : public std::enable_shared_from_this { public: + /** + * @brief Constructor with the message type and message pair. + * @param type The type of the publisher interface + * @param message_pair The message pair with the data to be published + */ explicit PublisherInterface(PublisherType type, std::shared_ptr message_pair = nullptr); + /** + * @brief Copy constructor from another PublisherInterface. + */ PublisherInterface(const PublisherInterface& publisher) = default; + /** + * @brief Default virtual destructor. + */ virtual ~PublisherInterface() = default; + /** + * @brief Get a pointer to a derived PublisherHandler instance from a PublisherInterface pointer. + * @details If a PublisherInterface pointer is used to address a derived PublisherHandler instance, + * this method will return a pointer to that derived instance through dynamic down-casting. + * The downcast will fail if the base PublisherInterface object has no reference count + * (if the object is not owned by any pointer), or if the derived object is not a correctly + * typed instance of a PublisherHandler. By default, an InvalidPointerCastException is thrown when + * the downcast fails. If this validation is disabled by setting the validate_pointer flag to false, + * it will not throw an exception and instead return a null pointer. + * @tparam PubT The ROS publisher type + * @tparam MsgT The ROS message type + * @param validate_pointer If true, throw an exception when downcasting fails + * @return A pointer to a derived PublisherHandler instance of the desired type, or a null pointer + * if downcasting failed and validate_pointer was set to false. + */ template std::shared_ptr> get_handler(bool validate_pointer = true); + /** + * @brief Activate ROS publisher of a derived PublisherHandler instance through the PublisherInterface pointer. + * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to + * a valid PublisherHandler instance or if the type of the message pair does not match the + * type of the PublisherHandler instance. + * @see PublisherInterface::get_handler() + */ void activate(); + /** + * @brief Deactivate ROS publisher of a derived PublisherHandler instance through the PublisherInterface pointer. + * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to + * a valid PublisherHandler instance or if the type of the message pair does not match the + * type of the PublisherHandler instance. + * @see PublisherInterface::get_handler() + */ void deactivate(); + /** + * @brief Publish the data stored in the message pair through the ROS publisher of a derived PublisherHandler + * instance through the PublisherInterface pointer. + * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to + * a valid PublisherHandler instance or if the type of the message pair does not match the + * type of the PublisherHandler instance. + * @see PublisherInterface::get_handler() + */ void publish(); + /** + * @brief Get the pointer to the message pair of the PublisherInterface. + */ [[nodiscard]] std::shared_ptr get_message_pair() const; + /** + * @brief Set the pointer to the message pair of the PublisherInterface. + */ void set_message_pair(const std::shared_ptr& message_pair); + /** + * @brief Get the type of the publisher interface. + * @see PublisherType + */ PublisherType get_type() const; private: + /** + * @brief Publish the data stored in the message pair through the ROS publisher of a derived PublisherHandler + * instance through the PublisherInterface pointer. + * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to a + * valid PublisherHandler instance or if the specified type does not match the type of the PublisherHandler instance. + * @see PublisherInterface::get_handler() + * @tparam MsgT Message type of the PublisherHandler + * @param message The ROS message to publish through the ROS publisher + */ template void publish(const MsgT& message); - PublisherType type_; - std::shared_ptr message_pair_; + PublisherType type_; ///< The type of the publisher interface + std::shared_ptr message_pair_; ///< The pointer to the stored MessagePair instance }; template diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp index 5d756312a..67dc14f49 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp @@ -2,6 +2,10 @@ namespace modulo_new_core::communication { +/** + * @brief Enum of supported ROS publisher types for the PublisherInterface. + * @see PublisherInterface + */ enum class PublisherType { PUBLISHER, LIFECYCLE_PUBLISHER }; From 56c2e22a74a39a6f4673f11265e88b52f65c007f Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 29 Apr 2022 18:19:36 +0200 Subject: [PATCH 25/71] Parameter translators in python (#65) * Fixes and move messages test * Implement parameter translators * Implement parameter tests * Improve tests --- .../src/ComponentInterface.cpp | 4 +- .../translators/parameter_translators.py | 154 ++++++++++++++++++ .../test/python_tests/conftest.py | 33 +++- .../test_messages.py} | 8 +- .../translators/test_parameters.py | 133 +++++++++++++++ 5 files changed, 323 insertions(+), 9 deletions(-) create mode 100644 source/modulo_new_core/modulo_new_core/translators/parameter_translators.py rename source/modulo_new_core/test/python_tests/{test_translators.py => translators/test_messages.py} (98%) create mode 100644 source/modulo_new_core/test/python_tests/translators/test_parameters.py diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 107d25ab2..b191c0819 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -2,12 +2,12 @@ namespace modulo_components { -template +template rclcpp::QoS ComponentInterface::get_qos() const { return this->qos_; } -template +template void ComponentInterface::set_qos(const rclcpp::QoS& qos) { this->qos_ = qos; } diff --git a/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py b/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py new file mode 100644 index 000000000..3bb32fc5e --- /dev/null +++ b/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py @@ -0,0 +1,154 @@ +from rclpy import Parameter +import state_representation as sr +import clproto +import numpy as np +from typing import Union + + +def write_parameter(parameter: sr.Parameter) -> Parameter: + if parameter.get_parameter_type() == sr.ParameterType.BOOL or \ + parameter.get_parameter_type() == sr.ParameterType.BOOL_ARRAY or \ + parameter.get_parameter_type() == sr.ParameterType.INT or \ + parameter.get_parameter_type() == sr.ParameterType.INT_ARRAY or \ + parameter.get_parameter_type() == sr.ParameterType.DOUBLE or \ + parameter.get_parameter_type() == sr.ParameterType.DOUBLE_ARRAY or \ + parameter.get_parameter_type() == sr.ParameterType.STRING or \ + parameter.get_parameter_type() == sr.ParameterType.STRING_ARRAY: + return Parameter(parameter.get_name(), value=parameter.get_value()) + elif parameter.get_parameter_type() == sr.ParameterType.STATE: + if parameter.get_parameter_state_type() == sr.StateType.CARTESIAN_STATE: + return Parameter(parameter.get_name(), Parameter.Type.STRING, clproto.to_json( + clproto.encode(parameter.get_value(), clproto.MessageType.CARTESIAN_STATE_MESSAGE))) + elif parameter.get_parameter_state_type() == sr.StateType.CARTESIAN_POSE: + return Parameter(parameter.get_name(), Parameter.Type.STRING, clproto.to_json( + clproto.encode(parameter.get_value(), clproto.MessageType.CARTESIAN_POSE_MESSAGE))) + elif parameter.get_parameter_state_type() == sr.StateType.JOINT_STATE: + return Parameter(parameter.get_name(), Parameter.Type.STRING, clproto.to_json( + clproto.encode(parameter.get_value(), clproto.MessageType.JOINT_STATE_MESSAGE))) + if parameter.get_parameter_state_type() == sr.StateType.JOINT_POSITIONS: + return Parameter(parameter.get_name(), Parameter.Type.STRING, clproto.to_json( + clproto.encode(parameter.get_value(), clproto.MessageType.JOINT_POSITIONS_MESSAGE))) + else: + raise RuntimeError(f"Parameter {parameter.get_name()} could not be written!") + elif parameter.get_parameter_type() == sr.ParameterType.VECTOR or \ + parameter.get_parameter_type() == sr.ParameterType.MATRIX: + return Parameter(parameter.get_name(), value=parameter.get_value().flatten().tolist()) + else: + raise RuntimeError(f"Parameter {parameter.get_name()} could not be written!") + + +def copy_parameter_value(source_parameter: sr.Parameter, parameter: sr.Parameter): + if source_parameter.get_parameter_type() != parameter.get_parameter_type(): + raise RuntimeError(f"Source parameter {source_parameter.get_name()} to be copied does not have the same type " + f"as destination parameter {parameter.get_name()}") + if source_parameter.get_parameter_type() == sr.ParameterType.BOOL or \ + source_parameter.get_parameter_type() == sr.ParameterType.BOOL_ARRAY or \ + source_parameter.get_parameter_type() == sr.ParameterType.INT or \ + source_parameter.get_parameter_type() == sr.ParameterType.INT_ARRAY or \ + source_parameter.get_parameter_type() == sr.ParameterType.DOUBLE or \ + source_parameter.get_parameter_type() == sr.ParameterType.DOUBLE_ARRAY or \ + source_parameter.get_parameter_type() == sr.ParameterType.STRING or \ + source_parameter.get_parameter_type() == sr.ParameterType.STRING_ARRAY or \ + source_parameter.get_parameter_type() == sr.ParameterType.VECTOR or \ + source_parameter.get_parameter_type() == sr.ParameterType.MATRIX: + parameter.set_value(source_parameter.get_value()) + elif source_parameter.get_parameter_type() == sr.ParameterType.STATE: + if source_parameter.get_parameter_state_type() != parameter.get_parameter_state_type(): + raise RuntimeError( + f"Source parameter {source_parameter.get_name()} to be copied does not have the same state type " + f"as destination parameter {parameter.get_name()}") + if source_parameter.get_parameter_state_type() == sr.StateType.CARTESIAN_STATE or \ + source_parameter.get_parameter_state_type() == sr.StateType.CARTESIAN_POSE or \ + source_parameter.get_parameter_state_type() == sr.StateType.JOINT_STATE or \ + source_parameter.get_parameter_state_type() == sr.StateType.JOINT_POSITIONS: + parameter.set_value(source_parameter.get_value()) + else: + raise RuntimeError(f"Could not copy the value from source parameter {source_parameter.get_name()} " + f"into parameter {parameter.get_name()}") + else: + raise RuntimeError(f"Could not copy the value from source parameter {source_parameter.get_name()} " + f"into parameter {parameter.get_name()}") + + +def read_parameter(ros_parameter: Parameter, parameter: Union[None, sr.Parameter] = None) -> sr.Parameter: + def read_new_parameter(ros_param: Parameter) -> sr.Parameter: + if ros_param.type_ == Parameter.Type.BOOL: + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().bool_value, sr.ParameterType.BOOL) + elif ros_param.type_ == Parameter.Type.BOOL_ARRAY: + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().bool_array_value, + sr.ParameterType.BOOL_ARRAY) + elif ros_param.type_ == Parameter.Type.INTEGER: + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().integer_value, sr.ParameterType.INT) + elif ros_param.type_ == Parameter.Type.INTEGER_ARRAY: + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().integer_array_value, + sr.ParameterType.INT_ARRAY) + elif ros_param.type_ == Parameter.Type.DOUBLE: + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().double_value, sr.ParameterType.DOUBLE) + elif ros_param.type_ == Parameter.Type.DOUBLE_ARRAY: + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().double_array_value, + sr.ParameterType.DOUBLE_ARRAY) + elif ros_param.type_ == Parameter.Type.STRING_ARRAY: + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().string_array_value, + sr.ParameterType.STRING_ARRAY) + elif ros_param.type_ == Parameter.Type.STRING: + encoding = "" + try: + encoding = clproto.from_json(ros_param.get_parameter_value().string_value) + except Exception: + if not clproto.is_valid(encoding): + return sr.Parameter(ros_param.name, ros_param.get_parameter_value().string_value, + sr.ParameterType.STRING) + message_type = clproto.check_message_type(encoding) + if message_type == clproto.MessageType.CARTESIAN_STATE_MESSAGE: + return sr.Parameter(ros_param.name, clproto.decode(encoding), sr.ParameterType.STATE, + sr.StateType.CARTESIAN_STATE) + elif message_type == clproto.MessageType.CARTESIAN_POSE_MESSAGE: + return sr.Parameter(ros_param.name, clproto.decode(encoding), sr.ParameterType.STATE, + sr.StateType.CARTESIAN_POSE) + elif message_type == clproto.MessageType.JOINT_STATE_MESSAGE: + return sr.Parameter(ros_param.name, clproto.decode(encoding), sr.ParameterType.STATE, + sr.StateType.JOINT_STATE) + elif message_type == clproto.MessageType.JOINT_POSITIONS_MESSAGE: + return sr.Parameter(ros_param.name, clproto.decode(encoding), sr.ParameterType.STATE, + sr.StateType.JOINT_POSITIONS) + else: + raise RuntimeError(f"Parameter {ros_param.name} has an unsupported encoded message type!") + elif ros_param.type_ == Parameter.Type.BYTE_ARRAY: + raise RuntimeError("Parameter byte arrays are not currently supported.") + else: + raise RuntimeError(f"Parameter {ros_param.name} could not be read!") + + new_parameter = read_new_parameter(ros_parameter) + if parameter is None: + return new_parameter + else: + copy_parameter_value(new_parameter, parameter) + return parameter + + +def read_parameter_const(ros_parameter: Parameter, parameter: sr.Parameter) -> sr.Parameter: + if ros_parameter.name != parameter.get_name(): + raise RuntimeError( + f"The ROS parameter {ros_parameter.name} to be read does not have the same name as the reference parameter {parameter.get_name()}") + new_parameter = read_parameter(ros_parameter) + if new_parameter.get_parameter_type() == parameter.get_parameter_type(): + return new_parameter + elif new_parameter.get_parameter_type() == sr.ParameterType.DOUBLE_ARRAY: + value = new_parameter.get_value() + if parameter.get_parameter_type() == sr.ParameterType.VECTOR: + vector = np.array(value) + return sr.Parameter(parameter.get_name(), vector, sr.ParameterType.VECTOR) + elif parameter.get_parameter_type() == sr.ParameterType.MATRIX: + matrix = parameter.get_value() + if matrix.size != len(value): + raise RuntimeError(f"The ROS parameter {ros_parameter.name} with type double array has size " + f"{len(value)} while the reference parameter matrix {parameter.get_name()} " + f"has size {len(matrix)}") + matrix = np.array(value).reshape(matrix.shape) + return sr.Parameter(parameter.get_name(), matrix, sr.ParameterType.MATRIX) + else: + raise RuntimeError(f"The ROS parameter {ros_parameter.name} with type double array cannot be interpreted " + f"by reference parameter {parameter.get_name()} (type code " + f"{parameter.get_parameter_type()}") + else: + raise RuntimeError(f"Something went wrong while reading parameter {parameter.get_name()}") diff --git a/source/modulo_new_core/test/python_tests/conftest.py b/source/modulo_new_core/test/python_tests/conftest.py index f2f923958..fc83484ff 100644 --- a/source/modulo_new_core/test/python_tests/conftest.py +++ b/source/modulo_new_core/test/python_tests/conftest.py @@ -1,18 +1,45 @@ +import clproto import pytest import rclpy.clock -from state_representation import CartesianState, JointState +import state_representation as sr @pytest.fixture def cart_state(): - return CartesianState().Random("test", "ref") + return sr.CartesianState().Random("test", "ref") @pytest.fixture def joint_state(): - return JointState().Random("robot", 3) + return sr.JointState().Random("robot", 3) @pytest.fixture def clock(): return rclpy.clock.Clock() + + +@pytest.fixture +def parameters(): + return {"bool": [True, sr.ParameterType.BOOL, False], + "bool_array": [[True, False], sr.ParameterType.BOOL_ARRAY, [False]], + "int": [1, sr.ParameterType.INT, 2], + "int_array": [[1, 2], sr.ParameterType.INT_ARRAY, [2]], + "double": [1.0, sr.ParameterType.DOUBLE, 2.0], + "double_array": [[1.0, 2.0], sr.ParameterType.DOUBLE_ARRAY, [2.0]], + "string": ["1", sr.ParameterType.STRING, "2"], + "string_array": [["1", "2"], sr.ParameterType.STRING_ARRAY, ["2"]] + } + + +@pytest.fixture +def state_parameters(): + return {"cartesian_state": [sr.CartesianState().Random("test"), sr.StateType.CARTESIAN_STATE, + clproto.CARTESIAN_STATE_MESSAGE], + "cartesian_pose": [sr.CartesianPose().Random("test"), sr.StateType.CARTESIAN_POSE, + clproto.CARTESIAN_POSE_MESSAGE], + "joint_state": [sr.JointState().Random("test", 3), sr.StateType.JOINT_STATE, + clproto.JOINT_STATE_MESSAGE], + "joint_positions": [sr.JointPositions().Random("test", 3), sr.StateType.JOINT_POSITIONS, + clproto.JOINT_POSITIONS_MESSAGE] + } diff --git a/source/modulo_new_core/test/python_tests/test_translators.py b/source/modulo_new_core/test/python_tests/translators/test_messages.py similarity index 98% rename from source/modulo_new_core/test/python_tests/test_translators.py rename to source/modulo_new_core/test/python_tests/translators/test_messages.py index 9ab280a0a..f693258a1 100644 --- a/source/modulo_new_core/test/python_tests/test_translators.py +++ b/source/modulo_new_core/test/python_tests/translators/test_messages.py @@ -52,7 +52,7 @@ def test_pose(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): msg = geometry_msgs.msg.Pose() modulo_writers.write_msg(msg, cart_state) assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation()) + assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") with pytest.raises(RuntimeError): @@ -64,7 +64,7 @@ def test_pose(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation()) + assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") modulo_readers.read_msg_stamped(new_state, msg_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() @@ -75,7 +75,7 @@ def test_transform(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): msg = geometry_msgs.msg.Transform() modulo_writers.write_msg(msg, cart_state) assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation()) + assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") with pytest.raises(RuntimeError): @@ -88,7 +88,7 @@ def test_transform(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert msg_stamped.child_frame_id == cart_state.get_name() assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation()) + assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") modulo_readers.read_msg_stamped(new_state, msg_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() diff --git a/source/modulo_new_core/test/python_tests/translators/test_parameters.py b/source/modulo_new_core/test/python_tests/translators/test_parameters.py new file mode 100644 index 000000000..c725d2080 --- /dev/null +++ b/source/modulo_new_core/test/python_tests/translators/test_parameters.py @@ -0,0 +1,133 @@ +import clproto +import numpy as np +import pytest +import state_representation as sr +from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter, read_parameter_const +from rclpy import Parameter + + +def assert_np_array_equal(a: np.array, b: np.array, places=3): + try: + np.testing.assert_almost_equal(a, b, decimal=places) + except AssertionError as e: + pytest.fail(f'{e}') + + +def test_parameter_write(parameters): + for name, value_types in parameters.items(): + param = sr.Parameter(name, value_types[0], value_types[1]) + ros_param = write_parameter(param) + assert ros_param.to_parameter_msg() == Parameter(name, value=value_types[0]).to_parameter_msg() + + +def test_parameter_read_write(parameters): + for name, value_types in parameters.items(): + ros_param = Parameter(name, value=value_types[0]) + param = read_parameter(ros_param) + assert ros_param.name == param.get_name() + assert param.get_parameter_type() == value_types[1] + assert param.get_value() == value_types[0] + + new_ros_param = write_parameter(param) + assert ros_param.to_parameter_msg() == new_ros_param.to_parameter_msg() + + +def test_parameter_const_read(parameters): + for name, value_types in parameters.items(): + param = sr.Parameter(name, value_types[0], value_types[1]) + ros_param = Parameter(name, value=value_types[0]) + + new_param = read_parameter_const(ros_param, param) + assert new_param.get_name() == param.get_name() + assert new_param.get_parameter_type() == param.get_parameter_type() + assert new_param.get_value() == param.get_value() + + new_param.set_name("other") + assert new_param.get_name() != param.get_name() + + +def test_parameter_non_const_read(parameters): + for name, value_types in parameters.items(): + param = sr.Parameter(name, value_types[2], value_types[1]) + ros_param = Parameter(name, value=value_types[0]) + + param_ref = param + new_param = read_parameter(ros_param, param) + assert new_param.get_name() == ros_param.name + assert new_param.get_parameter_type() == value_types[1] + assert new_param.get_value() == value_types[0] + + assert param_ref.get_name() == ros_param.name + assert param_ref.get_parameter_type() == value_types[1] + assert param_ref.get_value() == value_types[0] + + +def test_state_parameter_write(state_parameters): + for name, value_types in state_parameters.items(): + param = sr.Parameter(name, value_types[0], sr.ParameterType.STATE, value_types[1]) + ros_param = write_parameter(param) + assert ros_param.name == param.get_name() + state = clproto.decode(clproto.from_json(ros_param.value)) + assert state.get_name() == value_types[0].get_name() + assert_np_array_equal(state.data(), value_types[0].data()) + + +def test_state_parameter_read_write(state_parameters): + for name, value_types in state_parameters.items(): + json = clproto.to_json(clproto.encode(value_types[0], value_types[2])) + ros_param = Parameter(name, value=json) + param = read_parameter(ros_param) + assert param.get_name() == ros_param.name + assert param.get_parameter_type() == sr.ParameterType.STATE + assert param.get_parameter_state_type() == value_types[1] + + assert_np_array_equal(param.get_value().data(), value_types[0].data()) + assert param.get_value().get_name() == value_types[0].get_name() + + new_ros_param = write_parameter(param) + assert ros_param.name == new_ros_param.name + assert ros_param.type_ == new_ros_param.type_ + assert_np_array_equal( + clproto.decode(clproto.from_json(new_ros_param.get_parameter_value().string_value)).data(), + value_types[0].data()) + + +def test_vector_parameter_read_write(): + vec = np.random.rand(3) + param = sr.Parameter("vector", vec, sr.ParameterType.VECTOR) + ros_param = write_parameter(param) + assert ros_param.name == param.get_name() + assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY + ros_vec = ros_param.get_parameter_value().double_array_value + assert vec.tolist() == ros_vec.tolist() + + new_param = read_parameter(ros_param) + assert new_param.get_name() == ros_param.name + assert new_param.get_parameter_type() == sr.ParameterType.DOUBLE_ARRAY + assert new_param.get_value() == ros_vec.tolist() + + new_param = read_parameter_const(ros_param, param) + assert new_param.get_name() == ros_param.name + assert new_param.get_parameter_type() == sr.ParameterType.VECTOR + assert_np_array_equal(new_param.get_value(), vec) + + +def test_matrix_parameter_read_write(): + mat = np.random.rand(3, 4) + param = sr.Parameter("matrix", mat, sr.ParameterType.MATRIX) + ros_param = write_parameter(param) + assert ros_param.name == param.get_name() + assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY + ros_mat = ros_param.get_parameter_value().double_array_value + assert mat.flatten().tolist() == ros_mat.tolist() + + new_param = read_parameter(ros_param) + assert new_param.get_name() == ros_param.name + assert new_param.get_parameter_type() == sr.ParameterType.DOUBLE_ARRAY + assert new_param.get_value() == ros_mat.tolist() + + new_param = read_parameter_const(ros_param, param) + assert new_param.get_name() == ros_param.name + assert new_param.get_parameter_type() == sr.ParameterType.MATRIX + assert_np_array_equal(new_param.get_value(), mat) + assert new_param.get_value().shape == mat.shape From f4f9d505246e993788a04368b1539dfec180da8c Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 3 May 2022 08:17:36 +0200 Subject: [PATCH 26/71] Subscription handler/interface and add_input (#64) * Add SubscriptionInterface and Handler * Small fixes * Implement add_input * Update docstrings * Check if input/output exists before adding to map an/or creating interface * Use insert_or_assign more consistently * Rename test class --- Dockerfile | 3 - .../include/modulo_components/Component.hpp | 2 + .../modulo_components/ComponentInterface.hpp | 156 ++++++++++++++++-- .../modulo_components/LifecycleComponent.hpp | 2 + .../SignalAlreadyExistsException.hpp | 11 ++ .../test/cpp/test_component_interface.cpp | 107 ++++++------ .../test_component_interface_parameters.cpp | 8 +- source/modulo_new_core/CMakeLists.txt | 2 + .../communication/SubscriptionHandler.hpp | 46 ++++++ .../communication/SubscriptionInterface.hpp | 49 ++++++ .../src/communication/SubscriptionHandler.cpp | 79 +++++++++ .../communication/SubscriptionInterface.cpp | 19 +++ .../test_subscription_handler.cpp | 65 ++++++++ 13 files changed, 478 insertions(+), 71 deletions(-) create mode 100644 source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp create mode 100644 source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp create mode 100644 source/modulo_new_core/src/communication/SubscriptionHandler.cpp create mode 100644 source/modulo_new_core/src/communication/SubscriptionInterface.cpp create mode 100644 source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp diff --git a/Dockerfile b/Dockerfile index 1da2173c1..d6924edf7 100644 --- a/Dockerfile +++ b/Dockerfile @@ -1,8 +1,5 @@ ARG BASE_TAG=galactic FROM ghcr.io/aica-technology/ros2-control-libraries:${BASE_TAG} as dependencies - -# upgrade ament_cmake_python -RUN sudo apt update && sudo apt install -y ros-${ROS_DISTRO}-ament-cmake-python && sudo rm -rf /var/lib/apt/lists/* WORKDIR ${HOME}/ros2_ws diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 5fef83f66..b88b0f3bd 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -25,6 +25,7 @@ class Component : public ComponentInterface { * @param signal_name Name of the output signal * @param data Data to transmit on the output signal * @param fixed_topic If true, the topic name of the output signal is fixed + * @param default_topic If set, the default value for the topic name to use */ template void add_output( @@ -34,6 +35,7 @@ class Component : public ComponentInterface { private: using ComponentInterface::create_output; + using ComponentInterface::outputs_; using ComponentInterface::qos_; }; diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 50ca487dc..332ed4b87 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -15,10 +15,12 @@ #include #include #include +#include #include #include #include +#include "modulo_components/exceptions/SignalAlreadyExistsException.hpp" #include "modulo_components/utilities/utilities.hpp" #include "modulo_components/utilities/predicate_variant.hpp" @@ -26,10 +28,9 @@ namespace modulo_components { template class ComponentInterface : public NodeT { - friend class ComponentInterfaceTest; - public: friend class ComponentInterfacePublicInterface; + friend class ComponentInterfaceParameterPublicInterface; /** * @brief Constructor from node options @@ -140,6 +141,34 @@ class ComponentInterface : public NodeT { */ void set_predicate(const std::string& predicate_name, const std::function& predicate_function); + /** + * @brief Add and configure an input signal of the component. + * @tparam DataT Type of the data pointer + * @param signal_name Name of the output signal + * @param data Data to transmit on the output signal + * @param fixed_topic If true, the topic name of the output signal is fixed + * @param default_topic If set, the default value for the topic name to use + */ + template + void add_input( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, + const std::string& default_topic = "" + ); + + /** + * @brief Add and configure an input signal of the component. + * @tparam MsgT The ROS message type of the subscription + * @param signal_name Name of the output signal + * @param callback The callback to use for the subscription + * @param fixed_topic If true, the topic name of the output signal is fixed + * @param default_topic If set, the default value for the topic name to use + */ + template + void add_input( + const std::string& signal_name, const std::function)>& callback, + bool fixed_topic = false, const std::string& default_topic = "" + ); + /** * @brief Configure a transform broadcaster. */ @@ -218,6 +247,7 @@ class ComponentInterface : public NodeT { std::map predicates_; ///< Map of predicates std::map>> predicate_publishers_; ///< Map of predicate publishers + std::map> inputs_; state_representation::ParameterMap parameter_map_; ///< ParameterMap for handling parameters std::shared_ptr @@ -238,12 +268,14 @@ ComponentInterface::ComponentInterface( parameter_cb_handle_ = NodeT::add_on_set_parameters_callback( [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { return this->on_set_parameters_callback(parameters); - }); + } + ); this->add_parameter("period", 1.0, "The time interval in seconds for all periodic callbacks", true); this->step_timer_ = this->create_wall_timer( std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), - [this] { this->step(); }); + [this] { this->step(); } + ); } template @@ -272,13 +304,13 @@ void ComponentInterface::add_variant_predicate( ) { if (this->predicates_.find(name) != this->predicates_.end()) { RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate " << name << " already exists, overwriting."); - this->predicates_.at(name) = predicate; } else { - this->predicates_.insert(std::make_pair(name, predicate)); - this->predicate_publishers_.insert( - std::make_pair( - name, this->template create_publisher(this->generate_predicate_topic(name), 10))); + this->predicate_publishers_.insert_or_assign( + name, this->template create_publisher( + this->generate_predicate_topic(name), 10 + )); } + this->predicates_.insert_or_assign(name, predicate); } template @@ -425,6 +457,104 @@ void ComponentInterface::set_predicate( this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } +template +template +void ComponentInterface::add_input( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, + const std::string& default_topic +) { + using namespace modulo_new_core::communication; + try { + std::string parsed_signal_name = utilities::parse_signal_name(signal_name); + if (this->inputs_.find(parsed_signal_name) != this->inputs_.end()) { + throw exceptions::SignalAlreadyExistsException("Input with name '" + signal_name + "' already exists"); + } + std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; + this->add_parameter( + parsed_signal_name + "_topic", topic_name, "Output topic name of signal '" + parsed_signal_name + "'", + fixed_topic + ); + topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + auto message_pair = make_shared_message_pair(data, this->get_clock()); + std::shared_ptr subscription_interface; + switch (message_pair->get_type()) { + case MessageType::BOOL: { + auto subscription_handler = std::make_shared>(message_pair); + auto subscription = NodeT::template create_subscription( + topic_name, this->qos_, subscription_handler->get_callback()); + subscription_interface = subscription_handler->create_subscription_interface(subscription); + break; + } + case MessageType::FLOAT64: { + auto subscription_handler = std::make_shared>(message_pair); + auto subscription = NodeT::template create_subscription( + topic_name, this->qos_, subscription_handler->get_callback()); + subscription_interface = subscription_handler->create_subscription_interface(subscription); + break; + } + case MessageType::FLOAT64_MULTI_ARRAY: { + auto subscription_handler = + std::make_shared>(message_pair); + auto subscription = NodeT::template create_subscription( + topic_name, this->qos_, subscription_handler->get_callback()); + subscription_interface = subscription_handler->create_subscription_interface(subscription); + break; + } + case MessageType::INT32: { + auto subscription_handler = std::make_shared>(message_pair); + auto subscription = NodeT::template create_subscription( + topic_name, this->qos_, subscription_handler->get_callback()); + subscription_interface = subscription_handler->create_subscription_interface(subscription); + break; + } + case MessageType::STRING: { + auto subscription_handler = std::make_shared>(message_pair); + auto subscription = NodeT::template create_subscription( + topic_name, this->qos_, subscription_handler->get_callback()); + subscription_interface = subscription_handler->create_subscription_interface(subscription); + break; + } + case MessageType::ENCODED_STATE: { + auto subscription_handler = std::make_shared>(message_pair); + auto subscription = NodeT::template create_subscription( + topic_name, this->qos_, subscription_handler->get_callback()); + subscription_interface = subscription_handler->create_subscription_interface(subscription); + break; + } + } + this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what()); + } +} + +template +template +void ComponentInterface::add_input( + const std::string& signal_name, const std::function)>& callback, bool fixed_topic, + const std::string& default_topic +) { + using namespace modulo_new_core::communication; + try { + std::string parsed_signal_name = utilities::parse_signal_name(signal_name); + if (this->inputs_.find(parsed_signal_name) != this->inputs_.end()) { + throw exceptions::SignalAlreadyExistsException("Input with name '" + signal_name + "' already exists"); + } + std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; + this->add_parameter( + parsed_signal_name + "_topic", topic_name, "Output topic name of signal '" + parsed_signal_name + "'", + fixed_topic + ); + topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + auto subscription = NodeT::template create_subscription(topic_name, this->qos_, callback); + auto subscription_interface = + std::make_shared>()->create_subscription_interface(subscription); + this->inputs_.insert_or_assign(parsed_signal_name, subscription_interface); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add input '" << signal_name << "': " << ex.what()); + } +} + template void ComponentInterface::add_tf_broadcaster() { this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); @@ -472,10 +602,12 @@ inline void ComponentInterface::create_output( const std::string& default_topic ) { using namespace modulo_new_core::communication; + if (this->outputs_.find(signal_name) != this->outputs_.end()) { + throw exceptions::SignalAlreadyExistsException("Output with name '" + signal_name + "' already exists"); + } auto message_pair = make_shared_message_pair(data, this->get_clock()); - this->outputs_.insert( - std::make_pair( - signal_name, std::make_shared(this->publisher_type_, message_pair))); + this->outputs_.insert_or_assign( + signal_name, std::make_shared(this->publisher_type_, message_pair)); std::string topic_name = default_topic.empty() ? "~/" + signal_name : default_topic; this->add_parameter( signal_name + "_topic", topic_name, "Output topic name of signal '" + signal_name + "'", fixed_topic diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 4d866031a..1edbd5363 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -27,6 +27,7 @@ class LifecycleComponent : public ComponentInterface void add_output(const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false); @@ -51,6 +52,7 @@ class LifecycleComponent : public ComponentInterface::create_output; + using ComponentInterface::outputs_; using ComponentInterface::qos_; }; diff --git a/source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp b/source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp new file mode 100644 index 000000000..80e698635 --- /dev/null +++ b/source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp @@ -0,0 +1,11 @@ +#pragma once + +#include +#include + +namespace modulo_components::exceptions { +class SignalAlreadyExistsException : public std::runtime_error { +public: + explicit SignalAlreadyExistsException(const std::string& msg) : std::runtime_error(msg) {}; +}; +} \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 98183d5b2..0ee15c269 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -7,6 +7,19 @@ #include "modulo_new_core/EncodedState.hpp" namespace modulo_components { + +class ComponentInterfacePublicInterface : public ComponentInterface { +public: + explicit ComponentInterfacePublicInterface(const rclcpp::NodeOptions& node_options) : + ComponentInterface(node_options, modulo_new_core::communication::PublisherType::PUBLISHER) {} + using ComponentInterface::add_predicate; + using ComponentInterface::get_predicate; + using ComponentInterface::set_predicate; + using ComponentInterface::predicates_; + using ComponentInterface::add_input; + using ComponentInterface::inputs_; +}; + class ComponentInterfaceTest : public ::testing::Test { protected: static void SetUpTestSuite() { @@ -18,83 +31,73 @@ class ComponentInterfaceTest : public ::testing::Test { } void SetUp() override { - component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::PUBLISHER - ); - } - - std::map get_predicate_map() { - return component_->predicates_; + component_ = std::make_shared(rclcpp::NodeOptions()); } - void add_predicate(const std::string& name, bool value) { - component_->add_predicate(name, value); - } - - void add_predicate(const std::string& predicate_name, const std::function& predicate_function) { - component_->add_predicate(predicate_name, predicate_function); - } - - bool get_predicate(const std::string& predicate_name) const { - return component_->get_predicate(predicate_name); - } - - void set_predicate(const std::string& predicate_name, bool predicate_value) { - component_->set_predicate(predicate_name, predicate_value); - } - - void set_predicate(const std::string& predicate_name, const std::function& predicate_function) { - component_->set_predicate(predicate_name, predicate_function); - } - - std::shared_ptr> component_; + std::shared_ptr component_; }; TEST_F(ComponentInterfaceTest, AddBoolPredicate) { - add_predicate("foo", true); - auto predicates = get_predicate_map(); - auto predicate_iterator = predicates.find("foo"); - EXPECT_TRUE(predicate_iterator != predicates.end()); + this->component_->add_predicate("foo", true); + auto predicate_iterator = this->component_->predicates_.find("foo"); + EXPECT_TRUE(predicate_iterator != this->component_->predicates_.end()); auto value = std::get(predicate_iterator->second); EXPECT_TRUE(value); } TEST_F(ComponentInterfaceTest, AddFunctionPredicate) { - add_predicate("bar", [&]() { return false; }); - auto predicates = get_predicate_map(); - auto predicate_iterator = predicates.find("bar"); - EXPECT_TRUE(predicate_iterator != predicates.end()); + this->component_->add_predicate("bar", [&]() { return false; }); + auto predicate_iterator = component_->predicates_.find("bar"); + EXPECT_TRUE(predicate_iterator != component_->predicates_.end()); auto value_callback = std::get>(predicate_iterator->second); EXPECT_FALSE((value_callback)()); } TEST_F(ComponentInterfaceTest, GetPredicateValue) { - add_predicate("foo", true); - EXPECT_TRUE(get_predicate("foo")); - add_predicate("bar", [&]() { return true; }); - EXPECT_TRUE(get_predicate("bar")); + this->component_->add_predicate("foo", true); + EXPECT_TRUE(this->component_->get_predicate("foo")); + this->component_->add_predicate("bar", [&]() { return true; }); + EXPECT_TRUE(this->component_->get_predicate("bar")); // predicate does not exist, expect false - EXPECT_FALSE(get_predicate("test")); + EXPECT_FALSE(this->component_->get_predicate("test")); // error in callback function except false - add_predicate( + this->component_->add_predicate( "error", [&]() { throw std::runtime_error("An error occurred"); return false; } ); - EXPECT_FALSE(get_predicate("error")); + EXPECT_FALSE(this->component_->get_predicate("error")); } TEST_F(ComponentInterfaceTest, SetPredicateValue) { - add_predicate("foo", true); - set_predicate("foo", false); - EXPECT_FALSE(get_predicate("foo")); + this->component_->add_predicate("foo", true); + this->component_->set_predicate("foo", false); + EXPECT_FALSE(this->component_->get_predicate("foo")); // predicate does not exist so setting won't do anything - set_predicate("bar", true); - EXPECT_FALSE(get_predicate("bar")); - add_predicate("bar", true); - set_predicate("bar", [&]() { return false; }); - EXPECT_FALSE(get_predicate("bar")); + this->component_->set_predicate("bar", true); + EXPECT_FALSE(this->component_->get_predicate("bar")); + this->component_->add_predicate("bar", true); + this->component_->set_predicate("bar", [&]() { return false; }); + EXPECT_FALSE(this->component_->get_predicate("bar")); } -} // namespace modulo_components \ No newline at end of file +TEST_F(ComponentInterfaceTest, AddInput) { + auto data = std::make_shared(true); + EXPECT_NO_THROW(this->component_->add_input("_tEsT_#1@3", data, true)); + auto inputs_iterator = component_->inputs_.find("test_13"); + EXPECT_TRUE(inputs_iterator != component_->inputs_.end()); + + EXPECT_NO_THROW(this->component_->template add_input( + "_tEsT_#1@5", [](const std::shared_ptr) {} + )); + inputs_iterator = component_->inputs_.find("test_15"); + EXPECT_TRUE(inputs_iterator != component_->inputs_.end()); + + this->component_->template add_input( + "test_13", [](const std::shared_ptr) {} + ); + EXPECT_EQ(this->component_->inputs_.at("test_13")->get_message_pair()->get_type(), + modulo_new_core::communication::MessageType::BOOL); +} +} \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index 043761905..efea35366 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -10,9 +10,9 @@ namespace modulo_components { -class ComponentInterfacePublicInterface : public ComponentInterface { +class ComponentInterfaceParameterPublicInterface : public ComponentInterface { public: - ComponentInterfacePublicInterface(const rclcpp::NodeOptions& node_options) : + explicit ComponentInterfaceParameterPublicInterface(const rclcpp::NodeOptions& node_options) : ComponentInterface(node_options, modulo_new_core::communication::PublisherType::PUBLISHER) {} using ComponentInterface::add_parameter; using ComponentInterface::get_parameter; @@ -52,7 +52,7 @@ class ComponentInterfaceParameterTest : public ::testing::Test { } void SetUp() override { - component_ = std::make_shared(rclcpp::NodeOptions()); + component_ = std::make_shared(rclcpp::NodeOptions()); param_ = state_representation::make_shared_parameter("test", 1); } @@ -63,7 +63,7 @@ class ComponentInterfaceParameterTest : public ::testing::Test { EXPECT_EQ(component_->parameter_map_.get_parameter_value("test"), value); } - std::shared_ptr component_; + std::shared_ptr component_; std::shared_ptr> param_; }; diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index 742add05e..f755aa6f8 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -30,6 +30,8 @@ ament_auto_add_library(modulo_new_core SHARED ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp + ${PROJECT_SOURCE_DIR}/src/communication/SubscriptionHandler.cpp + ${PROJECT_SOURCE_DIR}/src/communication/SubscriptionInterface.cpp ${PROJECT_SOURCE_DIR}/src/translators/parameter_translators.cpp ${PROJECT_SOURCE_DIR}/src/translators/message_readers.cpp ${PROJECT_SOURCE_DIR}/src/translators/message_writers.cpp) diff --git a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp new file mode 100644 index 000000000..998b90586 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp @@ -0,0 +1,46 @@ +#pragma once + +#include "modulo_new_core/communication/SubscriptionInterface.hpp" + +namespace modulo_new_core::communication { + +template +class SubscriptionHandler : public SubscriptionInterface { +public: + explicit SubscriptionHandler(std::shared_ptr message_pair = nullptr); + + [[nodiscard]] std::shared_ptr> get_subscription() const; + + void set_subscription(const std::shared_ptr>& subscription); + + std::function)> get_callback(); + + std::shared_ptr + create_subscription_interface(const std::shared_ptr>& subscription); + +private: + std::shared_ptr> subscription_; +}; + +template +std::shared_ptr> SubscriptionHandler::get_subscription() const { + return this->subscription_; +} + +template +void SubscriptionHandler::set_subscription(const std::shared_ptr>& subscription) { + if (subscription == nullptr) { + throw exceptions::NullPointerException("Provide a valid pointer"); + } + this->subscription_ = subscription; +} + +template +std::shared_ptr SubscriptionHandler::create_subscription_interface( + const std::shared_ptr>& subscription +) { + this->set_subscription(subscription); + return std::shared_ptr(this->shared_from_this()); +} + +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp new file mode 100644 index 000000000..aeddf8855 --- /dev/null +++ b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp @@ -0,0 +1,49 @@ +#pragma once + +#include +#include "modulo_new_core/communication/MessagePair.hpp" + +namespace modulo_new_core::communication { + +// forward declaration of derived Subscription class +template +class SubscriptionHandler; + +class SubscriptionInterface : public std::enable_shared_from_this { +public: + explicit SubscriptionInterface(std::shared_ptr message_pair = nullptr); + + SubscriptionInterface(const SubscriptionInterface& subscription) = default; + + virtual ~SubscriptionInterface() = default; + + template + std::shared_ptr> get_handler(bool validate_pointer = true); + + [[nodiscard]] std::shared_ptr get_message_pair() const; + + void set_message_pair(const std::shared_ptr& message_pair); + +private: + std::shared_ptr message_pair_; +}; + +template +inline std::shared_ptr> SubscriptionInterface::get_handler(bool validate_pointer) { + std::shared_ptr> subscription_ptr; + try { + subscription_ptr = std::dynamic_pointer_cast>(this->shared_from_this()); + } catch (const std::exception& ex) { + if (validate_pointer) { + throw exceptions::InvalidPointerException("Subscription interface is not managed by a valid pointer"); + } + } + if (subscription_ptr == nullptr && validate_pointer) { + throw exceptions::InvalidPointerCastException( + "Unable to cast subscription interface to a subscription pointer of requested type" + ); + } + return subscription_ptr; +} + +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/SubscriptionHandler.cpp b/source/modulo_new_core/src/communication/SubscriptionHandler.cpp new file mode 100644 index 000000000..4a72b2d69 --- /dev/null +++ b/source/modulo_new_core/src/communication/SubscriptionHandler.cpp @@ -0,0 +1,79 @@ +#include "modulo_new_core/communication/SubscriptionHandler.hpp" + +#include + +namespace modulo_new_core::communication { + +template<> +SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : + SubscriptionInterface(std::move(message_pair)) {} + +template<> +SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : + SubscriptionInterface(std::move(message_pair)) {} +template<> +SubscriptionHandler::SubscriptionHandler( + std::shared_ptr message_pair +) : + SubscriptionInterface(std::move(message_pair)) {} + +template<> +SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : + SubscriptionInterface(std::move(message_pair)) {} + +template<> +SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : + SubscriptionInterface(std::move(message_pair)) {} + +template<> +SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : + SubscriptionInterface(std::move(message_pair)) {} + +template<> +std::function)> +SubscriptionHandler::get_callback() { + return [this](const std::shared_ptr message) { + this->get_message_pair()->template read(*message); + }; +} + +template<> +std::function)> +SubscriptionHandler::get_callback() { + return [this](const std::shared_ptr message) { + this->get_message_pair()->template read(*message); + }; +} + +template<> +std::function)> +SubscriptionHandler::get_callback() { + return [this](const std::shared_ptr message) { + this->get_message_pair()->template read>(*message); + }; +} + +template<> +std::function)> +SubscriptionHandler::get_callback() { + return [this](const std::shared_ptr message) { + this->get_message_pair()->template read(*message); + }; +} + +template<> +std::function)> +SubscriptionHandler::get_callback() { + return [this](const std::shared_ptr message) { + this->get_message_pair()->template read(*message); + }; +} + +template<> +std::function)> SubscriptionHandler::get_callback() { + return [this](const std::shared_ptr message) { + this->get_message_pair()->template read(*message); + }; +} + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/src/communication/SubscriptionInterface.cpp b/source/modulo_new_core/src/communication/SubscriptionInterface.cpp new file mode 100644 index 000000000..44266c91c --- /dev/null +++ b/source/modulo_new_core/src/communication/SubscriptionInterface.cpp @@ -0,0 +1,19 @@ +#include "modulo_new_core/communication/SubscriptionInterface.hpp" + +namespace modulo_new_core::communication { + +SubscriptionInterface::SubscriptionInterface(std::shared_ptr message_pair) : + message_pair_(std::move(message_pair)) {} + +std::shared_ptr SubscriptionInterface::get_message_pair() const { + return this->message_pair_; +} + +void SubscriptionInterface::set_message_pair(const std::shared_ptr& message_pair) { + if (message_pair == nullptr) { + throw exceptions::NullPointerException("Provide a valid pointer"); + } + this->message_pair_ = message_pair; +} + +}// namespace modulo_new_core::communication \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp new file mode 100644 index 000000000..4d203ab9e --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp @@ -0,0 +1,65 @@ +#include + +#include + +#include "modulo_new_core/communication/SubscriptionHandler.hpp" +#include "modulo_new_core/exceptions/NullPointerException.hpp" + +using namespace modulo_new_core::communication; + +template +static void test_subscription_interface(const std::shared_ptr& node, const DataT& value) { + // create message pair + auto data = std::make_shared(value); + auto msg_pair = std::make_shared>(data, node->get_clock()); + + // create subscription handler + auto subscription_handler = std::make_shared>(msg_pair); + auto subscription = node->template create_subscription( + "topic", 10, subscription_handler->get_callback()); + + // use in subscription interface + auto subscription_interface = subscription_handler->create_subscription_interface(subscription); +} + +class SubscriptionTest : public ::testing::Test { +public: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + +protected: + void SetUp() { + node = std::make_shared("test_node"); + } + + std::shared_ptr node; +}; + +TEST_F(SubscriptionTest, BasicTypes) { + test_subscription_interface(node, true); + test_subscription_interface(node, 0.1); + test_subscription_interface>(node, {0.1, 0.2, 0.3}); + test_subscription_interface(node, 1); + test_subscription_interface(node, "this"); +} + +TEST_F(SubscriptionTest, CartesianState) { + // create message pair + auto data = + std::make_shared(state_representation::CartesianState::Random("test")); + auto msg_pair = std::make_shared>( + data, node->get_clock()); + + // create subscription handler + auto subscription_handler = std::make_shared>(msg_pair); + auto subscription = node->create_subscription( + "topic", 10, subscription_handler->get_callback()); + + // use in subscription interface + auto subscription_interface = subscription_handler->create_subscription_interface(subscription); +} \ No newline at end of file From 915b4f97f911bea85984a03cda046afe6a086ecd Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 13 May 2022 15:07:54 +0200 Subject: [PATCH 27/71] Implement execution thread for component (#68) * Implement execution thread for component * Add docstrings * Remove thread joining to avoid blocking --- .../include/modulo_components/Component.hpp | 36 +++++++++++++-- .../modulo_components/ComponentInterface.hpp | 16 ++++++- source/modulo_components/src/Component.cpp | 46 +++++++++++++++++-- 3 files changed, 90 insertions(+), 8 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index b88b0f3bd..071c0e6b5 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include "modulo_components/ComponentInterface.hpp" @@ -13,12 +14,23 @@ class Component : public ComponentInterface { friend class ComponentPublicInterface; /** - * @brief Constructor from node options + * @brief Constructor from node options. * @param node_options node options as used in ROS2 Node */ - explicit Component(const rclcpp::NodeOptions& node_options); + explicit Component(const rclcpp::NodeOptions& node_options, bool start_thread = true); protected: + /** + * @brief Start the execution thread. + */ + void start_thread(); + + /** + * @brief Execute the component logic. To be redefined in derived classes. + * @return True, if the execution was successful, false otherwise + */ + virtual bool execute(); + /** * @brief Add and configure an output signal of the component. * @tparam DataT Type of the data pointer @@ -34,9 +46,24 @@ class Component : public ComponentInterface { ); private: + /** + * @brief Run the execution function in a try catch block and + * set the predicates according to the outcome of the execution. + */ + void run(); + + /** + * @brief Put the component in error state by setting the + * 'in_error_state' predicate to true. + */ + void raise_error() override; + using ComponentInterface::create_output; using ComponentInterface::outputs_; using ComponentInterface::qos_; + + std::thread run_thread_; + bool started_; }; template @@ -103,8 +130,9 @@ void Component::add_output( } } } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Failed to add output '" << signal_name << "': " << ex.what()); } } -} \ No newline at end of file +}// namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 332ed4b87..03f0a41ca 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -33,13 +33,18 @@ class ComponentInterface : public NodeT { friend class ComponentInterfaceParameterPublicInterface; /** - * @brief Constructor from node options + * @brief Constructor from node options. * @param node_options node options as used in ROS2 Node */ explicit ComponentInterface( const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type ); + /** + * @brief Virtual default destructor. + */ + virtual ~ComponentInterface() = default; + protected: /** * @brief Add a parameter. @@ -220,6 +225,12 @@ class ComponentInterface : public NodeT { [[nodiscard]] state_representation::CartesianPose lookup_transform(const std::string& frame_name, const std::string& reference_frame_name = "world") const; + /** + * @brief Raise an error, or set the component into error state. + * To be redefined in derived classes. + */ + virtual void raise_error(); + std::map> outputs_; ///< Map of outputs @@ -614,4 +625,7 @@ inline void ComponentInterface::create_output( ); } +template +inline void ComponentInterface::raise_error() {} + }// namespace modulo_components diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 0f69cbfe3..e3ce4c108 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -4,7 +4,47 @@ using namespace modulo_new_core::communication; namespace modulo_components { -Component::Component(const rclcpp::NodeOptions& node_options) : - ComponentInterface(node_options, PublisherType::PUBLISHER) {} +Component::Component(const rclcpp::NodeOptions& node_options, bool start_thread) : + ComponentInterface(node_options, PublisherType::PUBLISHER), started_(false) { + this->add_predicate("in_error_state", false); + this->add_predicate("is_finished", false); -} \ No newline at end of file + if (start_thread) { + this->start_thread(); + } +} + +void Component::start_thread() { + if (this->started_) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Run thread for component '" << this->get_name() << "has already been started"); + return; + } + this->started_ = true; + this->run_thread_ = std::thread([this]() { this->run(); }); +} + +void Component::run() { + try { + if (!this->execute()) { + this->raise_error(); + return; + } + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Failed to run component '" << this->get_name() << "': " << ex.what()); + this->raise_error(); + return; + } + this->set_predicate("is_finished", true); +} + +bool Component::execute() { + return true; +} + +void Component::raise_error() { + this->set_predicate("in_error_state", true); +} + +}// namespace modulo_components \ No newline at end of file From 7cb19f9a07835676060a4d610f1b14a449faf4ef Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 17 May 2022 14:26:03 +0200 Subject: [PATCH 28/71] Improve test coverage in modulo new core (#66) * Fix existing tests * Add communication test with pub/sub nodes * Use futures for spinning * Correctly read pointer of state * Finish modulo_new_core tests * Remove unnecessary include * Remove unused test method --- .../translators/message_readers.hpp | 166 +++++++++++++++++- .../communication/test_communication.cpp | 130 ++++++++++++++ .../communication/test_message_pair.cpp | 4 +- .../communication/test_publisher_handler.cpp | 2 +- .../test_subscription_handler.cpp | 2 +- .../parameters/test_parameter_translators.cpp | 2 +- 6 files changed, 297 insertions(+), 9 deletions(-) create mode 100644 source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp index cadeab27e..b36040f58 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp @@ -14,8 +14,9 @@ #include #include -#include -#include +#include +#include +#include #include "modulo_new_core/EncodedState.hpp" @@ -105,7 +106,7 @@ void read_msg(state_representation::JointState& state, const sensor_msgs::msg::J * @param state The Parameter to populate * @param msg The ROS msg to read from */ -template +template void read_msg(state_representation::Parameter& state, const U& msg) { state.set_value(msg.data); } @@ -151,9 +152,166 @@ void read_msg(std::string& state, const std_msgs::msg::String& msg); * @param state The state to populate * @param msg The ROS msg to read from */ -template +template inline void read_msg(T& state, const EncodedState& msg) { std::string tmp(msg.data.begin(), msg.data.end()); state = clproto::decode(tmp); } + +template<> +inline void read_msg(std::shared_ptr& state, const EncodedState& msg) { + using namespace state_representation; + std::string tmp(msg.data.begin(), msg.data.end()); + auto new_state = clproto::decode>(tmp); + switch (new_state->get_type()) { + case StateType::STATE: + *state = *new_state; + break; + case StateType::SPATIAL_STATE: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::CARTESIAN_STATE: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::CARTESIAN_POSE: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::CARTESIAN_TWIST: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::CARTESIAN_ACCELERATION: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::CARTESIAN_WRENCH: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::JACOBIAN: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::JOINT_STATE: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::JOINT_POSITIONS: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::JOINT_VELOCITIES: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::JOINT_ACCELERATIONS: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::JOINT_TORQUES: { + auto derived_state = std::dynamic_pointer_cast(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast(new_state); + state = derived_state; + break; + } + case StateType::PARAMETER: { + auto param_ptr = std::dynamic_pointer_cast(new_state); + switch (param_ptr->get_parameter_type()) { + case ParameterType::BOOL: { + auto derived_state = std::dynamic_pointer_cast>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>(new_state); + state = derived_state; + break; + } + case ParameterType::BOOL_ARRAY: { + auto derived_state = std::dynamic_pointer_cast>>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>>(new_state); + state = derived_state; + break; + } + case ParameterType::INT: { + auto derived_state = std::dynamic_pointer_cast>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>(new_state); + state = derived_state; + break; + } + case ParameterType::INT_ARRAY: { + auto derived_state = std::dynamic_pointer_cast>>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>>(new_state); + state = derived_state; + break; + } + case ParameterType::DOUBLE: { + auto derived_state = std::dynamic_pointer_cast>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>(new_state); + state = derived_state; + break; + } + case ParameterType::DOUBLE_ARRAY: { + auto derived_state = std::dynamic_pointer_cast>>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>>(new_state); + state = derived_state; + break; + } + case ParameterType::STRING: { + auto derived_state = std::dynamic_pointer_cast>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>(new_state); + state = derived_state; + break; + } + case ParameterType::STRING_ARRAY: { + auto + derived_state = std::dynamic_pointer_cast>>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>>(new_state); + state = derived_state; + break; + } + case ParameterType::VECTOR: { + auto derived_state = std::dynamic_pointer_cast>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>(new_state); + state = derived_state; + break; + } + case ParameterType::MATRIX: { + auto derived_state = std::dynamic_pointer_cast>(state->shared_from_this()); + *derived_state = *std::dynamic_pointer_cast>(new_state); + state = derived_state; + break; + } + default: + throw std::invalid_argument( + "The ParameterType contained by parameter " + param_ptr->get_name() + " is unsupported." + ); + } + break; + } + default: + throw std::invalid_argument("The StateType contained by state " + new_state->get_name() + " is unsupported."); + } +} }// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp new file mode 100644 index 000000000..920a0061c --- /dev/null +++ b/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp @@ -0,0 +1,130 @@ +#include + +#include "rclcpp/rclcpp.hpp" + +#include "modulo_new_core/communication/MessagePair.hpp" +#include "modulo_new_core/communication/PublisherHandler.hpp" +#include "modulo_new_core/communication/SubscriptionHandler.hpp" + +using namespace std::chrono_literals; +using namespace modulo_new_core::communication; + +template +class MinimalPublisher : public rclcpp::Node { +public: + MinimalPublisher(const std::string& topic_name, std::shared_ptr message_pair) : + Node("minimal_publisher") { + auto publisher = this->create_publisher(topic_name, 10); + this->publisher_interface_ = std::make_shared, MsgT>>( + PublisherType::PUBLISHER, publisher + )->create_publisher_interface(message_pair); + timer_ = this->create_wall_timer(10ms, [this]() { this->publisher_interface_->publish(); }); + } + +private: + rclcpp::TimerBase::SharedPtr timer_; + std::shared_ptr publisher_interface_; +}; + +template +class MinimalSubscriber : public rclcpp::Node { +public: + MinimalSubscriber(const std::string& topic_name, std::shared_ptr message_pair) : + Node("minimal_subscriber") { + this->received_future = this->received_.get_future(); + this->subscription_interface_ = std::make_shared>(message_pair); + auto subscription = this->create_subscription( + topic_name, 10, [this](const std::shared_ptr msg) { + this->subscription_interface_->template get_handler()->get_callback()(msg); + this->received_.set_value(); + } + ); + this->subscription_interface_ = + this->subscription_interface_->template get_handler()->create_subscription_interface(subscription); + } + + std::shared_future received_future; + +private: + std::promise received_; + std::shared_ptr subscription_interface_; +}; + +class CommunicationTest : public ::testing::Test { +protected: + void SetUp() override { + rclcpp::init(0, nullptr); + + exec_ = std::make_shared(); + clock_ = std::make_shared(); + } + + void TearDown() override { + rclcpp::shutdown(); + } + + template + void add_nodes( + const std::string& topic_name, const std::shared_ptr& pub_message, + const std::shared_ptr& sub_message + ) { + pub_node_ = std::make_shared>(topic_name, pub_message); + sub_node_ = std::make_shared>(topic_name, sub_message); + exec_->add_node(pub_node_); + exec_->add_node(sub_node_); + } + + void clear_nodes() { + pub_node_.reset(); + sub_node_.reset(); + } + + template + void communicate(const DataT& initial_value, const DataT& new_value) { + auto pub_data = std::make_shared(new_value); + auto pub_message = make_shared_message_pair(pub_data, this->clock_); + auto sub_data = std::make_shared(initial_value); + auto sub_message = make_shared_message_pair(sub_data, this->clock_); + this->add_nodes("/test_topic", pub_message, sub_message); + + this->exec_->template spin_until_future_complete( + std::dynamic_pointer_cast>(this->sub_node_)->received_future, 500ms + ); + + EXPECT_EQ(*pub_data, *sub_data); + this->clear_nodes(); + } + + std::shared_ptr exec_; + std::shared_ptr pub_node_; + std::shared_ptr sub_node_; + std::shared_ptr clock_; +}; + +TEST_F(CommunicationTest, BasicTypes) { + this->communicate(false, true); + this->communicate(1.0, 2.0); + this->communicate>({1.0, 2.0}, {3.0, 4.0}); + this->communicate(1, 2); + this->communicate("this", "that"); +} + +TEST_F(CommunicationTest, EncodedState) { + using namespace state_representation; + auto pub_state = std::make_shared(CartesianState::Random("this", "world")); + std::shared_ptr pub_data = pub_state; + auto pub_message = make_shared_message_pair(pub_data, this->clock_); + auto sub_state = std::make_shared(CartesianState::Identity("that", "base")); + std::shared_ptr sub_data = sub_state; + auto sub_message = make_shared_message_pair(sub_data, this->clock_); + this->add_nodes("/test_topic", pub_message, sub_message); + this->exec_->template spin_until_future_complete( + std::dynamic_pointer_cast>(this->sub_node_)->received_future, + 500ms + ); + + EXPECT_EQ(pub_state->get_name(), sub_state->get_name()); + EXPECT_EQ(pub_state->get_reference_frame(), sub_state->get_reference_frame()); + EXPECT_TRUE(std::dynamic_pointer_cast(pub_data)->data().isApprox( + std::dynamic_pointer_cast(sub_data)->data())); +} diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp index a627563de..ee3725a2f 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -48,7 +48,7 @@ TEST_F(MessagePairTest, BasicTypes) { test_message_interface("this", "that", clock_); } -TEST_F(MessagePairTest, TestCartesianState) { +TEST_F(MessagePairTest, EncodedState) { auto initial_value = state_representation::CartesianState::Random("test"); auto data = state_representation::make_shared_state(initial_value); auto msg_pair = @@ -73,7 +73,7 @@ TEST_F(MessagePairTest, TestCartesianState) { msg_pair->set_data(data); msg = modulo_new_core::EncodedState(); modulo_new_core::translators::write_msg(msg, data, clock_->now()); - msg_pair_interface->read(msg); + msg_pair_interface->read(msg); EXPECT_TRUE(initial_value.data().isApprox( std::dynamic_pointer_cast(msg_pair->get_data())->data())); } diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp index 8932c4dd7..0a4ce6a77 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp @@ -55,7 +55,7 @@ TEST_F(PublisherTest, BasicTypes) { test_publisher_interface(node, "this"); } -TEST_F(PublisherTest, CartesianState) { +TEST_F(PublisherTest, EncodedState) { // create message pair auto data = std::make_shared(state_representation::CartesianState::Random("test")); diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp index 4d203ab9e..f03bb80e5 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp @@ -48,7 +48,7 @@ TEST_F(SubscriptionTest, BasicTypes) { test_subscription_interface(node, "this"); } -TEST_F(SubscriptionTest, CartesianState) { +TEST_F(SubscriptionTest, EncodedState) { // create message pair auto data = std::make_shared(state_representation::CartesianState::Random("test")); diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp index a49c64d51..46c6989ec 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp @@ -33,7 +33,7 @@ static std::tuple< std::make_tuple(1.0, state_representation::ParameterType::DOUBLE), }, { std::make_tuple( - std::vector({true, false, true}), state_representation::ParameterType::DOUBLE_ARRAY), + std::vector({1.0, 2.0, 3.0}), state_representation::ParameterType::DOUBLE_ARRAY), }, { std::make_tuple("test", state_representation::ParameterType::STRING), }, { From dcba27ef86586cc020e404494c759b334c617490 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 19 May 2022 16:39:15 +0200 Subject: [PATCH 29/71] Add option to add deamon and update step function (#67) * Inline functions * Move generate predicate topic to utilities * Option to add daemon * Publish publishers, predicates and evaluate daemons on step * Clean up file and logging * Fix docstring and use qos for predicate publishers * Use structured bindings in for loops * Add docstrings * Add docstrings * Make step function protected virtual and step helper methods protected * Override step function and make it private * Remove log line --- .../include/modulo_components/Component.hpp | 11 +- .../modulo_components/ComponentInterface.hpp | 195 ++++++++++++------ .../modulo_components/LifecycleComponent.hpp | 12 +- .../modulo_components/utilities/utilities.hpp | 9 +- source/modulo_components/src/Component.cpp | 6 + .../src/LifecycleComponent.cpp | 8 + 6 files changed, 172 insertions(+), 69 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 071c0e6b5..be7aa167c 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -46,6 +46,12 @@ class Component : public ComponentInterface { ); private: + /** + * @brief Step function that is called periodically and publishes predicates, + * outputs, and evaluates daemon callbacks. + */ + void step() override; + /** * @brief Run the execution function in a try catch block and * set the predicates according to the outcome of the execution. @@ -61,13 +67,16 @@ class Component : public ComponentInterface { using ComponentInterface::create_output; using ComponentInterface::outputs_; using ComponentInterface::qos_; + using ComponentInterface::publish_predicates; + using ComponentInterface::publish_outputs; + using ComponentInterface::evaluate_daemon_callbacks; std::thread run_thread_; bool started_; }; template -void Component::add_output( +inline void Component::add_output( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, const std::string& default_topic ) { diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 03f0a41ca..4491608e2 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -46,6 +46,11 @@ class ComponentInterface : public NodeT { virtual ~ComponentInterface() = default; protected: + /** + * @brief Step function that is called periodically. + */ + virtual void step(); + /** * @brief Add a parameter. * @details This method stores a pointer reference to an existing Parameter object in the local parameter map @@ -149,9 +154,9 @@ class ComponentInterface : public NodeT { /** * @brief Add and configure an input signal of the component. * @tparam DataT Type of the data pointer - * @param signal_name Name of the output signal - * @param data Data to transmit on the output signal - * @param fixed_topic If true, the topic name of the output signal is fixed + * @param signal_name Name of the input signal + * @param data Data to receive on the input signal + * @param fixed_topic If true, the topic name of the input signal is fixed * @param default_topic If set, the default value for the topic name to use */ template @@ -163,9 +168,9 @@ class ComponentInterface : public NodeT { /** * @brief Add and configure an input signal of the component. * @tparam MsgT The ROS message type of the subscription - * @param signal_name Name of the output signal + * @param signal_name Name of the input signal * @param callback The callback to use for the subscription - * @param fixed_topic If true, the topic name of the output signal is fixed + * @param fixed_topic If true, the topic name of the input signal is fixed * @param default_topic If set, the default value for the topic name to use */ template @@ -174,6 +179,13 @@ class ComponentInterface : public NodeT { bool fixed_topic = false, const std::string& default_topic = "" ); + /** + * @brief Function to daemonize the callback function given in input. + * @param name The name of the daemon + * @param callback The callback of the daemon + */ + void add_daemon(const std::string& name, const std::function& callback); + /** * @brief Configure a transform broadcaster. */ @@ -225,6 +237,21 @@ class ComponentInterface : public NodeT { [[nodiscard]] state_representation::CartesianPose lookup_transform(const std::string& frame_name, const std::string& reference_frame_name = "world") const; + /** + * @brief Helper function to publish all predicates. + */ + void publish_predicates(); + + /** + * @brief Helper function to publish all output signals. + */ + void publish_outputs(); + + /** + * @brief Helper function to evaluate all daemon callbacks. + */ + void evaluate_daemon_callbacks(); + /** * @brief Raise an error, or set the component into error state. * To be redefined in derived classes. @@ -244,14 +271,20 @@ class ComponentInterface : public NodeT { */ rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector& parameters); - [[nodiscard]] std::string generate_predicate_topic(const std::string& predicate_name) const; - + /** + * @brief Add a predicate to the map of predicates. + * @param name The name of the predicate + * @param predicate The predicate variant + */ void add_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); + /** + * @brief Set the predicate given as parameter, if the predicate is not found does not do anything. + * @param name The name of the predicate + * @param predicate The predicate variant + */ void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); - void step(); - modulo_new_core::communication::PublisherType publisher_type_; ///< Type of the output publishers (one of PUBLISHER, LIFECYCLE_PUBLISHER) @@ -260,6 +293,8 @@ class ComponentInterface : public NodeT { predicate_publishers_; ///< Map of predicate publishers std::map> inputs_; + std::map> daemon_callbacks_; ///< Map of daemon callbacks + state_representation::ParameterMap parameter_map_; ///< ParameterMap for handling parameters std::shared_ptr parameter_cb_handle_; ///< ROS callback function handle for setting parameters @@ -290,43 +325,11 @@ ComponentInterface::ComponentInterface( } template -void ComponentInterface::step() { - for (const auto& predicate: this->predicates_) { - std_msgs::msg::Bool msg; - msg.data = this->get_predicate(predicate.first); - auto predicate_iterator = this->predicate_publishers_.find(predicate.first); - if (predicate_iterator == this->predicate_publishers_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, - "No publisher for predicate " << predicate.first << " found."); - return; - } - predicate_publishers_.at(predicate.first)->publish(msg); - } -} - -template -std::string ComponentInterface::generate_predicate_topic(const std::string& predicate_name) const { - return "/predicates/" + std::string(this->get_name()) + "/" + predicate_name; -} - -template -void ComponentInterface::add_variant_predicate( - const std::string& name, const utilities::PredicateVariant& predicate -) { - if (this->predicates_.find(name) != this->predicates_.end()) { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate " << name << " already exists, overwriting."); - } else { - this->predicate_publishers_.insert_or_assign( - name, this->template create_publisher( - this->generate_predicate_topic(name), 10 - )); - } - this->predicates_.insert_or_assign(name, predicate); -} +inline void ComponentInterface::step() {} template template -void ComponentInterface::add_parameter( +inline void ComponentInterface::add_parameter( const std::string& name, const T& value, const std::string& description, bool read_only ) { this->add_parameter(state_representation::make_shared_parameter(name, value), description, read_only); @@ -334,12 +337,12 @@ void ComponentInterface::add_parameter( template template -T ComponentInterface::get_parameter_value(const std::string& name) const { +inline T ComponentInterface::get_parameter_value(const std::string& name) const { return this->parameter_map_.template get_parameter_value(name); } template -void ComponentInterface::add_parameter( +inline void ComponentInterface::add_parameter( const std::shared_ptr& parameter, const std::string& description, bool read_only ) { @@ -356,14 +359,14 @@ void ComponentInterface::add_parameter( } template -std::shared_ptr +inline std::shared_ptr ComponentInterface::get_parameter(const std::string& name) const { return this->parameter_map_.get_parameter(name); } template template -void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { +inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter( modulo_new_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); if (!result.successful) { @@ -372,14 +375,14 @@ void ComponentInterface::set_parameter_value(const std::string& name, con } template -bool ComponentInterface::validate_parameter( +inline bool ComponentInterface::validate_parameter( const std::shared_ptr& ) { return true; } template -rcl_interfaces::msg::SetParametersResult +inline rcl_interfaces::msg::SetParametersResult ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { rcl_interfaces::msg::SetParametersResult result; result.successful = true; @@ -406,19 +409,34 @@ ComponentInterface::on_set_parameters_callback(const std::vector -void ComponentInterface::add_predicate(const std::string& name, bool predicate) { +inline void ComponentInterface::add_predicate(const std::string& name, bool predicate) { this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } template -void ComponentInterface::add_predicate( +inline void ComponentInterface::add_predicate( const std::string& name, const std::function& predicate ) { this->add_variant_predicate(name, utilities::PredicateVariant(predicate)); } template -bool ComponentInterface::get_predicate(const std::string& predicate_name) { +inline void ComponentInterface::add_variant_predicate( + const std::string& name, const utilities::PredicateVariant& predicate +) { + if (this->predicates_.find(name) != this->predicates_.end()) { + RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate " << name << " already exists, overwriting."); + } else { + this->predicate_publishers_.insert_or_assign( + name, this->template create_publisher( + utilities::generate_predicate_topic(this->get_name(), name), this->qos_ + )); + } + this->predicates_.insert_or_assign(name, predicate); +} + +template +inline bool ComponentInterface::get_predicate(const std::string& predicate_name) { auto predicate_iterator = this->predicates_.find(predicate_name); // if there is no predicate with that name simply return false with an error message if (predicate_iterator == this->predicates_.end()) { @@ -437,19 +455,19 @@ bool ComponentInterface::get_predicate(const std::string& predicate_name) try { value = (callback_function)(); } catch (const std::exception& e) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Error while evaluating the callback function: " << e.what()); } return value; } template -void ComponentInterface::set_variant_predicate( +inline void ComponentInterface::set_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate ) { auto predicate_iterator = this->predicates_.find(name); if (predicate_iterator == this->predicates_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Cannot set predicate " << name << " with a new value because it does not exist."); return; } @@ -457,12 +475,12 @@ void ComponentInterface::set_variant_predicate( } template -void ComponentInterface::set_predicate(const std::string& name, bool predicate) { +inline void ComponentInterface::set_predicate(const std::string& name, bool predicate) { this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } template -void ComponentInterface::set_predicate( +inline void ComponentInterface::set_predicate( const std::string& name, const std::function& predicate ) { this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); @@ -470,7 +488,7 @@ void ComponentInterface::set_predicate( template template -void ComponentInterface::add_input( +inline void ComponentInterface::add_input( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, const std::string& default_topic ) { @@ -541,7 +559,7 @@ void ComponentInterface::add_input( template template -void ComponentInterface::add_input( +inline void ComponentInterface::add_input( const std::string& signal_name, const std::function)>& callback, bool fixed_topic, const std::string& default_topic ) { @@ -567,21 +585,30 @@ void ComponentInterface::add_input( } template -void ComponentInterface::add_tf_broadcaster() { +inline void ComponentInterface::add_daemon(const std::string& name, const std::function& callback) { + if (this->daemon_callbacks_.find(name) != this->daemon_callbacks_.end()) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Daemon callback " << name << " already exists, overwriting."); + } + this->daemon_callbacks_.template insert_or_assign(name, callback); +} + +template +inline void ComponentInterface::add_tf_broadcaster() { this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); } template -void ComponentInterface::add_tf_listener() { +inline void ComponentInterface::add_tf_listener() { this->tf_buffer_ = std::make_shared(this->get_clock()); this->tf_listener_ = std::make_shared(*this->tf_buffer_); } template -void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { +inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { // TODO: throw here? if (this->tf_broadcaster_ == nullptr) { - RCLCPP_FATAL(this->get_logger(), "No tf broadcaster"); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock, 1000, "No tf broadcaster"); } geometry_msgs::msg::TransformStamped tf_message; modulo_new_core::translators::write_msg(tf_message, transform, this->get_clock()->now()); @@ -589,12 +616,12 @@ void ComponentInterface::send_transform(const state_representation::Carte } template -state_representation::CartesianPose ComponentInterface::lookup_transform( +inline state_representation::CartesianPose ComponentInterface::lookup_transform( const std::string& frame_name, const std::string& reference_frame_name ) const { // TODO: throw here? if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { - RCLCPP_FATAL(this->get_logger(), "No tf buffer / listener"); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock, 1000, "No tf buffer / listener"); } geometry_msgs::msg::TransformStamped transform; state_representation::CartesianPose result(frame_name, reference_frame_name); @@ -606,6 +633,44 @@ state_representation::CartesianPose ComponentInterface::lookup_transform( return result; } +template +inline void ComponentInterface::publish_predicates() { + for (const auto& predicate: this->predicates_) { + std_msgs::msg::Bool msg; + msg.data = this->get_predicate(predicate.first); + if (this->predicate_publishers_.find(predicate.first) == this->predicate_publishers_.end()) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "No publisher for predicate " << predicate.first << " found."); + return; + } + predicate_publishers_.at(predicate.first)->publish(msg); + } +} + +template +inline void ComponentInterface::publish_outputs() { + for (const auto& [signal, publisher]: this->outputs_) { + try { + publisher->publish(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Could not publish output " << signal << ": " << ex.what()); + } + } +} + +template +inline void ComponentInterface::evaluate_daemon_callbacks() { + for (const auto& [daemon, callback]: this->daemon_callbacks_) { + try { + callback(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Could not evaluate daemon callback " << daemon << ": " << ex.what()); + } + } +} + template template inline void ComponentInterface::create_output( diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 1edbd5363..f62d1b268 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -33,6 +33,13 @@ class LifecycleComponent : public ComponentInterface& data, bool fixed_topic = false); private: + /** + * @brief Step function that is called periodically and publishes predicates, + * outputs, evaluates daemon callbacks, and calls the on_step function redefined + * in the derived components. + */ + void step() override; + /** * @brief Configure all outputs. * @return True configuration was successful @@ -54,10 +61,13 @@ class LifecycleComponent : public ComponentInterface::create_output; using ComponentInterface::outputs_; using ComponentInterface::qos_; + using ComponentInterface::publish_predicates; + using ComponentInterface::publish_outputs; + using ComponentInterface::evaluate_daemon_callbacks; }; template -void +inline void LifecycleComponent::add_output(const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic) { try { std::string parsed_signal_name = utilities::parse_signal_name(signal_name); diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index 067c82511..66b46af56 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/source/modulo_components/include/modulo_components/utilities/utilities.hpp @@ -13,8 +13,9 @@ namespace modulo_components::utilities { * @param result the default argument value that is overwritten by reference if the given pattern is found * @return the value of the resultant string */ -static std::string parse_string_argument(const std::vector& args, const std::string& pattern, std::string& result) { - for (const auto& arg : args) { +static std::string +parse_string_argument(const std::vector& args, const std::string& pattern, std::string& result) { + for (const auto& arg: args) { std::string::size_type index = arg.find(pattern); if (index != std::string::npos) { result = arg; @@ -56,4 +57,8 @@ static std::string parse_signal_name(const std::string& signal_name) { return output; } +static std::string generate_predicate_topic(const std::string& component_name, const std::string& predicate_name) { + return "/predicates/" + component_name + "/" + predicate_name; +} + }// namespace modulo_components::utilities \ No newline at end of file diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index e3ce4c108..30c28bb7e 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -14,6 +14,12 @@ Component::Component(const rclcpp::NodeOptions& node_options, bool start_thread) } } +void Component::step() { + this->publish_predicates(); + this->publish_outputs(); + this->evaluate_daemon_callbacks(); +} + void Component::start_thread() { if (this->started_) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 30ea9d23e..6f04ee88e 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -7,6 +7,14 @@ namespace modulo_components { LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options) : ComponentInterface(node_options, PublisherType::LIFECYCLE_PUBLISHER) {} +void LifecycleComponent::step() { + // TODO do this only if active and add on_step +// this->publish_predicates(); +// this->publish_outputs(); +// this->evaluate_daemon_callbacks(); +// this->on_step(); +} + bool LifecycleComponent::configure_outputs() { bool success = true; for (auto& [name, interface]: this->outputs_) { From 48f3de7f8365e3d4b22aa0e75d618c0e24502d17 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 19 May 2022 16:39:37 +0200 Subject: [PATCH 30/71] Clean up python message translators (#69) * Rename message translators * Add docstrings and clean up message translators * Fix docstrings in cpp message translators * Remove double underscores from helper methods --- .../translators/message_readers.hpp | 32 ++--- .../translators/message_writers.hpp | 34 ++--- .../translators/message_readers.py | 102 +++++++++++++++ .../translators/message_writers.py | 118 ++++++++++++++++++ .../translators/read_state_conversion.py | 72 ----------- .../translators/write_state_conversion.py | 94 -------------- .../python_tests/translators/test_messages.py | 36 +++--- 7 files changed, 271 insertions(+), 217 deletions(-) create mode 100644 source/modulo_new_core/modulo_new_core/translators/message_readers.py create mode 100644 source/modulo_new_core/modulo_new_core/translators/message_writers.py delete mode 100644 source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py delete mode 100644 source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp index b36040f58..afa5aa3dc 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp @@ -39,63 +39,63 @@ void read_msg(state_representation::CartesianState& state, const geometry_msgs:: /** * @brief Convert a ROS geometry_msgs::msg::Pose to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& msg); /** * @brief Convert a ROS geometry_msgs::msg::PoseStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& msg); /** * @brief Convert a ROS geometry_msgs::msg::Transform to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& msg); /** * @brief Convert a ROS geometry_msgs::msg::TransformStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& msg); /** * @brief Convert a ROS geometry_msgs::msg::Twist to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& msg); /** * @brief Convert a ROS geometry_msgs::msg::TwistStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& msg); /** * @brief Convert a ROS geometry_msgs::msg::Wrench to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& msg); /** * @brief Convert a ROS geometry_msgs::msg::WrenchStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& msg); /** * @brief Convert a ROS sensor_msgs::msg::JointState to a JointState * @param state The JointState to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(state_representation::JointState& state, const sensor_msgs::msg::JointState& msg); @@ -104,7 +104,7 @@ void read_msg(state_representation::JointState& state, const sensor_msgs::msg::J * @tparam T all types of parameters supported in ROS std messages * @tparam U all types of parameters supported in ROS std messages * @param state The Parameter to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ template void read_msg(state_representation::Parameter& state, const U& msg) { @@ -114,35 +114,35 @@ void read_msg(state_representation::Parameter& state, const U& msg) { /** * @brief Convert a ROS std_msgs::msg::Bool to a boolean * @param state The state to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(bool& state, const std_msgs::msg::Bool& msg); /** * @brief Convert a ROS std_msgs::msg::Float64 to a double * @param state The state to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(double& state, const std_msgs::msg::Float64& msg); /** * @brief Convert a ROS std_msgs::msg::Float64MultiArray to a vector of double * @param state The state to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(std::vector& state, const std_msgs::msg::Float64MultiArray& msg); /** * @brief Convert a ROS std_msgs::msg::Int32 to an integer * @param state The state to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(int& state, const std_msgs::msg::Int32& msg); /** * @brief Convert a ROS std_msgs::msg::String to a string * @param state The state to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ void read_msg(std::string& state, const std_msgs::msg::String& msg); @@ -150,7 +150,7 @@ void read_msg(std::string& state, const std_msgs::msg::String& msg); * @brief Convert a ROS std_msgs::msg::UInt8MultiArray message to a State using protobuf decoding * @tparam a state_representation::State type object * @param state The state to populate - * @param msg The ROS msg to read from + * @param msg The ROS message to read from */ template inline void read_msg(T& state, const EncodedState& msg) { diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp index 472792566..809deb051 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp @@ -33,7 +33,7 @@ void write_msg(geometry_msgs::msg::Accel& msg, const state_representation::Carte /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::AccelStamped - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -41,7 +41,7 @@ void write_msg(geometry_msgs::msg::AccelStamped& msg, const state_representation /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Pose - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -49,7 +49,7 @@ void write_msg(geometry_msgs::msg::Pose& msg, const state_representation::Cartes /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::PoseStamped - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -57,7 +57,7 @@ void write_msg(geometry_msgs::msg::PoseStamped& msg, const state_representation: /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Transform - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -65,7 +65,7 @@ void write_msg(geometry_msgs::msg::Transform& msg, const state_representation::C /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::TransformStamped - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -73,7 +73,7 @@ void write_msg(geometry_msgs::msg::TransformStamped& msg, const state_representa /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Twist - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -81,7 +81,7 @@ void write_msg(geometry_msgs::msg::Twist& msg, const state_representation::Carte /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::TwistStamped - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -89,7 +89,7 @@ void write_msg(geometry_msgs::msg::TwistStamped& msg, const state_representation /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Wrench - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -97,7 +97,7 @@ void write_msg(geometry_msgs::msg::Wrench& msg, const state_representation::Cart /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::WrenchStamped - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -105,7 +105,7 @@ void write_msg(geometry_msgs::msg::WrenchStamped& msg, const state_representatio /** * @brief Convert a JointState to a ROS sensor_msgs::msg::JointState - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -113,7 +113,7 @@ void write_msg(sensor_msgs::msg::JointState& msg, const state_representation::Jo /** * @brief Convert a CartesianState to a ROS tf2_msgs::msg::TFMessage - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -123,7 +123,7 @@ void write_msg(tf2_msgs::msg::TFMessage& msg, const state_representation::Cartes * @brief Convert a Parameter to a ROS equivalent representation * @tparam T all types of parameters supported in ROS std messages * @tparam U all types of parameters supported in ROS std messages - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -132,7 +132,7 @@ void write_msg(U& msg, const state_representation::Parameter& state, const rc /** * @brief Convert a boolean to a ROS std_msgs::msg::Bool - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -140,7 +140,7 @@ void write_msg(std_msgs::msg::Bool& msg, const bool& state, const rclcpp::Time& /** * @brief Convert a double to a ROS std_msgs::msg::Float64 - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -148,7 +148,7 @@ void write_msg(std_msgs::msg::Float64& msg, const double& state, const rclcpp::T /** * @brief Convert a vector of double to a ROS std_msgs::msg::Float64MultiArray - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -156,7 +156,7 @@ void write_msg(std_msgs::msg::Float64MultiArray& msg, const std::vector& /** * @brief Convert an integer to a ROS std_msgs::msg::Int32 - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ @@ -164,7 +164,7 @@ void write_msg(std_msgs::msg::Int32& msg, const int& state, const rclcpp::Time& /** * @brief Convert a string to a ROS std_msgs::msg::String - * @param msg The ROS msg to populate + * @param msg The ROS message to populate * @param state The state to read from * @param time The time of the message */ diff --git a/source/modulo_new_core/modulo_new_core/translators/message_readers.py b/source/modulo_new_core/modulo_new_core/translators/message_readers.py new file mode 100644 index 000000000..fc807b15e --- /dev/null +++ b/source/modulo_new_core/modulo_new_core/translators/message_readers.py @@ -0,0 +1,102 @@ +from typing import List +from typing import TypeVar, Union + +import clproto +import geometry_msgs.msg as geometry +import sensor_msgs.msg +import sensor_msgs.msg +import state_representation as sr +from modulo_new_core.encoded_state import EncodedState + +MsgT = TypeVar('MsgT') +StateT = TypeVar('StateT') + + +def read_xyz(msg: Union[geometry.Point, geometry.Vector3]) -> List[float]: + """ + Helper function to read a list from a Point or Vector3 message. + + :param msg: The message to read from + """ + return [msg.x, msg.y, msg.z] + + +def read_quaternion(msg: geometry.Quaternion) -> List[float]: + """ + Helper function to read a list of quaternion coefficients (w,x,y,z) from a Quaternion message. + + :param msg: The message to read from + """ + return [msg.w, msg.x, msg.y, msg.z] + + +def read_msg(state: StateT, msg: MsgT): + """ + Convert a ROS message to a state_representation State. + + :param state: The state to populate + :param msg: The ROS message to read from + """ + if not isinstance(state, sr.State): + raise RuntimeError("This state type is not supported.") + if isinstance(state, sr.CartesianState): + if isinstance(msg, geometry.Accel): + state.set_linear_acceleration(read_xyz(msg.linear)) + state.set_angular_acceleration(read_xyz(msg.angular)) + elif isinstance(msg, geometry.Pose): + state.set_position(read_xyz(msg.position)) + state.set_orientation(read_quaternion(msg.orientation)) + elif isinstance(msg, geometry.Transform): + state.set_position(read_xyz(msg.translation)) + state.set_orientation(read_quaternion(msg.rotation)) + elif isinstance(msg, geometry.Twist): + state.set_linear_velocity(read_xyz(msg.linear)) + state.set_angular_velocity(read_xyz(msg.angular)) + elif isinstance(msg, geometry.Wrench): + state.set_force(read_xyz(msg.force)) + state.set_torque(read_xyz(msg.torque)) + else: + raise RuntimeError("The provided combination of state type and message type is not supported") + elif isinstance(msg, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): + state.set_names(msg.name) + if len(msg.position): + state.set_positions(msg.position) + if len(msg.velocity): + state.set_velocities(msg.velocity) + if len(msg.effort): + state.set_torques(msg.effort) + else: + raise RuntimeError("The provided combination of state type and message type is not supported") + return state + + +def read_stamped_msg(state: StateT, msg: MsgT): + """ + Convert a stamped ROS message to a state_representation State. + + :param state: The state to populate + :param msg: The ROS message to read from + """ + if isinstance(msg, geometry.AccelStamped): + read_msg(state, msg.accel) + elif isinstance(msg, geometry.PoseStamped): + read_msg(state, msg.pose) + elif isinstance(msg, geometry.TransformStamped): + read_msg(state, msg.transform) + state.set_name(msg.child_frame_id) + elif isinstance(msg, geometry.TwistStamped): + read_msg(state, msg.twist) + elif isinstance(msg, geometry.WrenchStamped): + read_msg(state, msg.wrench) + else: + raise RuntimeError("The provided combination of state type and message type is not supported") + state.set_reference_frame(msg.header.frame_id) + + +def read_clproto_msg(msg: EncodedState) -> StateT: + """ + Convert an EncodedState message to a state_representation State. + + :param msg: The EncodedState msg to read from + """ + return clproto.decode(msg.data.tobytes()) diff --git a/source/modulo_new_core/modulo_new_core/translators/message_writers.py b/source/modulo_new_core/modulo_new_core/translators/message_writers.py new file mode 100644 index 000000000..c3d0ed0f4 --- /dev/null +++ b/source/modulo_new_core/modulo_new_core/translators/message_writers.py @@ -0,0 +1,118 @@ +from typing import TypeVar, Union + +import clproto +import geometry_msgs.msg as geometry +import numpy as np +import rclpy.time +import sensor_msgs.msg +import state_representation as sr +from modulo_new_core.encoded_state import EncodedState + +MsgT = TypeVar('MsgT') +StateT = TypeVar('StateT') + + +def write_xyz(msg: Union[geometry.Point, geometry.Vector3], vector: np.array): + """ + Helper function to write a vector to a Point or Vector3 message. + + :param msg: The message to populate + :param vector: The vector to read from + """ + if vector.size != 3: + raise RuntimeError("Provide a vector of size 3 to transform to a Point or Vector3 message.") + msg.x = vector[0] + msg.y = vector[1] + msg.z = vector[2] + + +def write_quaternion(msg: geometry.Quaternion, quat: np.array): + """ + Helper function to write a vector of quaternion coefficients (w,x,y,z) to a Quaternion message. + + :param msg: The message to populate + :param quat: The vector to read from + """ + if quat.size != 4: + raise RuntimeError("Provide a vector of size 4 to transform to a Quaternion message.") + msg.w = quat[0] + msg.x = quat[1] + msg.y = quat[2] + msg.z = quat[3] + + +def write_msg(msg: MsgT, state: StateT): + """ + Convert a state_representation State to a ROS message. + + :param msg: The ROS message to populate + :param state: The state to read from + """ + if not isinstance(state, sr.State): + raise RuntimeError("This state type is not supported.") + if state.is_empty(): + raise RuntimeError(f"{state.get_name()} state is empty while attempting to write it to message.") + if isinstance(state, sr.CartesianState): + if isinstance(msg, geometry.Accel): + write_xyz(msg.linear, state.get_linear_acceleration()) + write_xyz(msg.angular, state.get_angular_acceleration()) + elif isinstance(msg, geometry.Pose): + write_xyz(msg.position, state.get_position()) + write_quaternion(msg.orientation, state.get_orientation_coefficients()) + elif isinstance(msg, geometry.Transform): + write_xyz(msg.translation, state.get_position()) + write_quaternion(msg.rotation, state.get_orientation_coefficients()) + elif isinstance(msg, geometry.Twist): + write_xyz(msg.linear, state.get_linear_velocity()) + write_xyz(msg.angular, state.get_angular_velocity()) + elif isinstance(msg, geometry.Wrench): + write_xyz(msg.force, state.get_force()) + write_xyz(msg.torque, state.get_torque()) + else: + raise RuntimeError("The provided combination of state type and message type is not supported") + elif isinstance(msg, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): + msg.name = state.get_names() + msg.position = state.get_positions().tolist() + msg.velocity = state.get_velocities().tolist() + msg.effort = state.get_torques().tolist() + else: + raise RuntimeError("The provided combination of state type and message type is not supported") + + +def write_stamped_msg(msg: MsgT, state: StateT, time: rclpy.time.Time): + """ + Convert a state_representation State to a stamped ROS message. + + :param msg: The ROS message to populate + :param state: The state to read from + :param time: The time of the message + """ + if isinstance(msg, geometry.AccelStamped): + write_msg(msg.accel, state) + elif isinstance(msg, geometry.PoseStamped): + write_msg(msg.pose, state) + elif isinstance(msg, geometry.TransformStamped): + write_msg(msg.transform, state) + msg.child_frame_id = state.get_name() + elif isinstance(msg, geometry.TwistStamped): + write_msg(msg.twist, state) + elif isinstance(msg, geometry.WrenchStamped): + write_msg(msg.wrench, state) + else: + raise RuntimeError("The provided combination of state type and message type is not supported") + msg.header.stamp = time + msg.header.frame_id = state.get_reference_frame() + + +def write_clproto_msg(state: StateT, clproto_message_type: clproto.MessageType) -> EncodedState(): + """ + Convert a state_representation State to an EncodedState message. + + :param state: The state to read from + :param clproto_message_type: The clproto message type to encode the state + """ + if not isinstance(state, sr.State): + raise RuntimeError("This state type is not supported.") + msg = EncodedState() + msg.data = clproto.encode(state, clproto_message_type) + return msg diff --git a/source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py b/source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py deleted file mode 100644 index 2d39687ce..000000000 --- a/source/modulo_new_core/modulo_new_core/translators/read_state_conversion.py +++ /dev/null @@ -1,72 +0,0 @@ -from typing import List - -import clproto -import geometry_msgs.msg as geometry -import sensor_msgs.msg -from modulo_new_core.encoded_state import EncodedState - - -def __read_xyz(msg) -> List[float]: - return [msg.x, msg.y, msg.z] - - -def __read_quaternion(msg: geometry.Quaternion) -> List[float]: - return [msg.w, msg.x, msg.y, msg.z] - - -def read_msg(state, msg): - if isinstance(msg, geometry.Accel): - state.set_linear_acceleration(__read_xyz(msg.linear)) - state.set_angular_acceleration(__read_xyz(msg.angular)) - elif isinstance(msg, geometry.Pose): - state.set_position(__read_xyz(msg.position)) - state.set_orientation(__read_quaternion(msg.orientation)) - elif isinstance(msg, geometry.Transform): - state.set_position(__read_xyz(msg.translation)) - state.set_orientation(__read_quaternion(msg.rotation)) - elif isinstance(msg, geometry.Twist): - state.set_linear_velocity(__read_xyz(msg.linear)) - state.set_angular_velocity(__read_xyz(msg.angular)) - elif isinstance(msg, geometry.Wrench): - state.set_force(__read_xyz(msg.force)) - state.set_torque(__read_xyz(msg.torque)) - elif isinstance(msg, sensor_msgs.msg.JointState): - state.set_names(msg.name) - if len(msg.position): - state.set_positions(msg.position) - if len(msg.velocity): - state.set_velocities(msg.velocity) - if len(msg.effort): - state.set_torques(msg.effort) - else: - raise RuntimeError("This message type is not supported") - return state - - -def read_msg_stamped(state, msg): - if isinstance(msg, geometry.AccelStamped): - read_msg(state, msg.accel) - state.set_reference_frame(msg.header.frame_id) - elif isinstance(msg, geometry.PoseStamped): - read_msg(state, msg.pose) - state.set_reference_frame(msg.header.frame_id) - elif isinstance(msg, geometry.TransformStamped): - read_msg(state, msg.transform) - state.set_reference_frame(msg.header.frame_id) - state.set_name(msg.child_frame_id) - elif isinstance(msg, geometry.TwistStamped): - read_msg(state, msg.twist) - state.set_reference_frame(msg.header.frame_id) - elif isinstance(msg, geometry.WrenchStamped): - read_msg(state, msg.wrench) - state.set_reference_frame(msg.header.frame_id) - else: - raise RuntimeError("This message type is not supported") - return state - - -def read_clproto_msg(msg: EncodedState): - if isinstance(msg, EncodedState): - return clproto.decode(msg.data.tobytes()) - else: - raise RuntimeError("Your input does not have the correct type") diff --git a/source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py b/source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py deleted file mode 100644 index be3b22a1d..000000000 --- a/source/modulo_new_core/modulo_new_core/translators/write_state_conversion.py +++ /dev/null @@ -1,94 +0,0 @@ -import clproto -import geometry_msgs.msg as geometry -import numpy as np -import rclpy.time -import sensor_msgs.msg -import state_representation as sr -from modulo_new_core.encoded_state import EncodedState - - -def __write_xyz(msg, vector: np.array): - msg.x = vector[0] - msg.y = vector[1] - msg.z = vector[2] - return msg - - -def __write_quaternion(msg: geometry.Quaternion, quat: np.array) -> geometry.Quaternion: - msg.w = quat[0] - msg.x = quat[1] - msg.y = quat[2] - msg.z = quat[3] - return msg - - -def write_msg(msg, state): - if not isinstance(state, sr.State): - raise RuntimeError("This state type is not supported") - if state.is_empty(): - raise RuntimeError(f"{state.get_name()} state is empty while attempting to write it to message") - if isinstance(msg, geometry.Point) and isinstance(state, sr.CartesianState): - __write_xyz(msg, state) - elif isinstance(msg, geometry.Vector3) and isinstance(state, sr.CartesianState): - __write_xyz(msg, state) - elif isinstance(msg, geometry.Quaternion) and isinstance(state, sr.CartesianState): - __write_quaternion(msg, state) - elif isinstance(msg, geometry.Accel) and isinstance(state, sr.CartesianState): - __write_xyz(msg.linear, state.get_linear_acceleration()) - __write_xyz(msg.angular, state.get_angular_acceleration()) - elif isinstance(msg, geometry.Pose) and isinstance(state, sr.CartesianState): - __write_xyz(msg.position, state.get_position()) - __write_quaternion(msg.orientation, state.get_orientation()) - elif isinstance(msg, geometry.Transform) and isinstance(state, sr.CartesianState): - __write_xyz(msg.translation, state.get_position()) - __write_quaternion(msg.rotation, state.get_orientation()) - elif isinstance(msg, geometry.Twist) and isinstance(state, sr.CartesianState): - __write_xyz(msg.linear, state.get_linear_velocity()) - __write_xyz(msg.angular, state.get_angular_velocity()) - elif isinstance(msg, geometry.Wrench) and isinstance(state, sr.CartesianState): - __write_xyz(msg.force, state.get_force()) - __write_xyz(msg.torque, state.get_torque()) - elif isinstance(msg, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): - msg.name = state.get_names() - msg.position = state.get_positions().tolist() - msg.velocity = state.get_velocities().tolist() - msg.effort = state.get_torques().tolist() - else: - raise RuntimeError("This message type is not supported") - return msg - - -def write_msg_stamped(msg, state, time: rclpy.time.Time): - if isinstance(msg, geometry.AccelStamped): - write_msg(msg.accel, state) - msg.header.stamp = time - msg.header.frame_id = state.get_reference_frame() - elif isinstance(msg, geometry.PoseStamped): - write_msg(msg.pose, state) - msg.header.stamp = time - msg.header.frame_id = state.get_reference_frame() - elif isinstance(msg, geometry.TransformStamped): - write_msg(msg.transform, state) - msg.header.stamp = time - msg.header.frame_id = state.get_reference_frame() - msg.child_frame_id = state.get_name() - elif isinstance(msg, geometry.TwistStamped): - write_msg(msg.twist, state) - msg.header.stamp = time - msg.header.frame_id = state.get_reference_frame() - elif isinstance(msg, geometry.WrenchStamped): - write_msg(msg.wrench, state) - msg.header.stamp = time - msg.header.frame_id = state.get_reference_frame() - else: - raise RuntimeError("This message type is not supported") - return state - - -def write_clproto_msg(state: sr.State, msg_type: clproto.MessageType) -> EncodedState(): - msg = EncodedState() - if isinstance(state, sr.State) and isinstance(msg_type, clproto.MessageType): - msg.data = clproto.encode(state, msg_type) - else: - raise RuntimeError("Your inputs do not have the correct types") - return msg diff --git a/source/modulo_new_core/test/python_tests/translators/test_messages.py b/source/modulo_new_core/test/python_tests/translators/test_messages.py index f693258a1..b9f869cd8 100644 --- a/source/modulo_new_core/test/python_tests/translators/test_messages.py +++ b/source/modulo_new_core/test/python_tests/translators/test_messages.py @@ -2,12 +2,12 @@ import geometry_msgs.msg import numpy as np import pytest -import rclpy import sensor_msgs.msg import state_representation as sr +from rclpy.clock import Clock -import modulo_new_core.translators.read_state_conversion as modulo_readers -import modulo_new_core.translators.write_state_conversion as modulo_writers +import modulo_new_core.translators.message_readers as modulo_readers +import modulo_new_core.translators.message_writers as modulo_writers def read_xyz(msg): @@ -25,7 +25,7 @@ def assert_np_array_equal(a: np.array, b: np.array, places=3): pytest.fail(f'{e}') -def test_accel(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): +def test_accel(cart_state: sr.CartesianState, clock: Clock): msg = geometry_msgs.msg.Accel() modulo_writers.write_msg(msg, cart_state) assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_acceleration()) @@ -38,17 +38,17 @@ def test_accel(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) msg_stamped = geometry_msgs.msg.AccelStamped() - modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_acceleration()) assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_acceleration()) new_state = sr.CartesianState("new") - modulo_readers.read_msg_stamped(new_state, msg_stamped) + modulo_readers.read_stamped_msg(new_state, msg_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) -def test_pose(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): +def test_pose(cart_state: sr.CartesianState, clock: Clock): msg = geometry_msgs.msg.Pose() modulo_writers.write_msg(msg, cart_state) assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) @@ -61,17 +61,17 @@ def test_pose(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) msg_stamped = geometry_msgs.msg.PoseStamped() - modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") - modulo_readers.read_msg_stamped(new_state, msg_stamped) + modulo_readers.read_stamped_msg(new_state, msg_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) -def test_transform(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): +def test_transform(cart_state: sr.CartesianState, clock: Clock): msg = geometry_msgs.msg.Transform() modulo_writers.write_msg(msg, cart_state) assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) @@ -84,19 +84,19 @@ def test_transform(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) msg_stamped = geometry_msgs.msg.TransformStamped() - modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert msg_stamped.child_frame_id == cart_state.get_name() assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") - modulo_readers.read_msg_stamped(new_state, msg_stamped) + modulo_readers.read_stamped_msg(new_state, msg_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert cart_state.get_name() == new_state.get_name() assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) -def test_twist(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): +def test_twist(cart_state: sr.CartesianState, clock: Clock): msg = geometry_msgs.msg.Twist() modulo_writers.write_msg(msg, cart_state) assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_velocity()) @@ -109,17 +109,17 @@ def test_twist(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) msg_stamped = geometry_msgs.msg.TwistStamped() - modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_velocity()) assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_velocity()) new_state = sr.CartesianState("new") - modulo_readers.read_msg_stamped(new_state, msg_stamped) + modulo_readers.read_stamped_msg(new_state, msg_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) -def test_wrench(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): +def test_wrench(cart_state: sr.CartesianState, clock: Clock): msg = geometry_msgs.msg.Wrench() modulo_writers.write_msg(msg, cart_state) assert_np_array_equal(read_xyz(msg.force), cart_state.get_force()) @@ -132,12 +132,12 @@ def test_wrench(cart_state: sr.CartesianState, clock: rclpy.clock.Clock): assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) msg_stamped = geometry_msgs.msg.WrenchStamped() - modulo_writers.write_msg_stamped(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.force), cart_state.get_force()) assert_np_array_equal(read_xyz(msg.torque), cart_state.get_torque()) new_state = sr.CartesianState("new") - modulo_readers.read_msg_stamped(new_state, msg_stamped) + modulo_readers.read_stamped_msg(new_state, msg_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) From bd55032c0ca8e858d3f23cfe44ac18950bb7e8cb Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 19 May 2022 17:30:58 +0200 Subject: [PATCH 31/71] Add python component with execution thread (#70) * Fix docstrings * Add python component with execution thread --- .../include/modulo_components/Component.hpp | 4 +- .../modulo_components/component.py | 72 +++++++++++++++++++ .../modulo_components/component_interface.py | 2 +- 3 files changed, 75 insertions(+), 3 deletions(-) create mode 100644 source/modulo_components/modulo_components/component.py diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index be7aa167c..24c02b625 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -71,8 +71,8 @@ class Component : public ComponentInterface { using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_daemon_callbacks; - std::thread run_thread_; - bool started_; + std::thread run_thread_; ///< The execution thread of the component + bool started_; ///< Flag that indicates if execution has started or not }; template diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py new file mode 100644 index 000000000..03663a886 --- /dev/null +++ b/source/modulo_components/modulo_components/component.py @@ -0,0 +1,72 @@ +from threading import Thread + +from modulo_components.component_interface import ComponentInterface + + +class Component(ComponentInterface): + """ + Class to represent a Component in python, following the same logic pattern + as the c++ modulo_component::Component class. + ... + Attributes: + started (bool): flag that indicates if execution has started or not + run_thread (Thread): the execution thread + """ + + def __init__(self, node_name: str, start_thread=True, *kargs, **kwargs): + """ + Constructs all the necessary attributes and declare all the parameters. + Parameters: + node_name (str): name of the node to be passed to the base Node class + start_thread (bool): start the execution thread at construction + """ + super().__init__(node_name, *kargs, **kwargs) + self.__started = False + self.__run_thread = None + self.add_predicate("in_error_state", False) + self.add_predicate("is_finished", False) + + if start_thread: + self.start_thread() + + def start_thread(self): + """ + Start the execution thread. + """ + if self.__started: + self.get_logger().error(f"Run thread for component {self.get_name()} has already been started", + throttle_duration_sec=1.0) + return + self.__started = True + self.__run_thread = Thread(target=self.__run) + self.__run_thread.start() + + def __run(self): + """ + Run the execution function in a try catch block and set + the predicates according to the outcome of the execution. + """ + try: + if not self.execute(): + self._raise_error() + return + except Exception as e: + self.get_logger().error(f"Failed to run component {self.get_name()}: {e}", throttle_duration_sec=1.0) + self._raise_error() + return + self.set_predicate("is_finished", True) + + def execute(self): + """ + Execute the component logic. To be redefined in the derived classes. + + :return: True, if the execution was successful, false otherwise + """ + return True + + def _raise_error(self): + """ + Put the component in error state by setting the 'in_error_state' + predicate to true. + """ + self.set_predicate("in_error_state", True) diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 79f344f45..55ef727a5 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -26,7 +26,7 @@ class ComponentInterface(Node): has_tf_broadcaster (bool): if true, add a tf broadcaster to enable the publishing of transforms. """ - def __init__(self, node_name, *kargs, **kwargs): + def __init__(self, node_name: str, *kargs, **kwargs): """ Constructs all the necessary attributes and declare all the parameters. Parameters: From 3e27f2c4c5418a3827b2ec32693f4606df2d5f29 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 24 May 2022 11:50:33 +0200 Subject: [PATCH 32/71] Minor improvements (#72) * Add virtual destructors * Rename deamon to periodic function * Move in_error_state to ComponentInterface * Fix docstring in LifecycleComponent * Hide ROS parameter methods * Have TF time point and duration as function arguments * Hide all ROS functions except a few required ones * Add test for raise_error * Add missing namespace comment * More docstrings --- .../include/modulo_components/Component.hpp | 16 ++--- .../modulo_components/ComponentInterface.hpp | 70 ++++++++++++------- .../modulo_components/LifecycleComponent.hpp | 20 ++++-- .../modulo_components/utilities/utilities.hpp | 6 ++ source/modulo_components/src/Component.cpp | 7 +- .../test/cpp/test_component_interface.cpp | 7 ++ 6 files changed, 80 insertions(+), 46 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 24c02b625..8371c1fbc 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -15,10 +15,15 @@ class Component : public ComponentInterface { /** * @brief Constructor from node options. - * @param node_options node options as used in ROS2 Node + * @param node_options Node options as used in ROS2 Node */ explicit Component(const rclcpp::NodeOptions& node_options, bool start_thread = true); + /** + * @brief Virtual default destructor. + */ + virtual ~Component() = default; + protected: /** * @brief Start the execution thread. @@ -58,18 +63,13 @@ class Component : public ComponentInterface { */ void run(); - /** - * @brief Put the component in error state by setting the - * 'in_error_state' predicate to true. - */ - void raise_error() override; - + using rclcpp::Node::create_publisher; using ComponentInterface::create_output; using ComponentInterface::outputs_; using ComponentInterface::qos_; using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; - using ComponentInterface::evaluate_daemon_callbacks; + using ComponentInterface::evaluate_periodic_callbacks; std::thread run_thread_; ///< The execution thread of the component bool started_; ///< Flag that indicates if execution has started or not diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 4491608e2..76a17e1c0 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -27,14 +27,14 @@ namespace modulo_components { template -class ComponentInterface : public NodeT { +class ComponentInterface : private NodeT { public: friend class ComponentInterfacePublicInterface; friend class ComponentInterfaceParameterPublicInterface; /** * @brief Constructor from node options. - * @param node_options node options as used in ROS2 Node + * @param node_options Node options as used in ROS2 Node / LifecycleNode */ explicit ComponentInterface( const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type @@ -45,6 +45,11 @@ class ComponentInterface : public NodeT { */ virtual ~ComponentInterface() = default; + using NodeT::get_node_base_interface; + using NodeT::get_name; + using NodeT::get_clock; + using NodeT::get_logger; + protected: /** * @brief Step function that is called periodically. @@ -55,7 +60,9 @@ class ComponentInterface : public NodeT { * @brief Add a parameter. * @details This method stores a pointer reference to an existing Parameter object in the local parameter map * and declares the equivalent ROS parameter on the ROS interface. - * @param parameter A ParameterInterface pointer to a Parameter instance. + * @param parameter A ParameterInterface pointer to a Parameter instance + * @param description The description of the parameter + * @param read_only If true, the value of the parameter cannot be changed after declaration */ void add_parameter( const std::shared_ptr& parameter, const std::string& description, @@ -69,6 +76,8 @@ class ComponentInterface : public NodeT { * @tparam T The type of the parameter * @param name The name of the parameter * @param value The value of the parameter + * @param description The description of the parameter + * @param read_only If true, the value of the parameter cannot be changed after declaration */ template void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false); @@ -180,11 +189,12 @@ class ComponentInterface : public NodeT { ); /** - * @brief Function to daemonize the callback function given in input. - * @param name The name of the daemon - * @param callback The callback of the daemon + * @brief Add a periodic callback function. + * @details The provided function is evaluated periodically at the component step period. + * @param name The name of the callback + * @param callback The callback function that is evaluated periodically */ - void add_daemon(const std::string& name, const std::function& callback); + void add_periodic_function(const std::string& name, const std::function& callback); /** * @brief Configure a transform broadcaster. @@ -232,10 +242,14 @@ class ComponentInterface : public NodeT { * @brief Look up a transform from TF. * @param frame_name The desired frame of the transform * @param reference_frame_name The desired reference frame of the transform + * @param time_point The time at which the value of the transform is desired (default: 0, will get the latest) + * @param duration How long to block the lookup call before failing * @return If it exists, the requested transform */ - [[nodiscard]] state_representation::CartesianPose - lookup_transform(const std::string& frame_name, const std::string& reference_frame_name = "world") const; + [[nodiscard]] state_representation::CartesianPose lookup_transform( + const std::string& frame_name, const std::string& reference_frame_name = "world", + const tf2::TimePoint& time_point = tf2::TimePoint(std::chrono::microseconds(0)), + const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10))) const; /** * @brief Helper function to publish all predicates. @@ -248,16 +262,18 @@ class ComponentInterface : public NodeT { void publish_outputs(); /** - * @brief Helper function to evaluate all daemon callbacks. + * @brief Helper function to evaluate all periodic function callbacks. */ - void evaluate_daemon_callbacks(); + void evaluate_periodic_callbacks(); /** - * @brief Raise an error, or set the component into error state. - * To be redefined in derived classes. + * @brief Put the component in error state by setting the + * 'in_error_state' predicate to true. */ virtual void raise_error(); + using NodeT::create_publisher; + std::map> outputs_; ///< Map of outputs @@ -293,7 +309,7 @@ class ComponentInterface : public NodeT { predicate_publishers_; ///< Map of predicate publishers std::map> inputs_; - std::map> daemon_callbacks_; ///< Map of daemon callbacks + std::map> periodic_callbacks_; ///< Map of periodic function callbacks state_representation::ParameterMap parameter_map_; ///< ParameterMap for handling parameters std::shared_ptr @@ -318,6 +334,8 @@ ComponentInterface::ComponentInterface( ); this->add_parameter("period", 1.0, "The time interval in seconds for all periodic callbacks", true); + this->add_predicate("in_error_state", false); + this->step_timer_ = this->create_wall_timer( std::chrono::nanoseconds(static_cast(this->get_parameter_value("period") * 1e9)), [this] { this->step(); } @@ -585,12 +603,13 @@ inline void ComponentInterface::add_input( } template -inline void ComponentInterface::add_daemon(const std::string& name, const std::function& callback) { - if (this->daemon_callbacks_.find(name) != this->daemon_callbacks_.end()) { +inline void +ComponentInterface::add_periodic_function(const std::string& name, const std::function& callback) { + if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Daemon callback " << name << " already exists, overwriting."); } - this->daemon_callbacks_.template insert_or_assign(name, callback); + this->periodic_callbacks_.template insert_or_assign(name, callback); } template @@ -617,7 +636,8 @@ inline void ComponentInterface::send_transform(const state_representation template inline state_representation::CartesianPose ComponentInterface::lookup_transform( - const std::string& frame_name, const std::string& reference_frame_name + const std::string& frame_name, const std::string& reference_frame_name, const tf2::TimePoint& time_point, + const tf2::Duration& duration ) const { // TODO: throw here? if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { @@ -625,10 +645,8 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra } geometry_msgs::msg::TransformStamped transform; state_representation::CartesianPose result(frame_name, reference_frame_name); - // TODO: timeout - transform = this->tf_buffer_->lookupTransform( - reference_frame_name, frame_name, tf2::TimePoint(std::chrono::microseconds(0)), - tf2::Duration(std::chrono::microseconds(10))); + // TODO: catch exception and rethrow + transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); modulo_new_core::translators::read_msg(result, transform); return result; } @@ -660,8 +678,8 @@ inline void ComponentInterface::publish_outputs() { } template -inline void ComponentInterface::evaluate_daemon_callbacks() { - for (const auto& [daemon, callback]: this->daemon_callbacks_) { +inline void ComponentInterface::evaluate_periodic_callbacks() { + for (const auto& [daemon, callback]: this->periodic_callbacks_) { try { callback(); } catch (const std::exception& ex) { @@ -691,6 +709,8 @@ inline void ComponentInterface::create_output( } template -inline void ComponentInterface::raise_error() {} +inline void ComponentInterface::raise_error() { + this->set_predicate("in_error_state", true); +} }// namespace modulo_components diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index f62d1b268..0d3af76e0 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -15,11 +15,16 @@ class LifecycleComponent : public ComponentInterface::create_output; using ComponentInterface::outputs_; using ComponentInterface::qos_; using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; - using ComponentInterface::evaluate_daemon_callbacks; + using ComponentInterface::evaluate_periodic_callbacks; }; template @@ -77,4 +83,4 @@ LifecycleComponent::add_output(const std::string& signal_name, const std::shared } } -} \ No newline at end of file +}// namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index 66b46af56..e78b3598c 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/source/modulo_components/include/modulo_components/utilities/utilities.hpp @@ -57,6 +57,12 @@ static std::string parse_signal_name(const std::string& signal_name) { return output; } +/** + * @brief Generate the topic name for a predicate from component name and predicate name. + * @param component_name The name of the component which the predicate belongs to + * @param predicate_name The name of the predicate + * @return The generated predicate topic as /predicates/component_name/predicate_name + */ static std::string generate_predicate_topic(const std::string& component_name, const std::string& predicate_name) { return "/predicates/" + component_name + "/" + predicate_name; } diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 30c28bb7e..912e8c9fc 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -6,7 +6,6 @@ namespace modulo_components { Component::Component(const rclcpp::NodeOptions& node_options, bool start_thread) : ComponentInterface(node_options, PublisherType::PUBLISHER), started_(false) { - this->add_predicate("in_error_state", false); this->add_predicate("is_finished", false); if (start_thread) { @@ -17,7 +16,7 @@ Component::Component(const rclcpp::NodeOptions& node_options, bool start_thread) void Component::step() { this->publish_predicates(); this->publish_outputs(); - this->evaluate_daemon_callbacks(); + this->evaluate_periodic_callbacks(); } void Component::start_thread() { @@ -49,8 +48,4 @@ bool Component::execute() { return true; } -void Component::raise_error() { - this->set_predicate("in_error_state", true); -} - }// namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 0ee15c269..619eeadd9 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -18,6 +18,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface::predicates_; using ComponentInterface::add_input; using ComponentInterface::inputs_; + using ComponentInterface::raise_error; }; class ComponentInterfaceTest : public ::testing::Test { @@ -100,4 +101,10 @@ TEST_F(ComponentInterfaceTest, AddInput) { EXPECT_EQ(this->component_->inputs_.at("test_13")->get_message_pair()->get_type(), modulo_new_core::communication::MessageType::BOOL); } + +TEST_F(ComponentInterfaceTest, RaiseError) { + EXPECT_FALSE(this->component_->get_predicate("in_error_state")); + this->component_->raise_error(); + EXPECT_TRUE(this->component_->get_predicate("in_error_state")); +} } \ No newline at end of file From ac3143a84a8c771742d9bf534022893a727f8ff1 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 8 Jun 2022 10:40:06 +0200 Subject: [PATCH 33/71] Improve components tests (#71) * Add test case for component and lifecycle component * Template component interface tests * Move qos getter and setter to header * Add more tests * Remove tf in tests for now * Component public interface test helpers * Add include directory to cpp tests for modulo_components with declarations for the public interface helper classes * Merge ...ParameterPublicInterface into PublicInterface and remove forward declaration from ComponentInterface * Remove class declarations from test sources * Use project source dir in cmake include Co-authored-by: Enrico Eberhard --- source/modulo_components/CMakeLists.txt | 1 + .../modulo_components/ComponentInterface.hpp | 18 +- .../src/ComponentInterface.cpp | 10 - .../component_public_interfaces.hpp | 75 ++++++ .../test/cpp/test_component.cpp | 20 +- .../test/cpp/test_component_interface.cpp | 86 ++++--- .../test_component_interface_parameters.cpp | 215 ++++++++---------- .../test/cpp/test_lifecycle_component.cpp | 17 +- 8 files changed, 260 insertions(+), 182 deletions(-) create mode 100644 source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 00e4a0d3a..084f66b23 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -39,6 +39,7 @@ ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_N if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_pytest REQUIRED) + include_directories(${PROJECT_SOURCE_DIR}/test/cpp/include) # add cpp tests file(GLOB_RECURSE TEST_CPP_SOURCES ${PROJECT_SOURCE_DIR}/test/ test_*.cpp) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 76a17e1c0..368788fd8 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -26,11 +27,14 @@ namespace modulo_components { +template +class ComponentInterfacePublicInterface; + template class ComponentInterface : private NodeT { public: - friend class ComponentInterfacePublicInterface; - friend class ComponentInterfaceParameterPublicInterface; + friend class ComponentInterfacePublicInterface; + friend class ComponentInterfacePublicInterface; /** * @brief Constructor from node options. @@ -713,4 +717,14 @@ inline void ComponentInterface::raise_error() { this->set_predicate("in_error_state", true); } +template +inline rclcpp::QoS ComponentInterface::get_qos() const { + return this->qos_; +} + +template +inline void ComponentInterface::set_qos(const rclcpp::QoS& qos) { + this->qos_ = qos; +} + }// namespace modulo_components diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index b191c0819..2f40a21cf 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -2,14 +2,4 @@ namespace modulo_components { -template -rclcpp::QoS ComponentInterface::get_qos() const { - return this->qos_; -} - -template -void ComponentInterface::set_qos(const rclcpp::QoS& qos) { - this->qos_ = qos; -} - }// namespace modulo_components diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp new file mode 100644 index 000000000..4e235f6a6 --- /dev/null +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include "modulo_components/ComponentInterface.hpp" +#include "modulo_components/Component.hpp" +#include "modulo_components/LifecycleComponent.hpp" + +using namespace state_representation; + +namespace modulo_components { + +template +class ComponentInterfacePublicInterface : public ComponentInterface { +public: + explicit ComponentInterfacePublicInterface( + const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type + ) : ComponentInterface(node_options, publisher_type) {} + using ComponentInterface::add_parameter; + using ComponentInterface::get_parameter; + using ComponentInterface::get_parameter_value; + using ComponentInterface::set_parameter_value; + using ComponentInterface::parameter_map_; + using ComponentInterface::add_predicate; + using ComponentInterface::get_predicate; + using ComponentInterface::set_predicate; + using ComponentInterface::predicates_; + using ComponentInterface::add_input; + using ComponentInterface::inputs_; + using ComponentInterface::create_output; + using ComponentInterface::outputs_; + using ComponentInterface::raise_error; + using ComponentInterface::get_qos; + using ComponentInterface::set_qos; + + bool validate_parameter(const std::shared_ptr&) override { + validate_parameter_was_called = true; + return validate_parameter_return_value; + } + + rclcpp::Parameter get_ros_parameter(const std::string& name) { + return NodeT::get_parameter(name); + } + + rcl_interfaces::msg::SetParametersResult set_ros_parameter(const rclcpp::Parameter& parameter) { + return NodeT::set_parameter(parameter); + } + + std::string get_parameter_description(const std::string& name) { + return NodeT::describe_parameter(name).description; + } + + bool validate_parameter_was_called = false; + bool validate_parameter_return_value = true; +}; + +class ComponentPublicInterface : public Component { +public: + explicit ComponentPublicInterface(const rclcpp::NodeOptions& node_options, bool start_thread = true) : + Component(node_options, start_thread) {} + using Component::add_output; + using Component::outputs_; + using Component::get_clock; +}; + +class LifecycleComponentPublicInterface : public LifecycleComponent { +public: + explicit LifecycleComponentPublicInterface(const rclcpp::NodeOptions& node_options) : LifecycleComponent(node_options) {} + using LifecycleComponent::add_output; + using LifecycleComponent::configure_outputs; + using LifecycleComponent::activate_outputs; + using LifecycleComponent::outputs_; + using LifecycleComponent::get_clock; +}; + + +} \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp index c7f22345d..05be6495c 100644 --- a/source/modulo_components/test/cpp/test_component.cpp +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -5,18 +5,12 @@ #include "modulo_components/Component.hpp" #include "modulo_new_core/EncodedState.hpp" +#include "test_modulo_components/component_public_interfaces.hpp" + using namespace state_representation; namespace modulo_components { -class ComponentPublicInterface : public Component { -public: - explicit ComponentPublicInterface(const rclcpp::NodeOptions& node_options) : Component(node_options) {} - using Component::add_output; - using Component::outputs_; - using Component::get_clock; -}; - class ComponentTest : public ::testing::Test { protected: static void SetUpTestSuite() { @@ -28,7 +22,7 @@ class ComponentTest : public ::testing::Test { } void SetUp() override { - component_ = std::make_shared(rclcpp::NodeOptions()); + component_ = std::make_shared(rclcpp::NodeOptions(), false); } std::shared_ptr component_; @@ -37,9 +31,13 @@ class ComponentTest : public ::testing::Test { TEST_F(ComponentTest, AddOutput) { std::shared_ptr data = make_shared_state(CartesianState::Random("test")); component_->add_output("_tEsT_#1@3", data, true); - auto outputs_iterator = component_->outputs_.find("test_13"); - EXPECT_TRUE(outputs_iterator != component_->outputs_.end()); + EXPECT_TRUE(component_->outputs_.find("test_13") != component_->outputs_.end()); EXPECT_NO_THROW(component_->outputs_.at("test_13")->publish()); + + auto new_data = std::make_shared(false); + component_->add_output("test_13", new_data, true); + EXPECT_EQ(component_->outputs_.at("test_13")->get_message_pair()->get_type(), + modulo_new_core::communication::MessageType::ENCODED_STATE); } } // namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 619eeadd9..c56c43e77 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -1,26 +1,13 @@ #include -#include -#include -#include #include "modulo_components/ComponentInterface.hpp" #include "modulo_new_core/EncodedState.hpp" -namespace modulo_components { +#include "test_modulo_components/component_public_interfaces.hpp" -class ComponentInterfacePublicInterface : public ComponentInterface { -public: - explicit ComponentInterfacePublicInterface(const rclcpp::NodeOptions& node_options) : - ComponentInterface(node_options, modulo_new_core::communication::PublisherType::PUBLISHER) {} - using ComponentInterface::add_predicate; - using ComponentInterface::get_predicate; - using ComponentInterface::set_predicate; - using ComponentInterface::predicates_; - using ComponentInterface::add_input; - using ComponentInterface::inputs_; - using ComponentInterface::raise_error; -}; +namespace modulo_components { +template class ComponentInterfaceTest : public ::testing::Test { protected: static void SetUpTestSuite() { @@ -32,13 +19,25 @@ class ComponentInterfaceTest : public ::testing::Test { } void SetUp() override { - component_ = std::make_shared(rclcpp::NodeOptions()); + if (std::is_same::value) { + this->component_ = std::make_shared>( + rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::PUBLISHER + ); + } else if (std::is_same::value) { + this->component_ = std::make_shared>( + rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER + ); + } } - std::shared_ptr component_; + std::shared_ptr> component_; + std::shared_ptr node_; }; -TEST_F(ComponentInterfaceTest, AddBoolPredicate) { +using NodeTypes = ::testing::Types; +TYPED_TEST_SUITE(ComponentInterfaceTest, NodeTypes); + +TYPED_TEST(ComponentInterfaceTest, AddBoolPredicate) { this->component_->add_predicate("foo", true); auto predicate_iterator = this->component_->predicates_.find("foo"); EXPECT_TRUE(predicate_iterator != this->component_->predicates_.end()); @@ -46,15 +45,15 @@ TEST_F(ComponentInterfaceTest, AddBoolPredicate) { EXPECT_TRUE(value); } -TEST_F(ComponentInterfaceTest, AddFunctionPredicate) { +TYPED_TEST(ComponentInterfaceTest, AddFunctionPredicate) { this->component_->add_predicate("bar", [&]() { return false; }); - auto predicate_iterator = component_->predicates_.find("bar"); - EXPECT_TRUE(predicate_iterator != component_->predicates_.end()); + auto predicate_iterator = this->component_->predicates_.find("bar"); + EXPECT_TRUE(predicate_iterator != this->component_->predicates_.end()); auto value_callback = std::get>(predicate_iterator->second); EXPECT_FALSE((value_callback)()); } -TEST_F(ComponentInterfaceTest, GetPredicateValue) { +TYPED_TEST(ComponentInterfaceTest, GetPredicateValue) { this->component_->add_predicate("foo", true); EXPECT_TRUE(this->component_->get_predicate("foo")); this->component_->add_predicate("bar", [&]() { return true; }); @@ -71,7 +70,7 @@ TEST_F(ComponentInterfaceTest, GetPredicateValue) { EXPECT_FALSE(this->component_->get_predicate("error")); } -TEST_F(ComponentInterfaceTest, SetPredicateValue) { +TYPED_TEST(ComponentInterfaceTest, SetPredicateValue) { this->component_->add_predicate("foo", true); this->component_->set_predicate("foo", false); EXPECT_FALSE(this->component_->get_predicate("foo")); @@ -83,17 +82,17 @@ TEST_F(ComponentInterfaceTest, SetPredicateValue) { EXPECT_FALSE(this->component_->get_predicate("bar")); } -TEST_F(ComponentInterfaceTest, AddInput) { +TYPED_TEST(ComponentInterfaceTest, AddInput) { auto data = std::make_shared(true); EXPECT_NO_THROW(this->component_->add_input("_tEsT_#1@3", data, true)); - auto inputs_iterator = component_->inputs_.find("test_13"); - EXPECT_TRUE(inputs_iterator != component_->inputs_.end()); + EXPECT_FALSE(this->component_->inputs_.find("test_13") == this->component_->inputs_.end()); + EXPECT_EQ(this->component_->template get_parameter_value("test_13_topic"), "~/test_13"); EXPECT_NO_THROW(this->component_->template add_input( - "_tEsT_#1@5", [](const std::shared_ptr) {} + "_tEsT_#1@5", [](const std::shared_ptr) {}, true, "/topic" )); - inputs_iterator = component_->inputs_.find("test_15"); - EXPECT_TRUE(inputs_iterator != component_->inputs_.end()); + EXPECT_FALSE(this->component_->inputs_.find("test_15") == this->component_->inputs_.end()); + EXPECT_EQ(this->component_->template get_parameter_value("test_15_topic"), "/topic"); this->component_->template add_input( "test_13", [](const std::shared_ptr) {} @@ -102,9 +101,32 @@ TEST_F(ComponentInterfaceTest, AddInput) { modulo_new_core::communication::MessageType::BOOL); } -TEST_F(ComponentInterfaceTest, RaiseError) { +TYPED_TEST(ComponentInterfaceTest, CreateOutput) { + auto data = std::make_shared(true); + EXPECT_NO_THROW(this->component_->create_output("test", data, true, "/topic")); + EXPECT_FALSE(this->component_->outputs_.find("test") == this->component_->outputs_.end()); + EXPECT_EQ(this->component_->template get_parameter_value("test_topic"), "/topic"); + + auto pub_interface = this->component_->outputs_.at("test"); + if (typeid(this->node_) == typeid(rclcpp::Node)) { + EXPECT_EQ(pub_interface->get_type(), modulo_new_core::communication::PublisherType::PUBLISHER); + } else if (typeid(this->node_) == typeid(rclcpp_lifecycle::LifecycleNode)) { + EXPECT_EQ(pub_interface->get_type(), modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER); + } + EXPECT_EQ(pub_interface->get_message_pair()->get_type(), modulo_new_core::communication::MessageType::BOOL); + EXPECT_THROW(pub_interface->publish(), modulo_new_core::exceptions::InvalidPointerCastException); +} + +TYPED_TEST(ComponentInterfaceTest, GetSetQoS) { + auto qos = rclcpp::QoS(5); + this->component_->set_qos(qos); + EXPECT_EQ(qos, this->component_->get_qos()); +} + + +TYPED_TEST(ComponentInterfaceTest, RaiseError) { EXPECT_FALSE(this->component_->get_predicate("in_error_state")); this->component_->raise_error(); EXPECT_TRUE(this->component_->get_predicate("in_error_state")); } -} \ No newline at end of file +}// namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index efea35366..31b1b6066 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -1,46 +1,15 @@ #include -#include -#include -#include #include #include "modulo_components/ComponentInterface.hpp" #include "modulo_new_core/EncodedState.hpp" -namespace modulo_components { - -class ComponentInterfaceParameterPublicInterface : public ComponentInterface { -public: - explicit ComponentInterfaceParameterPublicInterface(const rclcpp::NodeOptions& node_options) : - ComponentInterface(node_options, modulo_new_core::communication::PublisherType::PUBLISHER) {} - using ComponentInterface::add_parameter; - using ComponentInterface::get_parameter; - using ComponentInterface::get_parameter_value; - using ComponentInterface::set_parameter_value; - using ComponentInterface::parameter_map_; - - bool validate_parameter(const std::shared_ptr&) override { - validate_was_called = true; - return validation_return_value; - } - - rclcpp::Parameter get_ros_parameter(const std::string& name) { - return rclcpp::Node::get_parameter(name); - } - - rcl_interfaces::msg::SetParametersResult set_ros_parameter(const rclcpp::Parameter& parameter) { - return rclcpp::Node::set_parameter(parameter); - } +#include "test_modulo_components/component_public_interfaces.hpp" - std::string get_parameter_description(const std::string& name) { - return rclcpp::Node::describe_parameter(name).description; - } - - bool validate_was_called = false; - bool validation_return_value = true; -}; +namespace modulo_components { +template class ComponentInterfaceParameterTest : public ::testing::Test { protected: static void SetUpTestSuite() { @@ -52,145 +21,157 @@ class ComponentInterfaceParameterTest : public ::testing::Test { } void SetUp() override { - component_ = std::make_shared(rclcpp::NodeOptions()); + if (std::is_same::value) { + this->component_ = std::make_shared>( + rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::PUBLISHER + ); + } else if (std::is_same::value) { + this->component_ = std::make_shared>( + rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER + ); + } param_ = state_representation::make_shared_parameter("test", 1); } template void expect_parameter_value(const T& value) { - EXPECT_STREQ(component_->get_ros_parameter("test").value_to_string().c_str(), std::to_string(value).c_str()); - EXPECT_EQ(component_->get_parameter_value("test"), value); - EXPECT_EQ(component_->parameter_map_.get_parameter_value("test"), value); + EXPECT_STREQ(this->component_->get_ros_parameter("test").value_to_string().c_str(), std::to_string(value).c_str()); + EXPECT_EQ(this->component_->template get_parameter_value("test"), value); + EXPECT_EQ(this->component_->parameter_map_.template get_parameter_value("test"), value); } - std::shared_ptr component_; + std::shared_ptr> component_; std::shared_ptr> param_; }; +using NodeTypes = ::testing::Types; +TYPED_TEST_SUITE(ComponentInterfaceParameterTest, NodeTypes); -TEST_F(ComponentInterfaceParameterTest, AddParameter) { - EXPECT_THROW(auto discard = component_->get_parameter("test"), +TYPED_TEST(ComponentInterfaceParameterTest, AddParameter) { + EXPECT_THROW(auto discard = this->component_->get_parameter("test"), state_representation::exceptions::InvalidParameterException); - EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); - component_->add_parameter(param_, "Test parameter"); + EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + this->component_->add_parameter(this->param_, "Test parameter"); // Adding the parameter should declare and set the value and call the validation function - EXPECT_TRUE(component_->validate_was_called); - EXPECT_NO_THROW(auto discard = component_->get_parameter("test")); - EXPECT_NO_THROW(component_->get_ros_parameter("test")); - expect_parameter_value(1); + EXPECT_TRUE(this->component_->validate_parameter_was_called); + EXPECT_NO_THROW(auto discard = this->component_->get_parameter("test")); + EXPECT_NO_THROW(this->component_->get_ros_parameter("test")); + this->template expect_parameter_value(1); } -TEST_F(ComponentInterfaceParameterTest, AddNameValueParameter) { - EXPECT_THROW(auto discard = component_->get_parameter("test"), +TYPED_TEST(ComponentInterfaceParameterTest, AddNameValueParameter) { + EXPECT_THROW(auto discard = this->component_->get_parameter("test"), state_representation::exceptions::InvalidParameterException); - EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); - component_->add_parameter("test", 1, "Test parameter"); + EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + this->component_->add_parameter("test", 1, "Test parameter"); // Adding the parameter should declare and set the value and call the validation function - EXPECT_TRUE(component_->validate_was_called); - EXPECT_NO_THROW(auto discard = component_->get_parameter("test")); - EXPECT_NO_THROW(component_->get_ros_parameter("test")); - expect_parameter_value(1); + EXPECT_TRUE(this->component_->validate_parameter_was_called); + EXPECT_NO_THROW(auto discard = this->component_->get_parameter("test")); + EXPECT_NO_THROW(this->component_->get_ros_parameter("test")); + this->template expect_parameter_value(1); } -TEST_F(ComponentInterfaceParameterTest, AddParameterAgain) { +TYPED_TEST(ComponentInterfaceParameterTest, AddParameterAgain) { EXPECT_THROW(auto - discard = component_->get_parameter("test"), + discard = this->component_->get_parameter("test"), state_representation::exceptions::InvalidParameterException); - EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); - component_->add_parameter(param_, "Test parameter"); + EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + this->component_->add_parameter(this->param_, "Test parameter"); // Adding an existing parameter again should just set the value - component_->validate_was_called = false; - EXPECT_NO_THROW(component_->add_parameter("test", 2, "foo")); - EXPECT_TRUE(component_->validate_was_called); - expect_parameter_value(2); - EXPECT_EQ(param_->get_value(), 2); + this->component_->validate_parameter_was_called = false; + EXPECT_NO_THROW(this->component_->add_parameter("test", 2, "foo")); + EXPECT_TRUE(this->component_->validate_parameter_was_called); + this->template expect_parameter_value(2); + EXPECT_EQ(this->param_->get_value(), 2); } -TEST_F(ComponentInterfaceParameterTest, SetParameter) { +TYPED_TEST(ComponentInterfaceParameterTest, SetParameter) { // setting before adding should not work - EXPECT_THROW(component_->set_parameter_value("test", 1), rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_THROW(this->component_->set_parameter_value("test", 1), rclcpp::exceptions::ParameterNotDeclaredException); // validation should not be invoked as the parameter did not exist - EXPECT_FALSE(component_->validate_was_called); - EXPECT_THROW(component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_FALSE(this->component_->validate_parameter_was_called); + EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); - component_->add_parameter(param_, "Test parameter"); + this->component_->add_parameter(this->param_, "Test parameter"); // Setting the parameter value should call the validation function and update the referenced value - component_->validate_was_called = false; - EXPECT_NO_THROW(component_->set_parameter_value("test", 2)); - EXPECT_TRUE(component_->validate_was_called); - expect_parameter_value(2); - EXPECT_EQ(param_->get_value(), 2); + this->component_->validate_parameter_was_called = false; + EXPECT_NO_THROW(this->component_->set_parameter_value("test", 2)); + EXPECT_TRUE(this->component_->validate_parameter_was_called); + this->template expect_parameter_value(2); + EXPECT_EQ(this->param_->get_value(), 2); // If the validation function returns false, setting the parameter value should _not_ update the referenced value - component_->validate_was_called = false; - component_->validation_return_value = false; - EXPECT_THROW(component_->set_parameter_value("test", 3), state_representation::exceptions::InvalidParameterException); - EXPECT_TRUE(component_->validate_was_called); - expect_parameter_value(2); - EXPECT_EQ(param_->get_value(), 2); + this->component_->validate_parameter_was_called = false; + this->component_->validate_parameter_return_value = false; + EXPECT_THROW(this->component_->set_parameter_value("test", 3), + state_representation::exceptions::InvalidParameterException); + EXPECT_TRUE(this->component_->validate_parameter_was_called); + this->template expect_parameter_value(2); + EXPECT_EQ(this->param_->get_value(), 2); // Setting a value with an incompatible type should not update the parameter - EXPECT_THROW(component_->set_parameter_value("test", "foo"), + EXPECT_THROW(this->component_->template set_parameter_value("test", "foo"), state_representation::exceptions::InvalidParameterException); - EXPECT_TRUE(component_->validate_was_called); - expect_parameter_value(2); - EXPECT_EQ(param_->get_value(), 2); + EXPECT_TRUE(this->component_->validate_parameter_was_called); + this->template expect_parameter_value(2); + EXPECT_EQ(this->param_->get_value(), 2); } -TEST_F(ComponentInterfaceParameterTest, SetParameterROS) { - component_->add_parameter(param_, "Test parameter"); +TYPED_TEST(ComponentInterfaceParameterTest, SetParameterROS) { + this->component_->add_parameter(this->param_, "Test parameter"); // Setting the parameter value should call the validation function and update the referenced value - component_->validate_was_called = false; - auto result = component_->set_ros_parameter({"test", 2}); - EXPECT_TRUE(component_->validate_was_called); + this->component_->validate_parameter_was_called = false; + auto result = this->component_->set_ros_parameter({"test", 2}); + EXPECT_TRUE(this->component_->validate_parameter_was_called); EXPECT_TRUE(result.successful); - expect_parameter_value(2); - EXPECT_EQ(param_->get_value(), 2); + this->template expect_parameter_value(2); + EXPECT_EQ(this->param_->get_value(), 2); // If the validation function returns false, setting the parameter value should _not_ update the referenced value - component_->validate_was_called = false; - component_->validation_return_value = false; - result = component_->set_ros_parameter({"test", 3}); - EXPECT_TRUE(component_->validate_was_called); + this->component_->validate_parameter_was_called = false; + this->component_->validate_parameter_return_value = false; + result = this->component_->set_ros_parameter({"test", 3}); + EXPECT_TRUE(this->component_->validate_parameter_was_called); EXPECT_FALSE(result.successful); - expect_parameter_value(2); - EXPECT_EQ(param_->get_value(), 2); + this->template expect_parameter_value(2); + EXPECT_EQ(this->param_->get_value(), 2); } -TEST_F(ComponentInterfaceParameterTest, GetParameterDescription) { - component_->add_parameter(param_, "Test parameter"); - EXPECT_STREQ(component_->get_parameter_description("test").c_str(), "Test parameter"); +TYPED_TEST(ComponentInterfaceParameterTest, GetParameterDescription) { + this->component_->add_parameter(this->param_, "Test parameter"); + EXPECT_STREQ(this->component_->get_parameter_description("test").c_str(), "Test parameter"); - EXPECT_THROW(component_->get_parameter_description("foo"), rclcpp::exceptions::ParameterNotDeclaredException); + EXPECT_THROW(this->component_->get_parameter_description("foo"), rclcpp::exceptions::ParameterNotDeclaredException); } -TEST_F(ComponentInterfaceParameterTest, ReadOnlyParameter) { +TYPED_TEST(ComponentInterfaceParameterTest, ReadOnlyParameter) { // Adding a read-only parameter should behave normally - component_->add_parameter(param_, "Test parameter", true); - EXPECT_TRUE(component_->validate_was_called); - EXPECT_NO_THROW(auto discard = component_->get_parameter("test")); - EXPECT_NO_THROW(component_->get_ros_parameter("test")); - expect_parameter_value(1); + this->component_->add_parameter(this->param_, "Test parameter", true); + EXPECT_TRUE(this->component_->validate_parameter_was_called); + EXPECT_NO_THROW(auto discard = this->component_->get_parameter("test")); + EXPECT_NO_THROW(this->component_->get_ros_parameter("test")); + this->template expect_parameter_value(1); // Trying to set the value of the read-only parameter should fail before the validation step - component_->validate_was_called = false; - EXPECT_THROW(component_->set_parameter_value("test", 2), state_representation::exceptions::InvalidParameterException); - EXPECT_FALSE(component_->validate_was_called); - expect_parameter_value(1); - EXPECT_EQ(param_->get_value(), 1); + this->component_->validate_parameter_was_called = false; + EXPECT_THROW(this->component_->set_parameter_value("test", 2), + state_representation::exceptions::InvalidParameterException); + EXPECT_FALSE(this->component_->validate_parameter_was_called); + this->template expect_parameter_value(1); + EXPECT_EQ(this->param_->get_value(), 1); // Setting the value on the ROS interface should also fail - component_->validate_was_called = false; - auto result = component_->set_ros_parameter({"test", 2}); - EXPECT_FALSE(component_->validate_was_called); + this->component_->validate_parameter_was_called = false; + auto result = this->component_->set_ros_parameter({"test", 2}); + EXPECT_FALSE(this->component_->validate_parameter_was_called); EXPECT_FALSE(result.successful); - expect_parameter_value(1); - EXPECT_EQ(param_->get_value(), 1); + this->template expect_parameter_value(1); + EXPECT_EQ(this->param_->get_value(), 1); } } // namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/test/cpp/test_lifecycle_component.cpp b/source/modulo_components/test/cpp/test_lifecycle_component.cpp index b3f37da0f..6e328688a 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component.cpp @@ -5,20 +5,12 @@ #include "modulo_components/LifecycleComponent.hpp" #include "modulo_new_core/EncodedState.hpp" +#include "test_modulo_components/component_public_interfaces.hpp" + using namespace state_representation; namespace modulo_components { -class LifecycleComponentPublicInterface : public LifecycleComponent { -public: - explicit LifecycleComponentPublicInterface(const rclcpp::NodeOptions& node_options) : LifecycleComponent(node_options) {} - using LifecycleComponent::add_output; - using LifecycleComponent::configure_outputs; - using LifecycleComponent::activate_outputs; - using LifecycleComponent::outputs_; - using LifecycleComponent::get_clock; -}; - class LifecycleComponentTest : public ::testing::Test { protected: static void SetUpTestSuite() { @@ -44,6 +36,11 @@ TEST_F(LifecycleComponentTest, AddOutput) { EXPECT_NO_THROW(component_->configure_outputs()); EXPECT_NO_THROW(component_->activate_outputs()); EXPECT_NO_THROW(component_->outputs_.at("test")->publish()); + + auto new_data = std::make_shared(false); + component_->add_output("test", new_data, true); + EXPECT_EQ(component_->outputs_.at("test")->get_message_pair()->get_type(), + modulo_new_core::communication::MessageType::ENCODED_STATE); } } // namespace modulo_components \ No newline at end of file From dbac39ebbf81fa30a53a19a7682821e3af8b8b02 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 8 Jun 2022 10:41:16 +0200 Subject: [PATCH 34/71] Parameters in python ComponentInterface (#73) * Add component exceptions * Add methods to deal with get/set/add parameter and on change callback * Update constructor and imports * Add test for parameters in component interface * Add docstrings * Add TODO for read only * Apply suggestions from code review and add some TODOs --- .../modulo_components/ComponentInterface.hpp | 1 + .../modulo_components/component_interface.py | 211 +++++++++++++++--- .../modulo_components/exceptions/__init__.py | 0 .../exceptions/component_exceptions.py | 8 + .../test_component_interface_parameters.py | 159 +++++++++++++ 5 files changed, 342 insertions(+), 37 deletions(-) create mode 100644 source/modulo_components/modulo_components/exceptions/__init__.py create mode 100644 source/modulo_components/modulo_components/exceptions/component_exceptions.py create mode 100644 source/modulo_components/test/python/test_component_interface_parameters.py diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 368788fd8..0c5274342 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -392,6 +392,7 @@ inline void ComponentInterface::set_parameter_value(const std::string& na rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter( modulo_new_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); if (!result.successful) { + // TODO not throw here throw state_representation::exceptions::InvalidParameterException(result.reason); } } diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 55ef727a5..132d7b87b 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -1,12 +1,19 @@ +import sys +from typing import Union, TypeVar, Callable, List + +import rclpy +import state_representation as sr +from modulo_components.exceptions.component_exceptions import ComponentParameterError +from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter_const +from rcl_interfaces.msg import ParameterDescriptor +from rcl_interfaces.msg import SetParametersResult from rclpy.node import Node +from tf2_ros import TransformBroadcaster from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener -from tf2_ros import TransformBroadcaster -from rcl_interfaces.msg import ParameterDescriptor from std_msgs.msg import Bool -from typing import Union -from typing import Callable +T = TypeVar('T') class ComponentInterface(Node): @@ -17,13 +24,12 @@ class ComponentInterface(Node): Attributes: predicates (dict(Parameters(bool))): map of predicates added to the Component. predicate_publishers (dict(rclpy.Publisher(Bool))): map of publishers associated to each predicate. + parameter_dict: dict of parameters tf_buffer (tf2_ros.Buffer): the buffer to lookup transforms published on tf2. tf_listener (tf2_ros.TransformListener): the listener to lookup transforms published on tf2. tf_broadcaster (tf2_ros.TransformBroadcaster): the broadcaster to publish transforms on tf2 Parameters: period (double): period (in s) between step function calls. - has_tf_listener (bool): if true, add a tf listener to enable calls to lookup transforms. - has_tf_broadcaster (bool): if true, add a tf broadcaster to enable the publishing of transforms. """ def __init__(self, node_name: str, *kargs, **kwargs): @@ -33,30 +39,24 @@ def __init__(self, node_name: str, *kargs, **kwargs): node_name (str): name of the node to be passed to the base Node class """ super().__init__(node_name, *kargs, **kwargs) - self.declare_parameter('period', 0.1, - ParameterDescriptor(description="Period (in s) between step function calls.")) - self.declare_parameter('has_tf_listener', False, - ParameterDescriptor( - description="If true, add a tf listener to enable calls to lookup transforms.")) - self.declare_parameter('has_tf_broadcaster', False, - ParameterDescriptor( - description="If true, add a tf broadcaster to enable the publishing of transforms.")) - - self._period = self.get_parameter('period').get_parameter_value().double_value - self._has_tf_listener = self.get_parameter('has_tf_listener').get_parameter_value().double_value - self._has_tf_broadcaster = self.get_parameter('has_tf_broadcaster').get_parameter_value().double_value + self._parameter_dict = {} self._predicates = {} self._predicate_publishers = {} + # TODO add_XXX + self.__tf_buffer = None + self.__tf_listener = None + self.__tf_broadcaster = None - if self._has_tf_listener: - self.__tf_buffer = Buffer() - self.__tf_listener = TransformListener(self.__tf_buffer, self) - if self._has_tf_broadcaster: - self.__tf_broadcaster = TransformBroadcaster(self) + self.add_on_set_parameters_callback(self.__on_set_parameters_callback) + self.add_parameter(sr.Parameter("period", 0.1, sr.ParameterType.DOUBLE), + "Period (in s) between step function calls.") - self.create_timer(self._period, self.__step) + self.create_timer(self.get_parameter_value("period"), self.__step) - def __step(self): + def __step(self) -> None: + """ + Step function that is called periodically. + """ for predicate_name in self._predicates.keys(): msg = Bool() msg.data = self.get_predicate(predicate_name) @@ -67,29 +67,165 @@ def __step(self): self._predicate_publishers[predicate_name].publish(msg) def __generate_predicate_topic(self, predicate_name: str) -> str: - return f'/predicates/{self.get_name()}/{predicate_name}' + """ + Generate the predicate topic name from the name of the predicate. + + :param predicate_name: The predicate name + :return: The predicate topic as /predicates/component_name/predicate_name + """ + return f"/predicates/{self.get_name()}/{predicate_name}" + + def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, read_only=False) -> None: + """ + Add a parameter. This method stores either the name of the attribute corresponding to the parameter object or + a parameter object directly in the local parameter dictionary and declares the equivalent ROS parameter on the + ROS interface. If an attribute name is provided, changing the ROS parameter will also update the provided + attribute and vice versa. If the provided argument is not an attribute name or a Parameter object, nothing + happens. + + :param parameter: Either the name of the parameter attribute or the parameter itself + :param description: The parameter description + :param read_only: If True, the value of the parameter cannot be changed after declaration + """ + try: + if isinstance(parameter, sr.Parameter): + sr_parameter = parameter + elif isinstance(parameter, str): + attr = self.__getattribute__(parameter) + if isinstance(attr, sr.Parameter): + sr_parameter = attr + else: + raise TypeError("The attribute with the provided name does not contain a Parameter object.") + else: + raise TypeError("Provide either a state_representation.Parameter object or a string " + "containing the name of the attribute that refers to the parameter to add.") + except (AttributeError, TypeError) as e: + self.get_logger().error(f"Failed to add parameter: {e}") + return + ros_param = write_parameter(sr_parameter) + if not self.has_parameter(sr_parameter.get_name()): + self._parameter_dict[sr_parameter.get_name()] = parameter + # TODO ignore override + self.declare_parameter(ros_param.name, ros_param.value, + descriptor=ParameterDescriptor(description=description), ignore_override=read_only) + else: + self.set_parameters([ros_param]) + + def get_parameter(self, name: str) -> Union[sr.Parameter, rclpy.parameter.Parameter]: + """ + Get a parameter by name. If this method is called from a file that contains 'rclpy' in its path, the + rclpy.node.Node.get_parameter method will be invoked, otherwise return the parameter from the local parameter + dictionary. + + :param name: The name of the parameter + :return: The requested parameter + """ + co_filename = sys._getframe().f_back.f_code.co_filename + self.get_logger().debug(f"get_parameter called from {co_filename}") + if "rclpy" in co_filename: + return rclpy.node.Node.get_parameter(self, name) + else: + return self._get_component_parameter(name) + + def _get_component_parameter(self, name: str) -> sr.Parameter: + """ + Get the parameter from the parameter dictionary by its name. + + :param name: The name of the parameter + :return: The parameter, if it exists + :raises ComponentParameterError if the parameter does not exist + """ + if name not in self._parameter_dict.keys(): + raise ComponentParameterError(f"Failed to get parameter '{name}'") + if isinstance(self._parameter_dict[name], str): + return self.__getattribute__(self._parameter_dict[name]) + else: + return self._parameter_dict[name] + + def get_parameter_value(self, name: str) -> T: + """ + Get the parameter value from the parameter dictionary by its name. + + :param name: The name of the parameter + :return: The value of the parameter, if the parameter exists + :raises ComponentParameterError if the parameter does not exist + """ + return self._get_component_parameter(name).get_value() + + def set_parameter_value(self, name: str, value: T, parameter_type: sr.ParameterType) -> None: + """ + Set the value of a parameter. The parameter must have been previously declared. If the parameter is an + attribute, the attribute is updated. + + :param name: The name of the parameter + :param value: The value of the parameter + :param parameter_type: The type of the parameter + """ + ros_param = write_parameter(sr.Parameter(name, value, parameter_type)) + result = self.set_parameters([ros_param])[0] + if not result.successful: + # TODO not throw here + raise RuntimeError(result.reason) + + def _validate_parameter(self, parameter: sr.Parameter) -> bool: + """ + Parameter validation function to be redefined by derived Component classes. This method is automatically invoked + whenever the ROS interface tried to modify a parameter. If the validation returns True, the updated parameter + value (including any modifications) is applied. If the validation returns False, any changes to the parameter + are discarded and the parameter value is not changed. + + :param parameter: The parameter to be validated + :return: The validation result + """ + return True + + def __on_set_parameters_callback(self, ros_parameters: List[rclpy.Parameter]) -> SetParametersResult: + """ + Callback function to validate and update parameters on change. + + :param ros_parameters: The new parameter objects provided by the ROS interface + :return: The result of the validation + """ + result = SetParametersResult(successful=True) + for ros_param in ros_parameters: + try: + parameter = self._get_component_parameter(ros_param.name) + new_parameter = read_parameter_const(ros_param, parameter) + if not self._validate_parameter(new_parameter): + result.successful = False + result.reason = f"Parameter {ros_param.name} could not be set!" + else: + if isinstance(self._parameter_dict[ros_param.name], str): + self.__setattr__(self._parameter_dict[ros_param.name], new_parameter) + else: + self._parameter_dict[ros_param.name] = new_parameter + except Exception as e: + result.successful = False + result.reason += str(e) + return result def add_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable]): """ Add a predicate to the map of predicates. - :param predicate_name: the name of the associated predicate - :param predicate_value: the value of the predicate as a bool or a callable function + :param predicate_name: The name of the associated predicate + :param predicate_value: The value of the predicate as a bool or a callable function """ if predicate_name in self._predicates.keys(): self.get_logger().debug(f"Predicate {predicate_name} already exists, overwriting.") else: - self._predicate_publishers[predicate_name] = self.create_publisher(Bool, self.__generate_predicate_topic( - predicate_name), 10) + self._predicate_publishers[predicate_name] = self.create_publisher(Bool, + self.__generate_predicate_topic( + predicate_name), 10) self._predicates[predicate_name] = predicate_value def get_predicate(self, predicate_name: str) -> bool: """ Get the value of the predicate given as parameter. If the predicate is not found or the callable function fails, - return false. + this method returns False. - :param predicate_name: the name of the predicate to retrieve from the map of predicates - :return: the value of the predicate as a boolean + :param predicate_name: The name of the predicate to retrieve from the map of predicates + :return: The value of the predicate as a boolean """ if predicate_name not in self._predicates.keys(): self.get_logger().error(f"Predicate {predicate_name} does not exist, returning false.", @@ -110,11 +246,12 @@ def set_predicate(self, predicate_name: str, predicate_value: Union[bool, Callab """ Set the value of the predicate given as parameter, if the predicate is not found does not do anything. - :param predicate_name: the name of the predicate to retrieve from the map of predicates - :param predicate_value: the new value of the predicate as a bool or a callable function + :param predicate_name: The name of the predicate to retrieve from the map of predicates + :param predicate_value: The new value of the predicate as a bool or a callable function """ if predicate_name not in self._predicates.keys(): - self.get_logger().error(f"Cannot set predicate {predicate_name} with a new value because it does not exist.", - throttle_duration_sec=1.0) + self.get_logger().error( + f"Cannot set predicate {predicate_name} with a new value because it does not exist.", + throttle_duration_sec=1.0) return self._predicates[predicate_name] = predicate_value diff --git a/source/modulo_components/modulo_components/exceptions/__init__.py b/source/modulo_components/modulo_components/exceptions/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_components/modulo_components/exceptions/component_exceptions.py b/source/modulo_components/modulo_components/exceptions/component_exceptions.py new file mode 100644 index 000000000..0b9b1434c --- /dev/null +++ b/source/modulo_components/modulo_components/exceptions/component_exceptions.py @@ -0,0 +1,8 @@ +class ComponentError(Exception): + def __init__(self, message): + super().__init__(message) + + +class ComponentParameterError(ComponentError): + def __init__(self, message): + super().__init__(message) diff --git a/source/modulo_components/test/python/test_component_interface_parameters.py b/source/modulo_components/test/python/test_component_interface_parameters.py new file mode 100644 index 000000000..7b7865a52 --- /dev/null +++ b/source/modulo_components/test/python/test_component_interface_parameters.py @@ -0,0 +1,159 @@ +import pytest +import rclpy.exceptions +import rclpy.node +import state_representation as sr +from modulo_components.component_interface import ComponentInterface +from rcl_interfaces.msg import SetParametersResult + +from modulo_components.exceptions.component_exceptions import ComponentParameterError + + +class ComponentInterfaceTest(ComponentInterface): + def __init__(self, node_name, *kargs, **kwargs): + self.validation_return_value = True + super().__init__(node_name, *kargs, **kwargs) + self.validate_was_called = False + self.param = sr.Parameter("test", 1, sr.ParameterType.INT) + + def get_ros_parameter(self, name: str) -> rclpy.Parameter: + return rclpy.node.Node.get_parameter(self, name) + + def set_ros_parameter(self, param: rclpy.Parameter) -> SetParametersResult: + return rclpy.node.Node.set_parameters(self, [param])[0] + + def _validate_parameter(self, parameter: sr.Parameter) -> bool: + self.validate_was_called = True + return self.validation_return_value + + +@pytest.fixture() +def component_interface(ros_context): + yield ComponentInterfaceTest('component_interface') + + +def assert_param_value_equal(component_interface: ComponentInterfaceTest, name: str, value: float): + assert component_interface.get_ros_parameter(name).get_parameter_value().integer_value == value + assert component_interface.get_parameter_value(name) == value + assert component_interface.get_parameter(name).get_value() == value + + +def test_add_parameter(component_interface): + with pytest.raises(ComponentParameterError): + component_interface.get_parameter("test") + with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): + rclpy.node.Node.get_parameter(component_interface, "test") + + component_interface.add_parameter("param", "Test parameter") + assert component_interface.validate_was_called + component_interface.get_parameter("test") + rclpy.node.Node.get_parameter(component_interface, "test") + assert component_interface.param.get_parameter_type() == sr.ParameterType.INT + assert component_interface.param.get_value() == 1 + + +def test_add_parameter_again(component_interface): + with pytest.raises(ComponentParameterError): + component_interface.get_parameter_value("test") + with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): + rclpy.node.Node.get_parameter(component_interface, "test") + + component_interface.add_parameter("param", "Test parameter") + component_interface.validate_was_called = False + component_interface.add_parameter(sr.Parameter("test", 2, sr.ParameterType.INT), "foo") + assert component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 2) + assert component_interface.param.get_value() == 2 + + +def test_add_parameter_again_not_attribute(component_interface): + with pytest.raises(ComponentParameterError): + component_interface.get_parameter("test") + with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): + rclpy.node.Node.get_parameter(component_interface, "test") + + component_interface.add_parameter(component_interface.param, "Test parameter") + component_interface.validate_was_called = False + component_interface.add_parameter(sr.Parameter("test", 2, sr.ParameterType.INT), "foo") + assert component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 2) + assert component_interface.param.get_value() == 1 + + +def test_set_parameter(component_interface): + assert not component_interface.validate_was_called + with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): + component_interface.set_parameter_value("test", 1, sr.ParameterType.INT) + assert not component_interface.validate_was_called + with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): + component_interface.get_ros_parameter("test") + + component_interface.add_parameter("param", "Test parameter") + + component_interface.validate_was_called = False + component_interface.set_parameter_value("test", 2, sr.ParameterType.INT) + assert component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 2) + assert component_interface.param.get_value() == 2 + + component_interface.validate_was_called = False + component_interface.validation_return_value = False + with pytest.raises(RuntimeError): + component_interface.set_parameter_value("test", 3, sr.ParameterType.INT) + assert component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 2) + assert component_interface.param.get_value() == 2 + + with pytest.raises(RuntimeError): + component_interface.set_parameter_value("test", "test", sr.ParameterType.STRING) + assert component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 2) + assert component_interface.param.get_value() == 2 + + +def test_set_parameter_ros(component_interface): + component_interface.add_parameter("param", "Test parameter") + + component_interface.validate_was_called = False + result = component_interface.set_ros_parameter(rclpy.Parameter("test", value=2)) + assert component_interface.validate_was_called + assert result.successful + assert_param_value_equal(component_interface, "test", 2) + assert component_interface.param.get_value() == 2 + + component_interface.validate_was_called = False + component_interface.validation_return_value = False + result = component_interface.set_ros_parameter(rclpy.Parameter("test", value=3)) + assert component_interface.validate_was_called + assert not result.successful + assert_param_value_equal(component_interface, "test", 2) + assert component_interface.param.get_value() == 2 + + +def test_get_parameter_description(component_interface): + component_interface.add_parameter(component_interface.param, "Test parameter") + assert component_interface.describe_parameter("test").description == "Test parameter" + + with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): + component_interface.describe_parameter("foo") + + +def test_read_only_parameter(component_interface): + component_interface.add_parameter("param", "Test parameter", True) + assert component_interface.validate_was_called + component_interface.get_parameter("test") + component_interface.get_ros_parameter("test") + assert_param_value_equal(component_interface, "test", 1) + + # TODO read only + component_interface.validate_was_called = False + # with pytest.raises(RuntimeError): + # component_interface.set_parameter_value("test", 2, sr.ParameterType.INT) + # assert not component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 1) + assert component_interface.param.get_value() == 1 + + component_interface.validate_was_called = False + # result = component_interface.set_ros_parameter(rclpy.Parameter("test", value=2)) + # assert not component_interface.validate_was_called + # assert not result.successful + assert_param_value_equal(component_interface, "test", 1) From af23aee8b4d4fc988ba6ed94c091696de15878ac Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 9 Jun 2022 18:17:12 +0200 Subject: [PATCH 35/71] Handle logging and exceptions consistently (#74) * Add new exceptions, deriving from ComponentException * More consistency with exceptions and logging * Update parameter tests because exceptions are not raised anymore * Update docstrings of exceptions * Small corrections * Add logging to add_output * Update python to behave the same * Add contributing guide with bulletpoints about logging and exceptions --- source/modulo_components/CONTRIBUTING..md | 29 +++ .../include/modulo_components/Component.hpp | 5 +- .../modulo_components/ComponentInterface.hpp | 184 ++++++++++++------ .../exceptions/AddSignalException.hpp | 18 ++ .../exceptions/ComponentException.hpp | 18 ++ .../ComponentParameterException.hpp | 18 ++ .../exceptions/LookupTransformException.hpp | 18 ++ .../SignalAlreadyExistsException.hpp | 11 -- .../modulo_components/component_interface.py | 75 ++++--- source/modulo_components/src/Component.cpp | 18 +- .../src/LifecycleComponent.cpp | 2 + .../test_component_interface_parameters.cpp | 28 +-- .../test_component_interface_parameters.py | 9 +- 13 files changed, 305 insertions(+), 128 deletions(-) create mode 100644 source/modulo_components/CONTRIBUTING..md create mode 100644 source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp create mode 100644 source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp create mode 100644 source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp create mode 100644 source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp delete mode 100644 source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp diff --git a/source/modulo_components/CONTRIBUTING..md b/source/modulo_components/CONTRIBUTING..md new file mode 100644 index 000000000..27f5cd4b5 --- /dev/null +++ b/source/modulo_components/CONTRIBUTING..md @@ -0,0 +1,29 @@ +# Contributing + +This document is still work in progress. + +### Exceptions + +This section describes how a component should handle exceptions and errors. + +As a general rule, a component interface methods do not throw an exception or raise an error unless there is no other +meaningful option. More precisely, only non-void public/protected methods throw, i.e. all setters and `add_xxx` methods +do not throw but catch all exceptions and log an error. + +If an exception is thrown, it is either a `ComponentException` (in C++) or a `ComponentError` (in Python) or any +derived exception, such that all exceptions thrown by a component can be caught with those base exceptions (for example +in the periodic `step` function). + +Currently, the following methods throw: +- `get_parameter` +- `get_parameter_value` +- `lookup_transform` + +### Logging + +Similar to the exceptions, the logging of debug, info, and error messages should follow some principles: + +- Methods that catch an exception and are not allow to rethrow, log an error with the exception message +- `add_xxx` methods use non-throttled logging +- Setters and getters as well as all other methods that are expected to be called at a high frequency use throttled + logging \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 8371c1fbc..669270d22 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -85,6 +85,8 @@ inline void Component::add_output( std::string parsed_signal_name = utilities::parse_signal_name(signal_name); this->create_output(parsed_signal_name, data, fixed_topic, default_topic); auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Adding output '" << signal_name << "' with topic name '" << topic_name << "'."); auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair(); switch (message_pair->get_type()) { case MessageType::BOOL: { @@ -139,8 +141,7 @@ inline void Component::add_output( } } } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Failed to add output '" << signal_name << "': " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); } } diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 0c5274342..1ef016260 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -21,7 +21,9 @@ #include #include -#include "modulo_components/exceptions/SignalAlreadyExistsException.hpp" +#include "modulo_components/exceptions/AddSignalException.hpp" +#include "modulo_components/exceptions/ComponentParameterException.hpp" +#include "modulo_components/exceptions/LookupTransformException.hpp" #include "modulo_components/utilities/utilities.hpp" #include "modulo_components/utilities/predicate_variant.hpp" @@ -89,6 +91,7 @@ class ComponentInterface : private NodeT { /** * @brief Get a parameter by name. * @param name The name of the parameter + * @throws ComponentParameterException if the parameter could not be found * @return The ParameterInterface pointer to a Parameter instance */ [[nodiscard]] std::shared_ptr get_parameter(const std::string& name) const; @@ -97,6 +100,7 @@ class ComponentInterface : private NodeT { * @brief Get a parameter value by name. * @tparam T The type of the parameter * @param name The name of the parameter + * @throws ComponentParameterException if the parameter value could not be accessed * @return The value of the parameter */ template @@ -126,14 +130,14 @@ class ComponentInterface : private NodeT { virtual bool validate_parameter(const std::shared_ptr& parameter); /** - * @brief Add a predicate to the map of predicates + * @brief Add a predicate to the map of predicates. * @param predicate_name the name of the associated predicate * @param predicate_value the boolean value of the predicate */ void add_predicate(const std::string& predicate_name, bool predicate_value); /** - * @brief Add a predicate to the map of predicates based on a function to periodically call + * @brief Add a predicate to the map of predicates based on a function to periodically call. * @param predicate_name the name of the associated predicate * @param predicate_function the function to call that returns the value of the predicate */ @@ -142,24 +146,21 @@ class ComponentInterface : private NodeT { /** * @brief Get the logical value of a predicate. * @details If the predicate is not found or the callable function fails, the return value is false. - * @param predicate_name the name of the predicate to retrieve from the - * map of predicates + * @param predicate_name the name of the predicate to retrieve from the map of predicates * @return the value of the predicate as a boolean */ [[nodiscard]] bool get_predicate(const std::string& predicate_name); /** - * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything - * @param predicate_name the name of the predicate to retrieve from the - * map of predicates + * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything. + * @param predicate_name the name of the predicate to retrieve from the map of predicates * @param predicate_value the new value of the predicate */ void set_predicate(const std::string& predicate_name, bool predicate_value); /** - * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything - * @param predicate_name the name of the predicate to retrieve from the - * map of predicates + * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything. + * @param predicate_name the name of the predicate to retrieve from the map of predicates * @param predicate_function the function to call that returns the value of the predicate */ void set_predicate(const std::string& predicate_name, const std::function& predicate_function); @@ -211,12 +212,12 @@ class ComponentInterface : private NodeT { void add_tf_listener(); /** - * @brief Helper function to parse the signal name and add an unconfigured - * PublisherInterface to the map of outputs. + * @brief Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of outputs. * @tparam DataT Type of the data pointer * @param signal_name Name of the output signal * @param data Data to transmit on the output signal * @param fixed_topic If true, the topic name of the output signal is fixed + * @throws AddSignalException if the output could not be created (empty name, already registered) */ template void create_output( @@ -248,6 +249,7 @@ class ComponentInterface : private NodeT { * @param reference_frame_name The desired reference frame of the transform * @param time_point The time at which the value of the transform is desired (default: 0, will get the latest) * @param duration How long to block the lookup call before failing + * @throws LookupTransformException if TF buffer/listener are unconfigured or if the lookupTransform call failed * @return If it exists, the requested transform */ [[nodiscard]] state_representation::CartesianPose lookup_transform( @@ -354,13 +356,22 @@ template inline void ComponentInterface::add_parameter( const std::string& name, const T& value, const std::string& description, bool read_only ) { + if (name.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed to add parameter: Provide a non empty string as a name."); + return; + } this->add_parameter(state_representation::make_shared_parameter(name, value), description, read_only); } template template inline T ComponentInterface::get_parameter_value(const std::string& name) const { - return this->parameter_map_.template get_parameter_value(name); + try { + return this->parameter_map_.template get_parameter_value(name); + } catch (const state_representation::exceptions::InvalidParameterException& ex) { + throw exceptions::ComponentParameterException( + "Failed to get parameter value of parameter '" + name + "': " + ex.what()); + } } template @@ -368,32 +379,48 @@ inline void ComponentInterface::add_parameter( const std::shared_ptr& parameter, const std::string& description, bool read_only ) { - auto ros_param = modulo_new_core::translators::write_parameter(parameter); - if (!NodeT::has_parameter(parameter->get_name())) { - parameter_map_.set_parameter(parameter); - rcl_interfaces::msg::ParameterDescriptor descriptor; - descriptor.description = description; - descriptor.read_only = read_only; - NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); - } else { - NodeT::set_parameter(ros_param); + try { + auto ros_param = modulo_new_core::translators::write_parameter(parameter); + if (!NodeT::has_parameter(parameter->get_name())) { + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); + parameter_map_.set_parameter(parameter); + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.description = description; + descriptor.read_only = read_only; + NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); + } else { + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Parameter '" << parameter->get_name() << "' already exists, overwriting."); + NodeT::set_parameter(ros_param); + } + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add parameter '" << parameter->get_name() << "': " << ex.what()); } } template inline std::shared_ptr ComponentInterface::get_parameter(const std::string& name) const { - return this->parameter_map_.get_parameter(name); + try { + return this->parameter_map_.get_parameter(name); + } catch (const state_representation::exceptions::InvalidParameterException& ex) { + throw exceptions::ComponentParameterException("Failed to get parameter '" + name + "': " + ex.what()); + } } template template inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { - rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter( - modulo_new_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); - if (!result.successful) { - // TODO not throw here - throw state_representation::exceptions::InvalidParameterException(result.reason); + try { + rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter( + modulo_new_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); + if (!result.successful) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Failed to set parameter value of parameter '" << name << "': " << result.reason); + } + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Failed to set parameter value of parameter '" << name << "': " << ex.what()); } } @@ -447,9 +474,14 @@ template inline void ComponentInterface::add_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate ) { + if (name.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed to add predicate: Provide a non empty string as a name."); + return; + } if (this->predicates_.find(name) != this->predicates_.end()) { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate " << name << " already exists, overwriting."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate '" << name << "' already exists, overwriting."); } else { + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding predicate '" << name << "'."); this->predicate_publishers_.insert_or_assign( name, this->template create_publisher( utilities::generate_predicate_topic(this->get_name(), name), this->qos_ @@ -463,8 +495,9 @@ inline bool ComponentInterface::get_predicate(const std::string& predicat auto predicate_iterator = this->predicates_.find(predicate_name); // if there is no predicate with that name simply return false with an error message if (predicate_iterator == this->predicates_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 10000, - "Predicate " << predicate_name << " does not exists, returning false."); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Failed to get predicate '" << predicate_name + << "': Predicate does not exists, returning false."); return false; } // try to get the value from the variant as a bool @@ -477,9 +510,10 @@ inline bool ComponentInterface::get_predicate(const std::string& predicat bool value = false; try { value = (callback_function)(); - } catch (const std::exception& e) { + } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Error while evaluating the callback function: " << e.what()); + "Failed to evaluate callback of predicate'" << predicate_name << "', returning false:" + << ex.what()); } return value; } @@ -491,7 +525,7 @@ inline void ComponentInterface::set_variant_predicate( auto predicate_iterator = this->predicates_.find(name); if (predicate_iterator == this->predicates_.end()) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Cannot set predicate " << name << " with a new value because it does not exist."); + "Failed to set predicate '" << name << "': Predicate does not exist."); return; } predicate_iterator->second = predicate; @@ -518,8 +552,13 @@ inline void ComponentInterface::add_input( using namespace modulo_new_core::communication; try { std::string parsed_signal_name = utilities::parse_signal_name(signal_name); + if (parsed_signal_name.empty()) { + throw exceptions::AddSignalException( + "Failed to add input '" + signal_name + "': Parsed signal name is empty." + ); + } if (this->inputs_.find(parsed_signal_name) != this->inputs_.end()) { - throw exceptions::SignalAlreadyExistsException("Input with name '" + signal_name + "' already exists"); + throw exceptions::AddSignalException("Failed to add input '" + signal_name + "': Input already exists"); } std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; this->add_parameter( @@ -527,6 +566,8 @@ inline void ComponentInterface::add_input( fixed_topic ); topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Adding input '" << signal_name << "' with topic name '" << topic_name << "'."); auto message_pair = make_shared_message_pair(data, this->get_clock()); std::shared_ptr subscription_interface; switch (message_pair->get_type()) { @@ -589,8 +630,13 @@ inline void ComponentInterface::add_input( using namespace modulo_new_core::communication; try { std::string parsed_signal_name = utilities::parse_signal_name(signal_name); + if (parsed_signal_name.empty()) { + throw exceptions::AddSignalException( + "Failed to add input '" + signal_name + "': Parsed signal name is empty." + ); + } if (this->inputs_.find(parsed_signal_name) != this->inputs_.end()) { - throw exceptions::SignalAlreadyExistsException("Input with name '" + signal_name + "' already exists"); + throw exceptions::AddSignalException("Failed to add input '" + signal_name + "': Input already exists"); } std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; this->add_parameter( @@ -598,6 +644,8 @@ inline void ComponentInterface::add_input( fixed_topic ); topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Adding input '" << signal_name << "' with topic name '" << topic_name << "'."); auto subscription = NodeT::template create_subscription(topic_name, this->qos_, callback); auto subscription_interface = std::make_shared>()->create_subscription_interface(subscription); @@ -610,33 +658,53 @@ inline void ComponentInterface::add_input( template inline void ComponentInterface::add_periodic_function(const std::string& name, const std::function& callback) { + if (name.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed to add periodic function: Provide a non empty string as a name."); + return; + } if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Daemon callback " << name << " already exists, overwriting."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Periodic function '" << name << "' already exists, overwriting."); + } else { + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding periodic function '" << name << "'."); } this->periodic_callbacks_.template insert_or_assign(name, callback); } template inline void ComponentInterface::add_tf_broadcaster() { - this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); + if (this->tf_broadcaster_ == nullptr) { + RCLCPP_DEBUG(this->get_logger(), "Adding TF broadcaster."); + this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); + } else { + RCLCPP_DEBUG(this->get_logger(), "TF broadcaster already exists."); + } } template inline void ComponentInterface::add_tf_listener() { - this->tf_buffer_ = std::make_shared(this->get_clock()); - this->tf_listener_ = std::make_shared(*this->tf_buffer_); + if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { + RCLCPP_DEBUG(this->get_logger(), "Adding TF buffer and listener."); + this->tf_buffer_ = std::make_shared(this->get_clock()); + this->tf_listener_ = std::make_shared(*this->tf_buffer_); + } else { + RCLCPP_DEBUG(this->get_logger(), "TF buffer and listener already exist."); + } } template inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { - // TODO: throw here? if (this->tf_broadcaster_ == nullptr) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock, 1000, "No tf broadcaster"); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock, 1000, + "Failed to send transform: No tf broadcaster configured."); + return; + } + try { + geometry_msgs::msg::TransformStamped tf_message; + modulo_new_core::translators::write_msg(tf_message, transform, this->get_clock()->now()); + this->tf_broadcaster_->sendTransform(tf_message); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock, 1000, "Failed to send transform: " << ex.what()); } - geometry_msgs::msg::TransformStamped tf_message; - modulo_new_core::translators::write_msg(tf_message, transform, this->get_clock()->now()); - this->tf_broadcaster_->sendTransform(tf_message); } template @@ -644,14 +712,16 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra const std::string& frame_name, const std::string& reference_frame_name, const tf2::TimePoint& time_point, const tf2::Duration& duration ) const { - // TODO: throw here? if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock, 1000, "No tf buffer / listener"); + throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured."); } geometry_msgs::msg::TransformStamped transform; state_representation::CartesianPose result(frame_name, reference_frame_name); - // TODO: catch exception and rethrow - transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); + try { + transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); + } catch (const tf2::TransformException& ex) { + throw exceptions::LookupTransformException(std::string("Failed to lookup transform: ").append(ex.what())); + } modulo_new_core::translators::read_msg(result, transform); return result; } @@ -677,19 +747,19 @@ inline void ComponentInterface::publish_outputs() { publisher->publish(); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Could not publish output " << signal << ": " << ex.what()); + "Failed to publish output '" << signal << "': " << ex.what()); } } } template inline void ComponentInterface::evaluate_periodic_callbacks() { - for (const auto& [daemon, callback]: this->periodic_callbacks_) { + for (const auto& [name, callback]: this->periodic_callbacks_) { try { callback(); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Could not evaluate daemon callback " << daemon << ": " << ex.what()); + "Failed to evaluate periodic function callback '" << name << "': " << ex.what()); } } } @@ -701,8 +771,11 @@ inline void ComponentInterface::create_output( const std::string& default_topic ) { using namespace modulo_new_core::communication; + if (signal_name.empty()) { + throw exceptions::AddSignalException("Failed to add output: Provide a non empty string as a name."); + } if (this->outputs_.find(signal_name) != this->outputs_.end()) { - throw exceptions::SignalAlreadyExistsException("Output with name '" + signal_name + "' already exists"); + throw exceptions::AddSignalException("Output with name '" + signal_name + "' already exists"); } auto message_pair = make_shared_message_pair(data, this->get_clock()); this->outputs_.insert_or_assign( @@ -715,6 +788,7 @@ inline void ComponentInterface::create_output( template inline void ComponentInterface::raise_error() { + RCLCPP_DEBUG(this->get_logger(), "raise_error called: Setting predicate 'in_error_state' to true."); this->set_predicate("in_error_state", true); } diff --git a/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp b/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp new file mode 100644 index 000000000..528cb5e77 --- /dev/null +++ b/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "modulo_components/exceptions/ComponentException.hpp" + +namespace modulo_components::exceptions { + +/** + * @class AddSignalException + * @brief An exception class to notify errors when adding a signal. + * @details This is an exception class to be thrown if there is a + * problem while adding a signal to the component. + */ +class AddSignalException : public ComponentException { +public: + explicit AddSignalException(const std::string& msg) : ComponentException(msg) {}; +}; + +}// namespace modulo_components::exceptions \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp b/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp new file mode 100644 index 000000000..539fb8b03 --- /dev/null +++ b/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include +#include + +namespace modulo_components::exceptions { + +/** + * @class ComponentException + * @brief A base class for all component exceptions. + * @details This inherits from std::runtime_exception. + */ +class ComponentException : public std::runtime_error { +public: + explicit ComponentException(const std::string& msg) : std::runtime_error(msg) {}; +}; + +}// namespace modulo_components::exceptions \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp b/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp new file mode 100644 index 000000000..e74b5b9e1 --- /dev/null +++ b/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "modulo_components/exceptions/ComponentException.hpp" + +namespace modulo_components::exceptions { + +/** + * @class ComponentParameterException + * @brief An exception class to notify errors with component parameters. + * @details This is an exception class to be thrown if there is a problem + * with component parameters (overriding, inconsistent types, undeclared, ...). + */ +class ComponentParameterException : public ComponentException { +public: + explicit ComponentParameterException(const std::string& msg) : ComponentException(msg) {}; +}; + +}// namespace modulo_components::exceptions \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp b/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp new file mode 100644 index 000000000..655fb8f15 --- /dev/null +++ b/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp @@ -0,0 +1,18 @@ +#pragma once + +#include "modulo_components/exceptions/ComponentException.hpp" + +namespace modulo_components::exceptions { + +/** + * @class LookupTransformException + * @brief An exception class to notify an error while looking up TF transforms. + * @details This is an exception class to be thrown if there is a problem + * with looking up a TF transform (unconfigured buffer/listener, TF2 exception). + */ +class LookupTransformException : public ComponentException { +public: + explicit LookupTransformException(const std::string& msg) : ComponentException(msg) {}; +}; + +}// namespace modulo_components::exceptions \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp b/source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp deleted file mode 100644 index 80e698635..000000000 --- a/source/modulo_components/include/modulo_components/exceptions/SignalAlreadyExistsException.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo_components::exceptions { -class SignalAlreadyExistsException : public std::runtime_error { -public: - explicit SignalAlreadyExistsException(const std::string& msg) : std::runtime_error(msg) {}; -}; -} \ No newline at end of file diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 132d7b87b..ad3510fa2 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -95,21 +95,24 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r if isinstance(attr, sr.Parameter): sr_parameter = attr else: - raise TypeError("The attribute with the provided name does not contain a Parameter object.") + raise TypeError( + f"The attribute with the provided name '{parameter}' does not contain a Parameter object.") else: raise TypeError("Provide either a state_representation.Parameter object or a string " "containing the name of the attribute that refers to the parameter to add.") - except (AttributeError, TypeError) as e: + ros_param = write_parameter(sr_parameter) + if not self.has_parameter(sr_parameter.get_name()): + self.get_logger().debug(f"Adding parameter '{sr_parameter.get_name()}'.") + self._parameter_dict[sr_parameter.get_name()] = parameter + # TODO ignore override + self.declare_parameter(ros_param.name, ros_param.value, + descriptor=ParameterDescriptor(description=description), + ignore_override=read_only) + else: + self.get_logger().debug(f"Parameter '{sr_parameter.get_name()}' already exists, overwriting.") + self.set_parameters([ros_param]) + except Exception as e: self.get_logger().error(f"Failed to add parameter: {e}") - return - ros_param = write_parameter(sr_parameter) - if not self.has_parameter(sr_parameter.get_name()): - self._parameter_dict[sr_parameter.get_name()] = parameter - # TODO ignore override - self.declare_parameter(ros_param.name, ros_param.value, - descriptor=ParameterDescriptor(description=description), ignore_override=read_only) - else: - self.set_parameters([ros_param]) def get_parameter(self, name: str) -> Union[sr.Parameter, rclpy.parameter.Parameter]: """ @@ -118,37 +121,44 @@ def get_parameter(self, name: str) -> Union[sr.Parameter, rclpy.parameter.Parame dictionary. :param name: The name of the parameter + :raises ComponentParameterError if the parameter does not exist :return: The requested parameter """ - co_filename = sys._getframe().f_back.f_code.co_filename - self.get_logger().debug(f"get_parameter called from {co_filename}") - if "rclpy" in co_filename: - return rclpy.node.Node.get_parameter(self, name) - else: - return self._get_component_parameter(name) + try: + co_filename = sys._getframe().f_back.f_code.co_filename + self.get_logger().debug(f"get_parameter called from {co_filename}") + if "rclpy" in co_filename: + return rclpy.node.Node.get_parameter(self, name) + else: + return self._get_component_parameter(name) + except Exception as e: + raise ComponentParameterError(f"Failed to get parameter '{name}': {e}") def _get_component_parameter(self, name: str) -> sr.Parameter: """ Get the parameter from the parameter dictionary by its name. :param name: The name of the parameter - :return: The parameter, if it exists :raises ComponentParameterError if the parameter does not exist + :return: The parameter, if it exists """ if name not in self._parameter_dict.keys(): - raise ComponentParameterError(f"Failed to get parameter '{name}'") - if isinstance(self._parameter_dict[name], str): - return self.__getattribute__(self._parameter_dict[name]) - else: - return self._parameter_dict[name] + raise ComponentParameterError(f"Parameter '{name}' is not in the dict of parameters") + try: + if isinstance(self._parameter_dict[name], str): + return self.__getattribute__(self._parameter_dict[name]) + else: + return self._parameter_dict[name] + except AttributeError as e: + raise ComponentParameterError(f"{e}") def get_parameter_value(self, name: str) -> T: """ Get the parameter value from the parameter dictionary by its name. :param name: The name of the parameter - :return: The value of the parameter, if the parameter exists :raises ComponentParameterError if the parameter does not exist + :return: The value of the parameter, if the parameter exists """ return self._get_component_parameter(name).get_value() @@ -161,11 +171,15 @@ def set_parameter_value(self, name: str, value: T, parameter_type: sr.ParameterT :param value: The value of the parameter :param parameter_type: The type of the parameter """ - ros_param = write_parameter(sr.Parameter(name, value, parameter_type)) - result = self.set_parameters([ros_param])[0] - if not result.successful: - # TODO not throw here - raise RuntimeError(result.reason) + try: + ros_param = write_parameter(sr.Parameter(name, value, parameter_type)) + result = self.set_parameters([ros_param])[0] + if not result.successful: + self.get_logger().error(f"Failed to set parameter value of parameter '{name}': {result.reason}", + throttle_duration_sec=1.0) + except Exception as e: + self.get_logger().error(f"Failed to set parameter value of parameter '{name}': {e}", + throttle_duration_sec=1.0) def _validate_parameter(self, parameter: sr.Parameter) -> bool: """ @@ -211,9 +225,12 @@ def add_predicate(self, predicate_name: str, predicate_value: Union[bool, Callab :param predicate_name: The name of the associated predicate :param predicate_value: The value of the predicate as a bool or a callable function """ + if not predicate_name: + self.get_logger().error("Failed to add predicate: Provide a non empty string as a name.") if predicate_name in self._predicates.keys(): self.get_logger().debug(f"Predicate {predicate_name} already exists, overwriting.") else: + self.get_logger().debug(f"Adding predicate '{predicate_name}'.") self._predicate_publishers[predicate_name] = self.create_publisher(Bool, self.__generate_predicate_topic( predicate_name), 10) diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 912e8c9fc..ab8637a99 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -14,15 +14,19 @@ Component::Component(const rclcpp::NodeOptions& node_options, bool start_thread) } void Component::step() { - this->publish_predicates(); - this->publish_outputs(); - this->evaluate_periodic_callbacks(); + try { + this->publish_predicates(); + this->publish_outputs(); + this->evaluate_periodic_callbacks(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to execute step function:" << ex.what()); + this->raise_error(); + } } void Component::start_thread() { if (this->started_) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Run thread for component '" << this->get_name() << "has already been started"); + RCLCPP_ERROR(this->get_logger(), "Failed to start run thread: Thread has already been started."); return; } this->started_ = true; @@ -36,11 +40,11 @@ void Component::run() { return; } } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "Failed to run component '" << this->get_name() << "': " << ex.what()); + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to run the execute function: " << ex.what()); this->raise_error(); return; } + RCLCPP_DEBUG(this->get_logger(), "Execution finished, setting is_finished predicate to true"); this->set_predicate("is_finished", true); } diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 6f04ee88e..dc54cff47 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -20,6 +20,8 @@ bool LifecycleComponent::configure_outputs() { for (auto& [name, interface]: this->outputs_) { try { auto topic_name = this->get_parameter_value(name + "_topic"); + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Configuring output '" << name << "' with topic name '" << topic_name << "'."); auto message_pair = interface->get_message_pair(); switch (message_pair->get_type()) { case MessageType::BOOL: { diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index 31b1b6066..d694172c2 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -1,8 +1,6 @@ #include -#include - -#include "modulo_components/ComponentInterface.hpp" +#include "modulo_components/exceptions/ComponentParameterException.hpp" #include "modulo_new_core/EncodedState.hpp" #include "test_modulo_components/component_public_interfaces.hpp" @@ -47,8 +45,7 @@ using NodeTypes = ::testing::Typescomponent_->get_parameter("test"), - state_representation::exceptions::InvalidParameterException); + EXPECT_THROW(auto discard = this->component_->get_parameter("test"), exceptions::ComponentParameterException); EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); this->component_->add_parameter(this->param_, "Test parameter"); @@ -60,8 +57,7 @@ TYPED_TEST(ComponentInterfaceParameterTest, AddParameter) { } TYPED_TEST(ComponentInterfaceParameterTest, AddNameValueParameter) { - EXPECT_THROW(auto discard = this->component_->get_parameter("test"), - state_representation::exceptions::InvalidParameterException); + EXPECT_THROW(auto discard = this->component_->get_parameter("test"), exceptions::ComponentParameterException); EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); this->component_->add_parameter("test", 1, "Test parameter"); @@ -73,9 +69,7 @@ TYPED_TEST(ComponentInterfaceParameterTest, AddNameValueParameter) { } TYPED_TEST(ComponentInterfaceParameterTest, AddParameterAgain) { - EXPECT_THROW(auto - discard = this->component_->get_parameter("test"), - state_representation::exceptions::InvalidParameterException); + EXPECT_THROW(auto discard = this->component_->get_parameter("test"), exceptions::ComponentParameterException); EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); this->component_->add_parameter(this->param_, "Test parameter"); @@ -88,8 +82,9 @@ TYPED_TEST(ComponentInterfaceParameterTest, AddParameterAgain) { } TYPED_TEST(ComponentInterfaceParameterTest, SetParameter) { - // setting before adding should not work - EXPECT_THROW(this->component_->set_parameter_value("test", 1), rclcpp::exceptions::ParameterNotDeclaredException); + // setting before adding should not work and parameter should not be created + this->component_->set_parameter_value("test", 1); + EXPECT_THROW(this->component_->template get_parameter_value("test"), exceptions::ComponentParameterException); // validation should not be invoked as the parameter did not exist EXPECT_FALSE(this->component_->validate_parameter_was_called); @@ -107,15 +102,13 @@ TYPED_TEST(ComponentInterfaceParameterTest, SetParameter) { // If the validation function returns false, setting the parameter value should _not_ update the referenced value this->component_->validate_parameter_was_called = false; this->component_->validate_parameter_return_value = false; - EXPECT_THROW(this->component_->set_parameter_value("test", 3), - state_representation::exceptions::InvalidParameterException); + this->component_->set_parameter_value("test", 3); EXPECT_TRUE(this->component_->validate_parameter_was_called); this->template expect_parameter_value(2); EXPECT_EQ(this->param_->get_value(), 2); // Setting a value with an incompatible type should not update the parameter - EXPECT_THROW(this->component_->template set_parameter_value("test", "foo"), - state_representation::exceptions::InvalidParameterException); + this->component_->template set_parameter_value("test", "foo"); EXPECT_TRUE(this->component_->validate_parameter_was_called); this->template expect_parameter_value(2); EXPECT_EQ(this->param_->get_value(), 2); @@ -159,8 +152,7 @@ TYPED_TEST(ComponentInterfaceParameterTest, ReadOnlyParameter) { // Trying to set the value of the read-only parameter should fail before the validation step this->component_->validate_parameter_was_called = false; - EXPECT_THROW(this->component_->set_parameter_value("test", 2), - state_representation::exceptions::InvalidParameterException); + this->component_->set_parameter_value("test", 2); EXPECT_FALSE(this->component_->validate_parameter_was_called); this->template expect_parameter_value(1); EXPECT_EQ(this->param_->get_value(), 1); diff --git a/source/modulo_components/test/python/test_component_interface_parameters.py b/source/modulo_components/test/python/test_component_interface_parameters.py index 7b7865a52..e7b68c266 100644 --- a/source/modulo_components/test/python/test_component_interface_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_parameters.py @@ -81,8 +81,7 @@ def test_add_parameter_again_not_attribute(component_interface): def test_set_parameter(component_interface): assert not component_interface.validate_was_called - with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): - component_interface.set_parameter_value("test", 1, sr.ParameterType.INT) + component_interface.set_parameter_value("test", 1, sr.ParameterType.INT) assert not component_interface.validate_was_called with pytest.raises(rclpy.exceptions.ParameterNotDeclaredException): component_interface.get_ros_parameter("test") @@ -97,14 +96,12 @@ def test_set_parameter(component_interface): component_interface.validate_was_called = False component_interface.validation_return_value = False - with pytest.raises(RuntimeError): - component_interface.set_parameter_value("test", 3, sr.ParameterType.INT) + component_interface.set_parameter_value("test", 3, sr.ParameterType.INT) assert component_interface.validate_was_called assert_param_value_equal(component_interface, "test", 2) assert component_interface.param.get_value() == 2 - with pytest.raises(RuntimeError): - component_interface.set_parameter_value("test", "test", sr.ParameterType.STRING) + component_interface.set_parameter_value("test", "test", sr.ParameterType.STRING) assert component_interface.validate_was_called assert_param_value_equal(component_interface, "test", 2) assert component_interface.param.get_value() == 2 From 70d15877d760070b3c6f1e0ddab0378bc9fcf555 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Mon, 13 Jun 2022 10:48:09 +0200 Subject: [PATCH 36/71] Build and test packages in CI (#76) * Add build test action and workflow * Add build test action to use Dockerfile and entrypoint script to build and test each package in the source folder. Because of the limitations of GitHub docker actions syntax, some of the following compromises were made: * * Hardcode base image tag; build arg cannot be used because of limited action syntax, and ENV cannot be set before FROM context * * Invoke entrypoint as ros2 user with bash shell explicitly * * Write ros2 home path explicitly. GitHub CI overwrites HOME variable as /github/home, so the path /home/ros2 is set manually * Upgrade to control libraries 6.0 and use find_package for clproto 6.0 Co-authored-by: domire8 <71256590+domire8@users.noreply.github.com> --- .../actions/build-test-galactic/Dockerfile | 7 ++++ .../actions/build-test-galactic/action.yml | 5 +++ .../actions/build-test-galactic/entrypoint.sh | 34 +++++++++++++++++++ .github/workflows/build-test.yml | 23 +++++++++++++ source/modulo_components/CMakeLists.txt | 4 +-- source/modulo_new_core/CMakeLists.txt | 4 +-- 6 files changed, 73 insertions(+), 4 deletions(-) create mode 100644 .github/actions/build-test-galactic/Dockerfile create mode 100644 .github/actions/build-test-galactic/action.yml create mode 100755 .github/actions/build-test-galactic/entrypoint.sh create mode 100644 .github/workflows/build-test.yml diff --git a/.github/actions/build-test-galactic/Dockerfile b/.github/actions/build-test-galactic/Dockerfile new file mode 100644 index 000000000..d0e9f7dee --- /dev/null +++ b/.github/actions/build-test-galactic/Dockerfile @@ -0,0 +1,7 @@ +# https://docs.github.com/en/actions/creating-actions/dockerfile-support-for-github-actions +FROM ghcr.io/aica-technology/ros2-control-libraries:galactic + +# Copy and set the entrypoint commands to execute when the container starts, +# explicilty invoking as ros2 user and with bash interpreter. +COPY entrypoint.sh /entrypoint.sh +ENTRYPOINT ["sudo", "su", "-", "ros2", "-c", "/bin/bash", "-c", "/entrypoint.sh"] diff --git a/.github/actions/build-test-galactic/action.yml b/.github/actions/build-test-galactic/action.yml new file mode 100644 index 000000000..69747bc9f --- /dev/null +++ b/.github/actions/build-test-galactic/action.yml @@ -0,0 +1,5 @@ +name: 'Build and Test (Galactic)' +description: 'Build the source packages and run all unit tests' +runs: + using: 'docker' + image: 'Dockerfile' \ No newline at end of file diff --git a/.github/actions/build-test-galactic/entrypoint.sh b/.github/actions/build-test-galactic/entrypoint.sh new file mode 100755 index 000000000..1b162b174 --- /dev/null +++ b/.github/actions/build-test-galactic/entrypoint.sh @@ -0,0 +1,34 @@ +#!/bin/bash + +STEP=1 +build_and_test() { + PACKAGE=$1 + + echo ">>> ${STEP}: Building ${PACKAGE}..." + cp -r /github/workspace/source/"${PACKAGE}" ./src/"${PACKAGE}" + if ! colcon build --packages-select "${PACKAGE}"; then + echo ">>> [ERROR] Build stage ${STEP} failed!" + exit "${STEP}" + fi + echo ">>> Build stage ${STEP} completed successfully!" + STEP=$((STEP+1)) + + echo ">>> ${STEP}: Testing ${PACKAGE}..." + if ! colcon test --packages-select "${PACKAGE}" --return-code-on-test-failure; then + colcon test-result --verbose + echo ">>> [ERROR] Test stage ${STEP} failed!" + exit "${STEP}" + fi + echo ">>> Test stage ${STEP} completed successfully!" + STEP=$((STEP+1)) +} + +source /opt/ros/"${ROS_DISTRO}"/setup.bash +cd /home/ros2/ros2_ws + +build_and_test modulo_new_core +build_and_test modulo_core +build_and_test modulo_components + +echo ">>> All build and test stages completed successfully!" +exit 0 diff --git a/.github/workflows/build-test.yml b/.github/workflows/build-test.yml new file mode 100644 index 000000000..52f0e8de5 --- /dev/null +++ b/.github/workflows/build-test.yml @@ -0,0 +1,23 @@ +name: Build and Test + +# Run workflow on pushes to main and develop branches, on any pull request, or by manual dispatch +on: + push: + branches: + - main + - develop + pull_request: + workflow_dispatch: + +# Define the build test jobs +jobs: + build-test-galactic: + runs-on: ubuntu-latest + name: Galactic build and test + steps: + # First check out the repository + - name: Checkout + uses: actions/checkout@v2 + # Load the repository build-test action + - name: Build and Test + uses: ./.github/actions/build-test-galactic \ No newline at end of file diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 084f66b23..6b242b6fb 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -21,8 +21,8 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 5.1.0 REQUIRED COMPONENTS state_representation) -find_library(clproto REQUIRED) +find_package(control_libraries 6.0.0 REQUIRED COMPONENTS state_representation) +find_package(clproto 6.0.0 REQUIRED) include_directories(include) diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index f755aa6f8..629831c9a 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -21,8 +21,8 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 5.0.0 REQUIRED COMPONENTS state_representation) -find_library(clproto REQUIRED) +find_package(control_libraries 6.0.0 REQUIRED COMPONENTS state_representation) +find_package(clproto 6.0.0 REQUIRED) include_directories(include) From 8ef9af75691c98dd6beaa5c455479fce37c860ae Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Mon, 13 Jun 2022 11:30:12 +0200 Subject: [PATCH 37/71] TF improvements (#77) * Convert stamp field in writers * Change TF broadcaster to publisher and fix TF methods * Update cpp tests * Add TF methods and tests to python * Make predicate publisher always a rclcpp Publisher * Use galactic tag again for base image * Make if statement more readable Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> * Add TODO for static tf broadcaster Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- build-server.sh | 2 +- .../include/modulo_components/Component.hpp | 2 + .../modulo_components/ComponentInterface.hpp | 87 +++++++++++++++---- .../modulo_components/component_interface.py | 76 ++++++++++++++-- .../exceptions/component_exceptions.py | 5 ++ .../component_public_interfaces.hpp | 5 ++ .../test/cpp/test_component_interface.cpp | 14 ++- .../test/python/test_component_interface.py | 20 ++++- .../translators/message_writers.py | 2 +- .../python_tests/translators/test_messages.py | 10 +-- 10 files changed, 189 insertions(+), 34 deletions(-) diff --git a/build-server.sh b/build-server.sh index ed9a726d8..bc89003e1 100755 --- a/build-server.sh +++ b/build-server.sh @@ -1,5 +1,5 @@ #!/bin/bash -BASE_TAG=galactic-devel +BASE_TAG=galactic IMAGE_NAME=epfl-lasa/modulo IMAGE_TAG=latest diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 669270d22..639d23aee 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -70,6 +70,8 @@ class Component : public ComponentInterface { using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_periodic_callbacks; + using ComponentInterface::activate_tf_broadcaster; + using ComponentInterface::deactivate_tf_broadcaster; std::thread run_thread_; ///< The execution thread of the component bool started_; ///< Flag that indicates if execution has started or not diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 1ef016260..4d43a1660 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -6,9 +6,10 @@ #include #include +#include +#include #include #include -#include #include #include @@ -211,6 +212,16 @@ class ComponentInterface : private NodeT { */ void add_tf_listener(); + /** + * @brief Activate the transform broadcaster (for LifecycleComponents). + */ + void activate_tf_broadcaster(); + + /** + * @brief Deactivate the transform broadcaster (for LifecycleComponents). + */ + void deactivate_tf_broadcaster(); + /** * @brief Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of outputs. * @tparam DataT Type of the data pointer @@ -255,7 +266,7 @@ class ComponentInterface : private NodeT { [[nodiscard]] state_representation::CartesianPose lookup_transform( const std::string& frame_name, const std::string& reference_frame_name = "world", const tf2::TimePoint& time_point = tf2::TimePoint(std::chrono::microseconds(0)), - const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10))) const; + const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10))); /** * @brief Helper function to publish all predicates. @@ -324,7 +335,8 @@ class ComponentInterface : private NodeT { std::shared_ptr step_timer_; ///< Timer for the step function std::shared_ptr tf_buffer_; ///< TF buffer std::shared_ptr tf_listener_; ///< TF listener - std::shared_ptr tf_broadcaster_; ///< TF broadcaster + std::shared_ptr> tf_broadcaster_; ///< TF broadcaster + // TODO maybe add a static tf broadcaster }; template @@ -482,10 +494,11 @@ inline void ComponentInterface::add_variant_predicate( RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate '" << name << "' already exists, overwriting."); } else { RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding predicate '" << name << "'."); + auto publisher = this->template create_publisher( + utilities::generate_predicate_topic(this->get_name(), name), this->qos_ + ); this->predicate_publishers_.insert_or_assign( - name, this->template create_publisher( - utilities::generate_predicate_topic(this->get_name(), name), this->qos_ - )); + name, std::static_pointer_cast>(publisher)); } this->predicates_.insert_or_assign(name, predicate); } @@ -674,7 +687,8 @@ template inline void ComponentInterface::add_tf_broadcaster() { if (this->tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_logger(), "Adding TF broadcaster."); - this->tf_broadcaster_ = std::make_shared(this->shared_from_this()); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); + this->tf_broadcaster_ = this->template create_publisher("tf", this->qos_); } else { RCLCPP_DEBUG(this->get_logger(), "TF broadcaster already exists."); } @@ -684,6 +698,7 @@ template inline void ComponentInterface::add_tf_listener() { if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { RCLCPP_DEBUG(this->get_logger(), "Adding TF buffer and listener."); + console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); this->tf_buffer_ = std::make_shared(this->get_clock()); this->tf_listener_ = std::make_shared(*this->tf_buffer_); } else { @@ -691,19 +706,54 @@ inline void ComponentInterface::add_tf_listener() { } } +template +inline void ComponentInterface::activate_tf_broadcaster() { + if (this->publisher_type_ != modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER) { + return; + } + try { + RCLCPP_DEBUG(this->get_logger(), "Activating TF broadcaster."); + auto publisher = std::dynamic_pointer_cast>( + this->tf_broadcaster_ + ); + publisher->on_activate(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to activate TF broadcaster: " << ex.what()); + } +} + +template +inline void ComponentInterface::deactivate_tf_broadcaster() { + if (this->publisher_type_ == modulo_new_core::communication::PublisherType::PUBLISHER) { + return; + } + try { + RCLCPP_DEBUG(this->get_logger(), "Deactivating TF broadcaster."); + auto publisher = std::dynamic_pointer_cast>( + this->tf_broadcaster_ + ); + publisher->on_deactivate(); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to deactivate TF broadcaster: " << ex.what()); + } +} + template inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { if (this->tf_broadcaster_ == nullptr) { - RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock, 1000, - "Failed to send transform: No tf broadcaster configured."); + RCLCPP_ERROR_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Failed to send transform: No TF broadcaster configured."); return; } try { - geometry_msgs::msg::TransformStamped tf_message; - modulo_new_core::translators::write_msg(tf_message, transform, this->get_clock()->now()); - this->tf_broadcaster_->sendTransform(tf_message); + geometry_msgs::msg::TransformStamped transform_message; + modulo_new_core::translators::write_msg(transform_message, transform, this->get_clock()->now()); + tf2_msgs::msg::TFMessage tf_message; + tf_message.transforms.emplace_back(transform_message); + this->tf_broadcaster_->publish(tf_message); } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock, 1000, "Failed to send transform: " << ex.what()); + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Failed to send transform: " << ex.what()); } } @@ -711,19 +761,18 @@ template inline state_representation::CartesianPose ComponentInterface::lookup_transform( const std::string& frame_name, const std::string& reference_frame_name, const tf2::TimePoint& time_point, const tf2::Duration& duration -) const { +) { if (this->tf_buffer_ == nullptr || this->tf_listener_ == nullptr) { throw exceptions::LookupTransformException("Failed to lookup transform: To TF buffer / listener configured."); } - geometry_msgs::msg::TransformStamped transform; - state_representation::CartesianPose result(frame_name, reference_frame_name); try { - transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); + state_representation::CartesianPose result(frame_name, reference_frame_name); + auto transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); + modulo_new_core::translators::read_msg(result, transform); + return result; } catch (const tf2::TransformException& ex) { throw exceptions::LookupTransformException(std::string("Failed to lookup transform: ").append(ex.what())); } - modulo_new_core::translators::read_msg(result, transform); - return result; } template diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index ad3510fa2..afa65e5c8 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -3,15 +3,21 @@ import rclpy import state_representation as sr -from modulo_components.exceptions.component_exceptions import ComponentParameterError +import tf2_py +from geometry_msgs.msg import TransformStamped +from modulo_components.exceptions.component_exceptions import ComponentParameterError, LookupTransformError +from modulo_new_core.translators.message_readers import read_stamped_msg +from modulo_new_core.translators.message_writers import write_stamped_msg from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter_const from rcl_interfaces.msg import ParameterDescriptor from rcl_interfaces.msg import SetParametersResult +from rclpy.duration import Duration from rclpy.node import Node +from rclpy.time import Time +from std_msgs.msg import Bool from tf2_ros import TransformBroadcaster from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener -from std_msgs.msg import Bool T = TypeVar('T') @@ -42,10 +48,9 @@ def __init__(self, node_name: str, *kargs, **kwargs): self._parameter_dict = {} self._predicates = {} self._predicate_publishers = {} - # TODO add_XXX - self.__tf_buffer = None - self.__tf_listener = None - self.__tf_broadcaster = None + self.__tf_buffer: Buffer = None + self.__tf_listener: TransformListener = None + self.__tf_broadcaster: TransformBroadcaster = None self.add_on_set_parameters_callback(self.__on_set_parameters_callback) self.add_parameter(sr.Parameter("period", 0.1, sr.ParameterType.DOUBLE), @@ -272,3 +277,62 @@ def set_predicate(self, predicate_name: str, predicate_value: Union[bool, Callab throttle_duration_sec=1.0) return self._predicates[predicate_name] = predicate_value + + def add_tf_broadcaster(self): + """ + Configure a transform broadcaster. + """ + if not self.__tf_broadcaster: + self.get_logger().debug("Adding TF broadcaster.") + self.__tf_broadcaster = TransformBroadcaster(self) + else: + self.get_logger().error("TF broadcaster already exists.") + + def add_tf_listener(self): + """ + Configure a transform buffer and listener. + """ + if not self.__tf_buffer or not self.__tf_listener: + self.get_logger().debug("Adding TF buffer and listener.") + self.__tf_buffer = Buffer() + self.__tf_listener = TransformListener(self.__tf_buffer, self) + else: + self.get_logger().error("TF buffer and listener already exist.") + + def send_transform(self, transform: sr.CartesianPose): + """ + Send a transform to TF. + + :param transform: The transform to send + """ + if not self.__tf_broadcaster: + self.get_logger().error("Failed to send transform: No TF broadcaster configured.", + throttle_duration_sec=1.0) + return + try: + transform_message = TransformStamped() + write_stamped_msg(transform_message, transform, self.get_clock().now()) + self.__tf_broadcaster.sendTransform(transform_message) + except tf2_py.TransformException as e: + self.get_logger().error(f"Failed to send transform: {e}", throttle_duration_sec=1.0) + + def lookup_transform(self, frame_name: str, reference_frame_name="world", time_point=Time(), + duration=Duration(nanoseconds=1e4)) -> sr.CartesianPose: + """ + Look up a transform from TF. + + :param frame_name: The desired frame of the transform + :param reference_frame_name: The desired reference frame of the transform + :param time_point: The time at which the value of the transform is desired (default: 0, will get the latest) + :param duration: How long to block the lookup call before failing + :return: If it exists, the requested transform + """ + if not self.__tf_buffer or not self.__tf_listener: + raise LookupTransformError("Failed to lookup transform: To TF buffer / listener configured.") + try: + result = sr.CartesianPose(frame_name, reference_frame_name) + transform = self.__tf_buffer.lookup_transform(reference_frame_name, frame_name, time_point, duration) + read_stamped_msg(result, transform) + return result + except tf2_py.TransformException as e: + raise LookupTransformError(f"Failed to lookup transform: {e}") diff --git a/source/modulo_components/modulo_components/exceptions/component_exceptions.py b/source/modulo_components/modulo_components/exceptions/component_exceptions.py index 0b9b1434c..80393f2d3 100644 --- a/source/modulo_components/modulo_components/exceptions/component_exceptions.py +++ b/source/modulo_components/modulo_components/exceptions/component_exceptions.py @@ -6,3 +6,8 @@ def __init__(self, message): class ComponentParameterError(ComponentError): def __init__(self, message): super().__init__(message) + + +class LookupTransformError(ComponentError): + def __init__(self, message): + super().__init__(message) diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 4e235f6a6..b7fd3681f 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -27,6 +27,11 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::inputs_; using ComponentInterface::create_output; using ComponentInterface::outputs_; + using ComponentInterface::add_tf_broadcaster; + using ComponentInterface::add_tf_listener; + using ComponentInterface::activate_tf_broadcaster; + using ComponentInterface::send_transform; + using ComponentInterface::lookup_transform; using ComponentInterface::raise_error; using ComponentInterface::get_qos; using ComponentInterface::set_qos; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index c56c43e77..3430d0fb8 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -117,13 +117,25 @@ TYPED_TEST(ComponentInterfaceTest, CreateOutput) { EXPECT_THROW(pub_interface->publish(), modulo_new_core::exceptions::InvalidPointerCastException); } +TYPED_TEST(ComponentInterfaceTest, TF) { + this->component_->add_tf_broadcaster(); + this->component_->add_tf_listener(); + this->component_->activate_tf_broadcaster(); + auto send_tf = state_representation::CartesianPose::Random("test", "world"); + EXPECT_NO_THROW(this->component_->send_transform(send_tf)); + EXPECT_THROW(auto throw_tf = this->component_->lookup_transform("dummy", "world"), exceptions::LookupTransformException); + auto lookup_tf = this->component_->lookup_transform("test", "world"); + auto identity = send_tf * lookup_tf.inverse(); + EXPECT_FLOAT_EQ(identity.data().norm(), 1.); + EXPECT_FLOAT_EQ(abs(identity.get_orientation().w()), 1.); +} + TYPED_TEST(ComponentInterfaceTest, GetSetQoS) { auto qos = rclcpp::QoS(5); this->component_->set_qos(qos); EXPECT_EQ(qos, this->component_->get_qos()); } - TYPED_TEST(ComponentInterfaceTest, RaiseError) { EXPECT_FALSE(this->component_->get_predicate("in_error_state")); this->component_->raise_error(); diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 1e965f179..056ffba19 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -1,6 +1,9 @@ +import numpy as np import pytest - +import rclpy +import state_representation as sr from modulo_components.component_interface import ComponentInterface +from modulo_components.exceptions.component_exceptions import LookupTransformError def raise_(ex): @@ -46,3 +49,18 @@ def test_set_predicate(component_interface): component_interface.add_predicate('bar', True) component_interface.set_predicate('bar', lambda: False) assert not component_interface.get_predicate('bar') + + +def test_tf(component_interface): + component_interface.add_tf_broadcaster() + component_interface.add_tf_listener() + send_tf = sr.CartesianPose().Random("test", "world") + component_interface.send_transform(send_tf) + for i in range(10): + rclpy.spin_once(component_interface) + with pytest.raises(LookupTransformError): + component_interface.lookup_transform("dummy", "world") + lookup_tf = component_interface.lookup_transform("test", "world") + identity = send_tf * lookup_tf.inverse() + assert np.linalg.norm(identity.data()) - 1 < 1e-3 + assert abs(identity.get_orientation().w) - 1 < 1e-3 diff --git a/source/modulo_new_core/modulo_new_core/translators/message_writers.py b/source/modulo_new_core/modulo_new_core/translators/message_writers.py index c3d0ed0f4..caf0e9a8f 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_writers.py +++ b/source/modulo_new_core/modulo_new_core/translators/message_writers.py @@ -100,7 +100,7 @@ def write_stamped_msg(msg: MsgT, state: StateT, time: rclpy.time.Time): write_msg(msg.wrench, state) else: raise RuntimeError("The provided combination of state type and message type is not supported") - msg.header.stamp = time + msg.header.stamp = time.to_msg() msg.header.frame_id = state.get_reference_frame() diff --git a/source/modulo_new_core/test/python_tests/translators/test_messages.py b/source/modulo_new_core/test/python_tests/translators/test_messages.py index b9f869cd8..4f6f2e551 100644 --- a/source/modulo_new_core/test/python_tests/translators/test_messages.py +++ b/source/modulo_new_core/test/python_tests/translators/test_messages.py @@ -38,7 +38,7 @@ def test_accel(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) msg_stamped = geometry_msgs.msg.AccelStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_acceleration()) assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_acceleration()) @@ -61,7 +61,7 @@ def test_pose(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) msg_stamped = geometry_msgs.msg.PoseStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation_coefficients()) @@ -84,7 +84,7 @@ def test_transform(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) msg_stamped = geometry_msgs.msg.TransformStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert msg_stamped.child_frame_id == cart_state.get_name() assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) @@ -109,7 +109,7 @@ def test_twist(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) msg_stamped = geometry_msgs.msg.TwistStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_velocity()) assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_velocity()) @@ -132,7 +132,7 @@ def test_wrench(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) msg_stamped = geometry_msgs.msg.WrenchStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now().to_msg()) + modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) assert msg_stamped.header.frame_id == cart_state.get_reference_frame() assert_np_array_equal(read_xyz(msg.force), cart_state.get_force()) assert_np_array_equal(read_xyz(msg.torque), cart_state.get_torque()) From 2b204056906488ec5a9927431b6a47020c7109d4 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 14 Jun 2022 14:39:15 +0200 Subject: [PATCH 38/71] Lifecycle transitions for LifecycleComponent (#75) * Add lifecycle transitions * Fix namespace comment * Fixes after rebase * Correct docstrings Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> * Transition handling in lifecycle callbacks * Use previous state to validate transitions * Perform cleanup or deactivation actions when configuration or activation fails * Perform deactivation, cleanup and shutdown sequence based on previous state using a switch case and fallthrough * Further revise docstrings and logging statements * Modify visibility of inputs map * Make inputs protected in base interface and hide them in derived component classes so that the LifecycleComponent can clear them during cleanup * Clear signal maps during cleanup * Add on_error transition * Make virtual transition callbacks * Make bool on_xyz transitions virtual to be overridden in derived components * Create private handlers to invoke the virtual functions from the appropriate transitions * Add default topic to add_output Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Co-authored-by: Enrico Eberhard --- .../include/modulo_components/Component.hpp | 1 + .../modulo_components/ComponentInterface.hpp | 3 +- .../modulo_components/LifecycleComponent.hpp | 197 ++++++++++++++- .../src/LifecycleComponent.cpp | 225 +++++++++++++++++- 4 files changed, 412 insertions(+), 14 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 639d23aee..993ea5abb 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -65,6 +65,7 @@ class Component : public ComponentInterface { using rclcpp::Node::create_publisher; using ComponentInterface::create_output; + using ComponentInterface::inputs_; using ComponentInterface::outputs_; using ComponentInterface::qos_; using ComponentInterface::publish_predicates; diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 4d43a1660..187a377e2 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -291,6 +291,8 @@ class ComponentInterface : private NodeT { using NodeT::create_publisher; + std::map> + inputs_; ///< Map of inputs std::map> outputs_; ///< Map of outputs @@ -324,7 +326,6 @@ class ComponentInterface : private NodeT { std::map predicates_; ///< Map of predicates std::map>> predicate_publishers_; ///< Map of predicate publishers - std::map> inputs_; std::map> periodic_callbacks_; ///< Map of periodic function callbacks diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 0d3af76e0..0f2e2dd80 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -26,6 +26,59 @@ class LifecycleComponent : public ComponentInterface - void add_output(const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false); + void add_output( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, + const std::string& default_topic = "" + ); private: + /** + * @brief Transition callback for state 'Configuring'. + * @details on_configure callback is called when the lifecycle component enters the 'Configuring' transition state. + * The component must be in the 'Unconfigured' state. + * Depending on the return value of this function, the component may either transition to the 'Inactive' state + * via the 'configure' transition, stay 'Unconfigured' or go to 'ErrorProcessing'.\n + * TRANSITION_CALLBACK_SUCCESS transitions to 'Inactive'\n + * TRANSITION_CALLBACK_FAILURE transitions to 'Unconfigured'\n + * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to 'ErrorProcessing'\n + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_configure(const rclcpp_lifecycle::State& previous_state) override; + + /** + * @brief Configuration handler. + * @details This method configures outputs and invokes the virtual on_configure callback. + * @return True if configuration is successful, false otherwise + */ + bool configure(); + + /** + * @brief Transition callback for state 'CleaningUp'. + * @details on_cleanup callback is called when the lifecycle component enters the 'CleaningUp' transition state. + * The component must be in the 'Inactive' state. + * Depending on the return value of this function, the component may either transition to the 'Unconfigured' state + * via the 'cleanup' transition or go to 'ErrorProcessing'.\n + * TRANSITION_CALLBACK_SUCCESS transitions to 'Unconfigured'\n + * TRANSITION_CALLBACK_FAILURE, TRANSITION_CALLBACK_ERROR or any uncaught exceptions to 'ErrorProcessing'\n + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_cleanup(const rclcpp_lifecycle::State& previous_state) override; + + /** + * @brief Cleanup handler. + * @details This method resets inputs and outputs and invokes the virtual on_cleanup callback. + * @return True if cleanup is successful, false otherwise + */ + bool cleanup(); + + /** + * @brief Transition callback for state 'Activating'. + * @details on_activate callback is called when the lifecycle component enters the 'Activating' transition state. + * The component must be in the 'Inactive' state. + * Depending on the return value of this function, the component may either transition to the 'Active' state + * via the 'activate' transition, stay 'Inactive' or go to 'ErrorProcessing'.\n + * TRANSITION_CALLBACK_SUCCESS transitions to 'Active'\n + * TRANSITION_CALLBACK_FAILURE transitions to 'Inactive'\n + * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to 'ErrorProcessing'\n + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_activate(const rclcpp_lifecycle::State& previous_state) override; + + /** + * @brief Activation handler. + * @details This method activates outputs and invokes the virtual on_activate callback. + * @return True if activation is successful, false otherwise + */ + bool activate(); + + /** + * @brief Transition callback for state 'Deactivating'. + * @details on_deactivate callback is called when the lifecycle component enters the 'Deactivating' transition state. + * The component must be in the 'Active' state. + * Depending on the return value of this function, the component may either transition to the 'Inactive' state + * via the 'deactivate' transition or go to 'ErrorProcessing'.\n + * TRANSITION_CALLBACK_SUCCESS transitions to 'Inactive'\n + * TRANSITION_CALLBACK_FAILURE, TRANSITION_CALLBACK_ERROR or any uncaught exceptions to 'ErrorProcessing'\n + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + + /** + * @brief Deactivation handler. + * @details This method deactivates outputs and invokes the virtual on_deactivate callback. + * @return True if deactivation is successful, false otherwise + */ + bool deactivate(); + + /** + * @brief Transition callback for state 'ShuttingDown'. + * @details on_shutdown callback is called when the lifecycle component enters the 'ShuttingDown' transition state. + * This transition can be called from the 'Unconfigured', 'Inactive' and 'Active' states. + * Depending on the return value of this function, the component may either transition to the 'Finalized' state + * via the 'shutdown' transition or go to 'ErrorProcessing'.\n + * TRANSITION_CALLBACK_SUCCESS transitions to 'Finalized'\n + * TRANSITION_CALLBACK_FAILURE, TRANSITION_CALLBACK_ERROR or any uncaught exceptions to 'ErrorProcessing'\n + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_shutdown(const rclcpp_lifecycle::State& previous_state) override; + + /** + * @brief Shutdown handler. + * @details This method invokes the virtual on_shutdown callback. + * @return True if shutdown is successful, false otherwise + */ + bool shutdown(); + + /** + * @brief Transition callback for state 'ErrorProcessing'. + * @details on_error callback is called when the lifecycle component enters the 'ErrorProcessing' transition state. + * This transition can originate from any step. + * Depending on the return value of this function, the component may either transition to the 'Unconfigured' state + * or go to 'Finalized'.\n + * TRANSITION_CALLBACK_SUCCESS transitions to 'Unconfigured'\n + * TRANSITION_CALLBACK_FAILURE transitions to 'Finalized'\n + * TRANSITION_CALLBACK_ERROR should not be returned, and any exceptions should be caught and returned as a failure\n + */ + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn + on_error(const rclcpp_lifecycle::State& previous_state) override; + + /** + * @brief Error handler. + * @details This method invokes the virtual on_error callback. + * @return True if error handling is successful, false otherwise + */ + bool handle_error(); + /** * @brief Step function that is called periodically and publishes predicates, - * outputs, evaluates daemon callbacks, and calls the on_step function redefined - * in the derived components. + * outputs, evaluates daemon callbacks, and calls the on_step function. */ void step() override; @@ -63,8 +235,14 @@ class LifecycleComponent : public ComponentInterface::create_output; + using ComponentInterface::inputs_; using ComponentInterface::outputs_; using ComponentInterface::qos_; using ComponentInterface::publish_predicates; @@ -72,15 +250,22 @@ class LifecycleComponent : public ComponentInterface::evaluate_periodic_callbacks; }; +inline void LifecycleComponent::on_step() {} + template -inline void -LifecycleComponent::add_output(const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic) { +inline void LifecycleComponent::add_output( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, + const std::string& default_topic +) { try { std::string parsed_signal_name = utilities::parse_signal_name(signal_name); - this->create_output(parsed_signal_name, data, fixed_topic); + this->create_output(parsed_signal_name, data, fixed_topic, default_topic); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); } } +// TODO, if we raise error we need to manually call the lifecycle change state callback, +// call callback function that this service calls + }// namespace modulo_components \ No newline at end of file diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index dc54cff47..ce16cf8f6 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -1,18 +1,222 @@ #include "modulo_components/LifecycleComponent.hpp" +#include + using namespace modulo_new_core::communication; namespace modulo_components { LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options) : - ComponentInterface(node_options, PublisherType::LIFECYCLE_PUBLISHER) {} + ComponentInterface(node_options, PublisherType::LIFECYCLE_PUBLISHER) { + this->add_predicate("is_unconfigured", true); + this->add_predicate("is_inactive", false); + this->add_predicate("is_active", false); + this->add_predicate("is_finalized", false); +} void LifecycleComponent::step() { - // TODO do this only if active and add on_step -// this->publish_predicates(); -// this->publish_outputs(); -// this->evaluate_daemon_callbacks(); -// this->on_step(); + if (this->get_predicate("is_active")) { + this->publish_predicates(); + this->publish_outputs(); + this->evaluate_periodic_callbacks(); + this->on_step(); + } +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleComponent::on_configure(const rclcpp_lifecycle::State& previous_state) { + RCLCPP_DEBUG(this->get_logger(), "on_configure called from previous state %s", previous_state.label().c_str()); + if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED) { + RCLCPP_WARN(get_logger(), "Invalid transition 'configure' from state %s.", previous_state.label().c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + if (!this->configure()) { + RCLCPP_WARN(get_logger(), "Configuration failed! Reverting to the unconfigured state."); + // perform cleanup actions to ensure the component is unconfigured + if (this->cleanup()) { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } else { + RCLCPP_ERROR(get_logger(), + "Could not revert to the unconfigured state! Entering into the error processing transition state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + this->set_predicate("is_unconfigured", false); + this->set_predicate("is_inactive", true); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool LifecycleComponent::configure() { + bool result = this->configure_outputs(); + return result && this->on_configure(); +} + +bool LifecycleComponent::on_configure() { + return true; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleComponent::on_cleanup(const rclcpp_lifecycle::State& previous_state) { + RCLCPP_DEBUG(this->get_logger(), "on_cleanup called from previous state %s", previous_state.label().c_str()); + if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_WARN(get_logger(), "Invalid transition 'cleanup' from state %s.", previous_state.label().c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + if (!this->cleanup()) { + RCLCPP_ERROR(get_logger(), "Cleanup failed! Entering into the error processing transition state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + this->set_predicate("is_inactive", false); + this->set_predicate("is_unconfigured", true); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool LifecycleComponent::cleanup() { + bool result = this->cleanup_signals(); + return result && this->on_cleanup(); +} + +bool LifecycleComponent::on_cleanup() { + return true; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleComponent::on_activate(const rclcpp_lifecycle::State& previous_state) { + RCLCPP_DEBUG(this->get_logger(), "on_activate called from previous state %s", previous_state.label().c_str()); + if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_WARN(get_logger(), "Invalid transition 'activate' from state %s.", previous_state.label().c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + if (!this->activate()) { + RCLCPP_WARN(get_logger(), "Activation failed! Reverting to the inactive state."); + // perform deactivation actions to ensure the component is inactive + if (this->deactivate()) { + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } else { + RCLCPP_ERROR(get_logger(), + "Could not revert to the inactive state! Entering into the error processing transition state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + } + this->set_predicate("is_inactive", false); + this->set_predicate("is_active", true); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool LifecycleComponent::activate() { + bool result = this->activate_outputs(); + return result && this->on_activate(); +} + +bool LifecycleComponent::on_activate() { + return true; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleComponent::on_deactivate(const rclcpp_lifecycle::State& previous_state) { + RCLCPP_DEBUG(this->get_logger(), "on_deactivate called from previous state %s", previous_state.label().c_str()); + if (previous_state.id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { + RCLCPP_WARN(get_logger(), "Invalid transition 'deactivate' from state %s.", previous_state.label().c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + if (!this->deactivate()) { + RCLCPP_ERROR(get_logger(), "Deactivation failed! Entering into the error processing transition state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + this->set_predicate("is_active", false); + this->set_predicate("is_inactive", true); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool LifecycleComponent::deactivate() { + bool result = this->deactivate_outputs(); + return result && this->on_deactivate(); +} + +bool LifecycleComponent::on_deactivate() { + return true; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { + RCLCPP_DEBUG(this->get_logger(), "on_deactivate called from previous state %s", previous_state.label().c_str()); + switch (previous_state.id()) { + case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: + if (!this->deactivate()) { + RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate deactivation!"); + break; + } + [[fallthrough]]; + case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: + if (!this->cleanup()) { + RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate cleanup!"); + break; + } + [[fallthrough]]; + case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: + if (!this->shutdown()) { + break; + } + // TODO: reset and finalize all needed properties + // this->handlers_.clear(); + // this->daemons_.clear(); + // this->parameters_.clear(); + // this->shutdown_ = true; + this->set_predicate("is_unconfigured", false); + this->set_predicate("is_inactive", false); + this->set_predicate("is_active", false); + this->set_predicate("is_finalized", true); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + default: + RCLCPP_WARN(get_logger(), "Invalid transition 'shutdown' from state %s.", previous_state.label().c_str()); + break; + } + RCLCPP_ERROR(get_logger(), "Entering into the error processing transition state."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; +} + +bool LifecycleComponent::shutdown() { + return this->on_shutdown(); +} + +bool LifecycleComponent::on_shutdown() { + return true; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +LifecycleComponent::on_error(const rclcpp_lifecycle::State& previous_state) { + RCLCPP_DEBUG(this->get_logger(), "on_error called from previous state %s", previous_state.label().c_str()); + this->set_predicate("is_unconfigured", false); + this->set_predicate("is_inactive", false); + this->set_predicate("is_active", false); + this->set_predicate("is_finalized", false); + this->set_predicate("in_error_state", true); + bool error_handled; + try { + error_handled = this->handle_error(); + } catch (const std::exception& ex) { + RCLCPP_DEBUG(this->get_logger(), "Exception caught during on_error handling: %s", ex.what()); + error_handled = false; + } + if (!error_handled) { + RCLCPP_ERROR(get_logger(), "Error processing failed! Entering into the finalized state."); + // TODO: reset and finalize all needed properties + this->set_predicate("is_finalized", true); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + this->set_predicate("in_error_state", false); + this->set_predicate("is_unconfigured", true); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +bool LifecycleComponent::handle_error() { + return this->on_error(); +} + +bool LifecycleComponent::on_error() { + return true; } bool LifecycleComponent::configure_outputs() { @@ -85,6 +289,13 @@ bool LifecycleComponent::configure_outputs() { return success; } +bool LifecycleComponent::cleanup_signals() { + RCLCPP_DEBUG(this->get_logger(), "Clearing all inputs and outputs"); + this->inputs_.clear(); + this->outputs_.clear(); + return true; +} + bool LifecycleComponent::activate_outputs() { bool success = true; for (auto const& [name, interface]: this->outputs_) { @@ -111,4 +322,4 @@ bool LifecycleComponent::deactivate_outputs() { return success; } -} \ No newline at end of file +}// namespace modulo_components \ No newline at end of file From d574592493df0492b423803921e854293309b6d5 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 17 Jun 2022 10:23:24 +0200 Subject: [PATCH 39/71] Refactor 'msg' to 'message' for readability (#79) * Rename msg to message in C++ translators and MessagePair * Refactor msg to message in Python translators * Make tests pass in modulo_core and modulo_components --- .../modulo_components/ComponentInterface.hpp | 10 +- .../modulo_components/component_interface.py | 14 +- .../message_passing/PublisherHandler.hpp | 2 +- .../message_passing/SubscriptionHandler.hpp | 2 +- .../TransformListenerHandler.cpp | 2 +- .../communication/MessagePair.hpp | 16 +- .../translators/message_readers.hpp | 86 ++++----- .../translators/message_writers.hpp | 122 +++++++----- .../translators/message_readers.py | 96 +++++----- .../translators/message_writers.py | 104 +++++------ .../src/translators/message_readers.cpp | 110 +++++------ .../src/translators/message_writers.cpp | 175 ++++++++++-------- .../communication/test_communication.cpp | 4 +- .../communication/test_message_pair.cpp | 54 +++--- .../communication/test_publisher_handler.cpp | 12 +- .../test_subscription_handler.cpp | 8 +- .../cpp_tests/translators/test_messages.cpp | 94 +++++----- .../python_tests/translators/test_messages.py | 150 +++++++-------- 18 files changed, 553 insertions(+), 508 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 187a377e2..73fb3e50b 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -748,7 +748,7 @@ inline void ComponentInterface::send_transform(const state_representation } try { geometry_msgs::msg::TransformStamped transform_message; - modulo_new_core::translators::write_msg(transform_message, transform, this->get_clock()->now()); + modulo_new_core::translators::write_message(transform_message, transform, this->get_clock()->now()); tf2_msgs::msg::TFMessage tf_message; tf_message.transforms.emplace_back(transform_message); this->tf_broadcaster_->publish(tf_message); @@ -769,7 +769,7 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra try { state_representation::CartesianPose result(frame_name, reference_frame_name); auto transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); - modulo_new_core::translators::read_msg(result, transform); + modulo_new_core::translators::read_message(result, transform); return result; } catch (const tf2::TransformException& ex) { throw exceptions::LookupTransformException(std::string("Failed to lookup transform: ").append(ex.what())); @@ -779,14 +779,14 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra template inline void ComponentInterface::publish_predicates() { for (const auto& predicate: this->predicates_) { - std_msgs::msg::Bool msg; - msg.data = this->get_predicate(predicate.first); + std_msgs::msg::Bool message; + message.data = this->get_predicate(predicate.first); if (this->predicate_publishers_.find(predicate.first) == this->predicate_publishers_.end()) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "No publisher for predicate " << predicate.first << " found."); return; } - predicate_publishers_.at(predicate.first)->publish(msg); + predicate_publishers_.at(predicate.first)->publish(message); } } diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index afa65e5c8..6011a117b 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -6,8 +6,8 @@ import tf2_py from geometry_msgs.msg import TransformStamped from modulo_components.exceptions.component_exceptions import ComponentParameterError, LookupTransformError -from modulo_new_core.translators.message_readers import read_stamped_msg -from modulo_new_core.translators.message_writers import write_stamped_msg +from modulo_new_core.translators.message_readers import read_stamped_message +from modulo_new_core.translators.message_writers import write_stamped_message from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter_const from rcl_interfaces.msg import ParameterDescriptor from rcl_interfaces.msg import SetParametersResult @@ -63,13 +63,13 @@ def __step(self) -> None: Step function that is called periodically. """ for predicate_name in self._predicates.keys(): - msg = Bool() - msg.data = self.get_predicate(predicate_name) + message = Bool() + message.data = self.get_predicate(predicate_name) if predicate_name not in self._predicate_publishers.keys(): self.get_logger().error(f"No publisher for predicate {predicate_name} found.", throttle_duration_sec=1.0) return - self._predicate_publishers[predicate_name].publish(msg) + self._predicate_publishers[predicate_name].publish(message) def __generate_predicate_topic(self, predicate_name: str) -> str: """ @@ -311,7 +311,7 @@ def send_transform(self, transform: sr.CartesianPose): return try: transform_message = TransformStamped() - write_stamped_msg(transform_message, transform, self.get_clock().now()) + write_stamped_message(transform_message, transform, self.get_clock().now()) self.__tf_broadcaster.sendTransform(transform_message) except tf2_py.TransformException as e: self.get_logger().error(f"Failed to send transform: {e}", throttle_duration_sec=1.0) @@ -332,7 +332,7 @@ def lookup_transform(self, frame_name: str, reference_frame_name="world", time_p try: result = sr.CartesianPose(frame_name, reference_frame_name) transform = self.__tf_buffer.lookup_transform(reference_frame_name, frame_name, time_point, duration) - read_stamped_msg(result, transform) + read_stamped_message(result, transform) return result except tf2_py.TransformException as e: raise LookupTransformError(f"Failed to lookup transform: {e}") diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp index 1ace75818..f63a88250 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp @@ -99,7 +99,7 @@ PublisherHandler::PublisherHandler(const std::shared_ptr void PublisherHandler::publish(const RecT& recipient) { auto out_msg = std::make_unique(); - modulo_new_core::translators::write_msg(*out_msg, recipient, this->get_clock().now()); + modulo_new_core::translators::write_message(*out_msg, recipient, this->get_clock().now()); this->publisher_->publish(std::move(out_msg)); } diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp index c2b7091fc..8b49c2043 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp @@ -62,7 +62,7 @@ SubscriptionHandler::SubscriptionHandler(const std::shared_ptr void SubscriptionHandler::subscription_callback(const std::shared_ptr msg) { - modulo_new_core::translators::read_msg(static_cast(this->get_recipient()), *msg); + modulo_new_core::translators::read_message(static_cast(this->get_recipient()), *msg); } template diff --git a/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp b/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp index d4f71c1c1..d407f5d3b 100644 --- a/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp +++ b/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp @@ -9,7 +9,7 @@ const state_representation::CartesianPose TransformListenerHandler::lookup_trans frame_name, tf2::TimePoint(std::chrono::milliseconds(0)), tf2::Duration(this->get_timeout())); - modulo_new_core::translators::read_msg(result, transformStamped); + modulo_new_core::translators::read_message(result, transformStamped); return result; } }// namespace modulo::core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index ed3a962c3..e73385e78 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -59,9 +59,9 @@ inline MsgT MessagePair::write_message() const { if (this->data_ == nullptr) { throw exceptions::NullPointerException("The message pair data is not set, nothing to write"); } - auto msg = MsgT(); - translators::write_msg(msg, *this->data_, clock_->now()); - return msg; + auto message = MsgT(); + translators::write_message(message, *this->data_, clock_->now()); + return message; } template<> @@ -69,9 +69,9 @@ inline EncodedState MessagePair::writ if (this->data_ == nullptr) { throw exceptions::NullPointerException("The message pair data is not set, nothing to write"); } - auto msg = EncodedState(); - translators::write_msg(msg, this->data_, clock_->now()); - return msg; + auto message = EncodedState(); + translators::write_message(message, this->data_, clock_->now()); + return message; } template @@ -79,7 +79,7 @@ inline void MessagePair::read_message(const MsgT& message) { if (this->data_ == nullptr) { throw exceptions::NullPointerException("The message pair data is not set, nothing to read"); } - translators::read_msg(*this->data_, message); + translators::read_message(*this->data_, message); } template<> @@ -87,7 +87,7 @@ inline void MessagePair::read_message if (this->data_ == nullptr) { throw exceptions::NullPointerException("The message pair data is not set, nothing to read"); } - translators::read_msg(this->data_, message); + translators::read_message(this->data_, message); } template diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp index afa5aa3dc..59885a5dc 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp @@ -25,143 +25,143 @@ namespace modulo_new_core::translators { /** * @brief Convert a ROS geometry_msgs::msg::Accel to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& message); /** * @brief Convert a ROS geometry_msgs::msg::AccelStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS msg to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& message); /** * @brief Convert a ROS geometry_msgs::msg::Pose to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& message); /** * @brief Convert a ROS geometry_msgs::msg::PoseStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& message); /** * @brief Convert a ROS geometry_msgs::msg::Transform to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& message); /** * @brief Convert a ROS geometry_msgs::msg::TransformStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& message); /** * @brief Convert a ROS geometry_msgs::msg::Twist to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& message); /** * @brief Convert a ROS geometry_msgs::msg::TwistStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& message); /** * @brief Convert a ROS geometry_msgs::msg::Wrench to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& message); /** * @brief Convert a ROS geometry_msgs::msg::WrenchStamped to a CartesianState * @param state The CartesianState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& msg); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& message); /** * @brief Convert a ROS sensor_msgs::msg::JointState to a JointState * @param state The JointState to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(state_representation::JointState& state, const sensor_msgs::msg::JointState& msg); +void read_message(state_representation::JointState& state, const sensor_msgs::msg::JointState& message); /** * @brief Template function to convert a ROS std_msgs::msg::T to a Parameter - * @tparam T all types of parameters supported in ROS std messages - * @tparam U all types of parameters supported in ROS std messages + * @tparam T All types of parameters supported in ROS std messages + * @tparam U All types of parameters supported in ROS std messages * @param state The Parameter to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ template -void read_msg(state_representation::Parameter& state, const U& msg) { - state.set_value(msg.data); +void read_message(state_representation::Parameter& state, const U& message) { + state.set_value(message.data); } /** * @brief Convert a ROS std_msgs::msg::Bool to a boolean * @param state The state to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(bool& state, const std_msgs::msg::Bool& msg); +void read_message(bool& state, const std_msgs::msg::Bool& message); /** * @brief Convert a ROS std_msgs::msg::Float64 to a double * @param state The state to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(double& state, const std_msgs::msg::Float64& msg); +void read_message(double& state, const std_msgs::msg::Float64& message); /** * @brief Convert a ROS std_msgs::msg::Float64MultiArray to a vector of double * @param state The state to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(std::vector& state, const std_msgs::msg::Float64MultiArray& msg); +void read_message(std::vector& state, const std_msgs::msg::Float64MultiArray& message); /** * @brief Convert a ROS std_msgs::msg::Int32 to an integer * @param state The state to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(int& state, const std_msgs::msg::Int32& msg); +void read_message(int& state, const std_msgs::msg::Int32& message); /** * @brief Convert a ROS std_msgs::msg::String to a string * @param state The state to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ -void read_msg(std::string& state, const std_msgs::msg::String& msg); +void read_message(std::string& state, const std_msgs::msg::String& message); /** * @brief Convert a ROS std_msgs::msg::UInt8MultiArray message to a State using protobuf decoding - * @tparam a state_representation::State type object + * @tparam T A state_representation::State type * @param state The state to populate - * @param msg The ROS message to read from + * @param message The ROS message to read from */ template -inline void read_msg(T& state, const EncodedState& msg) { - std::string tmp(msg.data.begin(), msg.data.end()); +inline void read_message(T& state, const EncodedState& message) { + std::string tmp(message.data.begin(), message.data.end()); state = clproto::decode(tmp); } template<> -inline void read_msg(std::shared_ptr& state, const EncodedState& msg) { +inline void read_message(std::shared_ptr& state, const EncodedState& message) { using namespace state_representation; - std::string tmp(msg.data.begin(), msg.data.end()); + std::string tmp(message.data.begin(), message.data.end()); auto new_state = clproto::decode>(tmp); switch (new_state->get_type()) { case StateType::STATE: diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp index 809deb051..8a4ccf084 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp @@ -1,17 +1,17 @@ #pragma once -#include #include #include #include #include #include +#include +#include #include #include #include #include #include -#include #include #include @@ -25,161 +25,191 @@ namespace modulo_new_core::translators { /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Accel - * @param msg The ROS msg to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::Accel& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::Accel& message, const state_representation::CartesianState& state, const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::AccelStamped - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::AccelStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::AccelStamped& message, const state_representation::CartesianState& state, + const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Pose - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::Pose& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::Pose& message, const state_representation::CartesianState& state, const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::PoseStamped - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::PoseStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::PoseStamped& message, const state_representation::CartesianState& state, + const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Transform - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::Transform& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::Transform& message, const state_representation::CartesianState& state, const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::TransformStamped - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::TransformStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::TransformStamped& message, const state_representation::CartesianState& state, + const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Twist - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::Twist& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::Twist& message, const state_representation::CartesianState& state, const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::TwistStamped - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::TwistStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::TwistStamped& message, const state_representation::CartesianState& state, + const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Wrench - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::Wrench& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::Wrench& message, const state_representation::CartesianState& state, const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::WrenchStamped - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(geometry_msgs::msg::WrenchStamped& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + geometry_msgs::msg::WrenchStamped& message, const state_representation::CartesianState& state, + const rclcpp::Time& time +); /** * @brief Convert a JointState to a ROS sensor_msgs::msg::JointState - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(sensor_msgs::msg::JointState& msg, const state_representation::JointState& state, const rclcpp::Time& time); +void write_message( + sensor_msgs::msg::JointState& message, const state_representation::JointState& state, const rclcpp::Time& time +); /** * @brief Convert a CartesianState to a ROS tf2_msgs::msg::TFMessage - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(tf2_msgs::msg::TFMessage& msg, const state_representation::CartesianState& state, const rclcpp::Time& time); +void write_message( + tf2_msgs::msg::TFMessage& message, const state_representation::CartesianState& state, const rclcpp::Time& time +); /** * @brief Convert a Parameter to a ROS equivalent representation - * @tparam T all types of parameters supported in ROS std messages - * @tparam U all types of parameters supported in ROS std messages - * @param msg The ROS message to populate + * @tparam T All types of parameters supported in ROS std messages + * @tparam U All types of parameters supported in ROS std messages + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -template -void write_msg(U& msg, const state_representation::Parameter& state, const rclcpp::Time&); +template +void write_message(U& message, const state_representation::Parameter& state, const rclcpp::Time&); /** * @brief Convert a boolean to a ROS std_msgs::msg::Bool - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(std_msgs::msg::Bool& msg, const bool& state, const rclcpp::Time& time); +void write_message(std_msgs::msg::Bool& message, const bool& state, const rclcpp::Time& time); /** * @brief Convert a double to a ROS std_msgs::msg::Float64 - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(std_msgs::msg::Float64& msg, const double& state, const rclcpp::Time& time); +void write_message(std_msgs::msg::Float64& message, const double& state, const rclcpp::Time& time); /** * @brief Convert a vector of double to a ROS std_msgs::msg::Float64MultiArray - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(std_msgs::msg::Float64MultiArray& msg, const std::vector& state, const rclcpp::Time& time); +void +write_message(std_msgs::msg::Float64MultiArray& message, const std::vector& state, const rclcpp::Time& time); /** * @brief Convert an integer to a ROS std_msgs::msg::Int32 - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(std_msgs::msg::Int32& msg, const int& state, const rclcpp::Time& time); +void write_message(std_msgs::msg::Int32& message, const int& state, const rclcpp::Time& time); /** * @brief Convert a string to a ROS std_msgs::msg::String - * @param msg The ROS message to populate + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ -void write_msg(std_msgs::msg::String& msg, const std::string& state, const rclcpp::Time& time); +void write_message(std_msgs::msg::String& message, const std::string& state, const rclcpp::Time& time); /** - * @brief Convert a state to a ROS std_msgs::msg::UInt8MultiArray message using protobuf encoding - * @tparam a state_representation::State type object - * @param msg The ROS msg to populate + * @brief Convert a state to an EncodedState (std_msgs::msg::UInt8MultiArray) message using protobuf encoding + * @tparam T A state_representation::State type + * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message */ template -inline void write_msg(EncodedState& msg, const T& state, const rclcpp::Time&) { +inline void write_message(EncodedState& message, const T& state, const rclcpp::Time&) { std::string tmp = clproto::encode(state); - msg.data = std::vector(tmp.begin(), tmp.end()); + message.data = std::vector(tmp.begin(), tmp.end()); } }// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/modulo_new_core/translators/message_readers.py b/source/modulo_new_core/modulo_new_core/translators/message_readers.py index fc807b15e..892942ef8 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_readers.py +++ b/source/modulo_new_core/modulo_new_core/translators/message_readers.py @@ -12,91 +12,91 @@ StateT = TypeVar('StateT') -def read_xyz(msg: Union[geometry.Point, geometry.Vector3]) -> List[float]: +def read_xyz(message: Union[geometry.Point, geometry.Vector3]) -> List[float]: """ Helper function to read a list from a Point or Vector3 message. - :param msg: The message to read from + :param message: The message to read from """ - return [msg.x, msg.y, msg.z] + return [message.x, message.y, message.z] -def read_quaternion(msg: geometry.Quaternion) -> List[float]: +def read_quaternion(message: geometry.Quaternion) -> List[float]: """ Helper function to read a list of quaternion coefficients (w,x,y,z) from a Quaternion message. - :param msg: The message to read from + :param message: The message to read from """ - return [msg.w, msg.x, msg.y, msg.z] + return [message.w, message.x, message.y, message.z] -def read_msg(state: StateT, msg: MsgT): +def read_message(state: StateT, message: MsgT): """ Convert a ROS message to a state_representation State. :param state: The state to populate - :param msg: The ROS message to read from + :param message: The ROS message to read from """ if not isinstance(state, sr.State): raise RuntimeError("This state type is not supported.") if isinstance(state, sr.CartesianState): - if isinstance(msg, geometry.Accel): - state.set_linear_acceleration(read_xyz(msg.linear)) - state.set_angular_acceleration(read_xyz(msg.angular)) - elif isinstance(msg, geometry.Pose): - state.set_position(read_xyz(msg.position)) - state.set_orientation(read_quaternion(msg.orientation)) - elif isinstance(msg, geometry.Transform): - state.set_position(read_xyz(msg.translation)) - state.set_orientation(read_quaternion(msg.rotation)) - elif isinstance(msg, geometry.Twist): - state.set_linear_velocity(read_xyz(msg.linear)) - state.set_angular_velocity(read_xyz(msg.angular)) - elif isinstance(msg, geometry.Wrench): - state.set_force(read_xyz(msg.force)) - state.set_torque(read_xyz(msg.torque)) + if isinstance(message, geometry.Accel): + state.set_linear_acceleration(read_xyz(message.linear)) + state.set_angular_acceleration(read_xyz(message.angular)) + elif isinstance(message, geometry.Pose): + state.set_position(read_xyz(message.position)) + state.set_orientation(read_quaternion(message.orientation)) + elif isinstance(message, geometry.Transform): + state.set_position(read_xyz(message.translation)) + state.set_orientation(read_quaternion(message.rotation)) + elif isinstance(message, geometry.Twist): + state.set_linear_velocity(read_xyz(message.linear)) + state.set_angular_velocity(read_xyz(message.angular)) + elif isinstance(message, geometry.Wrench): + state.set_force(read_xyz(message.force)) + state.set_torque(read_xyz(message.torque)) else: raise RuntimeError("The provided combination of state type and message type is not supported") - elif isinstance(msg, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): - state.set_names(msg.name) - if len(msg.position): - state.set_positions(msg.position) - if len(msg.velocity): - state.set_velocities(msg.velocity) - if len(msg.effort): - state.set_torques(msg.effort) + elif isinstance(message, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): + state.set_names(message.name) + if len(message.position): + state.set_positions(message.position) + if len(message.velocity): + state.set_velocities(message.velocity) + if len(message.effort): + state.set_torques(message.effort) else: raise RuntimeError("The provided combination of state type and message type is not supported") return state -def read_stamped_msg(state: StateT, msg: MsgT): +def read_stamped_message(state: StateT, message: MsgT): """ Convert a stamped ROS message to a state_representation State. :param state: The state to populate - :param msg: The ROS message to read from + :param message: The ROS message to read from """ - if isinstance(msg, geometry.AccelStamped): - read_msg(state, msg.accel) - elif isinstance(msg, geometry.PoseStamped): - read_msg(state, msg.pose) - elif isinstance(msg, geometry.TransformStamped): - read_msg(state, msg.transform) - state.set_name(msg.child_frame_id) - elif isinstance(msg, geometry.TwistStamped): - read_msg(state, msg.twist) - elif isinstance(msg, geometry.WrenchStamped): - read_msg(state, msg.wrench) + if isinstance(message, geometry.AccelStamped): + read_message(state, message.accel) + elif isinstance(message, geometry.PoseStamped): + read_message(state, message.pose) + elif isinstance(message, geometry.TransformStamped): + read_message(state, message.transform) + state.set_name(message.child_frame_id) + elif isinstance(message, geometry.TwistStamped): + read_message(state, message.twist) + elif isinstance(message, geometry.WrenchStamped): + read_message(state, message.wrench) else: raise RuntimeError("The provided combination of state type and message type is not supported") - state.set_reference_frame(msg.header.frame_id) + state.set_reference_frame(message.header.frame_id) -def read_clproto_msg(msg: EncodedState) -> StateT: +def read_clproto_message(message: EncodedState) -> StateT: """ Convert an EncodedState message to a state_representation State. - :param msg: The EncodedState msg to read from + :param message: The EncodedState message to read from """ - return clproto.decode(msg.data.tobytes()) + return clproto.decode(message.data.tobytes()) diff --git a/source/modulo_new_core/modulo_new_core/translators/message_writers.py b/source/modulo_new_core/modulo_new_core/translators/message_writers.py index caf0e9a8f..c8f54be80 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_writers.py +++ b/source/modulo_new_core/modulo_new_core/translators/message_writers.py @@ -12,40 +12,40 @@ StateT = TypeVar('StateT') -def write_xyz(msg: Union[geometry.Point, geometry.Vector3], vector: np.array): +def write_xyz(message: Union[geometry.Point, geometry.Vector3], vector: np.array): """ Helper function to write a vector to a Point or Vector3 message. - :param msg: The message to populate + :param message: The message to populate :param vector: The vector to read from """ if vector.size != 3: raise RuntimeError("Provide a vector of size 3 to transform to a Point or Vector3 message.") - msg.x = vector[0] - msg.y = vector[1] - msg.z = vector[2] + message.x = vector[0] + message.y = vector[1] + message.z = vector[2] -def write_quaternion(msg: geometry.Quaternion, quat: np.array): +def write_quaternion(message: geometry.Quaternion, quat: np.array): """ Helper function to write a vector of quaternion coefficients (w,x,y,z) to a Quaternion message. - :param msg: The message to populate + :param message: The message to populate :param quat: The vector to read from """ if quat.size != 4: raise RuntimeError("Provide a vector of size 4 to transform to a Quaternion message.") - msg.w = quat[0] - msg.x = quat[1] - msg.y = quat[2] - msg.z = quat[3] + message.w = quat[0] + message.x = quat[1] + message.y = quat[2] + message.z = quat[3] -def write_msg(msg: MsgT, state: StateT): +def write_message(message: MsgT, state: StateT): """ Convert a state_representation State to a ROS message. - :param msg: The ROS message to populate + :param message: The ROS message to populate :param state: The state to read from """ if not isinstance(state, sr.State): @@ -53,58 +53,58 @@ def write_msg(msg: MsgT, state: StateT): if state.is_empty(): raise RuntimeError(f"{state.get_name()} state is empty while attempting to write it to message.") if isinstance(state, sr.CartesianState): - if isinstance(msg, geometry.Accel): - write_xyz(msg.linear, state.get_linear_acceleration()) - write_xyz(msg.angular, state.get_angular_acceleration()) - elif isinstance(msg, geometry.Pose): - write_xyz(msg.position, state.get_position()) - write_quaternion(msg.orientation, state.get_orientation_coefficients()) - elif isinstance(msg, geometry.Transform): - write_xyz(msg.translation, state.get_position()) - write_quaternion(msg.rotation, state.get_orientation_coefficients()) - elif isinstance(msg, geometry.Twist): - write_xyz(msg.linear, state.get_linear_velocity()) - write_xyz(msg.angular, state.get_angular_velocity()) - elif isinstance(msg, geometry.Wrench): - write_xyz(msg.force, state.get_force()) - write_xyz(msg.torque, state.get_torque()) + if isinstance(message, geometry.Accel): + write_xyz(message.linear, state.get_linear_acceleration()) + write_xyz(message.angular, state.get_angular_acceleration()) + elif isinstance(message, geometry.Pose): + write_xyz(message.position, state.get_position()) + write_quaternion(message.orientation, state.get_orientation_coefficients()) + elif isinstance(message, geometry.Transform): + write_xyz(message.translation, state.get_position()) + write_quaternion(message.rotation, state.get_orientation_coefficients()) + elif isinstance(message, geometry.Twist): + write_xyz(message.linear, state.get_linear_velocity()) + write_xyz(message.angular, state.get_angular_velocity()) + elif isinstance(message, geometry.Wrench): + write_xyz(message.force, state.get_force()) + write_xyz(message.torque, state.get_torque()) else: raise RuntimeError("The provided combination of state type and message type is not supported") - elif isinstance(msg, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): - msg.name = state.get_names() - msg.position = state.get_positions().tolist() - msg.velocity = state.get_velocities().tolist() - msg.effort = state.get_torques().tolist() + elif isinstance(message, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): + message.name = state.get_names() + message.position = state.get_positions().tolist() + message.velocity = state.get_velocities().tolist() + message.effort = state.get_torques().tolist() else: raise RuntimeError("The provided combination of state type and message type is not supported") -def write_stamped_msg(msg: MsgT, state: StateT, time: rclpy.time.Time): +def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): """ Convert a state_representation State to a stamped ROS message. - :param msg: The ROS message to populate + :param message: The ROS message to populate :param state: The state to read from :param time: The time of the message """ - if isinstance(msg, geometry.AccelStamped): - write_msg(msg.accel, state) - elif isinstance(msg, geometry.PoseStamped): - write_msg(msg.pose, state) - elif isinstance(msg, geometry.TransformStamped): - write_msg(msg.transform, state) - msg.child_frame_id = state.get_name() - elif isinstance(msg, geometry.TwistStamped): - write_msg(msg.twist, state) - elif isinstance(msg, geometry.WrenchStamped): - write_msg(msg.wrench, state) + if isinstance(message, geometry.AccelStamped): + write_message(message.accel, state) + elif isinstance(message, geometry.PoseStamped): + write_message(message.pose, state) + elif isinstance(message, geometry.TransformStamped): + write_message(message.transform, state) + message.child_frame_id = state.get_name() + elif isinstance(message, geometry.TwistStamped): + write_message(message.twist, state) + elif isinstance(message, geometry.WrenchStamped): + write_message(message.wrench, state) else: raise RuntimeError("The provided combination of state type and message type is not supported") - msg.header.stamp = time.to_msg() - msg.header.frame_id = state.get_reference_frame() + message.header.stamp = time.to_msg() + message.header.frame_id = state.get_reference_frame() -def write_clproto_msg(state: StateT, clproto_message_type: clproto.MessageType) -> EncodedState(): +def write_clproto_message(state: StateT, clproto_message_type: clproto.MessageType) -> EncodedState(): """ Convert a state_representation State to an EncodedState message. @@ -113,6 +113,6 @@ def write_clproto_msg(state: StateT, clproto_message_type: clproto.MessageType) """ if not isinstance(state, sr.State): raise RuntimeError("This state type is not supported.") - msg = EncodedState() - msg.data = clproto.encode(state, clproto_message_type) - return msg + message = EncodedState() + message.data = clproto.encode(state, clproto_message_type) + return message diff --git a/source/modulo_new_core/src/translators/message_readers.cpp b/source/modulo_new_core/src/translators/message_readers.cpp index bf16ae8ec..d49172824 100644 --- a/source/modulo_new_core/src/translators/message_readers.cpp +++ b/source/modulo_new_core/src/translators/message_readers.cpp @@ -2,99 +2,99 @@ namespace modulo_new_core::translators { -static Eigen::Vector3d read_point(const geometry_msgs::msg::Point& msg) { - return {msg.x, msg.y, msg.z}; +static Eigen::Vector3d read_point(const geometry_msgs::msg::Point& message) { + return {message.x, message.y, message.z}; } -static Eigen::Vector3d read_vector3(const geometry_msgs::msg::Vector3& msg) { - return {msg.x, msg.y, msg.z}; +static Eigen::Vector3d read_vector3(const geometry_msgs::msg::Vector3& message) { + return {message.x, message.y, message.z}; } -static Eigen::Quaterniond read_quaternion(const geometry_msgs::msg::Quaternion& msg) { - return {msg.w, msg.x, msg.y, msg.z}; +static Eigen::Quaterniond read_quaternion(const geometry_msgs::msg::Quaternion& message) { + return {message.w, message.x, message.y, message.z}; } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& msg) { - state.set_linear_acceleration(read_vector3(msg.linear)); - state.set_angular_acceleration(read_vector3(msg.angular)); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Accel& message) { + state.set_linear_acceleration(read_vector3(message.linear)); + state.set_angular_acceleration(read_vector3(message.angular)); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.accel); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::AccelStamped& message) { + state.set_reference_frame(message.header.frame_id); + read_message(state, message.accel); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& msg) { - state.set_position(read_point(msg.position)); - state.set_orientation(read_quaternion(msg.orientation)); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Pose& message) { + state.set_position(read_point(message.position)); + state.set_orientation(read_quaternion(message.orientation)); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.pose); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::PoseStamped& message) { + state.set_reference_frame(message.header.frame_id); + read_message(state, message.pose); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& msg) { - state.set_position(read_vector3(msg.translation)); - state.set_orientation(read_quaternion(msg.rotation)); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Transform& message) { + state.set_position(read_vector3(message.translation)); + state.set_orientation(read_quaternion(message.rotation)); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - state.set_name(msg.child_frame_id); - read_msg(state, msg.transform); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::TransformStamped& message) { + state.set_reference_frame(message.header.frame_id); + state.set_name(message.child_frame_id); + read_message(state, message.transform); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& msg) { - state.set_linear_velocity(read_vector3(msg.linear)); - state.set_angular_velocity(read_vector3(msg.angular)); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Twist& message) { + state.set_linear_velocity(read_vector3(message.linear)); + state.set_angular_velocity(read_vector3(message.angular)); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.twist); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::TwistStamped& message) { + state.set_reference_frame(message.header.frame_id); + read_message(state, message.twist); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& msg) { - state.set_force(read_vector3(msg.force)); - state.set_torque(read_vector3(msg.torque)); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::Wrench& message) { + state.set_force(read_vector3(message.force)); + state.set_torque(read_vector3(message.torque)); } -void read_msg(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& msg) { - state.set_reference_frame(msg.header.frame_id); - read_msg(state, msg.wrench); +void read_message(state_representation::CartesianState& state, const geometry_msgs::msg::WrenchStamped& message) { + state.set_reference_frame(message.header.frame_id); + read_message(state, message.wrench); } -void read_msg(state_representation::JointState& state, const sensor_msgs::msg::JointState& msg) { - state.set_names(msg.name); - if (!msg.position.empty()) { - state.set_positions(Eigen::VectorXd::Map(msg.position.data(), msg.position.size())); +void read_message(state_representation::JointState& state, const sensor_msgs::msg::JointState& message) { + state.set_names(message.name); + if (!message.position.empty()) { + state.set_positions(Eigen::VectorXd::Map(message.position.data(), message.position.size())); } - if (!msg.velocity.empty()) { - state.set_velocities(Eigen::VectorXd::Map(msg.velocity.data(), msg.velocity.size())); + if (!message.velocity.empty()) { + state.set_velocities(Eigen::VectorXd::Map(message.velocity.data(), message.velocity.size())); } - if (!msg.effort.empty()) { - state.set_torques(Eigen::VectorXd::Map(msg.effort.data(), msg.effort.size())); + if (!message.effort.empty()) { + state.set_torques(Eigen::VectorXd::Map(message.effort.data(), message.effort.size())); } } -void read_msg(bool& state, const std_msgs::msg::Bool& msg) { - state = msg.data; +void read_message(bool& state, const std_msgs::msg::Bool& message) { + state = message.data; } -void read_msg(double& state, const std_msgs::msg::Float64& msg) { - state = msg.data; +void read_message(double& state, const std_msgs::msg::Float64& message) { + state = message.data; } -void read_msg(std::vector& state, const std_msgs::msg::Float64MultiArray& msg) { - state = msg.data; +void read_message(std::vector& state, const std_msgs::msg::Float64MultiArray& message) { + state = message.data; } -void read_msg(int& state, const std_msgs::msg::Int32& msg) { - state = msg.data; +void read_message(int& state, const std_msgs::msg::Int32& message) { + state = message.data; } -void read_msg(std::string& state, const std_msgs::msg::String& msg) { - state = msg.data; +void read_message(std::string& state, const std_msgs::msg::String& message) { + state = message.data; } }// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/src/translators/message_writers.cpp b/source/modulo_new_core/src/translators/message_writers.cpp index ba582e437..36b4dda17 100644 --- a/source/modulo_new_core/src/translators/message_writers.cpp +++ b/source/modulo_new_core/src/translators/message_writers.cpp @@ -7,164 +7,179 @@ using namespace state_representation; namespace modulo_new_core::translators { -static void write_point(geometry_msgs::msg::Point& msg, const Eigen::Vector3d& vector) { - msg.x = vector.x(); - msg.y = vector.y(); - msg.z = vector.z(); +static void write_point(geometry_msgs::msg::Point& message, const Eigen::Vector3d& vector) { + message.x = vector.x(); + message.y = vector.y(); + message.z = vector.z(); } -static void write_vector3(geometry_msgs::msg::Vector3& msg, const Eigen::Vector3d& vector) { - msg.x = vector.x(); - msg.y = vector.y(); - msg.z = vector.z(); +static void write_vector3(geometry_msgs::msg::Vector3& message, const Eigen::Vector3d& vector) { + message.x = vector.x(); + message.y = vector.y(); + message.z = vector.z(); } -static void write_quaternion(geometry_msgs::msg::Quaternion& msg, const Eigen::Quaterniond& quat) { - msg.w = quat.w(); - msg.x = quat.x(); - msg.y = quat.y(); - msg.z = quat.z(); +static void write_quaternion(geometry_msgs::msg::Quaternion& message, const Eigen::Quaterniond& quat) { + message.w = quat.w(); + message.x = quat.x(); + message.y = quat.y(); + message.z = quat.z(); } -void write_msg(geometry_msgs::msg::Accel& msg, const CartesianState& state, const rclcpp::Time&) { +void write_message(geometry_msgs::msg::Accel& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } - write_vector3(msg.linear, state.get_linear_acceleration()); - write_vector3(msg.angular, state.get_angular_acceleration()); + write_vector3(message.linear, state.get_linear_acceleration()); + write_vector3(message.angular, state.get_angular_acceleration()); } -void write_msg(geometry_msgs::msg::AccelStamped& msg, const CartesianState& state, const rclcpp::Time& time) { - write_msg(msg.accel, state, time); - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); +void write_message(geometry_msgs::msg::AccelStamped& message, const CartesianState& state, const rclcpp::Time& time) { + write_message(message.accel, state, time); + message.header.stamp = time; + message.header.frame_id = state.get_reference_frame(); } -void write_msg(geometry_msgs::msg::Pose& msg, const CartesianState& state, const rclcpp::Time&) { +void write_message(geometry_msgs::msg::Pose& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } - write_point(msg.position, state.get_position()); - write_quaternion(msg.orientation, state.get_orientation()); + write_point(message.position, state.get_position()); + write_quaternion(message.orientation, state.get_orientation()); } -void write_msg(geometry_msgs::msg::PoseStamped& msg, const CartesianState& state, const rclcpp::Time& time) { - write_msg(msg.pose, state, time); - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); +void write_message(geometry_msgs::msg::PoseStamped& message, const CartesianState& state, const rclcpp::Time& time) { + write_message(message.pose, state, time); + message.header.stamp = time; + message.header.frame_id = state.get_reference_frame(); } -void write_msg(geometry_msgs::msg::Transform& msg, const CartesianState& state, const rclcpp::Time&) { +void write_message(geometry_msgs::msg::Transform& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } - write_vector3(msg.translation, state.get_position()); - write_quaternion(msg.rotation, state.get_orientation()); + write_vector3(message.translation, state.get_position()); + write_quaternion(message.rotation, state.get_orientation()); } -void write_msg(geometry_msgs::msg::TransformStamped& msg, const CartesianState& state, const rclcpp::Time& time) { - write_msg(msg.transform, state, time); - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); - msg.child_frame_id = state.get_name(); +void +write_message(geometry_msgs::msg::TransformStamped& message, const CartesianState& state, const rclcpp::Time& time) { + write_message(message.transform, state, time); + message.header.stamp = time; + message.header.frame_id = state.get_reference_frame(); + message.child_frame_id = state.get_name(); } -void write_msg(geometry_msgs::msg::Twist& msg, const CartesianState& state, const rclcpp::Time&) { +void write_message(geometry_msgs::msg::Twist& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } - write_vector3(msg.linear, state.get_linear_velocity()); - write_vector3(msg.angular, state.get_angular_velocity()); + write_vector3(message.linear, state.get_linear_velocity()); + write_vector3(message.angular, state.get_angular_velocity()); } -void write_msg(geometry_msgs::msg::TwistStamped& msg, const CartesianState& state, const rclcpp::Time& time) { - write_msg(msg.twist, state, time); - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); +void write_message(geometry_msgs::msg::TwistStamped& message, const CartesianState& state, const rclcpp::Time& time) { + write_message(message.twist, state, time); + message.header.stamp = time; + message.header.frame_id = state.get_reference_frame(); } -void write_msg(geometry_msgs::msg::Wrench& msg, const CartesianState& state, const rclcpp::Time&) { +void write_message(geometry_msgs::msg::Wrench& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } - write_vector3(msg.force, state.get_force()); - write_vector3(msg.torque, state.get_torque()); + write_vector3(message.force, state.get_force()); + write_vector3(message.torque, state.get_torque()); } -void write_msg(geometry_msgs::msg::WrenchStamped& msg, const CartesianState& state, const rclcpp::Time& time) { - write_msg(msg.wrench, state, time); - msg.header.stamp = time; - msg.header.frame_id = state.get_reference_frame(); +void write_message(geometry_msgs::msg::WrenchStamped& message, const CartesianState& state, const rclcpp::Time& time) { + write_message(message.wrench, state, time); + message.header.stamp = time; + message.header.frame_id = state.get_reference_frame(); } -void write_msg(sensor_msgs::msg::JointState& msg, const JointState& state, const rclcpp::Time& time) { +void write_message(sensor_msgs::msg::JointState& message, const JointState& state, const rclcpp::Time& time) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } - msg.header.stamp = time; - msg.name = state.get_names(); - msg.position = std::vector(state.get_positions().data(), state.get_positions().data() + state.get_positions().size()); - msg.velocity = std::vector(state.get_velocities().data(), state.get_velocities().data() + state.get_velocities().size()); - msg.effort = std::vector(state.get_torques().data(), state.get_torques().data() + state.get_torques().size()); + message.header.stamp = time; + message.name = state.get_names(); + message.position = + std::vector(state.get_positions().data(), state.get_positions().data() + state.get_positions().size()); + message.velocity = + std::vector(state.get_velocities().data(), state.get_velocities().data() + state.get_velocities().size()); + message.effort = + std::vector(state.get_torques().data(), state.get_torques().data() + state.get_torques().size()); } -void write_msg(tf2_msgs::msg::TFMessage& msg, const CartesianState& state, const rclcpp::Time& time) { +void write_message(tf2_msgs::msg::TFMessage& message, const CartesianState& state, const rclcpp::Time& time) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } geometry_msgs::msg::TransformStamped transform; - write_msg(transform, state, time); - msg.transforms.push_back(transform); + write_message(transform, state, time); + message.transforms.push_back(transform); } template -void write_msg(U& msg, const Parameter& state, const rclcpp::Time&) { +void write_message(U& message, const Parameter& state, const rclcpp::Time&) { if (state.is_empty()) { throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); } - msg.data = state.get_value(); + message.data = state.get_value(); } -template void write_msg(std_msgs::msg::Float64& msg, const Parameter& state, const rclcpp::Time&); +template void write_message( + std_msgs::msg::Float64& message, const Parameter& state, const rclcpp::Time& +); -template void write_msg>(std_msgs::msg::Float64MultiArray& msg, const Parameter>& state, const rclcpp::Time&); +template void write_message>( + std_msgs::msg::Float64MultiArray& message, const Parameter>& state, const rclcpp::Time& +); -template void write_msg(std_msgs::msg::Bool& msg, const Parameter& state, const rclcpp::Time&); +template void write_message( + std_msgs::msg::Bool& message, const Parameter& state, const rclcpp::Time& +); -template void write_msg(std_msgs::msg::String& msg, const Parameter& state, const rclcpp::Time&); +template void write_message( + std_msgs::msg::String& message, const Parameter& state, const rclcpp::Time& +); template<> -void write_msg(geometry_msgs::msg::Transform& msg, const Parameter& state, const rclcpp::Time& time) { - write_msg(msg, state.get_value(), time); +void +write_message(geometry_msgs::msg::Transform& message, const Parameter& state, const rclcpp::Time& time) { + write_message(message, state.get_value(), time); } template<> -void write_msg(geometry_msgs::msg::TransformStamped& msg, const Parameter& state, const rclcpp::Time& time) { - write_msg(msg, state.get_value(), time); +void write_message( + geometry_msgs::msg::TransformStamped& message, const Parameter& state, const rclcpp::Time& time +) { + write_message(message, state.get_value(), time); } template<> -void write_msg(tf2_msgs::msg::TFMessage& msg, const Parameter& state, const rclcpp::Time& time) { - write_msg(msg, state.get_value(), time); +void write_message(tf2_msgs::msg::TFMessage& message, const Parameter& state, const rclcpp::Time& time) { + write_message(message, state.get_value(), time); } -void write_msg(std_msgs::msg::Bool& msg, const bool& state, const rclcpp::Time&) { - msg.data = state; +void write_message(std_msgs::msg::Bool& message, const bool& state, const rclcpp::Time&) { + message.data = state; } -void write_msg(std_msgs::msg::Float64& msg, const double& state, const rclcpp::Time&) { - msg.data = state; +void write_message(std_msgs::msg::Float64& message, const double& state, const rclcpp::Time&) { + message.data = state; } -void write_msg(std_msgs::msg::Float64MultiArray& msg, const std::vector& state, const rclcpp::Time&) { - msg.data = state; +void write_message(std_msgs::msg::Float64MultiArray& message, const std::vector& state, const rclcpp::Time&) { + message.data = state; } -void write_msg(std_msgs::msg::Int32& msg, const int& state, const rclcpp::Time&) { - msg.data = state; +void write_message(std_msgs::msg::Int32& message, const int& state, const rclcpp::Time&) { + message.data = state; } -void write_msg(std_msgs::msg::String& msg, const std::string& state, const rclcpp::Time&) { - msg.data = state; +void write_message(std_msgs::msg::String& message, const std::string& state, const rclcpp::Time&) { + message.data = state; } }// namespace modulo_new_core::translators \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp index 920a0061c..b60721675 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp @@ -34,8 +34,8 @@ class MinimalSubscriber : public rclcpp::Node { this->received_future = this->received_.get_future(); this->subscription_interface_ = std::make_shared>(message_pair); auto subscription = this->create_subscription( - topic_name, 10, [this](const std::shared_ptr msg) { - this->subscription_interface_->template get_handler()->get_callback()(msg); + topic_name, 10, [this](const std::shared_ptr message) { + this->subscription_interface_->template get_handler()->get_callback()(message); this->received_.set_value(); } ); diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp index ee3725a2f..1aefd323c 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp @@ -9,24 +9,24 @@ static void test_message_interface( const DataT& initial_value, const DataT& new_value, const std::shared_ptr clock ) { auto data = std::make_shared(initial_value); - auto msg_pair = std::make_shared>(data, clock); - EXPECT_EQ(initial_value, *msg_pair->get_data()); - EXPECT_EQ(initial_value, msg_pair->write_message().data); + auto message_pair = std::make_shared>(data, clock); + EXPECT_EQ(initial_value, *message_pair->get_data()); + EXPECT_EQ(initial_value, message_pair->write_message().data); - std::shared_ptr msg_pair_interface(msg_pair); - auto msg = msg_pair_interface->template write(); - EXPECT_EQ(initial_value, msg.data); + std::shared_ptr message_pair_interface(message_pair); + auto message = message_pair_interface->template write(); + EXPECT_EQ(initial_value, message.data); *data = new_value; - EXPECT_EQ(new_value, *msg_pair->get_data()); - EXPECT_EQ(new_value, msg_pair->write_message().data); - msg = msg_pair_interface->template write(); - EXPECT_EQ(new_value, msg.data); + EXPECT_EQ(new_value, *message_pair->get_data()); + EXPECT_EQ(new_value, message_pair->write_message().data); + message = message_pair_interface->template write(); + EXPECT_EQ(new_value, message.data); - msg = MsgT(); - msg.data = initial_value; - msg_pair_interface->template read(msg); - EXPECT_EQ(initial_value, *msg_pair->get_data()); + message = MsgT(); + message.data = initial_value; + message_pair_interface->template read(message); + EXPECT_EQ(initial_value, *message_pair->get_data()); } class MessagePairTest : public ::testing::Test { @@ -51,29 +51,29 @@ TEST_F(MessagePairTest, BasicTypes) { TEST_F(MessagePairTest, EncodedState) { auto initial_value = state_representation::CartesianState::Random("test"); auto data = state_representation::make_shared_state(initial_value); - auto msg_pair = + auto message_pair = std::make_shared>(data, clock_); EXPECT_TRUE(initial_value.data().isApprox( - std::dynamic_pointer_cast(msg_pair->get_data())->data())); + std::dynamic_pointer_cast(message_pair->get_data())->data())); - std::shared_ptr msg_pair_interface(msg_pair); - auto msg = msg_pair_interface->write(); - std::string tmp(msg.data.begin(), msg.data.end()); + std::shared_ptr message_pair_interface(message_pair); + auto message = message_pair_interface->write(); + std::string tmp(message.data.begin(), message.data.end()); auto decoded = clproto::decode(tmp); EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); auto new_value = state_representation::CartesianState::Identity("world"); - msg_pair->set_data(state_representation::make_shared_state(new_value)); - msg = msg_pair_interface->write(); - tmp = std::string(msg.data.begin(), msg.data.end()); + message_pair->set_data(state_representation::make_shared_state(new_value)); + message = message_pair_interface->write(); + tmp = std::string(message.data.begin(), message.data.end()); decoded = clproto::decode(tmp); EXPECT_TRUE(new_value.data().isApprox(decoded.data())); data = state_representation::make_shared_state(initial_value); - msg_pair->set_data(data); - msg = modulo_new_core::EncodedState(); - modulo_new_core::translators::write_msg(msg, data, clock_->now()); - msg_pair_interface->read(msg); + message_pair->set_data(data); + message = modulo_new_core::EncodedState(); + modulo_new_core::translators::write_message(message, data, clock_->now()); + message_pair_interface->read(message); EXPECT_TRUE(initial_value.data().isApprox( - std::dynamic_pointer_cast(msg_pair->get_data())->data())); + std::dynamic_pointer_cast(message_pair->get_data())->data())); } diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp index 0a4ce6a77..694732398 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp @@ -12,7 +12,7 @@ template static void test_publisher_interface(const std::shared_ptr& node, const DataT& value) { // create message pair auto data = std::make_shared(value); - auto msg_pair = std::make_shared>(data, node->get_clock()); + auto message_pair = std::make_shared>(data, node->get_clock()); // create publisher handler auto publisher = node->create_publisher("topic", 10); @@ -22,10 +22,10 @@ static void test_publisher_interface(const std::shared_ptr& node, // use in publisher interface std::shared_ptr publisher_interface(publisher_handler); EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); - publisher_interface->set_message_pair(msg_pair); + publisher_interface->set_message_pair(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); - auto publisher_interface_ptr = publisher_handler->create_publisher_interface(msg_pair); + auto publisher_interface_ptr = publisher_handler->create_publisher_interface(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); } @@ -59,7 +59,7 @@ TEST_F(PublisherTest, EncodedState) { // create message pair auto data = std::make_shared(state_representation::CartesianState::Random("test")); - auto msg_pair = std::make_shared>( + auto message_pair = std::make_shared>( data, node->get_clock()); // create publisher handler @@ -72,9 +72,9 @@ TEST_F(PublisherTest, EncodedState) { // use in publisher interface std::shared_ptr publisher_interface(publisher_handler); EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); - publisher_interface->set_message_pair(msg_pair); + publisher_interface->set_message_pair(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); - publisher_interface = publisher_handler->create_publisher_interface(msg_pair); + publisher_interface = publisher_handler->create_publisher_interface(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); } \ No newline at end of file diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp index f03bb80e5..fc4c60b03 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp @@ -11,10 +11,10 @@ template static void test_subscription_interface(const std::shared_ptr& node, const DataT& value) { // create message pair auto data = std::make_shared(value); - auto msg_pair = std::make_shared>(data, node->get_clock()); + auto message_pair = std::make_shared>(data, node->get_clock()); // create subscription handler - auto subscription_handler = std::make_shared>(msg_pair); + auto subscription_handler = std::make_shared>(message_pair); auto subscription = node->template create_subscription( "topic", 10, subscription_handler->get_callback()); @@ -52,11 +52,11 @@ TEST_F(SubscriptionTest, EncodedState) { // create message pair auto data = std::make_shared(state_representation::CartesianState::Random("test")); - auto msg_pair = std::make_shared>( + auto message_pair = std::make_shared>( data, node->get_clock()); // create subscription handler - auto subscription_handler = std::make_shared>(msg_pair); + auto subscription_handler = std::make_shared>(message_pair); auto subscription = node->create_subscription( "topic", 10, subscription_handler->get_callback()); diff --git a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp index 546e6bc24..7ba286208 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp @@ -10,11 +10,11 @@ using namespace modulo_new_core::translators; template static void test_std_messages(const DataT& state, const rclcpp::Time& time) { - auto msg = MsgT(); - write_msg(msg, state, time); - EXPECT_EQ(msg.data, state); + auto message = MsgT(); + write_message(message, state, time); + EXPECT_EQ(message.data, state); DataT new_state; - read_msg(new_state, msg); + read_message(new_state, message); EXPECT_EQ(state, new_state); } @@ -50,67 +50,67 @@ class MessageTranslatorsTest : public ::testing::Test { TEST_F(MessageTranslatorsTest, TestAccel) { auto accel = geometry_msgs::msg::Accel(); - write_msg(accel, state_, clock_.now()); + write_message(accel, state_, clock_.now()); expect_vector_equal(state_.get_linear_acceleration(), accel.linear); expect_vector_equal(state_.get_angular_acceleration(), accel.angular); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_msg(accel, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); - read_msg(new_state, accel); + EXPECT_THROW(write_message(accel, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_message(new_state, accel); EXPECT_TRUE(new_state.get_acceleration().isApprox(state_.get_acceleration())); auto accel_stamped = geometry_msgs::msg::AccelStamped(); - write_msg(accel_stamped, state_, clock_.now()); + write_message(accel_stamped, state_, clock_.now()); EXPECT_EQ(state_.get_reference_frame(), accel_stamped.header.frame_id); expect_vector_equal(state_.get_linear_acceleration(), accel_stamped.accel.linear); expect_vector_equal(state_.get_angular_acceleration(), accel_stamped.accel.angular); new_state = state_representation::CartesianState("new"); - read_msg(new_state, accel_stamped); + read_message(new_state, accel_stamped); EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); EXPECT_TRUE(new_state.get_acceleration().isApprox(state_.get_acceleration())); } TEST_F(MessageTranslatorsTest, TestPose) { auto pose = geometry_msgs::msg::Pose(); - write_msg(pose, state_, clock_.now()); + write_message(pose, state_, clock_.now()); expect_point_equal(state_.get_position(), pose.position); expect_quaternion_equal(state_.get_orientation(), pose.orientation); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_msg(pose, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); - read_msg(new_state, pose); + EXPECT_THROW(write_message(pose, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_message(new_state, pose); EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); auto pose_stamped = geometry_msgs::msg::PoseStamped(); - write_msg(pose_stamped, state_, clock_.now()); + write_message(pose_stamped, state_, clock_.now()); EXPECT_EQ(state_.get_reference_frame(), pose_stamped.header.frame_id); expect_point_equal(state_.get_position(), pose_stamped.pose.position); expect_quaternion_equal(state_.get_orientation(), pose_stamped.pose.orientation); new_state = state_representation::CartesianState("new"); - read_msg(new_state, pose_stamped); + read_message(new_state, pose_stamped); EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); } TEST_F(MessageTranslatorsTest, TestTransform) { auto tf = geometry_msgs::msg::Transform(); - write_msg(tf, state_, clock_.now()); + write_message(tf, state_, clock_.now()); expect_vector_equal(state_.get_position(), tf.translation); expect_quaternion_equal(state_.get_orientation(), tf.rotation); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_msg(tf, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); - read_msg(new_state, tf); + EXPECT_THROW(write_message(tf, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_message(new_state, tf); EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); auto tf_stamped = geometry_msgs::msg::TransformStamped(); - write_msg(tf_stamped, state_, clock_.now()); + write_message(tf_stamped, state_, clock_.now()); EXPECT_EQ(state_.get_name(), tf_stamped.child_frame_id); EXPECT_EQ(state_.get_reference_frame(), tf_stamped.header.frame_id); expect_vector_equal(state_.get_position(), tf_stamped.transform.translation); expect_quaternion_equal(state_.get_orientation(), tf_stamped.transform.rotation); new_state = state_representation::CartesianState("new"); - read_msg(new_state, tf_stamped); + read_message(new_state, tf_stamped); EXPECT_EQ(new_state.get_name(), state_.get_name()); EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); @@ -118,61 +118,61 @@ TEST_F(MessageTranslatorsTest, TestTransform) { TEST_F(MessageTranslatorsTest, TestTwist) { auto twist = geometry_msgs::msg::Twist(); - write_msg(twist, state_, clock_.now()); + write_message(twist, state_, clock_.now()); expect_vector_equal(state_.get_linear_velocity(), twist.linear); expect_vector_equal(state_.get_angular_velocity(), twist.angular); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_msg(twist, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); - read_msg(new_state, twist); + EXPECT_THROW(write_message(twist, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_message(new_state, twist); EXPECT_TRUE(new_state.get_twist().isApprox(state_.get_twist())); auto twist_stamped = geometry_msgs::msg::TwistStamped(); - write_msg(twist_stamped, state_, clock_.now()); + write_message(twist_stamped, state_, clock_.now()); EXPECT_EQ(state_.get_reference_frame(), twist_stamped.header.frame_id); expect_vector_equal(state_.get_linear_velocity(), twist_stamped.twist.linear); expect_vector_equal(state_.get_angular_velocity(), twist_stamped.twist.angular); new_state = state_representation::CartesianState("new"); - read_msg(new_state, twist_stamped); + read_message(new_state, twist_stamped); EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); EXPECT_TRUE(new_state.get_twist().isApprox(state_.get_twist())); } TEST_F(MessageTranslatorsTest, TestWrench) { auto wrench = geometry_msgs::msg::Wrench(); - write_msg(wrench, state_, clock_.now()); + write_message(wrench, state_, clock_.now()); expect_vector_equal(state_.get_force(), wrench.force); expect_vector_equal(state_.get_torque(), wrench.torque); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_msg(wrench, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); - read_msg(new_state, wrench); + EXPECT_THROW(write_message(wrench, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_message(new_state, wrench); EXPECT_TRUE(new_state.get_wrench().isApprox(state_.get_wrench())); auto wrench_stamped = geometry_msgs::msg::WrenchStamped(); - write_msg(wrench_stamped, state_, clock_.now()); + write_message(wrench_stamped, state_, clock_.now()); EXPECT_EQ(state_.get_reference_frame(), wrench_stamped.header.frame_id); expect_vector_equal(state_.get_force(), wrench_stamped.wrench.force); expect_vector_equal(state_.get_torque(), wrench_stamped.wrench.torque); new_state = state_representation::CartesianState("new"); - read_msg(new_state, wrench_stamped); + read_message(new_state, wrench_stamped); EXPECT_EQ(new_state.get_reference_frame(), state_.get_reference_frame()); EXPECT_TRUE(new_state.get_wrench().isApprox(state_.get_wrench())); } TEST_F(MessageTranslatorsTest, TestJointState) { - auto msg = sensor_msgs::msg::JointState(); - write_msg(msg, joint_state_, clock_.now()); + auto message = sensor_msgs::msg::JointState(); + write_message(message, joint_state_, clock_.now()); for (unsigned int i = 0; i < joint_state_.get_size(); ++i) { - EXPECT_EQ(msg.name.at(i), joint_state_.get_names().at(i)); - EXPECT_NEAR(msg.position.at(i), joint_state_.get_position(i), 1e-5); - EXPECT_NEAR(msg.velocity.at(i), joint_state_.get_velocity(i), 1e-5); - EXPECT_NEAR(msg.effort.at(i), joint_state_.get_torque(i), 1e-5); + EXPECT_EQ(message.name.at(i), joint_state_.get_names().at(i)); + EXPECT_NEAR(message.position.at(i), joint_state_.get_position(i), 1e-5); + EXPECT_NEAR(message.velocity.at(i), joint_state_.get_velocity(i), 1e-5); + EXPECT_NEAR(message.effort.at(i), joint_state_.get_torque(i), 1e-5); } auto new_state = state_representation::JointState("test", {"1", "2", "3"}); - EXPECT_THROW(write_msg(msg, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); - read_msg(new_state, msg); + EXPECT_THROW(write_message(message, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + read_message(new_state, message); EXPECT_TRUE(new_state.get_positions().isApprox(joint_state_.get_positions())); EXPECT_TRUE(new_state.get_velocities().isApprox(joint_state_.get_velocities())); EXPECT_TRUE(new_state.get_torques().isApprox(joint_state_.get_torques())); @@ -187,22 +187,22 @@ TEST_F(MessageTranslatorsTest, TestStdMsgs) { test_std_messages(2, clock_.now()); test_std_messages("test", clock_.now()); - auto msg = std_msgs::msg::Float64MultiArray(); + auto message = std_msgs::msg::Float64MultiArray(); std::vector state = {0.3, 0.6}; - write_msg(msg, state, clock_.now()); + write_message(message, state, clock_.now()); std::vector new_state; - read_msg(new_state, msg); + read_message(new_state, message); for (std::size_t i = 0; i < state.size(); ++i) { - EXPECT_EQ(msg.data.at(i), state.at(i)); + EXPECT_EQ(message.data.at(i), state.at(i)); EXPECT_EQ(state.at(i), new_state.at(i)); } } TEST_F(MessageTranslatorsTest, TestEncodedState) { - auto msg = modulo_new_core::EncodedState(); - write_msg(msg, state_, clock_.now()); + auto message = modulo_new_core::EncodedState(); + write_message(message, state_, clock_.now()); state_representation::CartesianState new_state; - read_msg(new_state, msg); + read_message(new_state, message); EXPECT_TRUE(state_.data().isApprox(new_state.data())); EXPECT_EQ(state_.get_name(), new_state.get_name()); EXPECT_EQ(state_.get_reference_frame(), new_state.get_reference_frame()); @@ -210,10 +210,10 @@ TEST_F(MessageTranslatorsTest, TestEncodedState) { TEST_F(MessageTranslatorsTest, TestEncodedStatePointer) { auto state_ptr = state_representation::make_shared_state(state_); - auto msg = modulo_new_core::EncodedState(); - write_msg(msg, state_ptr, clock_.now()); + auto message = modulo_new_core::EncodedState(); + write_message(message, state_ptr, clock_.now()); auto new_state_ptr = state_representation::make_shared_state(state_representation::CartesianState()); - read_msg(new_state_ptr, msg); + read_message(new_state_ptr, message); auto new_state = *std::dynamic_pointer_cast(new_state_ptr); EXPECT_TRUE(state_.data().isApprox(new_state.data())); EXPECT_EQ(state_.get_name(), new_state.get_name()); diff --git a/source/modulo_new_core/test/python_tests/translators/test_messages.py b/source/modulo_new_core/test/python_tests/translators/test_messages.py index 4f6f2e551..f14d8a085 100644 --- a/source/modulo_new_core/test/python_tests/translators/test_messages.py +++ b/source/modulo_new_core/test/python_tests/translators/test_messages.py @@ -10,12 +10,12 @@ import modulo_new_core.translators.message_writers as modulo_writers -def read_xyz(msg): - return [msg.x, msg.y, msg.z] +def read_xyz(message): + return [message.x, message.y, message.z] -def read_quaternion(msg): - return [msg.w, msg.x, msg.y, msg.z] +def read_quaternion(message): + return [message.w, message.x, message.y, message.z] def assert_np_array_equal(a: np.array, b: np.array, places=3): @@ -26,135 +26,135 @@ def assert_np_array_equal(a: np.array, b: np.array, places=3): def test_accel(cart_state: sr.CartesianState, clock: Clock): - msg = geometry_msgs.msg.Accel() - modulo_writers.write_msg(msg, cart_state) - assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_acceleration()) - assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_acceleration()) + message = geometry_msgs.msg.Accel() + modulo_writers.write_message(message, cart_state) + assert_np_array_equal(read_xyz(message.linear), cart_state.get_linear_acceleration()) + assert_np_array_equal(read_xyz(message.angular), cart_state.get_angular_acceleration()) new_state = sr.CartesianState("new") with pytest.raises(RuntimeError): - modulo_writers.write_msg(new_state, msg) - modulo_readers.read_msg(new_state, msg) + modulo_writers.write_message(new_state, message) + modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) - msg_stamped = geometry_msgs.msg.AccelStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) - assert msg_stamped.header.frame_id == cart_state.get_reference_frame() - assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_acceleration()) - assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_acceleration()) + message_stamped = geometry_msgs.msg.AccelStamped() + modulo_writers.write_stamped_message(message_stamped, cart_state, clock.now()) + assert message_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(message.linear), cart_state.get_linear_acceleration()) + assert_np_array_equal(read_xyz(message.angular), cart_state.get_angular_acceleration()) new_state = sr.CartesianState("new") - modulo_readers.read_stamped_msg(new_state, msg_stamped) + modulo_readers.read_stamped_message(new_state, message_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) def test_pose(cart_state: sr.CartesianState, clock: Clock): - msg = geometry_msgs.msg.Pose() - modulo_writers.write_msg(msg, cart_state) - assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation_coefficients()) + message = geometry_msgs.msg.Pose() + modulo_writers.write_message(message, cart_state) + assert_np_array_equal(read_xyz(message.position), cart_state.get_position()) + assert_np_array_equal(read_quaternion(message.orientation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") with pytest.raises(RuntimeError): - modulo_writers.write_msg(new_state, msg) - modulo_readers.read_msg(new_state, msg) + modulo_writers.write_message(new_state, message) + modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) - msg_stamped = geometry_msgs.msg.PoseStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) - assert msg_stamped.header.frame_id == cart_state.get_reference_frame() - assert_np_array_equal(read_xyz(msg.position), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.orientation), cart_state.get_orientation_coefficients()) + message_stamped = geometry_msgs.msg.PoseStamped() + modulo_writers.write_stamped_message(message_stamped, cart_state, clock.now()) + assert message_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(message.position), cart_state.get_position()) + assert_np_array_equal(read_quaternion(message.orientation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") - modulo_readers.read_stamped_msg(new_state, msg_stamped) + modulo_readers.read_stamped_message(new_state, message_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) def test_transform(cart_state: sr.CartesianState, clock: Clock): - msg = geometry_msgs.msg.Transform() - modulo_writers.write_msg(msg, cart_state) - assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation_coefficients()) + message = geometry_msgs.msg.Transform() + modulo_writers.write_message(message, cart_state) + assert_np_array_equal(read_xyz(message.translation), cart_state.get_position()) + assert_np_array_equal(read_quaternion(message.rotation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") with pytest.raises(RuntimeError): - modulo_writers.write_msg(new_state, msg) - modulo_readers.read_msg(new_state, msg) + modulo_writers.write_message(new_state, message) + modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) - msg_stamped = geometry_msgs.msg.TransformStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) - assert msg_stamped.header.frame_id == cart_state.get_reference_frame() - assert msg_stamped.child_frame_id == cart_state.get_name() - assert_np_array_equal(read_xyz(msg.translation), cart_state.get_position()) - assert_np_array_equal(read_quaternion(msg.rotation), cart_state.get_orientation_coefficients()) + message_stamped = geometry_msgs.msg.TransformStamped() + modulo_writers.write_stamped_message(message_stamped, cart_state, clock.now()) + assert message_stamped.header.frame_id == cart_state.get_reference_frame() + assert message_stamped.child_frame_id == cart_state.get_name() + assert_np_array_equal(read_xyz(message.translation), cart_state.get_position()) + assert_np_array_equal(read_quaternion(message.rotation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") - modulo_readers.read_stamped_msg(new_state, msg_stamped) + modulo_readers.read_stamped_message(new_state, message_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert cart_state.get_name() == new_state.get_name() assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) def test_twist(cart_state: sr.CartesianState, clock: Clock): - msg = geometry_msgs.msg.Twist() - modulo_writers.write_msg(msg, cart_state) - assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_velocity()) - assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_velocity()) + message = geometry_msgs.msg.Twist() + modulo_writers.write_message(message, cart_state) + assert_np_array_equal(read_xyz(message.linear), cart_state.get_linear_velocity()) + assert_np_array_equal(read_xyz(message.angular), cart_state.get_angular_velocity()) new_state = sr.CartesianState("new") with pytest.raises(RuntimeError): - modulo_writers.write_msg(new_state, msg) - modulo_readers.read_msg(new_state, msg) + modulo_writers.write_message(new_state, message) + modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) - msg_stamped = geometry_msgs.msg.TwistStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) - assert msg_stamped.header.frame_id == cart_state.get_reference_frame() - assert_np_array_equal(read_xyz(msg.linear), cart_state.get_linear_velocity()) - assert_np_array_equal(read_xyz(msg.angular), cart_state.get_angular_velocity()) + message_stamped = geometry_msgs.msg.TwistStamped() + modulo_writers.write_stamped_message(message_stamped, cart_state, clock.now()) + assert message_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(message.linear), cart_state.get_linear_velocity()) + assert_np_array_equal(read_xyz(message.angular), cart_state.get_angular_velocity()) new_state = sr.CartesianState("new") - modulo_readers.read_stamped_msg(new_state, msg_stamped) + modulo_readers.read_stamped_message(new_state, message_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) def test_wrench(cart_state: sr.CartesianState, clock: Clock): - msg = geometry_msgs.msg.Wrench() - modulo_writers.write_msg(msg, cart_state) - assert_np_array_equal(read_xyz(msg.force), cart_state.get_force()) - assert_np_array_equal(read_xyz(msg.torque), cart_state.get_torque()) + message = geometry_msgs.msg.Wrench() + modulo_writers.write_message(message, cart_state) + assert_np_array_equal(read_xyz(message.force), cart_state.get_force()) + assert_np_array_equal(read_xyz(message.torque), cart_state.get_torque()) new_state = sr.CartesianState("new") with pytest.raises(RuntimeError): - modulo_writers.write_msg(new_state, msg) - modulo_readers.read_msg(new_state, msg) + modulo_writers.write_message(new_state, message) + modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) - msg_stamped = geometry_msgs.msg.WrenchStamped() - modulo_writers.write_stamped_msg(msg_stamped, cart_state, clock.now()) - assert msg_stamped.header.frame_id == cart_state.get_reference_frame() - assert_np_array_equal(read_xyz(msg.force), cart_state.get_force()) - assert_np_array_equal(read_xyz(msg.torque), cart_state.get_torque()) + message_stamped = geometry_msgs.msg.WrenchStamped() + modulo_writers.write_stamped_message(message_stamped, cart_state, clock.now()) + assert message_stamped.header.frame_id == cart_state.get_reference_frame() + assert_np_array_equal(read_xyz(message.force), cart_state.get_force()) + assert_np_array_equal(read_xyz(message.torque), cart_state.get_torque()) new_state = sr.CartesianState("new") - modulo_readers.read_stamped_msg(new_state, msg_stamped) + modulo_readers.read_stamped_message(new_state, message_stamped) assert cart_state.get_reference_frame() == new_state.get_reference_frame() assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) def test_joint_state(joint_state: sr.JointState): - msg = sensor_msgs.msg.JointState() - modulo_writers.write_msg(msg, joint_state) + message = sensor_msgs.msg.JointState() + modulo_writers.write_message(message, joint_state) for i in range(joint_state.get_size()): - assert msg.name[i] == joint_state.get_names()[i] - assert_np_array_equal(msg.position, joint_state.get_positions()) - assert_np_array_equal(msg.velocity, joint_state.get_velocities()) - assert_np_array_equal(msg.effort, joint_state.get_torques()) + assert message.name[i] == joint_state.get_names()[i] + assert_np_array_equal(message.position, joint_state.get_positions()) + assert_np_array_equal(message.velocity, joint_state.get_velocities()) + assert_np_array_equal(message.effort, joint_state.get_torques()) new_state = sr.JointState("test", ["1", "2", "3"]) with pytest.raises(RuntimeError): - modulo_writers.write_msg(new_state, msg) - modulo_readers.read_msg(new_state, msg) + modulo_writers.write_message(new_state, message) + modulo_readers.read_message(new_state, message) for i in range(joint_state.get_size()): assert new_state.get_names()[i] == joint_state.get_names()[i] assert_np_array_equal(new_state.get_positions(), joint_state.get_positions()) @@ -163,9 +163,9 @@ def test_joint_state(joint_state: sr.JointState): def test_encoded_state(cart_state: sr.CartesianState): - msg = modulo_writers.write_clproto_msg(cart_state, clproto.MessageType.CARTESIAN_STATE_MESSAGE) + message = modulo_writers.write_clproto_message(cart_state, clproto.MessageType.CARTESIAN_STATE_MESSAGE) - new_state = modulo_readers.read_clproto_msg(msg) + new_state = modulo_readers.read_clproto_message(message) assert_np_array_equal(new_state.data(), cart_state.data()) assert new_state.get_name() == cart_state.get_name() assert new_state.get_reference_frame() == cart_state.get_reference_frame() From 7ac128fefd20edd4f574aa3a8b837f458e38121e Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 17 Jun 2022 13:41:18 +0200 Subject: [PATCH 40/71] Extend documentation and consistent formatting (#80) * Update docstrings and have an empty line at the end of the file * Update docstrings in Python * Update docstrings and empty line at the end of the file in C++ modulo_components * Update docstrings in Python modulo_components --- .../include/modulo_components/Component.hpp | 7 +- .../modulo_components/ComponentInterface.hpp | 35 ++++++---- .../modulo_components/LifecycleComponent.hpp | 9 ++- .../exceptions/AddSignalException.hpp | 6 +- .../exceptions/ComponentException.hpp | 3 +- .../ComponentParameterException.hpp | 7 +- .../exceptions/LookupTransformException.hpp | 7 +- .../utilities/predicate_variant.hpp | 2 +- .../modulo_components/utilities/utilities.hpp | 3 +- .../modulo_components/component.py | 8 +-- .../modulo_components/component_interface.py | 23 +++---- .../exceptions/component_exceptions.py | 17 ++++- source/modulo_components/src/Component.cpp | 3 +- .../src/LifecycleComponent.cpp | 5 +- .../component_public_interfaces.hpp | 4 +- .../test/cpp/test_component.cpp | 5 +- .../test/cpp/test_component_interface.cpp | 2 +- .../test_component_interface_parameters.cpp | 3 +- .../test/cpp/test_lifecycle_component.cpp | 5 +- .../include/modulo_new_core/EncodedState.hpp | 2 +- .../communication/MessagePair.hpp | 10 +-- .../communication/MessagePairInterface.hpp | 38 ++++++----- .../communication/MessageType.hpp | 3 +- .../communication/PublisherHandler.hpp | 14 ++-- .../communication/PublisherInterface.hpp | 65 ++++++++++--------- .../communication/PublisherType.hpp | 3 +- .../communication/SubscriptionHandler.hpp | 31 ++++++++- .../communication/SubscriptionInterface.hpp | 48 +++++++++++++- .../IncompatibleParameterException.hpp | 9 ++- .../InvalidPointerCastException.hpp | 8 ++- .../exceptions/InvalidPointerException.hpp | 8 ++- .../NotLifecyclePublisherException.hpp | 11 ---- .../exceptions/NullPointerException.hpp | 8 ++- .../translators/message_readers.hpp | 2 +- .../translators/message_writers.hpp | 2 +- .../translators/parameter_translators.hpp | 21 ++++-- .../translators/message_readers.py | 17 ++--- .../translators/message_writers.py | 16 +++-- .../translators/parameter_translators.py | 54 +++++++++++++-- .../src/communication/MessagePair.cpp | 5 +- .../communication/MessagePairInterface.cpp | 3 +- .../src/communication/PublisherInterface.cpp | 3 +- .../src/communication/SubscriptionHandler.cpp | 3 +- .../communication/SubscriptionInterface.cpp | 5 +- .../src/translators/message_readers.cpp | 2 +- .../src/translators/message_writers.cpp | 2 +- .../src/translators/parameter_translators.cpp | 12 ++-- .../communication/test_publisher_handler.cpp | 2 +- .../test_subscription_handler.cpp | 2 +- .../test_parameter_eigen_translators.cpp | 2 +- .../test_parameter_state_translators.cpp | 2 +- .../cpp_tests/translators/test_messages.cpp | 2 +- 52 files changed, 363 insertions(+), 206 deletions(-) delete mode 100644 source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 993ea5abb..e978e51bc 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -1,6 +1,7 @@ #pragma once #include + #include #include "modulo_components/ComponentInterface.hpp" @@ -9,6 +10,9 @@ namespace modulo_components { +/** + * @brief TODO + */ class Component : public ComponentInterface { public: friend class ComponentPublicInterface; @@ -147,5 +151,4 @@ inline void Component::add_output( RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); } } - -}// namespace modulo_components \ No newline at end of file +}// namespace modulo_components diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 73fb3e50b..20ac2bf47 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -1,5 +1,6 @@ #pragma once +// TODO sort includes #include #include #include @@ -30,9 +31,17 @@ namespace modulo_components { +/** + * @brief TODO + * @tparam NodeT + */ template class ComponentInterfacePublicInterface; +/** + * @brief TODO + * @tparam NodeT + */ template class ComponentInterface : private NodeT { public: @@ -65,8 +74,8 @@ class ComponentInterface : private NodeT { /** * @brief Add a parameter. - * @details This method stores a pointer reference to an existing Parameter object in the local parameter map - * and declares the equivalent ROS parameter on the ROS interface. + * @details This method stores a pointer reference to an existing Parameter object in the local parameter map and + * declares the equivalent ROS parameter on the ROS interface. * @param parameter A ParameterInterface pointer to a Parameter instance * @param description The description of the parameter * @param read_only If true, the value of the parameter cannot be changed after declaration @@ -78,8 +87,8 @@ class ComponentInterface : private NodeT { /** * @brief Add a parameter. - * @details This method creates a new Parameter object instance to reference in the local parameter map - * and declares the equivalent ROS parameter on the ROS interface. + * @details This method creates a new Parameter object instance to reference in the local parameter map and declares + * the equivalent ROS parameter on the ROS interface. * @tparam T The type of the parameter * @param name The name of the parameter * @param value The value of the parameter @@ -109,8 +118,8 @@ class ComponentInterface : private NodeT { /** * @brief Set the value of a parameter. - * @details The parameter must have been previously declared. This method preserves the reference - * to the original Parameter instance + * @details The parameter must have been previously declared. This method preserves the reference to the original + * Parameter instance * @tparam T The type of the parameter * @param name The name of the parameter * @return The value of the parameter @@ -120,11 +129,11 @@ class ComponentInterface : private NodeT { /** * @brief Parameter validation function to be redefined by derived Component classes. - * @details This method is automatically invoked whenever the ROS interface tried to modify a parameter. - * Validation and sanitization can be performed by reading or writing the value of the parameter through the - * ParameterInterface pointer, depending on the parameter name and desired component behaviour. If the validation - * returns true, the updated parameter value (including any modifications) is applied. If the validation returns - * false, any changes to the parameter are discarded and the parameter value is not changed. + * @details This method is automatically invoked whenever the ROS interface tried to modify a parameter. Validation + * and sanitization can be performed by reading or writing the value of the parameter through the ParameterInterface + * pointer, depending on the parameter name and desired component behaviour. If the validation returns true, the + * updated parameter value (including any modifications) is applied. If the validation returns false, any changes to + * the parameter are discarded and the parameter value is not changed. * @param parameter A ParameterInterface pointer to a Parameter instance * @return The validation result */ @@ -284,8 +293,7 @@ class ComponentInterface : private NodeT { void evaluate_periodic_callbacks(); /** - * @brief Put the component in error state by setting the - * 'in_error_state' predicate to true. + * @brief Put the component in error state by setting the 'in_error_state' predicate to true. */ virtual void raise_error(); @@ -851,5 +859,4 @@ template inline void ComponentInterface::set_qos(const rclcpp::QoS& qos) { this->qos_ = qos; } - }// namespace modulo_components diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 0f2e2dd80..7acdc8358 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -3,13 +3,13 @@ #include #include "modulo_components/ComponentInterface.hpp" - #include "modulo_new_core/EncodedState.hpp" -#include -#include namespace modulo_components { +/** + * @brief TODO + */ class LifecycleComponent : public ComponentInterface { public: friend class LifecycleComponentPublicInterface; @@ -267,5 +267,4 @@ inline void LifecycleComponent::add_output( // TODO, if we raise error we need to manually call the lifecycle change state callback, // call callback function that this service calls - -}// namespace modulo_components \ No newline at end of file +}// namespace modulo_components diff --git a/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp b/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp index 528cb5e77..5f3ec9b43 100644 --- a/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/AddSignalException.hpp @@ -7,12 +7,10 @@ namespace modulo_components::exceptions { /** * @class AddSignalException * @brief An exception class to notify errors when adding a signal. - * @details This is an exception class to be thrown if there is a - * problem while adding a signal to the component. + * @details This is an exception class to be thrown if there is a problem while adding a signal to the component. */ class AddSignalException : public ComponentException { public: explicit AddSignalException(const std::string& msg) : ComponentException(msg) {}; }; - -}// namespace modulo_components::exceptions \ No newline at end of file +}// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp b/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp index 539fb8b03..497df127a 100644 --- a/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/ComponentException.hpp @@ -14,5 +14,4 @@ class ComponentException : public std::runtime_error { public: explicit ComponentException(const std::string& msg) : std::runtime_error(msg) {}; }; - -}// namespace modulo_components::exceptions \ No newline at end of file +}// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp b/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp index e74b5b9e1..fd6bd0a87 100644 --- a/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/ComponentParameterException.hpp @@ -7,12 +7,11 @@ namespace modulo_components::exceptions { /** * @class ComponentParameterException * @brief An exception class to notify errors with component parameters. - * @details This is an exception class to be thrown if there is a problem - * with component parameters (overriding, inconsistent types, undeclared, ...). + * @details This is an exception class to be thrown if there is a problem with component parameters + * (overriding, inconsistent types, undeclared, ...). */ class ComponentParameterException : public ComponentException { public: explicit ComponentParameterException(const std::string& msg) : ComponentException(msg) {}; }; - -}// namespace modulo_components::exceptions \ No newline at end of file +}// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp b/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp index 655fb8f15..2ea95ffa3 100644 --- a/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp +++ b/source/modulo_components/include/modulo_components/exceptions/LookupTransformException.hpp @@ -7,12 +7,11 @@ namespace modulo_components::exceptions { /** * @class LookupTransformException * @brief An exception class to notify an error while looking up TF transforms. - * @details This is an exception class to be thrown if there is a problem - * with looking up a TF transform (unconfigured buffer/listener, TF2 exception). + * @details This is an exception class to be thrown if there is a problem with looking up a TF transform + * (unconfigured buffer/listener, TF2 exception). */ class LookupTransformException : public ComponentException { public: explicit LookupTransformException(const std::string& msg) : ComponentException(msg) {}; }; - -}// namespace modulo_components::exceptions \ No newline at end of file +}// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/utilities/predicate_variant.hpp b/source/modulo_components/include/modulo_components/utilities/predicate_variant.hpp index 5ea249320..9e81212c3 100644 --- a/source/modulo_components/include/modulo_components/utilities/predicate_variant.hpp +++ b/source/modulo_components/include/modulo_components/utilities/predicate_variant.hpp @@ -6,4 +6,4 @@ namespace modulo_components::utilities { typedef std::variant> PredicateVariant; -}// namespace modulo_components::utilities \ No newline at end of file +}// namespace modulo_components::utilities diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index e78b3598c..b2952bdc1 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/source/modulo_components/include/modulo_components/utilities/utilities.hpp @@ -66,5 +66,4 @@ static std::string parse_signal_name(const std::string& signal_name) { static std::string generate_predicate_topic(const std::string& component_name, const std::string& predicate_name) { return "/predicates/" + component_name + "/" + predicate_name; } - -}// namespace modulo_components::utilities \ No newline at end of file +}// namespace modulo_components::utilities diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index 03663a886..1148ba76d 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -5,8 +5,8 @@ class Component(ComponentInterface): """ - Class to represent a Component in python, following the same logic pattern - as the c++ modulo_component::Component class. + Class to represent a Component in python, following the same logic pattern as the c++ modulo_component::Component + class. ... Attributes: started (bool): flag that indicates if execution has started or not @@ -43,8 +43,8 @@ def start_thread(self): def __run(self): """ - Run the execution function in a try catch block and set - the predicates according to the outcome of the execution. + Run the execution function in a try catch block and set the predicates according to the outcome of the + execution. """ try: if not self.execute(): diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 6011a117b..ecef2d2d8 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -1,5 +1,5 @@ import sys -from typing import Union, TypeVar, Callable, List +from typing import Callable, Dict, List, Optional, TypeVar, Union import rclpy import state_representation as sr @@ -13,6 +13,7 @@ from rcl_interfaces.msg import SetParametersResult from rclpy.duration import Duration from rclpy.node import Node +from rclpy.publisher import Publisher from rclpy.time import Time from std_msgs.msg import Bool from tf2_ros import TransformBroadcaster @@ -24,8 +25,8 @@ class ComponentInterface(Node): """ - Abstract class to represent a Component in python, following the same logic pattern - as the c++ modulo_component::ComponentInterface class. + Abstract class to represent a Component in python, following the same logic pattern as the C++ + modulo_component::ComponentInterface class. ... Attributes: predicates (dict(Parameters(bool))): map of predicates added to the Component. @@ -45,12 +46,12 @@ def __init__(self, node_name: str, *kargs, **kwargs): node_name (str): name of the node to be passed to the base Node class """ super().__init__(node_name, *kargs, **kwargs) - self._parameter_dict = {} - self._predicates = {} - self._predicate_publishers = {} - self.__tf_buffer: Buffer = None - self.__tf_listener: TransformListener = None - self.__tf_broadcaster: TransformBroadcaster = None + self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {} + self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {} + self._predicate_publishers: Dict[str, Publisher] = {} + self.__tf_buffer: Optional[Buffer] = None + self.__tf_listener: Optional[TransformListener] = None + self.__tf_broadcaster: Optional[TransformBroadcaster] = None self.add_on_set_parameters_callback(self.__on_set_parameters_callback) self.add_parameter(sr.Parameter("period", 0.1, sr.ParameterType.DOUBLE), @@ -223,7 +224,7 @@ def __on_set_parameters_callback(self, ros_parameters: List[rclpy.Parameter]) -> result.reason += str(e) return result - def add_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable]): + def add_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable[[], bool]]): """ Add a predicate to the map of predicates. @@ -264,7 +265,7 @@ def get_predicate(self, predicate_name: str) -> bool: return bool_value return value - def set_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable]): + def set_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable[[], bool]]): """ Set the value of the predicate given as parameter, if the predicate is not found does not do anything. diff --git a/source/modulo_components/modulo_components/exceptions/component_exceptions.py b/source/modulo_components/modulo_components/exceptions/component_exceptions.py index 80393f2d3..f96b8839b 100644 --- a/source/modulo_components/modulo_components/exceptions/component_exceptions.py +++ b/source/modulo_components/modulo_components/exceptions/component_exceptions.py @@ -1,13 +1,24 @@ class ComponentError(Exception): - def __init__(self, message): + """ + A base class for all component exceptions. + """ + def __init__(self, message: str): super().__init__(message) class ComponentParameterError(ComponentError): - def __init__(self, message): + """ + An exception class to notify errors with component parameters. This is an exception class to be thrown if there is a + problem with component parameters (overriding, inconsistent types, undeclared, ...). + """ + def __init__(self, message: str): super().__init__(message) class LookupTransformError(ComponentError): - def __init__(self, message): + """ + An exception class to notify an error while looking up TF transforms. This is an exception class to be thrown if + there is a problem with looking up a TF transform (unconfigured buffer/listener, TF2 exception). + """ + def __init__(self, message: str): super().__init__(message) diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index ab8637a99..ca9a7888a 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -51,5 +51,4 @@ void Component::run() { bool Component::execute() { return true; } - -}// namespace modulo_components \ No newline at end of file +}// namespace modulo_components diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index ce16cf8f6..b63c363cf 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -290,7 +290,7 @@ bool LifecycleComponent::configure_outputs() { } bool LifecycleComponent::cleanup_signals() { - RCLCPP_DEBUG(this->get_logger(), "Clearing all inputs and outputs"); + RCLCPP_DEBUG(this->get_logger(), "Clearing all inputs and outputs."); this->inputs_.clear(); this->outputs_.clear(); return true; @@ -321,5 +321,4 @@ bool LifecycleComponent::deactivate_outputs() { } return success; } - -}// namespace modulo_components \ No newline at end of file +}// namespace modulo_components diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index b7fd3681f..ea780f945 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -75,6 +75,4 @@ class LifecycleComponentPublicInterface : public LifecycleComponent { using LifecycleComponent::outputs_; using LifecycleComponent::get_clock; }; - - -} \ No newline at end of file +}// namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp index 05be6495c..261cec550 100644 --- a/source/modulo_components/test/cpp/test_component.cpp +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -1,5 +1,5 @@ #include -#include + #include #include "modulo_components/Component.hpp" @@ -39,5 +39,4 @@ TEST_F(ComponentTest, AddOutput) { EXPECT_EQ(component_->outputs_.at("test_13")->get_message_pair()->get_type(), modulo_new_core::communication::MessageType::ENCODED_STATE); } - -} // namespace modulo_components \ No newline at end of file +} // namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 3430d0fb8..c8e4a090f 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -141,4 +141,4 @@ TYPED_TEST(ComponentInterfaceTest, RaiseError) { this->component_->raise_error(); EXPECT_TRUE(this->component_->get_predicate("in_error_state")); } -}// namespace modulo_components \ No newline at end of file +}// namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index d694172c2..16f8e53f5 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -165,5 +165,4 @@ TYPED_TEST(ComponentInterfaceParameterTest, ReadOnlyParameter) { this->template expect_parameter_value(1); EXPECT_EQ(this->param_->get_value(), 1); } - -} // namespace modulo_components \ No newline at end of file +} // namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_lifecycle_component.cpp b/source/modulo_components/test/cpp/test_lifecycle_component.cpp index 6e328688a..b109d8cc7 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component.cpp @@ -1,5 +1,5 @@ #include -#include + #include #include "modulo_components/LifecycleComponent.hpp" @@ -42,5 +42,4 @@ TEST_F(LifecycleComponentTest, AddOutput) { EXPECT_EQ(component_->outputs_.at("test")->get_message_pair()->get_type(), modulo_new_core::communication::MessageType::ENCODED_STATE); } - -} // namespace modulo_components \ No newline at end of file +} // namespace modulo_components diff --git a/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp b/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp index 79c37eaa4..46b565d54 100644 --- a/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp +++ b/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp @@ -10,4 +10,4 @@ namespace modulo_new_core { */ typedef std_msgs::msg::UInt8MultiArray EncodedState; -} \ No newline at end of file +}// namespace modulo_new_core diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index e73385e78..6f8ba2956 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -1,18 +1,17 @@ #pragma once +#include + #include "modulo_new_core/communication/MessagePairInterface.hpp" #include "modulo_new_core/exceptions/NullPointerException.hpp" #include "modulo_new_core/translators/message_readers.hpp" #include "modulo_new_core/translators/message_writers.hpp" -#include - namespace modulo_new_core::communication { /** * @class MessagePair - * @brief The MessagePair stores a pointer to a variable and - * translates the value of this pointer back and forth between + * @brief The MessagePair stores a pointer to a variable and translates the value of this pointer back and forth between * the corresponding ROS messages. * @tparam MsgT ROS message type of the MessagePair * @tparam DataT Data type corresponding to the ROS message type @@ -30,12 +29,14 @@ class MessagePair : public MessagePairInterface { /** * @brief Write the value of the data pointer to a ROS message. * @return The value of the data pointer as a ROS message + * @throws NullPointerException if the data pointer is null */ [[nodiscard]] MsgT write_message() const; /** * @brief Read a ROS message and store the value in the data pointer. * @param message The ROS message to read + * @throws NullPointerException if the data pointer is null */ void read_message(const MsgT& message); @@ -46,6 +47,7 @@ class MessagePair : public MessagePairInterface { /** * @brief Set the data pointer. + * @throws NullPointerException if the provided data pointer is null */ void set_data(const std::shared_ptr& data); diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp index 86d22a2b6..91e8ba12e 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp @@ -14,8 +14,8 @@ class MessagePair; /** * @class MessagePairInterface - * @brief Interface class to enable non-templated writing and reading ROS messages - * from derived MessagePair instances through dynamic downcasting. + * @brief Interface class to enable non-templated writing and reading ROS messages from derived MessagePair instances + * through dynamic down-casting. */ class MessagePairInterface : public std::enable_shared_from_this { public: @@ -37,26 +37,29 @@ class MessagePairInterface : public std::enable_shared_from_this [[nodiscard]] std::shared_ptr> get_message_pair(bool validate_pointer = true); /** * @brief Get the ROS message of a derived MessagePair instance through the MessagePairInterface pointer. - * @details This throws an InvalidPointerCastException if the MessagePairInterface does not point to a - * valid MessagePair instance or if the specified types does not match the type of the MessagePair instance. + * @details This throws an InvalidPointerCastException if the MessagePairInterface does not point to a valid + * MessagePair instance or if the specified types does not match the type of the MessagePair instance. * @see MessagePairInterface::get_message_pair() * @tparam MsgT The ROS message type of the MessagePair * @tparam DataT The data type of the MessagePair @@ -66,10 +69,10 @@ class MessagePairInterface : public std::enable_shared_from_this inline void MessagePairInterface::read(const MsgT& message) { this->template get_message_pair()->read_message(message); } - }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp index 3987f669b..f0da9edbf 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp @@ -9,5 +9,4 @@ namespace modulo_new_core::communication { enum class MessageType { BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT32, STRING, ENCODED_STATE }; - -}// namespace modulo_new_core::communication \ No newline at end of file +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp index c3b39d2c7..59f20668c 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp @@ -3,14 +3,14 @@ #include #include "modulo_new_core/communication/PublisherInterface.hpp" -#include "modulo_new_core/exceptions/NotLifecyclePublisherException.hpp" +#include "modulo_new_core/exceptions/NullPointerException.hpp" namespace modulo_new_core::communication { /** * @class PublisherHandler - * @brief The PublisherHandler handles different types of ROS publishers to activate, deactivate and publish - * data with those publishers. + * @brief The PublisherHandler handles different types of ROS publishers to activate, deactivate and publish data with + * those publishers. * @tparam PubT The ROS publisher type * @tparam MsgT The ROS message type of the ROS publisher */ @@ -26,17 +26,20 @@ class PublisherHandler : public PublisherInterface { /** * @brief Activate the ROS publisher if applicable. + * @throws NullPointerException if the publisher pointer is null */ void on_activate(); /** * @brief Deactivate the ROS publisher if applicable. + * @throws NullPointerException if the publisher pointer is null */ void on_deactivate(); /** * @brief Publish the ROS message through the ROS publisher. * @param message The ROS message to publish + * @throws NullPointerException if the publisher pointer is null */ void publish(const MsgT& message) const; @@ -63,7 +66,7 @@ void PublisherHandler::on_activate() { if (this->get_type() == PublisherType::LIFECYCLE_PUBLISHER) { this->publisher_->on_activate(); } else { - RCLCPP_WARN(rclcpp::get_logger("PublisherHandler"), "Only LifecyclePublishers can be deactivated"); + RCLCPP_DEBUG(rclcpp::get_logger("PublisherHandler"), "Only LifecyclePublishers can be deactivated"); } } @@ -75,7 +78,7 @@ void PublisherHandler::on_deactivate() { if (this->get_type() == PublisherType::LIFECYCLE_PUBLISHER) { this->publisher_->on_deactivate(); } else { - RCLCPP_WARN(rclcpp::get_logger("PublisherHandler"), "Only LifecyclePublishers can be deactivated"); + RCLCPP_DEBUG(rclcpp::get_logger("PublisherHandler"), "Only LifecyclePublishers can be deactivated"); } } @@ -95,5 +98,4 @@ std::shared_ptr PublisherHandler::create_publish publisher_interface->set_message_pair(message_pair); return publisher_interface; } - }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp index f45783ab3..537c13d80 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp @@ -15,8 +15,8 @@ class PublisherHandler; /** * @class PublisherInterface - * @brief Interface class to enable non-templated activating/deactivating/publishing of ROS publishers - * from derived PublisherHandler instances through dynamic downcasting. + * @brief Interface class to enable non-templated activating/deactivating/publishing of ROS publishers from derived + * PublisherHandler instances through dynamic down-casting. */ class PublisherInterface : public std::enable_shared_from_this { public: @@ -39,47 +39,53 @@ class PublisherInterface : public std::enable_shared_from_this std::shared_ptr> get_handler(bool validate_pointer = true); /** * @brief Activate ROS publisher of a derived PublisherHandler instance through the PublisherInterface pointer. - * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to - * a valid PublisherHandler instance or if the type of the message pair does not match the - * type of the PublisherHandler instance. - * @see PublisherInterface::get_handler() + * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to a valid + * PublisherHandler instance or if the type of the message pair does not match the type of the PublisherHandler + * instance. + * @see PublisherInterface::get_handler + * @throws NullPointerException if the message pair pointer is null */ void activate(); /** * @brief Deactivate ROS publisher of a derived PublisherHandler instance through the PublisherInterface pointer. - * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to - * a valid PublisherHandler instance or if the type of the message pair does not match the - * type of the PublisherHandler instance. - * @see PublisherInterface::get_handler() + * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to a valid + * PublisherHandler instance or if the type of the message pair does not match the type of the PublisherHandler + * instance. + * @see PublisherInterface::get_handler + * @throws NullPointerException if the message pair pointer is null */ void deactivate(); /** * @brief Publish the data stored in the message pair through the ROS publisher of a derived PublisherHandler * instance through the PublisherInterface pointer. - * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to - * a valid PublisherHandler instance or if the type of the message pair does not match the - * type of the PublisherHandler instance. - * @see PublisherInterface::get_handler() + * @details This throws an InvalidPointerCastException if the PublisherInterface does not point to a valid + * PublisherHandler instance or if the type of the message pair does not match the type of the PublisherHandler + * instance. + * @see PublisherInterface::get_handler + * @throws NullPointerException if the message pair pointer is null */ void publish(); @@ -90,6 +96,7 @@ class PublisherInterface : public std::enable_shared_from_this& message_pair); @@ -101,11 +108,12 @@ class PublisherInterface : public std::enable_shared_from_this> PublisherInterface::get_han } return publisher_ptr; } - }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp b/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp index 67dc14f49..63ccf64b4 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp @@ -9,5 +9,4 @@ namespace modulo_new_core::communication { enum class PublisherType { PUBLISHER, LIFECYCLE_PUBLISHER }; - -}// namespace modulo_new_core::communication \ No newline at end of file +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp index 998b90586..d88801f73 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp @@ -1,25 +1,53 @@ #pragma once #include "modulo_new_core/communication/SubscriptionInterface.hpp" +#include "modulo_new_core/exceptions/NullPointerException.hpp" namespace modulo_new_core::communication { +/** + * @class SubscriptionHandler + * @brief The SubscriptionHandler handles different types of ROS subscriptions to receive data from those subscriptions. + * @tparam MsgT The ROS message type of the ROS subscription + */ template class SubscriptionHandler : public SubscriptionInterface { public: + /** + * @brief Constructor with the message pair. + * @param message_pair The pointer to the message pair with the data that should be updated through the subscription + */ explicit SubscriptionHandler(std::shared_ptr message_pair = nullptr); + /** + * @brief Getter of the ROS subscription. + */ [[nodiscard]] std::shared_ptr> get_subscription() const; + /** + * @brief Setter of the ROS subscription. + * @throws NullPointerException if the provided subscription pointer is null + */ void set_subscription(const std::shared_ptr>& subscription); + /** + * @brief Get a callback function that will be associated with the ROS subscription to receive and translate messages. + */ std::function)> get_callback(); + /** + * @brief Create an SubscriptionInterface pointer through an instance of a SubscriptionHandler by providing a ROS + * subscription. + * @details This throws a NullPointerException if the ROS subscription is null. + * @see SubscriptionHandler::set_subscription + * @param subscription The ROS subscription + * @return The resulting SubscriptionInterface pointer + */ std::shared_ptr create_subscription_interface(const std::shared_ptr>& subscription); private: - std::shared_ptr> subscription_; + std::shared_ptr> subscription_; ///< The pointer referring to the ROS subscription }; template @@ -42,5 +70,4 @@ std::shared_ptr SubscriptionHandler::create_subscri this->set_subscription(subscription); return std::shared_ptr(this->shared_from_this()); } - }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp index aeddf8855..1f55dddaa 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp @@ -1,31 +1,74 @@ #pragma once #include + #include "modulo_new_core/communication/MessagePair.hpp" +#include "modulo_new_core/exceptions/InvalidPointerCastException.hpp" +#include "modulo_new_core/exceptions/InvalidPointerException.hpp" namespace modulo_new_core::communication { -// forward declaration of derived Subscription class +// forward declaration of derived SubscriptionHandler class template class SubscriptionHandler; +/** + * @class SubscriptionInterface + * @brief Interface class to enable non-templated subscriptions with ROS subscriptions from derived SubscriptionHandler + * instances through dynamic down-casting. + */ class SubscriptionInterface : public std::enable_shared_from_this { public: + /** + * @brief Constructor with the message pair. + * @param message_pair The pointer to the message pair with the data that should be updated through the subscription + */ explicit SubscriptionInterface(std::shared_ptr message_pair = nullptr); + /** + * @brief Copy constructor from another SubscriptionInterface. + */ SubscriptionInterface(const SubscriptionInterface& subscription) = default; + /** + * @brief Default virtual destructor. + */ virtual ~SubscriptionInterface() = default; + /** + * @brief Get a pointer to a derived SubscriptionHandler instance from a SubscriptionInterface pointer. + * @details If a SubscriptionInterface pointer is used to address a derived SubscriptionHandler instance, this method + * will return a pointer to that derived instance through dynamic down-casting. The downcast will fail if the base + * SubscriptionInterface object has no reference count (if the object is not owned by any pointer), or if the derived + * object is not a correctly typed instance of a SubscriptionHandler. By default, an InvalidPointerCastException is + * thrown when the downcast fails. If this validation is disabled by setting the validate_pointer flag to false, it + * will not throw an exception and instead return a null pointer. + * @tparam PubT The ROS publisher type + * @tparam MsgT The ROS message type + * @throws InvalidPointerException if the base SubscriptionInterface object has no reference count and + * validate_pointer is set to true + * @throws InvalidPointerCastException if the derived object from the dynamic down-casting is not a correctly typed + * instance of a SubscriptionHandler + * @param validate_pointer If true, throw an exception when down-casting fails + * @return A pointer to a derived SubscriptionHandler instance of the desired type, or a null pointer + * if down-casting failed and validate_pointer was set to false. + */ template std::shared_ptr> get_handler(bool validate_pointer = true); + /** + * @brief Get the pointer to the message pair of the SubscriptionInterface. + */ [[nodiscard]] std::shared_ptr get_message_pair() const; + /** + * @brief Set the pointer to the message pair of the SubscriptionInterface. + * @throws NullPointerException if the provided message pair pointer is null + */ void set_message_pair(const std::shared_ptr& message_pair); private: - std::shared_ptr message_pair_; + std::shared_ptr message_pair_; ///< The pointer to the stored MessagePair instance }; template @@ -45,5 +88,4 @@ inline std::shared_ptr> SubscriptionInterface::get_han } return subscription_ptr; } - }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp index aae2aa72d..8305bdcd4 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp @@ -3,9 +3,16 @@ #include namespace modulo_new_core::exceptions { + +/** + * @class IncompatibleParameterException + * @brief An exception class to notify incompatibility when translating parameters from different sources. + * @details This is an exception class to be thrown if there is a problem while translating from a ROS parameter to a + * state_representation parameter and vice versa. + */ class IncompatibleParameterException : public state_representation::exceptions::InvalidParameterException { public: explicit IncompatibleParameterException(const std::string& msg) : state_representation::exceptions::InvalidParameterException(msg) {}; }; -} \ No newline at end of file +}// namespace modulo_new_core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp index 0c5584b8c..f69efdba4 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp @@ -4,8 +4,14 @@ #include namespace modulo_new_core::exceptions { + +/** + * @class InvalidPointerCastException + * @brief An exception class to notify if the result of getting an instance of a derived class through dynamic + * down-casting of an object of the base class is not a correctly typed instance of the derived class. + */ class InvalidPointerCastException : public std::runtime_error { public: explicit InvalidPointerCastException(const std::string& msg) : std::runtime_error(msg) {}; }; -} \ No newline at end of file +}// namespace modulo_new_core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp index 7d27dfb27..7cbc87e0e 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp @@ -4,8 +4,14 @@ #include namespace modulo_new_core::exceptions { + +/** + * @class InvalidPointerException + * @brief An exception class to notify if an object has no reference count (if the object is not owned by any pointer) + * when attempting to get a derived instance through dynamic down-casting. + */ class InvalidPointerException : public std::runtime_error { public: explicit InvalidPointerException(const std::string& msg) : std::runtime_error(msg) {}; }; -} \ No newline at end of file +}// namespace modulo_new_core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp deleted file mode 100644 index aa364d97d..000000000 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/NotLifecyclePublisherException.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo_new_core::exceptions { -class NotLifecyclePublisherException : public std::runtime_error { -public: - explicit NotLifecyclePublisherException(const std::string& msg) : std::runtime_error(msg) {}; -}; -} \ No newline at end of file diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp b/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp index 27fbaa247..326df9628 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp +++ b/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp @@ -4,8 +4,14 @@ #include namespace modulo_new_core::exceptions { + +/** + * @class NullPointerException + * @brief An exception class to notify that a certain pointer is null. + * @details This is an exception class to be thrown if a pointer is null or is trying to be set to a null pointer. + */ class NullPointerException : public std::runtime_error { public: explicit NullPointerException(const std::string& msg) : std::runtime_error(msg) {}; }; -} \ No newline at end of file +}// namespace modulo_new_core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp index 59885a5dc..d03b556b7 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp @@ -314,4 +314,4 @@ inline void read_message(std::shared_ptr& state, co throw std::invalid_argument("The StateType contained by state " + new_state->get_name() + " is unsupported."); } } -}// namespace modulo_new_core::translators \ No newline at end of file +}// namespace modulo_new_core::translators diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp index 8a4ccf084..716891745 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp @@ -212,4 +212,4 @@ inline void write_message(EncodedState& message, const T& state, const rclcpp::T std::string tmp = clproto::encode(state); message.data = std::vector(tmp.begin(), tmp.end()); } -}// namespace modulo_new_core::translators \ No newline at end of file +}// namespace modulo_new_core::translators diff --git a/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp b/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp index a849286be..b79225dcc 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp +++ b/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp @@ -14,6 +14,7 @@ namespace modulo_new_core::translators { * to modify the value of the parameter instance while preserving the reference of the original pointer. * @param source_parameter The ParameterInterface with a value to copy * @param parameter The destination ParameterInterface to be updated + * @throws IncompatibleParameterException if the copying failed */ void copy_parameter_value( const std::shared_ptr& source_parameter, @@ -22,18 +23,28 @@ void copy_parameter_value( /** * @brief Write a ROS Parameter from a ParameterInterface pointer. - * @param parameter the ParameterInterface pointer with a name and value + * @param parameter The ParameterInterface pointer with a name and value + * @throws IncompatibleParameterException if the ROS parameter could not be written * @return A new ROS Parameter object */ rclcpp::Parameter write_parameter(const std::shared_ptr& parameter); /** * @brief Create a new ParameterInterface from a ROS Parameter object. - * @param ros_parameter the ROS parameter object to read + * @param ros_parameter The ROS parameter object to read + * @throws IncompatibleParameterException if the ROS parameter could not be read * @return A new ParameterInterface pointer */ std::shared_ptr read_parameter(const rclcpp::Parameter& ros_parameter); +/** + * @brief Update the parameter value of a ParameterInterface from a ROS Parameter object only if the two parameters have + * the same name and the ROS Parameter value can be interpreted as a ParameterInterface value. + * @param ros_parameter The ROS parameter object to read + * @param parameter An existing ParameterInterface pointer + * @throws IncompatibleParameterException if the ROS parameter could not be read + * @return A new ParameterInterface pointer with the updated value + */ std::shared_ptr read_parameter_const( const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter @@ -42,11 +53,11 @@ std::shared_ptr read_parameter_const( /** * @brief Update the parameter value of a ParameterInterface from a ROS Parameter object. * @details The destination ParameterInterface must have a compatible parameter name and type. - * @param ros_parameter the ROS parameter object to read + * @param ros_parameter The ROS parameter object to read * @param parameter An existing ParameterInterface pointer + * @throws IncompatibleParameterException if the ROS parameter could not be read */ void read_parameter( const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter ); - -} \ No newline at end of file +}// namespace modulo_new_core::exceptions diff --git a/source/modulo_new_core/modulo_new_core/translators/message_readers.py b/source/modulo_new_core/modulo_new_core/translators/message_readers.py index 892942ef8..4b9e6d43f 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_readers.py +++ b/source/modulo_new_core/modulo_new_core/translators/message_readers.py @@ -1,12 +1,10 @@ -from typing import List -from typing import TypeVar, Union +from typing import List, TypeVar, Union import clproto import geometry_msgs.msg as geometry -import sensor_msgs.msg -import sensor_msgs.msg import state_representation as sr from modulo_new_core.encoded_state import EncodedState +from sensor_msgs.msg import JointState MsgT = TypeVar('MsgT') StateT = TypeVar('StateT') @@ -32,10 +30,11 @@ def read_quaternion(message: geometry.Quaternion) -> List[float]: def read_message(state: StateT, message: MsgT): """ - Convert a ROS message to a state_representation State. + Convert a ROS message to a state_representation State type. :param state: The state to populate :param message: The ROS message to read from + :raises Exception if the message could not be read """ if not isinstance(state, sr.State): raise RuntimeError("This state type is not supported.") @@ -57,7 +56,7 @@ def read_message(state: StateT, message: MsgT): state.set_torque(read_xyz(message.torque)) else: raise RuntimeError("The provided combination of state type and message type is not supported") - elif isinstance(message, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): + elif isinstance(message, JointState) and isinstance(state, sr.JointState): state.set_names(message.name) if len(message.position): state.set_positions(message.position) @@ -72,10 +71,11 @@ def read_message(state: StateT, message: MsgT): def read_stamped_message(state: StateT, message: MsgT): """ - Convert a stamped ROS message to a state_representation State. + Convert a stamped ROS message to a state_representation State type. :param state: The state to populate :param message: The ROS message to read from + :raises Exception if the message could not be read """ if isinstance(message, geometry.AccelStamped): read_message(state, message.accel) @@ -95,8 +95,9 @@ def read_stamped_message(state: StateT, message: MsgT): def read_clproto_message(message: EncodedState) -> StateT: """ - Convert an EncodedState message to a state_representation State. + Convert an EncodedState message to a state_representation State type. :param message: The EncodedState message to read from + :raises Exception if the message could not be read """ return clproto.decode(message.data.tobytes()) diff --git a/source/modulo_new_core/modulo_new_core/translators/message_writers.py b/source/modulo_new_core/modulo_new_core/translators/message_writers.py index c8f54be80..87183a039 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_writers.py +++ b/source/modulo_new_core/modulo_new_core/translators/message_writers.py @@ -4,9 +4,9 @@ import geometry_msgs.msg as geometry import numpy as np import rclpy.time -import sensor_msgs.msg import state_representation as sr from modulo_new_core.encoded_state import EncodedState +from sensor_msgs.msg import JointState MsgT = TypeVar('MsgT') StateT = TypeVar('StateT') @@ -18,6 +18,7 @@ def write_xyz(message: Union[geometry.Point, geometry.Vector3], vector: np.array :param message: The message to populate :param vector: The vector to read from + :raises Exception if the message could not be written """ if vector.size != 3: raise RuntimeError("Provide a vector of size 3 to transform to a Point or Vector3 message.") @@ -32,6 +33,7 @@ def write_quaternion(message: geometry.Quaternion, quat: np.array): :param message: The message to populate :param quat: The vector to read from + :raises Exception if the message could not be written """ if quat.size != 4: raise RuntimeError("Provide a vector of size 4 to transform to a Quaternion message.") @@ -43,10 +45,11 @@ def write_quaternion(message: geometry.Quaternion, quat: np.array): def write_message(message: MsgT, state: StateT): """ - Convert a state_representation State to a ROS message. + Convert a state_representation State type to a ROS message. :param message: The ROS message to populate :param state: The state to read from + :raises Exception if the message could not be written """ if not isinstance(state, sr.State): raise RuntimeError("This state type is not supported.") @@ -70,7 +73,7 @@ def write_message(message: MsgT, state: StateT): write_xyz(message.torque, state.get_torque()) else: raise RuntimeError("The provided combination of state type and message type is not supported") - elif isinstance(message, sensor_msgs.msg.JointState) and isinstance(state, sr.JointState): + elif isinstance(message, JointState) and isinstance(state, sr.JointState): message.name = state.get_names() message.position = state.get_positions().tolist() message.velocity = state.get_velocities().tolist() @@ -81,11 +84,12 @@ def write_message(message: MsgT, state: StateT): def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): """ - Convert a state_representation State to a stamped ROS message. + Convert a state_representation State type to a stamped ROS message. :param message: The ROS message to populate :param state: The state to read from :param time: The time of the message + :raises Exception if the message could not be written """ if isinstance(message, geometry.AccelStamped): write_message(message.accel, state) @@ -106,10 +110,12 @@ def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): def write_clproto_message(state: StateT, clproto_message_type: clproto.MessageType) -> EncodedState(): """ - Convert a state_representation State to an EncodedState message. + Convert a state_representation State type to an EncodedState message. :param state: The state to read from :param clproto_message_type: The clproto message type to encode the state + :raises Exception if the message could not be written + :return: The populated message """ if not isinstance(state, sr.State): raise RuntimeError("This state type is not supported.") diff --git a/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py b/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py index 3bb32fc5e..50eeb1cbd 100644 --- a/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py +++ b/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py @@ -1,11 +1,19 @@ -from rclpy import Parameter -import state_representation as sr +from typing import Optional + import clproto import numpy as np -from typing import Union +import state_representation as sr +from rclpy import Parameter def write_parameter(parameter: sr.Parameter) -> Parameter: + """ + Write a ROS Parameter from a state_representation Parameter. + + :param parameter: The state_representation parameter to read from + :raises Exception if the parameter could not be written + :return: The resulting ROS parameter + """ if parameter.get_parameter_type() == sr.ParameterType.BOOL or \ parameter.get_parameter_type() == sr.ParameterType.BOOL_ARRAY or \ parameter.get_parameter_type() == sr.ParameterType.INT or \ @@ -38,6 +46,15 @@ def write_parameter(parameter: sr.Parameter) -> Parameter: def copy_parameter_value(source_parameter: sr.Parameter, parameter: sr.Parameter): + """ + Copy the value of one state_representation Parameter into another. This helper function calls the getters and + setters of the appropriate parameter type to modify the value of the parameter instance while preserving the + reference to the original parameter. + + :param source_parameter: The parameter with a value to copy + :param parameter: The destination parameter to be updated + :raises Exception if the copying failed + """ if source_parameter.get_parameter_type() != parameter.get_parameter_type(): raise RuntimeError(f"Source parameter {source_parameter.get_name()} to be copied does not have the same type " f"as destination parameter {parameter.get_name()}") @@ -70,8 +87,24 @@ def copy_parameter_value(source_parameter: sr.Parameter, parameter: sr.Parameter f"into parameter {parameter.get_name()}") -def read_parameter(ros_parameter: Parameter, parameter: Union[None, sr.Parameter] = None) -> sr.Parameter: +def read_parameter(ros_parameter: Parameter, parameter: Optional[sr.Parameter] = None) -> sr.Parameter: + """ + Update the parameter value of a state_representation Parameter from a ROS Parameter object. + + :param ros_parameter: The ROS Parameter to read from + :param parameter: The state_representation Parameter to populate + :raises Exception if the ROS Parameter could not be read + :return: The resulting state_representation Parameter + """ + def read_new_parameter(ros_param: Parameter) -> sr.Parameter: + """ + Read a ROS Parameter object and translate to a state_representation Parameter object. + + :param ros_param: The ROS Parameter to read from + :raises Exception if the ROS Parameter could not be read + :return: The resulting state_representation Parameter + """ if ros_param.type_ == Parameter.Type.BOOL: return sr.Parameter(ros_param.name, ros_param.get_parameter_value().bool_value, sr.ParameterType.BOOL) elif ros_param.type_ == Parameter.Type.BOOL_ARRAY: @@ -127,9 +160,18 @@ def read_new_parameter(ros_param: Parameter) -> sr.Parameter: def read_parameter_const(ros_parameter: Parameter, parameter: sr.Parameter) -> sr.Parameter: + """ + Update the parameter value of a state_representation Parameter from a ROS Parameter object only if the value of + the ROS Parameter can be interpreted as the value of the original state_representation Parameter. + + :param ros_parameter: The ROS Parameter to read from + :param parameter: The state_representation Parameter to update + :raises Exception if the ROS Parameter could not be read + :return: The resulting state_representation Parameter + """ if ros_parameter.name != parameter.get_name(): - raise RuntimeError( - f"The ROS parameter {ros_parameter.name} to be read does not have the same name as the reference parameter {parameter.get_name()}") + raise RuntimeError(f"The ROS parameter {ros_parameter.name} to be read does not have " + f"the same name as the reference parameter {parameter.get_name()}") new_parameter = read_parameter(ros_parameter) if new_parameter.get_parameter_type() == parameter.get_parameter_type(): return new_parameter diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_new_core/src/communication/MessagePair.cpp index a28ba42fc..bbcf95590 100644 --- a/source/modulo_new_core/src/communication/MessagePair.cpp +++ b/source/modulo_new_core/src/communication/MessagePair.cpp @@ -1,5 +1,7 @@ #include "modulo_new_core/communication/MessagePair.hpp" +#include + namespace modulo_new_core::communication { template<> @@ -75,5 +77,4 @@ std::shared_ptr make_shared_message_pair( ) { return std::make_shared>(data, clock); } - -}// namespace modulo_new_core::communication \ No newline at end of file +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/MessagePairInterface.cpp b/source/modulo_new_core/src/communication/MessagePairInterface.cpp index 2b4c5a097..164d01f65 100644 --- a/source/modulo_new_core/src/communication/MessagePairInterface.cpp +++ b/source/modulo_new_core/src/communication/MessagePairInterface.cpp @@ -7,5 +7,4 @@ MessagePairInterface::MessagePairInterface(MessageType type) : type_(type) {} MessageType MessagePairInterface::get_type() const { return this->type_; } - -}// namespace modulo_new_core::communication \ No newline at end of file +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/PublisherInterface.cpp b/source/modulo_new_core/src/communication/PublisherInterface.cpp index 2da1cf06a..e72be4915 100644 --- a/source/modulo_new_core/src/communication/PublisherInterface.cpp +++ b/source/modulo_new_core/src/communication/PublisherInterface.cpp @@ -132,5 +132,4 @@ void PublisherInterface::set_message_pair(const std::shared_ptrtype_; } - -}// namespace modulo_new_core::communication \ No newline at end of file +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/SubscriptionHandler.cpp b/source/modulo_new_core/src/communication/SubscriptionHandler.cpp index 4a72b2d69..8b55f21ed 100644 --- a/source/modulo_new_core/src/communication/SubscriptionHandler.cpp +++ b/source/modulo_new_core/src/communication/SubscriptionHandler.cpp @@ -75,5 +75,4 @@ std::function)> SubscriptionHandlerget_message_pair()->template read(*message); }; } - -}// namespace modulo_new_core::communication \ No newline at end of file +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/SubscriptionInterface.cpp b/source/modulo_new_core/src/communication/SubscriptionInterface.cpp index 44266c91c..81b691f57 100644 --- a/source/modulo_new_core/src/communication/SubscriptionInterface.cpp +++ b/source/modulo_new_core/src/communication/SubscriptionInterface.cpp @@ -1,5 +1,7 @@ #include "modulo_new_core/communication/SubscriptionInterface.hpp" +#include "modulo_new_core/exceptions/NullPointerException.hpp" + namespace modulo_new_core::communication { SubscriptionInterface::SubscriptionInterface(std::shared_ptr message_pair) : @@ -15,5 +17,4 @@ void SubscriptionInterface::set_message_pair(const std::shared_ptrmessage_pair_ = message_pair; } - -}// namespace modulo_new_core::communication \ No newline at end of file +}// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/translators/message_readers.cpp b/source/modulo_new_core/src/translators/message_readers.cpp index d49172824..f31580908 100644 --- a/source/modulo_new_core/src/translators/message_readers.cpp +++ b/source/modulo_new_core/src/translators/message_readers.cpp @@ -97,4 +97,4 @@ void read_message(int& state, const std_msgs::msg::Int32& message) { void read_message(std::string& state, const std_msgs::msg::String& message) { state = message.data; } -}// namespace modulo_new_core::translators \ No newline at end of file +}// namespace modulo_new_core::translators diff --git a/source/modulo_new_core/src/translators/message_writers.cpp b/source/modulo_new_core/src/translators/message_writers.cpp index 36b4dda17..b927c486f 100644 --- a/source/modulo_new_core/src/translators/message_writers.cpp +++ b/source/modulo_new_core/src/translators/message_writers.cpp @@ -182,4 +182,4 @@ void write_message(std_msgs::msg::Int32& message, const int& state, const rclcpp void write_message(std_msgs::msg::String& message, const std::string& state, const rclcpp::Time&) { message.data = state; } -}// namespace modulo_new_core::translators \ No newline at end of file +}// namespace modulo_new_core::translators diff --git a/source/modulo_new_core/src/translators/parameter_translators.cpp b/source/modulo_new_core/src/translators/parameter_translators.cpp index c75c6ceaa..62fe85659 100644 --- a/source/modulo_new_core/src/translators/parameter_translators.cpp +++ b/source/modulo_new_core/src/translators/parameter_translators.cpp @@ -1,14 +1,13 @@ #include "modulo_new_core/translators/parameter_translators.hpp" -#include +#include #include -#include +#include #include +#include #include "modulo_new_core/exceptions/IncompatibleParameterException.hpp" -#include - using namespace state_representation; namespace modulo_new_core::translators { @@ -224,14 +223,12 @@ std::shared_ptr read_parameter_const( "The ROS parameter " + ros_parameter.get_name() + " with type double array cannot be interpreted by reference parameter " + parameter->get_name() + " (type code " + std::to_string(static_cast(parameter->get_parameter_type())) + ")"); - break; } break; } default: throw state_representation::exceptions::InvalidParameterException( "Something went wrong while reading parameter " + parameter->get_name()); - break; } return new_parameter; } @@ -242,5 +239,4 @@ void read_parameter( auto new_parameter = read_parameter_const(ros_parameter, parameter); copy_parameter_value(new_parameter, parameter); } - -} \ No newline at end of file +}// namespace modulo_new_core::translators diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp index 694732398..3174c1e9a 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp @@ -77,4 +77,4 @@ TEST_F(PublisherTest, EncodedState) { publisher_interface = publisher_handler->create_publisher_interface(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); -} \ No newline at end of file +} diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp index fc4c60b03..3d347d0dc 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp @@ -62,4 +62,4 @@ TEST_F(SubscriptionTest, EncodedState) { // use in subscription interface auto subscription_interface = subscription_handler->create_subscription_interface(subscription); -} \ No newline at end of file +} diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp index 3f96e39b0..407c19971 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp @@ -63,4 +63,4 @@ TEST(ParameterEigenTranslationTest, EigenMatrix) { Eigen::MatrixXd new_mat; EXPECT_NO_THROW(new_mat = new_param->get_parameter_value()); EXPECT_EQ(new_mat, mat); -} \ No newline at end of file +} diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp index 2d12e1cc9..fe4f7026e 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp @@ -92,4 +92,4 @@ using ParameterStateTestTypes = testing::Types< state_representation::JointState, state_representation::JointPositions >; -INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ParameterStateTranslationTest, ParameterStateTestTypes); \ No newline at end of file +INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ParameterStateTranslationTest, ParameterStateTestTypes); diff --git a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp index 7ba286208..755bebf53 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp +++ b/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp @@ -218,4 +218,4 @@ TEST_F(MessageTranslatorsTest, TestEncodedStatePointer) { EXPECT_TRUE(state_.data().isApprox(new_state.data())); EXPECT_EQ(state_.get_name(), new_state.get_name()); EXPECT_EQ(state_.get_reference_frame(), new_state.get_reference_frame()); -} \ No newline at end of file +} From da52fd63aee853f709bbe3b410fdc17a76a15541 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Mon, 20 Jun 2022 15:08:07 +0200 Subject: [PATCH 41/71] Fix MessagePair to enable creating from any SR type (#82) --- .../communication/MessagePair.hpp | 38 ++++++++++++++++++- .../src/communication/MessagePair.cpp | 37 ------------------ .../communication/test_communication.cpp | 9 ++--- 3 files changed, 39 insertions(+), 45 deletions(-) diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp index 6f8ba2956..968b16448 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp @@ -106,7 +106,41 @@ inline void MessagePair::set_data(const std::shared_ptr& dat } template -std::shared_ptr -make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock); +inline std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>( + std::dynamic_pointer_cast(data), clock + ); +} + +template<> +inline std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} +template<> +inline std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} + +template<> +inline std::shared_ptr make_shared_message_pair( + const std::shared_ptr>& data, const std::shared_ptr& clock +) { + return std::make_shared>>(data, clock); +} + +template<> +inline std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} + +template<> +inline std::shared_ptr +make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { + return std::make_shared>(data, clock); +} }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_new_core/src/communication/MessagePair.cpp index bbcf95590..7087c4a00 100644 --- a/source/modulo_new_core/src/communication/MessagePair.cpp +++ b/source/modulo_new_core/src/communication/MessagePair.cpp @@ -40,41 +40,4 @@ MessagePair::MessagePair( ) : MessagePairInterface(MessageType::ENCODED_STATE), data_(std::move(data)), clock_(std::move(clock)) {} -template<> -std::shared_ptr -make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { - return std::make_shared>(data, clock); -} - -template<> -std::shared_ptr -make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { - return std::make_shared>(data, clock); -} - -template<> -std::shared_ptr make_shared_message_pair( - const std::shared_ptr>& data, const std::shared_ptr& clock -) { - return std::make_shared>>(data, clock); -} - -template<> -std::shared_ptr -make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { - return std::make_shared>(data, clock); -} - -template<> -std::shared_ptr -make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { - return std::make_shared>(data, clock); -} - -template<> -std::shared_ptr make_shared_message_pair( - const std::shared_ptr& data, const std::shared_ptr& clock -) { - return std::make_shared>(data, clock); -} }// namespace modulo_new_core::communication diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp b/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp index b60721675..befcf1add 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp +++ b/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp @@ -112,11 +112,9 @@ TEST_F(CommunicationTest, BasicTypes) { TEST_F(CommunicationTest, EncodedState) { using namespace state_representation; auto pub_state = std::make_shared(CartesianState::Random("this", "world")); - std::shared_ptr pub_data = pub_state; - auto pub_message = make_shared_message_pair(pub_data, this->clock_); + auto pub_message = make_shared_message_pair(pub_state, this->clock_); auto sub_state = std::make_shared(CartesianState::Identity("that", "base")); - std::shared_ptr sub_data = sub_state; - auto sub_message = make_shared_message_pair(sub_data, this->clock_); + auto sub_message = make_shared_message_pair(sub_state, this->clock_); this->add_nodes("/test_topic", pub_message, sub_message); this->exec_->template spin_until_future_complete( std::dynamic_pointer_cast>(this->sub_node_)->received_future, @@ -125,6 +123,5 @@ TEST_F(CommunicationTest, EncodedState) { EXPECT_EQ(pub_state->get_name(), sub_state->get_name()); EXPECT_EQ(pub_state->get_reference_frame(), sub_state->get_reference_frame()); - EXPECT_TRUE(std::dynamic_pointer_cast(pub_data)->data().isApprox( - std::dynamic_pointer_cast(sub_data)->data())); + EXPECT_TRUE(pub_state->data().isApprox(sub_state->data())); } From 2e3e9ba36667cbec7a52dfc6e30fcb969f7cb60a Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 21 Jun 2022 11:30:28 +0200 Subject: [PATCH 42/71] Add QoS and periodic callbacks to Python ComponentInterface (#81) * Rename add_periodic_function to add_periodic_callback * Add periodic callbacks to Python ComponentInterface * Add QoS to Python ComponentInterface * Move generate_predicate_topic to utilities and add helper publish function (Python) * Correctly define step and raise_error in the right place (Python) * Put step in try catch block and always publish predicates * Warn instead of debug line if overwriting * Add TODO for class docstring --- .../modulo_components/ComponentInterface.hpp | 10 +- .../modulo_components/component.py | 23 +-- .../modulo_components/component_interface.py | 139 ++++++++++++------ .../modulo_components/utilities/__init__.py | 0 .../modulo_components/utilities/utilities.py | 9 ++ .../src/LifecycleComponent.cpp | 14 +- 6 files changed, 133 insertions(+), 62 deletions(-) create mode 100644 source/modulo_components/modulo_components/utilities/__init__.py create mode 100644 source/modulo_components/modulo_components/utilities/utilities.py diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 20ac2bf47..9162fbab2 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -209,7 +209,7 @@ class ComponentInterface : private NodeT { * @param name The name of the callback * @param callback The callback function that is evaluated periodically */ - void add_periodic_function(const std::string& name, const std::function& callback); + void add_periodic_callback(const std::string& name, const std::function& callback); /** * @brief Configure a transform broadcaster. @@ -410,7 +410,7 @@ inline void ComponentInterface::add_parameter( descriptor.read_only = read_only; NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); } else { - RCLCPP_DEBUG_STREAM(this->get_logger(), + RCLCPP_WARN_STREAM(this->get_logger(), "Parameter '" << parameter->get_name() << "' already exists, overwriting."); NodeT::set_parameter(ros_param); } @@ -500,7 +500,7 @@ inline void ComponentInterface::add_variant_predicate( return; } if (this->predicates_.find(name) != this->predicates_.end()) { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Predicate '" << name << "' already exists, overwriting."); + RCLCPP_WARN_STREAM(this->get_logger(), "Predicate '" << name << "' already exists, overwriting."); } else { RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding predicate '" << name << "'."); auto publisher = this->template create_publisher( @@ -679,13 +679,13 @@ inline void ComponentInterface::add_input( template inline void -ComponentInterface::add_periodic_function(const std::string& name, const std::function& callback) { +ComponentInterface::add_periodic_callback(const std::string& name, const std::function& callback) { if (name.empty()) { RCLCPP_ERROR(this->get_logger(), "Failed to add periodic function: Provide a non empty string as a name."); return; } if (this->periodic_callbacks_.find(name) != this->periodic_callbacks_.end()) { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Periodic function '" << name << "' already exists, overwriting."); + RCLCPP_WARN_STREAM(this->get_logger(), "Periodic function '" << name << "' already exists, overwriting."); } else { RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding periodic function '" << name << "'."); } diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index 1148ba76d..4c95890dc 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -23,12 +23,22 @@ def __init__(self, node_name: str, start_thread=True, *kargs, **kwargs): super().__init__(node_name, *kargs, **kwargs) self.__started = False self.__run_thread = None - self.add_predicate("in_error_state", False) self.add_predicate("is_finished", False) if start_thread: self.start_thread() + def _step(self): + """ + Step function that is called periodically. + """ + try: + # TODO catch here or in helpers...? (or re raise with ComponentError) + self._publish_predicates() + self._evaluate_periodic_callbacks() + except Exception as e: + self.get_logger().error(f"Failed to execute step function: {e}", throttle_duration_sec=1.0) + def start_thread(self): """ Start the execution thread. @@ -48,11 +58,11 @@ def __run(self): """ try: if not self.execute(): - self._raise_error() + self.raise_error() return except Exception as e: self.get_logger().error(f"Failed to run component {self.get_name()}: {e}", throttle_duration_sec=1.0) - self._raise_error() + self.raise_error() return self.set_predicate("is_finished", True) @@ -63,10 +73,3 @@ def execute(self): :return: True, if the execution was successful, false otherwise """ return True - - def _raise_error(self): - """ - Put the component in error state by setting the 'in_error_state' - predicate to true. - """ - self.set_predicate("in_error_state", True) diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index ecef2d2d8..8968623b3 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -6,6 +6,7 @@ import tf2_py from geometry_msgs.msg import TransformStamped from modulo_components.exceptions.component_exceptions import ComponentParameterError, LookupTransformError +from modulo_components.utilities.utilities import generate_predicate_topic from modulo_new_core.translators.message_readers import read_stamped_message from modulo_new_core.translators.message_writers import write_stamped_message from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter_const @@ -14,6 +15,7 @@ from rclpy.duration import Duration from rclpy.node import Node from rclpy.publisher import Publisher +from rclpy.qos import QoSProfile from rclpy.time import Time from std_msgs.msg import Bool from tf2_ros import TransformBroadcaster @@ -28,10 +30,13 @@ class ComponentInterface(Node): Abstract class to represent a Component in python, following the same logic pattern as the C++ modulo_component::ComponentInterface class. ... + # TODO class docstring and attributes Attributes: predicates (dict(Parameters(bool))): map of predicates added to the Component. predicate_publishers (dict(rclpy.Publisher(Bool))): map of publishers associated to each predicate. - parameter_dict: dict of parameters + parameter_dict (dict(Publisher)): dict of parameters + periodic_callbacks (dict(Callable): dict of periodic callback functions + qos (rclpy.qos.QoSProfile: the Quality of Service for publishers and subscribers tf_buffer (tf2_ros.Buffer): the buffer to lookup transforms published on tf2. tf_listener (tf2_ros.TransformListener): the listener to lookup transforms published on tf2. tf_broadcaster (tf2_ros.TransformBroadcaster): the broadcaster to publish transforms on tf2 @@ -49,37 +54,26 @@ def __init__(self, node_name: str, *kargs, **kwargs): self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {} self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {} self._predicate_publishers: Dict[str, Publisher] = {} + self._periodic_callbacks: Dict[str, Callable[[], None]] = {} self.__tf_buffer: Optional[Buffer] = None self.__tf_listener: Optional[TransformListener] = None self.__tf_broadcaster: Optional[TransformBroadcaster] = None + self._qos = QoSProfile(depth=10) + self.add_on_set_parameters_callback(self.__on_set_parameters_callback) self.add_parameter(sr.Parameter("period", 0.1, sr.ParameterType.DOUBLE), "Period (in s) between step function calls.") - self.create_timer(self.get_parameter_value("period"), self.__step) + self.add_predicate("in_error_state", False) - def __step(self) -> None: - """ - Step function that is called periodically. - """ - for predicate_name in self._predicates.keys(): - message = Bool() - message.data = self.get_predicate(predicate_name) - if predicate_name not in self._predicate_publishers.keys(): - self.get_logger().error(f"No publisher for predicate {predicate_name} found.", - throttle_duration_sec=1.0) - return - self._predicate_publishers[predicate_name].publish(message) + self.create_timer(self.get_parameter_value("period"), self._step) - def __generate_predicate_topic(self, predicate_name: str) -> str: + def _step(self) -> None: """ - Generate the predicate topic name from the name of the predicate. - - :param predicate_name: The predicate name - :return: The predicate topic as /predicates/component_name/predicate_name + Step function that is called periodically. """ - return f"/predicates/{self.get_name()}/{predicate_name}" + pass def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, read_only=False) -> None: """ @@ -115,7 +109,7 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r descriptor=ParameterDescriptor(description=description), ignore_override=read_only) else: - self.get_logger().debug(f"Parameter '{sr_parameter.get_name()}' already exists, overwriting.") + self.get_logger().warn(f"Parameter '{sr_parameter.get_name()}' already exists, overwriting.") self.set_parameters([ros_param]) except Exception as e: self.get_logger().error(f"Failed to add parameter: {e}") @@ -224,37 +218,37 @@ def __on_set_parameters_callback(self, ros_parameters: List[rclpy.Parameter]) -> result.reason += str(e) return result - def add_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable[[], bool]]): + def add_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): """ Add a predicate to the map of predicates. - :param predicate_name: The name of the associated predicate - :param predicate_value: The value of the predicate as a bool or a callable function + :param name: The name of the associated predicate + :param value: The value of the predicate as a bool or a callable function """ - if not predicate_name: + if not name: self.get_logger().error("Failed to add predicate: Provide a non empty string as a name.") - if predicate_name in self._predicates.keys(): - self.get_logger().debug(f"Predicate {predicate_name} already exists, overwriting.") + if name in self._predicates.keys(): + self.get_logger().warn(f"Predicate {name} already exists, overwriting.") else: - self.get_logger().debug(f"Adding predicate '{predicate_name}'.") - self._predicate_publishers[predicate_name] = self.create_publisher(Bool, - self.__generate_predicate_topic( - predicate_name), 10) - self._predicates[predicate_name] = predicate_value + self.get_logger().debug(f"Adding predicate '{name}'.") + self._predicate_publishers[name] = self.create_publisher(Bool, + generate_predicate_topic(self.get_name(), name), + 10) + self._predicates[name] = value - def get_predicate(self, predicate_name: str) -> bool: + def get_predicate(self, name: str) -> bool: """ Get the value of the predicate given as parameter. If the predicate is not found or the callable function fails, this method returns False. - :param predicate_name: The name of the predicate to retrieve from the map of predicates + :param name: The name of the predicate to retrieve from the map of predicates :return: The value of the predicate as a boolean """ - if predicate_name not in self._predicates.keys(): - self.get_logger().error(f"Predicate {predicate_name} does not exist, returning false.", + if name not in self._predicates.keys(): + self.get_logger().error(f"Predicate {name} does not exist, returning false.", throttle_duration_sec=1.0) return False - value = self._predicates[predicate_name] + value = self._predicates[name] if callable(value): bool_value = False try: @@ -265,19 +259,19 @@ def get_predicate(self, predicate_name: str) -> bool: return bool_value return value - def set_predicate(self, predicate_name: str, predicate_value: Union[bool, Callable[[], bool]]): + def set_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): """ Set the value of the predicate given as parameter, if the predicate is not found does not do anything. - :param predicate_name: The name of the predicate to retrieve from the map of predicates - :param predicate_value: The new value of the predicate as a bool or a callable function + :param name: The name of the predicate to retrieve from the map of predicates + :param value: The new value of the predicate as a bool or a callable function """ - if predicate_name not in self._predicates.keys(): + if name not in self._predicates.keys(): self.get_logger().error( - f"Cannot set predicate {predicate_name} with a new value because it does not exist.", + f"Cannot set predicate {name} with a new value because it does not exist.", throttle_duration_sec=1.0) return - self._predicates[predicate_name] = predicate_value + self._predicates[name] = value def add_tf_broadcaster(self): """ @@ -337,3 +331,62 @@ def lookup_transform(self, frame_name: str, reference_frame_name="world", time_p return result except tf2_py.TransformException as e: raise LookupTransformError(f"Failed to lookup transform: {e}") + + def get_qos(self) -> QoSProfile: + """ + Getter of the Quality of Service attribute. + """ + return self._qos + + def set_qos(self, qos: QoSProfile): + """ + Setter of the Quality of Service for ROS publishers and subscribers. + + :param qos: The desired Quality of Service + """ + self._qos = qos + + def add_periodic_callback(self, name: str, callback: Callable[[], None]): + """ + Add a periodic callback function. The provided function is evaluated periodically at the component step period. + + :param name: The name of the callback + :param callback: The callback function that is evaluated periodically + """ + if not name: + self.get_logger().error("Failed to add periodic function: Provide a non empty string as a name.") + return + if name in self._periodic_callbacks.keys(): + self.get_logger().warn(f"Periodic function '{name}' already exists, overwriting.") + else: + self.get_logger().debug(f"Adding periodic function '{name}'.") + self._periodic_callbacks[name] = callback + + def _publish_predicates(self): + """ + Helper function to publish all predicates. + """ + for name in self._predicates.keys(): + message = Bool() + message.data = self.get_predicate(name) + if name not in self._predicate_publishers.keys(): + self.get_logger().error(f"No publisher for predicate {name} found.", throttle_duration_sec=1.0) + return + self._predicate_publishers[name].publish(message) + + def _evaluate_periodic_callbacks(self): + """ + Helper function to evaluate all periodic function callbacks. + """ + for name, callback in self._periodic_callbacks.items(): + try: + callback() + except Exception as e: + self.get_logger().error(f"Failed to evaluate periodic function callback '{name}': {e}", + throttle_duration_sec=1.0) + + def raise_error(self): + """ + Put the component in error state by setting the 'in_error_state' predicate to true. + """ + self.set_predicate("in_error_state", True) diff --git a/source/modulo_components/modulo_components/utilities/__init__.py b/source/modulo_components/modulo_components/utilities/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_components/modulo_components/utilities/utilities.py b/source/modulo_components/modulo_components/utilities/utilities.py new file mode 100644 index 000000000..c34590626 --- /dev/null +++ b/source/modulo_components/modulo_components/utilities/utilities.py @@ -0,0 +1,9 @@ +def generate_predicate_topic(node_name: str, predicate_name: str) -> str: + """ + Generate a topic name from the name of node and the predicate. + + :param node_name: The name of the node + :param predicate_name: The name of the associated predicate + :return: The name of the topic + """ + return f'/predicates/{node_name}/{predicate_name}' diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index b63c363cf..a2d66e399 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -15,11 +15,17 @@ LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options) } void LifecycleComponent::step() { - if (this->get_predicate("is_active")) { + try { this->publish_predicates(); - this->publish_outputs(); - this->evaluate_periodic_callbacks(); - this->on_step(); + if (this->get_predicate("is_active")) { + this->publish_outputs(); + this->evaluate_periodic_callbacks(); + this->on_step(); + } + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to execute step function:" << ex.what()); + // TODO handle error in lifecycle component +// this->raise_error(); } } From 7a58bbbc05c2d2ce7787eb086995cb7e71cba14c Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 22 Jun 2022 16:47:28 +0200 Subject: [PATCH 43/71] Make sure predicate is a bool (#85) * Make sure predicate is a bool * Catch AssertionError instead * Continue instead of return in for loop * Revert changes --- .../modulo_components/component_interface.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 8968623b3..2df276b63 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -368,10 +368,16 @@ def _publish_predicates(self): """ for name in self._predicates.keys(): message = Bool() - message.data = self.get_predicate(name) + value = self.get_predicate(name) + try: + message.data = value + except AssertionError: + self.get_logger().error(f"Predicate '{name}' has invalid type: expected 'bool', got '{type(value)}'.", + throttle_duration_sec=1.0) + continue if name not in self._predicate_publishers.keys(): - self.get_logger().error(f"No publisher for predicate {name} found.", throttle_duration_sec=1.0) - return + self.get_logger().error(f"No publisher for predicate '{name}' found.", throttle_duration_sec=1.0) + continue self._predicate_publishers[name].publish(message) def _evaluate_periodic_callbacks(self): From 4654b8b51af841f99108eba8b68238ec7a068f35 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 24 Jun 2022 08:25:33 +0200 Subject: [PATCH 44/71] Inherit from NodeT with public keyword (#84) * Inherit from NodeT with public keyword * Change back to TF broadcaster * Predicate publishers as rclcpp::Publishers * LifecycleComponent predicates based on get_current_state * Add maybe_unused to utilities --- .../include/modulo_components/Component.hpp | 4 +- .../modulo_components/ComponentInterface.hpp | 73 ++++--------------- .../modulo_components/LifecycleComponent.hpp | 2 +- .../modulo_components/utilities/utilities.hpp | 10 ++- .../src/LifecycleComponent.cpp | 42 +++++------ .../component_public_interfaces.hpp | 3 - .../test/cpp/test_component_interface.cpp | 1 - 7 files changed, 41 insertions(+), 94 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index e978e51bc..41f8052be 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -67,7 +67,7 @@ class Component : public ComponentInterface { */ void run(); - using rclcpp::Node::create_publisher; + // TODO hide ROS methods using ComponentInterface::create_output; using ComponentInterface::inputs_; using ComponentInterface::outputs_; @@ -75,8 +75,6 @@ class Component : public ComponentInterface { using ComponentInterface::publish_predicates; using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_periodic_callbacks; - using ComponentInterface::activate_tf_broadcaster; - using ComponentInterface::deactivate_tf_broadcaster; std::thread run_thread_; ///< The execution thread of the component bool started_; ///< Flag that indicates if execution has started or not diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 9162fbab2..432bc526a 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -3,12 +3,12 @@ // TODO sort includes #include #include -#include -#include +#include #include #include #include +#include #include #include @@ -43,7 +43,7 @@ class ComponentInterfacePublicInterface; * @tparam NodeT */ template -class ComponentInterface : private NodeT { +class ComponentInterface : public NodeT { public: friend class ComponentInterfacePublicInterface; friend class ComponentInterfacePublicInterface; @@ -61,10 +61,7 @@ class ComponentInterface : private NodeT { */ virtual ~ComponentInterface() = default; - using NodeT::get_node_base_interface; - using NodeT::get_name; - using NodeT::get_clock; - using NodeT::get_logger; + // TODO hide ROS methods protected: /** @@ -221,16 +218,6 @@ class ComponentInterface : private NodeT { */ void add_tf_listener(); - /** - * @brief Activate the transform broadcaster (for LifecycleComponents). - */ - void activate_tf_broadcaster(); - - /** - * @brief Deactivate the transform broadcaster (for LifecycleComponents). - */ - void deactivate_tf_broadcaster(); - /** * @brief Helper function to parse the signal name and add an unconfigured PublisherInterface to the map of outputs. * @tparam DataT Type of the data pointer @@ -297,8 +284,6 @@ class ComponentInterface : private NodeT { */ virtual void raise_error(); - using NodeT::create_publisher; - std::map> inputs_; ///< Map of inputs std::map> @@ -344,7 +329,7 @@ class ComponentInterface : private NodeT { std::shared_ptr step_timer_; ///< Timer for the step function std::shared_ptr tf_buffer_; ///< TF buffer std::shared_ptr tf_listener_; ///< TF listener - std::shared_ptr> tf_broadcaster_; ///< TF broadcaster + std::shared_ptr tf_broadcaster_; ///< TF broadcaster // TODO maybe add a static tf broadcaster }; @@ -459,6 +444,9 @@ ComponentInterface::on_set_parameters_callback(const std::vector::add_variant_predicate( RCLCPP_WARN_STREAM(this->get_logger(), "Predicate '" << name << "' already exists, overwriting."); } else { RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding predicate '" << name << "'."); - auto publisher = this->template create_publisher( - utilities::generate_predicate_topic(this->get_name(), name), this->qos_ + auto publisher = rclcpp::create_publisher( + *this, utilities::generate_predicate_topic(this->get_name(), name), this->qos_ ); - this->predicate_publishers_.insert_or_assign( - name, std::static_pointer_cast>(publisher)); + this->predicate_publishers_.insert_or_assign(name, publisher); } this->predicates_.insert_or_assign(name, predicate); } @@ -697,7 +684,7 @@ inline void ComponentInterface::add_tf_broadcaster() { if (this->tf_broadcaster_ == nullptr) { RCLCPP_DEBUG(this->get_logger(), "Adding TF broadcaster."); console_bridge::setLogLevel(console_bridge::CONSOLE_BRIDGE_LOG_NONE); - this->tf_broadcaster_ = this->template create_publisher("tf", this->qos_); + this->tf_broadcaster_ = std::make_shared(*this); } else { RCLCPP_DEBUG(this->get_logger(), "TF broadcaster already exists."); } @@ -715,38 +702,6 @@ inline void ComponentInterface::add_tf_listener() { } } -template -inline void ComponentInterface::activate_tf_broadcaster() { - if (this->publisher_type_ != modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER) { - return; - } - try { - RCLCPP_DEBUG(this->get_logger(), "Activating TF broadcaster."); - auto publisher = std::dynamic_pointer_cast>( - this->tf_broadcaster_ - ); - publisher->on_activate(); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to activate TF broadcaster: " << ex.what()); - } -} - -template -inline void ComponentInterface::deactivate_tf_broadcaster() { - if (this->publisher_type_ == modulo_new_core::communication::PublisherType::PUBLISHER) { - return; - } - try { - RCLCPP_DEBUG(this->get_logger(), "Deactivating TF broadcaster."); - auto publisher = std::dynamic_pointer_cast>( - this->tf_broadcaster_ - ); - publisher->on_deactivate(); - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to deactivate TF broadcaster: " << ex.what()); - } -} - template inline void ComponentInterface::send_transform(const state_representation::CartesianPose& transform) { if (this->tf_broadcaster_ == nullptr) { @@ -757,9 +712,7 @@ inline void ComponentInterface::send_transform(const state_representation try { geometry_msgs::msg::TransformStamped transform_message; modulo_new_core::translators::write_message(transform_message, transform, this->get_clock()->now()); - tf2_msgs::msg::TFMessage tf_message; - tf_message.transforms.emplace_back(transform_message); - this->tf_broadcaster_->publish(tf_message); + this->tf_broadcaster_->sendTransform(transform_message); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Failed to send transform: " << ex.what()); diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 7acdc8358..cc267d0be 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -240,7 +240,7 @@ class LifecycleComponent : public ComponentInterface::create_output; using ComponentInterface::inputs_; using ComponentInterface::outputs_; diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index b2952bdc1..dd313e3eb 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/source/modulo_components/include/modulo_components/utilities/utilities.hpp @@ -13,7 +13,7 @@ namespace modulo_components::utilities { * @param result the default argument value that is overwritten by reference if the given pattern is found * @return the value of the resultant string */ -static std::string +[[maybe_unused]] static std::string parse_string_argument(const std::vector& args, const std::string& pattern, std::string& result) { for (const auto& arg: args) { std::string::size_type index = arg.find(pattern); @@ -32,7 +32,8 @@ parse_string_argument(const std::vector& args, const std::string& p * @param fallback the default name if the NodeOptions structure cannot be parsed * @return the parsed node name or the fallback name */ -static std::string parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback = "") { +[[maybe_unused]] static std::string +parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback = "") { std::string node_name(fallback); const std::string pattern("__node:="); return parse_string_argument(options.arguments(), pattern, node_name); @@ -45,7 +46,7 @@ static std::string parse_node_name(const rclcpp::NodeOptions& options, const std * @param signal_name The input string * @return The sanitized string */ -static std::string parse_signal_name(const std::string& signal_name) { +[[maybe_unused]] static std::string parse_signal_name(const std::string& signal_name) { std::string output; for (char c: signal_name) { if ((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9') || c == '_') { @@ -63,7 +64,8 @@ static std::string parse_signal_name(const std::string& signal_name) { * @param predicate_name The name of the predicate * @return The generated predicate topic as /predicates/component_name/predicate_name */ -static std::string generate_predicate_topic(const std::string& component_name, const std::string& predicate_name) { +[[maybe_unused]] static std::string +generate_predicate_topic(const std::string& component_name, const std::string& predicate_name) { return "/predicates/" + component_name + "/" + predicate_name; } }// namespace modulo_components::utilities diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index a2d66e399..7d8a2fb47 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -8,10 +8,26 @@ namespace modulo_components { LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options) : ComponentInterface(node_options, PublisherType::LIFECYCLE_PUBLISHER) { - this->add_predicate("is_unconfigured", true); - this->add_predicate("is_inactive", false); - this->add_predicate("is_active", false); - this->add_predicate("is_finalized", false); + this->add_predicate( + "is_unconfigured", [this] { + return this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; + } + ); + this->add_predicate( + "is_inactive", [this] { + return this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; + } + ); + this->add_predicate( + "is_active", [this] { + return this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE; + } + ); + this->add_predicate( + "is_finalized", [this] { + return this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED; + } + ); } void LifecycleComponent::step() { @@ -47,8 +63,6 @@ LifecycleComponent::on_configure(const rclcpp_lifecycle::State& previous_state) return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } - this->set_predicate("is_unconfigured", false); - this->set_predicate("is_inactive", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -72,8 +86,6 @@ LifecycleComponent::on_cleanup(const rclcpp_lifecycle::State& previous_state) { RCLCPP_ERROR(get_logger(), "Cleanup failed! Entering into the error processing transition state."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - this->set_predicate("is_inactive", false); - this->set_predicate("is_unconfigured", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -104,8 +116,6 @@ LifecycleComponent::on_activate(const rclcpp_lifecycle::State& previous_state) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } } - this->set_predicate("is_inactive", false); - this->set_predicate("is_active", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -129,8 +139,6 @@ LifecycleComponent::on_deactivate(const rclcpp_lifecycle::State& previous_state) RCLCPP_ERROR(get_logger(), "Deactivation failed! Entering into the error processing transition state."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - this->set_predicate("is_active", false); - this->set_predicate("is_inactive", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -170,10 +178,6 @@ LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { // this->daemons_.clear(); // this->parameters_.clear(); // this->shutdown_ = true; - this->set_predicate("is_unconfigured", false); - this->set_predicate("is_inactive", false); - this->set_predicate("is_active", false); - this->set_predicate("is_finalized", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; default: RCLCPP_WARN(get_logger(), "Invalid transition 'shutdown' from state %s.", previous_state.label().c_str()); @@ -194,10 +198,6 @@ bool LifecycleComponent::on_shutdown() { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_error(const rclcpp_lifecycle::State& previous_state) { RCLCPP_DEBUG(this->get_logger(), "on_error called from previous state %s", previous_state.label().c_str()); - this->set_predicate("is_unconfigured", false); - this->set_predicate("is_inactive", false); - this->set_predicate("is_active", false); - this->set_predicate("is_finalized", false); this->set_predicate("in_error_state", true); bool error_handled; try { @@ -209,11 +209,9 @@ LifecycleComponent::on_error(const rclcpp_lifecycle::State& previous_state) { if (!error_handled) { RCLCPP_ERROR(get_logger(), "Error processing failed! Entering into the finalized state."); // TODO: reset and finalize all needed properties - this->set_predicate("is_finalized", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } this->set_predicate("in_error_state", false); - this->set_predicate("is_unconfigured", true); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index ea780f945..b89b8b62d 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -29,7 +29,6 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::outputs_; using ComponentInterface::add_tf_broadcaster; using ComponentInterface::add_tf_listener; - using ComponentInterface::activate_tf_broadcaster; using ComponentInterface::send_transform; using ComponentInterface::lookup_transform; using ComponentInterface::raise_error; @@ -63,7 +62,6 @@ class ComponentPublicInterface : public Component { Component(node_options, start_thread) {} using Component::add_output; using Component::outputs_; - using Component::get_clock; }; class LifecycleComponentPublicInterface : public LifecycleComponent { @@ -73,6 +71,5 @@ class LifecycleComponentPublicInterface : public LifecycleComponent { using LifecycleComponent::configure_outputs; using LifecycleComponent::activate_outputs; using LifecycleComponent::outputs_; - using LifecycleComponent::get_clock; }; }// namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index c8e4a090f..f95822324 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -120,7 +120,6 @@ TYPED_TEST(ComponentInterfaceTest, CreateOutput) { TYPED_TEST(ComponentInterfaceTest, TF) { this->component_->add_tf_broadcaster(); this->component_->add_tf_listener(); - this->component_->activate_tf_broadcaster(); auto send_tf = state_representation::CartesianPose::Random("test", "world"); EXPECT_NO_THROW(this->component_->send_transform(send_tf)); EXPECT_THROW(auto throw_tf = this->component_->lookup_transform("dummy", "world"), exceptions::LookupTransformException); From f2ebfbc5fb48cc4326462220a7fdeb3c6d80a172 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Mon, 27 Jun 2022 08:18:15 +0200 Subject: [PATCH 45/71] Add inputs and outputs to python component interface (#83) * Slightly refactor create_output to reduce duplication and improve logging (C++) * Add translators for std_msgs (Python) * Add parse_signal_name and AddSignalError * Add output and input to ComponentInterface (Python) * Update tests (Python) * Update docstrings * Add publishing of outputs to component step * Update docstrings and TODO * Fix docstrings --- .../include/modulo_components/Component.hpp | 6 +- .../modulo_components/ComponentInterface.hpp | 50 ++++--- .../modulo_components/LifecycleComponent.hpp | 4 +- .../modulo_components/component.py | 34 +++++ .../modulo_components/component_interface.py | 135 +++++++++++++++++- .../exceptions/component_exceptions.py | 13 ++ .../modulo_components/utilities/utilities.py | 16 +++ .../src/LifecycleComponent.cpp | 5 + .../test/python/test_component.py | 21 +++ .../test/python/test_component_interface.py | 14 ++ .../translators/message_readers.py | 23 ++- .../translators/message_writers.py | 19 ++- .../python_tests/translators/test_messages.py | 13 +- 13 files changed, 313 insertions(+), 40 deletions(-) create mode 100644 source/modulo_components/test/python/test_component.py diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 41f8052be..638a911ff 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -87,11 +87,10 @@ inline void Component::add_output( ) { using namespace modulo_new_core::communication; try { - std::string parsed_signal_name = utilities::parse_signal_name(signal_name); - this->create_output(parsed_signal_name, data, fixed_topic, default_topic); + auto parsed_signal_name = this->create_output(signal_name, data, fixed_topic, default_topic); auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); RCLCPP_DEBUG_STREAM(this->get_logger(), - "Adding output '" << signal_name << "' with topic name '" << topic_name << "'."); + "Adding output '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); auto message_pair = this->outputs_.at(parsed_signal_name)->get_message_pair(); switch (message_pair->get_type()) { case MessageType::BOOL: { @@ -146,6 +145,7 @@ inline void Component::add_output( } } } catch (const std::exception& ex) { + // TODO if modulo::communication had a base exception, could catch that RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); } } diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 432bc526a..d6e623e02 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -180,6 +180,8 @@ class ComponentInterface : public NodeT { * @param fixed_topic If true, the topic name of the input signal is fixed * @param default_topic If set, the default value for the topic name to use */ + // TODO could be nice to add an optional callback here that would be executed from within the subscription callback + // in order to manipulate the data pointer upon reception of a message template void add_input( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, @@ -224,12 +226,14 @@ class ComponentInterface : public NodeT { * @param signal_name Name of the output signal * @param data Data to transmit on the output signal * @param fixed_topic If true, the topic name of the output signal is fixed + * @param default_topic If set, the default value for the topic name to use * @throws AddSignalException if the output could not be created (empty name, already registered) + * @return The parsed signal name */ template - void create_output( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, - const std::string& default_topic = "" + std::string create_output( + const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, + const std::string& default_topic ); /** @@ -777,24 +781,38 @@ inline void ComponentInterface::evaluate_periodic_callbacks() { template template -inline void ComponentInterface::create_output( +inline std::string ComponentInterface::create_output( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, const std::string& default_topic ) { using namespace modulo_new_core::communication; - if (signal_name.empty()) { - throw exceptions::AddSignalException("Failed to add output: Provide a non empty string as a name."); - } - if (this->outputs_.find(signal_name) != this->outputs_.end()) { - throw exceptions::AddSignalException("Output with name '" + signal_name + "' already exists"); + try { + auto parsed_signal_name = utilities::parse_signal_name(signal_name); + if (parsed_signal_name.empty()) { + throw exceptions::AddSignalException( + "The parsed signal name for signal '" + signal_name + + "'is empty. Provide a string with valid characters for the signal name ([a-zA-Z0-9_])." + ); + } + RCLCPP_DEBUG_STREAM(this->get_logger(), + "Creating output '" << parsed_signal_name << "' (provided signal name was '" << signal_name + << "'."); + if (this->outputs_.find(parsed_signal_name) != this->outputs_.end()) { + throw exceptions::AddSignalException("Output with name '" + signal_name + "' already exists."); + } + auto message_pair = make_shared_message_pair(data, this->get_clock()); + this->outputs_.insert_or_assign( + parsed_signal_name, std::make_shared(this->publisher_type_, message_pair)); + std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; + this->add_parameter( + parsed_signal_name + "_topic", topic_name, "Output topic name of signal '" + parsed_signal_name + "'", + fixed_topic + ); + return parsed_signal_name; + } catch (const std::exception& ex) { + // TODO if modulo::communication had a base exception, could catch that + throw exceptions::AddSignalException(std::string(ex.what())); } - auto message_pair = make_shared_message_pair(data, this->get_clock()); - this->outputs_.insert_or_assign( - signal_name, std::make_shared(this->publisher_type_, message_pair)); - std::string topic_name = default_topic.empty() ? "~/" + signal_name : default_topic; - this->add_parameter( - signal_name + "_topic", topic_name, "Output topic name of signal '" + signal_name + "'", fixed_topic - ); } template diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index cc267d0be..31c20669f 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -258,9 +258,9 @@ inline void LifecycleComponent::add_output( const std::string& default_topic ) { try { - std::string parsed_signal_name = utilities::parse_signal_name(signal_name); - this->create_output(parsed_signal_name, data, fixed_topic, default_topic); + this->create_output(signal_name, data, fixed_topic, default_topic); } catch (const std::exception& ex) { + // TODO if modulo::communication had a base exception, could catch that RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); } } diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index 4c95890dc..b21060f43 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -1,7 +1,11 @@ from threading import Thread +from typing import TypeVar +import clproto from modulo_components.component_interface import ComponentInterface +MsgT = TypeVar('MsgT') + class Component(ComponentInterface): """ @@ -35,6 +39,7 @@ def _step(self): try: # TODO catch here or in helpers...? (or re raise with ComponentError) self._publish_predicates() + self._publish_outputs() self._evaluate_periodic_callbacks() except Exception as e: self.get_logger().error(f"Failed to execute step function: {e}", throttle_duration_sec=1.0) @@ -73,3 +78,32 @@ def execute(self): :return: True, if the execution was successful, false otherwise """ return True + + def _raise_error(self): + """ + Put the component in error state by setting the 'in_error_state' + predicate to true. + """ + self.set_predicate("in_error_state", True) + + def add_output(self, signal_name: str, data: str, message_type: MsgT, + clproto_message_type=clproto.MessageType.UNKNOWN_MESSAGE, fixed_topic=False, default_topic=""): + """ + Add and configure an output signal of the component. + + :param signal_name: Name of the output signal + :param data: Name of the attribute to transmit over the channel + :param message_type: The ROS message type of the output + :param clproto_message_type: The clproto message type, if applicable + :param fixed_topic: If true, the topic name of the output signal is fixed + :param default_topic: If set, the default value for the topic name to use + """ + try: + parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, fixed_topic, + default_topic) + topic_name = self.get_parameter_value(parsed_signal_name + "_topic") + self.get_logger().debug(f"Adding output '{parsed_signal_name}' with topic name '{topic_name}'.") + publisher = self.create_publisher(message_type, topic_name, self._qos) + self._outputs[parsed_signal_name]["publisher"] = publisher + except Exception as e: + self.get_logger().error(f"Failed to add output '{signal_name}: {e}") diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 2df276b63..3f188caeb 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -1,12 +1,18 @@ import sys +from functools import partial from typing import Callable, Dict, List, Optional, TypeVar, Union +import clproto +import modulo_new_core.translators.message_readers as modulo_readers +import modulo_new_core.translators.message_writers as modulo_writers import rclpy import state_representation as sr import tf2_py from geometry_msgs.msg import TransformStamped -from modulo_components.exceptions.component_exceptions import ComponentParameterError, LookupTransformError -from modulo_components.utilities.utilities import generate_predicate_topic +from modulo_components.exceptions.component_exceptions import AddSignalError, ComponentParameterError, \ + LookupTransformError +from modulo_components.utilities.utilities import generate_predicate_topic, parse_signal_name +from modulo_new_core.encoded_state import EncodedState from modulo_new_core.translators.message_readers import read_stamped_message from modulo_new_core.translators.message_writers import write_stamped_message from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter_const @@ -17,11 +23,12 @@ from rclpy.publisher import Publisher from rclpy.qos import QoSProfile from rclpy.time import Time -from std_msgs.msg import Bool +from std_msgs.msg import Bool, Int32, Float64, Float64MultiArray, String from tf2_ros import TransformBroadcaster from tf2_ros.buffer import Buffer from tf2_ros.transform_listener import TransformListener +MsgT = TypeVar('MsgT') T = TypeVar('T') @@ -37,6 +44,8 @@ class ComponentInterface(Node): parameter_dict (dict(Publisher)): dict of parameters periodic_callbacks (dict(Callable): dict of periodic callback functions qos (rclpy.qos.QoSProfile: the Quality of Service for publishers and subscribers + inputs: dict of inputs + outputs: dict of output tf_buffer (tf2_ros.Buffer): the buffer to lookup transforms published on tf2. tf_listener (tf2_ros.TransformListener): the listener to lookup transforms published on tf2. tf_broadcaster (tf2_ros.TransformBroadcaster): the broadcaster to publish transforms on tf2 @@ -55,6 +64,8 @@ def __init__(self, node_name: str, *kargs, **kwargs): self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {} self._predicate_publishers: Dict[str, Publisher] = {} self._periodic_callbacks: Dict[str, Callable[[], None]] = {} + self._inputs = {} + self._outputs = {} self.__tf_buffer: Optional[Buffer] = None self.__tf_listener: Optional[TransformListener] = None self.__tf_broadcaster: Optional[TransformBroadcaster] = None @@ -273,6 +284,108 @@ def set_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): return self._predicates[name] = value + def _create_output(self, signal_name: str, data: str, message_type: MsgT, + clproto_message_type: clproto.MessageType, fixed_topic: bool, default_topic: str) -> str: + """ + Helper function to parse the signal name and add an output without Publisher to the dict of outputs. + + :param signal_name: Name of the output signal + :param data: Name of the attribute to transmit over the channel + :param message_type: The ROS message type of the output + :param clproto_message_type: The clproto message type, if applicable + :param fixed_topic: If true, the topic name of the output signal is fixed + :param default_topic: If set, the default value for the topic name to use + :raises AddSignalError if there is a problem adding the output + :return: The parsed signal name + """ + try: + if message_type == EncodedState and clproto_message_type == clproto.MessageType.UNKNOWN_MESSAGE: + raise AddSignalError(f"Provide a valid clproto message type for outputs of type EncodedState.") + parsed_signal_name = parse_signal_name(signal_name) + if not parsed_signal_name: + raise AddSignalError("The parsed signal name is empty. Provide a string with valid " + "characters for the signal name ([a-zA-Z0-9_]).") + if parsed_signal_name in self._outputs.keys(): + raise AddSignalError(f"Output with parsed name '{parsed_signal_name}' already exists.") + topic_name = default_topic if default_topic else "~/" + parsed_signal_name + self.add_parameter(sr.Parameter(parsed_signal_name + "_topic", topic_name, sr.ParameterType.STRING), + f"Output topic name of signal '{parsed_signal_name}'", fixed_topic) + translator = None + if message_type == Bool or message_type == Float64 or \ + message_type == Float64MultiArray or message_type == Int32 or message_type == String: + translator = modulo_writers.write_std_message + elif message_type == EncodedState: + translator = partial(modulo_writers.write_clproto_message, + clproto_message_type=clproto_message_type) + else: + raise AddSignalError("The provided message type is not supported to create a component output") + self._outputs[parsed_signal_name] = {"attribute": data, "message_type": message_type, + "translator": translator} + return parsed_signal_name + except Exception as e: + raise AddSignalError(f"{e}") + + def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Callable): + """ + Subscription callback for the ROS subscriptions. + + :param message: The message from the ROS network + :param attribute_name: The name of the attribute that is updated by the subscription + :param reader: A callable that can read the ROS message and translate to the desired type + """ + try: + self.__setattr__(attribute_name, reader(message)) + except Exception as e: + self.get_logger().error(f"Failed to read message for attribute {attribute_name}", throttle_duration_sec=1.0) + + def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, fixed_topic=False, + default_topic=""): + # TODO could be nice to add an optional callback here that would be executed from within the subscription + # callback in order to manipulate the data pointer upon reception of a message + """ + Add and configure an input signal of the component. + + :param signal_name: Name of the output signal + :param subscription: The callback to use for the subscription + :param message_type: ROS message type of the subscription + :param fixed_topic: If true, the topic name of the output signal is fixed + :param default_topic: If set, the default value for the topic name to use + """ + try: + parsed_signal_name = parse_signal_name(signal_name) + if not parsed_signal_name: + raise AddSignalError(f"Failed to add input '{signal_name}': Parsed signal name is empty.") + if parsed_signal_name in self._inputs.keys(): + raise AddSignalError(f"Failed to add input '{parsed_signal_name}': Input already exists") + topic_name = default_topic if default_topic else "~/" + parsed_signal_name + self.add_parameter(sr.Parameter(parsed_signal_name + "_topic", topic_name, sr.ParameterType.STRING), + f"Input topic name of signal '{parsed_signal_name}'", fixed_topic) + topic_name = self.get_parameter_value(parsed_signal_name + "_topic") + self.get_logger().debug(f"Adding input '{parsed_signal_name}' with topic name '{topic_name}'.") + if isinstance(subscription, Callable): + self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, subscription, + self._qos) + elif isinstance(subscription, str): + if message_type == Bool or message_type == Float64 or \ + message_type == Float64MultiArray or message_type == Int32 or message_type == String: + self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, + partial(self.__subscription_callback, + attribute_name=subscription, + reader=modulo_readers.read_std_message), + self._qos) + elif message_type == EncodedState: + self._inputs[parsed_signal_name] = self.create_subscription(message_type, topic_name, + partial(self.__subscription_callback, + attribute_name=subscription, + reader=modulo_readers.read_clproto_message), + self._qos) + else: + raise TypeError("The provided message type is not supported to create a component input") + else: + raise TypeError("Provide either a string containing the name of an attribute or a callable.") + except Exception as e: + self.get_logger().error(f"Failed to add input '{signal_name}': {e}") + def add_tf_broadcaster(self): """ Configure a transform broadcaster. @@ -306,7 +419,7 @@ def send_transform(self, transform: sr.CartesianPose): return try: transform_message = TransformStamped() - write_stamped_message(transform_message, transform, self.get_clock().now()) + modulo_writers.write_stamped_message(transform_message, transform, self.get_clock().now()) self.__tf_broadcaster.sendTransform(transform_message) except tf2_py.TransformException as e: self.get_logger().error(f"Failed to send transform: {e}", throttle_duration_sec=1.0) @@ -327,7 +440,7 @@ def lookup_transform(self, frame_name: str, reference_frame_name="world", time_p try: result = sr.CartesianPose(frame_name, reference_frame_name) transform = self.__tf_buffer.lookup_transform(reference_frame_name, frame_name, time_point, duration) - read_stamped_message(result, transform) + modulo_readers.read_stamped_message(result, transform) return result except tf2_py.TransformException as e: raise LookupTransformError(f"Failed to lookup transform: {e}") @@ -380,6 +493,18 @@ def _publish_predicates(self): continue self._predicate_publishers[name].publish(message) + def _publish_outputs(self): + """ + Helper function to publish all outputs. + """ + for signal, output_dict in self._outputs.items(): + try: + message = output_dict["message_type"]() + output_dict["translator"](message, self.__getattribute__(output_dict["attribute"])) + output_dict["publisher"].publish(message) + except Exception as e: + self.get_logger().error(f"{e}") + def _evaluate_periodic_callbacks(self): """ Helper function to evaluate all periodic function callbacks. diff --git a/source/modulo_components/modulo_components/exceptions/component_exceptions.py b/source/modulo_components/modulo_components/exceptions/component_exceptions.py index f96b8839b..50fe21e9d 100644 --- a/source/modulo_components/modulo_components/exceptions/component_exceptions.py +++ b/source/modulo_components/modulo_components/exceptions/component_exceptions.py @@ -2,6 +2,17 @@ class ComponentError(Exception): """ A base class for all component exceptions. """ + + def __init__(self, message: str): + super().__init__(message) + + +class AddSignalError(ComponentError): + """ + An exception class to notify errors when adding a signal. This is an exception class to be thrown if there is a + problem while adding a signal to the component. + """ + def __init__(self, message: str): super().__init__(message) @@ -11,6 +22,7 @@ class ComponentParameterError(ComponentError): An exception class to notify errors with component parameters. This is an exception class to be thrown if there is a problem with component parameters (overriding, inconsistent types, undeclared, ...). """ + def __init__(self, message: str): super().__init__(message) @@ -20,5 +32,6 @@ class LookupTransformError(ComponentError): An exception class to notify an error while looking up TF transforms. This is an exception class to be thrown if there is a problem with looking up a TF transform (unconfigured buffer/listener, TF2 exception). """ + def __init__(self, message: str): super().__init__(message) diff --git a/source/modulo_components/modulo_components/utilities/utilities.py b/source/modulo_components/modulo_components/utilities/utilities.py index c34590626..f8ec4fa19 100644 --- a/source/modulo_components/modulo_components/utilities/utilities.py +++ b/source/modulo_components/modulo_components/utilities/utilities.py @@ -1,3 +1,6 @@ +import re + + def generate_predicate_topic(node_name: str, predicate_name: str) -> str: """ Generate a topic name from the name of node and the predicate. @@ -7,3 +10,16 @@ def generate_predicate_topic(node_name: str, predicate_name: str) -> str: :return: The name of the topic """ return f'/predicates/{node_name}/{predicate_name}' + + +def parse_signal_name(signal_name: str) -> str: + """ + Parse a string signal name from a user-provided input. + This functions removes all characters different from a-z, A-Z, 0-9, and _ from a string. + + :param signal_name: The input string + :return: The sanitized string + """ + sanitized_string = re.sub("\W", "", signal_name, flags=re.ASCII).lower() + sanitized_string = sanitized_string.lstrip("_") + return sanitized_string diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 7d8a2fb47..4a5466b1a 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -286,6 +286,7 @@ bool LifecycleComponent::configure_outputs() { } } } catch (const std::exception& ex) { + // TODO if modulo::communication had a base exception, could catch that success = false; RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to configure output '" << name << "': " << ex.what()); } @@ -306,10 +307,12 @@ bool LifecycleComponent::activate_outputs() { try { interface->activate(); } catch (const std::exception& ex) { + // TODO if modulo::communication had a base exception, could catch that success = false; RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to activate output '" << name << "': " << ex.what()); } } + RCLCPP_DEBUG(this->get_logger(), "All outputs activated."); return success; } @@ -319,10 +322,12 @@ bool LifecycleComponent::deactivate_outputs() { try { interface->deactivate(); } catch (const std::exception& ex) { + // TODO if modulo::communication had a base exception, could catch that success = false; RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to deactivate output '" << name << "': " << ex.what()); } } + RCLCPP_DEBUG(this->get_logger(), "All outputs deactivated."); return success; } }// namespace modulo_components diff --git a/source/modulo_components/test/python/test_component.py b/source/modulo_components/test/python/test_component.py new file mode 100644 index 000000000..0b4aa9d3b --- /dev/null +++ b/source/modulo_components/test/python/test_component.py @@ -0,0 +1,21 @@ +import pytest +from modulo_components.component import Component +from std_msgs.msg import Bool, String + + +@pytest.fixture() +def component(ros_context): + yield Component('component') + + +def test_add_output(component): + component.add_output("_tEsT_#1@3", "test", Bool) + assert "test_13" in component._outputs.keys() + assert component.get_parameter_value("test_13_topic") == "~/test_13" + + component.add_output("_tEsT_#1@5", "test", Bool, default_topic="/topic") + assert "test_15" in component._outputs.keys() + assert component.get_parameter_value("test_15_topic") == "/topic" + + component.add_output("test_13", "test", String) + assert component._outputs["test_13"]["message_type"] == Bool diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 056ffba19..7af131737 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -4,6 +4,7 @@ import state_representation as sr from modulo_components.component_interface import ComponentInterface from modulo_components.exceptions.component_exceptions import LookupTransformError +from std_msgs.msg import Bool, String def raise_(ex): @@ -51,6 +52,19 @@ def test_set_predicate(component_interface): assert not component_interface.get_predicate('bar') +def test_add_input(component_interface): + component_interface.add_input("_tEsT_#1@3", "test", Bool) + assert "test_13" in component_interface._inputs.keys() + assert component_interface.get_parameter_value("test_13_topic") == "~/test_13" + + component_interface.add_input("_tEsT_#1@5", "test", Bool, default_topic="/topic") + assert "test_15" in component_interface._inputs.keys() + assert component_interface.get_parameter_value("test_15_topic") == "/topic" + + component_interface.add_input("test_13", "test", String) + assert component_interface._inputs["test_13"].msg_type == Bool + + def test_tf(component_interface): component_interface.add_tf_broadcaster() component_interface.add_tf_listener() diff --git a/source/modulo_new_core/modulo_new_core/translators/message_readers.py b/source/modulo_new_core/modulo_new_core/translators/message_readers.py index 4b9e6d43f..32a52b175 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_readers.py +++ b/source/modulo_new_core/modulo_new_core/translators/message_readers.py @@ -6,6 +6,7 @@ from modulo_new_core.encoded_state import EncodedState from sensor_msgs.msg import JointState +DataT = TypeVar('DataT') MsgT = TypeVar('MsgT') StateT = TypeVar('StateT') @@ -15,6 +16,7 @@ def read_xyz(message: Union[geometry.Point, geometry.Vector3]) -> List[float]: Helper function to read a list from a Point or Vector3 message. :param message: The message to read from + :return: The vector coefficients as a list """ return [message.x, message.y, message.z] @@ -24,17 +26,19 @@ def read_quaternion(message: geometry.Quaternion) -> List[float]: Helper function to read a list of quaternion coefficients (w,x,y,z) from a Quaternion message. :param message: The message to read from + :return: The quaternion coefficients as a list """ return [message.w, message.x, message.y, message.z] -def read_message(state: StateT, message: MsgT): +def read_message(state: StateT, message: MsgT) -> StateT: """ Convert a ROS message to a state_representation State type. :param state: The state to populate :param message: The ROS message to read from :raises Exception if the message could not be read + :return: The populated state """ if not isinstance(state, sr.State): raise RuntimeError("This state type is not supported.") @@ -69,13 +73,14 @@ def read_message(state: StateT, message: MsgT): return state -def read_stamped_message(state: StateT, message: MsgT): +def read_stamped_message(state: StateT, message: MsgT) -> StateT: """ Convert a stamped ROS message to a state_representation State type. :param state: The state to populate :param message: The ROS message to read from :raises Exception if the message could not be read + :return: The populated state """ if isinstance(message, geometry.AccelStamped): read_message(state, message.accel) @@ -91,6 +96,17 @@ def read_stamped_message(state: StateT, message: MsgT): else: raise RuntimeError("The provided combination of state type and message type is not supported") state.set_reference_frame(message.header.frame_id) + return state + + +def read_std_message(message: MsgT) -> DataT: + """ + Read the data field of a std_msg.msg message. + + :param message: The message to read from + :return: The data field of the message + """ + return message.data def read_clproto_message(message: EncodedState) -> StateT: @@ -98,6 +114,7 @@ def read_clproto_message(message: EncodedState) -> StateT: Convert an EncodedState message to a state_representation State type. :param message: The EncodedState message to read from - :raises Exception if the message could not be read + :raises Exception if the message could not be read: + :return: The decoded clproto message """ return clproto.decode(message.data.tobytes()) diff --git a/source/modulo_new_core/modulo_new_core/translators/message_writers.py b/source/modulo_new_core/modulo_new_core/translators/message_writers.py index 87183a039..45aff299e 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_writers.py +++ b/source/modulo_new_core/modulo_new_core/translators/message_writers.py @@ -8,6 +8,7 @@ from modulo_new_core.encoded_state import EncodedState from sensor_msgs.msg import JointState +DataT = TypeVar('DataT') MsgT = TypeVar('MsgT') StateT = TypeVar('StateT') @@ -108,17 +109,25 @@ def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): message.header.frame_id = state.get_reference_frame() -def write_clproto_message(state: StateT, clproto_message_type: clproto.MessageType) -> EncodedState(): +def write_std_message(message: MsgT, data: DataT): """ - Convert a state_representation State type to an EncodedState message. + Convert to a std_msgs.msg message. + :param message: The std_msgs.msg message to populate + :param data: The data to read from + """ + message.data = data + + +def write_clproto_message(message: EncodedState, state: StateT, clproto_message_type: clproto.MessageType): + """ + Convert a state_representation State to an EncodedState message. + + :param message: The EncodedState message to populate :param state: The state to read from :param clproto_message_type: The clproto message type to encode the state :raises Exception if the message could not be written - :return: The populated message """ if not isinstance(state, sr.State): raise RuntimeError("This state type is not supported.") - message = EncodedState() message.data = clproto.encode(state, clproto_message_type) - return message diff --git a/source/modulo_new_core/test/python_tests/translators/test_messages.py b/source/modulo_new_core/test/python_tests/translators/test_messages.py index f14d8a085..3b3224cc7 100644 --- a/source/modulo_new_core/test/python_tests/translators/test_messages.py +++ b/source/modulo_new_core/test/python_tests/translators/test_messages.py @@ -1,13 +1,13 @@ import clproto import geometry_msgs.msg +import modulo_new_core.translators.message_readers as modulo_readers +import modulo_new_core.translators.message_writers as modulo_writers import numpy as np import pytest -import sensor_msgs.msg import state_representation as sr +from modulo_new_core.encoded_state import EncodedState from rclpy.clock import Clock - -import modulo_new_core.translators.message_readers as modulo_readers -import modulo_new_core.translators.message_writers as modulo_writers +from sensor_msgs.msg import JointState def read_xyz(message): @@ -143,7 +143,7 @@ def test_wrench(cart_state: sr.CartesianState, clock: Clock): def test_joint_state(joint_state: sr.JointState): - message = sensor_msgs.msg.JointState() + message = JointState() modulo_writers.write_message(message, joint_state) for i in range(joint_state.get_size()): assert message.name[i] == joint_state.get_names()[i] @@ -163,7 +163,8 @@ def test_joint_state(joint_state: sr.JointState): def test_encoded_state(cart_state: sr.CartesianState): - message = modulo_writers.write_clproto_message(cart_state, clproto.MessageType.CARTESIAN_STATE_MESSAGE) + message = EncodedState() + modulo_writers.write_clproto_message(message, cart_state, clproto.MessageType.CARTESIAN_STATE_MESSAGE) new_state = modulo_readers.read_clproto_message(message) assert_np_array_equal(new_state.data(), cart_state.data()) From 8929cdc1d4a3f05062ef95f5517ba6fe87203a7f Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Mon, 27 Jun 2022 13:30:51 +0200 Subject: [PATCH 46/71] Translation of empty parameters (#86) * Fix writing of an empty parameter (Python) * Use galactic-devel image for empty Parameter * Base image tag for build-test * Update required cl version --- .github/actions/build-test-galactic/Dockerfile | 2 +- build-server.sh | 2 +- source/modulo_components/CMakeLists.txt | 2 +- source/modulo_new_core/CMakeLists.txt | 2 +- .../translators/parameter_translators.py | 17 +++++++++++------ .../test/python_tests/conftest.py | 17 +++++++++-------- .../python_tests/translators/test_parameters.py | 12 ++++++++++++ 7 files changed, 36 insertions(+), 18 deletions(-) diff --git a/.github/actions/build-test-galactic/Dockerfile b/.github/actions/build-test-galactic/Dockerfile index d0e9f7dee..731001a7d 100644 --- a/.github/actions/build-test-galactic/Dockerfile +++ b/.github/actions/build-test-galactic/Dockerfile @@ -1,5 +1,5 @@ # https://docs.github.com/en/actions/creating-actions/dockerfile-support-for-github-actions -FROM ghcr.io/aica-technology/ros2-control-libraries:galactic +FROM ghcr.io/aica-technology/ros2-control-libraries:galactic-devel # Copy and set the entrypoint commands to execute when the container starts, # explicilty invoking as ros2 user and with bash interpreter. diff --git a/build-server.sh b/build-server.sh index bc89003e1..ed9a726d8 100755 --- a/build-server.sh +++ b/build-server.sh @@ -1,5 +1,5 @@ #!/bin/bash -BASE_TAG=galactic +BASE_TAG=galactic-devel IMAGE_NAME=epfl-lasa/modulo IMAGE_TAG=latest diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 6b242b6fb..9afc3a34c 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 6.0.0 REQUIRED COMPONENTS state_representation) +find_package(control_libraries 6.0.2 REQUIRED COMPONENTS state_representation) find_package(clproto 6.0.0 REQUIRED) include_directories(include) diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt index 629831c9a..c9d4889be 100644 --- a/source/modulo_new_core/CMakeLists.txt +++ b/source/modulo_new_core/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 6.0.0 REQUIRED COMPONENTS state_representation) +find_package(control_libraries 6.0.2 REQUIRED COMPONENTS state_representation) find_package(clproto 6.0.0 REQUIRED) include_directories(include) diff --git a/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py b/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py index 50eeb1cbd..51932bf0d 100644 --- a/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py +++ b/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py @@ -15,14 +15,18 @@ def write_parameter(parameter: sr.Parameter) -> Parameter: :return: The resulting ROS parameter """ if parameter.get_parameter_type() == sr.ParameterType.BOOL or \ - parameter.get_parameter_type() == sr.ParameterType.BOOL_ARRAY or \ parameter.get_parameter_type() == sr.ParameterType.INT or \ - parameter.get_parameter_type() == sr.ParameterType.INT_ARRAY or \ parameter.get_parameter_type() == sr.ParameterType.DOUBLE or \ - parameter.get_parameter_type() == sr.ParameterType.DOUBLE_ARRAY or \ - parameter.get_parameter_type() == sr.ParameterType.STRING or \ - parameter.get_parameter_type() == sr.ParameterType.STRING_ARRAY: + parameter.get_parameter_type() == sr.ParameterType.STRING: return Parameter(parameter.get_name(), value=parameter.get_value()) + elif parameter.get_parameter_type() == sr.ParameterType.BOOL_ARRAY: + return Parameter(parameter.get_name(), value=parameter.get_value(), type_=Parameter.Type.BOOL_ARRAY) + elif parameter.get_parameter_type() == sr.ParameterType.INT_ARRAY: + return Parameter(parameter.get_name(), value=parameter.get_value(), type_=Parameter.Type.INTEGER_ARRAY) + elif parameter.get_parameter_type() == sr.ParameterType.DOUBLE_ARRAY: + return Parameter(parameter.get_name(), value=parameter.get_value(), type_=Parameter.Type.DOUBLE_ARRAY) + elif parameter.get_parameter_type() == sr.ParameterType.STRING_ARRAY: + return Parameter(parameter.get_name(), value=parameter.get_value(), type_=Parameter.Type.STRING_ARRAY) elif parameter.get_parameter_type() == sr.ParameterType.STATE: if parameter.get_parameter_state_type() == sr.StateType.CARTESIAN_STATE: return Parameter(parameter.get_name(), Parameter.Type.STRING, clproto.to_json( @@ -40,7 +44,8 @@ def write_parameter(parameter: sr.Parameter) -> Parameter: raise RuntimeError(f"Parameter {parameter.get_name()} could not be written!") elif parameter.get_parameter_type() == sr.ParameterType.VECTOR or \ parameter.get_parameter_type() == sr.ParameterType.MATRIX: - return Parameter(parameter.get_name(), value=parameter.get_value().flatten().tolist()) + return Parameter(parameter.get_name(), value=parameter.get_value().flatten().tolist(), + type_=Parameter.Type.DOUBLE_ARRAY) else: raise RuntimeError(f"Parameter {parameter.get_name()} could not be written!") diff --git a/source/modulo_new_core/test/python_tests/conftest.py b/source/modulo_new_core/test/python_tests/conftest.py index fc83484ff..5db826a04 100644 --- a/source/modulo_new_core/test/python_tests/conftest.py +++ b/source/modulo_new_core/test/python_tests/conftest.py @@ -2,6 +2,7 @@ import pytest import rclpy.clock import state_representation as sr +from rclpy import Parameter @pytest.fixture @@ -21,14 +22,14 @@ def clock(): @pytest.fixture def parameters(): - return {"bool": [True, sr.ParameterType.BOOL, False], - "bool_array": [[True, False], sr.ParameterType.BOOL_ARRAY, [False]], - "int": [1, sr.ParameterType.INT, 2], - "int_array": [[1, 2], sr.ParameterType.INT_ARRAY, [2]], - "double": [1.0, sr.ParameterType.DOUBLE, 2.0], - "double_array": [[1.0, 2.0], sr.ParameterType.DOUBLE_ARRAY, [2.0]], - "string": ["1", sr.ParameterType.STRING, "2"], - "string_array": [["1", "2"], sr.ParameterType.STRING_ARRAY, ["2"]] + return {"bool": [True, sr.ParameterType.BOOL, False, Parameter.Type.BOOL], + "bool_array": [[True, False], sr.ParameterType.BOOL_ARRAY, [False], Parameter.Type.BOOL_ARRAY], + "int": [1, sr.ParameterType.INT, 2, Parameter.Type.INTEGER], + "int_array": [[1, 2], sr.ParameterType.INT_ARRAY, [2], Parameter.Type.INTEGER_ARRAY], + "double": [1.0, sr.ParameterType.DOUBLE, 2.0, Parameter.Type.DOUBLE], + "double_array": [[1.0, 2.0], sr.ParameterType.DOUBLE_ARRAY, [2.0], Parameter.Type.DOUBLE_ARRAY], + "string": ["1", sr.ParameterType.STRING, "2", Parameter.Type.STRING], + "string_array": [["1", "2"], sr.ParameterType.STRING_ARRAY, ["2"], Parameter.Type.STRING_ARRAY] } diff --git a/source/modulo_new_core/test/python_tests/translators/test_parameters.py b/source/modulo_new_core/test/python_tests/translators/test_parameters.py index c725d2080..d398c87ee 100644 --- a/source/modulo_new_core/test/python_tests/translators/test_parameters.py +++ b/source/modulo_new_core/test/python_tests/translators/test_parameters.py @@ -15,6 +15,9 @@ def assert_np_array_equal(a: np.array, b: np.array, places=3): def test_parameter_write(parameters): for name, value_types in parameters.items(): + param = sr.Parameter(name, value_types[1]) + ros_param = write_parameter(param) + assert ros_param.type_ == value_types[3] param = sr.Parameter(name, value_types[0], value_types[1]) ros_param = write_parameter(param) assert ros_param.to_parameter_msg() == Parameter(name, value=value_types[0]).to_parameter_msg() @@ -64,6 +67,9 @@ def test_parameter_non_const_read(parameters): def test_state_parameter_write(state_parameters): for name, value_types in state_parameters.items(): + param = sr.Parameter(name, sr.ParameterType.STATE, value_types[1]) + ros_param = write_parameter(param) + assert ros_param.type_ == Parameter.Type.STRING param = sr.Parameter(name, value_types[0], sr.ParameterType.STATE, value_types[1]) ros_param = write_parameter(param) assert ros_param.name == param.get_name() @@ -94,6 +100,9 @@ def test_state_parameter_read_write(state_parameters): def test_vector_parameter_read_write(): vec = np.random.rand(3) + param = sr.Parameter("vector", sr.ParameterType.VECTOR) + ros_param = write_parameter(param) + assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY param = sr.Parameter("vector", vec, sr.ParameterType.VECTOR) ros_param = write_parameter(param) assert ros_param.name == param.get_name() @@ -114,6 +123,9 @@ def test_vector_parameter_read_write(): def test_matrix_parameter_read_write(): mat = np.random.rand(3, 4) + param = sr.Parameter("vector", sr.ParameterType.MATRIX) + ros_param = write_parameter(param) + assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY param = sr.Parameter("matrix", mat, sr.ParameterType.MATRIX) ros_param = write_parameter(param) assert ros_param.name == param.get_name() From b565fd56c4b9799452eb8d6593147aeec3faeca8 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 29 Jun 2022 10:37:17 +0200 Subject: [PATCH 47/71] Add fallback name to constructors (#87) --- .../include/modulo_components/Component.hpp | 6 +++++- .../include/modulo_components/ComponentInterface.hpp | 12 ++++++++---- .../include/modulo_components/LifecycleComponent.hpp | 5 ++++- source/modulo_components/src/Component.cpp | 4 ++-- source/modulo_components/src/LifecycleComponent.cpp | 6 ++++-- 5 files changed, 23 insertions(+), 10 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 638a911ff..93d59873e 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -20,8 +20,12 @@ class Component : public ComponentInterface { /** * @brief Constructor from node options. * @param node_options Node options as used in ROS2 Node + * @param start_thread If true, start the execution thread on construction + * @param fallback_name The name of the component if it was not provided through the node options */ - explicit Component(const rclcpp::NodeOptions& node_options, bool start_thread = true); + explicit Component( + const rclcpp::NodeOptions& node_options, bool start_thread = true, const std::string& fallback_name = "Component" + ); /** * @brief Virtual default destructor. diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index d6e623e02..150145d20 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -51,9 +51,12 @@ class ComponentInterface : public NodeT { /** * @brief Constructor from node options. * @param node_options Node options as used in ROS2 Node / LifecycleNode + * @param publisher_type The type of publisher that also indicates if the component is lifecycle or not + * @param fallback_name The name of the component if it was not provided through the node options */ explicit ComponentInterface( - const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type + const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type, + const std::string& fallback_name = "ComponentInterface" ); /** @@ -339,9 +342,10 @@ class ComponentInterface : public NodeT { template ComponentInterface::ComponentInterface( - const rclcpp::NodeOptions& options, modulo_new_core::communication::PublisherType publisher_type + const rclcpp::NodeOptions& options, modulo_new_core::communication::PublisherType publisher_type, + const std::string& fallback_name ) : - NodeT(utilities::parse_node_name(options, "ComponentInterface"), options), publisher_type_(publisher_type) { + NodeT(utilities::parse_node_name(options, fallback_name), options), publisher_type_(publisher_type) { // register the parameter change callback handler parameter_cb_handle_ = NodeT::add_on_set_parameters_callback( [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { @@ -400,7 +404,7 @@ inline void ComponentInterface::add_parameter( NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); } else { RCLCPP_WARN_STREAM(this->get_logger(), - "Parameter '" << parameter->get_name() << "' already exists, overwriting."); + "Parameter '" << parameter->get_name() << "' already exists, overwriting."); NodeT::set_parameter(ros_param); } } catch (const std::exception& ex) { diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 31c20669f..922919f44 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -17,8 +17,11 @@ class LifecycleComponent : public ComponentInterface(node_options, PublisherType::PUBLISHER), started_(false) { +Component::Component(const rclcpp::NodeOptions& node_options, bool start_thread, const std::string& fallback_name) : + ComponentInterface(node_options, PublisherType::PUBLISHER, fallback_name), started_(false) { this->add_predicate("is_finished", false); if (start_thread) { diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 4a5466b1a..23d6c308c 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -6,8 +6,10 @@ using namespace modulo_new_core::communication; namespace modulo_components { -LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options) : - ComponentInterface(node_options, PublisherType::LIFECYCLE_PUBLISHER) { +LifecycleComponent::LifecycleComponent(const rclcpp::NodeOptions& node_options, const std::string& fallback_name) : + ComponentInterface( + node_options, PublisherType::LIFECYCLE_PUBLISHER, fallback_name + ) { this->add_predicate( "is_unconfigured", [this] { return this->get_current_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; From 8f5b9c5d8b336d1025310b78bf79dde4f8c5dae4 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 30 Jun 2022 07:56:14 +0200 Subject: [PATCH 48/71] Add lifecycle component in Python (#89) * Add Python lifecycle component * Docstring, TODO * Make configure and activate protected * Use a state to publish predicates * Fix log line * Remove TODO * Move to new transition call hierarchy * Choose more verbose callback name * Minor fixes --- .../modulo_components/component_interface.py | 2 +- .../modulo_components/lifecycle_component.py | 251 ++++++++++++++++++ .../src/LifecycleComponent.cpp | 2 +- 3 files changed, 253 insertions(+), 2 deletions(-) create mode 100644 source/modulo_components/modulo_components/lifecycle_component.py diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 3f188caeb..73ced35ea 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -346,7 +346,7 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag Add and configure an input signal of the component. :param signal_name: Name of the output signal - :param subscription: The callback to use for the subscription + :param subscription: Name of the attribute to receive messages for or the callback to use for the subscription :param message_type: ROS message type of the subscription :param fixed_topic: If true, the topic name of the output signal is fixed :param default_topic: If set, the default value for the topic name to use diff --git a/source/modulo_components/modulo_components/lifecycle_component.py b/source/modulo_components/modulo_components/lifecycle_component.py new file mode 100644 index 000000000..6b2e250a2 --- /dev/null +++ b/source/modulo_components/modulo_components/lifecycle_component.py @@ -0,0 +1,251 @@ +from typing import TypeVar + +import clproto +from lifecycle_msgs.msg import Transition +from lifecycle_msgs.srv import ChangeState +from lifecycle_msgs.msg import State +from modulo_components.component_interface import ComponentInterface +from modulo_components.exceptions.component_exceptions import AddSignalError + +MsgT = TypeVar('MsgT') + + +class LifecycleComponent(ComponentInterface): + """ + Class to represent a LifecycleComponent in python, following the same logic pattern + as the c++ modulo_component::LifecycleComponent class. + ... + Attributes: + # TODO + """ + + def __init__(self, node_name: str, *kargs, **kwargs): + """ + Constructs all the necessary attributes and declare all the parameters. + Parameters: + node_name (str): name of the component to be passed to the base Node class + """ + super().__init__(node_name, *kargs, **kwargs) + self.__state = State.PRIMARY_STATE_UNCONFIGURED + + # add the service to mimic the lifecycle paradigm + self._change_state_srv = self.create_service(ChangeState, '~/change_state', self.__change_state) + + self.add_predicate("is_unconfigured", lambda: self.__state == State.PRIMARY_STATE_UNCONFIGURED) + self.add_predicate("is_inactive", lambda: self.__state == State.PRIMARY_STATE_INACTIVE) + self.add_predicate("is_active", lambda: self.__state == State.PRIMARY_STATE_ACTIVE) + self.add_predicate("is_finalized", lambda: self.__state == State.PRIMARY_STATE_FINALIZED) + + def __change_state(self, request: ChangeState.Request, response: ChangeState.Response) -> ChangeState.Response: + """ + Change state service callback which calls the requested transition method. + + :param request: The lifecycle_msgs.msg.ChangeState.Request message + :param response: The lifecycle_msgs.msg.ChangeState.Response message + :return: The response message + """ + self.get_logger().debug(f'Change state service called with request {request}') + if request.transition.id == Transition.TRANSITION_CONFIGURE: + response = self.__on_configure(request, response) + elif request.transition.id == Transition.TRANSITION_CLEANUP: + response = self.__on_cleanup(request, response) + elif request.transition.id == Transition.TRANSITION_ACTIVATE: + response = self.__on_activate(request, response) + elif request.transition.id == Transition.TRANSITION_DEACTIVATE: + response = self.__on_deactivate(request, response) + # TODO the other transitions + else: + self.get_logger().error(f'Unsupported state transition! {request}') + response.success = False + return response + + def __on_configure(self, request: ChangeState.Request, response: ChangeState.Response) -> ChangeState.Response: + """ + Configure service callback. Simply call the handle_configure function. + + :param request: Request to change the state to inactive + :param response: The response object from the service call + :return: True if the component configured successfully + """ + if not self.__state == State.PRIMARY_STATE_UNCONFIGURED: + response.success = False + return response + + response.success = self.__handle_configure() + if response.success: + self.__state = State.PRIMARY_STATE_INACTIVE + return response + + def __handle_configure(self) -> bool: + """ + Handle the configure transition by related transition steps and invoking the user callback. + + :return: True if the transition was successful + """ + return self.__configure_outputs() and self.on_configure_callback() + + def on_configure_callback(self) -> bool: + """ + Function called from the configure transition service callback. To be redefined in derived classes. + + :return: True if the component configured successfully + """ + return True + + def __on_cleanup(self, request: ChangeState.Request, response: ChangeState.Response) -> ChangeState.Response: + """ + Cleanup service callback. Simply call the handle_cleanup function. + + :param request: Request to change the state to unconfigured + :param response: The response object from the service call + :return: True if the component was cleaned up successfully + """ + if self.__state != State.PRIMARY_STATE_INACTIVE: + response.success = False + return response + + response.success = self.__handle_cleanup() + if response.success: + self.__state = State.PRIMARY_STATE_UNCONFIGURED + return response + + def __handle_cleanup(self) -> bool: + """ + Handle the cleanup transition by related transition steps and invoking the user callback. + + :return: True if the transition was successful + """ + return self.on_cleanup_callback() + + def on_cleanup_callback(self) -> bool: + """ + Function called from the cleanup transition service callback. To be redefined in derived classes. + + :return: True if the component was cleaned up successfully + """ + return True + + def __on_activate(self, request: ChangeState.Request, response: ChangeState.Response) -> ChangeState.Response: + """ + Activate service callback. Simply call the handle_activate function. + + :param request: Request to change the state to active state + :param response: The response object from the service call + :return: True if the component is activated successfully + """ + if self.__state != State.PRIMARY_STATE_INACTIVE: + response.success = False + return response + + response.success = self.__handle_activate() + if response.success: + self.__state = State.PRIMARY_STATE_ACTIVE + return response + + def __handle_activate(self) -> bool: + """ + Handle the activate transition by related transition steps and invoking the user callback. + + :return: True if the transition was successful + """ + return self.on_activate_callback() + + def on_activate_callback(self) -> bool: + """ + Function called from the activate transition service callback. To be redefined in derived classes. + + :return: True if the component was activated successfully + """ + return True + + def __on_deactivate(self, request: ChangeState.Request, response: ChangeState.Response) -> ChangeState.Response: + """ + Dectivate service callback. Simply call the handle_deactivate function. + + :param request: Request to change the state to inactive state + :param response: The response object from the service call + :return: True if the component is deactivated successfully + """ + if self.__state != State.PRIMARY_STATE_ACTIVE: + response.success = False + return response + + response.success = self.__handle_deactivate() + if response.success: + self.__state = State.PRIMARY_STATE_INACTIVE + return response + + def __handle_deactivate(self) -> bool: + """ + Handle the deactivate transition by related transition steps and invoking the user callback. + + :return: True if the transition was successful + """ + return self.on_deactivate_callback() + + def on_deactivate_callback(self) -> bool: + """ + Function called from the deactivate transition service callback. To be redefined in derived classes. + + :return: True if the component was deactivated successfully + """ + return True + + # TODO on shutdown and on error + + def _step(self): + """ + Step function that is called periodically and publishes predicates, outputs, evaluates daemon callbacks, and + calls the on_step function. + """ + try: + self._publish_predicates() + if self.__state == State.PRIMARY_STATE_ACTIVE: + self._publish_outputs() + self._evaluate_periodic_callbacks() + self.on_step_callback() + except Exception as e: + self.get_logger().error(f"Failed to execute step function: {e}", throttle_duration_sec=1.0) + + def on_step_callback(self): + """ + Steps to execute periodically. To be redefined by derived classes. + """ + pass + + def __configure_outputs(self) -> bool: + """ + Configure all outputs. + + :return: True if configuration was successful + """ + success = True + for signal_name, output_dict in self._outputs.items(): + try: + topic_name = self.get_parameter_value(signal_name + "_topic") + self.get_logger().debug(f"Configuring output '{signal_name}' with topic name '{topic_name}'.") + publisher = self.create_publisher(output_dict["message_type"], topic_name, self._qos) + self._outputs[signal_name]["publisher"] = publisher + except Exception as e: + success = False + self.get_logger().debug(f"Failed to configure output '{signal_name}': {e}") + return success + + def add_output(self, signal_name: str, data: str, message_type: MsgT, + clproto_message_type=clproto.MessageType.UNKNOWN_MESSAGE, fixed_topic=False, default_topic=""): + """ + Add an output signal of the component. + + :param signal_name: Name of the output signal + :param data: Name of the attribute to transmit over the channel + :param message_type: The ROS message type of the output + :param clproto_message_type: The clproto message type, if applicable + :param fixed_topic: If true, the topic name of the output signal is fixed + :param default_topic: If set, the default value for the topic name to use + """ + try: + parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, fixed_topic, + default_topic) + self.get_logger().debug(f"Adding output '{parsed_signal_name}.") + except AddSignalError as e: + self.get_logger().error(f"Failed to add output '{signal_name}': {e}") diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 23d6c308c..9983e4751 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -155,7 +155,7 @@ bool LifecycleComponent::on_deactivate() { rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { - RCLCPP_DEBUG(this->get_logger(), "on_deactivate called from previous state %s", previous_state.label().c_str()); + RCLCPP_DEBUG(this->get_logger(), "on_shutdown called from previous state %s", previous_state.label().c_str()); switch (previous_state.id()) { case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; From 8c1754958fed8dbf5c33a513b2c13eb5398bd9ad Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 30 Jun 2022 07:57:26 +0200 Subject: [PATCH 49/71] Rename transition handlers and callbacks (#90) --- .../modulo_components/LifecycleComponent.hpp | 26 ++++----- .../src/LifecycleComponent.cpp | 54 +++++++++---------- 2 files changed, 40 insertions(+), 40 deletions(-) diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 922919f44..058c76f92 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -35,7 +35,7 @@ class LifecycleComponent : public ComponentInterface::evaluate_periodic_callbacks; }; -inline void LifecycleComponent::on_step() {} +inline void LifecycleComponent::on_step_callback() {} template inline void LifecycleComponent::add_output( diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index 9983e4751..dfebb14a5 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -38,7 +38,7 @@ void LifecycleComponent::step() { if (this->get_predicate("is_active")) { this->publish_outputs(); this->evaluate_periodic_callbacks(); - this->on_step(); + this->on_step_callback(); } } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to execute step function:" << ex.what()); @@ -54,10 +54,10 @@ LifecycleComponent::on_configure(const rclcpp_lifecycle::State& previous_state) RCLCPP_WARN(get_logger(), "Invalid transition 'configure' from state %s.", previous_state.label().c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - if (!this->configure()) { + if (!this->handle_configure()) { RCLCPP_WARN(get_logger(), "Configuration failed! Reverting to the unconfigured state."); // perform cleanup actions to ensure the component is unconfigured - if (this->cleanup()) { + if (this->handle_cleanup()) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } else { RCLCPP_ERROR(get_logger(), @@ -68,12 +68,12 @@ LifecycleComponent::on_configure(const rclcpp_lifecycle::State& previous_state) return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -bool LifecycleComponent::configure() { +bool LifecycleComponent::handle_configure() { bool result = this->configure_outputs(); - return result && this->on_configure(); + return result && this->on_configure_callback(); } -bool LifecycleComponent::on_configure() { +bool LifecycleComponent::on_configure_callback() { return true; } @@ -84,19 +84,19 @@ LifecycleComponent::on_cleanup(const rclcpp_lifecycle::State& previous_state) { RCLCPP_WARN(get_logger(), "Invalid transition 'cleanup' from state %s.", previous_state.label().c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - if (!this->cleanup()) { + if (!this->handle_cleanup()) { RCLCPP_ERROR(get_logger(), "Cleanup failed! Entering into the error processing transition state."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -bool LifecycleComponent::cleanup() { +bool LifecycleComponent::handle_cleanup() { bool result = this->cleanup_signals(); - return result && this->on_cleanup(); + return result && this->on_cleanup_callback(); } -bool LifecycleComponent::on_cleanup() { +bool LifecycleComponent::on_cleanup_callback() { return true; } @@ -107,10 +107,10 @@ LifecycleComponent::on_activate(const rclcpp_lifecycle::State& previous_state) { RCLCPP_WARN(get_logger(), "Invalid transition 'activate' from state %s.", previous_state.label().c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - if (!this->activate()) { + if (!this->handle_activate()) { RCLCPP_WARN(get_logger(), "Activation failed! Reverting to the inactive state."); // perform deactivation actions to ensure the component is inactive - if (this->deactivate()) { + if (this->handle_deactivate()) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } else { RCLCPP_ERROR(get_logger(), @@ -121,12 +121,12 @@ LifecycleComponent::on_activate(const rclcpp_lifecycle::State& previous_state) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -bool LifecycleComponent::activate() { +bool LifecycleComponent::handle_activate() { bool result = this->activate_outputs(); - return result && this->on_activate(); + return result && this->on_activate_callback(); } -bool LifecycleComponent::on_activate() { +bool LifecycleComponent::on_activate_callback() { return true; } @@ -137,19 +137,19 @@ LifecycleComponent::on_deactivate(const rclcpp_lifecycle::State& previous_state) RCLCPP_WARN(get_logger(), "Invalid transition 'deactivate' from state %s.", previous_state.label().c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - if (!this->deactivate()) { + if (!this->handle_deactivate()) { RCLCPP_ERROR(get_logger(), "Deactivation failed! Entering into the error processing transition state."); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } -bool LifecycleComponent::deactivate() { +bool LifecycleComponent::handle_deactivate() { bool result = this->deactivate_outputs(); - return result && this->on_deactivate(); + return result && this->on_deactivate_callback(); } -bool LifecycleComponent::on_deactivate() { +bool LifecycleComponent::on_deactivate_callback() { return true; } @@ -160,19 +160,19 @@ LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { case lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED: return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; case lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE: - if (!this->deactivate()) { + if (!this->handle_deactivate()) { RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate deactivation!"); break; } [[fallthrough]]; case lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE: - if (!this->cleanup()) { + if (!this->handle_cleanup()) { RCLCPP_DEBUG(get_logger(), "Shutdown failed during intermediate cleanup!"); break; } [[fallthrough]]; case lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED: - if (!this->shutdown()) { + if (!this->handle_shutdown()) { break; } // TODO: reset and finalize all needed properties @@ -189,11 +189,11 @@ LifecycleComponent::on_shutdown(const rclcpp_lifecycle::State& previous_state) { return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } -bool LifecycleComponent::shutdown() { - return this->on_shutdown(); +bool LifecycleComponent::handle_shutdown() { + return this->on_shutdown_callback(); } -bool LifecycleComponent::on_shutdown() { +bool LifecycleComponent::on_shutdown_callback() { return true; } @@ -218,10 +218,10 @@ LifecycleComponent::on_error(const rclcpp_lifecycle::State& previous_state) { } bool LifecycleComponent::handle_error() { - return this->on_error(); + return this->on_error_callback(); } -bool LifecycleComponent::on_error() { +bool LifecycleComponent::on_error_callback() { return true; } From 5001a819cf8fb7db8406ded716a2b30fcee1bbc9 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 30 Jun 2022 09:27:50 +0200 Subject: [PATCH 50/71] Remove old modulo_core and rename modulo_new_core (#91) * Delete modulo_core * Update package xmls * Rename modulo_new_core to modulo_core * Fix formatting (C++) * Fix formatting (Python) * Remove duplicate from github action --- .../actions/build-test-galactic/entrypoint.sh | 1 - Dockerfile | 1 - .../include/modulo_components/Component.hpp | 9 +- .../modulo_components/ComponentInterface.hpp | 52 +- .../modulo_components/LifecycleComponent.hpp | 2 +- .../modulo_components/component_interface.py | 31 +- source/modulo_components/package.xml | 15 +- source/modulo_components/src/Component.cpp | 2 +- .../src/LifecycleComponent.cpp | 13 +- .../component_public_interfaces.hpp | 5 +- .../test/cpp/test_component.cpp | 6 +- .../test/cpp/test_component_interface.cpp | 21 +- .../test_component_interface_parameters.cpp | 7 +- .../test/cpp/test_lifecycle_component.cpp | 6 +- .../test_component_interface_parameters.py | 3 +- .../test/test_modulo_components.cpp | 1 - source/modulo_core/CMakeLists.txt | 121 ++- source/modulo_core/README.md | 61 +- .../examples/cartesianCircular.cpp | 133 ---- .../modulo_core/examples/cartesianLinear.cpp | 139 ---- source/modulo_core/examples/helicoidal.cpp | 151 ---- source/modulo_core/examples/joints.cpp | 127 --- .../examples/movingReferenceFrame.cpp | 188 ----- .../modulo_core/include/modulo_core/Cell.hpp | 753 ------------------ .../include/modulo_core/Component.hpp | 147 ---- .../include/modulo_core}/EncodedState.hpp | 4 +- .../include/modulo_core/Monitor.hpp | 88 -- .../include/modulo_core/Recorder.hpp | 127 --- .../communication/CommunicationHandler.hpp | 93 --- .../communication/MessagePair.hpp | 12 +- .../communication/MessagePairInterface.hpp | 10 +- .../communication/MessageType.hpp | 4 +- .../communication/PublisherHandler.hpp | 8 +- .../communication/PublisherInterface.hpp | 12 +- .../communication/PublisherType.hpp | 4 +- .../communication/SubscriptionHandler.hpp | 8 +- .../communication/SubscriptionInterface.hpp | 10 +- .../message_passing/MessagePassingHandler.hpp | 85 -- .../message_passing/PublisherHandler.hpp | 144 ---- .../message_passing/SubscriptionHandler.hpp | 87 -- .../TransformBroadcasterHandler.hpp | 39 - .../TransformListenerHandler.hpp | 68 -- .../service_client/ClientHandler.hpp | 107 --- .../CommunicationTimeoutException.hpp | 12 - .../IncompatibleParameterException.hpp | 4 +- .../InvalidPointerCastException.hpp | 4 +- .../exceptions/InvalidPointerException.hpp | 4 +- .../exceptions/NotImplementedExcpetion.hpp | 11 - .../exceptions/NullPointerException.hpp | 4 +- .../exceptions/ParameterNotFoundException.hpp | 12 - .../PredicateAlreadyRegisteredException.hpp | 11 - .../exceptions/PredicateNotFoundException.hpp | 11 - .../ServiceNotAvailableException.hpp | 12 - .../exceptions/UnconfiguredNodeException.hpp | 11 - .../translators/message_readers.hpp | 6 +- .../translators/message_writers.hpp | 6 +- .../translators/parameter_translators.hpp | 4 +- .../modulo_core/utilities/utilities.hpp | 72 -- .../launch/example_control_loop.sh | 8 - .../launch/example_moving_reference_frame.sh | 10 - .../modulo_core}/__init__.py | 0 .../modulo_core}/encoded_state.py | 0 .../modulo_core}/translators/__init__.py | 0 .../translators/message_readers.py | 2 +- .../translators/message_writers.py | 2 +- .../translators/parameter_translators.py | 0 source/modulo_core/package.xml | 20 +- .../setup.cfg | 0 source/modulo_core/src/Cell.cpp | 702 ---------------- source/modulo_core/src/Component.cpp | 109 --- source/modulo_core/src/Monitor.cpp | 44 - source/modulo_core/src/Recorder.cpp | 64 -- .../communication/CommunicationHandler.cpp | 5 - .../src/communication/MessagePair.cpp | 6 +- .../communication/MessagePairInterface.cpp | 6 +- .../src/communication/PublisherInterface.cpp | 12 +- .../src/communication/SubscriptionHandler.cpp | 6 +- .../communication/SubscriptionInterface.cpp | 8 +- .../message_passing/MessagePassingHandler.cpp | 11 - .../TransformBroadcasterHandler.cpp | 10 - .../TransformListenerHandler.cpp | 15 - .../src/translators/message_readers.cpp | 6 +- .../src/translators/message_writers.cpp | 6 +- .../src/translators/parameter_translators.cpp | 8 +- .../communication/test_communication.cpp | 13 +- .../communication/test_message_pair.cpp | 16 +- .../communication/test_publisher_handler.cpp | 24 +- .../test_subscription_handler.cpp | 12 +- .../test_parameter_eigen_translators.cpp | 4 +- .../test_parameter_state_translators.cpp | 27 +- .../parameters/test_parameter_translators.cpp | 4 +- .../cpp_tests/translators/test_messages.cpp | 10 +- .../test/python_tests/conftest.py | 0 .../python_tests/translators/test_messages.py | 6 +- .../translators/test_parameters.py | 2 +- .../test/test_modulo_new_core.cpp | 0 source/modulo_new_core/CMakeLists.txt | 66 -- source/modulo_new_core/README.md | 1 - source/modulo_new_core/package.xml | 26 - 99 files changed, 275 insertions(+), 4085 deletions(-) delete mode 100644 source/modulo_core/examples/cartesianCircular.cpp delete mode 100644 source/modulo_core/examples/cartesianLinear.cpp delete mode 100644 source/modulo_core/examples/helicoidal.cpp delete mode 100644 source/modulo_core/examples/joints.cpp delete mode 100644 source/modulo_core/examples/movingReferenceFrame.cpp delete mode 100644 source/modulo_core/include/modulo_core/Cell.hpp delete mode 100644 source/modulo_core/include/modulo_core/Component.hpp rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/EncodedState.hpp (78%) delete mode 100644 source/modulo_core/include/modulo_core/Monitor.hpp delete mode 100644 source/modulo_core/include/modulo_core/Recorder.hpp delete mode 100644 source/modulo_core/include/modulo_core/communication/CommunicationHandler.hpp rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/MessagePair.hpp (93%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/MessagePairInterface.hpp (94%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/MessageType.hpp (71%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/PublisherHandler.hpp (93%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/PublisherInterface.hpp (94%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/PublisherType.hpp (68%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/SubscriptionHandler.hpp (92%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/communication/SubscriptionInterface.hpp (93%) delete mode 100644 source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp delete mode 100644 source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp delete mode 100644 source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp delete mode 100644 source/modulo_core/include/modulo_core/communication/message_passing/TransformBroadcasterHandler.hpp delete mode 100644 source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp delete mode 100644 source/modulo_core/include/modulo_core/communication/service_client/ClientHandler.hpp delete mode 100644 source/modulo_core/include/modulo_core/exceptions/CommunicationTimeoutException.hpp rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/exceptions/IncompatibleParameterException.hpp (89%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/exceptions/InvalidPointerCastException.hpp (85%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/exceptions/InvalidPointerException.hpp (84%) delete mode 100644 source/modulo_core/include/modulo_core/exceptions/NotImplementedExcpetion.hpp rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/exceptions/NullPointerException.hpp (84%) delete mode 100644 source/modulo_core/include/modulo_core/exceptions/ParameterNotFoundException.hpp delete mode 100644 source/modulo_core/include/modulo_core/exceptions/PredicateAlreadyRegisteredException.hpp delete mode 100644 source/modulo_core/include/modulo_core/exceptions/PredicateNotFoundException.hpp delete mode 100644 source/modulo_core/include/modulo_core/exceptions/ServiceNotAvailableException.hpp delete mode 100644 source/modulo_core/include/modulo_core/exceptions/UnconfiguredNodeException.hpp rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/translators/message_readers.hpp (99%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/translators/message_writers.hpp (98%) rename source/{modulo_new_core/include/modulo_new_core => modulo_core/include/modulo_core}/translators/parameter_translators.hpp (97%) delete mode 100644 source/modulo_core/include/modulo_core/utilities/utilities.hpp delete mode 100755 source/modulo_core/launch/example_control_loop.sh delete mode 100644 source/modulo_core/launch/example_moving_reference_frame.sh rename source/{modulo_new_core/modulo_new_core => modulo_core/modulo_core}/__init__.py (100%) rename source/{modulo_new_core/modulo_new_core => modulo_core/modulo_core}/encoded_state.py (100%) rename source/{modulo_new_core/modulo_new_core => modulo_core/modulo_core}/translators/__init__.py (100%) rename source/{modulo_new_core/modulo_new_core => modulo_core/modulo_core}/translators/message_readers.py (98%) rename source/{modulo_new_core/modulo_new_core => modulo_core/modulo_core}/translators/message_writers.py (98%) rename source/{modulo_new_core/modulo_new_core => modulo_core/modulo_core}/translators/parameter_translators.py (100%) rename source/{modulo_new_core => modulo_core}/setup.cfg (100%) delete mode 100644 source/modulo_core/src/Cell.cpp delete mode 100644 source/modulo_core/src/Component.cpp delete mode 100644 source/modulo_core/src/Monitor.cpp delete mode 100644 source/modulo_core/src/Recorder.cpp delete mode 100644 source/modulo_core/src/communication/CommunicationHandler.cpp rename source/{modulo_new_core => modulo_core}/src/communication/MessagePair.cpp (91%) rename source/{modulo_new_core => modulo_core}/src/communication/MessagePairInterface.cpp (50%) rename source/{modulo_new_core => modulo_core}/src/communication/PublisherInterface.cpp (94%) rename source/{modulo_new_core => modulo_core}/src/communication/SubscriptionHandler.cpp (95%) rename source/{modulo_new_core => modulo_core}/src/communication/SubscriptionInterface.cpp (70%) delete mode 100644 source/modulo_core/src/communication/message_passing/MessagePassingHandler.cpp delete mode 100644 source/modulo_core/src/communication/message_passing/TransformBroadcasterHandler.cpp delete mode 100644 source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp rename source/{modulo_new_core => modulo_core}/src/translators/message_readers.cpp (96%) rename source/{modulo_new_core => modulo_core}/src/translators/message_writers.cpp (98%) rename source/{modulo_new_core => modulo_core}/src/translators/parameter_translators.cpp (98%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/communication/test_communication.cpp (90%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/communication/test_message_pair.cpp (81%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/communication/test_publisher_handler.cpp (72%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/communication/test_subscription_handler.cpp (82%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp (96%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp (81%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/translators/parameters/test_parameter_translators.cpp (97%) rename source/{modulo_new_core => modulo_core}/test/cpp_tests/translators/test_messages.cpp (97%) rename source/{modulo_new_core => modulo_core}/test/python_tests/conftest.py (100%) rename source/{modulo_new_core => modulo_core}/test/python_tests/translators/test_messages.py (97%) rename source/{modulo_new_core => modulo_core}/test/python_tests/translators/test_parameters.py (98%) rename source/{modulo_new_core => modulo_core}/test/test_modulo_new_core.cpp (100%) delete mode 100644 source/modulo_new_core/CMakeLists.txt delete mode 100644 source/modulo_new_core/README.md delete mode 100644 source/modulo_new_core/package.xml diff --git a/.github/actions/build-test-galactic/entrypoint.sh b/.github/actions/build-test-galactic/entrypoint.sh index 1b162b174..46a7818f5 100755 --- a/.github/actions/build-test-galactic/entrypoint.sh +++ b/.github/actions/build-test-galactic/entrypoint.sh @@ -26,7 +26,6 @@ build_and_test() { source /opt/ros/"${ROS_DISTRO}"/setup.bash cd /home/ros2/ros2_ws -build_and_test modulo_new_core build_and_test modulo_core build_and_test modulo_components diff --git a/Dockerfile b/Dockerfile index d6924edf7..c15099a85 100644 --- a/Dockerfile +++ b/Dockerfile @@ -6,7 +6,6 @@ WORKDIR ${HOME}/ros2_ws FROM dependencies as modulo-core COPY --chown=${USER} ./source/modulo_core ./src/modulo_core -COPY --chown=${USER} ./source/modulo_new_core ./src/modulo_new_core RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build" diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 93d59873e..80b689f7b 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -6,7 +6,7 @@ #include "modulo_components/ComponentInterface.hpp" #include "modulo_components/utilities/utilities.hpp" -#include "modulo_new_core/EncodedState.hpp" +#include "modulo_core/EncodedState.hpp" namespace modulo_components { @@ -89,7 +89,7 @@ inline void Component::add_output( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, const std::string& default_topic ) { - using namespace modulo_new_core::communication; + using namespace modulo_core::communication; try { auto parsed_signal_name = this->create_output(signal_name, data, fixed_topic, default_topic); auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); @@ -139,10 +139,9 @@ inline void Component::add_output( break; } case MessageType::ENCODED_STATE: { - auto publisher = this->create_publisher(topic_name, this->qos_); + auto publisher = this->create_publisher(topic_name, this->qos_); this->outputs_.at(parsed_signal_name) = - std::make_shared, - modulo_new_core::EncodedState>>( + std::make_shared, modulo_core::EncodedState>>( PublisherType::PUBLISHER, publisher )->create_publisher_interface(message_pair); break; diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 150145d20..afbef8966 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -15,13 +15,13 @@ #include #include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include #include "modulo_components/exceptions/AddSignalException.hpp" #include "modulo_components/exceptions/ComponentParameterException.hpp" @@ -55,7 +55,7 @@ class ComponentInterface : public NodeT { * @param fallback_name The name of the component if it was not provided through the node options */ explicit ComponentInterface( - const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type, + const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type, const std::string& fallback_name = "ComponentInterface" ); @@ -183,8 +183,8 @@ class ComponentInterface : public NodeT { * @param fixed_topic If true, the topic name of the input signal is fixed * @param default_topic If set, the default value for the topic name to use */ - // TODO could be nice to add an optional callback here that would be executed from within the subscription callback - // in order to manipulate the data pointer upon reception of a message + // TODO could be nice to add an optional callback here that would be executed from within the subscription callback + // in order to manipulate the data pointer upon reception of a message template void add_input( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, @@ -291,10 +291,8 @@ class ComponentInterface : public NodeT { */ virtual void raise_error(); - std::map> - inputs_; ///< Map of inputs - std::map> - outputs_; ///< Map of outputs + std::map> inputs_; ///< Map of inputs + std::map> outputs_; ///< Map of outputs rclcpp::QoS qos_ = rclcpp::QoS(10); ///< Quality of Service for ROS publishers and subscribers @@ -320,7 +318,7 @@ class ComponentInterface : public NodeT { */ void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); - modulo_new_core::communication::PublisherType + modulo_core::communication::PublisherType publisher_type_; ///< Type of the output publishers (one of PUBLISHER, LIFECYCLE_PUBLISHER) std::map predicates_; ///< Map of predicates @@ -342,7 +340,7 @@ class ComponentInterface : public NodeT { template ComponentInterface::ComponentInterface( - const rclcpp::NodeOptions& options, modulo_new_core::communication::PublisherType publisher_type, + const rclcpp::NodeOptions& options, modulo_core::communication::PublisherType publisher_type, const std::string& fallback_name ) : NodeT(utilities::parse_node_name(options, fallback_name), options), publisher_type_(publisher_type) { @@ -394,7 +392,7 @@ inline void ComponentInterface::add_parameter( bool read_only ) { try { - auto ros_param = modulo_new_core::translators::write_parameter(parameter); + auto ros_param = modulo_core::translators::write_parameter(parameter); if (!NodeT::has_parameter(parameter->get_name())) { RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); parameter_map_.set_parameter(parameter); @@ -427,7 +425,7 @@ template inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { try { rcl_interfaces::msg::SetParametersResult result = NodeT::set_parameter( - modulo_new_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); + modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))); if (!result.successful) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Failed to set parameter value of parameter '" << name << "': " << result.reason); @@ -459,13 +457,13 @@ ComponentInterface::on_set_parameters_callback(const std::vector::add_input( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, const std::string& default_topic ) { - using namespace modulo_new_core::communication; + using namespace modulo_core::communication; try { std::string parsed_signal_name = utilities::parse_signal_name(signal_name); if (parsed_signal_name.empty()) { @@ -625,8 +623,8 @@ inline void ComponentInterface::add_input( break; } case MessageType::ENCODED_STATE: { - auto subscription_handler = std::make_shared>(message_pair); - auto subscription = NodeT::template create_subscription( + auto subscription_handler = std::make_shared>(message_pair); + auto subscription = NodeT::template create_subscription( topic_name, this->qos_, subscription_handler->get_callback()); subscription_interface = subscription_handler->create_subscription_interface(subscription); break; @@ -644,7 +642,7 @@ inline void ComponentInterface::add_input( const std::string& signal_name, const std::function)>& callback, bool fixed_topic, const std::string& default_topic ) { - using namespace modulo_new_core::communication; + using namespace modulo_core::communication; try { std::string parsed_signal_name = utilities::parse_signal_name(signal_name); if (parsed_signal_name.empty()) { @@ -719,7 +717,7 @@ inline void ComponentInterface::send_transform(const state_representation } try { geometry_msgs::msg::TransformStamped transform_message; - modulo_new_core::translators::write_message(transform_message, transform, this->get_clock()->now()); + modulo_core::translators::write_message(transform_message, transform, this->get_clock()->now()); this->tf_broadcaster_->sendTransform(transform_message); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, @@ -738,7 +736,7 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra try { state_representation::CartesianPose result(frame_name, reference_frame_name); auto transform = this->tf_buffer_->lookupTransform(reference_frame_name, frame_name, time_point, duration); - modulo_new_core::translators::read_message(result, transform); + modulo_core::translators::read_message(result, transform); return result; } catch (const tf2::TransformException& ex) { throw exceptions::LookupTransformException(std::string("Failed to lookup transform: ").append(ex.what())); @@ -789,7 +787,7 @@ inline std::string ComponentInterface::create_output( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, const std::string& default_topic ) { - using namespace modulo_new_core::communication; + using namespace modulo_core::communication; try { auto parsed_signal_name = utilities::parse_signal_name(signal_name); if (parsed_signal_name.empty()) { diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 058c76f92..fc27f9a0e 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -3,7 +3,7 @@ #include #include "modulo_components/ComponentInterface.hpp" -#include "modulo_new_core/EncodedState.hpp" +#include "modulo_core/EncodedState.hpp" namespace modulo_components { diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 73ced35ea..07067c922 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -3,30 +3,25 @@ from typing import Callable, Dict, List, Optional, TypeVar, Union import clproto -import modulo_new_core.translators.message_readers as modulo_readers -import modulo_new_core.translators.message_writers as modulo_writers -import rclpy +import modulo_core.translators.message_readers as modulo_readers +import modulo_core.translators.message_writers as modulo_writers import state_representation as sr -import tf2_py from geometry_msgs.msg import TransformStamped from modulo_components.exceptions.component_exceptions import AddSignalError, ComponentParameterError, \ LookupTransformError from modulo_components.utilities.utilities import generate_predicate_topic, parse_signal_name -from modulo_new_core.encoded_state import EncodedState -from modulo_new_core.translators.message_readers import read_stamped_message -from modulo_new_core.translators.message_writers import write_stamped_message -from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter_const -from rcl_interfaces.msg import ParameterDescriptor -from rcl_interfaces.msg import SetParametersResult +from modulo_core.encoded_state import EncodedState +from modulo_core.translators.parameter_translators import write_parameter, read_parameter_const +from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult from rclpy.duration import Duration from rclpy.node import Node +from rclpy.parameter import Parameter from rclpy.publisher import Publisher from rclpy.qos import QoSProfile from rclpy.time import Time from std_msgs.msg import Bool, Int32, Float64, Float64MultiArray, String -from tf2_ros import TransformBroadcaster -from tf2_ros.buffer import Buffer -from tf2_ros.transform_listener import TransformListener +from tf2_py import TransformException +from tf2_ros import Buffer, TransformBroadcaster, TransformListener MsgT = TypeVar('MsgT') T = TypeVar('T') @@ -125,7 +120,7 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r except Exception as e: self.get_logger().error(f"Failed to add parameter: {e}") - def get_parameter(self, name: str) -> Union[sr.Parameter, rclpy.parameter.Parameter]: + def get_parameter(self, name: str) -> Union[sr.Parameter, Parameter]: """ Get a parameter by name. If this method is called from a file that contains 'rclpy' in its path, the rclpy.node.Node.get_parameter method will be invoked, otherwise return the parameter from the local parameter @@ -139,7 +134,7 @@ def get_parameter(self, name: str) -> Union[sr.Parameter, rclpy.parameter.Parame co_filename = sys._getframe().f_back.f_code.co_filename self.get_logger().debug(f"get_parameter called from {co_filename}") if "rclpy" in co_filename: - return rclpy.node.Node.get_parameter(self, name) + return Node.get_parameter(self, name) else: return self._get_component_parameter(name) except Exception as e: @@ -204,7 +199,7 @@ def _validate_parameter(self, parameter: sr.Parameter) -> bool: """ return True - def __on_set_parameters_callback(self, ros_parameters: List[rclpy.Parameter]) -> SetParametersResult: + def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetParametersResult: """ Callback function to validate and update parameters on change. @@ -421,7 +416,7 @@ def send_transform(self, transform: sr.CartesianPose): transform_message = TransformStamped() modulo_writers.write_stamped_message(transform_message, transform, self.get_clock().now()) self.__tf_broadcaster.sendTransform(transform_message) - except tf2_py.TransformException as e: + except TransformException as e: self.get_logger().error(f"Failed to send transform: {e}", throttle_duration_sec=1.0) def lookup_transform(self, frame_name: str, reference_frame_name="world", time_point=Time(), @@ -442,7 +437,7 @@ def lookup_transform(self, frame_name: str, reference_frame_name="world", time_p transform = self.__tf_buffer.lookup_transform(reference_frame_name, frame_name, time_point, duration) modulo_readers.read_stamped_message(result, transform) return result - except tf2_py.TransformException as e: + except TransformException as e: raise LookupTransformError(f"Failed to lookup transform: {e}") def get_qos(self) -> QoSProfile: diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index da223ef97..f38b9a336 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -4,22 +4,17 @@ modulo_components 0.0.1 TODO - TODO - TODO - TODO + Baptiste Busch + Enrico Eberhard + Dominic Reber TODO: License declaration ament_cmake_auto ament_cmake_python - rclcpp - rclpy - rclcpp_components - rcutils modulo_core - modulo_new_core - - launch_ros + rclcpp_lifecycle + tf2_ros ament_lint_auto ament_lint_common diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index c609ef978..2e6f12d31 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -1,6 +1,6 @@ #include "modulo_components/Component.hpp" -using namespace modulo_new_core::communication; +using namespace modulo_core::communication; namespace modulo_components { diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index dfebb14a5..d7cbd732f 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -2,7 +2,7 @@ #include -using namespace modulo_new_core::communication; +using namespace modulo_core::communication; namespace modulo_components { @@ -278,12 +278,11 @@ bool LifecycleComponent::configure_outputs() { break; } case MessageType::ENCODED_STATE: { - auto publisher = this->create_publisher(topic_name, this->qos_); - interface = - std::make_shared, - modulo_new_core::EncodedState>>( - PublisherType::LIFECYCLE_PUBLISHER, publisher - )->create_publisher_interface(message_pair); + auto publisher = this->create_publisher(topic_name, this->qos_); + interface = std::make_shared, + modulo_core::EncodedState>>( + PublisherType::LIFECYCLE_PUBLISHER, publisher + )->create_publisher_interface(message_pair); break; } } diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index b89b8b62d..6ef8678d4 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -12,7 +12,7 @@ template class ComponentInterfacePublicInterface : public ComponentInterface { public: explicit ComponentInterfacePublicInterface( - const rclcpp::NodeOptions& node_options, modulo_new_core::communication::PublisherType publisher_type + const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type ) : ComponentInterface(node_options, publisher_type) {} using ComponentInterface::add_parameter; using ComponentInterface::get_parameter; @@ -66,7 +66,8 @@ class ComponentPublicInterface : public Component { class LifecycleComponentPublicInterface : public LifecycleComponent { public: - explicit LifecycleComponentPublicInterface(const rclcpp::NodeOptions& node_options) : LifecycleComponent(node_options) {} + explicit LifecycleComponentPublicInterface(const rclcpp::NodeOptions& node_options) : + LifecycleComponent(node_options) {} using LifecycleComponent::add_output; using LifecycleComponent::configure_outputs; using LifecycleComponent::activate_outputs; diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp index 261cec550..98d1a3d9c 100644 --- a/source/modulo_components/test/cpp/test_component.cpp +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -2,9 +2,7 @@ #include -#include "modulo_components/Component.hpp" -#include "modulo_new_core/EncodedState.hpp" - +#include "modulo_core/EncodedState.hpp" #include "test_modulo_components/component_public_interfaces.hpp" using namespace state_representation; @@ -37,6 +35,6 @@ TEST_F(ComponentTest, AddOutput) { auto new_data = std::make_shared(false); component_->add_output("test_13", new_data, true); EXPECT_EQ(component_->outputs_.at("test_13")->get_message_pair()->get_type(), - modulo_new_core::communication::MessageType::ENCODED_STATE); + modulo_core::communication::MessageType::ENCODED_STATE); } } // namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index f95822324..90d0d1868 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -1,8 +1,6 @@ #include -#include "modulo_components/ComponentInterface.hpp" -#include "modulo_new_core/EncodedState.hpp" - +#include "modulo_core/EncodedState.hpp" #include "test_modulo_components/component_public_interfaces.hpp" namespace modulo_components { @@ -21,11 +19,11 @@ class ComponentInterfaceTest : public ::testing::Test { void SetUp() override { if (std::is_same::value) { this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::PUBLISHER + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER ); } else if (std::is_same::value) { this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER ); } } @@ -98,7 +96,7 @@ TYPED_TEST(ComponentInterfaceTest, AddInput) { "test_13", [](const std::shared_ptr) {} ); EXPECT_EQ(this->component_->inputs_.at("test_13")->get_message_pair()->get_type(), - modulo_new_core::communication::MessageType::BOOL); + modulo_core::communication::MessageType::BOOL); } TYPED_TEST(ComponentInterfaceTest, CreateOutput) { @@ -109,12 +107,12 @@ TYPED_TEST(ComponentInterfaceTest, CreateOutput) { auto pub_interface = this->component_->outputs_.at("test"); if (typeid(this->node_) == typeid(rclcpp::Node)) { - EXPECT_EQ(pub_interface->get_type(), modulo_new_core::communication::PublisherType::PUBLISHER); + EXPECT_EQ(pub_interface->get_type(), modulo_core::communication::PublisherType::PUBLISHER); } else if (typeid(this->node_) == typeid(rclcpp_lifecycle::LifecycleNode)) { - EXPECT_EQ(pub_interface->get_type(), modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER); + EXPECT_EQ(pub_interface->get_type(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER); } - EXPECT_EQ(pub_interface->get_message_pair()->get_type(), modulo_new_core::communication::MessageType::BOOL); - EXPECT_THROW(pub_interface->publish(), modulo_new_core::exceptions::InvalidPointerCastException); + EXPECT_EQ(pub_interface->get_message_pair()->get_type(), modulo_core::communication::MessageType::BOOL); + EXPECT_THROW(pub_interface->publish(), modulo_core::exceptions::InvalidPointerCastException); } TYPED_TEST(ComponentInterfaceTest, TF) { @@ -122,7 +120,8 @@ TYPED_TEST(ComponentInterfaceTest, TF) { this->component_->add_tf_listener(); auto send_tf = state_representation::CartesianPose::Random("test", "world"); EXPECT_NO_THROW(this->component_->send_transform(send_tf)); - EXPECT_THROW(auto throw_tf = this->component_->lookup_transform("dummy", "world"), exceptions::LookupTransformException); + EXPECT_THROW(auto throw_tf = this->component_->lookup_transform("dummy", "world"), + exceptions::LookupTransformException); auto lookup_tf = this->component_->lookup_transform("test", "world"); auto identity = send_tf * lookup_tf.inverse(); EXPECT_FLOAT_EQ(identity.data().norm(), 1.); diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index 16f8e53f5..941898e5c 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -1,8 +1,7 @@ #include #include "modulo_components/exceptions/ComponentParameterException.hpp" -#include "modulo_new_core/EncodedState.hpp" - +#include "modulo_core/EncodedState.hpp" #include "test_modulo_components/component_public_interfaces.hpp" namespace modulo_components { @@ -21,11 +20,11 @@ class ComponentInterfaceParameterTest : public ::testing::Test { void SetUp() override { if (std::is_same::value) { this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::PUBLISHER + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER ); } else if (std::is_same::value) { this->component_ = std::make_shared>( - rclcpp::NodeOptions(), modulo_new_core::communication::PublisherType::LIFECYCLE_PUBLISHER + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER ); } param_ = state_representation::make_shared_parameter("test", 1); diff --git a/source/modulo_components/test/cpp/test_lifecycle_component.cpp b/source/modulo_components/test/cpp/test_lifecycle_component.cpp index b109d8cc7..b7147dea1 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component.cpp @@ -2,9 +2,7 @@ #include -#include "modulo_components/LifecycleComponent.hpp" -#include "modulo_new_core/EncodedState.hpp" - +#include "modulo_core/EncodedState.hpp" #include "test_modulo_components/component_public_interfaces.hpp" using namespace state_representation; @@ -40,6 +38,6 @@ TEST_F(LifecycleComponentTest, AddOutput) { auto new_data = std::make_shared(false); component_->add_output("test", new_data, true); EXPECT_EQ(component_->outputs_.at("test")->get_message_pair()->get_type(), - modulo_new_core::communication::MessageType::ENCODED_STATE); + modulo_core::communication::MessageType::ENCODED_STATE); } } // namespace modulo_components diff --git a/source/modulo_components/test/python/test_component_interface_parameters.py b/source/modulo_components/test/python/test_component_interface_parameters.py index e7b68c266..02e7cc667 100644 --- a/source/modulo_components/test/python/test_component_interface_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_parameters.py @@ -3,9 +3,8 @@ import rclpy.node import state_representation as sr from modulo_components.component_interface import ComponentInterface -from rcl_interfaces.msg import SetParametersResult - from modulo_components.exceptions.component_exceptions import ComponentParameterError +from rcl_interfaces.msg import SetParametersResult class ComponentInterfaceTest(ComponentInterface): diff --git a/source/modulo_components/test/test_modulo_components.cpp b/source/modulo_components/test/test_modulo_components.cpp index e7bb1d13c..697a9d70c 100644 --- a/source/modulo_components/test/test_modulo_components.cpp +++ b/source/modulo_components/test/test_modulo_components.cpp @@ -1,6 +1,5 @@ #include - int main(int argc, char** argv) { ::testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index ccd364b91..10c0d9342 100644 --- a/source/modulo_core/CMakeLists.txt +++ b/source/modulo_core/CMakeLists.txt @@ -1,7 +1,12 @@ -cmake_minimum_required(VERSION 3.9) -project(modulo_core VERSION 1.1.0) +cmake_minimum_required(VERSION 3.15) +project(modulo_core) -# Default to C++17 +# default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + +# default to C++17 if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 17) endif() @@ -12,76 +17,50 @@ endif() # find dependencies find_package(ament_cmake_auto REQUIRED) +find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -list(APPEND CMAKE_PREFIX_PATH /opt/openrobots) -find_package(pinocchio REQUIRED) -find_package(OsqpEigen REQUIRED) -find_package(osqp REQUIRED) - -find_library(protobuf REQUIRED) -find_library(clproto REQUIRED) -find_library(state_representation REQUIRED) -find_library(dynamical_systems REQUIRED) -find_library(controllers REQUIRED) -find_library(robot_model REQUIRED) - -set(CORE_SOURCES - src/communication/message_passing/TransformListenerHandler.cpp - src/communication/message_passing/TransformBroadcasterHandler.cpp - src/communication/message_passing/MessagePassingHandler.cpp - src/communication/CommunicationHandler.cpp - src/Cell.cpp - src/Component.cpp - src/Monitor.cpp - src/Recorder.cpp -) - -include_directories( - include - ${STATE_REPRESENTATION_INCLUDE_DIR} - ${DYNAMICAL_SYSTEMS_INCLUDE_DIR} - ${CONTROLLERS_INCLUDE_DIR} - ${ROBOT_MODEL_INCLUDE_DIR} - ${PINOCCHIO_INCLUDE_DIR} - ${OsqpEigen_INCLUDE_DIR} - /opt/openrobots/include - ${CLPROTO_INCLUDE_DIR} -) - -ament_auto_add_library(${PROJECT_NAME} SHARED ${CORE_SOURCES}) -target_link_libraries(${PROJECT_NAME} - ${PINOCCHIO_LIBRARIES} - OsqpEigen::OsqpEigen - osqp::osqp - clproto - protobuf - state_representation - dynamical_systems - controllers - robot_model -) - -ament_auto_add_executable(example_cartesian_linear examples/cartesianLinear.cpp) -target_link_libraries(example_cartesian_linear ${PROJECT_NAME}) - -ament_auto_add_executable(example_cartesian_circular examples/cartesianCircular.cpp) -target_link_libraries(example_cartesian_circular ${PROJECT_NAME}) - -ament_auto_add_executable(example_helicoidal examples/helicoidal.cpp) -target_link_libraries(example_helicoidal ${PROJECT_NAME}) - -ament_auto_add_executable(example_joints examples/joints.cpp) -target_link_libraries(example_joints ${PROJECT_NAME}) - -ament_auto_add_executable(example_moving_reference_frame examples/movingReferenceFrame.cpp) -target_link_libraries(example_moving_reference_frame ${PROJECT_NAME}) +find_package(control_libraries 6.0.2 REQUIRED COMPONENTS state_representation) +find_package(clproto 6.0.0 REQUIRED) -ament_auto_package() +include_directories(include) + +ament_auto_add_library(modulo_core SHARED + ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp + ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp + ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp + ${PROJECT_SOURCE_DIR}/src/communication/SubscriptionHandler.cpp + ${PROJECT_SOURCE_DIR}/src/communication/SubscriptionInterface.cpp + ${PROJECT_SOURCE_DIR}/src/translators/parameter_translators.cpp + ${PROJECT_SOURCE_DIR}/src/translators/message_readers.cpp + ${PROJECT_SOURCE_DIR}/src/translators/message_writers.cpp) + +# install Python modules +ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + + # run cpp tests + file(GLOB_RECURSE TEST_CPP_SOURCES ${PROJECT_SOURCE_DIR}/test/test_*.cpp) -# Install launch files. -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME}/ -) + ament_add_gtest(test_modulo_core ${TEST_CPP_SOURCES}) + target_include_directories(test_modulo_core PRIVATE include) + target_link_libraries(test_modulo_core modulo_core state_representation clproto) + + # prevent pluginlib from using boost + target_compile_definitions(test_modulo_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + + # run python tests + file(GLOB_RECURSE TEST_PYTHON_SOURCES ${PROJECT_SOURCE_DIR}/test/test_*.py) + + foreach(TEST_PYTHON IN LISTS TEST_PYTHON_SOURCES) + get_filename_component(TEST_FILENAME ${TEST_PYTHON} NAME_WE) + ament_add_pytest_test(${TEST_FILENAME} ${TEST_PYTHON}) + endforeach() + +endif() + +ament_auto_package() diff --git a/source/modulo_core/README.md b/source/modulo_core/README.md index 2351d5107..31183b9a5 100644 --- a/source/modulo_core/README.md +++ b/source/modulo_core/README.md @@ -1,60 +1 @@ -# Modulo - -This is a ROS2 package that implements the communication protocol between the modules. Each module is able to send/receive states as they are implemented in the [state_representation](../../lib/state_representation) library, and also communicate with ros2 tf2 server to send/lookup tf transforms. To implement your own modules you have to inherit a new class from the `Modulo::Core::Cell`{:.cpp} base class - -```cpp -class MyRobotInterface : public Modulo::Core::Cell -``` - -A module usually contains a set of publisher and subscriber. For example, to add a publisher to our `MyRobotInterface`{.cpp} class, that will publish the state of the robot, we do: - -```cpp -class MyRobotInterface : public Modulo::Core::Cell -{ -public: - std::shared_ptr current_state; - - explicit MyRobotInterface(const std::string & node_name, const std::chrono::milliseconds & period) : - Cell(node_name, period), - current_state(std::make_shared("my_robot")) - { - // a publisher contains a channel name and a shared_ptr to the state to publish - // shared_ptr are used to enable asynchronous communication between the modules - this->add_publisher("/robot/joint_state", this->current_state); - } -} - - -``` - -As communication is asynchronous, the `JointState`{.cpp} will be published periodically at a frequency defined in the node under the `period` argument or at the `period` defined in the argument of the publisher. If no modifications has been made to the state it is considered as empty and will not be published. By default, such a timeout occurs after two `period`. You can change this behavior by adding an argument to the pubisher: - -The core structure of the module should be implemented in the `step` function: - -```cpp -class MyRobotInterface : public ModuloCore::RobotInterface -{ -public: - std::shared_ptr current_state; - - explicit MyRobotInterface(const std::string & node_name, const std::chrono::milliseconds & period) : - Cell(node_name, period), - current_state(std::make_shared("my_robot")) - { - // a publisher contains a channel name and a shared_ptr to the state to publish - // shared_ptr are used to enable asynchronous communication between the modules - this->add_publisher("/robot/joint_state", this->current_state); - } - - void step() - { - // read your robot state and store it in the current position - // this is an example where you should define the read state function on your own - this->current_position = {...} - } -} -``` - -No need to do anything else. The step function is called periodically at a frequency defined in the node under the `period` argument. And the `current_state` variable will be published over `robot/joint_state` channel on ros2 network automatically. - -We recommend keeping this step function short, especially if you use a small period. Otherwise, latency will occur in your control loop. Complete examples of control loops including multiple nodes are written in the [tests](examples/) for each representation spaces. +# aica_msgs \ No newline at end of file diff --git a/source/modulo_core/examples/cartesianCircular.cpp b/source/modulo_core/examples/cartesianCircular.cpp deleted file mode 100644 index 0641e81d6..000000000 --- a/source/modulo_core/examples/cartesianCircular.cpp +++ /dev/null @@ -1,133 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "modulo_core/Cell.hpp" - -using namespace state_representation; -using namespace dynamical_systems; - -namespace { -class MotionGenerator : public modulo::core::Cell { -private: - std::shared_ptr current_pose; - std::shared_ptr desired_twist; - std::shared_ptr> motion_generator; - -public: - explicit MotionGenerator(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - current_pose(std::make_shared("robot_test")), - desired_twist(std::make_shared("robot_test")), - motion_generator(CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::CIRCULAR)) { - motion_generator->set_parameter_value("limit_cycle", Ellipsoid("attractor")); - } - - bool on_configure() { - this->add_subscription("/robot_test/pose", this->current_pose); - this->add_publisher("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - if (!this->current_pose->is_empty()) { - *this->desired_twist = this->motion_generator->evaluate(*this->current_pose); - } else { - this->desired_twist->initialize(); - } - } -}; - -class ConsoleVisualizer : public modulo::core::Cell { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - -public: - explicit ConsoleVisualizer(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_pose(std::make_shared("robot_test")), - desired_twist(std::make_shared("robot_test")) {} - - bool on_configure() { - this->add_subscription("/robot_test/pose", this->robot_pose); - this->add_subscription("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - std::ostringstream os; - os << std::endl; - os << "##### ROBOT POSE #####" << std::endl; - os << *this->robot_pose << std::endl; - os << "##### DESIRED TWIST #####" << std::endl; - os << *this->desired_twist << std::endl; - RCLCPP_INFO(get_logger(), "%s", os.str().c_str()); - } -}; - -class SimulatedRobotInterface : public modulo::core::Cell { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - std::chrono::milliseconds dt; - -public: - explicit SimulatedRobotInterface(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_pose( - std::make_shared( - "robot_test", Eigen::Vector3d::Random(), Eigen::Quaterniond::UnitRandom())), - desired_twist(std::make_shared("robot_test")), - dt(period) {} - - bool on_configure() { - this->add_subscription("/ds/desired_twist", this->desired_twist); - this->add_publisher("/robot_test/pose", this->robot_pose); - return true; - } - - void step() { - if (!this->desired_twist->is_empty()) { - *this->robot_pose = dt * *this->desired_twist + *this->robot_pose; - } - this->send_transform(*this->robot_pose); - } -}; -}// namespace - -/** - * A lifecycle node has the same node API - * as a regular node. This means we can spawn a - * node, give it a name and add it to the executor. - */ -int main(int argc, char* argv[]) { - // force flush of the stdout buffer. - // this ensures a correct sync of all prints - // even when executed simultaneously within the launch file. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor exe; - - std::shared_ptr lmg = std::make_shared("motion_generator", 1ms); - std::shared_ptr cv = std::make_shared("visualizer", 100ms); - std::shared_ptr sri = std::make_shared("robot_interface", 1ms); - - exe.add_node(lmg->get_node_base_interface()); - exe.add_node(cv->get_node_base_interface()); - exe.add_node(sri->get_node_base_interface()); - - exe.spin(); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/source/modulo_core/examples/cartesianLinear.cpp b/source/modulo_core/examples/cartesianLinear.cpp deleted file mode 100644 index 07d7c8a29..000000000 --- a/source/modulo_core/examples/cartesianLinear.cpp +++ /dev/null @@ -1,139 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include - -#include "modulo_core/Cell.hpp" -#include "modulo_core/Component.hpp" - -using namespace state_representation; -using namespace dynamical_systems; - -namespace { -class LinearMotionGenerator : public modulo::core::Cell { -private: - std::shared_ptr current_pose; - std::shared_ptr desired_twist; - std::shared_ptr> motion_generator; - -public: - explicit LinearMotionGenerator(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - current_pose(std::make_shared("robot_test")), - desired_twist(std::make_shared("robot_test")), - motion_generator( - CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)) { - this->motion_generator->set_parameter_value("attractor", CartesianPose::Random("robot_test")); - } - - bool on_configure() { - this->add_state_subscription("/robot_test/pose", this->current_pose); - this->add_state_publisher("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - if (!this->current_pose->is_empty()) { - *this->desired_twist = this->motion_generator->evaluate(*this->current_pose); - // change attractor if previous was reached - if (this->current_pose->dist(this->motion_generator->get_parameter_value("attractor")) < 1e-3) { - this->motion_generator->set_parameter_value("attractor", CartesianPose::Random("robot_test")); - } - } else { - this->desired_twist->initialize(); - } - this->send_transform(this->motion_generator->get_parameter_value("attractor"), "attractor"); - } -}; - -class ConsoleVisualizer : public modulo::core::Component { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - -public: - explicit ConsoleVisualizer(const std::string& node_name, const std::chrono::milliseconds& period) : - Component(node_name, period), - robot_pose(std::make_shared("robot_test")), - desired_twist(std::make_shared("robot_test")) {} - - bool on_configure() { - this->add_state_subscription("/robot_test/pose", this->robot_pose); - this->add_state_subscription("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - std::ostringstream os; - os << std::endl; - os << "##### ROBOT POSE #####" << std::endl; - os << *this->robot_pose << std::endl; - os << "##### DESIRED TWIST #####" << std::endl; - os << *this->desired_twist << std::endl; - RCLCPP_INFO(get_logger(), "%s", os.str().c_str()); - } -}; - -class SimulatedRobotInterface : public modulo::core::Cell { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - std::chrono::milliseconds dt; - -public: - explicit SimulatedRobotInterface(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_pose( - std::make_shared( - "robot_test", Eigen::Vector3d(1.18, 0, 1.6), Eigen::Quaterniond(0.73, 0, 0.68, 0))), - desired_twist(std::make_shared("robot_test")), - dt(period) {} - - bool on_configure() { - this->add_state_subscription("/ds/desired_twist", this->desired_twist); - this->add_state_publisher("/robot_test/pose", this->robot_pose); - return true; - } - - void step() { - if (!this->desired_twist->is_empty()) { - *this->robot_pose = dt * *this->desired_twist + *this->robot_pose; - } - this->send_transform(*this->robot_pose); - } -}; -}// namespace - -/** - * A lifecycle node has the same node API - * as a regular node. This means we can spawn a - * node, give it a name and add it to the executor. - */ -int main(int argc, char* argv[]) { - // force flush of the stdout buffer. - // this ensures a correct sync of all prints - // even when executed simultaneously within the launch file. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - rclcpp::init(argc, argv); - - rclcpp::executors::SingleThreadedExecutor exe; - - std::shared_ptr lmg = std::make_shared("motion_generator", 1ms); - std::shared_ptr cv = std::make_shared("visualizer", 100ms); - std::shared_ptr sri = std::make_shared("robot_interface", 1ms); - - exe.add_node(lmg->get_node_base_interface()); - exe.add_node(cv->get_node_base_interface()); - exe.add_node(sri->get_node_base_interface()); - - exe.spin(); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/source/modulo_core/examples/helicoidal.cpp b/source/modulo_core/examples/helicoidal.cpp deleted file mode 100644 index a9d9cc5b5..000000000 --- a/source/modulo_core/examples/helicoidal.cpp +++ /dev/null @@ -1,151 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include - -#include "modulo_core/Cell.hpp" - -using namespace modulo::core; -using namespace state_representation; -using namespace dynamical_systems; - -namespace { -class MotionGenerator : public Cell { -private: - std::shared_ptr current_pose; - std::shared_ptr desired_twist; - std::shared_ptr> ring_motion_generator; - std::shared_ptr> linear_motion_generator; - double radius; - double radius_decay; - -public: - explicit MotionGenerator(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - current_pose(std::make_shared("robot_test")), - desired_twist(std::make_shared("robot_test")), - ring_motion_generator(CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::RING)), - linear_motion_generator( - CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)), - radius(1.0), - radius_decay(0.99) { - this->linear_motion_generator->set_parameter_value("attractor", CartesianPose::Identity("robot_test")); - this->ring_motion_generator->set_parameter_value("center", CartesianPose::Identity("center")); - } - - bool on_configure() { - this->add_subscription("/robot_test/pose", this->current_pose); - this->add_publisher("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - if (!this->current_pose->is_empty()) { - double ring_distance = this->current_pose->dist( - this->ring_motion_generator->get_parameter_value("center"), CartesianStateVariable::POSITION - ) - 0.01; - if (ring_distance > .03) { - this->ring_motion_generator->set_parameter_value("radius", radius); - *this->desired_twist = this->ring_motion_generator->evaluate(*this->current_pose); - this->radius *= this->radius_decay; - } else { - *this->desired_twist = this->linear_motion_generator->evaluate(*this->current_pose); - } - this->desired_twist->clamp(0.4, 2.0); - } else { - this->desired_twist->initialize(); - } - } -}; - -class ConsoleVisualizer : public Cell { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - -public: - explicit ConsoleVisualizer(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_pose(std::make_shared("robot_test")), - desired_twist(std::make_shared("robot_test")) {} - - bool on_configure() { - this->add_subscription("/robot_test/pose", this->robot_pose); - this->add_subscription("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - std::ostringstream os; - os << std::endl; - os << "##### ROBOT POSE #####" << std::endl; - os << *this->robot_pose << std::endl; - os << "##### DESIRED TWIST #####" << std::endl; - os << *this->desired_twist << std::endl; - RCLCPP_INFO(get_logger(), "%s", os.str().c_str()); - } -}; - -class SimulatedRobotInterface : public Cell { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - std::chrono::milliseconds dt; - -public: - explicit SimulatedRobotInterface(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_pose( - std::make_shared( - "robot_test", Eigen::Vector3d::Random(), Eigen::Quaterniond::UnitRandom())), - desired_twist(std::make_shared("robot_test")), - dt(period) {} - - bool on_configure() { - this->add_subscription("/ds/desired_twist", this->desired_twist); - this->add_publisher("/robot_test/pose", this->robot_pose); - return true; - } - - void step() { - if (!this->desired_twist->is_empty()) { - *this->robot_pose = dt * *this->desired_twist + *this->robot_pose; - } - this->send_transform(*this->robot_pose); - } -}; -}// namespace - -/** - * A lifecycle node has the same node API - * as a regular node. This means we can spawn a - * node, give it a name and add it to the executor. - */ -int main(int argc, char* argv[]) { - // force flush of the stdout buffer. - // this ensures a correct sync of all prints - // even when executed simultaneously within the launch file. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor exe; - - std::shared_ptr lmg = std::make_shared("motion_generator", 1ms); - std::shared_ptr cv = std::make_shared("visualizer", 100ms); - std::shared_ptr sri = std::make_shared("robot_interface", 1ms); - - exe.add_node(lmg->get_node_base_interface()); - exe.add_node(cv->get_node_base_interface()); - exe.add_node(sri->get_node_base_interface()); - - exe.spin(); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/source/modulo_core/examples/joints.cpp b/source/modulo_core/examples/joints.cpp deleted file mode 100644 index dcccae5e2..000000000 --- a/source/modulo_core/examples/joints.cpp +++ /dev/null @@ -1,127 +0,0 @@ -#include - -#include -#include -#include - -#include "modulo_core/Cell.hpp" - -using namespace state_representation; -using namespace dynamical_systems; - -namespace { -class LinearMotionGenerator : public modulo::core::Cell { -private: - std::shared_ptr current_positions; - std::shared_ptr desired_velocities; - std::shared_ptr> motion_generator; - -public: - explicit LinearMotionGenerator(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - current_positions(std::make_shared("robot", 6)), - desired_velocities(std::make_shared("robot", 6)), - motion_generator(JointDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)) { - motion_generator->set_parameter_value("attractor", JointState::Random("robot", 6)); - } - - bool on_configure() { - this->add_subscription("/robot/joint_state", this->current_positions); - this->add_publisher("/ds/desired_velocities", this->desired_velocities); - return true; - } - - void step() { - if (!this->current_positions->is_empty()) { - *this->desired_velocities = this->motion_generator->evaluate(*this->current_positions); - } else { - this->desired_velocities->initialize(); - } - } -}; - -class ConsoleVisualizer : public modulo::core::Cell { -private: - std::shared_ptr robot_positions; - std::shared_ptr desired_velocities; - -public: - explicit ConsoleVisualizer(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_positions(std::make_shared("robot", 6)), - desired_velocities(std::make_shared("robot", 6)) {} - - bool on_configure() { - this->add_subscription("/robot/joint_state", this->robot_positions); - this->add_subscription("/ds/desired_velocities", this->desired_velocities); - return true; - } - - void step() { - std::ostringstream os; - os << "##### ROBOT POSITIONS #####" << std::endl; - os << *this->robot_positions << std::endl; - os << "##### DESIRED VELOCITY #####" << std::endl; - os << *this->desired_velocities << std::endl; - RCLCPP_INFO(get_logger(), "%s", os.str().c_str()); - } -}; - -class SimulatedRobotInterface : public modulo::core::Cell { -private: - std::shared_ptr robot_state; - std::shared_ptr desired_velocities; - double dt; - -public: - explicit SimulatedRobotInterface(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_state(std::make_shared("robot", 6)), - desired_velocities(std::make_shared("robot", 6)), - dt(0.001) {} - - bool on_configure() { - this->robot_state->set_positions(Eigen::VectorXd::Random(6)); - this->add_subscription("/ds/desired_velocities", this->desired_velocities); - this->add_publisher("/robot/joint_state", this->robot_state); - return true; - } - - void step() { - if (!this->desired_velocities->is_empty()) { - this->robot_state->set_positions((dt * *this->desired_velocities).get_velocities()); - this->robot_state->set_velocities(this->desired_velocities->get_velocities()); - } - } -}; -}// namespace - -/** - * A lifecycle node has the same node API - * as a regular node. This means we can spawn a - * node, give it a name and add it to the executor. - */ -int main(int argc, char* argv[]) { - // force flush of the stdout buffer. - // this ensures a correct sync of all prints - // even when executed simultaneously within the launch file. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - rclcpp::init(argc, argv); - - rclcpp::executors::SingleThreadedExecutor exe; - - std::shared_ptr lmg = std::make_shared("motion_generator", 1ms); - std::shared_ptr cv = std::make_shared("visualizer", 100ms); - std::shared_ptr sri = std::make_shared("robot_interface", 1ms); - - exe.add_node(lmg->get_node_base_interface()); - exe.add_node(cv->get_node_base_interface()); - exe.add_node(sri->get_node_base_interface()); - - exe.spin(); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/source/modulo_core/examples/movingReferenceFrame.cpp b/source/modulo_core/examples/movingReferenceFrame.cpp deleted file mode 100644 index d9131dc3b..000000000 --- a/source/modulo_core/examples/movingReferenceFrame.cpp +++ /dev/null @@ -1,188 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "modulo_core/Cell.hpp" - -using namespace state_representation; -using namespace dynamical_systems; - -namespace { -class MotionGenerator : public modulo::core::Cell { -private: - std::shared_ptr object_state; - std::shared_ptr current_pose; - std::shared_ptr desired_twist; - std::shared_ptr> motion_generator; - -public: - explicit MotionGenerator(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - object_state(std::make_shared("object", "world")), - current_pose(std::make_shared("robot_test", "world")), - desired_twist(std::make_shared("robot_test", "world")), - motion_generator(CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::CIRCULAR)) { - motion_generator->set_parameter_value("limit_cycle", Ellipsoid("attractor", "object")); - } - - bool on_configure() { - this->add_subscription("/object/pose", this->object_state); - this->add_subscription("/object/twist", this->object_state); - this->add_subscription("/robot_test/pose", this->current_pose); - this->add_publisher("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - if (!this->current_pose->is_empty()) { - *this->desired_twist = this->motion_generator->evaluate(*this->current_pose); - } else { - this->desired_twist->initialize(); - } - if (!this->object_state->is_empty()) { - this->motion_generator->set_base_frame(*this->object_state); - } - } -}; - -class ConsoleVisualizer : public modulo::core::Cell { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - -public: - explicit ConsoleVisualizer(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_pose(std::make_shared("robot_test")), - desired_twist(std::make_shared("robot_test")) {} - - bool on_configure() { - this->add_subscription("/robot_test/pose", this->robot_pose); - this->add_subscription("/ds/desired_twist", this->desired_twist); - return true; - } - - void step() { - std::ostringstream os; - os << std::endl; - os << "##### ROBOT POSE #####" << std::endl; - os << *this->robot_pose << std::endl; - os << "##### DESIRED TWIST #####" << std::endl; - os << *this->desired_twist << std::endl; - RCLCPP_INFO(get_logger(), "%s", os.str().c_str()); - } -}; - -class SimulatedObject : public modulo::core::Cell { -private: - std::shared_ptr object_pose; - std::shared_ptr object_twist; - std::shared_ptr> motion_generator; - std::chrono::milliseconds dt; - double sign; - -public: - explicit SimulatedObject(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - object_pose( - std::make_shared( - CartesianPose::Identity("object"))), - object_twist( - std::make_shared(CartesianTwist("object"))), - motion_generator( - CartesianDynamicalSystemFactory::create_dynamical_system(DYNAMICAL_SYSTEM_TYPE::POINT_ATTRACTOR)), - dt(period), - sign(-1) { - motion_generator->set_parameter_value("attractor", CartesianPose("object_attractor", 2., 0., 0.)); - } - - bool on_configure() { - this->add_publisher("/object/pose", this->object_pose); - this->add_publisher("/object/twist", this->object_twist); - return true; - } - - void step() { - // compute the dynamics of the object - *this->object_twist = this->motion_generator->evaluate(*this->object_pose); - this->object_twist->set_angular_velocity(Eigen::Vector3d(0.2, 0., 0.)); - this->object_twist->clamp(0.5, 0.2); - *this->object_pose = dt * *this->object_twist + *this->object_pose; - - // change attractor if previous was reached - if (this->object_pose->dist(this->motion_generator->get_parameter_value("attractor")) < 1e-3) { - this->motion_generator->set_parameter_value( - "attractor", CartesianPose("object_attractor", sign * 2., 0., 0.)); - sign *= -1; - } - this->send_transform(*this->object_pose, "object"); - } -}; - -class SimulatedRobotInterface : public modulo::core::Cell { -private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; - std::chrono::milliseconds dt; - -public: - explicit SimulatedRobotInterface(const std::string& node_name, const std::chrono::milliseconds& period) : - Cell(node_name, period), - robot_pose( - std::make_shared( - "robot_test", Eigen::Vector3d::Random(), Eigen::Quaterniond::UnitRandom())), - desired_twist(std::make_shared("robot_test")), - dt(period) {} - - bool on_configure() { - this->add_subscription("/ds/desired_twist", this->desired_twist); - this->add_publisher("/robot_test/pose", this->robot_pose); - return true; - } - - void step() { - if (!this->desired_twist->is_empty()) { - *this->robot_pose = dt * *this->desired_twist + *this->robot_pose; - } - this->send_transform(*this->robot_pose); - } -}; -}// namespace - -/** - * A lifecycle node has the same node API - * as a regular node. This means we can spawn a - * node, give it a name and add it to the executor. - */ -int main(int argc, char* argv[]) { - // force flush of the stdout buffer. - // this ensures a correct sync of all prints - // even when executed simultaneously within the launch file. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - rclcpp::init(argc, argv); - - rclcpp::executors::MultiThreadedExecutor exe; - - std::shared_ptr lmg = std::make_shared("motion_generator", 1ms); - std::shared_ptr cv = std::make_shared("visualizer", 100ms); - std::shared_ptr sri = std::make_shared("robot_interface", 1ms); - std::shared_ptr so = std::make_shared("simulated_object", 1ms); - - exe.add_node(lmg->get_node_base_interface()); - exe.add_node(cv->get_node_base_interface()); - exe.add_node(sri->get_node_base_interface()); - exe.add_node(so->get_node_base_interface()); - - exe.spin(); - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file diff --git a/source/modulo_core/include/modulo_core/Cell.hpp b/source/modulo_core/include/modulo_core/Cell.hpp deleted file mode 100644 index 065cc35ff..000000000 --- a/source/modulo_core/include/modulo_core/Cell.hpp +++ /dev/null @@ -1,753 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "modulo_new_core/EncodedState.hpp" -#include "modulo_core/communication/message_passing/PublisherHandler.hpp" -#include "modulo_core/communication/message_passing/SubscriptionHandler.hpp" -#include "modulo_core/communication/message_passing/TransformBroadcasterHandler.hpp" -#include "modulo_core/communication/message_passing/TransformListenerHandler.hpp" -#include "modulo_core/communication/service_client/ClientHandler.hpp" - -using namespace std::chrono_literals; - -namespace modulo::core { -/** - * @class Cell - * @brief Abstract class to define a Cell - * A Cell is the base class of the whole architecture. - * It handles all the basic ROS communications such as - * definitions of subscriptions, publishers and service - * calls. It can then be derived into a MotionGenerator, - * a Controller, a Sensor, or a RobotInterface. It is - * derived from a lifecycle node which allows to use - * ROS2 state machine functionalities for nodes. - */ -class Cell : public rclcpp_lifecycle::LifecycleNode { -private: - bool configured_; ///< boolean that informs that the node has been configured, i.e passed by the on_configure state - bool active_; ///< boolean that informs that the node has been activated, i.e passed by the on_activate state - bool shutdown_; ///< boolean that informs that the node has been shutdown, i.e passed by the on_shutdown state - std::shared_ptr update_parameters_timer_; ///< reference to the timer for periodically that periodicall update the parameters - std::chrono::nanoseconds period_; ///< rate of the publisher functions in nanoseconds - std::map> parameters_; ///< list for storing parameters - std::map, bool>> handlers_;///< map for storing publishers, subscriptions and tf - std::list> timers_; ///< reference to timers for periodically called functions - std::list> daemons_; ///< reference to timers for daemonized functions - - /** - * @brief Function to clear all publishers, subscriptions and services - */ - void reset(); - - /** - * @brief Function to add a default transform broadcaster to the map of handlers - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param period the period to wait between two publishing - * @param always_active if true, always publish the transform even if the node is not configured - */ - template - void add_transform_broadcaster(const std::chrono::duration& period, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Function to add a default transform listener to the map of handlers - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param timeout the period after wich to consider that the handler has timeout - */ - template - void add_transform_listener(const std::chrono::duration& timeout); - - /** - * @brief Update the value of the parameters from the parameter server - */ - void update_parameters(); - -protected: - /** - * @brief Getter of the handlers attribute - * @return Reference to the handlers attribute - */ - const std::map, bool>>& get_handlers() const; - - /** - * @brief Get the pointer to the desired parameter - * @param parameter_name the name of the desired parameter - */ - const std::shared_ptr get_parameter_pointer(const std::string& parameter_name) const; - -public: - /** - * @brief Cell constructor with arguments needed from ROS2 node - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param node_name name of the ROS node - * @param period the period of each step function call - * @param intra_process_comms ROS2 parameter to declare if nodes share the same memory for instant process communication - */ - template - explicit Cell(const std::string& node_name, const std::chrono::duration& period, bool intra_process_comms = false); - - /** - * @brief Cell construction from ROS2 NodeOptions - * @param options NodeOptions containing a node name in the remapping arguments list and a "period" parameter - * with a value in seconds in the parameter override list - */ - explicit Cell(const rclcpp::NodeOptions& options); - - /** - * @brief Destructor - */ - ~Cell(); - - /** - * @brief Getter of the period attribute - * @return Reference to the period attribute - */ - const std::chrono::nanoseconds& get_period() const; - - /** - * @brief Getter of the configured boolean attribute - * @return true if configured - */ - bool is_configured() const; - - /** - * @brief Getter of the active boolean attribute - * @return true if active - */ - bool is_active() const; - - /** - * @brief Getter of the shutdown boolean attribute - * @return true if shutdown - */ - bool is_shutdown() const; - - /** - * @brief Template function to add a generic publisher to the map of handlers - * @tparam MsgT template value for accepting any type of ROS2 messages - * @tparam RecT template value for accepting any type of recipient - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param channel unique name of the publish channel that is used as key to the map - * @param recipient the state that contain the data to be published - * @param period the period to wait between two publishing - * @param always_active if true, always publish even if the node is not configured - * @param queue_size publisher parameters indicating the maximum size of the buffer - */ - template - void add_publisher(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Template function to add a generic publisher to the map of handlers - * @tparam MsgT template value for accepting any type of ROS2 messages - * @tparam RecT template value for accepting any type of recipient - * @param channel unique name of the publish channel that is used as key to the map - * @param recipient the state that contain the data to be published - * @param always_active if true, always publish even if the node is not configured - * @param queue_size publisher parameters indicating the maximum size of the buffer - */ - template - void add_publisher(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Template function to add a specific state publisher that makes use of protobuf encodings - * to the map of handlers - * @tparam RecT template value for accepting any State type recipient - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param channel unique name of the publish channel that is used as key to the map - * @param recipient the state that contain the data to be published - * @param period the period to wait between two publishing - * @param always_active if true, always publish even if the node is not configured - * @param queue_size publisher parameters indicating the maximum size of the buffer - */ - template - void add_state_publisher(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Template function to add a specific state publisher that makes use of protobuf encodings - * to the map of handlers - * @tparam RecT template value for accepting any State type recipient - * @param channel unique name of the publish channel that is used as key to the map - * @param recipient the state that contain the data to be published - * @param always_active if true, always publish even if the node is not configured - * @param queue_size publisher parameters indicating the maximum size of the buffer - */ - template - void add_state_publisher(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Template function to add a generic subscription to the map of handlers - * @tparam MsgT template value for accepting any type of ROS2 messages - * @tparam RecT template value for accepting any type of recipient - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param channel unique name of the subscription channel that is used as key to the map - * @param recipient the state that will contain the received data - * @param timeout the period after wich to consider that the subscriber has timeout - * @param always_active if true, always receive messages from the topic even if the node is not configured - * @param queue_size subscriber parameters indicating the maximum size of the buffer - */ - template - void add_subscription(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& timeout, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Template function to add a generic subscription to the map of handlers - * @param channel unique name of the subscription channel that is used as key to the map - * @param recipient the state that will contain the received data - * @param always_active if true, always receive messages from the topic even if the node is not configured - * @param nb_period_to_timeout the number of period before considering that the subscription has timeout - * @param queue_size subscriber parameters indicating the maximum size of the buffer - */ - template - void add_subscription(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active = false, - unsigned int nb_period_to_timeout = 10, - int queue_size = 10); - - /** - * @brief Template function to add a specific state subscription that makes use of - * protobuf decoding to the map of handlers - * @tparam RecT template value for accepting any State type recipient - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param channel unique name of the subscription channel that is used as key to the map - * @param recipient the state that will contain the received data - * @param timeout the period after wich to consider that the subscriber has timeout - * @param always_active if true, always receive messages from the topic even if the node is not configured - * @param queue_size subscriber parameters indicating the maximum size of the buffer - */ - template - void add_state_subscription(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& timeout, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Template function to add a specific state subscription that makes use of - * protobuf decoding to the map of handlers - * @tparam RecT template value for accepting any State type recipient - * @param channel unique name of the subscription channel that is used as key to the map - * @param recipient the state that will contain the received data - * @param always_active if true, always receive messages from the topic even if the node is not configured - * @param nb_period_to_timeout the number of period before considering that the subscription has timeout - * @param queue_size subscriber parameters indicating the maximum size of the buffer - */ - template - void add_state_subscription(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active = false, - unsigned int nb_period_to_timeout = 10, - int queue_size = 10); - - /** - * @brief Template function to add a generic client to the map of handlers - * @tparam srvT tamplate value to accept any type of ROS2 services - * @tparam DurationT template value for accepting any type of std::chrono duration values - * @param channel unique name of the communication topic between the client and the server - * @param timeout period before considering the server is not responding - */ - template - void add_client(const std::string& channel, const std::chrono::duration& timeout); - - /** - * @brief Function to add a generic transform broadcaster to the map of handlers - * @param recipient the state that contain the data to be published - * @param period the period to wait between two publishing - * @param always_active if true, always publish the transform even if the node is not configured - */ - template - void add_transform_broadcaster(const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Function to add a generic transform broadcaster to the map of handlers - * @param recipient the state that contain the data to be published - * @param period the period to wait between two publishing - * @param always_active if true, always publish the transform even if the node is not configured - */ - template - void add_transform_broadcaster(const state_representation::CartesianPose& recipient, - const std::chrono::duration& period, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Function to add a generic transform broadcaster to the map of handlers - * @param recipient the state that contain the data to be published - * @param period the period to wait between two publishing - * @param always_active if true, always publish the transform even if the node is not configured - */ - template - void add_transform_broadcaster(const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Function to add a generic transform broadcaster to the map of handlers - * @param recipient the state that contain the data to be published - * @param always_active if true, always publish the transform even if the node is not configured - */ - void add_transform_broadcaster(const std::shared_ptr& recipient, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Function to add a generic transform broadcaster to the map of handlers - * @param recipient the state that contain the data to be published - * @param always_active if true, always publish the transform even if the node is not configured - */ - void add_transform_broadcaster(const state_representation::CartesianPose& recipient, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Function to add a generic transform broadcaster to the map of handlers - * @param recipient the state that contain the data to be published - * @param always_active if true, always publish the transform even if the node is not configured - */ - void add_transform_broadcaster(const std::shared_ptr& recipient, - bool always_active = false, - int queue_size = 10); - - /** - * @brief Add a parameter to be updated by the parameter server - * @param parameter The parameter using the state_representation of a parameter - * @param prefix a string for prefixing the parameter name - */ - template - void add_parameter(const std::shared_ptr>& parameter, const std::string& prefix = ""); - - /** - * @brief Add multiple parameters to be updated by the parameter server - * @param parameters the list of parameters - * @param prefix a string for prefixing the parameter name - */ - void add_parameters(const std::list>& parameters, const std::string& prefix = ""); - - /** - * @brief Set the value of a declared parameter - * @param parameter the new value of the parameter - */ - template - void set_parameter_value(const std::string& parameter_name, const T& value); - - /** - * @brief Set the value of a declared parameter - * @param parameter the new value of the parameter - */ - template - void set_parameter_value(const std::shared_ptr>& parameter); - - /** - * @brief Set the value of a declared parameter - * @param parameter the new value of the parameter - */ - void set_parameter_value(const std::shared_ptr& parameter); - - /** - * @brief Function to send a transform using the generic transform broadcaster - * @param transform the transformation to send - * @param name the new name to give to the transform. If empty it will use the name provided in transform - */ - void send_transform(const state_representation::CartesianState& transform, const std::string& name = "") const; - - /** - * @brief Send a request to the server and wait for its response - * @param channel the channel of communication - * @param request the request to send - * @return the response from the server - */ - template - std::shared_ptr send_blocking_request(const std::string& channel, const std::shared_ptr& request); - - /** - * @brief Send a request to the server without waiting for its response - * @param channel the channel of communication - * @param request the request to send - * @return the response from the server - */ - template - std::shared_future> send_request(const std::string& channel, const std::shared_ptr& request); - - /** - * @brief Function to get a transform from the generic transform listener - * @param frame_name name of the frame to look for - * @param the frame in wich to express the transform - * @return the CartesianPose representing the transformation - */ - const state_representation::CartesianPose lookup_transform(const std::string& frame_name, const std::string& reference_frame = "world") const; - - /** - * @brief Transition callback for state configuring - * on_configure callback is being called when the lifecycle node - * enters the "configuring" state. - * Depending on the return value of this function, the state machine - * either invokes a transition to the "inactive" state or stays - * in "unconfigured". - * TRANSITION_CALLBACK_SUCCESS transitions to "inactive" - * TRANSITION_CALLBACK_FAILURE transitions to "unconfigured" - * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure(const rclcpp_lifecycle::State&) override; - - /** - * @brief Proxy function for the on_configure ROS2 lifecycle function. - * This function is called by the main on_configure function and is made to - * adapted to the derived class. - */ - virtual bool on_configure(); - - /** - * @brief Transition callback for state activating - * on_activate callback is being called when the lifecycle node - * enters the "activating" state. - * Depending on the return value of this function, the state machine - * either invokes a transition to the "active" state or stays - * in "inactive". - * TRANSITION_CALLBACK_SUCCESS transitions to "active" - * TRANSITION_CALLBACK_FAILURE transitions to "inactive" - * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State&) override; - - /** - * @brief Proxy function for the on_activate ROS2 lifecycle function. - * This function is called by the main on_activate function and is made to - * adapted to the derived class. - */ - virtual bool on_activate(); - - /** - * @brief Transition callback for state deactivating - * on_deactivate callback is being called when the lifecycle node - * enters the "deactivating" state. - * Depending on the return value of this function, the state machine - * either invokes a transition to the "inactive" state or stays - * in "active". - * TRANSITION_CALLBACK_SUCCESS transitions to "inactive" - * TRANSITION_CALLBACK_FAILURE transitions to "active" - * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State&) override; - - /** - * @brief Proxy function for the on_deactivate ROS2 lifecycle function. - * This function is called by the main on_deactivate function and is made to - * adapted to the derived class. - */ - virtual bool on_deactivate(); - - /** - * @brief Transition callback for state cleaningup - * on_cleanup callback is being called when the lifecycle node - * enters the "cleaningup" state. - * Depending on the return value of this function, the state machine - * either invokes a transition to the "unconfigured" state or stays - * in "inactive". - * TRANSITION_CALLBACK_SUCCESS transitions to "unconfigured" - * TRANSITION_CALLBACK_FAILURE transitions to "inactive" - * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup(const rclcpp_lifecycle::State&) override; - - /** - * @brief Proxy function for the on_cleanup ROS2 lifecycle function. - * This function is called by the main on_cleanup function and is made to - * adapted to the derived class. - */ - virtual bool on_cleanup(); - - /** - * @brief Transition callback for state shutting down - * on_shutdown callback is being called when the lifecycle node - * enters the "shuttingdown" state. - * Depending on the return value of this function, the state machine - * either invokes a transition to the "finalized" state or stays - * in its current state. - * TRANSITION_CALLBACK_SUCCESS transitions to "finalized" - * TRANSITION_CALLBACK_FAILURE transitions to current state - * TRANSITION_CALLBACK_ERROR or any uncaught exceptions to "errorprocessing" - */ - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_shutdown(const rclcpp_lifecycle::State& state) override; - - /** - * @brief Proxy function for the on_shutdown ROS2 lifecycle function. - * This function is called by the main on_shutdown function and is made to - * adapted to the derived class. - */ - virtual bool on_shutdown(); - - /** - * @brief Function computing one step of calculation. It is called periodically in the run function. - */ - virtual void step(); - - /** - * @brief Main loop that will be executed in parallel of the rest. At each time step it calls the step function. - */ - void run(); - - /** - * @brief Function to periodically call the given callback_function with guard protection and only - * when the node is active - * @param callback_function the function to call - */ - void run_periodic_call(const std::function& callback_function); - - /** - * @brief Function to add a periodic call to the function given in input - * @details This function is similar to the add_deamon one with - * the difference that the callback_function is only called when the node - * is active. The desired design is, therefore, that it should only be called - * in the on_configure function, as periodic_call will be cleaned when on_cleanup - * is called. - * @param callback_function the function to call - * @param period the period between two calls - */ - void add_periodic_call(const std::function& callback_function, const std::chrono::nanoseconds& period); - - /** - * @brief Function to daemonize the callback function given in input - * @details This function is similar to the add_periodic_call one with - * the difference that a daemon is always active, including when the node - * is not configured. The desired design is, therefore, that it should be - * called only in the constructor to avoid potential duplicates. - * @param callback_function the function to call - * @param period the period between two calls - */ - void add_daemon(const std::function& callback_function, const std::chrono::nanoseconds& period); -}; - -template -Cell::Cell(const std::string& node_name, const std::chrono::duration& period, bool intra_process_comms) : rclcpp_lifecycle::LifecycleNode(node_name, rclcpp::NodeOptions().use_intra_process_comms(intra_process_comms)), - configured_(false), - active_(false), - shutdown_(false), - period_(period) { - // add the update parameters call - this->update_parameters_timer_ = this->create_wall_timer(this->period_, [this] { this->update_parameters(); }); -} - -inline const std::map, bool>>& Cell::get_handlers() const { - return this->handlers_; -} - -inline bool Cell::is_configured() const { - return this->configured_; -} - -inline bool Cell::is_active() const { - return this->active_; -} - -inline bool Cell::is_shutdown() const { - return this->shutdown_; -} - -inline const std::chrono::nanoseconds& Cell::get_period() const { - return this->period_; -} - -inline const std::shared_ptr Cell::get_parameter_pointer(const std::string& parameter_name) const { - return this->parameters_.at(parameter_name); -} - -template -void Cell::set_parameter_value(const std::shared_ptr>& parameter) { - this->set_parameter_value(parameter->get_name(), parameter->get_value()); -} - -template -void Cell::add_publisher(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active, - int queue_size) { - auto handler = std::make_shared>(recipient, this->get_clock()); - handler->set_publisher(this->create_publisher(channel, queue_size)); - handler->set_timer(this->create_wall_timer(period, [handler] { handler->publish_callback(); })); - this->handlers_.insert(std::make_pair(channel, std::make_pair(handler, always_active))); - // if the handler is always active, activate it directly - if (always_active) { - handler->activate(); - } -} - -template -void Cell::add_publisher(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active, - int queue_size) { - this->add_publisher(channel, recipient, this->period_, always_active, queue_size); -} - -template -void Cell::add_state_publisher(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active, - int queue_size) { - this->add_publisher(channel, recipient, period, always_active, queue_size); -} - -template -void Cell::add_state_publisher(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active, - int queue_size) { - this->add_publisher(channel, recipient, always_active, queue_size); -} - -template -void Cell::add_subscription(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& timeout, - bool always_active, - int queue_size) { - auto handler = std::make_shared>(recipient, timeout); - handler->set_subscription(this->create_subscription(channel, queue_size, [handler](const std::shared_ptr msg) { handler->subscription_callback(msg); })); - this->handlers_.insert(std::make_pair(channel, std::make_pair(handler, always_active))); - if (always_active) { - handler->activate(); - } -} - -template -void Cell::add_subscription(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active, - unsigned int nb_period_to_timeout, - int queue_size) { - this->add_subscription(channel, recipient, nb_period_to_timeout * this->period_, always_active, queue_size); -} - -template -void Cell::add_state_subscription(const std::string& channel, - const std::shared_ptr& recipient, - const std::chrono::duration& timeout, - bool always_active, - int queue_size) { - this->add_subscription(channel, recipient, timeout, always_active, queue_size); -} - -template -void Cell::add_state_subscription(const std::string& channel, - const std::shared_ptr& recipient, - bool always_active, - unsigned int nb_period_to_timeout, - int queue_size) { - this->add_subscription(channel, recipient, nb_period_to_timeout, always_active, queue_size); -} - -template -void Cell::add_transform_broadcaster(const std::chrono::duration& period, - bool always_active, - int queue_size) { - auto handler = std::make_shared(this->get_clock()); - handler->set_publisher(this->create_publisher("tf", queue_size)); - handler->set_timer(this->create_wall_timer(period, [handler] { handler->publish_callback(); })); - this->handlers_.insert(std::make_pair("tf_broadcaster", std::make_pair(handler, always_active))); - if (always_active) { - handler->activate(); - } -} - -template -void Cell::add_transform_broadcaster(const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active, - int queue_size) { - auto handler = std::make_shared(recipient, this->get_clock()); - handler->set_publisher(this->create_publisher("tf", queue_size)); - handler->set_timer(this->create_wall_timer(period, [handler] { handler->publish_callback(); })); - this->handlers_.insert(std::make_pair(recipient->get_name() + "_in_" + recipient->get_reference_frame() + "_broadcaster", std::make_pair(handler, always_active))); - if (always_active) { - handler->activate(); - } -} - -template -void Cell::add_transform_broadcaster(const state_representation::CartesianPose& recipient, - const std::chrono::duration& period, - bool always_active, - int queue_size) { - this->add_transform_broadcaster(std::make_shared(recipient), period, always_active, queue_size); -} - -template -void Cell::add_transform_broadcaster(const std::shared_ptr& recipient, - const std::chrono::duration& period, - bool always_active, - int queue_size) { - using namespace communication; - using namespace state_representation; - auto parameter = std::static_pointer_cast>(recipient); - auto handler = std::make_shared, tf2_msgs::msg::TFMessage>>(parameter, this->get_clock()); - handler->set_publisher(this->create_publisher("tf", queue_size)); - handler->set_timer(this->create_wall_timer(period, [handler] { handler->publish_callback(); })); - this->handlers_.insert(std::make_pair(parameter->get_value().get_name() + "_in_" + parameter->get_value().get_reference_frame() + "_broadcaster", std::make_pair(handler, always_active))); - if (always_active) { - handler->activate(); - } -} - -template -void Cell::add_transform_listener(const std::chrono::duration& timeout) { - auto handler = std::make_shared(timeout, this->get_clock()); - this->handlers_.insert(std::make_pair("tf_listener", std::make_pair(handler, false))); -} - -template -void Cell::add_client(const std::string& channel, const std::chrono::duration& timeout) { - auto handler = std::make_shared>(timeout); - handler->set_client(this->create_client(channel)); - this->handlers_.insert(std::make_pair(channel, std::make_pair(handler, false))); -} - -template -std::shared_ptr Cell::send_blocking_request(const std::string& channel, const std::shared_ptr& request) { - return static_cast&>(*this->handlers_.at(channel).first).send_blocking_request(request); -} - -template -std::shared_future> Cell::send_request(const std::string& channel, const std::shared_ptr& request) { - return static_cast&>(*this->handlers_.at(channel).first).send_request(request); -} -}// namespace modulo::core diff --git a/source/modulo_core/include/modulo_core/Component.hpp b/source/modulo_core/include/modulo_core/Component.hpp deleted file mode 100644 index 8844a5c4d..000000000 --- a/source/modulo_core/include/modulo_core/Component.hpp +++ /dev/null @@ -1,147 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include -#include - -#include "modulo_core/Cell.hpp" - -namespace modulo::core { -/** - * @class Component - * @brief Abstract class to define an component, handling base predicates - */ -class Component : public core::Cell { -private: - std::map> predicates_;///< map of predicates - std::map> predicate_functions_; ///< map of predicate functions evaluated at each step - - /** - * @brief Initialize all the parameters needed in the constructors - */ - void on_init(); - - /** - * @brief Periodically called function that evaluates the predicates functions - */ - void evaluate_predicate_functions(); - - /** - * @brief Create a topic name from the predicate name - * @param predicate_name The predicate name - * @return A corresponding topic name - */ - std::string generate_predicate_topic(const std::string& predicate_name) const; - -protected: - /** - * @brief Add a predicate to the map of predicates - * @param predicate the predicate to add - */ - void add_predicate(const std::shared_ptr& predicate); - - /** - * @brief Add a predicate to the map of predicates based on a function to periodically call - * @param predicate_name the name of the associated predicate - * @param predicate_function the function to periodically call that returns the value of the predicate - */ - void add_predicate(const std::string& predicate_name, const std::function& predicate_function); - - /** - * @brief Add an external predicate to the map of predicates which value will be registered through - * the subscribed channel provided - * @param predicate_name the name of the predicate - * @param channel the associated subscribed channel - */ - void add_received_predicate(const std::string& predicate_name, const std::string& channel); - - /** - * @brief Get the value of the predicate given as parameter - * @param predicate_name the name of the predicate to retrieve from the - * map of predicates - * @return the value of the predicate as a boolean - */ - bool get_predicate_value(const std::string& predicate_name) const; - - /** - * @brief Set the value of the predicate given as parameter - * @param predicate_name the name of the predicate to retrieve from the - * map of predicates - * @param value the new value of the predicate - */ - void set_predicate_value(const std::string& predicate_name, bool value); - - /** - * @brief Add an event to the map of predicates - * @param event the event to add - */ - void add_event(const std::shared_ptr& event); - - /** - * @brief Add an event to the map of predicates based on a function to periodically call - * @param event_name the name of the associated event - * @param event_function the function to periodically call that returns the value of the event - */ - void add_event(const std::string& event_name, const std::function& event_function); - - /** - * @brief Add an external event to the map of predicates which value will be registered through - * the subscribed channel provided - * @param event_name the name of the event - * @param channel the associated subscribed channel - */ - void add_received_event(const std::string& event_name, const std::string& channel); - - /** - * @brief Read the value of the event given as parameter - * @param event_name the name of the event to retrieve from the - * map of predicates - * @return the value of the evant as a boolean - */ - bool read_event_value(const std::string& event_name); - - /** - * @brief Set the value of the event given as parameter - * @param event_name the name of the event to retrieve from the - * map of predicates - * @param value the new value of the event - */ - void set_event_value(const std::string& event_name, bool value); - -public: - /** - * @brief Constructor for the Component class - * @tparam DurationT template value to accept any type durations from the std::chrono library - * @param name name of the node - * @param period rate used by each publisher of the class - * @param intra_process_comms if true, activate intra processes communication by sharing pointers instead of messages - */ - template - explicit Component(const std::string& node_name, - const std::chrono::duration& period, - bool intra_process_comms = false); - - /** - * @brief Component construction from ROS2 NodeOptions - * @param options NodeOptions containing a node name in the remapping arguments list and a "period" parameter - * with a value in seconds in the parameter override list - */ - explicit Component(const rclcpp::NodeOptions& options); - - /** - * @brief Get the list of predicates of the action - * @return the list of predicates - */ - const std::list> get_predicates() const; -}; - -template -Component::Component(const std::string& node_name, - const std::chrono::duration& period, - bool intra_process_comms) : Cell(node_name, period, intra_process_comms) { - this->on_init(); -} -}// namespace modulo::core diff --git a/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp b/source/modulo_core/include/modulo_core/EncodedState.hpp similarity index 78% rename from source/modulo_new_core/include/modulo_new_core/EncodedState.hpp rename to source/modulo_core/include/modulo_core/EncodedState.hpp index 46b565d54..e542ecfb5 100644 --- a/source/modulo_new_core/include/modulo_new_core/EncodedState.hpp +++ b/source/modulo_core/include/modulo_core/EncodedState.hpp @@ -2,7 +2,7 @@ #include -namespace modulo_new_core { +namespace modulo_core { /** * @typedef EncodedState @@ -10,4 +10,4 @@ namespace modulo_new_core { */ typedef std_msgs::msg::UInt8MultiArray EncodedState; -}// namespace modulo_new_core +}// namespace modulo_core diff --git a/source/modulo_core/include/modulo_core/Monitor.hpp b/source/modulo_core/include/modulo_core/Monitor.hpp deleted file mode 100644 index 3423bdf01..000000000 --- a/source/modulo_core/include/modulo_core/Monitor.hpp +++ /dev/null @@ -1,88 +0,0 @@ -#pragma once - -#include -#include - -#include "modulo_core/Cell.hpp" - -namespace modulo::core { -/** - * @class Monitor - * @brief Abstract class to define a Monitor - */ -class Monitor : public Cell { -private: - std::list monitored_node_;//< the list of nodes to monitor - -public: - /** - * @brief Constructor for the Monitor class - * @param node_name name of the ROS node - * @param period rate used by each publisher of the class - */ - template - explicit Monitor(const std::string& node_name, - const std::list& monitored_node, - const std::chrono::duration& period, - bool intra_process_comms = false); - - /** - * @brief Destructor - */ - ~Monitor(); - - /** - * @brief This function is called time the configure call - * is made from the lifecycle server. It is used to - * define behavior such as connecting to a database - * or resetting an history buffer. After being - * configured the node can be activated. - */ - virtual bool on_configure(); - - /** - * @brief This function is called time the activate call - * is made from the lifecycle server. It activates publishing - * and subsciptions and can be extended to start a recording - * or replay. - */ - virtual bool on_activate(); - - /** - * @brief This function is called time the deactivate call - * is made from the lifecycle server. It deactivates publishing - * and subsciptions and can be extended to stop a recording - * or a replay. - */ - virtual bool on_deactivate(); - - /** - * @brief This function is called time the cleanup call - * is made from the lifecycle server. It cleans the node - * and can be extended to close connections to a database - * or delete pointers. After cleanup a new configure call - * can be made. - */ - virtual bool on_cleanup(); - - /** - * @brief This function is called time the shutdown call - * is made from the lifecycle server. It terminates the node. - * Each elements needed to be cleaned before termination should - * be here. - */ - virtual bool on_shutdown(); - - /** - * @brief Function computing one step of calculation. It is called periodically in the run function. - */ - void step(); -}; - -template -Monitor::Monitor(const std::string& node_name, - const std::list& monitored_node, - const std::chrono::duration& period, - bool intra_process_comms) : Cell(node_name, period, intra_process_comms), monitored_node_(monitored_node) {} - -}// namespace modulo::core diff --git a/source/modulo_core/include/modulo_core/Recorder.hpp b/source/modulo_core/include/modulo_core/Recorder.hpp deleted file mode 100644 index cbb5640e9..000000000 --- a/source/modulo_core/include/modulo_core/Recorder.hpp +++ /dev/null @@ -1,127 +0,0 @@ -#pragma once - -#include - -#include "modulo_core/Cell.hpp" - -namespace modulo::core { -/** - * @class Recorder - * @brief Abstract class to define a Recorder - * A Recorder is used to listen on specific topics and record - * the received data. - */ -class Recorder : public Cell { -private: - std::chrono::time_point start_time;///< start time of current recording session, reset on activate - std::chrono::time_point end_time; ///< end time of current recording session, reset on deactivate - -public: - /** - * @brief Constructor for the Recorder class - * @param node_name name of the ROS node - * @param period rate used by each publisher of the class - */ - template - explicit Recorder(const std::string& node_name, const std::chrono::duration& period, bool intra_process_comms = false); - - /** - * @brief Destructor - */ - ~Recorder(); - - /** - * @brief Getter of the start_time attribute - * @return start_time - */ - const auto& get_start_time() const; - - /** - * @brief Getter of the end_time attribute - * @return end_time - */ - const auto& get_end_time() const; - - /** - * @brief This function is called time the configure call - * is made from the lifecycle server. It is used to - * define behavior such as connecting to a database - * or resetting an history buffer. After being - * configured the node can be activated. - */ - virtual bool on_configure(); - - /** - * @brief This function is called time the activate call - * is made from the lifecycle server. It activates publishing - * and subsciptions and can be extended to start a recording - * or replay. - */ - bool on_activate(); - - /** - * @brief This function is called time the deactivate call - * is made from the lifecycle server. It deactivates publishing - * and subsciptions and can be extended to stop a recording - * or a replay. - */ - bool on_deactivate(); - - /** - * @brief This function is called time the cleanup call - * is made from the lifecycle server. It cleans the node - * and can be extended to close connections to a database - * or delete pointers. After cleanup a new configure call - * can be made. - */ - virtual bool on_cleanup(); - - /** - * @brief This function is called time the shutdown call - * is made from the lifecycle server. It terminates the node. - * Each elements needed to be cleaned before termination should - * be here. - */ - virtual bool on_shutdown(); - - /** - * @brief Function computing one step of calculation. It is called periodically in the run function. - */ - void step(); - - /** - * @brief Generic function to record a state. This function is called in the step - * function for each subscription the recorder has - * @param state the state to be recorded - * @return true if the state was successfully recorded - */ - bool record(const state_representation::State& state) const; - - /** - * @brief Abstract function to record a CartesianState. This function needs to be - * implemeted in the derived class. - * @param state the state to be recorded - * @return true if the state was successfully recorded - */ - virtual bool record(const state_representation::CartesianState& state) const; - - /** - * @brief Abstract function to record a JointState. This function needs to be - * implemeted in the derived class. - * @param state the state to be recorded - * @return true if the state was successfully recorded - */ - virtual bool record(const state_representation::JointState& state) const; -}; - -template -Recorder::Recorder(const std::string& node_name, const std::chrono::duration& period, bool intra_process_comms) : Cell(node_name, period, intra_process_comms) {} - -inline const auto& Recorder::get_start_time() const { - return start_time; -} - -inline const auto& Recorder::get_end_time() const { - return end_time; -} -}// namespace modulo::core diff --git a/source/modulo_core/include/modulo_core/communication/CommunicationHandler.hpp b/source/modulo_core/include/modulo_core/communication/CommunicationHandler.hpp deleted file mode 100644 index 3c31dfb68..000000000 --- a/source/modulo_core/include/modulo_core/communication/CommunicationHandler.hpp +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -namespace modulo::core::communication { -/** - * @enum CommunicationType - * @brief enumeration of the possible type of communication interfaces - */ -enum class CommunicationType { - PUBLISHER, - SUBSCRIPTION, - TRANSFORMLISTENER, - CLIENT, -}; - -/** - * @class CommunicationHandler - * @brief Abstract class to define a Communication interface - */ -class CommunicationHandler { -private: - const CommunicationType type_; ///< type of the handler from the CommunicationType enumeration - std::chrono::nanoseconds timeout_;///< period before considered time out - -public: - /** - * @brief Constructor for a CommunicationHandler - * @param type the type of CommunicationHandler from the CommunicationType enumeration - * @param clock reference to the Cell clock - */ - explicit CommunicationHandler(const CommunicationType& type); - - /** - * @brief Constructor for a CommunicationHandler with a timeout - * @param type the type of CommunicationHandler from the CommunicationType enumeration - * @param timeout period before considered time out - * @param clock reference to the Cell clock - */ - template - explicit CommunicationHandler(const CommunicationType& type, - const std::chrono::duration& timeout); - - /** - * @brief Getter of the CommunicationType - * @return the type of the handler - */ - const CommunicationType& get_type(); - - /** - * @brief Getter of the timeout period - * @return the timeout period - */ - const std::chrono::nanoseconds& get_timeout() const; - - /** - * @brief Virtual function to activate a handler - */ - virtual void activate(); - - /** - * @brief Virtual function to deactivate a handler - */ - virtual void deactivate(); - - /** - * @brief Virtual function to check if the handler is timed out - */ - virtual void check_timeout(); -}; - -template -CommunicationHandler::CommunicationHandler(const CommunicationType& type, - const std::chrono::duration& timeout) : type_(type), - timeout_(timeout) {} - -inline const CommunicationType& CommunicationHandler::get_type() { - return this->type_; -} - -inline const std::chrono::nanoseconds& CommunicationHandler::get_timeout() const { - return this->timeout_; -} - -inline void CommunicationHandler::activate() {} - -inline void CommunicationHandler::deactivate() {} - -inline void CommunicationHandler::check_timeout() {} -}// namespace modulo::core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp b/source/modulo_core/include/modulo_core/communication/MessagePair.hpp similarity index 93% rename from source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp rename to source/modulo_core/include/modulo_core/communication/MessagePair.hpp index 968b16448..3c0213653 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePair.hpp +++ b/source/modulo_core/include/modulo_core/communication/MessagePair.hpp @@ -2,12 +2,12 @@ #include -#include "modulo_new_core/communication/MessagePairInterface.hpp" -#include "modulo_new_core/exceptions/NullPointerException.hpp" -#include "modulo_new_core/translators/message_readers.hpp" -#include "modulo_new_core/translators/message_writers.hpp" +#include "modulo_core/communication/MessagePairInterface.hpp" +#include "modulo_core/exceptions/NullPointerException.hpp" +#include "modulo_core/translators/message_readers.hpp" +#include "modulo_core/translators/message_writers.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { /** * @class MessagePair @@ -143,4 +143,4 @@ inline std::shared_ptr make_shared_message_pair(const std::shared_ptr& data, const std::shared_ptr& clock) { return std::make_shared>(data, clock); } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp b/source/modulo_core/include/modulo_core/communication/MessagePairInterface.hpp similarity index 94% rename from source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp rename to source/modulo_core/include/modulo_core/communication/MessagePairInterface.hpp index 91e8ba12e..c2193fdc8 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessagePairInterface.hpp +++ b/source/modulo_core/include/modulo_core/communication/MessagePairInterface.hpp @@ -2,11 +2,11 @@ #include -#include "modulo_new_core/communication/MessageType.hpp" -#include "modulo_new_core/exceptions/InvalidPointerCastException.hpp" -#include "modulo_new_core/exceptions/InvalidPointerException.hpp" +#include "modulo_core/communication/MessageType.hpp" +#include "modulo_core/exceptions/InvalidPointerCastException.hpp" +#include "modulo_core/exceptions/InvalidPointerException.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { // forward declaration of derived MessagePair class template @@ -117,4 +117,4 @@ template inline void MessagePairInterface::read(const MsgT& message) { this->template get_message_pair()->read_message(message); } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp b/source/modulo_core/include/modulo_core/communication/MessageType.hpp similarity index 71% rename from source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp rename to source/modulo_core/include/modulo_core/communication/MessageType.hpp index f0da9edbf..a367110d4 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/MessageType.hpp +++ b/source/modulo_core/include/modulo_core/communication/MessageType.hpp @@ -1,6 +1,6 @@ #pragma once -namespace modulo_new_core::communication { +namespace modulo_core::communication { /** * @brief Enum of all supported ROS message types for the MessagePairInterface @@ -9,4 +9,4 @@ namespace modulo_new_core::communication { enum class MessageType { BOOL, FLOAT64, FLOAT64_MULTI_ARRAY, INT32, STRING, ENCODED_STATE }; -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp b/source/modulo_core/include/modulo_core/communication/PublisherHandler.hpp similarity index 93% rename from source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp rename to source/modulo_core/include/modulo_core/communication/PublisherHandler.hpp index 59f20668c..dc386ea9c 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/PublisherHandler.hpp @@ -2,10 +2,10 @@ #include -#include "modulo_new_core/communication/PublisherInterface.hpp" -#include "modulo_new_core/exceptions/NullPointerException.hpp" +#include "modulo_core/communication/PublisherInterface.hpp" +#include "modulo_core/exceptions/NullPointerException.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { /** * @class PublisherHandler @@ -98,4 +98,4 @@ std::shared_ptr PublisherHandler::create_publish publisher_interface->set_message_pair(message_pair); return publisher_interface; } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp b/source/modulo_core/include/modulo_core/communication/PublisherInterface.hpp similarity index 94% rename from source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp rename to source/modulo_core/include/modulo_core/communication/PublisherInterface.hpp index 537c13d80..7647bb3c9 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherInterface.hpp +++ b/source/modulo_core/include/modulo_core/communication/PublisherInterface.hpp @@ -2,12 +2,12 @@ #include -#include "modulo_new_core/communication/MessagePair.hpp" -#include "modulo_new_core/communication/PublisherType.hpp" -#include "modulo_new_core/exceptions/InvalidPointerCastException.hpp" -#include "modulo_new_core/exceptions/InvalidPointerException.hpp" +#include "modulo_core/communication/MessagePair.hpp" +#include "modulo_core/communication/PublisherType.hpp" +#include "modulo_core/exceptions/InvalidPointerCastException.hpp" +#include "modulo_core/exceptions/InvalidPointerException.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { // forward declaration of derived Publisher class template @@ -141,4 +141,4 @@ inline std::shared_ptr> PublisherInterface::get_han } return publisher_ptr; } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp b/source/modulo_core/include/modulo_core/communication/PublisherType.hpp similarity index 68% rename from source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp rename to source/modulo_core/include/modulo_core/communication/PublisherType.hpp index 63ccf64b4..de74a74ea 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/PublisherType.hpp +++ b/source/modulo_core/include/modulo_core/communication/PublisherType.hpp @@ -1,6 +1,6 @@ #pragma once -namespace modulo_new_core::communication { +namespace modulo_core::communication { /** * @brief Enum of supported ROS publisher types for the PublisherInterface. @@ -9,4 +9,4 @@ namespace modulo_new_core::communication { enum class PublisherType { PUBLISHER, LIFECYCLE_PUBLISHER }; -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp similarity index 92% rename from source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp rename to source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp index d88801f73..095a5fb0b 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionHandler.hpp +++ b/source/modulo_core/include/modulo_core/communication/SubscriptionHandler.hpp @@ -1,9 +1,9 @@ #pragma once -#include "modulo_new_core/communication/SubscriptionInterface.hpp" -#include "modulo_new_core/exceptions/NullPointerException.hpp" +#include "modulo_core/communication/SubscriptionInterface.hpp" +#include "modulo_core/exceptions/NullPointerException.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { /** * @class SubscriptionHandler @@ -70,4 +70,4 @@ std::shared_ptr SubscriptionHandler::create_subscri this->set_subscription(subscription); return std::shared_ptr(this->shared_from_this()); } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp b/source/modulo_core/include/modulo_core/communication/SubscriptionInterface.hpp similarity index 93% rename from source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp rename to source/modulo_core/include/modulo_core/communication/SubscriptionInterface.hpp index 1f55dddaa..c2f43326d 100644 --- a/source/modulo_new_core/include/modulo_new_core/communication/SubscriptionInterface.hpp +++ b/source/modulo_core/include/modulo_core/communication/SubscriptionInterface.hpp @@ -2,11 +2,11 @@ #include -#include "modulo_new_core/communication/MessagePair.hpp" -#include "modulo_new_core/exceptions/InvalidPointerCastException.hpp" -#include "modulo_new_core/exceptions/InvalidPointerException.hpp" +#include "modulo_core/communication/MessagePair.hpp" +#include "modulo_core/exceptions/InvalidPointerCastException.hpp" +#include "modulo_core/exceptions/InvalidPointerException.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { // forward declaration of derived SubscriptionHandler class template @@ -88,4 +88,4 @@ inline std::shared_ptr> SubscriptionInterface::get_han } return subscription_ptr; } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp deleted file mode 100644 index cb115d157..000000000 --- a/source/modulo_core/include/modulo_core/communication/message_passing/MessagePassingHandler.hpp +++ /dev/null @@ -1,85 +0,0 @@ -#pragma once - -#include -#include -#include -#include - -#include "modulo_core/communication/CommunicationHandler.hpp" -#include "modulo_new_core/translators/message_readers.hpp" -#include "modulo_new_core/translators/message_writers.hpp" - -namespace modulo::core::communication { -/** - * @class MessagePassingHandler - * @brief Abstract class to define a MessagePassingHandler type communication interface - */ -class MessagePassingHandler : public CommunicationHandler { -private: - std::shared_ptr recipient_ = nullptr;///< recipient associated to the handler - bool asynchronous_; ///< indicate if the handler is asynchronous or not (for publishing mainly) - -public: - /** - * @brief Constructor for a MessagePassingHandler without timeout nor recipient - * @param type the type of MessagePassingHandler from the CommunicationType enumeration - */ - explicit MessagePassingHandler(const CommunicationType& type); - - /** - * @brief Constructor for a MessagePassingHandler without timeout - * @param type the type of MessagePassingHandler from the CommunicationType enumeration - * @param recipient recipient associated to the handler - */ - explicit MessagePassingHandler(const CommunicationType& type, - const std::shared_ptr& recipient); - - /** - * @brief Constructor for a MessagePassingHandler - * @param type the type of CommunicationHandler from the CommunicationType enumeration - * @param recipient recipient associated to the handler - * @param timeout period before considered time out - */ - template - explicit MessagePassingHandler(const CommunicationType& type, - const std::shared_ptr& recipient, - const std::chrono::duration& timeout); - - /** - * @brief Getter of the recipient - * @return the recipient - */ - const state_representation::State& get_recipient() const; - - /** - * @brief Getter of the recipient as a non const value - * @return the recipient - */ - state_representation::State& get_recipient(); - - /** - * @brief Getter of the asynchronous attribute - * @return bool true if asynchronous - */ - bool is_asynchronous() const; -}; - -template -MessagePassingHandler::MessagePassingHandler(const CommunicationType& type, - const std::shared_ptr& recipient, - const std::chrono::duration& timeout) : CommunicationHandler(type, timeout), - recipient_(recipient), - asynchronous_(true) {} - -inline const state_representation::State& MessagePassingHandler::get_recipient() const { - return *this->recipient_; -} - -inline state_representation::State& MessagePassingHandler::get_recipient() { - return *this->recipient_; -} - -inline bool MessagePassingHandler::is_asynchronous() const { - return this->asynchronous_; -} -}// namespace modulo::core::communication diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp deleted file mode 100644 index f63a88250..000000000 --- a/source/modulo_core/include/modulo_core/communication/message_passing/PublisherHandler.hpp +++ /dev/null @@ -1,144 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "modulo_core/communication/message_passing/MessagePassingHandler.hpp" - -namespace modulo::core::communication { -/** - * @class PublisherHandler - * @brief Class to define a publisher - * @tparam RecT the type of recipient (of state_representation::State base class) - * @tparam MsgT the type of associated ROS2 message - * - */ -template -class PublisherHandler : public MessagePassingHandler { -private: - std::shared_ptr> publisher_;///< reference to the ROS2 publisher - std::shared_ptr clock_; ///< reference to the Cell clock - std::shared_ptr timer_; ///< reference to the associated timer - bool activated_; ///< indicate if the publisher is activated or not - -protected: - /** - * @brief Function to publish the values of the recipient over the network - * @param recipient [description] - */ - void publish(const RecT& recipient); - -public: - /** - * @brief Constructor of a PublisherHandler - * @param recipient the recipent associated to the publisher - * @param clock reference to the Cell clock - */ - explicit PublisherHandler(const std::shared_ptr& recipient, - const std::shared_ptr& clock); - - /** - * @brief Constructor of a PublisherHandler without a recipient for one-shot publishing - * @param clock reference to the Cell clock - */ - explicit PublisherHandler(const std::shared_ptr& clock); - - /** - * @brief Function to publish periodically - */ - void publish_callback(); - - /** - * @brief Getter of the clock reference - * @return the clock reference - */ - const rclcpp::Clock& get_clock() const; - - /** - * @brief Getter of the clock as a non const reference - * @return the clock reference - */ - rclcpp::Clock& get_clock(); - - /** - * @brief Setter of the publisher reference - * @param publisher the reference to the publisher - */ - void set_publisher(const std::shared_ptr>& publisher); - - /** - * @brief Setter of the timer reference - * @param timer the reference to the timer - */ - void set_timer(const std::shared_ptr& timer); - - /** - * @brief Function to activate the publisher - */ - void activate(); - - /** - * @brief Function to deactivate the publisher - */ - void deactivate(); -}; - -template -PublisherHandler::PublisherHandler(const std::shared_ptr& recipient, - const std::shared_ptr& clock) : MessagePassingHandler(CommunicationType::PUBLISHER, - recipient), - clock_(clock), - activated_(false) {} - -template -PublisherHandler::PublisherHandler(const std::shared_ptr& clock) : MessagePassingHandler(CommunicationType::PUBLISHER), - clock_(clock), - activated_(false) {} - -template -void PublisherHandler::publish(const RecT& recipient) { - auto out_msg = std::make_unique(); - modulo_new_core::translators::write_message(*out_msg, recipient, this->get_clock().now()); - this->publisher_->publish(std::move(out_msg)); -} - -template -void PublisherHandler::publish_callback() { - if (this->is_asynchronous() && this->activated_ && !this->get_recipient().is_empty()) { - this->publish(static_cast(this->get_recipient())); - } -} - -template -inline const rclcpp::Clock& PublisherHandler::get_clock() const { - return *this->clock_; -} - -template -inline rclcpp::Clock& PublisherHandler::get_clock() { - return *this->clock_; -} - -template -inline void PublisherHandler::set_publisher(const std::shared_ptr>& publisher) { - this->publisher_ = std::move(publisher); -} - -template -inline void PublisherHandler::set_timer(const std::shared_ptr& timer) { - this->timer_ = std::move(timer); -} - -template -inline void PublisherHandler::activate() { - this->activated_ = true; - this->publisher_->on_activate(); -} - -template -inline void PublisherHandler::deactivate() { - this->activated_ = false; - this->publisher_->on_deactivate(); -} -}// namespace modulo::core::communication diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp deleted file mode 100644 index 8b49c2043..000000000 --- a/source/modulo_core/include/modulo_core/communication/message_passing/SubscriptionHandler.hpp +++ /dev/null @@ -1,87 +0,0 @@ -#pragma once - -#include "modulo_core/communication/message_passing/MessagePassingHandler.hpp" - -namespace modulo::core::communication { -/** - * @class SubscriptionHandler - * @brief Class to define a subscription - * @tparam RecT the type of recipient (of state_representation::State base class) - * @tparam MsgT the type of associated ROS2 message - * - */ -template -class SubscriptionHandler : public MessagePassingHandler { -private: - std::shared_ptr> subscription_;///< reference to the ROS2 subscription - -public: - /** - * @brief Constructor of a SubscriptionHandler - * @param recipient the recipient associated to the subscription - * @param timeout the period before timeout - */ - template - explicit SubscriptionHandler(const std::shared_ptr& recipient, - const std::chrono::duration& timeout); - - /** - * @brief Callback function to receive the message from the network - * @param msg reference to the ROS message - */ - void subscription_callback(const std::shared_ptr msg); - - /** - * @brief Setter of the subscription reference - * @param subscription the reference to the subscription - */ - void set_subscription(const std::shared_ptr>& subscription); - - /** - * @brief Function to activate the subscription - */ - void activate(); - - /** - * @brief Function to deactivate the subscription - */ - void deactivate(); - - /** - * @brief Function to check if the subscription is timed out - */ - void check_timeout(); -}; - -template -template -SubscriptionHandler::SubscriptionHandler(const std::shared_ptr& recipient, - const std::chrono::duration& timeout) : MessagePassingHandler(CommunicationType::SUBSCRIPTION, - recipient, - timeout) {} - -template -void SubscriptionHandler::subscription_callback(const std::shared_ptr msg) { - modulo_new_core::translators::read_message(static_cast(this->get_recipient()), *msg); -} - -template -inline void SubscriptionHandler::set_subscription(const std::shared_ptr>& subscription) { - this->subscription_ = std::move(subscription); -} - -template -inline void SubscriptionHandler::activate() {} - -template -inline void SubscriptionHandler::deactivate() {} - -template -inline void SubscriptionHandler::check_timeout() { - if (this->get_timeout() != std::chrono::nanoseconds::zero()) { - if (this->get_recipient().is_deprecated(this->get_timeout())) { - this->get_recipient().initialize(); - } - } -} -}// namespace modulo::core::communication diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/TransformBroadcasterHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/TransformBroadcasterHandler.hpp deleted file mode 100644 index 33f905623..000000000 --- a/source/modulo_core/include/modulo_core/communication/message_passing/TransformBroadcasterHandler.hpp +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once - -#include -#include - -#include "modulo_core/communication/message_passing/PublisherHandler.hpp" - -namespace modulo::core::communication { -/** - * @class TransformBroadcasterHandler - * @brief Class to define a transform broadcaster - */ -class TransformBroadcasterHandler : public PublisherHandler { -public: - /** - * @brief Constructor for and asychronous TransformBroadcaster handler - * @param recipient the associated recipient to publish - * @param clock reference to the Cell clock - */ - explicit TransformBroadcasterHandler(const std::shared_ptr& recipient, - const std::shared_ptr& clock); - - /** - * @brief Constructor for TransformBroadcaster handler without an associated recipient - * @param clock reference to the Cell clock - */ - explicit TransformBroadcasterHandler(const std::shared_ptr& clock); - - /** - * @brief Function to send a transform over the network - * @param transform the transformation to send - */ - void send_transform(const state_representation::CartesianState& transform); -}; - -inline void TransformBroadcasterHandler::send_transform(const state_representation::CartesianState& transform) { - this->publish(transform); -} -}// namespace modulo::core::communication diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp deleted file mode 100644 index 99db8377d..000000000 --- a/source/modulo_core/include/modulo_core/communication/message_passing/TransformListenerHandler.hpp +++ /dev/null @@ -1,68 +0,0 @@ -#pragma once - -#include -#include -#include - -#include "modulo_core/communication/message_passing/MessagePassingHandler.hpp" - -namespace modulo::core::communication { -/** - * @class TransformListenerHandler - * @brief Class to define a transform listener - */ -class TransformListenerHandler : public MessagePassingHandler { -private: - tf2_ros::Buffer buffer_; ///< tf2 ROS buffer to read transformation from - std::unique_ptr tf_listener_;///< reference to the ROS2 transform listener - -public: - /** - * @brief Constructor for an asychronous TransformListener - * @param recipient the associated recipient to store received transforms - * @param timeout period before timeout - * @param clock reference to the Cell clock - */ - template - explicit TransformListenerHandler(const std::shared_ptr& recipient, - const std::chrono::duration& timeout, - const std::shared_ptr& clock); - - /** - * @brief Constructor for a TransformListener without a recipient - * @param timeout period before timeout - * @param clock reference to the Cell clock - */ - template - explicit TransformListenerHandler(const std::chrono::duration& timeout, - const std::shared_ptr& clock); - - /** - * @brief Function to look up a transform over the network - * @param frame_name name of the frame associated to the transform - * @param reference_frame name of its desired reference frame - * @return the transform as a CartesianPose - */ - const state_representation::CartesianPose lookup_transform(const std::string& frame_name, - const std::string& reference_frame) const; -}; - -template -TransformListenerHandler::TransformListenerHandler(const std::shared_ptr& recipient, - const std::chrono::duration& timeout, - const std::shared_ptr& clock) : MessagePassingHandler(CommunicationType::TRANSFORMLISTENER, - recipient, - timeout), - buffer_(clock) { - this->tf_listener_ = std::make_unique(buffer_); -} - -template -TransformListenerHandler::TransformListenerHandler(const std::chrono::duration& timeout, - const std::shared_ptr& clock) : MessagePassingHandler(CommunicationType::TRANSFORMLISTENER, - std::make_shared(), - timeout), - buffer_(clock) { - this->tf_listener_ = std::make_unique(buffer_); -} -}// namespace modulo::core::communication diff --git a/source/modulo_core/include/modulo_core/communication/service_client/ClientHandler.hpp b/source/modulo_core/include/modulo_core/communication/service_client/ClientHandler.hpp deleted file mode 100644 index bff6d848d..000000000 --- a/source/modulo_core/include/modulo_core/communication/service_client/ClientHandler.hpp +++ /dev/null @@ -1,107 +0,0 @@ -#pragma once - -#include "modulo_core/communication/CommunicationHandler.hpp" -#include "modulo_core/exceptions/CommunicationTimeoutException.hpp" -#include "modulo_core/exceptions/ServiceNotAvailableException.hpp" - -namespace modulo::core::communication { -/** - * @class ClientHandler - * @brief Class to define a client - * @tparam SrvT the type of service - */ -template -class ClientHandler : public CommunicationHandler { -private: - std::shared_ptr> client_;///< reference to the ROS2 client - -public: - /** - * @brief Constructor of a ClientHandler - * @param timeout period before timeout - */ - template - explicit ClientHandler(const std::chrono::duration& timeout); - - /** - * @brief Setter of the publisher reference - * @param publisher the reference to the publisher - */ - void set_client(const std::shared_ptr>& client); - - /** - * @brief Static function to wait for the result of a request - * @tparam WaitTimeT a period to wait in std::chrono - * @param future_result the future result as a shared_future - * @param time_to_wait the maximum time to wait for the result - * @return the status of the waiting if the waited time is greater than the allowed time the status is timeout - */ - template - static std::future_status wait_for_result(const std::shared_future>& future_result, - const std::chrono::duration& time_to_wait); - - /** - * @brief Send a request to the server asymchronously withtout waiting for the answer - * @param request the request - * @return the response as a future pointer - */ - std::shared_future> send_request(const std::shared_ptr& request) const; - - /** - * @brief Send a request to the server asymchronously and wait for the answer - * @param request the request - * @return the response as a shared pointer - */ - std::shared_ptr send_blocking_request(const std::shared_ptr& request) const; -}; - -template -template -ClientHandler::ClientHandler(const std::chrono::duration& timeout) : CommunicationHandler(CommunicationType::CLIENT, - timeout) {} - -template -inline void ClientHandler::set_client(const std::shared_ptr>& client) { - this->client_ = std::move(client); - if (!this->client_->wait_for_service(this->get_timeout())) { - throw exceptions::ServiceNotAvailableException(this->client_->get_service_name()); - } -} - -template -template -std::future_status ClientHandler::wait_for_result(const std::shared_future>& future_result, - const std::chrono::duration& time_to_wait) { - auto end = std::chrono::steady_clock::now() + time_to_wait; - std::chrono::milliseconds wait_period(1); - std::future_status status = std::future_status::timeout; - do { - auto now = std::chrono::steady_clock::now(); - auto time_left = end - now; - if (time_left <= std::chrono::seconds(0)) break; - status = future_result.wait_for((time_left < wait_period) ? time_left : wait_period); - } while (rclcpp::ok() && status != std::future_status::ready); - return status; -} - -template -std::shared_future> ClientHandler::send_request(const std::shared_ptr& request) const { - if (!this->client_->wait_for_service(this->get_timeout())) { - throw exceptions::ServiceNotAvailableException(this->client_->get_service_name()); - } - return this->client_->async_send_request(request); -} - -template -std::shared_ptr ClientHandler::send_blocking_request(const std::shared_ptr& request) const { - if (!this->client_->wait_for_service(this->get_timeout())) { - throw exceptions::ServiceNotAvailableException(this->client_->get_service_name()); - } - auto response = this->client_->async_send_request(request); - std::future_status status = ClientHandler::wait_for_result(response, this->get_timeout()); - if (status != std::future_status::ready) { - throw exceptions::CommunicationTimeoutException(this->client_->get_service_name()); - } - return response.get(); -} -}// namespace modulo::core::communication diff --git a/source/modulo_core/include/modulo_core/exceptions/CommunicationTimeoutException.hpp b/source/modulo_core/include/modulo_core/exceptions/CommunicationTimeoutException.hpp deleted file mode 100644 index d0b413c61..000000000 --- a/source/modulo_core/include/modulo_core/exceptions/CommunicationTimeoutException.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo::core::exceptions { -class CommunicationTimeoutException : public std::runtime_error { -public: - explicit CommunicationTimeoutException(const std::string& service_name) : - runtime_error("Communication with service " + service_name + " timed out"){}; -}; -}// namespace modulo::core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp b/source/modulo_core/include/modulo_core/exceptions/IncompatibleParameterException.hpp similarity index 89% rename from source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp rename to source/modulo_core/include/modulo_core/exceptions/IncompatibleParameterException.hpp index 8305bdcd4..3c6aa19f7 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/IncompatibleParameterException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/IncompatibleParameterException.hpp @@ -2,7 +2,7 @@ #include -namespace modulo_new_core::exceptions { +namespace modulo_core::exceptions { /** * @class IncompatibleParameterException @@ -15,4 +15,4 @@ class IncompatibleParameterException : public state_representation::exceptions:: explicit IncompatibleParameterException(const std::string& msg) : state_representation::exceptions::InvalidParameterException(msg) {}; }; -}// namespace modulo_new_core::exceptions +}// namespace modulo_core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerCastException.hpp similarity index 85% rename from source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp rename to source/modulo_core/include/modulo_core/exceptions/InvalidPointerCastException.hpp index f69efdba4..51dd823c6 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerCastException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerCastException.hpp @@ -3,7 +3,7 @@ #include #include -namespace modulo_new_core::exceptions { +namespace modulo_core::exceptions { /** * @class InvalidPointerCastException @@ -14,4 +14,4 @@ class InvalidPointerCastException : public std::runtime_error { public: explicit InvalidPointerCastException(const std::string& msg) : std::runtime_error(msg) {}; }; -}// namespace modulo_new_core::exceptions +}// namespace modulo_core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerException.hpp similarity index 84% rename from source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp rename to source/modulo_core/include/modulo_core/exceptions/InvalidPointerException.hpp index 7cbc87e0e..02d00fdce 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/InvalidPointerException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerException.hpp @@ -3,7 +3,7 @@ #include #include -namespace modulo_new_core::exceptions { +namespace modulo_core::exceptions { /** * @class InvalidPointerException @@ -14,4 +14,4 @@ class InvalidPointerException : public std::runtime_error { public: explicit InvalidPointerException(const std::string& msg) : std::runtime_error(msg) {}; }; -}// namespace modulo_new_core::exceptions +}// namespace modulo_core::exceptions diff --git a/source/modulo_core/include/modulo_core/exceptions/NotImplementedExcpetion.hpp b/source/modulo_core/include/modulo_core/exceptions/NotImplementedExcpetion.hpp deleted file mode 100644 index 80cb42182..000000000 --- a/source/modulo_core/include/modulo_core/exceptions/NotImplementedExcpetion.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo::core::exceptions { -class NotImplementedException : public std::logic_error { -public: - explicit NotImplementedException(const std::string& msg) : logic_error(msg){}; -}; -}// namespace modulo::core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp b/source/modulo_core/include/modulo_core/exceptions/NullPointerException.hpp similarity index 84% rename from source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp rename to source/modulo_core/include/modulo_core/exceptions/NullPointerException.hpp index 326df9628..c33d8ba7b 100644 --- a/source/modulo_new_core/include/modulo_new_core/exceptions/NullPointerException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/NullPointerException.hpp @@ -3,7 +3,7 @@ #include #include -namespace modulo_new_core::exceptions { +namespace modulo_core::exceptions { /** * @class NullPointerException @@ -14,4 +14,4 @@ class NullPointerException : public std::runtime_error { public: explicit NullPointerException(const std::string& msg) : std::runtime_error(msg) {}; }; -}// namespace modulo_new_core::exceptions +}// namespace modulo_core::exceptions diff --git a/source/modulo_core/include/modulo_core/exceptions/ParameterNotFoundException.hpp b/source/modulo_core/include/modulo_core/exceptions/ParameterNotFoundException.hpp deleted file mode 100644 index 2914588ad..000000000 --- a/source/modulo_core/include/modulo_core/exceptions/ParameterNotFoundException.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo::core::exceptions { -class ParameterNotFoundException : public std::runtime_error { -public: - explicit ParameterNotFoundException(const std::string& parameter_name) : - runtime_error("Parameter " + parameter_name + " not found in the list of parameters"){}; -}; -}// namespace modulo::core::exceptions diff --git a/source/modulo_core/include/modulo_core/exceptions/PredicateAlreadyRegisteredException.hpp b/source/modulo_core/include/modulo_core/exceptions/PredicateAlreadyRegisteredException.hpp deleted file mode 100644 index dea673493..000000000 --- a/source/modulo_core/include/modulo_core/exceptions/PredicateAlreadyRegisteredException.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo::core::exceptions { -class PredicateAlreadyRegisteredException : public std::runtime_error { -public: - explicit PredicateAlreadyRegisteredException(const std::string& predicate_name) : runtime_error("Predicate " + predicate_name + " is already registered in the list of predicates"){}; -}; -}// namespace modulo::core::exceptions diff --git a/source/modulo_core/include/modulo_core/exceptions/PredicateNotFoundException.hpp b/source/modulo_core/include/modulo_core/exceptions/PredicateNotFoundException.hpp deleted file mode 100644 index 3c7277ab9..000000000 --- a/source/modulo_core/include/modulo_core/exceptions/PredicateNotFoundException.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo::core::exceptions { -class PredicateNotFoundException : public std::runtime_error { -public: - explicit PredicateNotFoundException(const std::string& predicate_name) : runtime_error("Predicate " + predicate_name + " not found in the list of parameters"){}; -}; -}// namespace modulo::core::exceptions diff --git a/source/modulo_core/include/modulo_core/exceptions/ServiceNotAvailableException.hpp b/source/modulo_core/include/modulo_core/exceptions/ServiceNotAvailableException.hpp deleted file mode 100644 index abf310396..000000000 --- a/source/modulo_core/include/modulo_core/exceptions/ServiceNotAvailableException.hpp +++ /dev/null @@ -1,12 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo::core::exceptions { -class ServiceNotAvailableException : public std::runtime_error { -public: - explicit ServiceNotAvailableException(const std::string& service_name) : - runtime_error("Service " + service_name + " is not available"){}; -}; -}// namespace modulo::core::exceptions diff --git a/source/modulo_core/include/modulo_core/exceptions/UnconfiguredNodeException.hpp b/source/modulo_core/include/modulo_core/exceptions/UnconfiguredNodeException.hpp deleted file mode 100644 index bd2284ebb..000000000 --- a/source/modulo_core/include/modulo_core/exceptions/UnconfiguredNodeException.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once - -#include -#include - -namespace modulo::core::exceptions { -class UnconfiguredNodeException : public std::runtime_error { -public: - explicit UnconfiguredNodeException(const std::string& msg) : runtime_error(msg){}; -}; -}// namespace modulo::core::exceptions diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp b/source/modulo_core/include/modulo_core/translators/message_readers.hpp similarity index 99% rename from source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp rename to source/modulo_core/include/modulo_core/translators/message_readers.hpp index d03b556b7..66478e44c 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_readers.hpp +++ b/source/modulo_core/include/modulo_core/translators/message_readers.hpp @@ -18,9 +18,9 @@ #include #include -#include "modulo_new_core/EncodedState.hpp" +#include "modulo_core/EncodedState.hpp" -namespace modulo_new_core::translators { +namespace modulo_core::translators { /** * @brief Convert a ROS geometry_msgs::msg::Accel to a CartesianState @@ -314,4 +314,4 @@ inline void read_message(std::shared_ptr& state, co throw std::invalid_argument("The StateType contained by state " + new_state->get_name() + " is unsupported."); } } -}// namespace modulo_new_core::translators +}// namespace modulo_core::translators diff --git a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp b/source/modulo_core/include/modulo_core/translators/message_writers.hpp similarity index 98% rename from source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp rename to source/modulo_core/include/modulo_core/translators/message_writers.hpp index 716891745..c88219aa0 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/message_writers.hpp +++ b/source/modulo_core/include/modulo_core/translators/message_writers.hpp @@ -19,9 +19,9 @@ #include #include -#include "modulo_new_core/EncodedState.hpp" +#include "modulo_core/EncodedState.hpp" -namespace modulo_new_core::translators { +namespace modulo_core::translators { /** * @brief Convert a CartesianState to a ROS geometry_msgs::msg::Accel @@ -212,4 +212,4 @@ inline void write_message(EncodedState& message, const T& state, const rclcpp::T std::string tmp = clproto::encode(state); message.data = std::vector(tmp.begin(), tmp.end()); } -}// namespace modulo_new_core::translators +}// namespace modulo_core::translators diff --git a/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp b/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp similarity index 97% rename from source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp rename to source/modulo_core/include/modulo_core/translators/parameter_translators.hpp index b79225dcc..1dbaf40c3 100644 --- a/source/modulo_new_core/include/modulo_new_core/translators/parameter_translators.hpp +++ b/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp @@ -3,7 +3,7 @@ #include #include -namespace modulo_new_core::translators { +namespace modulo_core::translators { /** * @brief Copy the value of one parameter interface into another. @@ -60,4 +60,4 @@ std::shared_ptr read_parameter_const( void read_parameter( const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter ); -}// namespace modulo_new_core::exceptions +}// namespace modulo_core::exceptions diff --git a/source/modulo_core/include/modulo_core/utilities/utilities.hpp b/source/modulo_core/include/modulo_core/utilities/utilities.hpp deleted file mode 100644 index aa782e0fb..000000000 --- a/source/modulo_core/include/modulo_core/utilities/utilities.hpp +++ /dev/null @@ -1,72 +0,0 @@ -#pragma once - -#include - -#include - - -namespace modulo::core::utilities { - -/** - * @brief Parse a string argument value from an argument list given a pattern prefix. - * @param args a vector of string arguments - * @param pattern the prefix pattern to find and strip - * @param result the default argument value that is overwritten by reference if the given pattern is found - * @return the value of the resultant string - */ -static std::string parse_string_argument(const std::vector& args, const std::string& pattern, std::string& result) { - for (const auto& arg : args) { - std::string::size_type index = arg.find(pattern); - if (index != std::string::npos) { - result = arg; - result.erase(index, pattern.length()); - break; - } - } - return result; -} - -/** - * @brief Parse a string node name from NodeOptions arguments. - * @param options the NodeOptions structure to parse - * @param fallback the default name if the NodeOptions structure cannot be parsed - * @return the parsed node name or the fallback name - */ -std::string parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback="") { - std::string node_name(fallback); - const std::string pattern("__node:="); - return parse_string_argument(options.arguments(), pattern, node_name); -} - -/** - * @brief Parse a string node namespace from NodeOptions arguments. - * @param options the NodeOptions structure to parse - * @param fallback the default namespace if the NodeOptions structure cannot be parsed - * @return the parsed node namespace or the fallback namespace - */ -std::string parse_node_namespace(const rclcpp::NodeOptions& options, const std::string& fallback="") { - std::string node_namespace(fallback); - const std::string pattern("__ns:="); - return parse_string_argument(options.arguments(), pattern, node_namespace); -} - -/** - * @brief Parse a period in seconds from NodeOptions parameters. - * @details The parameter must have the name "period" - * and be interpretable as a value in seconds of type double - * @param options the NodeOptions structure to parse - * @param default_period the default period in seconds - * @return the parsed period or the default period as a chrono duration - */ -std::chrono::nanoseconds parse_period(const rclcpp::NodeOptions& options, double default_period = 0.001) { - double period = default_period; - for (auto& param : options.parameter_overrides()) { - if (param.get_name() == "period") { - period = param.as_double(); - break; - } - } - return std::chrono::nanoseconds(static_cast(period * 1e9)); -} - -} \ No newline at end of file diff --git a/source/modulo_core/launch/example_control_loop.sh b/source/modulo_core/launch/example_control_loop.sh deleted file mode 100755 index 6bb3adc83..000000000 --- a/source/modulo_core/launch/example_control_loop.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash -ros2 lifecycle set /robot_interface configure -ros2 lifecycle set /visualizer configure -ros2 lifecycle set /motion_generator configure - -ros2 lifecycle set /robot_interface activate -ros2 lifecycle set /visualizer activate -ros2 lifecycle set /motion_generator activate \ No newline at end of file diff --git a/source/modulo_core/launch/example_moving_reference_frame.sh b/source/modulo_core/launch/example_moving_reference_frame.sh deleted file mode 100644 index 15be8f478..000000000 --- a/source/modulo_core/launch/example_moving_reference_frame.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/bin/bash -ros2 lifecycle set /simulated_object configure -ros2 lifecycle set /robot_interface configure -ros2 lifecycle set /visualizer configure -ros2 lifecycle set /motion_generator configure - -ros2 lifecycle set /simulated_object activate -ros2 lifecycle set /robot_interface activate -ros2 lifecycle set /visualizer activate -ros2 lifecycle set /motion_generator activate \ No newline at end of file diff --git a/source/modulo_new_core/modulo_new_core/__init__.py b/source/modulo_core/modulo_core/__init__.py similarity index 100% rename from source/modulo_new_core/modulo_new_core/__init__.py rename to source/modulo_core/modulo_core/__init__.py diff --git a/source/modulo_new_core/modulo_new_core/encoded_state.py b/source/modulo_core/modulo_core/encoded_state.py similarity index 100% rename from source/modulo_new_core/modulo_new_core/encoded_state.py rename to source/modulo_core/modulo_core/encoded_state.py diff --git a/source/modulo_new_core/modulo_new_core/translators/__init__.py b/source/modulo_core/modulo_core/translators/__init__.py similarity index 100% rename from source/modulo_new_core/modulo_new_core/translators/__init__.py rename to source/modulo_core/modulo_core/translators/__init__.py diff --git a/source/modulo_new_core/modulo_new_core/translators/message_readers.py b/source/modulo_core/modulo_core/translators/message_readers.py similarity index 98% rename from source/modulo_new_core/modulo_new_core/translators/message_readers.py rename to source/modulo_core/modulo_core/translators/message_readers.py index 32a52b175..9c68a8313 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_readers.py +++ b/source/modulo_core/modulo_core/translators/message_readers.py @@ -3,7 +3,7 @@ import clproto import geometry_msgs.msg as geometry import state_representation as sr -from modulo_new_core.encoded_state import EncodedState +from modulo_core.encoded_state import EncodedState from sensor_msgs.msg import JointState DataT = TypeVar('DataT') diff --git a/source/modulo_new_core/modulo_new_core/translators/message_writers.py b/source/modulo_core/modulo_core/translators/message_writers.py similarity index 98% rename from source/modulo_new_core/modulo_new_core/translators/message_writers.py rename to source/modulo_core/modulo_core/translators/message_writers.py index 45aff299e..94d723170 100644 --- a/source/modulo_new_core/modulo_new_core/translators/message_writers.py +++ b/source/modulo_core/modulo_core/translators/message_writers.py @@ -5,7 +5,7 @@ import numpy as np import rclpy.time import state_representation as sr -from modulo_new_core.encoded_state import EncodedState +from modulo_core.encoded_state import EncodedState from sensor_msgs.msg import JointState DataT = TypeVar('DataT') diff --git a/source/modulo_new_core/modulo_new_core/translators/parameter_translators.py b/source/modulo_core/modulo_core/translators/parameter_translators.py similarity index 100% rename from source/modulo_new_core/modulo_new_core/translators/parameter_translators.py rename to source/modulo_core/modulo_core/translators/parameter_translators.py diff --git a/source/modulo_core/package.xml b/source/modulo_core/package.xml index 110a3d28e..c87ce2567 100644 --- a/source/modulo_core/package.xml +++ b/source/modulo_core/package.xml @@ -1,25 +1,23 @@ - + modulo_core - 1.1.0 - TODO: Package description - buschbapti + 0.0.1 + TODO + Baptiste Busch + Enrico Eberhard + Dominic Reber TODO: License declaration ament_cmake_auto + ament_cmake_python rclcpp - rclcpp_lifecycle - std_msgs + rclpy geometry_msgs - nav_msgs - trajectory_msgs sensor_msgs - lifecycle_msgs + std_msgs tf2_msgs - tf2_ros - modulo_new_core ament_lint_auto ament_lint_common diff --git a/source/modulo_new_core/setup.cfg b/source/modulo_core/setup.cfg similarity index 100% rename from source/modulo_new_core/setup.cfg rename to source/modulo_core/setup.cfg diff --git a/source/modulo_core/src/Cell.cpp b/source/modulo_core/src/Cell.cpp deleted file mode 100644 index 3d12b0fea..000000000 --- a/source/modulo_core/src/Cell.cpp +++ /dev/null @@ -1,702 +0,0 @@ -#include "modulo_core/Cell.hpp" - -#include - -#include -#include -#include -#include - -#include "modulo_core/utilities/utilities.hpp" -#include "modulo_core/exceptions/UnconfiguredNodeException.hpp" - -using namespace rclcpp_lifecycle::node_interfaces; - -namespace modulo::core { - -Cell::Cell(const rclcpp::NodeOptions& options) : - rclcpp_lifecycle::LifecycleNode(utilities::parse_node_name(options, "Cell"), options), - configured_(false), - active_(false), - shutdown_(false), - period_(utilities::parse_period(options)) { - // add the update parameters call - this->add_daemon([this] { this->update_parameters(); }, this->period_); -} - -Cell::~Cell() { - RCUTILS_LOG_INFO_NAMED(get_name(), "Shutting down the node before destruction"); - if (this->on_shutdown(this->get_current_state()) != LifecycleNodeInterface::CallbackReturn::SUCCESS) { - RCUTILS_LOG_ERROR_NAMED(get_name(), "Error during the shutdown process, shutting down anyway."); - } -} - -void Cell::reset() { - this->active_ = false; - this->configured_ = false; - // clear the map of handlers only for non always activated ones - for (auto it = this->handlers_.cbegin(); it != this->handlers_.cend(); /* no increment */) { - if (!it->second.second) { - it = this->handlers_.erase(it); - } else { - ++it; - } - } - this->timers_.clear(); -} - -template -void Cell::add_parameter(const std::shared_ptr>& parameter, const std::string& prefix) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - this->declare_parameter(parameter->get_name(), parameter->get_value()); -} - -template<> -void Cell::add_parameter( - const std::shared_ptr>& parameter, const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - this->declare_parameter(parameter->get_name(), static_cast(parameter->get_value())); -} - -template<> -void Cell::add_parameter>( - const std::shared_ptr>>& parameter, const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - this->declare_parameter( - parameter->get_name(), std::vector(parameter->get_value().begin(), parameter->get_value().end())); -} - -template void Cell::add_parameter(const std::shared_ptr>& parameter, const std::string& prefix); - -template void Cell::add_parameter>(const std::shared_ptr>>& parameter, const std::string& prefix); - -template void Cell::add_parameter(const std::shared_ptr>& parameter, const std::string& prefix); - -template void Cell::add_parameter>(const std::shared_ptr>>& parameter, const std::string& prefix); - -template void Cell::add_parameter(const std::shared_ptr>& parameter, const std::string& prefix); - -template void Cell::add_parameter>(const std::shared_ptr>>& parameter, const std::string& prefix); - -template<> -void Cell::add_parameter( - const std::shared_ptr>& parameter, - const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - this->declare_parameter>(parameter->get_name(), parameter->get_value().to_std_vector()); -} - -template<> -void Cell::add_parameter( - const std::shared_ptr>& parameter, - const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - state_representation::CartesianPose value(parameter->get_value()); - this->declare_parameter>(parameter->get_name(), value.to_std_vector()); -} - -template<> -void Cell::add_parameter( - const std::shared_ptr>& parameter, - const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - this->declare_parameter>(parameter->get_name(), parameter->get_value().to_std_vector()); -} - -template<> -void Cell::add_parameter( - const std::shared_ptr>& parameter, - const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - state_representation::JointPositions value(parameter->get_value()); - this->declare_parameter>(parameter->get_name(), value.to_std_vector()); -} - -template<> -void Cell::add_parameter( - const std::shared_ptr>& parameter, - const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - this->declare_parameter>(parameter->get_name(), parameter->get_value().to_std_vector()); -} - -template<> -void Cell::add_parameter( - const std::shared_ptr>& parameter, const std::string& prefix -) { - std::string tprefix = (!prefix.empty()) ? prefix + "_" : ""; - parameter->set_name(tprefix + parameter->get_name()); - this->parameters_.insert(std::make_pair(parameter->get_name(), parameter)); - std::vector value = - std::vector(parameter->get_value().data(), parameter->get_value().data() + parameter->get_value().size()); - this->declare_parameter>(parameter->get_name(), value); -} - -void Cell::add_parameters( - const std::list>& parameters, const std::string& prefix -) { - using namespace state_representation; - using namespace state_representation::exceptions; - for (auto& param: parameters) { - switch (param->get_parameter_type()) { - case ParameterType::INT: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case ParameterType::INT_ARRAY: { - this->add_parameter(std::static_pointer_cast>>(param), prefix); - break; - } - - case ParameterType::DOUBLE: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case ParameterType::DOUBLE_ARRAY: { - this->add_parameter(std::static_pointer_cast>>(param), prefix); - break; - } - - case ParameterType::BOOL: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case ParameterType::BOOL_ARRAY: { - this->add_parameter(std::static_pointer_cast>>(param), prefix); - break; - } - - case ParameterType::STRING: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - case ParameterType::STRING_ARRAY: { - this->add_parameter(std::static_pointer_cast>>(param), prefix); - break; - } - - case ParameterType::STATE: { - switch (param->get_parameter_state_type()) { - case StateType::CARTESIAN_STATE: - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - case StateType::CARTESIAN_POSE: - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - case StateType::JOINT_STATE: - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - case StateType::JOINT_POSITIONS: - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - case StateType::GEOMETRY_ELLIPSOID: - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - default: { - throw UnrecognizedParameterTypeException("The Parameter state type is not available"); - } - } - break; - } - - case ParameterType::MATRIX: { - this->add_parameter(std::static_pointer_cast>(param), prefix); - break; - } - - default: { - throw UnrecognizedParameterTypeException("The Parameter type is not available"); - } - } - } -} - -template -void Cell::set_parameter_value(const std::string& parameter_name, const T& value) { - this->set_parameter(rclcpp::Parameter(parameter_name, value)); -} - -template void Cell::set_parameter_value(const std::string& parameter_name, const int& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const std::vector& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const double& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const std::vector& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const bool& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const std::vector& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const char* const& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const std::string& value); - -template void Cell::set_parameter_value(const std::string& parameter_name, const std::vector& value); - -template<> -void Cell::set_parameter_value(const std::string& parameter_name, const state_representation::CartesianState& value) { - std::vector vector_value = value.to_std_vector(); - this->set_parameter_value>(parameter_name, vector_value); -} - -template<> -void Cell::set_parameter_value(const std::string& parameter_name, const state_representation::CartesianPose& value) { - std::vector vector_value = value.to_std_vector(); - this->set_parameter_value>(parameter_name, vector_value); -} - -template<> -void Cell::set_parameter_value(const std::string& parameter_name, const state_representation::JointState& value) { - std::vector vector_value = value.to_std_vector(); - this->set_parameter_value>(parameter_name, vector_value); -} - -template<> -void Cell::set_parameter_value(const std::string& parameter_name, const state_representation::JointPositions& value) { - std::vector vector_value = value.to_std_vector(); - this->set_parameter_value>(parameter_name, vector_value); -} - -template<> -void Cell::set_parameter_value(const std::string& parameter_name, const state_representation::Ellipsoid& value) { - std::vector vector_value = value.to_std_vector(); - this->set_parameter_value>(parameter_name, vector_value); -} - -template<> -void Cell::set_parameter_value(const std::string& parameter_name, const Eigen::MatrixXd& value) { - std::vector vector_value = std::vector(value.data(), value.data() + value.size()); - this->set_parameter_value>(parameter_name, vector_value); -} - -void Cell::set_parameter_value(const std::shared_ptr& parameter) { - using namespace state_representation; - using namespace state_representation::exceptions; - switch (parameter->get_parameter_type()) { - case ParameterType::INT: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case ParameterType::INT_ARRAY: { - this->set_parameter_value(std::static_pointer_cast>>(parameter)); - break; - } - - case ParameterType::DOUBLE: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case ParameterType::DOUBLE_ARRAY: { - this->set_parameter_value(std::static_pointer_cast>>(parameter)); - break; - } - - case ParameterType::BOOL: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case ParameterType::BOOL_ARRAY: { - this->set_parameter_value(std::static_pointer_cast>>(parameter)); - break; - } - - case ParameterType::STRING: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - case ParameterType::STRING_ARRAY: { - this->set_parameter_value(std::static_pointer_cast>>(parameter)); - break; - } - - case ParameterType::STATE: { - switch (parameter->get_parameter_state_type()) { - case StateType::CARTESIAN_STATE: - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - case StateType::CARTESIAN_POSE: - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - case StateType::JOINT_STATE: - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - case StateType::JOINT_POSITIONS: - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - case StateType::GEOMETRY_ELLIPSOID: - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - default: { - throw UnrecognizedParameterTypeException("The Parameter state type is not available"); - } - } - break; - } - - case ParameterType::MATRIX: { - this->set_parameter_value(std::static_pointer_cast>(parameter)); - break; - } - - default: { - throw UnrecognizedParameterTypeException("The Parameter type is not available"); - } - } -} - -void Cell::add_transform_broadcaster( - const std::shared_ptr& recipient, bool always_active, int queue_size -) { - this->add_transform_broadcaster(recipient, this->get_period(), always_active, queue_size); -} - -void Cell::add_transform_broadcaster( - const state_representation::CartesianPose& recipient, bool always_active, int queue_size -) { - this->add_transform_broadcaster(recipient, this->get_period(), always_active, queue_size); -} - -void Cell::add_transform_broadcaster( - const std::shared_ptr& recipient, bool always_active, int queue_size -) { - this->add_transform_broadcaster(recipient, this->get_period(), always_active, queue_size); -} - -void Cell::send_transform(const state_representation::CartesianState& transform, const std::string& name) const { - if (!this->configured_) { - throw exceptions::UnconfiguredNodeException( - "The node is not yet configured. Call the lifecycle configure before using this function" - ); - } - state_representation::CartesianState transform_copy(transform); - if (!name.empty()) { transform_copy.set_name(name); } - std::static_pointer_cast( - this->handlers_.at("tf_broadcaster").first - )->send_transform(transform_copy); -} - -const state_representation::CartesianPose Cell::lookup_transform(const std::string& frame_name, const std::string& reference_frame) const { - if (!this->configured_) { - throw exceptions::UnconfiguredNodeException( - "The node is not yet configured. Call the lifecycle configure before using this function" - ); - } - return std::static_pointer_cast( - this->handlers_.at("tf_listener").first - )->lookup_transform(frame_name, reference_frame); -} - -LifecycleNodeInterface::CallbackReturn Cell::on_configure(const rclcpp_lifecycle::State&) { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_configure() is called."); - this->configured_ = true; - // call the proxy on_configure function - if (!this->on_configure()) { - RCLCPP_ERROR(get_logger(), "Configuration failed"); - this->reset(); - return LifecycleNodeInterface::CallbackReturn::FAILURE; - } - // add the run periodic call - this->timers_.push_back(this->create_wall_timer(this->period_, [this] { this->run(); })); - // add default transform broadcaster and transform listener - this->add_transform_broadcaster(this->period_, true); - this->add_transform_listener(10 * this->period_); - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -bool Cell::on_configure() { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_configure of the Cell class called"); - return true; -} - -LifecycleNodeInterface::CallbackReturn Cell::on_activate(const rclcpp_lifecycle::State&) { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_activate() is called."); - // call the proxy on_activate function - if (!this->on_activate()) { - RCLCPP_ERROR(get_logger(), "Activation failed."); - return LifecycleNodeInterface::CallbackReturn::FAILURE; - } - // set all handlers to activated - this->active_ = true; - for (auto& h : this->handlers_) { - h.second.first->activate(); - } - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -bool Cell::on_activate() { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_activate of the Cell class called"); - return true; -} - -LifecycleNodeInterface::CallbackReturn Cell::on_deactivate(const rclcpp_lifecycle::State&) { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate() is called."); - // call the proxy on_deactivate function - if (!this->on_deactivate()) { - RCLCPP_ERROR(get_logger(), "Deactivation failed."); - return LifecycleNodeInterface::CallbackReturn::FAILURE; - } - // set all handlers to not activated except for the one always active - this->active_ = false; - for (auto& h : this->handlers_) { - if (!h.second.second) { - h.second.first->deactivate(); - } - } - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -bool Cell::on_deactivate() { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_deactivate of the Cell class called"); - return true; -} - -LifecycleNodeInterface::CallbackReturn Cell::on_cleanup(const rclcpp_lifecycle::State&) { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_cleanup() is called."); - // call the proxy on_cleanup function - if (!this->on_cleanup()) { - RCLCPP_ERROR(get_logger(), "Cleanup failed."); - return LifecycleNodeInterface::CallbackReturn::FAILURE; - } - // reset all handlers - this->reset(); - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -bool Cell::on_cleanup() { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_cleanup of the Cell class called"); - return true; -} - -LifecycleNodeInterface::CallbackReturn Cell::on_shutdown(const rclcpp_lifecycle::State& state) { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_shutdown() is called from state %s.", state.label().c_str()); - uint8_t current_state = state.id(); - // if the node is already shutdown just return success - if (current_state == lifecycle_msgs::msg::State::PRIMARY_STATE_FINALIZED) { - return LifecycleNodeInterface::CallbackReturn::SUCCESS; - } - // check current state and eventually deactivate and deconfigure - if (current_state == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) { - RCUTILS_LOG_INFO_NAMED(get_name(), "Node is active, deactivating it before shutdown."); - auto callback_return = this->on_deactivate(this->get_current_state()); - if (callback_return != LifecycleNodeInterface::CallbackReturn::SUCCESS) { - return callback_return; - } - current_state = lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE; - } - if (current_state == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { - RCUTILS_LOG_INFO_NAMED(get_name(), "Node is inactive, cleaning up before shutdown."); - auto callback_return = this->on_cleanup(this->get_current_state()); - if (callback_return != LifecycleNodeInterface::CallbackReturn::SUCCESS) { - return callback_return; - } - current_state = lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED; - } - // call the proxy on_shutdown function - if (!this->on_shutdown()) { - RCLCPP_ERROR(get_logger(), "Shutdown failed."); - return LifecycleNodeInterface::CallbackReturn::FAILURE; - } - // reset all handlers for clean shutdown - this->reset(); - this->handlers_.clear(); - this->daemons_.clear(); - this->parameters_.clear(); - this->shutdown_ = true; - return LifecycleNodeInterface::CallbackReturn::SUCCESS; -} - -bool Cell::on_shutdown() { - RCUTILS_LOG_INFO_NAMED(get_name(), "on_shutdown of the Cell class called"); - return true; -} - -void Cell::run() { - for (auto& h : this->handlers_) { - h.second.first->check_timeout(); - } - if (this->active_) { - this->step(); - } -} - -void Cell::step() {} - -void Cell::run_periodic_call(const std::function& callback_function) { - if (this->active_) { - callback_function(); - } -} - -void Cell::add_periodic_call(const std::function& callback_function, const std::chrono::nanoseconds& period) { - this->timers_.push_back(this->create_wall_timer(period, [this, callback_function] { this->run_periodic_call(callback_function); })); -} - -void Cell::add_daemon(const std::function& callback_function, const std::chrono::nanoseconds& period) { - this->daemons_.push_back(this->create_wall_timer(period, callback_function)); -} - -void Cell::update_parameters() { - using namespace state_representation; - using namespace state_representation::exceptions; - try { - for (auto&[key, param]: this->parameters_) { - switch (param->get_parameter_type()) { - case ParameterType::INT: { - int value = static_cast(this->get_parameter(param->get_name()).as_int()); - std::static_pointer_cast>(param)->set_value(value); - break; - } - - case ParameterType::INT_ARRAY: { - std::vector tmp = this->get_parameter(param->get_name()).as_integer_array(); - std::vector value(tmp.begin(), tmp.end()); - std::static_pointer_cast>>(param)->set_value(value); - break; - } - - case ParameterType::DOUBLE: { - double value = this->get_parameter(param->get_name()).as_double(); - std::static_pointer_cast>(param)->set_value(value); - break; - } - - case ParameterType::DOUBLE_ARRAY: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>>(param)->set_value(value); - break; - } - - case ParameterType::BOOL: { - bool value = this->get_parameter(param->get_name()).as_bool(); - std::static_pointer_cast>(param)->set_value(value); - break; - } - - case ParameterType::BOOL_ARRAY: { - std::vector value = this->get_parameter(param->get_name()).as_bool_array(); - std::static_pointer_cast>>(param)->set_value(value); - break; - } - - case ParameterType::STRING: { - std::string value = this->get_parameter(param->get_name()).as_string(); - std::static_pointer_cast>(param)->set_value(value); - break; - } - - case ParameterType::STRING_ARRAY: { - std::vector value = this->get_parameter(param->get_name()).as_string_array(); - std::static_pointer_cast>>(param)->set_value(value); - break; - } - - case ParameterType::STATE: { - switch (param->get_parameter_state_type()) { - case StateType::CARTESIAN_STATE: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().set_data(value); - break; - } - case StateType::CARTESIAN_POSE: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().CartesianPose::set_data(value); - break; - } - case StateType::JOINT_STATE: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().set_data(value); - break; - } - case StateType::JOINT_POSITIONS: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().JointPositions::set_data( - value - ); - break; - } - case StateType::GEOMETRY_ELLIPSOID: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - std::static_pointer_cast>(param)->get_value().set_data(value); - break; - } - default: { - throw UnrecognizedParameterTypeException("The Parameter state type is not available"); - } - } - break; - } - - case ParameterType::MATRIX: { - std::vector value = this->get_parameter(param->get_name()).as_double_array(); - size_t rows = std::static_pointer_cast>(param)->get_value().rows(); - size_t cols = std::static_pointer_cast>(param)->get_value().cols(); - size_t size = std::static_pointer_cast>(param)->get_value().size(); - // depending on the size of the received parameter produce a different matrix - Eigen::MatrixXd matrix_value(rows, cols); - if (value.size() == size)// equal size direct copy - { - matrix_value = Eigen::MatrixXd::Map(value.data(), rows, cols); - } else if (value.size() == rows && value.size() == cols)// diagonal matrix with only diagonal values set - { - Eigen::VectorXd diagonal_coefficients = Eigen::VectorXd::Map(value.data(), value.size()); - matrix_value = diagonal_coefficients.asDiagonal(); - } else if (value.size() == 1)// single element means iso diagonal matrix - { - matrix_value = value[0] * Eigen::MatrixXd::Identity(rows, cols); - } else// any other sizes generates an error - { - throw IncompatibleSizeException( - "The value set does not have the correct expected size of " + std::to_string(rows) + "x" - + std::to_string(cols) + "elements" - ); - } - std::static_pointer_cast>(param)->set_value(matrix_value); - break; - } - - default: { - throw UnrecognizedParameterTypeException("The Parameter type is not available"); - } - } - } - } catch (rclcpp::ParameterTypeException& e) { - RCLCPP_ERROR(this->get_logger(), e.what()); - } catch (IncompatibleSizeException& e) { - RCLCPP_ERROR(this->get_logger(), e.what()); - } catch (UnrecognizedParameterTypeException& e) { - RCLCPP_ERROR(this->get_logger(), e.what()); - } -} -}// namespace modulo::core diff --git a/source/modulo_core/src/Component.cpp b/source/modulo_core/src/Component.cpp deleted file mode 100644 index 3c549f65e..000000000 --- a/source/modulo_core/src/Component.cpp +++ /dev/null @@ -1,109 +0,0 @@ -#include "modulo_core/Component.hpp" - -#include "modulo_core/exceptions/PredicateAlreadyRegisteredException.hpp" -#include "modulo_core/exceptions/PredicateNotFoundException.hpp" - -#include - -namespace modulo::core { - -void Component::on_init() { - this->declare_parameter("predicate_checking_period", 100); - this->add_predicate("is_configured", [this] { return this->is_configured(); }); - this->add_predicate("is_active", [this] { return this->is_active(); }); - this->add_daemon([this] { this->evaluate_predicate_functions(); }, - std::chrono::milliseconds(this->get_parameter("predicate_checking_period").as_int())); -} - -Component::Component(const rclcpp::NodeOptions& options) : Cell(options) { - this->on_init(); -} - -void Component::evaluate_predicate_functions() { - for (auto const& [key, val] : this->predicate_functions_) { - this->set_predicate_value(key, (val)()); - } -} - -std::string Component::generate_predicate_topic(const std::string& predicate_name) const { - return "/predicates/" + std::string(this->get_name()) + "/" + predicate_name; -} - -void Component::add_predicate(const std::shared_ptr& predicate) { - std::string predicate_name = predicate->get_name(); - if (this->predicates_.find(predicate_name) != this->predicates_.end()) { - throw exceptions::PredicateAlreadyRegisteredException(predicate_name); - } - // add the predicate to the map - this->predicates_.insert(std::make_pair(predicate_name, predicate)); - // add the publisher predicate - this->add_publisher(this->generate_predicate_topic(predicate_name), predicate, true); -} - -void Component::add_predicate(const std::string& predicate_name, const std::function& predicate_function) { - // insert the predicate function in the list overwriting it if it already exists - this->predicate_functions_.insert_or_assign(predicate_name, predicate_function); - // crate a predicate with named prefixed with the action name - auto predicate = std::make_shared(predicate_name); - // try to insert the predicate in the map. If it already exists, do nothing to just - // update the predicate_function - try { - this->add_predicate(predicate); - } catch (exceptions::PredicateAlreadyRegisteredException&) { - RCLCPP_DEBUG(this->get_logger(), "Predicate with same name already exists, overwriting predicate callback function"); - } -} - -void Component::add_received_predicate(const std::string& predicate_name, const std::string& channel) { - auto predicate = std::make_shared(predicate_name); - this->predicates_.insert(std::make_pair(predicate_name, predicate)); - // add a subscription to the channel where the predicate is published - this->add_subscription(channel, predicate, true); -} - -bool Component::get_predicate_value(const std::string& predicate_name) const { - auto predicate_iterator = this->predicates_.find(predicate_name); - if (predicate_iterator == this->predicates_.end()) { - throw exceptions::PredicateNotFoundException(predicate_name); - } - return predicate_iterator->second->get_value(); -} - -void Component::set_predicate_value(const std::string& predicate_name, bool value) { - auto predicate_iterator = this->predicates_.find(predicate_name); - if (predicate_iterator == this->predicates_.end()) { - throw exceptions::PredicateNotFoundException(predicate_name); - } - return predicate_iterator->second->set_value(value); -} - -void Component::add_event(const std::shared_ptr& event) { - this->add_predicate(event); -} - -void Component::add_event(const std::string& event_name, const std::function& event_function) { - this->add_predicate(event_name, event_function); -} - -void Component::add_received_event(const std::string& event_name, const std::string& channel) { - this->add_received_predicate(event_name, channel); -} - -bool Component::read_event_value(const std::string& event_name) { - auto event_iterator = this->predicates_.find(event_name); - if (event_iterator == this->predicates_.end()) { - throw exceptions::PredicateNotFoundException(event_name); - } - return std::dynamic_pointer_cast(event_iterator->second)->read_value(); -} - -void Component::set_event_value(const std::string& event_name, bool value) { - this->set_predicate_value(event_name, value); -} - -const std::list> Component::get_predicates() const { - std::list> predicate_list; - std::transform(this->predicates_.begin(), this->predicates_.end(), std::back_inserter(predicate_list), [](const std::map>::value_type& val) { return val.second; }); - return predicate_list; -} -}// namespace modulo::core diff --git a/source/modulo_core/src/Monitor.cpp b/source/modulo_core/src/Monitor.cpp deleted file mode 100644 index 10babf423..000000000 --- a/source/modulo_core/src/Monitor.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include "modulo_core/Monitor.hpp" - -#include "modulo_core/exceptions/CommunicationTimeoutException.hpp" - -namespace modulo::core { -Monitor::~Monitor() { - this->on_shutdown(); -} - -bool Monitor::on_configure() { - for (auto& name : this->monitored_node_) this->add_client(name + "/get_state", - std::chrono::milliseconds(100)); - return true; -} - -bool Monitor::on_activate() { - return true; -} - -bool Monitor::on_deactivate() { - return true; -} - -bool Monitor::on_cleanup() { - return true; -} - -bool Monitor::on_shutdown() { - return true; -} - -void Monitor::step() { - for (auto& name : this->monitored_node_) { - try { - auto request = std::make_shared(); - auto response = this->send_blocking_request(name + "/get_state", request); - RCLCPP_INFO(get_logger(), "Node %s status is %s", name.c_str(), response->current_state.label.c_str()); - } catch (exceptions::CommunicationTimeoutException& e) { - RCLCPP_ERROR(get_logger(), e.what()); - } - } - RCLCPP_INFO(get_logger(), "----------------------"); -} -}// namespace modulo::core diff --git a/source/modulo_core/src/Recorder.cpp b/source/modulo_core/src/Recorder.cpp deleted file mode 100644 index 1629efd9d..000000000 --- a/source/modulo_core/src/Recorder.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include "modulo_core/Recorder.hpp" - -#include - -namespace modulo::core { -Recorder::~Recorder() { - this->on_shutdown(); -} - -bool Recorder::on_configure() { - return true; -} - -bool Recorder::on_activate() { - this->start_time = std::chrono::system_clock::now(); - return true; -} - -bool Recorder::on_deactivate() { - this->end_time = std::chrono::system_clock::now(); - return true; -} - -bool Recorder::on_cleanup() { - return true; -} - -bool Recorder::on_shutdown() { - return true; -} - -void Recorder::step() { - for (auto& h : this->get_handlers()) { - if (h.second.first->get_type() == communication::CommunicationType::SUBSCRIPTION) { - const communication::MessagePassingHandler& subscription = static_cast(*h.second.first); - if (!this->record(subscription.get_recipient())) RCLCPP_ERROR(this->get_logger(), "Unable to record %s", subscription.get_recipient().get_name().c_str()); - } - } -} - -bool Recorder::record(const state_representation::State& state) const { - switch (state.get_type()) { - case state_representation::StateType::CARTESIAN_STATE: - return record(static_cast(state)); - - case state_representation::StateType::JOINT_STATE: - return record(static_cast(state)); - - default: - RCLCPP_ERROR(this->get_logger(), "Recording function for %s not defined for this type of state", state.get_name().c_str()); - return false; - } -} - -bool Recorder::record(const state_representation::CartesianState& state) const { - RCLCPP_WARN(this->get_logger(), "Trying to record %s from the base class", state.get_name().c_str()); - return false; -} - -bool Recorder::record(const state_representation::JointState& state) const { - RCLCPP_WARN(this->get_logger(), "Trying to record %s from the base class", state.get_name().c_str()); - return false; -} -}// namespace modulo::core diff --git a/source/modulo_core/src/communication/CommunicationHandler.cpp b/source/modulo_core/src/communication/CommunicationHandler.cpp deleted file mode 100644 index 7a9851d12..000000000 --- a/source/modulo_core/src/communication/CommunicationHandler.cpp +++ /dev/null @@ -1,5 +0,0 @@ -#include "modulo_core/communication/CommunicationHandler.hpp" - -namespace modulo::core::communication { -CommunicationHandler::CommunicationHandler(const CommunicationType& type) : type_(type) {} -}// namespace modulo::core::communication diff --git a/source/modulo_new_core/src/communication/MessagePair.cpp b/source/modulo_core/src/communication/MessagePair.cpp similarity index 91% rename from source/modulo_new_core/src/communication/MessagePair.cpp rename to source/modulo_core/src/communication/MessagePair.cpp index 7087c4a00..5a479512e 100644 --- a/source/modulo_new_core/src/communication/MessagePair.cpp +++ b/source/modulo_core/src/communication/MessagePair.cpp @@ -1,8 +1,8 @@ -#include "modulo_new_core/communication/MessagePair.hpp" +#include "modulo_core/communication/MessagePair.hpp" #include -namespace modulo_new_core::communication { +namespace modulo_core::communication { template<> MessagePair::MessagePair( @@ -40,4 +40,4 @@ MessagePair::MessagePair( ) : MessagePairInterface(MessageType::ENCODED_STATE), data_(std::move(data)), clock_(std::move(clock)) {} -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/src/communication/MessagePairInterface.cpp b/source/modulo_core/src/communication/MessagePairInterface.cpp similarity index 50% rename from source/modulo_new_core/src/communication/MessagePairInterface.cpp rename to source/modulo_core/src/communication/MessagePairInterface.cpp index 164d01f65..0c831a6a8 100644 --- a/source/modulo_new_core/src/communication/MessagePairInterface.cpp +++ b/source/modulo_core/src/communication/MessagePairInterface.cpp @@ -1,10 +1,10 @@ -#include "modulo_new_core/communication/MessagePairInterface.hpp" +#include "modulo_core/communication/MessagePairInterface.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { MessagePairInterface::MessagePairInterface(MessageType type) : type_(type) {} MessageType MessagePairInterface::get_type() const { return this->type_; } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/src/communication/PublisherInterface.cpp b/source/modulo_core/src/communication/PublisherInterface.cpp similarity index 94% rename from source/modulo_new_core/src/communication/PublisherInterface.cpp rename to source/modulo_core/src/communication/PublisherInterface.cpp index e72be4915..4294460e3 100644 --- a/source/modulo_new_core/src/communication/PublisherInterface.cpp +++ b/source/modulo_core/src/communication/PublisherInterface.cpp @@ -1,4 +1,4 @@ -#include "modulo_new_core/communication/PublisherInterface.hpp" +#include "modulo_core/communication/PublisherInterface.hpp" #include @@ -10,12 +10,12 @@ #include #include -#include "modulo_new_core/communication/PublisherHandler.hpp" -#include "modulo_new_core/exceptions/NullPointerException.hpp" +#include "modulo_core/communication/PublisherHandler.hpp" +#include "modulo_core/exceptions/NullPointerException.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { -PublisherInterface::PublisherInterface(PublisherType type, std::shared_ptr message_pair) : +PublisherInterface::PublisherInterface(PublisherType type, std::shared_ptr message_pair) : type_(type), message_pair_(std::move(message_pair)) {} void PublisherInterface::activate() { @@ -132,4 +132,4 @@ void PublisherInterface::set_message_pair(const std::shared_ptrtype_; } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/src/communication/SubscriptionHandler.cpp b/source/modulo_core/src/communication/SubscriptionHandler.cpp similarity index 95% rename from source/modulo_new_core/src/communication/SubscriptionHandler.cpp rename to source/modulo_core/src/communication/SubscriptionHandler.cpp index 8b55f21ed..ba823ca80 100644 --- a/source/modulo_new_core/src/communication/SubscriptionHandler.cpp +++ b/source/modulo_core/src/communication/SubscriptionHandler.cpp @@ -1,8 +1,8 @@ -#include "modulo_new_core/communication/SubscriptionHandler.hpp" +#include "modulo_core/communication/SubscriptionHandler.hpp" #include -namespace modulo_new_core::communication { +namespace modulo_core::communication { template<> SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : @@ -75,4 +75,4 @@ std::function)> SubscriptionHandlerget_message_pair()->template read(*message); }; } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_new_core/src/communication/SubscriptionInterface.cpp b/source/modulo_core/src/communication/SubscriptionInterface.cpp similarity index 70% rename from source/modulo_new_core/src/communication/SubscriptionInterface.cpp rename to source/modulo_core/src/communication/SubscriptionInterface.cpp index 81b691f57..66b4f6599 100644 --- a/source/modulo_new_core/src/communication/SubscriptionInterface.cpp +++ b/source/modulo_core/src/communication/SubscriptionInterface.cpp @@ -1,8 +1,8 @@ -#include "modulo_new_core/communication/SubscriptionInterface.hpp" +#include "modulo_core/communication/SubscriptionInterface.hpp" -#include "modulo_new_core/exceptions/NullPointerException.hpp" +#include "modulo_core/exceptions/NullPointerException.hpp" -namespace modulo_new_core::communication { +namespace modulo_core::communication { SubscriptionInterface::SubscriptionInterface(std::shared_ptr message_pair) : message_pair_(std::move(message_pair)) {} @@ -17,4 +17,4 @@ void SubscriptionInterface::set_message_pair(const std::shared_ptrmessage_pair_ = message_pair; } -}// namespace modulo_new_core::communication +}// namespace modulo_core::communication diff --git a/source/modulo_core/src/communication/message_passing/MessagePassingHandler.cpp b/source/modulo_core/src/communication/message_passing/MessagePassingHandler.cpp deleted file mode 100644 index a2c2a840c..000000000 --- a/source/modulo_core/src/communication/message_passing/MessagePassingHandler.cpp +++ /dev/null @@ -1,11 +0,0 @@ -#include "modulo_core/communication/message_passing/MessagePassingHandler.hpp" - -namespace modulo::core::communication { -MessagePassingHandler::MessagePassingHandler(const CommunicationType& type) : CommunicationHandler(type), - asynchronous_(false) {} - -MessagePassingHandler::MessagePassingHandler(const CommunicationType& type, - const std::shared_ptr& recipient) : CommunicationHandler(type), - recipient_(recipient), - asynchronous_(true) {} -}// namespace modulo::core::communication diff --git a/source/modulo_core/src/communication/message_passing/TransformBroadcasterHandler.cpp b/source/modulo_core/src/communication/message_passing/TransformBroadcasterHandler.cpp deleted file mode 100644 index 23c08f010..000000000 --- a/source/modulo_core/src/communication/message_passing/TransformBroadcasterHandler.cpp +++ /dev/null @@ -1,10 +0,0 @@ -#include "modulo_core/communication/message_passing/TransformBroadcasterHandler.hpp" - -namespace modulo::core::communication { -TransformBroadcasterHandler::TransformBroadcasterHandler(const std::shared_ptr& recipient, - const std::shared_ptr& clock) : PublisherHandler(recipient, - clock) {} - -TransformBroadcasterHandler::TransformBroadcasterHandler(const std::shared_ptr& clock) : PublisherHandler(std::make_shared(), - clock) {} -}// namespace modulo::core::communication diff --git a/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp b/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp deleted file mode 100644 index d407f5d3b..000000000 --- a/source/modulo_core/src/communication/message_passing/TransformListenerHandler.cpp +++ /dev/null @@ -1,15 +0,0 @@ -#include "modulo_core/communication/message_passing/TransformListenerHandler.hpp" - -namespace modulo::core::communication { -const state_representation::CartesianPose TransformListenerHandler::lookup_transform(const std::string& frame_name, - const std::string& reference_frame) const { - geometry_msgs::msg::TransformStamped transformStamped; - state_representation::CartesianPose result(frame_name, reference_frame); - transformStamped = this->buffer_.lookupTransform(reference_frame, - frame_name, - tf2::TimePoint(std::chrono::milliseconds(0)), - tf2::Duration(this->get_timeout())); - modulo_new_core::translators::read_message(result, transformStamped); - return result; -} -}// namespace modulo::core::communication diff --git a/source/modulo_new_core/src/translators/message_readers.cpp b/source/modulo_core/src/translators/message_readers.cpp similarity index 96% rename from source/modulo_new_core/src/translators/message_readers.cpp rename to source/modulo_core/src/translators/message_readers.cpp index f31580908..647514e93 100644 --- a/source/modulo_new_core/src/translators/message_readers.cpp +++ b/source/modulo_core/src/translators/message_readers.cpp @@ -1,6 +1,6 @@ -#include "modulo_new_core/translators/message_readers.hpp" +#include "modulo_core/translators/message_readers.hpp" -namespace modulo_new_core::translators { +namespace modulo_core::translators { static Eigen::Vector3d read_point(const geometry_msgs::msg::Point& message) { return {message.x, message.y, message.z}; @@ -97,4 +97,4 @@ void read_message(int& state, const std_msgs::msg::Int32& message) { void read_message(std::string& state, const std_msgs::msg::String& message) { state = message.data; } -}// namespace modulo_new_core::translators +}// namespace modulo_core::translators diff --git a/source/modulo_new_core/src/translators/message_writers.cpp b/source/modulo_core/src/translators/message_writers.cpp similarity index 98% rename from source/modulo_new_core/src/translators/message_writers.cpp rename to source/modulo_core/src/translators/message_writers.cpp index b927c486f..557cffe0a 100644 --- a/source/modulo_new_core/src/translators/message_writers.cpp +++ b/source/modulo_core/src/translators/message_writers.cpp @@ -1,11 +1,11 @@ -#include "modulo_new_core/translators/message_writers.hpp" +#include "modulo_core/translators/message_writers.hpp" #include #include using namespace state_representation; -namespace modulo_new_core::translators { +namespace modulo_core::translators { static void write_point(geometry_msgs::msg::Point& message, const Eigen::Vector3d& vector) { message.x = vector.x(); @@ -182,4 +182,4 @@ void write_message(std_msgs::msg::Int32& message, const int& state, const rclcpp void write_message(std_msgs::msg::String& message, const std::string& state, const rclcpp::Time&) { message.data = state; } -}// namespace modulo_new_core::translators +}// namespace modulo_core::translators diff --git a/source/modulo_new_core/src/translators/parameter_translators.cpp b/source/modulo_core/src/translators/parameter_translators.cpp similarity index 98% rename from source/modulo_new_core/src/translators/parameter_translators.cpp rename to source/modulo_core/src/translators/parameter_translators.cpp index 62fe85659..6cf7a7ce8 100644 --- a/source/modulo_new_core/src/translators/parameter_translators.cpp +++ b/source/modulo_core/src/translators/parameter_translators.cpp @@ -1,4 +1,4 @@ -#include "modulo_new_core/translators/parameter_translators.hpp" +#include "modulo_core/translators/parameter_translators.hpp" #include #include @@ -6,11 +6,11 @@ #include #include -#include "modulo_new_core/exceptions/IncompatibleParameterException.hpp" +#include "modulo_core/exceptions/IncompatibleParameterException.hpp" using namespace state_representation; -namespace modulo_new_core::translators { +namespace modulo_core::translators { void copy_parameter_value( const std::shared_ptr& source_parameter, @@ -239,4 +239,4 @@ void read_parameter( auto new_parameter = read_parameter_const(ros_parameter, parameter); copy_parameter_value(new_parameter, parameter); } -}// namespace modulo_new_core::translators +}// namespace modulo_core::translators diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp b/source/modulo_core/test/cpp_tests/communication/test_communication.cpp similarity index 90% rename from source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp rename to source/modulo_core/test/cpp_tests/communication/test_communication.cpp index befcf1add..43c98d98d 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_communication.cpp +++ b/source/modulo_core/test/cpp_tests/communication/test_communication.cpp @@ -2,12 +2,12 @@ #include "rclcpp/rclcpp.hpp" -#include "modulo_new_core/communication/MessagePair.hpp" -#include "modulo_new_core/communication/PublisherHandler.hpp" -#include "modulo_new_core/communication/SubscriptionHandler.hpp" +#include "modulo_core/communication/MessagePair.hpp" +#include "modulo_core/communication/PublisherHandler.hpp" +#include "modulo_core/communication/SubscriptionHandler.hpp" using namespace std::chrono_literals; -using namespace modulo_new_core::communication; +using namespace modulo_core::communication; template class MinimalPublisher : public rclcpp::Node { @@ -115,10 +115,9 @@ TEST_F(CommunicationTest, EncodedState) { auto pub_message = make_shared_message_pair(pub_state, this->clock_); auto sub_state = std::make_shared(CartesianState::Identity("that", "base")); auto sub_message = make_shared_message_pair(sub_state, this->clock_); - this->add_nodes("/test_topic", pub_message, sub_message); + this->add_nodes("/test_topic", pub_message, sub_message); this->exec_->template spin_until_future_complete( - std::dynamic_pointer_cast>(this->sub_node_)->received_future, - 500ms + std::dynamic_pointer_cast>(this->sub_node_)->received_future, 500ms ); EXPECT_EQ(pub_state->get_name(), sub_state->get_name()); diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp b/source/modulo_core/test/cpp_tests/communication/test_message_pair.cpp similarity index 81% rename from source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp rename to source/modulo_core/test/cpp_tests/communication/test_message_pair.cpp index 1aefd323c..28fd5b0a9 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_message_pair.cpp +++ b/source/modulo_core/test/cpp_tests/communication/test_message_pair.cpp @@ -1,8 +1,8 @@ #include -#include "modulo_new_core/communication/MessagePair.hpp" +#include "modulo_core/communication/MessagePair.hpp" -using namespace modulo_new_core::communication; +using namespace modulo_core::communication; template static void test_message_interface( @@ -52,28 +52,28 @@ TEST_F(MessagePairTest, EncodedState) { auto initial_value = state_representation::CartesianState::Random("test"); auto data = state_representation::make_shared_state(initial_value); auto message_pair = - std::make_shared>(data, clock_); + std::make_shared>(data, clock_); EXPECT_TRUE(initial_value.data().isApprox( std::dynamic_pointer_cast(message_pair->get_data())->data())); std::shared_ptr message_pair_interface(message_pair); - auto message = message_pair_interface->write(); + auto message = message_pair_interface->write(); std::string tmp(message.data.begin(), message.data.end()); auto decoded = clproto::decode(tmp); EXPECT_TRUE(initial_value.data().isApprox(decoded.data())); auto new_value = state_representation::CartesianState::Identity("world"); message_pair->set_data(state_representation::make_shared_state(new_value)); - message = message_pair_interface->write(); + message = message_pair_interface->write(); tmp = std::string(message.data.begin(), message.data.end()); decoded = clproto::decode(tmp); EXPECT_TRUE(new_value.data().isApprox(decoded.data())); data = state_representation::make_shared_state(initial_value); message_pair->set_data(data); - message = modulo_new_core::EncodedState(); - modulo_new_core::translators::write_message(message, data, clock_->now()); - message_pair_interface->read(message); + message = modulo_core::EncodedState(); + modulo_core::translators::write_message(message, data, clock_->now()); + message_pair_interface->read(message); EXPECT_TRUE(initial_value.data().isApprox( std::dynamic_pointer_cast(message_pair->get_data())->data())); } diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp b/source/modulo_core/test/cpp_tests/communication/test_publisher_handler.cpp similarity index 72% rename from source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp rename to source/modulo_core/test/cpp_tests/communication/test_publisher_handler.cpp index 3174c1e9a..0e6c1463c 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_publisher_handler.cpp +++ b/source/modulo_core/test/cpp_tests/communication/test_publisher_handler.cpp @@ -2,11 +2,11 @@ #include -#include "modulo_new_core/communication/PublisherHandler.hpp" -#include "modulo_new_core/communication/MessagePair.hpp" -#include "modulo_new_core/exceptions/NullPointerException.hpp" +#include "modulo_core/communication/PublisherHandler.hpp" +#include "modulo_core/communication/MessagePair.hpp" +#include "modulo_core/exceptions/NullPointerException.hpp" -using namespace modulo_new_core::communication; +using namespace modulo_core::communication; template static void test_publisher_interface(const std::shared_ptr& node, const DataT& value) { @@ -21,7 +21,7 @@ static void test_publisher_interface(const std::shared_ptr& node, // use in publisher interface std::shared_ptr publisher_interface(publisher_handler); - EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); + EXPECT_THROW(publisher_interface->publish(), modulo_core::exceptions::NullPointerException); publisher_interface->set_message_pair(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); @@ -59,19 +59,19 @@ TEST_F(PublisherTest, EncodedState) { // create message pair auto data = std::make_shared(state_representation::CartesianState::Random("test")); - auto message_pair = std::make_shared>( + auto message_pair = std::make_shared>( data, node->get_clock()); // create publisher handler - auto publisher = node->create_publisher("topic", 10); - auto publisher_handler = std::make_shared, - modulo_new_core::EncodedState>>( - PublisherType::PUBLISHER, publisher - ); + auto publisher = node->create_publisher("topic", 10); + auto publisher_handler = + std::make_shared, modulo_core::EncodedState>>( + PublisherType::PUBLISHER, publisher + ); // use in publisher interface std::shared_ptr publisher_interface(publisher_handler); - EXPECT_THROW(publisher_interface->publish(), modulo_new_core::exceptions::NullPointerException); + EXPECT_THROW(publisher_interface->publish(), modulo_core::exceptions::NullPointerException); publisher_interface->set_message_pair(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); diff --git a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp b/source/modulo_core/test/cpp_tests/communication/test_subscription_handler.cpp similarity index 82% rename from source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp rename to source/modulo_core/test/cpp_tests/communication/test_subscription_handler.cpp index 3d347d0dc..d7f2a2fbe 100644 --- a/source/modulo_new_core/test/cpp_tests/communication/test_subscription_handler.cpp +++ b/source/modulo_core/test/cpp_tests/communication/test_subscription_handler.cpp @@ -2,10 +2,10 @@ #include -#include "modulo_new_core/communication/SubscriptionHandler.hpp" -#include "modulo_new_core/exceptions/NullPointerException.hpp" +#include "modulo_core/communication/SubscriptionHandler.hpp" +#include "modulo_core/exceptions/NullPointerException.hpp" -using namespace modulo_new_core::communication; +using namespace modulo_core::communication; template static void test_subscription_interface(const std::shared_ptr& node, const DataT& value) { @@ -52,12 +52,12 @@ TEST_F(SubscriptionTest, EncodedState) { // create message pair auto data = std::make_shared(state_representation::CartesianState::Random("test")); - auto message_pair = std::make_shared>( + auto message_pair = std::make_shared>( data, node->get_clock()); // create subscription handler - auto subscription_handler = std::make_shared>(message_pair); - auto subscription = node->create_subscription( + auto subscription_handler = std::make_shared>(message_pair); + auto subscription = node->create_subscription( "topic", 10, subscription_handler->get_callback()); // use in subscription interface diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp similarity index 96% rename from source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp rename to source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp index 407c19971..d777e7c60 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp +++ b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_eigen_translators.cpp @@ -1,8 +1,8 @@ #include -#include "modulo_new_core/translators/parameter_translators.hpp" +#include "modulo_core/translators/parameter_translators.hpp" -using namespace modulo_new_core::translators; +using namespace modulo_core::translators; TEST(ParameterEigenTranslationTest, EigenVector) { Eigen::VectorXd vec = Eigen::VectorXd::Random(5); diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp similarity index 81% rename from source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp rename to source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp index fe4f7026e..e5a1dc2f6 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp +++ b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_state_translators.cpp @@ -6,23 +6,20 @@ #include #include -#include "modulo_new_core/translators/parameter_translators.hpp" +#include "modulo_core/translators/parameter_translators.hpp" -using namespace modulo_new_core::translators; +using namespace modulo_core::translators; // Parameterized test fixture by type and expected value // See also: http://www.ashermancinelli.com/gtest-type-val-param -static std::tuple< - state_representation::CartesianState, - state_representation::CartesianPose, - state_representation::JointState, - state_representation::JointPositions -> parameter_state_test_cases { +static std::tuple parameter_state_test_cases{ state_representation::CartesianState::Random("frame", "reference"), state_representation::CartesianPose::Random("frame", "reference"), - state_representation::JointState::Random("robot", 3), - state_representation::JointPositions::Random("robot", 3), + state_representation::JointState::Random("robot", 3), state_representation::JointPositions::Random("robot", 3), }; template @@ -86,10 +83,8 @@ TYPED_TEST_P(ParameterStateTranslationTest, ConstRead) { REGISTER_TYPED_TEST_SUITE_P(ParameterStateTranslationTest, Write, ReadAndReWrite, ConstRead); -using ParameterStateTestTypes = testing::Types< - state_representation::CartesianState, - state_representation::CartesianPose, - state_representation::JointState, - state_representation::JointPositions ->; +using ParameterStateTestTypes = testing::Types; INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ParameterStateTranslationTest, ParameterStateTestTypes); diff --git a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp similarity index 97% rename from source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp rename to source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp index 46c6989ec..3c26755f2 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp +++ b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp @@ -1,8 +1,8 @@ #include -#include "modulo_new_core/translators/parameter_translators.hpp" +#include "modulo_core/translators/parameter_translators.hpp" -using namespace modulo_new_core::translators; +using namespace modulo_core::translators; // Parameterized test fixture by type and expected value // See also: http://www.ashermancinelli.com/gtest-type-val-param diff --git a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp b/source/modulo_core/test/cpp_tests/translators/test_messages.cpp similarity index 97% rename from source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp rename to source/modulo_core/test/cpp_tests/translators/test_messages.cpp index 755bebf53..a098bc138 100644 --- a/source/modulo_new_core/test/cpp_tests/translators/test_messages.cpp +++ b/source/modulo_core/test/cpp_tests/translators/test_messages.cpp @@ -1,12 +1,12 @@ #include -#include "modulo_new_core/translators/message_readers.hpp" -#include "modulo_new_core/translators/message_writers.hpp" +#include "modulo_core/translators/message_readers.hpp" +#include "modulo_core/translators/message_writers.hpp" #include #include -using namespace modulo_new_core::translators; +using namespace modulo_core::translators; template static void test_std_messages(const DataT& state, const rclcpp::Time& time) { @@ -199,7 +199,7 @@ TEST_F(MessageTranslatorsTest, TestStdMsgs) { } TEST_F(MessageTranslatorsTest, TestEncodedState) { - auto message = modulo_new_core::EncodedState(); + auto message = modulo_core::EncodedState(); write_message(message, state_, clock_.now()); state_representation::CartesianState new_state; read_message(new_state, message); @@ -210,7 +210,7 @@ TEST_F(MessageTranslatorsTest, TestEncodedState) { TEST_F(MessageTranslatorsTest, TestEncodedStatePointer) { auto state_ptr = state_representation::make_shared_state(state_); - auto message = modulo_new_core::EncodedState(); + auto message = modulo_core::EncodedState(); write_message(message, state_ptr, clock_.now()); auto new_state_ptr = state_representation::make_shared_state(state_representation::CartesianState()); read_message(new_state_ptr, message); diff --git a/source/modulo_new_core/test/python_tests/conftest.py b/source/modulo_core/test/python_tests/conftest.py similarity index 100% rename from source/modulo_new_core/test/python_tests/conftest.py rename to source/modulo_core/test/python_tests/conftest.py diff --git a/source/modulo_new_core/test/python_tests/translators/test_messages.py b/source/modulo_core/test/python_tests/translators/test_messages.py similarity index 97% rename from source/modulo_new_core/test/python_tests/translators/test_messages.py rename to source/modulo_core/test/python_tests/translators/test_messages.py index 3b3224cc7..1e92e196e 100644 --- a/source/modulo_new_core/test/python_tests/translators/test_messages.py +++ b/source/modulo_core/test/python_tests/translators/test_messages.py @@ -1,11 +1,11 @@ import clproto import geometry_msgs.msg -import modulo_new_core.translators.message_readers as modulo_readers -import modulo_new_core.translators.message_writers as modulo_writers +import modulo_core.translators.message_readers as modulo_readers +import modulo_core.translators.message_writers as modulo_writers import numpy as np import pytest import state_representation as sr -from modulo_new_core.encoded_state import EncodedState +from modulo_core.encoded_state import EncodedState from rclpy.clock import Clock from sensor_msgs.msg import JointState diff --git a/source/modulo_new_core/test/python_tests/translators/test_parameters.py b/source/modulo_core/test/python_tests/translators/test_parameters.py similarity index 98% rename from source/modulo_new_core/test/python_tests/translators/test_parameters.py rename to source/modulo_core/test/python_tests/translators/test_parameters.py index d398c87ee..3469aab2d 100644 --- a/source/modulo_new_core/test/python_tests/translators/test_parameters.py +++ b/source/modulo_core/test/python_tests/translators/test_parameters.py @@ -2,7 +2,7 @@ import numpy as np import pytest import state_representation as sr -from modulo_new_core.translators.parameter_translators import write_parameter, read_parameter, read_parameter_const +from modulo_core.translators.parameter_translators import write_parameter, read_parameter, read_parameter_const from rclpy import Parameter diff --git a/source/modulo_new_core/test/test_modulo_new_core.cpp b/source/modulo_core/test/test_modulo_new_core.cpp similarity index 100% rename from source/modulo_new_core/test/test_modulo_new_core.cpp rename to source/modulo_core/test/test_modulo_new_core.cpp diff --git a/source/modulo_new_core/CMakeLists.txt b/source/modulo_new_core/CMakeLists.txt deleted file mode 100644 index c9d4889be..000000000 --- a/source/modulo_new_core/CMakeLists.txt +++ /dev/null @@ -1,66 +0,0 @@ -cmake_minimum_required(VERSION 3.15) -project(modulo_new_core) - -# default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# default to C++17 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 17) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -find_package(ament_cmake_python REQUIRED) - -ament_auto_find_build_dependencies() - -find_package(control_libraries 6.0.2 REQUIRED COMPONENTS state_representation) -find_package(clproto 6.0.0 REQUIRED) - -include_directories(include) - -ament_auto_add_library(modulo_new_core SHARED - ${PROJECT_SOURCE_DIR}/src/communication/MessagePair.cpp - ${PROJECT_SOURCE_DIR}/src/communication/MessagePairInterface.cpp - ${PROJECT_SOURCE_DIR}/src/communication/PublisherInterface.cpp - ${PROJECT_SOURCE_DIR}/src/communication/SubscriptionHandler.cpp - ${PROJECT_SOURCE_DIR}/src/communication/SubscriptionInterface.cpp - ${PROJECT_SOURCE_DIR}/src/translators/parameter_translators.cpp - ${PROJECT_SOURCE_DIR}/src/translators/message_readers.cpp - ${PROJECT_SOURCE_DIR}/src/translators/message_writers.cpp) - -# install Python modules -ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_pytest REQUIRED) - - # run cpp tests - file(GLOB_RECURSE TEST_CPP_SOURCES ${PROJECT_SOURCE_DIR}/test/test_*.cpp) - - ament_add_gtest(test_modulo_new_core ${TEST_CPP_SOURCES}) - target_include_directories(test_modulo_new_core PRIVATE include) - target_link_libraries(test_modulo_new_core modulo_new_core state_representation clproto) - - # prevent pluginlib from using boost - target_compile_definitions(test_modulo_new_core PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") - - # run python tests - file(GLOB_RECURSE TEST_PYTHON_SOURCES ${PROJECT_SOURCE_DIR}/test/test_*.py) - - foreach(TEST_PYTHON IN LISTS TEST_PYTHON_SOURCES) - get_filename_component(TEST_FILENAME ${TEST_PYTHON} NAME_WE) - ament_add_pytest_test(${TEST_FILENAME} ${TEST_PYTHON}) - endforeach() - -endif() - -ament_auto_package() diff --git a/source/modulo_new_core/README.md b/source/modulo_new_core/README.md deleted file mode 100644 index 31183b9a5..000000000 --- a/source/modulo_new_core/README.md +++ /dev/null @@ -1 +0,0 @@ -# aica_msgs \ No newline at end of file diff --git a/source/modulo_new_core/package.xml b/source/modulo_new_core/package.xml deleted file mode 100644 index 385d8a412..000000000 --- a/source/modulo_new_core/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - modulo_new_core - 0.0.1 - TODO - Dominic Reber - TODO: License declaration - - ament_cmake_auto - ament_cmake_python - - rclcpp - rclpy - geometry_msgs - sensor_msgs - std_msgs - tf2_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - From 6cebc4effc9ca2dc383ed604571fa8bf0299bd29 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 1 Jul 2022 16:43:33 +0200 Subject: [PATCH 51/71] Improve exception handling in modulo_core (#94) * Make exceptions inherit from CoreException * Throw MessageTranslationException for message translators * Throw ParameterTranslationException for parameter translators * Create modulo core errors in Python * Update translators to throw core error in Python --- .../modulo_core/communication/MessagePair.hpp | 2 + .../modulo_core/exceptions/CoreException.hpp | 17 ++++++ .../InvalidPointerCastException.hpp | 9 +-- .../exceptions/InvalidPointerException.hpp | 8 +-- .../MessageTranslationException.hpp | 17 ++++++ .../exceptions/NullPointerException.hpp | 8 +-- ....hpp => ParameterTranslationException.hpp} | 9 +-- .../translators/message_readers.hpp | 14 +++-- .../translators/message_writers.hpp | 23 +++++++- .../translators/parameter_translators.hpp | 12 ++-- .../modulo_core/exceptions/__init__.py | 0 .../modulo_core/exceptions/core_exceptions.py | 27 +++++++++ .../translators/message_readers.py | 37 ++++++++----- .../translators/message_writers.py | 32 ++++++----- .../translators/parameter_translators.py | 55 ++++++++++--------- .../src/translators/message_readers.cpp | 22 +++++--- .../src/translators/message_writers.cpp | 33 ++++++++--- .../src/translators/parameter_translators.cpp | 24 ++++---- .../cpp_tests/translators/test_messages.cpp | 12 ++-- .../python_tests/translators/test_messages.py | 13 +++-- 20 files changed, 249 insertions(+), 125 deletions(-) create mode 100644 source/modulo_core/include/modulo_core/exceptions/CoreException.hpp create mode 100644 source/modulo_core/include/modulo_core/exceptions/MessageTranslationException.hpp rename source/modulo_core/include/modulo_core/exceptions/{IncompatibleParameterException.hpp => ParameterTranslationException.hpp} (56%) create mode 100644 source/modulo_core/modulo_core/exceptions/__init__.py create mode 100644 source/modulo_core/modulo_core/exceptions/core_exceptions.py diff --git a/source/modulo_core/include/modulo_core/communication/MessagePair.hpp b/source/modulo_core/include/modulo_core/communication/MessagePair.hpp index 3c0213653..9c57b1ff5 100644 --- a/source/modulo_core/include/modulo_core/communication/MessagePair.hpp +++ b/source/modulo_core/include/modulo_core/communication/MessagePair.hpp @@ -30,6 +30,7 @@ class MessagePair : public MessagePairInterface { * @brief Write the value of the data pointer to a ROS message. * @return The value of the data pointer as a ROS message * @throws NullPointerException if the data pointer is null + * @throws MessageTranslationException if the data could not be written to message */ [[nodiscard]] MsgT write_message() const; @@ -37,6 +38,7 @@ class MessagePair : public MessagePairInterface { * @brief Read a ROS message and store the value in the data pointer. * @param message The ROS message to read * @throws NullPointerException if the data pointer is null + * @throws MessageTranslationException if the message could not be read */ void read_message(const MsgT& message); diff --git a/source/modulo_core/include/modulo_core/exceptions/CoreException.hpp b/source/modulo_core/include/modulo_core/exceptions/CoreException.hpp new file mode 100644 index 000000000..e3ffa73eb --- /dev/null +++ b/source/modulo_core/include/modulo_core/exceptions/CoreException.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include +#include + +namespace modulo_core::exceptions { + +/** + * @class CoreException + * @brief A base class for all core exceptions. + * @details This inherits from std::runtime_exception. + */ +class CoreException : public std::runtime_error { +public: + explicit CoreException(const std::string& msg) : std::runtime_error(msg) {}; +}; +}// namespace modulo_core::exceptions diff --git a/source/modulo_core/include/modulo_core/exceptions/InvalidPointerCastException.hpp b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerCastException.hpp index 51dd823c6..5c4aa714c 100644 --- a/source/modulo_core/include/modulo_core/exceptions/InvalidPointerCastException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerCastException.hpp @@ -1,7 +1,6 @@ #pragma once -#include -#include +#include "modulo_core/exceptions/CoreException.hpp" namespace modulo_core::exceptions { @@ -10,8 +9,10 @@ namespace modulo_core::exceptions { * @brief An exception class to notify if the result of getting an instance of a derived class through dynamic * down-casting of an object of the base class is not a correctly typed instance of the derived class. */ -class InvalidPointerCastException : public std::runtime_error { +class InvalidPointerCastException : public CoreException { public: - explicit InvalidPointerCastException(const std::string& msg) : std::runtime_error(msg) {}; + explicit InvalidPointerCastException(const std::string& msg) : CoreException(msg) {}; }; }// namespace modulo_core::exceptions + + diff --git a/source/modulo_core/include/modulo_core/exceptions/InvalidPointerException.hpp b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerException.hpp index 02d00fdce..de9a8676d 100644 --- a/source/modulo_core/include/modulo_core/exceptions/InvalidPointerException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/InvalidPointerException.hpp @@ -1,7 +1,6 @@ #pragma once -#include -#include +#include "modulo_core/exceptions/CoreException.hpp" namespace modulo_core::exceptions { @@ -10,8 +9,9 @@ namespace modulo_core::exceptions { * @brief An exception class to notify if an object has no reference count (if the object is not owned by any pointer) * when attempting to get a derived instance through dynamic down-casting. */ -class InvalidPointerException : public std::runtime_error { +class InvalidPointerException : public CoreException { public: - explicit InvalidPointerException(const std::string& msg) : std::runtime_error(msg) {}; + explicit InvalidPointerException(const std::string& msg) : CoreException(msg) {}; }; }// namespace modulo_core::exceptions + diff --git a/source/modulo_core/include/modulo_core/exceptions/MessageTranslationException.hpp b/source/modulo_core/include/modulo_core/exceptions/MessageTranslationException.hpp new file mode 100644 index 000000000..c5418bfcb --- /dev/null +++ b/source/modulo_core/include/modulo_core/exceptions/MessageTranslationException.hpp @@ -0,0 +1,17 @@ +#pragma once + +#include "modulo_core/exceptions/CoreException.hpp" + +namespace modulo_core::exceptions { + +/** + * @class MessageTranslationException + * @brief An exception class to notify that the translation of a ROS message failed. + */ +class MessageTranslationException : public CoreException { +public: + explicit MessageTranslationException(const std::string& msg) : CoreException(msg) {}; +}; +}// namespace modulo_core::exceptions + + diff --git a/source/modulo_core/include/modulo_core/exceptions/NullPointerException.hpp b/source/modulo_core/include/modulo_core/exceptions/NullPointerException.hpp index c33d8ba7b..ac0167bd5 100644 --- a/source/modulo_core/include/modulo_core/exceptions/NullPointerException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/NullPointerException.hpp @@ -1,7 +1,6 @@ #pragma once -#include -#include +#include "modulo_core/exceptions/CoreException.hpp" namespace modulo_core::exceptions { @@ -10,8 +9,9 @@ namespace modulo_core::exceptions { * @brief An exception class to notify that a certain pointer is null. * @details This is an exception class to be thrown if a pointer is null or is trying to be set to a null pointer. */ -class NullPointerException : public std::runtime_error { +class NullPointerException : public CoreException { public: - explicit NullPointerException(const std::string& msg) : std::runtime_error(msg) {}; + explicit NullPointerException(const std::string& msg) : CoreException(msg) {}; }; }// namespace modulo_core::exceptions + diff --git a/source/modulo_core/include/modulo_core/exceptions/IncompatibleParameterException.hpp b/source/modulo_core/include/modulo_core/exceptions/ParameterTranslationException.hpp similarity index 56% rename from source/modulo_core/include/modulo_core/exceptions/IncompatibleParameterException.hpp rename to source/modulo_core/include/modulo_core/exceptions/ParameterTranslationException.hpp index 3c6aa19f7..6e5010c5b 100644 --- a/source/modulo_core/include/modulo_core/exceptions/IncompatibleParameterException.hpp +++ b/source/modulo_core/include/modulo_core/exceptions/ParameterTranslationException.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include "modulo_core/exceptions/CoreException.hpp" namespace modulo_core::exceptions { @@ -10,9 +10,10 @@ namespace modulo_core::exceptions { * @details This is an exception class to be thrown if there is a problem while translating from a ROS parameter to a * state_representation parameter and vice versa. */ -class IncompatibleParameterException : public state_representation::exceptions::InvalidParameterException { +class ParameterTranslationException : public CoreException { public: - explicit IncompatibleParameterException(const std::string& msg) : - state_representation::exceptions::InvalidParameterException(msg) {}; + explicit ParameterTranslationException(const std::string& msg) : CoreException(msg) {}; }; }// namespace modulo_core::exceptions + + diff --git a/source/modulo_core/include/modulo_core/translators/message_readers.hpp b/source/modulo_core/include/modulo_core/translators/message_readers.hpp index 66478e44c..34fe7cfb0 100644 --- a/source/modulo_core/include/modulo_core/translators/message_readers.hpp +++ b/source/modulo_core/include/modulo_core/translators/message_readers.hpp @@ -19,6 +19,7 @@ #include #include "modulo_core/EncodedState.hpp" +#include "modulo_core/exceptions/MessageTranslationException.hpp" namespace modulo_core::translators { @@ -151,11 +152,16 @@ void read_message(std::string& state, const std_msgs::msg::String& message); * @tparam T A state_representation::State type * @param state The state to populate * @param message The ROS message to read from + * @throws MessageTranslationException if the translation failed or is not supported. */ template inline void read_message(T& state, const EncodedState& message) { - std::string tmp(message.data.begin(), message.data.end()); - state = clproto::decode(tmp); + try { + std::string tmp(message.data.begin(), message.data.end()); + state = clproto::decode(tmp); + } catch (const std::exception& ex) { + throw exceptions::MessageTranslationException(ex.what()); + } } template<> @@ -304,14 +310,14 @@ inline void read_message(std::shared_ptr& state, co break; } default: - throw std::invalid_argument( + throw exceptions::MessageTranslationException( "The ParameterType contained by parameter " + param_ptr->get_name() + " is unsupported." ); } break; } default: - throw std::invalid_argument("The StateType contained by state " + new_state->get_name() + " is unsupported."); + throw exceptions::MessageTranslationException("The StateType contained by state " + new_state->get_name() + " is unsupported."); } } }// namespace modulo_core::translators diff --git a/source/modulo_core/include/modulo_core/translators/message_writers.hpp b/source/modulo_core/include/modulo_core/translators/message_writers.hpp index c88219aa0..068f4f0a9 100644 --- a/source/modulo_core/include/modulo_core/translators/message_writers.hpp +++ b/source/modulo_core/include/modulo_core/translators/message_writers.hpp @@ -20,6 +20,7 @@ #include #include "modulo_core/EncodedState.hpp" +#include "modulo_core/exceptions/MessageTranslationException.hpp" namespace modulo_core::translators { @@ -28,6 +29,7 @@ namespace modulo_core::translators { * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::Accel& message, const state_representation::CartesianState& state, const rclcpp::Time& time @@ -38,6 +40,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::AccelStamped& message, const state_representation::CartesianState& state, @@ -49,6 +52,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::Pose& message, const state_representation::CartesianState& state, const rclcpp::Time& time @@ -59,6 +63,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::PoseStamped& message, const state_representation::CartesianState& state, @@ -70,6 +75,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::Transform& message, const state_representation::CartesianState& state, const rclcpp::Time& time @@ -80,6 +86,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::TransformStamped& message, const state_representation::CartesianState& state, @@ -91,6 +98,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::Twist& message, const state_representation::CartesianState& state, const rclcpp::Time& time @@ -101,6 +109,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::TwistStamped& message, const state_representation::CartesianState& state, @@ -112,6 +121,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::Wrench& message, const state_representation::CartesianState& state, const rclcpp::Time& time @@ -122,6 +132,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( geometry_msgs::msg::WrenchStamped& message, const state_representation::CartesianState& state, @@ -133,6 +144,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( sensor_msgs::msg::JointState& message, const state_representation::JointState& state, const rclcpp::Time& time @@ -143,6 +155,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ void write_message( tf2_msgs::msg::TFMessage& message, const state_representation::CartesianState& state, const rclcpp::Time& time @@ -155,6 +168,7 @@ void write_message( * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the provided state is empty. */ template void write_message(U& message, const state_representation::Parameter& state, const rclcpp::Time&); @@ -206,10 +220,15 @@ void write_message(std_msgs::msg::String& message, const std::string& state, con * @param message The ROS message to populate * @param state The state to read from * @param time The time of the message + * @throws MessageTranslationException if the translation failed or is not supported. */ template inline void write_message(EncodedState& message, const T& state, const rclcpp::Time&) { - std::string tmp = clproto::encode(state); - message.data = std::vector(tmp.begin(), tmp.end()); + try { + std::string tmp = clproto::encode(state); + message.data = std::vector(tmp.begin(), tmp.end()); + } catch (const std::exception& ex) { + throw exceptions::MessageTranslationException(ex.what()); + } } }// namespace modulo_core::translators diff --git a/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp b/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp index 1dbaf40c3..12fe2fa03 100644 --- a/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp +++ b/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp @@ -1,6 +1,6 @@ #pragma once -#include +#include #include namespace modulo_core::translators { @@ -14,7 +14,7 @@ namespace modulo_core::translators { * to modify the value of the parameter instance while preserving the reference of the original pointer. * @param source_parameter The ParameterInterface with a value to copy * @param parameter The destination ParameterInterface to be updated - * @throws IncompatibleParameterException if the copying failed + * @throws ParameterTranslationException if the copying failed */ void copy_parameter_value( const std::shared_ptr& source_parameter, @@ -24,7 +24,7 @@ void copy_parameter_value( /** * @brief Write a ROS Parameter from a ParameterInterface pointer. * @param parameter The ParameterInterface pointer with a name and value - * @throws IncompatibleParameterException if the ROS parameter could not be written + * @throws ParameterTranslationException if the ROS parameter could not be written * @return A new ROS Parameter object */ rclcpp::Parameter write_parameter(const std::shared_ptr& parameter); @@ -32,7 +32,7 @@ rclcpp::Parameter write_parameter(const std::shared_ptr read_parameter(const rclcpp::Parameter& ros_parameter); @@ -42,7 +42,7 @@ std::shared_ptr read_parameter(const r * the same name and the ROS Parameter value can be interpreted as a ParameterInterface value. * @param ros_parameter The ROS parameter object to read * @param parameter An existing ParameterInterface pointer - * @throws IncompatibleParameterException if the ROS parameter could not be read + * @throws ParameterTranslationException if the ROS parameter could not be read * @return A new ParameterInterface pointer with the updated value */ std::shared_ptr read_parameter_const( @@ -55,7 +55,7 @@ std::shared_ptr read_parameter_const( * @details The destination ParameterInterface must have a compatible parameter name and type. * @param ros_parameter The ROS parameter object to read * @param parameter An existing ParameterInterface pointer - * @throws IncompatibleParameterException if the ROS parameter could not be read + * @throws ParameterTranslationException if the ROS parameter could not be read */ void read_parameter( const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter diff --git a/source/modulo_core/modulo_core/exceptions/__init__.py b/source/modulo_core/modulo_core/exceptions/__init__.py new file mode 100644 index 000000000..e69de29bb diff --git a/source/modulo_core/modulo_core/exceptions/core_exceptions.py b/source/modulo_core/modulo_core/exceptions/core_exceptions.py new file mode 100644 index 000000000..b6fb3155f --- /dev/null +++ b/source/modulo_core/modulo_core/exceptions/core_exceptions.py @@ -0,0 +1,27 @@ +class CoreError(Exception): + """ + A base class for all core exceptions. + """ + + def __init__(self, message: str): + super().__init__(message) + + +class MessageTranslationError(CoreError): + """ + An exception class to notify that the translation of a ROS message failed. + """ + + def __init__(self, message: str): + super().__init__(message) + + +class ParameterTranslationError(CoreError): + """ + An exception class to notify incompatibility when translating parameters from different sources. This is an + exception class to be thrown if there is a problem while translating from a ROS parameter to a state_representation + parameter and vice versa. + """ + + def __init__(self, message: str): + super().__init__(message) diff --git a/source/modulo_core/modulo_core/translators/message_readers.py b/source/modulo_core/modulo_core/translators/message_readers.py index 9c68a8313..f02ae72ca 100644 --- a/source/modulo_core/modulo_core/translators/message_readers.py +++ b/source/modulo_core/modulo_core/translators/message_readers.py @@ -4,6 +4,7 @@ import geometry_msgs.msg as geometry import state_representation as sr from modulo_core.encoded_state import EncodedState +from modulo_core.exceptions.core_exceptions import MessageTranslationError from sensor_msgs.msg import JointState DataT = TypeVar('DataT') @@ -37,11 +38,11 @@ def read_message(state: StateT, message: MsgT) -> StateT: :param state: The state to populate :param message: The ROS message to read from - :raises Exception if the message could not be read + :raises MessageTranslationError if the message could not be read :return: The populated state """ if not isinstance(state, sr.State): - raise RuntimeError("This state type is not supported.") + raise MessageTranslationError("This state type is not supported.") if isinstance(state, sr.CartesianState): if isinstance(message, geometry.Accel): state.set_linear_acceleration(read_xyz(message.linear)) @@ -59,17 +60,20 @@ def read_message(state: StateT, message: MsgT) -> StateT: state.set_force(read_xyz(message.force)) state.set_torque(read_xyz(message.torque)) else: - raise RuntimeError("The provided combination of state type and message type is not supported") + raise MessageTranslationError("The provided combination of state type and message type is not supported") elif isinstance(message, JointState) and isinstance(state, sr.JointState): - state.set_names(message.name) - if len(message.position): - state.set_positions(message.position) - if len(message.velocity): - state.set_velocities(message.velocity) - if len(message.effort): - state.set_torques(message.effort) + try: + state.set_names(message.name) + if len(message.position): + state.set_positions(message.position) + if len(message.velocity): + state.set_velocities(message.velocity) + if len(message.effort): + state.set_torques(message.effort) + except Exception as e: + raise MessageTranslationError(f"{e}") else: - raise RuntimeError("The provided combination of state type and message type is not supported") + raise MessageTranslationError("The provided combination of state type and message type is not supported") return state @@ -79,7 +83,7 @@ def read_stamped_message(state: StateT, message: MsgT) -> StateT: :param state: The state to populate :param message: The ROS message to read from - :raises Exception if the message could not be read + :raises MessageTranslationError if the message could not be read :return: The populated state """ if isinstance(message, geometry.AccelStamped): @@ -94,7 +98,7 @@ def read_stamped_message(state: StateT, message: MsgT) -> StateT: elif isinstance(message, geometry.WrenchStamped): read_message(state, message.wrench) else: - raise RuntimeError("The provided combination of state type and message type is not supported") + raise MessageTranslationError("The provided combination of state type and message type is not supported") state.set_reference_frame(message.header.frame_id) return state @@ -114,7 +118,10 @@ def read_clproto_message(message: EncodedState) -> StateT: Convert an EncodedState message to a state_representation State type. :param message: The EncodedState message to read from - :raises Exception if the message could not be read: + :raises MessageTranslationError if the message could not be read: :return: The decoded clproto message """ - return clproto.decode(message.data.tobytes()) + try: + return clproto.decode(message.data.tobytes()) + except Exception as e: + MessageTranslationError(f"{e}") diff --git a/source/modulo_core/modulo_core/translators/message_writers.py b/source/modulo_core/modulo_core/translators/message_writers.py index 94d723170..0d36ebfdc 100644 --- a/source/modulo_core/modulo_core/translators/message_writers.py +++ b/source/modulo_core/modulo_core/translators/message_writers.py @@ -6,6 +6,7 @@ import rclpy.time import state_representation as sr from modulo_core.encoded_state import EncodedState +from modulo_core.exceptions.core_exceptions import MessageTranslationError from sensor_msgs.msg import JointState DataT = TypeVar('DataT') @@ -19,10 +20,10 @@ def write_xyz(message: Union[geometry.Point, geometry.Vector3], vector: np.array :param message: The message to populate :param vector: The vector to read from - :raises Exception if the message could not be written + :raises MessageTranslationError if the message could not be written """ if vector.size != 3: - raise RuntimeError("Provide a vector of size 3 to transform to a Point or Vector3 message.") + raise MessageTranslationError("Provide a vector of size 3 to transform to a Point or Vector3 message.") message.x = vector[0] message.y = vector[1] message.z = vector[2] @@ -34,10 +35,10 @@ def write_quaternion(message: geometry.Quaternion, quat: np.array): :param message: The message to populate :param quat: The vector to read from - :raises Exception if the message could not be written + :raises MessageTranslationError if the message could not be written """ if quat.size != 4: - raise RuntimeError("Provide a vector of size 4 to transform to a Quaternion message.") + raise MessageTranslationError("Provide a vector of size 4 to transform to a Quaternion message.") message.w = quat[0] message.x = quat[1] message.y = quat[2] @@ -50,12 +51,12 @@ def write_message(message: MsgT, state: StateT): :param message: The ROS message to populate :param state: The state to read from - :raises Exception if the message could not be written + :raises MessageTranslationError if the message could not be written """ if not isinstance(state, sr.State): - raise RuntimeError("This state type is not supported.") + raise MessageTranslationError("This state type is not supported.") if state.is_empty(): - raise RuntimeError(f"{state.get_name()} state is empty while attempting to write it to message.") + raise MessageTranslationError(f"{state.get_name()} state is empty while attempting to write it to message.") if isinstance(state, sr.CartesianState): if isinstance(message, geometry.Accel): write_xyz(message.linear, state.get_linear_acceleration()) @@ -73,14 +74,14 @@ def write_message(message: MsgT, state: StateT): write_xyz(message.force, state.get_force()) write_xyz(message.torque, state.get_torque()) else: - raise RuntimeError("The provided combination of state type and message type is not supported") + raise MessageTranslationError("The provided combination of state type and message type is not supported") elif isinstance(message, JointState) and isinstance(state, sr.JointState): message.name = state.get_names() message.position = state.get_positions().tolist() message.velocity = state.get_velocities().tolist() message.effort = state.get_torques().tolist() else: - raise RuntimeError("The provided combination of state type and message type is not supported") + raise MessageTranslationError("The provided combination of state type and message type is not supported") def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): @@ -90,7 +91,7 @@ def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): :param message: The ROS message to populate :param state: The state to read from :param time: The time of the message - :raises Exception if the message could not be written + :raises MessageTranslationError if the message could not be written """ if isinstance(message, geometry.AccelStamped): write_message(message.accel, state) @@ -104,7 +105,7 @@ def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): elif isinstance(message, geometry.WrenchStamped): write_message(message.wrench, state) else: - raise RuntimeError("The provided combination of state type and message type is not supported") + raise MessageTranslationError("The provided combination of state type and message type is not supported") message.header.stamp = time.to_msg() message.header.frame_id = state.get_reference_frame() @@ -126,8 +127,11 @@ def write_clproto_message(message: EncodedState, state: StateT, clproto_message_ :param message: The EncodedState message to populate :param state: The state to read from :param clproto_message_type: The clproto message type to encode the state - :raises Exception if the message could not be written + :raises MessageTranslationError if the message could not be written """ if not isinstance(state, sr.State): - raise RuntimeError("This state type is not supported.") - message.data = clproto.encode(state, clproto_message_type) + raise MessageTranslationError("This state type is not supported.") + try: + message.data = clproto.encode(state, clproto_message_type) + except Exception as e: + raise MessageTranslationError(f"{e}") diff --git a/source/modulo_core/modulo_core/translators/parameter_translators.py b/source/modulo_core/modulo_core/translators/parameter_translators.py index 51932bf0d..249e8890c 100644 --- a/source/modulo_core/modulo_core/translators/parameter_translators.py +++ b/source/modulo_core/modulo_core/translators/parameter_translators.py @@ -3,6 +3,7 @@ import clproto import numpy as np import state_representation as sr +from modulo_core.exceptions.core_exceptions import ParameterTranslationError from rclpy import Parameter @@ -11,7 +12,7 @@ def write_parameter(parameter: sr.Parameter) -> Parameter: Write a ROS Parameter from a state_representation Parameter. :param parameter: The state_representation parameter to read from - :raises Exception if the parameter could not be written + :raises ParameterTranslationError if the parameter could not be written :return: The resulting ROS parameter """ if parameter.get_parameter_type() == sr.ParameterType.BOOL or \ @@ -41,13 +42,13 @@ def write_parameter(parameter: sr.Parameter) -> Parameter: return Parameter(parameter.get_name(), Parameter.Type.STRING, clproto.to_json( clproto.encode(parameter.get_value(), clproto.MessageType.JOINT_POSITIONS_MESSAGE))) else: - raise RuntimeError(f"Parameter {parameter.get_name()} could not be written!") + raise ParameterTranslationError(f"Parameter {parameter.get_name()} could not be written!") elif parameter.get_parameter_type() == sr.ParameterType.VECTOR or \ parameter.get_parameter_type() == sr.ParameterType.MATRIX: return Parameter(parameter.get_name(), value=parameter.get_value().flatten().tolist(), type_=Parameter.Type.DOUBLE_ARRAY) else: - raise RuntimeError(f"Parameter {parameter.get_name()} could not be written!") + raise ParameterTranslationError(f"Parameter {parameter.get_name()} could not be written!") def copy_parameter_value(source_parameter: sr.Parameter, parameter: sr.Parameter): @@ -58,11 +59,12 @@ def copy_parameter_value(source_parameter: sr.Parameter, parameter: sr.Parameter :param source_parameter: The parameter with a value to copy :param parameter: The destination parameter to be updated - :raises Exception if the copying failed + :raises ParameterTranslationError if the copying failed """ if source_parameter.get_parameter_type() != parameter.get_parameter_type(): - raise RuntimeError(f"Source parameter {source_parameter.get_name()} to be copied does not have the same type " - f"as destination parameter {parameter.get_name()}") + raise ParameterTranslationError( + f"Source parameter {source_parameter.get_name()} to be copied does not have the same type " + f"as destination parameter {parameter.get_name()}") if source_parameter.get_parameter_type() == sr.ParameterType.BOOL or \ source_parameter.get_parameter_type() == sr.ParameterType.BOOL_ARRAY or \ source_parameter.get_parameter_type() == sr.ParameterType.INT or \ @@ -76,7 +78,7 @@ def copy_parameter_value(source_parameter: sr.Parameter, parameter: sr.Parameter parameter.set_value(source_parameter.get_value()) elif source_parameter.get_parameter_type() == sr.ParameterType.STATE: if source_parameter.get_parameter_state_type() != parameter.get_parameter_state_type(): - raise RuntimeError( + raise ParameterTranslationError( f"Source parameter {source_parameter.get_name()} to be copied does not have the same state type " f"as destination parameter {parameter.get_name()}") if source_parameter.get_parameter_state_type() == sr.StateType.CARTESIAN_STATE or \ @@ -85,11 +87,11 @@ def copy_parameter_value(source_parameter: sr.Parameter, parameter: sr.Parameter source_parameter.get_parameter_state_type() == sr.StateType.JOINT_POSITIONS: parameter.set_value(source_parameter.get_value()) else: - raise RuntimeError(f"Could not copy the value from source parameter {source_parameter.get_name()} " - f"into parameter {parameter.get_name()}") + raise ParameterTranslationError(f"Could not copy the value from source parameter " + f"{source_parameter.get_name()} into parameter {parameter.get_name()}") else: - raise RuntimeError(f"Could not copy the value from source parameter {source_parameter.get_name()} " - f"into parameter {parameter.get_name()}") + raise ParameterTranslationError(f"Could not copy the value from source parameter {source_parameter.get_name()} " + f"into parameter {parameter.get_name()}") def read_parameter(ros_parameter: Parameter, parameter: Optional[sr.Parameter] = None) -> sr.Parameter: @@ -98,7 +100,7 @@ def read_parameter(ros_parameter: Parameter, parameter: Optional[sr.Parameter] = :param ros_parameter: The ROS Parameter to read from :param parameter: The state_representation Parameter to populate - :raises Exception if the ROS Parameter could not be read + :raises ParameterTranslationError if the ROS Parameter could not be read :return: The resulting state_representation Parameter """ @@ -107,7 +109,7 @@ def read_new_parameter(ros_param: Parameter) -> sr.Parameter: Read a ROS Parameter object and translate to a state_representation Parameter object. :param ros_param: The ROS Parameter to read from - :raises Exception if the ROS Parameter could not be read + :raises ParameterTranslationError if the ROS Parameter could not be read :return: The resulting state_representation Parameter """ if ros_param.type_ == Parameter.Type.BOOL: @@ -150,11 +152,11 @@ def read_new_parameter(ros_param: Parameter) -> sr.Parameter: return sr.Parameter(ros_param.name, clproto.decode(encoding), sr.ParameterType.STATE, sr.StateType.JOINT_POSITIONS) else: - raise RuntimeError(f"Parameter {ros_param.name} has an unsupported encoded message type!") + raise ParameterTranslationError(f"Parameter {ros_param.name} has an unsupported encoded message type!") elif ros_param.type_ == Parameter.Type.BYTE_ARRAY: - raise RuntimeError("Parameter byte arrays are not currently supported.") + raise ParameterTranslationError("Parameter byte arrays are not currently supported.") else: - raise RuntimeError(f"Parameter {ros_param.name} could not be read!") + raise ParameterTranslationError(f"Parameter {ros_param.name} could not be read!") new_parameter = read_new_parameter(ros_parameter) if parameter is None: @@ -171,12 +173,12 @@ def read_parameter_const(ros_parameter: Parameter, parameter: sr.Parameter) -> s :param ros_parameter: The ROS Parameter to read from :param parameter: The state_representation Parameter to update - :raises Exception if the ROS Parameter could not be read + :raises ParameterTranslationError if the ROS Parameter could not be read :return: The resulting state_representation Parameter """ if ros_parameter.name != parameter.get_name(): - raise RuntimeError(f"The ROS parameter {ros_parameter.name} to be read does not have " - f"the same name as the reference parameter {parameter.get_name()}") + raise ParameterTranslationError(f"The ROS parameter {ros_parameter.name} to be read does not have " + f"the same name as the reference parameter {parameter.get_name()}") new_parameter = read_parameter(ros_parameter) if new_parameter.get_parameter_type() == parameter.get_parameter_type(): return new_parameter @@ -188,14 +190,15 @@ def read_parameter_const(ros_parameter: Parameter, parameter: sr.Parameter) -> s elif parameter.get_parameter_type() == sr.ParameterType.MATRIX: matrix = parameter.get_value() if matrix.size != len(value): - raise RuntimeError(f"The ROS parameter {ros_parameter.name} with type double array has size " - f"{len(value)} while the reference parameter matrix {parameter.get_name()} " - f"has size {len(matrix)}") + raise ParameterTranslationError( + f"The ROS parameter {ros_parameter.name} with type double array has size " + f"{len(value)} while the reference parameter matrix {parameter.get_name()} " + f"has size {len(matrix)}") matrix = np.array(value).reshape(matrix.shape) return sr.Parameter(parameter.get_name(), matrix, sr.ParameterType.MATRIX) else: - raise RuntimeError(f"The ROS parameter {ros_parameter.name} with type double array cannot be interpreted " - f"by reference parameter {parameter.get_name()} (type code " - f"{parameter.get_parameter_type()}") + raise ParameterTranslationError( + f"The ROS parameter {ros_parameter.name} with type double array cannot be interpreted " + f"by reference parameter {parameter.get_name()} (type code {parameter.get_parameter_type()}") else: - raise RuntimeError(f"Something went wrong while reading parameter {parameter.get_name()}") + raise ParameterTranslationError(f"Something went wrong while reading parameter {parameter.get_name()}") diff --git a/source/modulo_core/src/translators/message_readers.cpp b/source/modulo_core/src/translators/message_readers.cpp index 647514e93..3bd9bcf78 100644 --- a/source/modulo_core/src/translators/message_readers.cpp +++ b/source/modulo_core/src/translators/message_readers.cpp @@ -66,15 +66,19 @@ void read_message(state_representation::CartesianState& state, const geometry_ms } void read_message(state_representation::JointState& state, const sensor_msgs::msg::JointState& message) { - state.set_names(message.name); - if (!message.position.empty()) { - state.set_positions(Eigen::VectorXd::Map(message.position.data(), message.position.size())); - } - if (!message.velocity.empty()) { - state.set_velocities(Eigen::VectorXd::Map(message.velocity.data(), message.velocity.size())); - } - if (!message.effort.empty()) { - state.set_torques(Eigen::VectorXd::Map(message.effort.data(), message.effort.size())); + try { + state.set_names(message.name); + if (!message.position.empty()) { + state.set_positions(Eigen::VectorXd::Map(message.position.data(), message.position.size())); + } + if (!message.velocity.empty()) { + state.set_velocities(Eigen::VectorXd::Map(message.velocity.data(), message.velocity.size())); + } + if (!message.effort.empty()) { + state.set_torques(Eigen::VectorXd::Map(message.effort.data(), message.effort.size())); + } + } catch (const std::exception& ex) { + throw exceptions::MessageTranslationException(ex.what()); } } diff --git a/source/modulo_core/src/translators/message_writers.cpp b/source/modulo_core/src/translators/message_writers.cpp index 557cffe0a..fbbffd6a3 100644 --- a/source/modulo_core/src/translators/message_writers.cpp +++ b/source/modulo_core/src/translators/message_writers.cpp @@ -1,6 +1,5 @@ #include "modulo_core/translators/message_writers.hpp" -#include #include using namespace state_representation; @@ -28,7 +27,9 @@ static void write_quaternion(geometry_msgs::msg::Quaternion& message, const Eige void write_message(geometry_msgs::msg::Accel& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } write_vector3(message.linear, state.get_linear_acceleration()); write_vector3(message.angular, state.get_angular_acceleration()); @@ -42,7 +43,9 @@ void write_message(geometry_msgs::msg::AccelStamped& message, const CartesianSta void write_message(geometry_msgs::msg::Pose& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } write_point(message.position, state.get_position()); write_quaternion(message.orientation, state.get_orientation()); @@ -56,7 +59,9 @@ void write_message(geometry_msgs::msg::PoseStamped& message, const CartesianStat void write_message(geometry_msgs::msg::Transform& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } write_vector3(message.translation, state.get_position()); write_quaternion(message.rotation, state.get_orientation()); @@ -72,7 +77,9 @@ write_message(geometry_msgs::msg::TransformStamped& message, const CartesianStat void write_message(geometry_msgs::msg::Twist& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } write_vector3(message.linear, state.get_linear_velocity()); write_vector3(message.angular, state.get_angular_velocity()); @@ -86,7 +93,9 @@ void write_message(geometry_msgs::msg::TwistStamped& message, const CartesianSta void write_message(geometry_msgs::msg::Wrench& message, const CartesianState& state, const rclcpp::Time&) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } write_vector3(message.force, state.get_force()); write_vector3(message.torque, state.get_torque()); @@ -100,7 +109,9 @@ void write_message(geometry_msgs::msg::WrenchStamped& message, const CartesianSt void write_message(sensor_msgs::msg::JointState& message, const JointState& state, const rclcpp::Time& time) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } message.header.stamp = time; message.name = state.get_names(); @@ -114,7 +125,9 @@ void write_message(sensor_msgs::msg::JointState& message, const JointState& stat void write_message(tf2_msgs::msg::TFMessage& message, const CartesianState& state, const rclcpp::Time& time) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } geometry_msgs::msg::TransformStamped transform; write_message(transform, state, time); @@ -124,7 +137,9 @@ void write_message(tf2_msgs::msg::TFMessage& message, const CartesianState& stat template void write_message(U& message, const Parameter& state, const rclcpp::Time&) { if (state.is_empty()) { - throw exceptions::EmptyStateException(state.get_name() + " state is empty while attempting to publish it"); + throw exceptions::MessageTranslationException( + state.get_name() + " state is empty while attempting to write it to message" + ); } message.data = state.get_value(); } diff --git a/source/modulo_core/src/translators/parameter_translators.cpp b/source/modulo_core/src/translators/parameter_translators.cpp index 6cf7a7ce8..f55b38143 100644 --- a/source/modulo_core/src/translators/parameter_translators.cpp +++ b/source/modulo_core/src/translators/parameter_translators.cpp @@ -6,7 +6,7 @@ #include #include -#include "modulo_core/exceptions/IncompatibleParameterException.hpp" +#include "modulo_core/exceptions/ParameterTranslationException.hpp" using namespace state_representation; @@ -17,7 +17,7 @@ void copy_parameter_value( const std::shared_ptr& parameter ) { if (source_parameter->get_parameter_type() != parameter->get_parameter_type()) { - throw exceptions::IncompatibleParameterException( + throw exceptions::ParameterTranslationException( "Source parameter " + source_parameter->get_name() + " to be copied does not have the same type as destination parameter " + parameter->get_name()); } @@ -54,7 +54,7 @@ void copy_parameter_value( return; case ParameterType::STATE: if (source_parameter->get_parameter_state_type() != parameter->get_parameter_state_type()) { - throw exceptions::IncompatibleParameterException( + throw exceptions::ParameterTranslationException( "Source parameter " + source_parameter->get_name() + " to be copied does not have the same parameter state type as destination parameter " + parameter->get_name()); @@ -79,7 +79,7 @@ void copy_parameter_value( default: break; } - throw InvalidParameterException( + throw exceptions::ParameterTranslationException( "Could not copy the value from source parameter " + source_parameter->get_name() + " into parameter " + parameter->get_name()); } @@ -130,7 +130,7 @@ rclcpp::Parameter write_parameter(const std::shared_ptrget_name() + " could not be written!"); + throw exceptions::ParameterTranslationException("Parameter " + parameter->get_name() + " could not be written!"); } std::shared_ptr read_parameter(const rclcpp::Parameter& parameter) { @@ -171,17 +171,17 @@ std::shared_ptr read_parameter(const r case clproto::JOINT_POSITIONS_MESSAGE: return make_shared_parameter(parameter.get_name(), clproto::decode(encoding)); default: - throw InvalidParameterException( + throw exceptions::ParameterTranslationException( "Parameter " + parameter.get_name() + " has an unsupported encoded message type"); } } case rclcpp::PARAMETER_BYTE_ARRAY: // TODO: try clproto decode, re-use logic from above - throw InvalidParameterException("Parameter byte arrays are not currently supported."); + throw exceptions::ParameterTranslationException("Parameter byte arrays are not currently supported."); default: break; } - throw InvalidParameterException("Parameter " + parameter.get_name() + " could not be read!"); + throw exceptions::ParameterTranslationException("Parameter " + parameter.get_name() + " could not be read!"); } std::shared_ptr read_parameter_const( @@ -189,7 +189,7 @@ std::shared_ptr read_parameter_const( const std::shared_ptr& parameter ) { if (ros_parameter.get_name() != parameter->get_name()) { - throw exceptions::IncompatibleParameterException( + throw exceptions::ParameterTranslationException( "The ROS parameter " + ros_parameter.get_name() + " to be read does not have the same name as the reference parameter " + parameter->get_name()); } @@ -209,7 +209,7 @@ std::shared_ptr read_parameter_const( case ParameterType::MATRIX: { auto matrix = parameter->get_parameter_value(); if (static_cast(matrix.size()) != value.size()) { - throw exceptions::IncompatibleParameterException( + throw exceptions::ParameterTranslationException( "The ROS parameter " + ros_parameter.get_name() + " with type double array has size " + std::to_string(value.size()) + " while the reference parameter matrix " + parameter->get_name() + " has size " + std::to_string(matrix.size())); @@ -219,7 +219,7 @@ std::shared_ptr read_parameter_const( break; } default: - throw exceptions::IncompatibleParameterException( + throw exceptions::ParameterTranslationException( "The ROS parameter " + ros_parameter.get_name() + " with type double array cannot be interpreted by reference parameter " + parameter->get_name() + " (type code " + std::to_string(static_cast(parameter->get_parameter_type())) + ")"); @@ -227,7 +227,7 @@ std::shared_ptr read_parameter_const( break; } default: - throw state_representation::exceptions::InvalidParameterException( + throw exceptions::ParameterTranslationException( "Something went wrong while reading parameter " + parameter->get_name()); } return new_parameter; diff --git a/source/modulo_core/test/cpp_tests/translators/test_messages.cpp b/source/modulo_core/test/cpp_tests/translators/test_messages.cpp index a098bc138..5b62708b0 100644 --- a/source/modulo_core/test/cpp_tests/translators/test_messages.cpp +++ b/source/modulo_core/test/cpp_tests/translators/test_messages.cpp @@ -55,7 +55,7 @@ TEST_F(MessageTranslatorsTest, TestAccel) { expect_vector_equal(state_.get_angular_acceleration(), accel.angular); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_message(accel, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + EXPECT_THROW(write_message(accel, new_state, clock_.now()), modulo_core::exceptions::MessageTranslationException); read_message(new_state, accel); EXPECT_TRUE(new_state.get_acceleration().isApprox(state_.get_acceleration())); @@ -77,7 +77,7 @@ TEST_F(MessageTranslatorsTest, TestPose) { expect_quaternion_equal(state_.get_orientation(), pose.orientation); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_message(pose, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + EXPECT_THROW(write_message(pose, new_state, clock_.now()), modulo_core::exceptions::MessageTranslationException); read_message(new_state, pose); EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); @@ -99,7 +99,7 @@ TEST_F(MessageTranslatorsTest, TestTransform) { expect_quaternion_equal(state_.get_orientation(), tf.rotation); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_message(tf, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + EXPECT_THROW(write_message(tf, new_state, clock_.now()), modulo_core::exceptions::MessageTranslationException); read_message(new_state, tf); EXPECT_TRUE(new_state.get_pose().isApprox(state_.get_pose())); @@ -123,7 +123,7 @@ TEST_F(MessageTranslatorsTest, TestTwist) { expect_vector_equal(state_.get_angular_velocity(), twist.angular); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_message(twist, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + EXPECT_THROW(write_message(twist, new_state, clock_.now()), modulo_core::exceptions::MessageTranslationException); read_message(new_state, twist); EXPECT_TRUE(new_state.get_twist().isApprox(state_.get_twist())); @@ -145,7 +145,7 @@ TEST_F(MessageTranslatorsTest, TestWrench) { expect_vector_equal(state_.get_torque(), wrench.torque); state_representation::CartesianState new_state("new"); - EXPECT_THROW(write_message(wrench, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + EXPECT_THROW(write_message(wrench, new_state, clock_.now()), modulo_core::exceptions::MessageTranslationException); read_message(new_state, wrench); EXPECT_TRUE(new_state.get_wrench().isApprox(state_.get_wrench())); @@ -171,7 +171,7 @@ TEST_F(MessageTranslatorsTest, TestJointState) { } auto new_state = state_representation::JointState("test", {"1", "2", "3"}); - EXPECT_THROW(write_message(message, new_state, clock_.now()), state_representation::exceptions::EmptyStateException); + EXPECT_THROW(write_message(message, new_state, clock_.now()), modulo_core::exceptions::MessageTranslationException); read_message(new_state, message); EXPECT_TRUE(new_state.get_positions().isApprox(joint_state_.get_positions())); EXPECT_TRUE(new_state.get_velocities().isApprox(joint_state_.get_velocities())); diff --git a/source/modulo_core/test/python_tests/translators/test_messages.py b/source/modulo_core/test/python_tests/translators/test_messages.py index 1e92e196e..28ef504d0 100644 --- a/source/modulo_core/test/python_tests/translators/test_messages.py +++ b/source/modulo_core/test/python_tests/translators/test_messages.py @@ -6,6 +6,7 @@ import pytest import state_representation as sr from modulo_core.encoded_state import EncodedState +from modulo_core.exceptions.core_exceptions import MessageTranslationError from rclpy.clock import Clock from sensor_msgs.msg import JointState @@ -32,7 +33,7 @@ def test_accel(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(read_xyz(message.angular), cart_state.get_angular_acceleration()) new_state = sr.CartesianState("new") - with pytest.raises(RuntimeError): + with pytest.raises(MessageTranslationError): modulo_writers.write_message(new_state, message) modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_acceleration(), new_state.get_acceleration()) @@ -55,7 +56,7 @@ def test_pose(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(read_quaternion(message.orientation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") - with pytest.raises(RuntimeError): + with pytest.raises(MessageTranslationError): modulo_writers.write_message(new_state, message) modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) @@ -78,7 +79,7 @@ def test_transform(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(read_quaternion(message.rotation), cart_state.get_orientation_coefficients()) new_state = sr.CartesianState("new") - with pytest.raises(RuntimeError): + with pytest.raises(MessageTranslationError): modulo_writers.write_message(new_state, message) modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_pose(), new_state.get_pose()) @@ -103,7 +104,7 @@ def test_twist(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(read_xyz(message.angular), cart_state.get_angular_velocity()) new_state = sr.CartesianState("new") - with pytest.raises(RuntimeError): + with pytest.raises(MessageTranslationError): modulo_writers.write_message(new_state, message) modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_twist(), new_state.get_twist()) @@ -126,7 +127,7 @@ def test_wrench(cart_state: sr.CartesianState, clock: Clock): assert_np_array_equal(read_xyz(message.torque), cart_state.get_torque()) new_state = sr.CartesianState("new") - with pytest.raises(RuntimeError): + with pytest.raises(MessageTranslationError): modulo_writers.write_message(new_state, message) modulo_readers.read_message(new_state, message) assert_np_array_equal(cart_state.get_wrench(), new_state.get_wrench()) @@ -152,7 +153,7 @@ def test_joint_state(joint_state: sr.JointState): assert_np_array_equal(message.effort, joint_state.get_torques()) new_state = sr.JointState("test", ["1", "2", "3"]) - with pytest.raises(RuntimeError): + with pytest.raises(MessageTranslationError): modulo_writers.write_message(new_state, message) modulo_readers.read_message(new_state, message) for i in range(joint_state.get_size()): From 28b7366453dba1f5bc6e855a92eb3a70145adc6b Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Tue, 5 Jul 2022 08:35:57 +0200 Subject: [PATCH 52/71] Don't run image build on push on develop (#95) --- .github/workflows/build-push.yml | 1 - 1 file changed, 1 deletion(-) diff --git a/.github/workflows/build-push.yml b/.github/workflows/build-push.yml index 879671fb2..39a2a56ff 100644 --- a/.github/workflows/build-push.yml +++ b/.github/workflows/build-push.yml @@ -5,7 +5,6 @@ on: push: branches: - main - - develop release: types: [published] workflow_dispatch: From fee750b8f47786e102519026af6edd358e5c825f Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Wed, 6 Jul 2022 10:46:23 +0200 Subject: [PATCH 53/71] Improve parameter translators for NOT_SET cases (#96) * Add utility to translate ROS parameter type * Handle NOT_SET parameters in translators * Use new helper function from state representation * Remove duplicated line --- source/modulo_core/CMakeLists.txt | 2 +- .../translators/parameter_translators.hpp | 9 +++ .../translators/parameter_translators.py | 38 ++++++++++++- .../src/translators/parameter_translators.cpp | 56 +++++++++++++++---- .../parameters/test_parameter_translators.cpp | 10 +++- .../translators/test_parameters.py | 19 +++++-- 6 files changed, 112 insertions(+), 22 deletions(-) diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index 10c0d9342..af5fa334b 100644 --- a/source/modulo_core/CMakeLists.txt +++ b/source/modulo_core/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 6.0.2 REQUIRED COMPONENTS state_representation) +find_package(control_libraries 6.0.5 REQUIRED COMPONENTS state_representation) find_package(clproto 6.0.0 REQUIRED) include_directories(include) diff --git a/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp b/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp index 12fe2fa03..22aad31ee 100644 --- a/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp +++ b/source/modulo_core/include/modulo_core/translators/parameter_translators.hpp @@ -60,4 +60,13 @@ std::shared_ptr read_parameter_const( void read_parameter( const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter ); + + +/** + * @brief Given a state representation parameter type, get the corresponding ROS parameter type. + * @param parameter_type The state representation parameter type + * @return The corresponding ROS parameter type + */ +rclcpp::ParameterType get_ros_parameter_type(const state_representation::ParameterType& parameter_type); + }// namespace modulo_core::exceptions diff --git a/source/modulo_core/modulo_core/translators/parameter_translators.py b/source/modulo_core/modulo_core/translators/parameter_translators.py index 249e8890c..ba0e221cf 100644 --- a/source/modulo_core/modulo_core/translators/parameter_translators.py +++ b/source/modulo_core/modulo_core/translators/parameter_translators.py @@ -1,3 +1,4 @@ +from copy import copy from typing import Optional import clproto @@ -15,7 +16,9 @@ def write_parameter(parameter: sr.Parameter) -> Parameter: :raises ParameterTranslationError if the parameter could not be written :return: The resulting ROS parameter """ - if parameter.get_parameter_type() == sr.ParameterType.BOOL or \ + if parameter.is_empty(): + return Parameter(parameter.get_name(), value=None, type_=Parameter.Type.NOT_SET) + elif parameter.get_parameter_type() == sr.ParameterType.BOOL or \ parameter.get_parameter_type() == sr.ParameterType.INT or \ parameter.get_parameter_type() == sr.ParameterType.DOUBLE or \ parameter.get_parameter_type() == sr.ParameterType.STRING: @@ -179,6 +182,8 @@ def read_parameter_const(ros_parameter: Parameter, parameter: sr.Parameter) -> s if ros_parameter.name != parameter.get_name(): raise ParameterTranslationError(f"The ROS parameter {ros_parameter.name} to be read does not have " f"the same name as the reference parameter {parameter.get_name()}") + if ros_parameter.type_ == Parameter.Type.NOT_SET: + return copy(parameter) new_parameter = read_parameter(ros_parameter) if new_parameter.get_parameter_type() == parameter.get_parameter_type(): return new_parameter @@ -201,4 +206,33 @@ def read_parameter_const(ros_parameter: Parameter, parameter: sr.Parameter) -> s f"The ROS parameter {ros_parameter.name} with type double array cannot be interpreted " f"by reference parameter {parameter.get_name()} (type code {parameter.get_parameter_type()}") else: - raise ParameterTranslationError(f"Something went wrong while reading parameter {parameter.get_name()}") + raise ParameterTranslationError( + f"The ROS parameter has an incompatible type for component parameter '{parameter.get_name()}': " + f"expected {parameter.get_parameter_type().name}, got {ros_parameter.type_.name}") + + +def get_ros_parameter_type(parameter_type: sr.ParameterType) -> Parameter.Type: + """ + Given a state representation parameter type, get the corresponding ROS parameter type. + + :param parameter_type: The state representation parameter type + :return: The corresponding ROS parameter type + """ + if parameter_type == sr.ParameterType.BOOL: + return Parameter.Type.BOOL + elif parameter_type == sr.ParameterType.BOOL_ARRAY: + return Parameter.Type.BOOL_ARRAY + elif parameter_type == sr.ParameterType.INT: + return Parameter.Type.INTEGER + elif parameter_type == sr.ParameterType.INT_ARRAY: + return Parameter.Type.INTEGER_ARRAY + elif parameter_type == sr.ParameterType.DOUBLE: + return Parameter.Type.DOUBLE + elif parameter_type in [sr.ParameterType.DOUBLE_ARRAY, sr.ParameterType.VECTOR, sr.ParameterType.MATRIX]: + return Parameter.Type.DOUBLE_ARRAY + elif parameter_type in [sr.ParameterType.STRING, sr.ParameterType.STATE]: + return Parameter.Type.STRING + elif parameter_type == sr.ParameterType.STRING_ARRAY: + return Parameter.Type.STRING_ARRAY + else: + return Parameter.Type.NOT_SET diff --git a/source/modulo_core/src/translators/parameter_translators.cpp b/source/modulo_core/src/translators/parameter_translators.cpp index f55b38143..bcb0abd17 100644 --- a/source/modulo_core/src/translators/parameter_translators.cpp +++ b/source/modulo_core/src/translators/parameter_translators.cpp @@ -60,16 +60,16 @@ void copy_parameter_value( + parameter->get_name()); } switch (source_parameter->get_parameter_state_type()) { - case state_representation::StateType::CARTESIAN_STATE: + case StateType::CARTESIAN_STATE: parameter->set_parameter_value(source_parameter->get_parameter_value()); return; - case state_representation::StateType::CARTESIAN_POSE: + case StateType::CARTESIAN_POSE: parameter->set_parameter_value(source_parameter->get_parameter_value()); return; - case state_representation::StateType::JOINT_STATE: + case StateType::JOINT_STATE: parameter->set_parameter_value(source_parameter->get_parameter_value()); return; - case state_representation::StateType::JOINT_POSITIONS: + case StateType::JOINT_POSITIONS: parameter->set_parameter_value(source_parameter->get_parameter_value()); return; default: @@ -84,7 +84,10 @@ void copy_parameter_value( + parameter->get_name()); } -rclcpp::Parameter write_parameter(const std::shared_ptr& parameter) { +rclcpp::Parameter write_parameter(const std::shared_ptr& parameter) { + if (parameter->is_empty()) { + return rclcpp::Parameter(parameter->get_name()); + } switch (parameter->get_parameter_type()) { case ParameterType::BOOL: return {parameter->get_name(), parameter->get_parameter_value()}; @@ -133,7 +136,7 @@ rclcpp::Parameter write_parameter(const std::shared_ptrget_name() + " could not be written!"); } -std::shared_ptr read_parameter(const rclcpp::Parameter& parameter) { +std::shared_ptr read_parameter(const rclcpp::Parameter& parameter) { switch (parameter.get_type()) { case rclcpp::PARAMETER_BOOL: return make_shared_parameter(parameter.get_name(), parameter.as_bool()); @@ -172,7 +175,8 @@ std::shared_ptr read_parameter(const r return make_shared_parameter(parameter.get_name(), clproto::decode(encoding)); default: throw exceptions::ParameterTranslationException( - "Parameter " + parameter.get_name() + " has an unsupported encoded message type"); + "Parameter " + parameter.get_name() + " has an unsupported encoded message type" + ); } } case rclcpp::PARAMETER_BYTE_ARRAY: @@ -184,15 +188,18 @@ std::shared_ptr read_parameter(const r throw exceptions::ParameterTranslationException("Parameter " + parameter.get_name() + " could not be read!"); } -std::shared_ptr read_parameter_const( - const rclcpp::Parameter& ros_parameter, - const std::shared_ptr& parameter +std::shared_ptr read_parameter_const( + const rclcpp::Parameter& ros_parameter, const std::shared_ptr& parameter ) { if (ros_parameter.get_name() != parameter->get_name()) { throw exceptions::ParameterTranslationException( "The ROS parameter " + ros_parameter.get_name() + " to be read does not have the same name as the reference parameter " + parameter->get_name()); } + if (ros_parameter.get_type() == rclcpp::PARAMETER_NOT_SET) { + return make_shared_parameter_interface( + parameter->get_name(), parameter->get_parameter_type(), parameter->get_parameter_state_type()); + } auto new_parameter = read_parameter(ros_parameter); if (new_parameter->get_parameter_type() == parameter->get_parameter_type()) { return new_parameter; @@ -222,7 +229,8 @@ std::shared_ptr read_parameter_const( throw exceptions::ParameterTranslationException( "The ROS parameter " + ros_parameter.get_name() + " with type double array cannot be interpreted by reference parameter " + parameter->get_name() - + " (type code " + std::to_string(static_cast(parameter->get_parameter_type())) + ")"); + + " (type code " + std::to_string(static_cast(parameter->get_parameter_type())) + ")" + ); } break; } @@ -239,4 +247,30 @@ void read_parameter( auto new_parameter = read_parameter_const(ros_parameter, parameter); copy_parameter_value(new_parameter, parameter); } + +rclcpp::ParameterType get_ros_parameter_type(const ParameterType& parameter_type) { + switch (parameter_type) { + case ParameterType::BOOL: + return rclcpp::ParameterType::PARAMETER_BOOL; + case ParameterType::BOOL_ARRAY: + return rclcpp::ParameterType::PARAMETER_BOOL_ARRAY; + case ParameterType::INT: + return rclcpp::ParameterType::PARAMETER_INTEGER; + case ParameterType::INT_ARRAY: + return rclcpp::ParameterType::PARAMETER_INTEGER_ARRAY; + case ParameterType::DOUBLE: + return rclcpp::ParameterType::PARAMETER_DOUBLE; + case ParameterType::DOUBLE_ARRAY: + case ParameterType::VECTOR: + case ParameterType::MATRIX: + return rclcpp::ParameterType::PARAMETER_DOUBLE_ARRAY; + case ParameterType::STRING: + return rclcpp::ParameterType::PARAMETER_STRING; + case ParameterType::STRING_ARRAY: + case ParameterType::STATE: + return rclcpp::ParameterType::PARAMETER_STRING_ARRAY; + default: + return rclcpp::ParameterType::PARAMETER_NOT_SET; + } +} }// namespace modulo_core::translators diff --git a/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp index 3c26755f2..7201f89b7 100644 --- a/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp +++ b/source/modulo_core/test/cpp_tests/translators/parameters/test_parameter_translators.cpp @@ -1,5 +1,6 @@ #include +#include "modulo_core/exceptions/ParameterTranslationException.hpp" #include "modulo_core/translators/parameter_translators.hpp" using namespace modulo_core::translators; @@ -54,16 +55,21 @@ TYPED_TEST_SUITE_P(ParameterTranslationTest); TYPED_TEST_P(ParameterTranslationTest, Write) { for (auto const& test_case : this->test_cases_) { - auto param = state_representation::make_shared_parameter("test", std::get<0>(test_case)); + auto param = std::make_shared>("test"); rclcpp::Parameter ros_param; EXPECT_NO_THROW(ros_param = write_parameter(param)); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_NOT_SET); + param = state_representation::make_shared_parameter("test", std::get<0>(test_case)); + EXPECT_NO_THROW(ros_param = write_parameter(param)); EXPECT_EQ(ros_param, rclcpp::Parameter("test", std::get<0>(test_case))); } } TYPED_TEST_P(ParameterTranslationTest, ReadAndReWrite) { + auto ros_param = rclcpp::Parameter("test"); + EXPECT_THROW(read_parameter(ros_param), modulo_core::exceptions::ParameterTranslationException); for (auto const& [value, type]: this->test_cases_) { - auto ros_param = rclcpp::Parameter("test", value); + ros_param = rclcpp::Parameter("test", value); std::shared_ptr param; ASSERT_NO_THROW(param = read_parameter(ros_param)); EXPECT_EQ(param->get_name(), ros_param.get_name()); diff --git a/source/modulo_core/test/python_tests/translators/test_parameters.py b/source/modulo_core/test/python_tests/translators/test_parameters.py index 3469aab2d..498fc7d0d 100644 --- a/source/modulo_core/test/python_tests/translators/test_parameters.py +++ b/source/modulo_core/test/python_tests/translators/test_parameters.py @@ -3,6 +3,7 @@ import pytest import state_representation as sr from modulo_core.translators.parameter_translators import write_parameter, read_parameter, read_parameter_const +from modulo_core.exceptions.core_exceptions import ParameterTranslationError from rclpy import Parameter @@ -17,13 +18,18 @@ def test_parameter_write(parameters): for name, value_types in parameters.items(): param = sr.Parameter(name, value_types[1]) ros_param = write_parameter(param) - assert ros_param.type_ == value_types[3] + assert ros_param.type_ == Parameter.Type.NOT_SET param = sr.Parameter(name, value_types[0], value_types[1]) ros_param = write_parameter(param) + assert ros_param.type_ == value_types[3] assert ros_param.to_parameter_msg() == Parameter(name, value=value_types[0]).to_parameter_msg() def test_parameter_read_write(parameters): + # A ROS parameter with type NOT_SET cannot be interpreted as sr.Parameter + ros_param = Parameter("name", type_=Parameter.Type.NOT_SET) + with pytest.raises(ParameterTranslationError): + read_parameter(ros_param) for name, value_types in parameters.items(): ros_param = Parameter(name, value=value_types[0]) param = read_parameter(ros_param) @@ -69,9 +75,10 @@ def test_state_parameter_write(state_parameters): for name, value_types in state_parameters.items(): param = sr.Parameter(name, sr.ParameterType.STATE, value_types[1]) ros_param = write_parameter(param) - assert ros_param.type_ == Parameter.Type.STRING + assert ros_param.type_ == Parameter.Type.NOT_SET param = sr.Parameter(name, value_types[0], sr.ParameterType.STATE, value_types[1]) ros_param = write_parameter(param) + assert ros_param.type_ == Parameter.Type.STRING assert ros_param.name == param.get_name() state = clproto.decode(clproto.from_json(ros_param.value)) assert state.get_name() == value_types[0].get_name() @@ -102,11 +109,11 @@ def test_vector_parameter_read_write(): vec = np.random.rand(3) param = sr.Parameter("vector", sr.ParameterType.VECTOR) ros_param = write_parameter(param) - assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY + assert ros_param.type_ == Parameter.Type.NOT_SET param = sr.Parameter("vector", vec, sr.ParameterType.VECTOR) ros_param = write_parameter(param) - assert ros_param.name == param.get_name() assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY + assert ros_param.name == param.get_name() ros_vec = ros_param.get_parameter_value().double_array_value assert vec.tolist() == ros_vec.tolist() @@ -125,11 +132,11 @@ def test_matrix_parameter_read_write(): mat = np.random.rand(3, 4) param = sr.Parameter("vector", sr.ParameterType.MATRIX) ros_param = write_parameter(param) - assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY + assert ros_param.type_ == Parameter.Type.NOT_SET param = sr.Parameter("matrix", mat, sr.ParameterType.MATRIX) ros_param = write_parameter(param) - assert ros_param.name == param.get_name() assert ros_param.type_ == Parameter.Type.DOUBLE_ARRAY + assert ros_param.name == param.get_name() ros_mat = ros_param.get_parameter_value().double_array_value assert mat.flatten().tolist() == ros_mat.tolist() From 7e169a78b777be8027797d94bed3d9e345f09285 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 8 Jul 2022 09:36:24 +0200 Subject: [PATCH 54/71] Don't start execute thread on construction (#98) * Don't start execute thread on construction * Fix test * Switch constructor arguments * Remove start thread option from constructor * Rename functions --- .../include/modulo_components/Component.hpp | 20 +++++------- .../modulo_components/component.py | 32 +++++++------------ source/modulo_components/src/Component.cpp | 20 +++++------- .../component_public_interfaces.hpp | 5 +-- .../test/cpp/test_component.cpp | 2 +- 5 files changed, 31 insertions(+), 48 deletions(-) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 80b689f7b..2de1c156c 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -20,12 +20,9 @@ class Component : public ComponentInterface { /** * @brief Constructor from node options. * @param node_options Node options as used in ROS2 Node - * @param start_thread If true, start the execution thread on construction * @param fallback_name The name of the component if it was not provided through the node options */ - explicit Component( - const rclcpp::NodeOptions& node_options, bool start_thread = true, const std::string& fallback_name = "Component" - ); + explicit Component(const rclcpp::NodeOptions& node_options, const std::string& fallback_name = "Component"); /** * @brief Virtual default destructor. @@ -36,13 +33,13 @@ class Component : public ComponentInterface { /** * @brief Start the execution thread. */ - void start_thread(); + void execute(); /** * @brief Execute the component logic. To be redefined in derived classes. * @return True, if the execution was successful, false otherwise */ - virtual bool execute(); + virtual bool on_execute_callback(); /** * @brief Add and configure an output signal of the component. @@ -60,16 +57,15 @@ class Component : public ComponentInterface { private: /** - * @brief Step function that is called periodically and publishes predicates, - * outputs, and evaluates daemon callbacks. + * @brief Step function that is called periodically and publishes predicates, outputs, and evaluates daemon callbacks. */ void step() override; /** - * @brief Run the execution function in a try catch block and - * set the predicates according to the outcome of the execution. + * @brief Run the execution function in a try catch block and set the predicates according to the outcome of the + * execution. */ - void run(); + void on_execute(); // TODO hide ROS methods using ComponentInterface::create_output; @@ -80,7 +76,7 @@ class Component : public ComponentInterface { using ComponentInterface::publish_outputs; using ComponentInterface::evaluate_periodic_callbacks; - std::thread run_thread_; ///< The execution thread of the component + std::thread execute_thread_; ///< The execution thread of the component bool started_; ///< Flag that indicates if execution has started or not }; diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index b21060f43..35d5df180 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -17,7 +17,7 @@ class Component(ComponentInterface): run_thread (Thread): the execution thread """ - def __init__(self, node_name: str, start_thread=True, *kargs, **kwargs): + def __init__(self, node_name: str, *kargs, **kwargs): """ Constructs all the necessary attributes and declare all the parameters. Parameters: @@ -26,12 +26,9 @@ def __init__(self, node_name: str, start_thread=True, *kargs, **kwargs): """ super().__init__(node_name, *kargs, **kwargs) self.__started = False - self.__run_thread = None + self.__execute_thread = None self.add_predicate("is_finished", False) - if start_thread: - self.start_thread() - def _step(self): """ Step function that is called periodically. @@ -44,34 +41,34 @@ def _step(self): except Exception as e: self.get_logger().error(f"Failed to execute step function: {e}", throttle_duration_sec=1.0) - def start_thread(self): + def execute(self): """ Start the execution thread. """ if self.__started: - self.get_logger().error(f"Run thread for component {self.get_name()} has already been started", - throttle_duration_sec=1.0) + self.get_logger().error("Failed to start execution thread: Thread has already been started.") return self.__started = True - self.__run_thread = Thread(target=self.__run) - self.__run_thread.start() + self.__execute_thread = Thread(target=self.__on_execute) + self.__execute_thread.start() - def __run(self): + def __on_execute(self): """ Run the execution function in a try catch block and set the predicates according to the outcome of the execution. """ try: - if not self.execute(): + if not self.on_execute_callback(): self.raise_error() return except Exception as e: - self.get_logger().error(f"Failed to run component {self.get_name()}: {e}", throttle_duration_sec=1.0) + self.get_logger().error(f"Failed to run the execute function: {e}") self.raise_error() return + self.get_logger().debug("Execution finished, setting 'is_finished' predicate to true.") self.set_predicate("is_finished", True) - def execute(self): + def on_execute_callback(self): """ Execute the component logic. To be redefined in the derived classes. @@ -79,13 +76,6 @@ def execute(self): """ return True - def _raise_error(self): - """ - Put the component in error state by setting the 'in_error_state' - predicate to true. - """ - self.set_predicate("in_error_state", True) - def add_output(self, signal_name: str, data: str, message_type: MsgT, clproto_message_type=clproto.MessageType.UNKNOWN_MESSAGE, fixed_topic=False, default_topic=""): """ diff --git a/source/modulo_components/src/Component.cpp b/source/modulo_components/src/Component.cpp index 2e6f12d31..1b8a12f71 100644 --- a/source/modulo_components/src/Component.cpp +++ b/source/modulo_components/src/Component.cpp @@ -4,13 +4,9 @@ using namespace modulo_core::communication; namespace modulo_components { -Component::Component(const rclcpp::NodeOptions& node_options, bool start_thread, const std::string& fallback_name) : +Component::Component(const rclcpp::NodeOptions& node_options, const std::string& fallback_name) : ComponentInterface(node_options, PublisherType::PUBLISHER, fallback_name), started_(false) { this->add_predicate("is_finished", false); - - if (start_thread) { - this->start_thread(); - } } void Component::step() { @@ -24,18 +20,18 @@ void Component::step() { } } -void Component::start_thread() { +void Component::execute() { if (this->started_) { - RCLCPP_ERROR(this->get_logger(), "Failed to start run thread: Thread has already been started."); + RCLCPP_ERROR(this->get_logger(), "Failed to start execution thread: Thread has already been started."); return; } this->started_ = true; - this->run_thread_ = std::thread([this]() { this->run(); }); + this->execute_thread_ = std::thread([this]() { this->on_execute(); }); } -void Component::run() { +void Component::on_execute() { try { - if (!this->execute()) { + if (!this->on_execute_callback()) { this->raise_error(); return; } @@ -44,11 +40,11 @@ void Component::run() { this->raise_error(); return; } - RCLCPP_DEBUG(this->get_logger(), "Execution finished, setting is_finished predicate to true"); + RCLCPP_DEBUG(this->get_logger(), "Execution finished, setting 'is_finished' predicate to true."); this->set_predicate("is_finished", true); } -bool Component::execute() { +bool Component::on_execute_callback() { return true; } }// namespace modulo_components diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 6ef8678d4..c97cca2e7 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -58,8 +58,9 @@ class ComponentInterfacePublicInterface : public ComponentInterface { class ComponentPublicInterface : public Component { public: - explicit ComponentPublicInterface(const rclcpp::NodeOptions& node_options, bool start_thread = true) : - Component(node_options, start_thread) {} + explicit ComponentPublicInterface( + const rclcpp::NodeOptions& node_options, const std::string& fallback_name = "ComponentPublicInterface" + ) : Component(node_options, fallback_name) {} using Component::add_output; using Component::outputs_; }; diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp index 98d1a3d9c..0314f514b 100644 --- a/source/modulo_components/test/cpp/test_component.cpp +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -20,7 +20,7 @@ class ComponentTest : public ::testing::Test { } void SetUp() override { - component_ = std::make_shared(rclcpp::NodeOptions(), false); + component_ = std::make_shared(rclcpp::NodeOptions()); } std::shared_ptr component_; From 518cf9dff59aa91d9f68a463948b83369b1da6bc Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 8 Jul 2022 09:36:35 +0200 Subject: [PATCH 55/71] Execute user transition callback first (#100) * Don't allow adding outputs for certain lifecycle states * Execute user callback first and then additional methods * Internal first for activate --- .../modulo_components/LifecycleComponent.hpp | 7 +++++++ .../modulo_components/lifecycle_component.py | 5 ++++- .../modulo_components/src/LifecycleComponent.cpp | 14 ++++++-------- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index fc27f9a0e..859fad8d3 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include "modulo_components/ComponentInterface.hpp" @@ -260,6 +261,12 @@ inline void LifecycleComponent::add_output( const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, const std::string& default_topic ) { + if (this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED + && this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_WARN_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "Adding output in state " << this->get_current_state().label() << " is not allowed."); + return; + } try { this->create_output(signal_name, data, fixed_topic, default_topic); } catch (const std::exception& ex) { diff --git a/source/modulo_components/modulo_components/lifecycle_component.py b/source/modulo_components/modulo_components/lifecycle_component.py index 6b2e250a2..e0bf36807 100644 --- a/source/modulo_components/modulo_components/lifecycle_component.py +++ b/source/modulo_components/modulo_components/lifecycle_component.py @@ -82,7 +82,7 @@ def __handle_configure(self) -> bool: :return: True if the transition was successful """ - return self.__configure_outputs() and self.on_configure_callback() + return self.on_configure_callback() and self.__configure_outputs() def on_configure_callback(self) -> bool: """ @@ -243,6 +243,9 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT, :param fixed_topic: If true, the topic name of the output signal is fixed :param default_topic: If set, the default value for the topic name to use """ + if self.__state not in [State.PRIMARY_STATE_UNCONFIGURED, State.PRIMARY_STATE_INACTIVE]: + self.get_logger().warn(f"Adding output in state {self.__state} is not allowed.", throttle_duration_sec=1.0) + return try: parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, fixed_topic, default_topic) diff --git a/source/modulo_components/src/LifecycleComponent.cpp b/source/modulo_components/src/LifecycleComponent.cpp index d7cbd732f..0368470d4 100644 --- a/source/modulo_components/src/LifecycleComponent.cpp +++ b/source/modulo_components/src/LifecycleComponent.cpp @@ -1,7 +1,5 @@ #include "modulo_components/LifecycleComponent.hpp" -#include - using namespace modulo_core::communication; namespace modulo_components { @@ -69,8 +67,8 @@ LifecycleComponent::on_configure(const rclcpp_lifecycle::State& previous_state) } bool LifecycleComponent::handle_configure() { - bool result = this->configure_outputs(); - return result && this->on_configure_callback(); + bool result = this->on_configure_callback(); + return result && this->configure_outputs(); } bool LifecycleComponent::on_configure_callback() { @@ -92,8 +90,8 @@ LifecycleComponent::on_cleanup(const rclcpp_lifecycle::State& previous_state) { } bool LifecycleComponent::handle_cleanup() { - bool result = this->cleanup_signals(); - return result && this->on_cleanup_callback(); + bool result = this->on_cleanup_callback(); + return result && this->cleanup_signals(); } bool LifecycleComponent::on_cleanup_callback() { @@ -145,8 +143,8 @@ LifecycleComponent::on_deactivate(const rclcpp_lifecycle::State& previous_state) } bool LifecycleComponent::handle_deactivate() { - bool result = this->deactivate_outputs(); - return result && this->on_deactivate_callback(); + bool result = this->on_deactivate_callback(); + return result && this->deactivate_outputs(); } bool LifecycleComponent::on_deactivate_callback() { From 6226ebac471bca725ea8f86ca19a91bf50a41bdf Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 8 Jul 2022 14:45:39 +0200 Subject: [PATCH 56/71] Handle empty parameters in ComponentInterface (#97) * Better exception message * Expect control libraries 6.0.6 * Handle adding of empty parameters in component interface * Improve error messages and doc * Add extensive tests to check same behavior in cpp and python * Move validation down to allow for parameter overrides before throwing * Add flag to check if on_set_parameter_callback needs to be called manually --- source/modulo_components/CMakeLists.txt | 2 +- .../modulo_components/ComponentInterface.hpp | 52 ++-- .../modulo_components/component_interface.py | 56 ++--- .../component_public_interfaces.hpp | 5 +- ...t_component_interface_empty_parameters.cpp | 234 ++++++++++++++++++ .../test_component_interface_parameters.cpp | 5 +- ...st_component_interface_empty_parameters.py | 152 ++++++++++++ .../test_component_interface_parameters.py | 22 +- .../src/translators/parameter_translators.cpp | 3 +- 9 files changed, 463 insertions(+), 68 deletions(-) create mode 100644 source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp create mode 100644 source/modulo_components/test/python/test_component_interface_empty_parameters.py diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 9afc3a34c..4f9d7f0cb 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 6.0.2 REQUIRED COMPONENTS state_representation) +find_package(control_libraries 6.0.6 REQUIRED COMPONENTS state_representation) find_package(clproto 6.0.0 REQUIRED) include_directories(include) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index afbef8966..113b8202e 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -19,6 +19,7 @@ #include #include #include +#include #include #include #include @@ -79,6 +80,7 @@ class ComponentInterface : public NodeT { * @param parameter A ParameterInterface pointer to a Parameter instance * @param description The description of the parameter * @param read_only If true, the value of the parameter cannot be changed after declaration + * @raise ComponentParameterError if the parameter could not be added */ void add_parameter( const std::shared_ptr& parameter, const std::string& description, @@ -94,6 +96,7 @@ class ComponentInterface : public NodeT { * @param value The value of the parameter * @param description The description of the parameter * @param read_only If true, the value of the parameter cannot be changed after declaration + * @raise ComponentParameterError if the parameter could not be added */ template void add_parameter(const std::string& name, const T& value, const std::string& description, bool read_only = false); @@ -336,6 +339,8 @@ class ComponentInterface : public NodeT { std::shared_ptr tf_listener_; ///< TF listener std::shared_ptr tf_broadcaster_; ///< TF broadcaster // TODO maybe add a static tf broadcaster + + bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called }; template @@ -391,22 +396,40 @@ inline void ComponentInterface::add_parameter( const std::shared_ptr& parameter, const std::string& description, bool read_only ) { + this->set_parameter_callback_called_ = false; + rclcpp::Parameter ros_param; try { - auto ros_param = modulo_core::translators::write_parameter(parameter); - if (!NodeT::has_parameter(parameter->get_name())) { - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); - parameter_map_.set_parameter(parameter); + ros_param = modulo_core::translators::write_parameter(parameter); + } catch (const modulo_core::exceptions::ParameterTranslationException& ex) { + throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); + } + if (!NodeT::has_parameter(parameter->get_name())) { + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); + this->parameter_map_.set_parameter(parameter); + try { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = description; descriptor.read_only = read_only; - NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); - } else { - RCLCPP_WARN_STREAM(this->get_logger(), - "Parameter '" << parameter->get_name() << "' already exists, overwriting."); - NodeT::set_parameter(ros_param); + if (parameter->is_empty()) { + descriptor.dynamic_typing = true; + descriptor.type = modulo_core::translators::get_ros_parameter_type(parameter->get_parameter_type()); + NodeT::declare_parameter(parameter->get_name(), rclcpp::ParameterValue{}, descriptor); + } else { + NodeT::declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); + } + if (!this->set_parameter_callback_called_) { + auto result = this->on_set_parameters_callback({NodeT::get_parameters({parameter->get_name()})}); + if (!result.successful) { + NodeT::undeclare_parameter(parameter->get_name()); + throw exceptions::ComponentParameterException(result.reason); + } + } + } catch (const std::exception& ex) { + this->parameter_map_.remove_parameter(parameter->get_name()); + throw exceptions::ComponentParameterException("Failed to add parameter: " + std::string(ex.what())); } - } catch (const std::exception& ex) { - RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add parameter '" << parameter->get_name() << "': " << ex.what()); + } else { + RCLCPP_WARN_STREAM(this->get_logger(), "Parameter '" << parameter->get_name() << "' already exists."); } } @@ -458,10 +481,10 @@ ComponentInterface::on_set_parameters_callback(const std::vectorvalidate_parameter(new_parameter)) { result.successful = false; - result.reason += "Parameter " + ros_parameter.get_name() + " could not be set! "; - } else { + result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!"; + } else if (!new_parameter->is_empty()) { // update the value of the parameter in the map modulo_core::translators::copy_parameter_value(new_parameter, parameter); } @@ -470,6 +493,7 @@ ComponentInterface::on_set_parameters_callback(const std::vectorset_parameter_callback_called_ = true; return result; } diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 07067c922..d55b3068d 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -11,7 +11,8 @@ LookupTransformError from modulo_components.utilities.utilities import generate_predicate_topic, parse_signal_name from modulo_core.encoded_state import EncodedState -from modulo_core.translators.parameter_translators import write_parameter, read_parameter_const +from modulo_core.exceptions.core_exceptions import ParameterTranslationError +from modulo_core.translators.parameter_translators import get_ros_parameter_type, read_parameter_const, write_parameter from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult from rclpy.duration import Duration from rclpy.node import Node @@ -31,29 +32,9 @@ class ComponentInterface(Node): """ Abstract class to represent a Component in python, following the same logic pattern as the C++ modulo_component::ComponentInterface class. - ... - # TODO class docstring and attributes - Attributes: - predicates (dict(Parameters(bool))): map of predicates added to the Component. - predicate_publishers (dict(rclpy.Publisher(Bool))): map of publishers associated to each predicate. - parameter_dict (dict(Publisher)): dict of parameters - periodic_callbacks (dict(Callable): dict of periodic callback functions - qos (rclpy.qos.QoSProfile: the Quality of Service for publishers and subscribers - inputs: dict of inputs - outputs: dict of output - tf_buffer (tf2_ros.Buffer): the buffer to lookup transforms published on tf2. - tf_listener (tf2_ros.TransformListener): the listener to lookup transforms published on tf2. - tf_broadcaster (tf2_ros.TransformBroadcaster): the broadcaster to publish transforms on tf2 - Parameters: - period (double): period (in s) between step function calls. """ def __init__(self, node_name: str, *kargs, **kwargs): - """ - Constructs all the necessary attributes and declare all the parameters. - Parameters: - node_name (str): name of the node to be passed to the base Node class - """ super().__init__(node_name, *kargs, **kwargs) self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {} self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {} @@ -92,6 +73,7 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r :param parameter: Either the name of the parameter attribute or the parameter itself :param description: The parameter description :param read_only: If True, the value of the parameter cannot be changed after declaration + :raises ComponentParameterError if the parameter could not be added """ try: if isinstance(parameter, sr.Parameter): @@ -107,18 +89,24 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r raise TypeError("Provide either a state_representation.Parameter object or a string " "containing the name of the attribute that refers to the parameter to add.") ros_param = write_parameter(sr_parameter) - if not self.has_parameter(sr_parameter.get_name()): - self.get_logger().debug(f"Adding parameter '{sr_parameter.get_name()}'.") - self._parameter_dict[sr_parameter.get_name()] = parameter - # TODO ignore override - self.declare_parameter(ros_param.name, ros_param.value, - descriptor=ParameterDescriptor(description=description), - ignore_override=read_only) - else: - self.get_logger().warn(f"Parameter '{sr_parameter.get_name()}' already exists, overwriting.") - self.set_parameters([ros_param]) - except Exception as e: - self.get_logger().error(f"Failed to add parameter: {e}") + except (TypeError, ParameterTranslationError) as e: + raise ComponentParameterError(f"Failed to add parameter: {e}") + if not self.has_parameter(sr_parameter.get_name()): + self.get_logger().debug(f"Adding parameter '{sr_parameter.get_name()}'.") + self._parameter_dict[sr_parameter.get_name()] = parameter + try: + descriptor = ParameterDescriptor(description=description, read_only=read_only) + if sr_parameter.is_empty(): + descriptor.dynamic_typing = True + descriptor.type = get_ros_parameter_type(sr_parameter.get_parameter_type()).value + self.declare_parameter(ros_param.name, None, descriptor=descriptor) + else: + self.declare_parameter(ros_param.name, ros_param.value, descriptor=descriptor) + except Exception as e: + del self._parameter_dict[sr_parameter.get_name()] + raise ComponentParameterError(f"Failed to add parameter: {e}") + else: + self.get_logger().warn(f"Parameter '{sr_parameter.get_name()}' already exists.") def get_parameter(self, name: str) -> Union[sr.Parameter, Parameter]: """ @@ -213,7 +201,7 @@ def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetPa new_parameter = read_parameter_const(ros_param, parameter) if not self._validate_parameter(new_parameter): result.successful = False - result.reason = f"Parameter {ros_param.name} could not be set!" + result.reason = f"Validation of parameter '{ros_param.name}' returned false!" else: if isinstance(self._parameter_dict[ros_param.name], str): self.__setattr__(self._parameter_dict[ros_param.name], new_parameter) diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index c97cca2e7..594bc4fbd 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -12,8 +12,9 @@ template class ComponentInterfacePublicInterface : public ComponentInterface { public: explicit ComponentInterfacePublicInterface( - const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type - ) : ComponentInterface(node_options, publisher_type) {} + const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type, + const std::string& fallback_name = "ComponentInterfacePublicInterface" + ) : ComponentInterface(node_options, publisher_type, fallback_name) {} using ComponentInterface::add_parameter; using ComponentInterface::get_parameter; using ComponentInterface::get_parameter_value; diff --git a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp new file mode 100644 index 000000000..ca456e589 --- /dev/null +++ b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp @@ -0,0 +1,234 @@ +#include + +#include "modulo_components/exceptions/ComponentParameterException.hpp" +#include "modulo_core/EncodedState.hpp" +#include "test_modulo_components/component_public_interfaces.hpp" + +namespace modulo_components { + +template +class EmptyParameterInterface : public ComponentInterfacePublicInterface { +public: + explicit EmptyParameterInterface( + const rclcpp::NodeOptions& node_options, modulo_core::communication::PublisherType publisher_type, + const std::string& fallback_name = "EmptyParameterInterface", bool allow_empty = true, bool add_parameter = true, + bool empty_parameter = true + ) : ComponentInterfacePublicInterface(node_options, publisher_type, fallback_name), allow_empty_(allow_empty) { + if (add_parameter) { + if (empty_parameter) { + this->add_parameter(std::make_shared>("name"), "Test parameter"); + } else { + this->add_parameter(std::make_shared>("name", "test"), "Test parameter"); + } + } + }; + +private: + bool validate_parameter(const std::shared_ptr& parameter) override { + if (parameter->get_name() == "name") { + if (parameter->is_empty()) { + return this->allow_empty_; + } else if (parameter->get_parameter_value().empty()) { + RCLCPP_ERROR(this->get_logger(), "Provide a non empty value for parameter 'name'"); + return false; + } + } + return true; + } + + bool allow_empty_; +}; + +template +class ComponentInterfaceEmptyParameterTest : public ::testing::Test { +protected: + static void SetUpTestSuite() { + rclcpp::init(0, nullptr); + } + + static void TearDownTestSuite() { + rclcpp::shutdown(); + } + + void SetUp() override { + if (std::is_same::value) { + this->component_ = std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER + ); + } else if (std::is_same::value) { + this->component_ = std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER + ); + } + } + + std::shared_ptr> component_; +}; +using NodeTypes = ::testing::Types; +TYPED_TEST_SUITE(ComponentInterfaceEmptyParameterTest, NodeTypes); + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, NotAllowEmptyOnConstruction) { + if (std::is_same::value) { + EXPECT_THROW(std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false + ), modulo_components::exceptions::ComponentParameterException); + } else if (std::is_same::value) { + EXPECT_THROW(std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, + "EmptyParameterComponent", false + ), modulo_components::exceptions::ComponentParameterException); + } +} + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, NotAllowEmpty) { + std::shared_ptr> component; + if (std::is_same::value) { + component = std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false, + false + ); + } else if (std::is_same::value) { + component = std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, + "EmptyParameterComponent", false, false + ); + } + EXPECT_THROW(component->add_parameter(std::make_shared>("name"), "Test parameter"), + exceptions::ComponentParameterException); + EXPECT_THROW(auto param = component->get_parameter("name"), exceptions::ComponentParameterException); + EXPECT_THROW(component->get_ros_parameter("name"), rclcpp::exceptions::ParameterNotDeclaredException); +} + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, AllowEmpty) { + std::shared_ptr> component; + if (std::is_same::value) { + component = std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", true, + false + ); + } else if (std::is_same::value) { + component = std::make_shared>( + rclcpp::NodeOptions(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, + "EmptyParameterComponent", true, false + ); + } + EXPECT_NO_THROW(component->add_parameter(std::make_shared>("name"), "Test parameter")); + EXPECT_EQ(component->get_parameter("name")->get_parameter_type(), ParameterType::STRING); + EXPECT_TRUE(component->get_parameter("name")->is_empty()); + EXPECT_EQ(component->get_ros_parameter("name").get_type(), rclcpp::PARAMETER_NOT_SET); +} + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, ValidateEmptyParameter) { + // component comes with empty parameter 'name' + EXPECT_EQ(this->component_->get_parameter("name")->get_parameter_type(), ParameterType::STRING); + EXPECT_TRUE(this->component_->get_parameter("name")->is_empty()); + auto ros_param = rclcpp::Parameter(); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("name");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_NOT_SET); + + // Trying to overwrite a parameter is not possible + this->component_->add_parameter(std::make_shared>("name"), "Test parameter"); + EXPECT_EQ(this->component_->get_parameter("name")->get_parameter_type(), ParameterType::STRING); + EXPECT_TRUE(this->component_->get_parameter("name")->is_empty()); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("name");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_NOT_SET); + + // Set parameter value from ROS interface should update ROS parameter type + this->component_->set_ros_parameter({"name", "test"}); + EXPECT_EQ(this->component_->get_parameter("name")->get_parameter_type(), ParameterType::STRING); + EXPECT_EQ(this->component_->get_parameter("name")->template get_parameter_value(), "test"); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("name");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_STRING); + EXPECT_EQ(ros_param.template get_value(), "test"); + + // Set parameter value from component interface + this->component_->template set_parameter_value("name", "again"); + EXPECT_EQ(this->component_->get_parameter("name")->get_parameter_type(), ParameterType::STRING); + EXPECT_EQ(this->component_->get_parameter("name")->template get_parameter_value(), "again"); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("name");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_STRING); + EXPECT_EQ(ros_param.template get_value(), "again"); + + // Setting it with empty value should be rejected in parameter evaluation + this->component_->template set_parameter_value("name", ""); + EXPECT_EQ(this->component_->get_parameter("name")->get_parameter_type(), ParameterType::STRING); + EXPECT_EQ(this->component_->get_parameter("name")->template get_parameter_value(), "again"); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("name");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_STRING); + EXPECT_EQ(ros_param.template get_value(), "again"); + + // Setting it with empty value should be rejected in parameter evaluation + this->component_->set_ros_parameter({"name", ""}); + EXPECT_EQ(this->component_->get_parameter("name")->get_parameter_type(), ParameterType::STRING); + EXPECT_EQ(this->component_->get_parameter("name")->template get_parameter_value(), "again"); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("name");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_STRING); + EXPECT_EQ(ros_param.template get_value(), "again"); + + // TODO clarify that behavior somewhere + // Setting a parameter with type NOT_SET undeclares that parameter + this->component_->set_ros_parameter(rclcpp::Parameter("name", rclcpp::ParameterValue{})); + EXPECT_THROW(this->component_->get_ros_parameter("name"), rclcpp::exceptions::ParameterNotDeclaredException); +} + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, ChangeParameterType) { + // Add parameter from component interface + this->component_->add_parameter(std::make_shared>("int"), "Test parameter"); + EXPECT_TRUE(this->component_->describe_parameter("int").dynamic_typing); + this->component_->template set_parameter_value("int", 1); + EXPECT_EQ(this->component_->get_parameter("int")->get_parameter_type(), ParameterType::INT); + EXPECT_EQ(this->component_->get_parameter("int")->template get_parameter_value(), 1); + auto ros_param = rclcpp::Parameter(); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("int");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(ros_param.template get_value(), 1); + + // Set parameter value from component interface with different type should not work + this->component_->template set_parameter_value("int", 2.0); + EXPECT_EQ(this->component_->get_parameter("int")->get_parameter_type(), ParameterType::INT); + EXPECT_EQ(this->component_->get_parameter("int")->template get_parameter_value(), 1); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("int");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(ros_param.template get_value(), 1); + + // Set parameter value from ROS interface with different type should not work + auto result = this->component_->set_ros_parameter(rclcpp::Parameter("int", 2.0)); + EXPECT_FALSE(result.successful); + EXPECT_EQ(this->component_->get_parameter("int")->get_parameter_type(), ParameterType::INT); + EXPECT_EQ(this->component_->get_parameter("int")->template get_parameter_value(), 1); + EXPECT_NO_THROW(ros_param = this->component_->get_ros_parameter("int");); + EXPECT_EQ(ros_param.get_type(), rclcpp::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(ros_param.template get_value(), 1); +} + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, ParameterOverrides) { + // Construction with not allowing empty parameters but providing the parameter override should succeed + if (std::is_same::value) { + EXPECT_NO_THROW(std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name", "test")}), + modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false + )); + } else if (std::is_same::value) { + EXPECT_NO_THROW(std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name", "test")}), + modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, "EmptyParameterComponent", false + )); + } +} + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, ParameterOverridesEmpty) { + // Construction with not allowing empty parameters and providing an uninitialized parameter override should not succeed + std::shared_ptr> component; + if (std::is_same::value) { + EXPECT_THROW(component = std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name")}), + modulo_core::communication::PublisherType::PUBLISHER, "EmptyParameterComponent", false, true, false + ), modulo_components::exceptions::ComponentParameterException); + } else if (std::is_same::value) { + EXPECT_THROW(std::make_shared>( + rclcpp::NodeOptions().parameter_overrides({rclcpp::Parameter("name")}), + modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER, "EmptyParameterComponent", false, true, false + ), modulo_components::exceptions::ComponentParameterException); + } +} +} // namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index 941898e5c..2badd4b58 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -75,9 +75,8 @@ TYPED_TEST(ComponentInterfaceParameterTest, AddParameterAgain) { // Adding an existing parameter again should just set the value this->component_->validate_parameter_was_called = false; EXPECT_NO_THROW(this->component_->add_parameter("test", 2, "foo")); - EXPECT_TRUE(this->component_->validate_parameter_was_called); - this->template expect_parameter_value(2); - EXPECT_EQ(this->param_->get_value(), 2); + EXPECT_FALSE(this->component_->validate_parameter_was_called); + this->template expect_parameter_value(1); } TYPED_TEST(ComponentInterfaceParameterTest, SetParameter) { diff --git a/source/modulo_components/test/python/test_component_interface_empty_parameters.py b/source/modulo_components/test/python/test_component_interface_empty_parameters.py new file mode 100644 index 000000000..643f2a6ba --- /dev/null +++ b/source/modulo_components/test/python/test_component_interface_empty_parameters.py @@ -0,0 +1,152 @@ +import pytest +import rclpy +import state_representation as sr +from modulo_components.component_interface import ComponentInterface +from modulo_components.exceptions.component_exceptions import ComponentParameterError +from rcl_interfaces.msg import SetParametersResult +from rclpy.exceptions import ParameterNotDeclaredException +from rclpy.node import Node +from rclpy.parameter import Parameter + + +class EmtpyParameterInterface(ComponentInterface): + def __init__(self, node_name, allow_empty=True, add_parameter=True, empty_parameter=True, *kargs, **kwargs): + super().__init__(node_name, *kargs, **kwargs) + self._allow_empty = allow_empty + if add_parameter: + if empty_parameter: + self.add_parameter(sr.Parameter("name", sr.ParameterType.STRING), "Test parameter") + else: + self.add_parameter(sr.Parameter("name", "test", sr.ParameterType.STRING), "Test parameter") + + def get_ros_parameter(self, name: str) -> rclpy.Parameter: + return rclpy.node.Node.get_parameter(self, name) + + def set_ros_parameter(self, param: rclpy.Parameter) -> SetParametersResult: + return rclpy.node.Node.set_parameters(self, [param])[0] + + def _validate_parameter(self, parameter: sr.Parameter) -> bool: + if parameter.get_name() == "name": + if parameter.is_empty(): + return self._allow_empty + elif not parameter.get_value(): + self.get_logger().error("Provide a non empty value for parameter 'name'") + return False + return True + + +@pytest.fixture() +def component_test(ros_context): + yield EmtpyParameterInterface("component") + + +def test_not_allow_empty_on_construction(ros_context): + # Construction with empty parameter should raise if the empty parameter is not allowed + with pytest.raises(ComponentParameterError): + EmtpyParameterInterface("component", False) + + +def test_not_allow_empty(ros_context): + component = EmtpyParameterInterface("component", allow_empty=False, add_parameter=False) + with pytest.raises(ComponentParameterError): + component.add_parameter(sr.Parameter("name", sr.ParameterType.STRING), "Test parameter") + with pytest.raises(ComponentParameterError): + component.get_parameter("name") + with pytest.raises(ParameterNotDeclaredException): + assert Node.get_parameter(component, "name") + + component = EmtpyParameterInterface("component", allow_empty=True, add_parameter=False) + component.add_parameter(sr.Parameter("name", sr.ParameterType.STRING), "Test parameter") + assert component.get_parameter("name").get_parameter_type() == sr.ParameterType.STRING + assert component.get_parameter("name").is_empty() + assert Node.get_parameter(component, "name").type_ == Parameter.Type.NOT_SET + + +def test_validate_empty_parameter(component_test): + # component_test comes with empty parameter 'name' + assert component_test.get_parameter("name").get_parameter_type() == sr.ParameterType.STRING + assert component_test.get_parameter("name").is_empty() + assert Node.get_parameter(component_test, "name").type_ == Parameter.Type.NOT_SET + + # Trying to overwrite a parameter is not possible + component_test.add_parameter(sr.Parameter("name", sr.ParameterType.BOOL), "Test parameter") + assert component_test.get_parameter("name").get_parameter_type() == sr.ParameterType.STRING + assert component_test.get_parameter("name").is_empty() + assert Node.get_parameter(component_test, "name").type_ == Parameter.Type.NOT_SET + + # Set parameter with empty parameter isn't possible because there is no method for that + # component_test.set_parameter(sr.Parameter("name", sr.ParameterType.STRING)) + + # Set parameter value from ROS interface should update ROS parameter type + Node.set_parameters(component_test, [Parameter("name", value="test")]) + assert component_test.get_parameter("name").get_parameter_type() == sr.ParameterType.STRING + assert component_test.get_parameter_value("name") == "test" + assert Node.get_parameter(component_test, "name").type_ == Parameter.Type.STRING + assert Node.get_parameter(component_test, "name").value == "test" + + # Set parameter value from component interface + component_test.set_parameter_value("name", "again", sr.ParameterType.STRING) + assert component_test.get_parameter("name").get_parameter_type() == sr.ParameterType.STRING + assert component_test.get_parameter_value("name") == "again" + assert Node.get_parameter(component_test, "name").type_ == Parameter.Type.STRING + assert Node.get_parameter(component_test, "name").value == "again" + + # Setting it with empty value should be rejected in parameter evaluation + component_test.set_parameter_value("name", "", sr.ParameterType.STRING) + assert component_test.get_parameter("name").get_parameter_type() == sr.ParameterType.STRING + assert component_test.get_parameter_value("name") == "again" + assert Node.get_parameter(component_test, "name").type_ == Parameter.Type.STRING + assert Node.get_parameter(component_test, "name").value == "again" + + # Setting it with empty value should be rejected in parameter evaluation + Node.set_parameters(component_test, [Parameter("name", type_=Parameter.Type.STRING, value="")]) + assert component_test.get_parameter("name").get_parameter_type() == sr.ParameterType.STRING + assert component_test.get_parameter_value("name") == "again" + assert Node.get_parameter(component_test, "name").type_ == Parameter.Type.STRING + assert Node.get_parameter(component_test, "name").value == "again" + + # TODO + # Setting a parameter with type NOT_SET undeclares that parameter + Node.set_parameters(component_test, [Parameter("name", type_=Parameter.Type.NOT_SET)]) + with pytest.raises(ParameterNotDeclaredException): + Node.get_parameter(component_test, "name") + + +def test_change_parameter_type(component_test): + # Add parameter from component interface + component_test.add_parameter(sr.Parameter("int", sr.ParameterType.INT), "Test parameter") + assert component_test.describe_parameter("int").dynamic_typing + component_test.set_parameter_value("int", 1, sr.ParameterType.INT) + assert component_test.get_parameter("int").get_parameter_type() == sr.ParameterType.INT + assert component_test.get_parameter_value("int") == 1 + ros_param = component_test.get_ros_parameter("int") + assert ros_param.type_ == Parameter.Type.INTEGER + assert ros_param.value == 1 + + # Set parameter value from component interface with different type should not work + component_test.set_parameter_value("int", 2.0, sr.ParameterType.DOUBLE) + assert component_test.get_parameter("int").get_parameter_type() == sr.ParameterType.INT + assert component_test.get_parameter_value("int") == 1 + ros_param = component_test.get_ros_parameter("int") + assert ros_param.type_ == Parameter.Type.INTEGER + assert ros_param.value == 1 + + # Set parameter value from ROS interface with different type should not work + component_test.set_ros_parameter(Parameter("int", value=2.0)) + assert component_test.get_parameter("int").get_parameter_type() == sr.ParameterType.INT + assert component_test.get_parameter_value("int") == 1 + ros_param = component_test.get_ros_parameter("int") + assert ros_param.type_ == Parameter.Type.INTEGER + assert ros_param.value == 1 + + +def test_parameter_overrides(ros_context): + # Construction with not allowing empty parameters but providing the parameter override should succeed + EmtpyParameterInterface("component", False, parameter_overrides=[Parameter("name", value="test")]) + + +def test_parameter_overrides_empty(ros_context): + # Construction with not allowing empty parameters and providing an uninitialized parameter override should not succeed + with pytest.raises(ComponentParameterError): + EmtpyParameterInterface("component", allow_empty=False, empty_parameter=False, + parameter_overrides=[Parameter("name")]) diff --git a/source/modulo_components/test/python/test_component_interface_parameters.py b/source/modulo_components/test/python/test_component_interface_parameters.py index 02e7cc667..4a7c76ead 100644 --- a/source/modulo_components/test/python/test_component_interface_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_parameters.py @@ -59,9 +59,8 @@ def test_add_parameter_again(component_interface): component_interface.add_parameter("param", "Test parameter") component_interface.validate_was_called = False component_interface.add_parameter(sr.Parameter("test", 2, sr.ParameterType.INT), "foo") - assert component_interface.validate_was_called - assert_param_value_equal(component_interface, "test", 2) - assert component_interface.param.get_value() == 2 + assert not component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 1) def test_add_parameter_again_not_attribute(component_interface): @@ -73,9 +72,8 @@ def test_add_parameter_again_not_attribute(component_interface): component_interface.add_parameter(component_interface.param, "Test parameter") component_interface.validate_was_called = False component_interface.add_parameter(sr.Parameter("test", 2, sr.ParameterType.INT), "foo") - assert component_interface.validate_was_called - assert_param_value_equal(component_interface, "test", 2) - assert component_interface.param.get_value() == 1 + assert not component_interface.validate_was_called + assert_param_value_equal(component_interface, "test", 1) def test_set_parameter(component_interface): @@ -140,16 +138,14 @@ def test_read_only_parameter(component_interface): component_interface.get_ros_parameter("test") assert_param_value_equal(component_interface, "test", 1) - # TODO read only component_interface.validate_was_called = False - # with pytest.raises(RuntimeError): - # component_interface.set_parameter_value("test", 2, sr.ParameterType.INT) - # assert not component_interface.validate_was_called + component_interface.set_parameter_value("test", 2, sr.ParameterType.INT) + assert not component_interface.validate_was_called assert_param_value_equal(component_interface, "test", 1) assert component_interface.param.get_value() == 1 component_interface.validate_was_called = False - # result = component_interface.set_ros_parameter(rclpy.Parameter("test", value=2)) - # assert not component_interface.validate_was_called - # assert not result.successful + result = component_interface.set_ros_parameter(rclpy.Parameter("test", value=2)) + assert not component_interface.validate_was_called + assert not result.successful assert_param_value_equal(component_interface, "test", 1) diff --git a/source/modulo_core/src/translators/parameter_translators.cpp b/source/modulo_core/src/translators/parameter_translators.cpp index bcb0abd17..c36bf74a4 100644 --- a/source/modulo_core/src/translators/parameter_translators.cpp +++ b/source/modulo_core/src/translators/parameter_translators.cpp @@ -236,7 +236,8 @@ std::shared_ptr read_parameter_const( } default: throw exceptions::ParameterTranslationException( - "Something went wrong while reading parameter " + parameter->get_name()); + "Incompatible parameter type encountered while reading parameter '" + parameter->get_name() + "'." + ); } return new_parameter; } From c4b956c1844bf9790fec89e4d959c562a43c7858 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 14 Jul 2022 10:51:11 +0200 Subject: [PATCH 57/71] Minor improvements (#101) * Check if parameter already exists upon add_signal * Add exception and utilities to __init__.py * Link libraries correctly * Typehint for on execute callback * Use control libraries 6.0.7 with new operator * Switch default_topic and read_only arguments --- source/modulo_components/CMakeLists.txt | 5 -- .../include/modulo_components/Component.hpp | 12 ++--- .../modulo_components/ComponentInterface.hpp | 52 +++++++++++-------- .../modulo_components/LifecycleComponent.hpp | 12 ++--- .../modulo_components/component.py | 10 ++-- .../modulo_components/component_interface.py | 37 +++++++------ .../modulo_components/exceptions/__init__.py | 2 + .../modulo_components/lifecycle_component.py | 10 ++-- .../modulo_components/utilities/__init__.py | 1 + .../test/cpp/test_component.cpp | 4 +- .../test/cpp/test_component_interface.cpp | 6 +-- .../test/cpp/test_lifecycle_component.cpp | 4 +- .../test/python/test_component_interface.py | 2 +- ...st_component_interface_empty_parameters.py | 2 +- .../test_component_interface_parameters.py | 2 +- source/modulo_core/CMakeLists.txt | 4 +- source/modulo_core/modulo_core/__init__.py | 1 + .../modulo_core/exceptions/__init__.py | 1 + .../translators/message_readers.py | 4 +- .../translators/message_writers.py | 6 +-- .../translators/parameter_translators.py | 4 +- .../src/translators/message_writers.cpp | 16 +++--- .../python_tests/translators/test_messages.py | 4 +- .../translators/test_parameters.py | 2 +- 24 files changed, 110 insertions(+), 93 deletions(-) diff --git a/source/modulo_components/CMakeLists.txt b/source/modulo_components/CMakeLists.txt index 4f9d7f0cb..228e65d86 100644 --- a/source/modulo_components/CMakeLists.txt +++ b/source/modulo_components/CMakeLists.txt @@ -21,9 +21,6 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 6.0.6 REQUIRED COMPONENTS state_representation) -find_package(clproto 6.0.0 REQUIRED) - include_directories(include) ament_auto_add_library(${PROJECT_NAME} SHARED @@ -31,8 +28,6 @@ ament_auto_add_library(${PROJECT_NAME} SHARED ${PROJECT_SOURCE_DIR}/src/Component.cpp ${PROJECT_SOURCE_DIR}/src/LifecycleComponent.cpp) -target_link_libraries(${PROJECT_NAME} clproto) - # Install Python modules ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) diff --git a/source/modulo_components/include/modulo_components/Component.hpp b/source/modulo_components/include/modulo_components/Component.hpp index 2de1c156c..762e86234 100644 --- a/source/modulo_components/include/modulo_components/Component.hpp +++ b/source/modulo_components/include/modulo_components/Component.hpp @@ -46,13 +46,13 @@ class Component : public ComponentInterface { * @tparam DataT Type of the data pointer * @param signal_name Name of the output signal * @param data Data to transmit on the output signal - * @param fixed_topic If true, the topic name of the output signal is fixed * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the output signal is fixed */ template void add_output( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, - const std::string& default_topic = "" + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic = "", + bool fixed_topic = false ); private: @@ -82,12 +82,12 @@ class Component : public ComponentInterface { template inline void Component::add_output( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, - const std::string& default_topic + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, + bool fixed_topic ) { using namespace modulo_core::communication; try { - auto parsed_signal_name = this->create_output(signal_name, data, fixed_topic, default_topic); + auto parsed_signal_name = this->create_output(signal_name, data, default_topic, fixed_topic); auto topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding output '" << parsed_signal_name << "' with topic name '" << topic_name << "'."); diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 113b8202e..c012b0f21 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -183,15 +183,15 @@ class ComponentInterface : public NodeT { * @tparam DataT Type of the data pointer * @param signal_name Name of the input signal * @param data Data to receive on the input signal - * @param fixed_topic If true, the topic name of the input signal is fixed * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the input signal is fixed */ // TODO could be nice to add an optional callback here that would be executed from within the subscription callback // in order to manipulate the data pointer upon reception of a message template void add_input( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, - const std::string& default_topic = "" + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic = "", + bool fixed_topic = false ); /** @@ -199,13 +199,13 @@ class ComponentInterface : public NodeT { * @tparam MsgT The ROS message type of the subscription * @param signal_name Name of the input signal * @param callback The callback to use for the subscription - * @param fixed_topic If true, the topic name of the input signal is fixed * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the input signal is fixed */ template void add_input( const std::string& signal_name, const std::function)>& callback, - bool fixed_topic = false, const std::string& default_topic = "" + const std::string& default_topic = "", bool fixed_topic = false ); /** @@ -231,15 +231,15 @@ class ComponentInterface : public NodeT { * @tparam DataT Type of the data pointer * @param signal_name Name of the output signal * @param data Data to transmit on the output signal - * @param fixed_topic If true, the topic name of the output signal is fixed * @param default_topic If set, the default value for the topic name to use + * @param fixed_topic If true, the topic name of the output signal is fixed * @throws AddSignalException if the output could not be created (empty name, already registered) * @return The parsed signal name */ template std::string create_output( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, - const std::string& default_topic + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, + bool fixed_topic ); /** @@ -585,8 +585,8 @@ inline void ComponentInterface::set_predicate( template template inline void ComponentInterface::add_input( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, - const std::string& default_topic + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, + bool fixed_topic ) { using namespace modulo_core::communication; try { @@ -600,10 +600,14 @@ inline void ComponentInterface::add_input( throw exceptions::AddSignalException("Failed to add input '" + signal_name + "': Input already exists"); } std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; - this->add_parameter( - parsed_signal_name + "_topic", topic_name, "Output topic name of signal '" + parsed_signal_name + "'", - fixed_topic - ); + auto parameter_name = parsed_signal_name + "_topic"; + if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { + this->set_parameter_value(parameter_name, topic_name); + } else { + this->add_parameter( + parameter_name, topic_name, "Input topic name of signal '" + parsed_signal_name + "'", fixed_topic + ); + } topic_name = this->get_parameter_value(parsed_signal_name + "_topic"); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding input '" << signal_name << "' with topic name '" << topic_name << "'."); @@ -663,8 +667,8 @@ inline void ComponentInterface::add_input( template template inline void ComponentInterface::add_input( - const std::string& signal_name, const std::function)>& callback, bool fixed_topic, - const std::string& default_topic + const std::string& signal_name, const std::function)>& callback, + const std::string& default_topic, bool fixed_topic ) { using namespace modulo_core::communication; try { @@ -808,8 +812,8 @@ inline void ComponentInterface::evaluate_periodic_callbacks() { template template inline std::string ComponentInterface::create_output( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, - const std::string& default_topic + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, + bool fixed_topic ) { using namespace modulo_core::communication; try { @@ -830,10 +834,14 @@ inline std::string ComponentInterface::create_output( this->outputs_.insert_or_assign( parsed_signal_name, std::make_shared(this->publisher_type_, message_pair)); std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; - this->add_parameter( - parsed_signal_name + "_topic", topic_name, "Output topic name of signal '" + parsed_signal_name + "'", - fixed_topic - ); + auto parameter_name = parsed_signal_name + "_topic"; + if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { + this->set_parameter_value(parameter_name, topic_name); + } else { + this->add_parameter( + parameter_name, topic_name, "Output topic name of signal '" + parsed_signal_name + "'", fixed_topic + ); + } return parsed_signal_name; } catch (const std::exception& ex) { // TODO if modulo::communication had a base exception, could catch that diff --git a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp index 859fad8d3..0c9014160 100644 --- a/source/modulo_components/include/modulo_components/LifecycleComponent.hpp +++ b/source/modulo_components/include/modulo_components/LifecycleComponent.hpp @@ -88,13 +88,13 @@ class LifecycleComponent : public ComponentInterface void add_output( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic = false, - const std::string& default_topic = "" + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic = "", + bool fixed_topic = false ); private: @@ -258,8 +258,8 @@ inline void LifecycleComponent::on_step_callback() {} template inline void LifecycleComponent::add_output( - const std::string& signal_name, const std::shared_ptr& data, bool fixed_topic, - const std::string& default_topic + const std::string& signal_name, const std::shared_ptr& data, const std::string& default_topic, + bool fixed_topic ) { if (this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_UNCONFIGURED && this->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -268,7 +268,7 @@ inline void LifecycleComponent::add_output( return; } try { - this->create_output(signal_name, data, fixed_topic, default_topic); + this->create_output(signal_name, data, default_topic, fixed_topic); } catch (const std::exception& ex) { // TODO if modulo::communication had a base exception, could catch that RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add output '" << signal_name << "': " << ex.what()); diff --git a/source/modulo_components/modulo_components/component.py b/source/modulo_components/modulo_components/component.py index 35d5df180..20b7439be 100644 --- a/source/modulo_components/modulo_components/component.py +++ b/source/modulo_components/modulo_components/component.py @@ -68,7 +68,7 @@ def __on_execute(self): self.get_logger().debug("Execution finished, setting 'is_finished' predicate to true.") self.set_predicate("is_finished", True) - def on_execute_callback(self): + def on_execute_callback(self) -> bool: """ Execute the component logic. To be redefined in the derived classes. @@ -77,7 +77,7 @@ def on_execute_callback(self): return True def add_output(self, signal_name: str, data: str, message_type: MsgT, - clproto_message_type=clproto.MessageType.UNKNOWN_MESSAGE, fixed_topic=False, default_topic=""): + clproto_message_type=clproto.MessageType.UNKNOWN_MESSAGE, default_topic="", fixed_topic=False): """ Add and configure an output signal of the component. @@ -85,12 +85,12 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT, :param data: Name of the attribute to transmit over the channel :param message_type: The ROS message type of the output :param clproto_message_type: The clproto message type, if applicable - :param fixed_topic: If true, the topic name of the output signal is fixed :param default_topic: If set, the default value for the topic name to use + :param fixed_topic: If true, the topic name of the output signal is fixed """ try: - parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, fixed_topic, - default_topic) + parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, + default_topic, fixed_topic) topic_name = self.get_parameter_value(parsed_signal_name + "_topic") self.get_logger().debug(f"Adding output '{parsed_signal_name}' with topic name '{topic_name}'.") publisher = self.create_publisher(message_type, topic_name, self._qos) diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index d55b3068d..760489b40 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -7,11 +7,10 @@ import modulo_core.translators.message_writers as modulo_writers import state_representation as sr from geometry_msgs.msg import TransformStamped -from modulo_components.exceptions.component_exceptions import AddSignalError, ComponentParameterError, \ - LookupTransformError -from modulo_components.utilities.utilities import generate_predicate_topic, parse_signal_name -from modulo_core.encoded_state import EncodedState -from modulo_core.exceptions.core_exceptions import ParameterTranslationError +from modulo_components.exceptions import AddSignalError, ComponentParameterError, LookupTransformError +from modulo_components.utilities import generate_predicate_topic, parse_signal_name +from modulo_core import EncodedState +from modulo_core.exceptions import ParameterTranslationError from modulo_core.translators.parameter_translators import get_ros_parameter_type, read_parameter_const, write_parameter from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult from rclpy.duration import Duration @@ -267,8 +266,8 @@ def set_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): return self._predicates[name] = value - def _create_output(self, signal_name: str, data: str, message_type: MsgT, - clproto_message_type: clproto.MessageType, fixed_topic: bool, default_topic: str) -> str: + def _create_output(self, signal_name: str, data: str, message_type: MsgT, clproto_message_type: clproto.MessageType, + default_topic: str, fixed_topic: bool) -> str: """ Helper function to parse the signal name and add an output without Publisher to the dict of outputs. @@ -276,8 +275,8 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, :param data: Name of the attribute to transmit over the channel :param message_type: The ROS message type of the output :param clproto_message_type: The clproto message type, if applicable - :param fixed_topic: If true, the topic name of the output signal is fixed :param default_topic: If set, the default value for the topic name to use + :param fixed_topic: If true, the topic name of the output signal is fixed :raises AddSignalError if there is a problem adding the output :return: The parsed signal name """ @@ -291,8 +290,12 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, if parsed_signal_name in self._outputs.keys(): raise AddSignalError(f"Output with parsed name '{parsed_signal_name}' already exists.") topic_name = default_topic if default_topic else "~/" + parsed_signal_name - self.add_parameter(sr.Parameter(parsed_signal_name + "_topic", topic_name, sr.ParameterType.STRING), - f"Output topic name of signal '{parsed_signal_name}'", fixed_topic) + parameter_name = parsed_signal_name + "_topic" + if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): + self.set_parameter_value(parameter_name, topic_name) + else: + self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), + f"Output topic name of signal '{parsed_signal_name}'", fixed_topic) translator = None if message_type == Bool or message_type == Float64 or \ message_type == Float64MultiArray or message_type == Int32 or message_type == String: @@ -321,8 +324,8 @@ def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Ca except Exception as e: self.get_logger().error(f"Failed to read message for attribute {attribute_name}", throttle_duration_sec=1.0) - def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, fixed_topic=False, - default_topic=""): + def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, default_topic="", + fixed_topic=False): # TODO could be nice to add an optional callback here that would be executed from within the subscription # callback in order to manipulate the data pointer upon reception of a message """ @@ -331,8 +334,8 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag :param signal_name: Name of the output signal :param subscription: Name of the attribute to receive messages for or the callback to use for the subscription :param message_type: ROS message type of the subscription - :param fixed_topic: If true, the topic name of the output signal is fixed :param default_topic: If set, the default value for the topic name to use + :param fixed_topic: If true, the topic name of the output signal is fixed """ try: parsed_signal_name = parse_signal_name(signal_name) @@ -341,8 +344,12 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag if parsed_signal_name in self._inputs.keys(): raise AddSignalError(f"Failed to add input '{parsed_signal_name}': Input already exists") topic_name = default_topic if default_topic else "~/" + parsed_signal_name - self.add_parameter(sr.Parameter(parsed_signal_name + "_topic", topic_name, sr.ParameterType.STRING), - f"Input topic name of signal '{parsed_signal_name}'", fixed_topic) + parameter_name = parsed_signal_name + "_topic" + if self.has_parameter(parameter_name) and self.get_parameter(parameter_name).is_empty(): + self.set_parameter_value(parameter_name, topic_name) + else: + self.add_parameter(sr.Parameter(parameter_name, topic_name, sr.ParameterType.STRING), + f"Input topic name of signal '{parsed_signal_name}'", fixed_topic) topic_name = self.get_parameter_value(parsed_signal_name + "_topic") self.get_logger().debug(f"Adding input '{parsed_signal_name}' with topic name '{topic_name}'.") if isinstance(subscription, Callable): diff --git a/source/modulo_components/modulo_components/exceptions/__init__.py b/source/modulo_components/modulo_components/exceptions/__init__.py index e69de29bb..55c2bb1dc 100644 --- a/source/modulo_components/modulo_components/exceptions/__init__.py +++ b/source/modulo_components/modulo_components/exceptions/__init__.py @@ -0,0 +1,2 @@ +from modulo_components.exceptions.component_exceptions import AddSignalError, ComponentError, ComponentParameterError, \ + LookupTransformError diff --git a/source/modulo_components/modulo_components/lifecycle_component.py b/source/modulo_components/modulo_components/lifecycle_component.py index e0bf36807..752fb1cf4 100644 --- a/source/modulo_components/modulo_components/lifecycle_component.py +++ b/source/modulo_components/modulo_components/lifecycle_component.py @@ -5,7 +5,7 @@ from lifecycle_msgs.srv import ChangeState from lifecycle_msgs.msg import State from modulo_components.component_interface import ComponentInterface -from modulo_components.exceptions.component_exceptions import AddSignalError +from modulo_components.exceptions import AddSignalError MsgT = TypeVar('MsgT') @@ -232,7 +232,7 @@ def __configure_outputs(self) -> bool: return success def add_output(self, signal_name: str, data: str, message_type: MsgT, - clproto_message_type=clproto.MessageType.UNKNOWN_MESSAGE, fixed_topic=False, default_topic=""): + clproto_message_type=clproto.MessageType.UNKNOWN_MESSAGE, default_topic="", fixed_topic=False): """ Add an output signal of the component. @@ -240,15 +240,15 @@ def add_output(self, signal_name: str, data: str, message_type: MsgT, :param data: Name of the attribute to transmit over the channel :param message_type: The ROS message type of the output :param clproto_message_type: The clproto message type, if applicable - :param fixed_topic: If true, the topic name of the output signal is fixed :param default_topic: If set, the default value for the topic name to use + :param fixed_topic: If true, the topic name of the output signal is fixed """ if self.__state not in [State.PRIMARY_STATE_UNCONFIGURED, State.PRIMARY_STATE_INACTIVE]: self.get_logger().warn(f"Adding output in state {self.__state} is not allowed.", throttle_duration_sec=1.0) return try: - parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, fixed_topic, - default_topic) + parsed_signal_name = self._create_output(signal_name, data, message_type, clproto_message_type, + default_topic, fixed_topic) self.get_logger().debug(f"Adding output '{parsed_signal_name}.") except AddSignalError as e: self.get_logger().error(f"Failed to add output '{signal_name}': {e}") diff --git a/source/modulo_components/modulo_components/utilities/__init__.py b/source/modulo_components/modulo_components/utilities/__init__.py index e69de29bb..899b322a7 100644 --- a/source/modulo_components/modulo_components/utilities/__init__.py +++ b/source/modulo_components/modulo_components/utilities/__init__.py @@ -0,0 +1 @@ +from modulo_components.utilities.utilities import generate_predicate_topic, parse_signal_name diff --git a/source/modulo_components/test/cpp/test_component.cpp b/source/modulo_components/test/cpp/test_component.cpp index 0314f514b..a8a13576f 100644 --- a/source/modulo_components/test/cpp/test_component.cpp +++ b/source/modulo_components/test/cpp/test_component.cpp @@ -28,12 +28,12 @@ class ComponentTest : public ::testing::Test { TEST_F(ComponentTest, AddOutput) { std::shared_ptr data = make_shared_state(CartesianState::Random("test")); - component_->add_output("_tEsT_#1@3", data, true); + component_->add_output("_tEsT_#1@3", data); EXPECT_TRUE(component_->outputs_.find("test_13") != component_->outputs_.end()); EXPECT_NO_THROW(component_->outputs_.at("test_13")->publish()); auto new_data = std::make_shared(false); - component_->add_output("test_13", new_data, true); + component_->add_output("test_13", new_data); EXPECT_EQ(component_->outputs_.at("test_13")->get_message_pair()->get_type(), modulo_core::communication::MessageType::ENCODED_STATE); } diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 90d0d1868..c42bbc687 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -82,12 +82,12 @@ TYPED_TEST(ComponentInterfaceTest, SetPredicateValue) { TYPED_TEST(ComponentInterfaceTest, AddInput) { auto data = std::make_shared(true); - EXPECT_NO_THROW(this->component_->add_input("_tEsT_#1@3", data, true)); + EXPECT_NO_THROW(this->component_->add_input("_tEsT_#1@3", data)); EXPECT_FALSE(this->component_->inputs_.find("test_13") == this->component_->inputs_.end()); EXPECT_EQ(this->component_->template get_parameter_value("test_13_topic"), "~/test_13"); EXPECT_NO_THROW(this->component_->template add_input( - "_tEsT_#1@5", [](const std::shared_ptr) {}, true, "/topic" + "_tEsT_#1@5", [](const std::shared_ptr) {}, "/topic", true )); EXPECT_FALSE(this->component_->inputs_.find("test_15") == this->component_->inputs_.end()); EXPECT_EQ(this->component_->template get_parameter_value("test_15_topic"), "/topic"); @@ -101,7 +101,7 @@ TYPED_TEST(ComponentInterfaceTest, AddInput) { TYPED_TEST(ComponentInterfaceTest, CreateOutput) { auto data = std::make_shared(true); - EXPECT_NO_THROW(this->component_->create_output("test", data, true, "/topic")); + EXPECT_NO_THROW(this->component_->create_output("test", data, "/topic", true)); EXPECT_FALSE(this->component_->outputs_.find("test") == this->component_->outputs_.end()); EXPECT_EQ(this->component_->template get_parameter_value("test_topic"), "/topic"); diff --git a/source/modulo_components/test/cpp/test_lifecycle_component.cpp b/source/modulo_components/test/cpp/test_lifecycle_component.cpp index b7147dea1..02a4d9361 100644 --- a/source/modulo_components/test/cpp/test_lifecycle_component.cpp +++ b/source/modulo_components/test/cpp/test_lifecycle_component.cpp @@ -28,7 +28,7 @@ class LifecycleComponentTest : public ::testing::Test { TEST_F(LifecycleComponentTest, AddOutput) { std::shared_ptr data = make_shared_state(CartesianState::Random("test")); - component_->add_output("test", data, true); + component_->add_output("test", data); auto outputs_iterator = component_->outputs_.find("test"); EXPECT_TRUE(outputs_iterator != component_->outputs_.end()); EXPECT_NO_THROW(component_->configure_outputs()); @@ -36,7 +36,7 @@ TEST_F(LifecycleComponentTest, AddOutput) { EXPECT_NO_THROW(component_->outputs_.at("test")->publish()); auto new_data = std::make_shared(false); - component_->add_output("test", new_data, true); + component_->add_output("test", new_data); EXPECT_EQ(component_->outputs_.at("test")->get_message_pair()->get_type(), modulo_core::communication::MessageType::ENCODED_STATE); } diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 7af131737..afd242996 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -3,7 +3,7 @@ import rclpy import state_representation as sr from modulo_components.component_interface import ComponentInterface -from modulo_components.exceptions.component_exceptions import LookupTransformError +from modulo_components.exceptions import LookupTransformError from std_msgs.msg import Bool, String diff --git a/source/modulo_components/test/python/test_component_interface_empty_parameters.py b/source/modulo_components/test/python/test_component_interface_empty_parameters.py index 643f2a6ba..5fc882b5b 100644 --- a/source/modulo_components/test/python/test_component_interface_empty_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_empty_parameters.py @@ -2,7 +2,7 @@ import rclpy import state_representation as sr from modulo_components.component_interface import ComponentInterface -from modulo_components.exceptions.component_exceptions import ComponentParameterError +from modulo_components.exceptions import ComponentParameterError from rcl_interfaces.msg import SetParametersResult from rclpy.exceptions import ParameterNotDeclaredException from rclpy.node import Node diff --git a/source/modulo_components/test/python/test_component_interface_parameters.py b/source/modulo_components/test/python/test_component_interface_parameters.py index 4a7c76ead..ad038901f 100644 --- a/source/modulo_components/test/python/test_component_interface_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_parameters.py @@ -3,7 +3,7 @@ import rclpy.node import state_representation as sr from modulo_components.component_interface import ComponentInterface -from modulo_components.exceptions.component_exceptions import ComponentParameterError +from modulo_components.exceptions import ComponentParameterError from rcl_interfaces.msg import SetParametersResult diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index af5fa334b..05b5152b7 100644 --- a/source/modulo_core/CMakeLists.txt +++ b/source/modulo_core/CMakeLists.txt @@ -21,7 +21,7 @@ find_package(ament_cmake_python REQUIRED) ament_auto_find_build_dependencies() -find_package(control_libraries 6.0.5 REQUIRED COMPONENTS state_representation) +find_package(control_libraries 6.0.7 REQUIRED COMPONENTS state_representation) find_package(clproto 6.0.0 REQUIRED) include_directories(include) @@ -36,6 +36,8 @@ ament_auto_add_library(modulo_core SHARED ${PROJECT_SOURCE_DIR}/src/translators/message_readers.cpp ${PROJECT_SOURCE_DIR}/src/translators/message_writers.cpp) +target_link_libraries(${PROJECT_NAME} clproto state_representation) + # install Python modules ament_python_install_package(${PROJECT_NAME} SCRIPTS_DESTINATION lib/${PROJECT_NAME}) diff --git a/source/modulo_core/modulo_core/__init__.py b/source/modulo_core/modulo_core/__init__.py index e69de29bb..e3ef07abe 100644 --- a/source/modulo_core/modulo_core/__init__.py +++ b/source/modulo_core/modulo_core/__init__.py @@ -0,0 +1 @@ +from modulo_core.encoded_state import EncodedState \ No newline at end of file diff --git a/source/modulo_core/modulo_core/exceptions/__init__.py b/source/modulo_core/modulo_core/exceptions/__init__.py index e69de29bb..f452aca34 100644 --- a/source/modulo_core/modulo_core/exceptions/__init__.py +++ b/source/modulo_core/modulo_core/exceptions/__init__.py @@ -0,0 +1 @@ +from modulo_core.exceptions.core_exceptions import CoreError, MessageTranslationError, ParameterTranslationError diff --git a/source/modulo_core/modulo_core/translators/message_readers.py b/source/modulo_core/modulo_core/translators/message_readers.py index f02ae72ca..ed82a2813 100644 --- a/source/modulo_core/modulo_core/translators/message_readers.py +++ b/source/modulo_core/modulo_core/translators/message_readers.py @@ -3,8 +3,8 @@ import clproto import geometry_msgs.msg as geometry import state_representation as sr -from modulo_core.encoded_state import EncodedState -from modulo_core.exceptions.core_exceptions import MessageTranslationError +from modulo_core import EncodedState +from modulo_core.exceptions import MessageTranslationError from sensor_msgs.msg import JointState DataT = TypeVar('DataT') diff --git a/source/modulo_core/modulo_core/translators/message_writers.py b/source/modulo_core/modulo_core/translators/message_writers.py index 0d36ebfdc..390f0164f 100644 --- a/source/modulo_core/modulo_core/translators/message_writers.py +++ b/source/modulo_core/modulo_core/translators/message_writers.py @@ -5,8 +5,8 @@ import numpy as np import rclpy.time import state_representation as sr -from modulo_core.encoded_state import EncodedState -from modulo_core.exceptions.core_exceptions import MessageTranslationError +from modulo_core import EncodedState +from modulo_core.exceptions import MessageTranslationError from sensor_msgs.msg import JointState DataT = TypeVar('DataT') @@ -55,7 +55,7 @@ def write_message(message: MsgT, state: StateT): """ if not isinstance(state, sr.State): raise MessageTranslationError("This state type is not supported.") - if state.is_empty(): + if not state: raise MessageTranslationError(f"{state.get_name()} state is empty while attempting to write it to message.") if isinstance(state, sr.CartesianState): if isinstance(message, geometry.Accel): diff --git a/source/modulo_core/modulo_core/translators/parameter_translators.py b/source/modulo_core/modulo_core/translators/parameter_translators.py index ba0e221cf..38e89177d 100644 --- a/source/modulo_core/modulo_core/translators/parameter_translators.py +++ b/source/modulo_core/modulo_core/translators/parameter_translators.py @@ -4,7 +4,7 @@ import clproto import numpy as np import state_representation as sr -from modulo_core.exceptions.core_exceptions import ParameterTranslationError +from modulo_core.exceptions import ParameterTranslationError from rclpy import Parameter @@ -16,7 +16,7 @@ def write_parameter(parameter: sr.Parameter) -> Parameter: :raises ParameterTranslationError if the parameter could not be written :return: The resulting ROS parameter """ - if parameter.is_empty(): + if not parameter: return Parameter(parameter.get_name(), value=None, type_=Parameter.Type.NOT_SET) elif parameter.get_parameter_type() == sr.ParameterType.BOOL or \ parameter.get_parameter_type() == sr.ParameterType.INT or \ diff --git a/source/modulo_core/src/translators/message_writers.cpp b/source/modulo_core/src/translators/message_writers.cpp index fbbffd6a3..d6e79969b 100644 --- a/source/modulo_core/src/translators/message_writers.cpp +++ b/source/modulo_core/src/translators/message_writers.cpp @@ -26,7 +26,7 @@ static void write_quaternion(geometry_msgs::msg::Quaternion& message, const Eige } void write_message(geometry_msgs::msg::Accel& message, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); @@ -42,7 +42,7 @@ void write_message(geometry_msgs::msg::AccelStamped& message, const CartesianSta } void write_message(geometry_msgs::msg::Pose& message, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); @@ -58,7 +58,7 @@ void write_message(geometry_msgs::msg::PoseStamped& message, const CartesianStat } void write_message(geometry_msgs::msg::Transform& message, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); @@ -76,7 +76,7 @@ write_message(geometry_msgs::msg::TransformStamped& message, const CartesianStat } void write_message(geometry_msgs::msg::Twist& message, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); @@ -92,7 +92,7 @@ void write_message(geometry_msgs::msg::TwistStamped& message, const CartesianSta } void write_message(geometry_msgs::msg::Wrench& message, const CartesianState& state, const rclcpp::Time&) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); @@ -108,7 +108,7 @@ void write_message(geometry_msgs::msg::WrenchStamped& message, const CartesianSt } void write_message(sensor_msgs::msg::JointState& message, const JointState& state, const rclcpp::Time& time) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); @@ -124,7 +124,7 @@ void write_message(sensor_msgs::msg::JointState& message, const JointState& stat } void write_message(tf2_msgs::msg::TFMessage& message, const CartesianState& state, const rclcpp::Time& time) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); @@ -136,7 +136,7 @@ void write_message(tf2_msgs::msg::TFMessage& message, const CartesianState& stat template void write_message(U& message, const Parameter& state, const rclcpp::Time&) { - if (state.is_empty()) { + if (!state) { throw exceptions::MessageTranslationException( state.get_name() + " state is empty while attempting to write it to message" ); diff --git a/source/modulo_core/test/python_tests/translators/test_messages.py b/source/modulo_core/test/python_tests/translators/test_messages.py index 28ef504d0..c1d12f541 100644 --- a/source/modulo_core/test/python_tests/translators/test_messages.py +++ b/source/modulo_core/test/python_tests/translators/test_messages.py @@ -5,8 +5,8 @@ import numpy as np import pytest import state_representation as sr -from modulo_core.encoded_state import EncodedState -from modulo_core.exceptions.core_exceptions import MessageTranslationError +from modulo_core import EncodedState +from modulo_core.exceptions import MessageTranslationError from rclpy.clock import Clock from sensor_msgs.msg import JointState diff --git a/source/modulo_core/test/python_tests/translators/test_parameters.py b/source/modulo_core/test/python_tests/translators/test_parameters.py index 498fc7d0d..c36f525fc 100644 --- a/source/modulo_core/test/python_tests/translators/test_parameters.py +++ b/source/modulo_core/test/python_tests/translators/test_parameters.py @@ -3,7 +3,7 @@ import pytest import state_representation as sr from modulo_core.translators.parameter_translators import write_parameter, read_parameter, read_parameter_const -from modulo_core.exceptions.core_exceptions import ParameterTranslationError +from modulo_core.exceptions import ParameterTranslationError from rclpy import Parameter From 1dd3f8ecf10e927950b6658f107986dbf594d9af Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 14 Jul 2022 14:15:21 +0200 Subject: [PATCH 58/71] Improved exception handling (#99) * Try catch in subscription callback * Try catch publishing in PublisherInterface * Try catch message translators in Python * Improve try catching in component interface * Update tests * Just rethrow exception Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- .../modulo_components/ComponentInterface.hpp | 2 +- .../modulo_components/component_interface.py | 9 +- .../test/cpp/test_component_interface.cpp | 2 +- .../communication/PublisherInterface.hpp | 2 +- .../communication/SubscriptionHandler.hpp | 1 + .../translators/message_readers.py | 94 ++++++++++--------- .../translators/message_writers.py | 90 +++++++++--------- .../src/communication/PublisherInterface.cpp | 48 +++++----- .../src/communication/SubscriptionHandler.cpp | 54 ++++++++--- .../communication/test_publisher_handler.cpp | 6 +- 10 files changed, 178 insertions(+), 130 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index c012b0f21..d81c38e7b 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -790,7 +790,7 @@ inline void ComponentInterface::publish_outputs() { for (const auto& [signal, publisher]: this->outputs_) { try { publisher->publish(); - } catch (const std::exception& ex) { + } catch (const modulo_core::exceptions::CoreException& ex) { RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Failed to publish output '" << signal << "': " << ex.what()); } diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 760489b40..fa0d0950e 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -10,7 +10,7 @@ from modulo_components.exceptions import AddSignalError, ComponentParameterError, LookupTransformError from modulo_components.utilities import generate_predicate_topic, parse_signal_name from modulo_core import EncodedState -from modulo_core.exceptions import ParameterTranslationError +from modulo_core.exceptions import MessageTranslationError, ParameterTranslationError from modulo_core.translators.parameter_translators import get_ros_parameter_type, read_parameter_const, write_parameter from rcl_interfaces.msg import ParameterDescriptor, SetParametersResult from rclpy.duration import Duration @@ -321,8 +321,9 @@ def __subscription_callback(self, message: MsgT, attribute_name: str, reader: Ca """ try: self.__setattr__(attribute_name, reader(message)) - except Exception as e: - self.get_logger().error(f"Failed to read message for attribute {attribute_name}", throttle_duration_sec=1.0) + except (AttributeError, MessageTranslationError) as e: + self.get_logger().warn(f"Failed to read message for attribute {attribute_name}: {e}", + throttle_duration_sec=1.0) def add_input(self, signal_name: str, subscription: Union[str, Callable], message_type: MsgT, default_topic="", fixed_topic=False): @@ -411,7 +412,7 @@ def send_transform(self, transform: sr.CartesianPose): transform_message = TransformStamped() modulo_writers.write_stamped_message(transform_message, transform, self.get_clock().now()) self.__tf_broadcaster.sendTransform(transform_message) - except TransformException as e: + except (MessageTranslationError, TransformException) as e: self.get_logger().error(f"Failed to send transform: {e}", throttle_duration_sec=1.0) def lookup_transform(self, frame_name: str, reference_frame_name="world", time_point=Time(), diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index c42bbc687..c132fb7f9 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -112,7 +112,7 @@ TYPED_TEST(ComponentInterfaceTest, CreateOutput) { EXPECT_EQ(pub_interface->get_type(), modulo_core::communication::PublisherType::LIFECYCLE_PUBLISHER); } EXPECT_EQ(pub_interface->get_message_pair()->get_type(), modulo_core::communication::MessageType::BOOL); - EXPECT_THROW(pub_interface->publish(), modulo_core::exceptions::InvalidPointerCastException); + EXPECT_THROW(pub_interface->publish(), modulo_core::exceptions::CoreException); } TYPED_TEST(ComponentInterfaceTest, TF) { diff --git a/source/modulo_core/include/modulo_core/communication/PublisherInterface.hpp b/source/modulo_core/include/modulo_core/communication/PublisherInterface.hpp index 7647bb3c9..6c268fbcb 100644 --- a/source/modulo_core/include/modulo_core/communication/PublisherInterface.hpp +++ b/source/modulo_core/include/modulo_core/communication/PublisherInterface.hpp @@ -85,7 +85,7 @@ class PublisherInterface : public std::enable_shared_from_this> subscription_; ///< The pointer referring to the ROS subscription + std::shared_ptr clock_; ///< ROS clock for throttling log }; template diff --git a/source/modulo_core/modulo_core/translators/message_readers.py b/source/modulo_core/modulo_core/translators/message_readers.py index ed82a2813..4308faeea 100644 --- a/source/modulo_core/modulo_core/translators/message_readers.py +++ b/source/modulo_core/modulo_core/translators/message_readers.py @@ -41,39 +41,42 @@ def read_message(state: StateT, message: MsgT) -> StateT: :raises MessageTranslationError if the message could not be read :return: The populated state """ - if not isinstance(state, sr.State): - raise MessageTranslationError("This state type is not supported.") - if isinstance(state, sr.CartesianState): - if isinstance(message, geometry.Accel): - state.set_linear_acceleration(read_xyz(message.linear)) - state.set_angular_acceleration(read_xyz(message.angular)) - elif isinstance(message, geometry.Pose): - state.set_position(read_xyz(message.position)) - state.set_orientation(read_quaternion(message.orientation)) - elif isinstance(message, geometry.Transform): - state.set_position(read_xyz(message.translation)) - state.set_orientation(read_quaternion(message.rotation)) - elif isinstance(message, geometry.Twist): - state.set_linear_velocity(read_xyz(message.linear)) - state.set_angular_velocity(read_xyz(message.angular)) - elif isinstance(message, geometry.Wrench): - state.set_force(read_xyz(message.force)) - state.set_torque(read_xyz(message.torque)) + try: + if not isinstance(state, sr.State): + raise MessageTranslationError("This state type is not supported.") + if isinstance(state, sr.CartesianState): + if isinstance(message, geometry.Accel): + state.set_linear_acceleration(read_xyz(message.linear)) + state.set_angular_acceleration(read_xyz(message.angular)) + elif isinstance(message, geometry.Pose): + state.set_position(read_xyz(message.position)) + state.set_orientation(read_quaternion(message.orientation)) + elif isinstance(message, geometry.Transform): + state.set_position(read_xyz(message.translation)) + state.set_orientation(read_quaternion(message.rotation)) + elif isinstance(message, geometry.Twist): + state.set_linear_velocity(read_xyz(message.linear)) + state.set_angular_velocity(read_xyz(message.angular)) + elif isinstance(message, geometry.Wrench): + state.set_force(read_xyz(message.force)) + state.set_torque(read_xyz(message.torque)) + else: + raise MessageTranslationError("The provided combination of state type and message type is not supported") + elif isinstance(message, JointState) and isinstance(state, sr.JointState): + try: + state.set_names(message.name) + if len(message.position): + state.set_positions(message.position) + if len(message.velocity): + state.set_velocities(message.velocity) + if len(message.effort): + state.set_torques(message.effort) + except Exception as e: + raise MessageTranslationError(f"{e}") else: raise MessageTranslationError("The provided combination of state type and message type is not supported") - elif isinstance(message, JointState) and isinstance(state, sr.JointState): - try: - state.set_names(message.name) - if len(message.position): - state.set_positions(message.position) - if len(message.velocity): - state.set_velocities(message.velocity) - if len(message.effort): - state.set_torques(message.effort) - except Exception as e: - raise MessageTranslationError(f"{e}") - else: - raise MessageTranslationError("The provided combination of state type and message type is not supported") + except Exception as e: + raise MessageTranslationError(f"{e}") return state @@ -86,19 +89,22 @@ def read_stamped_message(state: StateT, message: MsgT) -> StateT: :raises MessageTranslationError if the message could not be read :return: The populated state """ - if isinstance(message, geometry.AccelStamped): - read_message(state, message.accel) - elif isinstance(message, geometry.PoseStamped): - read_message(state, message.pose) - elif isinstance(message, geometry.TransformStamped): - read_message(state, message.transform) - state.set_name(message.child_frame_id) - elif isinstance(message, geometry.TwistStamped): - read_message(state, message.twist) - elif isinstance(message, geometry.WrenchStamped): - read_message(state, message.wrench) - else: - raise MessageTranslationError("The provided combination of state type and message type is not supported") + try: + if isinstance(message, geometry.AccelStamped): + read_message(state, message.accel) + elif isinstance(message, geometry.PoseStamped): + read_message(state, message.pose) + elif isinstance(message, geometry.TransformStamped): + read_message(state, message.transform) + state.set_name(message.child_frame_id) + elif isinstance(message, geometry.TwistStamped): + read_message(state, message.twist) + elif isinstance(message, geometry.WrenchStamped): + read_message(state, message.wrench) + else: + raise MessageTranslationError("The provided combination of state type and message type is not supported") + except MessageTranslationError as e: + raise MessageTranslationError(f"{e}") state.set_reference_frame(message.header.frame_id) return state diff --git a/source/modulo_core/modulo_core/translators/message_writers.py b/source/modulo_core/modulo_core/translators/message_writers.py index 390f0164f..119dbf851 100644 --- a/source/modulo_core/modulo_core/translators/message_writers.py +++ b/source/modulo_core/modulo_core/translators/message_writers.py @@ -53,35 +53,38 @@ def write_message(message: MsgT, state: StateT): :param state: The state to read from :raises MessageTranslationError if the message could not be written """ - if not isinstance(state, sr.State): - raise MessageTranslationError("This state type is not supported.") - if not state: - raise MessageTranslationError(f"{state.get_name()} state is empty while attempting to write it to message.") - if isinstance(state, sr.CartesianState): - if isinstance(message, geometry.Accel): - write_xyz(message.linear, state.get_linear_acceleration()) - write_xyz(message.angular, state.get_angular_acceleration()) - elif isinstance(message, geometry.Pose): - write_xyz(message.position, state.get_position()) - write_quaternion(message.orientation, state.get_orientation_coefficients()) - elif isinstance(message, geometry.Transform): - write_xyz(message.translation, state.get_position()) - write_quaternion(message.rotation, state.get_orientation_coefficients()) - elif isinstance(message, geometry.Twist): - write_xyz(message.linear, state.get_linear_velocity()) - write_xyz(message.angular, state.get_angular_velocity()) - elif isinstance(message, geometry.Wrench): - write_xyz(message.force, state.get_force()) - write_xyz(message.torque, state.get_torque()) + try: + if not isinstance(state, sr.State): + raise MessageTranslationError("This state type is not supported.") + if not state: + raise MessageTranslationError(f"{state.get_name()} state is empty while attempting to write it to message.") + if isinstance(state, sr.CartesianState): + if isinstance(message, geometry.Accel): + write_xyz(message.linear, state.get_linear_acceleration()) + write_xyz(message.angular, state.get_angular_acceleration()) + elif isinstance(message, geometry.Pose): + write_xyz(message.position, state.get_position()) + write_quaternion(message.orientation, state.get_orientation_coefficients()) + elif isinstance(message, geometry.Transform): + write_xyz(message.translation, state.get_position()) + write_quaternion(message.rotation, state.get_orientation_coefficients()) + elif isinstance(message, geometry.Twist): + write_xyz(message.linear, state.get_linear_velocity()) + write_xyz(message.angular, state.get_angular_velocity()) + elif isinstance(message, geometry.Wrench): + write_xyz(message.force, state.get_force()) + write_xyz(message.torque, state.get_torque()) + else: + raise MessageTranslationError("The provided combination of state type and message type is not supported") + elif isinstance(message, JointState) and isinstance(state, sr.JointState): + message.name = state.get_names() + message.position = state.get_positions().tolist() + message.velocity = state.get_velocities().tolist() + message.effort = state.get_torques().tolist() else: raise MessageTranslationError("The provided combination of state type and message type is not supported") - elif isinstance(message, JointState) and isinstance(state, sr.JointState): - message.name = state.get_names() - message.position = state.get_positions().tolist() - message.velocity = state.get_velocities().tolist() - message.effort = state.get_torques().tolist() - else: - raise MessageTranslationError("The provided combination of state type and message type is not supported") + except Exception as e: + raise MessageTranslationError(f"{e}") def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): @@ -93,21 +96,24 @@ def write_stamped_message(message: MsgT, state: StateT, time: rclpy.time.Time): :param time: The time of the message :raises MessageTranslationError if the message could not be written """ - if isinstance(message, geometry.AccelStamped): - write_message(message.accel, state) - elif isinstance(message, geometry.PoseStamped): - write_message(message.pose, state) - elif isinstance(message, geometry.TransformStamped): - write_message(message.transform, state) - message.child_frame_id = state.get_name() - elif isinstance(message, geometry.TwistStamped): - write_message(message.twist, state) - elif isinstance(message, geometry.WrenchStamped): - write_message(message.wrench, state) - else: - raise MessageTranslationError("The provided combination of state type and message type is not supported") - message.header.stamp = time.to_msg() - message.header.frame_id = state.get_reference_frame() + try: + if isinstance(message, geometry.AccelStamped): + write_message(message.accel, state) + elif isinstance(message, geometry.PoseStamped): + write_message(message.pose, state) + elif isinstance(message, geometry.TransformStamped): + write_message(message.transform, state) + message.child_frame_id = state.get_name() + elif isinstance(message, geometry.TwistStamped): + write_message(message.twist, state) + elif isinstance(message, geometry.WrenchStamped): + write_message(message.wrench, state) + else: + raise MessageTranslationError("The provided combination of state type and message type is not supported") + message.header.stamp = time.to_msg() + message.header.frame_id = state.get_reference_frame() + except Exception as e: + raise MessageTranslationError(f"{e}") def write_std_message(message: MsgT, data: DataT): diff --git a/source/modulo_core/src/communication/PublisherInterface.cpp b/source/modulo_core/src/communication/PublisherInterface.cpp index 4294460e3..9bb0f2705 100644 --- a/source/modulo_core/src/communication/PublisherInterface.cpp +++ b/source/modulo_core/src/communication/PublisherInterface.cpp @@ -81,28 +81,32 @@ void PublisherInterface::deactivate() { } void PublisherInterface::publish() { - if (this->message_pair_ == nullptr) { - throw exceptions::NullPointerException("Message pair is not set, nothing to publish"); - } - switch (this->message_pair_->get_type()) { - case MessageType::BOOL: - this->publish(this->message_pair_->write()); - break; - case MessageType::FLOAT64: - this->publish(this->message_pair_->write()); - break; - case MessageType::FLOAT64_MULTI_ARRAY: - this->publish(this->message_pair_->write>()); - break; - case MessageType::INT32: - this->publish(this->message_pair_->write()); - break; - case MessageType::STRING: - this->publish(this->message_pair_->write()); - break; - case MessageType::ENCODED_STATE: - this->publish(this->message_pair_->write()); - break; + try { + if (this->message_pair_ == nullptr) { + throw exceptions::NullPointerException("Message pair is not set, nothing to publish"); + } + switch (this->message_pair_->get_type()) { + case MessageType::BOOL: + this->publish(this->message_pair_->write()); + break; + case MessageType::FLOAT64: + this->publish(this->message_pair_->write()); + break; + case MessageType::FLOAT64_MULTI_ARRAY: + this->publish(this->message_pair_->write>()); + break; + case MessageType::INT32: + this->publish(this->message_pair_->write()); + break; + case MessageType::STRING: + this->publish(this->message_pair_->write()); + break; + case MessageType::ENCODED_STATE: + this->publish(this->message_pair_->write()); + break; + } + } catch (const exceptions::CoreException& ex) { + throw; } } diff --git a/source/modulo_core/src/communication/SubscriptionHandler.cpp b/source/modulo_core/src/communication/SubscriptionHandler.cpp index ba823ca80..fcee696d8 100644 --- a/source/modulo_core/src/communication/SubscriptionHandler.cpp +++ b/source/modulo_core/src/communication/SubscriptionHandler.cpp @@ -6,34 +6,39 @@ namespace modulo_core::communication { template<> SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : - SubscriptionInterface(std::move(message_pair)) {} + SubscriptionInterface(std::move(message_pair)), clock_(std::make_shared()) {} template<> SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : - SubscriptionInterface(std::move(message_pair)) {} + SubscriptionInterface(std::move(message_pair)), clock_(std::make_shared()) {} template<> SubscriptionHandler::SubscriptionHandler( std::shared_ptr message_pair ) : - SubscriptionInterface(std::move(message_pair)) {} + SubscriptionInterface(std::move(message_pair)), clock_(std::make_shared()) {} template<> SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : - SubscriptionInterface(std::move(message_pair)) {} + SubscriptionInterface(std::move(message_pair)), clock_(std::make_shared()) {} template<> SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : - SubscriptionInterface(std::move(message_pair)) {} + SubscriptionInterface(std::move(message_pair)), clock_(std::make_shared()) {} template<> SubscriptionHandler::SubscriptionHandler(std::shared_ptr message_pair) : - SubscriptionInterface(std::move(message_pair)) {} + SubscriptionInterface(std::move(message_pair)), clock_(std::make_shared()) {} template<> std::function)> SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { - this->get_message_pair()->template read(*message); + try { + this->get_message_pair()->template read(*message); + } catch (const exceptions::CoreException& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Exception in subscription callback: " << ex.what()); + } }; } @@ -41,7 +46,12 @@ template<> std::function)> SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { - this->get_message_pair()->template read(*message); + try { + this->get_message_pair()->template read(*message); + } catch (const exceptions::CoreException& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Exception in subscription callback: " << ex.what()); + } }; } @@ -49,7 +59,12 @@ template<> std::function)> SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { - this->get_message_pair()->template read>(*message); + try { + this->get_message_pair()->template read>(*message); + } catch (const exceptions::CoreException& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Exception in subscription callback: " << ex.what()); + } }; } @@ -57,7 +72,12 @@ template<> std::function)> SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { - this->get_message_pair()->template read(*message); + try { + this->get_message_pair()->template read(*message); + } catch (const exceptions::CoreException& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Exception in subscription callback: " << ex.what()); + } }; } @@ -65,14 +85,24 @@ template<> std::function)> SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { - this->get_message_pair()->template read(*message); + try { + this->get_message_pair()->template read(*message); + } catch (const exceptions::CoreException& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Exception in subscription callback: " << ex.what()); + } }; } template<> std::function)> SubscriptionHandler::get_callback() { return [this](const std::shared_ptr message) { - this->get_message_pair()->template read(*message); + try { + this->get_message_pair()->template read(*message); + } catch (const exceptions::CoreException& ex) { + RCLCPP_WARN_STREAM_THROTTLE(rclcpp::get_logger("SubscriptionHandler"), *this->clock_, 1000, + "Exception in subscription callback: " << ex.what()); + } }; } }// namespace modulo_core::communication diff --git a/source/modulo_core/test/cpp_tests/communication/test_publisher_handler.cpp b/source/modulo_core/test/cpp_tests/communication/test_publisher_handler.cpp index 0e6c1463c..5aa69a27a 100644 --- a/source/modulo_core/test/cpp_tests/communication/test_publisher_handler.cpp +++ b/source/modulo_core/test/cpp_tests/communication/test_publisher_handler.cpp @@ -4,7 +4,7 @@ #include "modulo_core/communication/PublisherHandler.hpp" #include "modulo_core/communication/MessagePair.hpp" -#include "modulo_core/exceptions/NullPointerException.hpp" +#include "modulo_core/exceptions/CoreException.hpp" using namespace modulo_core::communication; @@ -21,7 +21,7 @@ static void test_publisher_interface(const std::shared_ptr& node, // use in publisher interface std::shared_ptr publisher_interface(publisher_handler); - EXPECT_THROW(publisher_interface->publish(), modulo_core::exceptions::NullPointerException); + EXPECT_THROW(publisher_interface->publish(), modulo_core::exceptions::CoreException); publisher_interface->set_message_pair(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); @@ -71,7 +71,7 @@ TEST_F(PublisherTest, EncodedState) { // use in publisher interface std::shared_ptr publisher_interface(publisher_handler); - EXPECT_THROW(publisher_interface->publish(), modulo_core::exceptions::NullPointerException); + EXPECT_THROW(publisher_interface->publish(), modulo_core::exceptions::CoreException); publisher_interface->set_message_pair(message_pair); EXPECT_NO_THROW(publisher_interface->publish()); From e6443cfe61f7c29428529c5269adc3cd95adf2c9 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Thu, 14 Jul 2022 14:44:27 +0200 Subject: [PATCH 59/71] Add trigger and send predicate on set (#102) * Add trigger to component interface * Send predicate on set_predicate * Update tests * Publish trigger predicate right away Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Co-authored-by: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> --- .../modulo_components/ComponentInterface.hpp | 79 +++++++++++++++++-- .../modulo_components/component_interface.py | 78 +++++++++++++++--- .../component_public_interfaces.hpp | 3 + .../test/cpp/test_component_interface.cpp | 15 ++++ .../test/python/test_component_interface.py | 15 ++++ 5 files changed, 169 insertions(+), 21 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index d81c38e7b..0d0e30f76 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -166,6 +166,8 @@ class ComponentInterface : public NodeT { /** * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything. + * @details Even though the predicates are published periodically, the new value of this predicate will be published + * once immediately after setting it. * @param predicate_name the name of the predicate to retrieve from the map of predicates * @param predicate_value the new value of the predicate */ @@ -173,11 +175,26 @@ class ComponentInterface : public NodeT { /** * @brief Set the value of the predicate given as parameter, if the predicate is not found does not do anything. + * @details Even though the predicates are published periodically, the new value of this predicate will be published + * once immediately after setting it. * @param predicate_name the name of the predicate to retrieve from the map of predicates * @param predicate_function the function to call that returns the value of the predicate */ void set_predicate(const std::string& predicate_name, const std::function& predicate_function); + /** + * @brief Add a trigger to the component. Triggers are predicates that are always false except when it's triggered in + * which case it is set back to false immediately after it is read. + * @param trigger_name The name of the trigger + */ + void add_trigger(const std::string& trigger_name); + + /** + * @brief Latch the trigger with the provided name. + * @param trigger_name The name of the trigger + */ + void trigger(const std::string& trigger_name); + /** * @brief Add and configure an input signal of the component. * @tparam DataT Type of the data pointer @@ -274,6 +291,12 @@ class ComponentInterface : public NodeT { const tf2::TimePoint& time_point = tf2::TimePoint(std::chrono::microseconds(0)), const tf2::Duration& duration = tf2::Duration(std::chrono::microseconds(10))); + /** + * @brief Helper function to publish a predicate. + * @param name The name of the predicate to publish + */ + void publish_predicate(const std::string& name); + /** * @brief Helper function to publish all predicates. */ @@ -327,6 +350,7 @@ class ComponentInterface : public NodeT { std::map predicates_; ///< Map of predicates std::map>> predicate_publishers_; ///< Map of predicate publishers + std::map triggers_; ///< Map of triggers std::map> periodic_callbacks_; ///< Map of periodic function callbacks @@ -557,6 +581,39 @@ inline bool ComponentInterface::get_predicate(const std::string& predicat return value; } +template +inline void ComponentInterface::add_trigger(const std::string& trigger_name) { + if (trigger_name.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed to add trigger: Provide a non empty string as a name."); + return; + } + if (this->triggers_.find(trigger_name) != this->triggers_.end() + || this->predicates_.find(trigger_name) != this->predicates_.end()) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add trigger: there is already a trigger or " + "predicate with name '" << trigger_name << "'."); + return; + } + this->triggers_.insert_or_assign(trigger_name, false); + this->add_predicate( + trigger_name, [this, &trigger_name] { + auto value = this->triggers_.at(trigger_name); + this->triggers_.at(trigger_name) = false; + return value; + } + ); +} + +template +inline void ComponentInterface::trigger(const std::string& trigger_name) { + if (this->triggers_.find(trigger_name) == this->triggers_.end()) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to trigger: could not find trigger" + " with name '" << trigger_name << "'."); + return; + } + this->triggers_.at(trigger_name) = true; + publish_predicate(trigger_name); +} + template inline void ComponentInterface::set_variant_predicate( const std::string& name, const utilities::PredicateVariant& predicate @@ -568,6 +625,7 @@ inline void ComponentInterface::set_variant_predicate( return; } predicate_iterator->second = predicate; + this->publish_predicate(name); } template @@ -771,17 +829,22 @@ inline state_representation::CartesianPose ComponentInterface::lookup_tra } } +template +inline void ComponentInterface::publish_predicate(const std::string& name) { + std_msgs::msg::Bool message; + message.data = this->get_predicate(name); + if (this->predicate_publishers_.find(name) == this->predicate_publishers_.end()) { + RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, + "No publisher for predicate '" << name << "' found."); + return; + } + predicate_publishers_.at(name)->publish(message); +} + template inline void ComponentInterface::publish_predicates() { for (const auto& predicate: this->predicates_) { - std_msgs::msg::Bool message; - message.data = this->get_predicate(predicate.first); - if (this->predicate_publishers_.find(predicate.first) == this->predicate_publishers_.end()) { - RCLCPP_ERROR_STREAM_THROTTLE(this->get_logger(), *this->get_clock(), 1000, - "No publisher for predicate " << predicate.first << " found."); - return; - } - predicate_publishers_.at(predicate.first)->publish(message); + this->publish_predicate(predicate.first); } } diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index fa0d0950e..3c6084d4c 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -38,6 +38,7 @@ def __init__(self, node_name: str, *kargs, **kwargs): self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {} self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {} self._predicate_publishers: Dict[str, Publisher] = {} + self._triggers: Dict[str, bool] = {} self._periodic_callbacks: Dict[str, Callable[[], None]] = {} self._inputs = {} self._outputs = {} @@ -254,7 +255,9 @@ def get_predicate(self, name: str) -> bool: def set_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): """ - Set the value of the predicate given as parameter, if the predicate is not found does not do anything. + Set the value of the predicate given as parameter, if the predicate is not found does not do anything. Even + though the predicates are published periodically, the new value of this predicate will be published once + immediately after setting it. :param name: The name of the predicate to retrieve from the map of predicates :param value: The new value of the predicate as a bool or a callable function @@ -265,6 +268,47 @@ def set_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): throttle_duration_sec=1.0) return self._predicates[name] = value + self._publish_predicate(name) + + def add_trigger(self, trigger_name: str): + """ + Add a trigger to the component. Triggers are predicates that are always false except when it's triggered in + which case it is set back to false immediately after it is read. + + :param trigger_name: The name of the trigger + """ + if not trigger_name: + self.get_logger().error("Failed to add trigger: Provide a non empty string as a name.") + return + if trigger_name in self._triggers.keys() or trigger_name in self._predicates.keys(): + self.get_logger().error( + f"Failed to add trigger: there is already a trigger or predicate with name '{trigger_name}'.") + return + self._triggers[trigger_name] = False + self.add_predicate(trigger_name, partial(self.__get_trigger_value, trigger_name=trigger_name)) + + def __get_trigger_value(self, trigger_name: str) -> bool: + """ + Get the trigger value and set it to false independent of the previous value. + + :param trigger_name: The name of the trigger + :return: The value of the trigger + """ + value = self._triggers[trigger_name] + self._triggers[trigger_name] = False + return value + + def trigger(self, trigger_name: str): + """ + Latch the trigger with the provided name. + + :param trigger_name: The name of the trigger + """ + if not trigger_name in self._triggers.keys(): + self.get_logger().error(f"Failed to trigger: could not find trigger with name '{trigger_name}'.") + return + self._triggers[trigger_name] = True + self._publish_predicate(trigger_name) def _create_output(self, signal_name: str, data: str, message_type: MsgT, clproto_message_type: clproto.MessageType, default_topic: str, fixed_topic: bool) -> str: @@ -466,23 +510,31 @@ def add_periodic_callback(self, name: str, callback: Callable[[], None]): self.get_logger().debug(f"Adding periodic function '{name}'.") self._periodic_callbacks[name] = callback + def _publish_predicate(self, name): + """ + Helper function to publish a predicate. + + :param name: The name of the predicate to publish + """ + message = Bool() + value = self.get_predicate(name) + try: + message.data = value + except AssertionError: + self.get_logger().error(f"Predicate '{name}' has invalid type: expected 'bool', got '{type(value)}'.", + throttle_duration_sec=1.0) + return + if name not in self._predicate_publishers.keys(): + self.get_logger().error(f"No publisher for predicate '{name}' found.", throttle_duration_sec=1.0) + return + self._predicate_publishers[name].publish(message) + def _publish_predicates(self): """ Helper function to publish all predicates. """ for name in self._predicates.keys(): - message = Bool() - value = self.get_predicate(name) - try: - message.data = value - except AssertionError: - self.get_logger().error(f"Predicate '{name}' has invalid type: expected 'bool', got '{type(value)}'.", - throttle_duration_sec=1.0) - continue - if name not in self._predicate_publishers.keys(): - self.get_logger().error(f"No publisher for predicate '{name}' found.", throttle_duration_sec=1.0) - continue - self._predicate_publishers[name].publish(message) + self._publish_predicate(name) def _publish_outputs(self): """ diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 594bc4fbd..0d75e867b 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -24,6 +24,9 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::get_predicate; using ComponentInterface::set_predicate; using ComponentInterface::predicates_; + using ComponentInterface::add_trigger; + using ComponentInterface::trigger; + using ComponentInterface::triggers_; using ComponentInterface::add_input; using ComponentInterface::inputs_; using ComponentInterface::create_output; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index c132fb7f9..66797a8a3 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -139,4 +139,19 @@ TYPED_TEST(ComponentInterfaceTest, RaiseError) { this->component_->raise_error(); EXPECT_TRUE(this->component_->get_predicate("in_error_state")); } + +TYPED_TEST(ComponentInterfaceTest, AddTrigger) { + EXPECT_NO_THROW(this->component_->add_trigger("trigger")); + ASSERT_FALSE(this->component_->triggers_.find("trigger") == this->component_->triggers_.end()); + EXPECT_FALSE(this->component_->triggers_.at("trigger")); + EXPECT_FALSE(this->component_->get_predicate("trigger")); + EXPECT_NO_THROW(this->component_->trigger("trigger")); + // When reading, the trigger will be true only once + this->component_->triggers_.at("trigger") = true; + EXPECT_TRUE(this->component_->triggers_.at("trigger")); + EXPECT_TRUE(this->component_->get_predicate("trigger")); + // After the predicate function was evaluated once, the trigger is back to false + EXPECT_FALSE(this->component_->triggers_.at("trigger")); + EXPECT_FALSE(this->component_->get_predicate("trigger")); +} }// namespace modulo_components diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index afd242996..7581a3b87 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -78,3 +78,18 @@ def test_tf(component_interface): identity = send_tf * lookup_tf.inverse() assert np.linalg.norm(identity.data()) - 1 < 1e-3 assert abs(identity.get_orientation().w) - 1 < 1e-3 + + +def test_add_trigger(component_interface): + component_interface.add_trigger("trigger") + assert "trigger" in component_interface._triggers.keys() + assert not component_interface._triggers["trigger"] + assert not component_interface.get_predicate("trigger") + component_interface.trigger("trigger") + # When reading, the trigger will be true only once + component_interface._triggers["trigger"] = True + assert component_interface._triggers["trigger"] + assert component_interface.get_predicate("trigger") + # After the predicate function was evaluated once, the trigger is back to false + assert not component_interface._triggers["trigger"] + assert not component_interface.get_predicate("trigger") From 31417226bbdc7ffe3e91b3cc2b7bb9e2b498a139 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Sat, 16 Jul 2022 00:08:12 +0200 Subject: [PATCH 60/71] Component services (#103) * Add component interfaces package * Define service messages for empty and string triggers * Add component_interfaces to Dockerfile stage * Add dependency of modulo_components on component interfaces * Initial C++ add_service implementation * Initial Python add_service implementation * Code safety improvements * Parse service name using parse_signal_name utility * Wrap function in try catch block * Wrap service callback in try block * Try to execute the user callback safely and return a failure with exception message code on any exception * Support more return type variants in Python service callback * Add test stubs * Add component interfaces to CI build test action * Elevate visibility of add_service in C++ test interface --- .../actions/build-test-galactic/entrypoint.sh | 1 + Dockerfile | 10 ++- .../CMakeLists.txt | 9 ++ .../modulo_component_interfaces/package.xml | 21 +++++ .../srv/EmptyTrigger.srv | 3 + .../srv/StringTrigger.srv | 4 + .../modulo_components/ComponentInterface.hpp | 84 +++++++++++++++++++ .../modulo_components/component_interface.py | 51 +++++++++++ source/modulo_components/package.xml | 1 + .../component_public_interfaces.hpp | 1 + .../test/cpp/test_component_interface.cpp | 14 ++++ .../test/python/test_component_interface.py | 12 +++ 12 files changed, 209 insertions(+), 2 deletions(-) create mode 100644 source/modulo_component_interfaces/CMakeLists.txt create mode 100644 source/modulo_component_interfaces/package.xml create mode 100644 source/modulo_component_interfaces/srv/EmptyTrigger.srv create mode 100644 source/modulo_component_interfaces/srv/StringTrigger.srv diff --git a/.github/actions/build-test-galactic/entrypoint.sh b/.github/actions/build-test-galactic/entrypoint.sh index 46a7818f5..d3c0204a2 100755 --- a/.github/actions/build-test-galactic/entrypoint.sh +++ b/.github/actions/build-test-galactic/entrypoint.sh @@ -26,6 +26,7 @@ build_and_test() { source /opt/ros/"${ROS_DISTRO}"/setup.bash cd /home/ros2/ros2_ws +build_and_test modulo_component_interfaces build_and_test modulo_core build_and_test modulo_components diff --git a/Dockerfile b/Dockerfile index c15099a85..b8f36a9f9 100644 --- a/Dockerfile +++ b/Dockerfile @@ -3,12 +3,18 @@ FROM ghcr.io/aica-technology/ros2-control-libraries:${BASE_TAG} as dependencies WORKDIR ${HOME}/ros2_ws -FROM dependencies as modulo-core +FROM dependencies as modulo-component-interfaces -COPY --chown=${USER} ./source/modulo_core ./src/modulo_core +COPY --chown=${USER} ./source/modulo_component_interfaces ./src/modulo_component_interfaces RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build" +FROM modulo-component-interfaces as modulo-core + +COPY --chown=${USER} ./source/modulo_core ./src/modulo_core +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build --packages-select modulo_core" + + FROM modulo-core as modulo-components COPY --chown=${USER} ./source/modulo_components ./src/modulo_components diff --git a/source/modulo_component_interfaces/CMakeLists.txt b/source/modulo_component_interfaces/CMakeLists.txt new file mode 100644 index 000000000..76cc0ad95 --- /dev/null +++ b/source/modulo_component_interfaces/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.15) +project(modulo_component_interfaces) + +find_package(rosidl_default_generators REQUIRED) + +file(GLOB SRVS RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} srv/*.srv) +rosidl_generate_interfaces(${PROJECT_NAME} ${SRVS}) + +ament_package() \ No newline at end of file diff --git a/source/modulo_component_interfaces/package.xml b/source/modulo_component_interfaces/package.xml new file mode 100644 index 000000000..1795007ab --- /dev/null +++ b/source/modulo_component_interfaces/package.xml @@ -0,0 +1,21 @@ + + + + modulo_component_interfaces + 0.0.1 + Interface package for communicating with modulo components through the ROS framework + Enrico Eberhard + TODO: License declaration + + ament_cmake + + rcl_interfaces + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + + ament_cmake + + diff --git a/source/modulo_component_interfaces/srv/EmptyTrigger.srv b/source/modulo_component_interfaces/srv/EmptyTrigger.srv new file mode 100644 index 000000000..cf5bd38fd --- /dev/null +++ b/source/modulo_component_interfaces/srv/EmptyTrigger.srv @@ -0,0 +1,3 @@ +--- +bool success +string message \ No newline at end of file diff --git a/source/modulo_component_interfaces/srv/StringTrigger.srv b/source/modulo_component_interfaces/srv/StringTrigger.srv new file mode 100644 index 000000000..92d543837 --- /dev/null +++ b/source/modulo_component_interfaces/srv/StringTrigger.srv @@ -0,0 +1,4 @@ +string payload +--- +bool success +string message \ No newline at end of file diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 0d0e30f76..b7c1a5490 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -24,6 +24,9 @@ #include #include +#include +#include + #include "modulo_components/exceptions/AddSignalException.hpp" #include "modulo_components/exceptions/ComponentParameterException.hpp" #include "modulo_components/exceptions/LookupTransformException.hpp" @@ -32,6 +35,15 @@ namespace modulo_components { +/** + * @struct ComponentServiceResponse + * @brief TODO + */ +struct ComponentServiceResponse { + bool success; + std::string message; +}; + /** * @brief TODO * @tparam NodeT @@ -225,6 +237,26 @@ class ComponentInterface : public NodeT { const std::string& default_topic = "", bool fixed_topic = false ); + /** + * @brief Add a service to trigger a callback function with no input arguments. + * @param service_name The name of the service + * @param callback A service callback function with no arguments that returns a ComponentServiceResponse + */ + void add_service(const std::string& service_name, const std::function& callback); + + /** + * @brief Add a service to trigger a callback function with a string payload. + * @details The string payload can have an arbitrary format to parameterize and control the callback behaviour + * as desired. It is the responsibility of the service callback to parse the string according to some payload format. + * When adding a service with a string payload, be sure to document the payload format appropriately. + * @param service_name The name of the service + * @param callback A service callback function with a string argument that returns a ComponentServiceResponse + */ + void add_service( + const std::string& service_name, + const std::function& callback + ); + /** * @brief Add a periodic callback function. * @details The provided function is evaluated periodically at the component step period. @@ -756,6 +788,58 @@ inline void ComponentInterface::add_input( } } +template +inline void ComponentInterface::add_service( + const std::string& service_name, const std::function& callback +) { + try { + std::string parsed_service_name = utilities::parse_signal_name(service_name); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding empty service '" << parsed_service_name << "."); + NodeT::template create_service( + parsed_service_name, [callback]( + const std::shared_ptr, + std::shared_ptr response + ) { + try { + auto callback_response = callback(); + response->success = callback_response.success; + response->message = callback_response.message; + } catch (const std::exception& ex) { + response->success = false; + response->message = ex.what(); + } + }); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add service '" << service_name << "': " << ex.what()); + } +} + +template +inline void ComponentInterface::add_service( + const std::string& service_name, const std::function& callback +) { + try { + std::string parsed_service_name = utilities::parse_signal_name(service_name); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding string service '" << parsed_service_name << "."); + NodeT::template create_service( + parsed_service_name, [callback]( + const std::shared_ptr request, + std::shared_ptr response + ) { + try { + auto callback_response = callback(request->payload); + response->success = callback_response.success; + response->message = callback_response.message; + } catch (const std::exception& ex) { + response->success = false; + response->message = ex.what(); + } + }); + } catch (const std::exception& ex) { + RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add service '" << service_name << "': " << ex.what()); + } +} + template inline void ComponentInterface::add_periodic_callback(const std::string& name, const std::function& callback) { diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 3c6084d4c..fec62b424 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -1,4 +1,5 @@ import sys +import inspect from functools import partial from typing import Callable, Dict, List, Optional, TypeVar, Union @@ -7,6 +8,7 @@ import modulo_core.translators.message_writers as modulo_writers import state_representation as sr from geometry_msgs.msg import TransformStamped +from modulo_component_interfaces.srv import EmptyTrigger, StringTrigger from modulo_components.exceptions import AddSignalError, ComponentParameterError, LookupTransformError from modulo_components.utilities import generate_predicate_topic, parse_signal_name from modulo_core import EncodedState @@ -421,6 +423,55 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag except Exception as e: self.get_logger().error(f"Failed to add input '{signal_name}': {e}") + def add_service(self, service_name: str, callback: Union[Callable[[], dict], Callable[[str], dict]]): + """ + Add a service to trigger a callback function. + The callback should take either no arguments (empty service) or a single string argument (string service). + The string payload can have an arbitrary format to parameterize and control the callback behaviour as desired. + It is the responsibility of the service callback to parse the string according to some payload format. + When adding a service with a string payload, be sure to document the payload format appropriately. + + :param service_name: The name of the service + :param callback: The callback function to execute + :return: A dict with the outcome of the service call, containing "success" and "message" fields in the format + {"success": [True | False], "message": "..."} + """ + def callback_wrapper(request, response, cb): + try: + if hasattr(request, "payload"): + ret = cb(request.payload) + else: + ret = cb() + + # if the return does not contain a success field or bool result, + # but the callback completes without error, assume it was successful + response.success = True + if isinstance(ret, dict): + if "success" in ret.keys(): + response.success = ret["success"] + if "message" in ret.keys(): + response.message = ret["message"] + elif isinstance(ret, bool): + response.success = ret + except Exception as e: + response.success = False + response.message = f"{e}" + return response + + try: + parsed_service_name = parse_signal_name(service_name) + signature = inspect.signature(callback) + if len(signature.parameters) == 0: + self.get_logger().debug(f"Adding empty service {parsed_service_name}") + service_type = EmptyTrigger + else: + self.get_logger().debug(f"Adding string service {parsed_service_name}") + service_type = StringTrigger + self.create_service(service_type, parsed_service_name, + lambda request, response: callback_wrapper(request, response, callback)) + except Exception as e: + self.get_logger().error(f"Failed to add service '{service_name}': {e}") + def add_tf_broadcaster(self): """ Configure a transform broadcaster. diff --git a/source/modulo_components/package.xml b/source/modulo_components/package.xml index f38b9a336..13b1e863e 100644 --- a/source/modulo_components/package.xml +++ b/source/modulo_components/package.xml @@ -13,6 +13,7 @@ ament_cmake_python modulo_core + modulo_component_interfaces rclcpp_lifecycle tf2_ros diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index 0d75e867b..ab2748486 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -28,6 +28,7 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::trigger; using ComponentInterface::triggers_; using ComponentInterface::add_input; + using ComponentInterface::add_service; using ComponentInterface::inputs_; using ComponentInterface::create_output; using ComponentInterface::outputs_; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 66797a8a3..6c8839ccf 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -99,6 +99,20 @@ TYPED_TEST(ComponentInterfaceTest, AddInput) { modulo_core::communication::MessageType::BOOL); } +TYPED_TEST(ComponentInterfaceTest, AddService) { + auto empty_callback = []() -> ComponentServiceResponse { + return ComponentServiceResponse(); + }; + EXPECT_NO_THROW(this->component_->add_service("empty", empty_callback)); + + auto string_callback = [](const std::string&) -> ComponentServiceResponse { + return ComponentServiceResponse(); + }; + EXPECT_NO_THROW(this->component_->add_service("string", string_callback)); + + // TODO: use a service client to trigger the service and test the behaviour +} + TYPED_TEST(ComponentInterfaceTest, CreateOutput) { auto data = std::make_shared(true); EXPECT_NO_THROW(this->component_->create_output("test", data, "/topic", true)); diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 7581a3b87..644e7d650 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -65,6 +65,18 @@ def test_add_input(component_interface): assert component_interface._inputs["test_13"].msg_type == Bool +def test_add_service(component_interface): + def empty_callback(): + return {"success": True, "message": "test"} + component_interface.add_service("empty", empty_callback) + + def string_callback(): + return {"success": True, "message": "test"} + component_interface.add_service("empty", string_callback) + + # TODO: use a service client to trigger the service and test the behaviour + + def test_tf(component_interface): component_interface.add_tf_broadcaster() component_interface.add_tf_listener() From 877f8a5a038e0092b6de634f06ef8fb76670d378 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Mon, 18 Jul 2022 18:16:32 +0200 Subject: [PATCH 61/71] Create service under local namespace (#104) * Prepend "~/" to the parsed service topic * Formatting fixes in log lines --- .../include/modulo_components/ComponentInterface.hpp | 8 ++++---- .../modulo_components/component_interface.py | 6 +++--- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index b7c1a5490..65ac3c759 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -794,9 +794,9 @@ inline void ComponentInterface::add_service( ) { try { std::string parsed_service_name = utilities::parse_signal_name(service_name); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding empty service '" << parsed_service_name << "."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding empty service '" << parsed_service_name << "'."); NodeT::template create_service( - parsed_service_name, [callback]( + "~/" + parsed_service_name, [callback]( const std::shared_ptr, std::shared_ptr response ) { @@ -820,9 +820,9 @@ inline void ComponentInterface::add_service( ) { try { std::string parsed_service_name = utilities::parse_signal_name(service_name); - RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding string service '" << parsed_service_name << "."); + RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding string service '" << parsed_service_name << "'."); NodeT::template create_service( - parsed_service_name, [callback]( + "~/" + parsed_service_name, [callback]( const std::shared_ptr request, std::shared_ptr response ) { diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index fec62b424..fde1d651b 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -462,12 +462,12 @@ def callback_wrapper(request, response, cb): parsed_service_name = parse_signal_name(service_name) signature = inspect.signature(callback) if len(signature.parameters) == 0: - self.get_logger().debug(f"Adding empty service {parsed_service_name}") + self.get_logger().debug(f"Adding empty service '{parsed_service_name}'") service_type = EmptyTrigger else: - self.get_logger().debug(f"Adding string service {parsed_service_name}") + self.get_logger().debug(f"Adding string service '{parsed_service_name}'") service_type = StringTrigger - self.create_service(service_type, parsed_service_name, + self.create_service(service_type, "~/" + parsed_service_name, lambda request, response: callback_wrapper(request, response, callback)) except Exception as e: self.get_logger().error(f"Failed to add service '{service_name}': {e}") From dd11a40b8cc35e352b4376125c646ef71e2afa69 Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Wed, 20 Jul 2022 16:46:30 +0200 Subject: [PATCH 62/71] Add service improvements (#105) * Rename parse_signal_name utility to be generic for topics * Implement AddServiceException/Error * Revise add_input and add_service (C++) * Store a map of service pointers to ensure they stay in scope and to enable checking for previously created services * Reduce code duplication by making separate validation helper functions to parse the input and service names * Add checks for empty or previously registered service names * Revise add_input and add_service (Python) * Extend add_service tests * ComponentServiceResponse docstring --- .../modulo_components/ComponentInterface.hpp | 87 ++++++++++++++----- .../exceptions/AddServiceException.hpp | 16 ++++ .../modulo_components/utilities/utilities.hpp | 8 +- .../modulo_components/component_interface.py | 20 +++-- .../modulo_components/exceptions/__init__.py | 4 +- .../exceptions/component_exceptions.py | 10 +++ .../modulo_components/utilities/__init__.py | 2 +- .../modulo_components/utilities/utilities.py | 8 +- .../component_public_interfaces.hpp | 2 + .../test/cpp/test_component_interface.cpp | 30 +++++++ .../test/python/test_component_interface.py | 18 ++++ 11 files changed, 163 insertions(+), 42 deletions(-) create mode 100644 source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index 65ac3c759..a4d2341d1 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -27,6 +27,7 @@ #include #include +#include "modulo_components/exceptions/AddServiceException.hpp" #include "modulo_components/exceptions/AddSignalException.hpp" #include "modulo_components/exceptions/ComponentParameterException.hpp" #include "modulo_components/exceptions/LookupTransformException.hpp" @@ -37,7 +38,9 @@ namespace modulo_components { /** * @struct ComponentServiceResponse - * @brief TODO + * @brief Response structure to be returned by component services. + * @details The structure contains a bool success field and a string message field. + * This information is used to provide feedback on the service outcome to the service client. */ struct ComponentServiceResponse { bool success; @@ -376,6 +379,22 @@ class ComponentInterface : public NodeT { */ void set_variant_predicate(const std::string& name, const utilities::PredicateVariant& predicate); + /** + * @brief Validate an add_input request by parsing the signal name and checking the map of registered inputs. + * @param signal_name The name of the input signal + * @throws AddSignalException if the input could not be created (empty name or already registered) + * @return The parsed signal name + */ + std::string validate_input_signal_name(const std::string& signal_name); + + /** + * @brief Validate an add_service request by parsing the service name and checking the maps of registered services. + * @param service_name The name of the service + * @throws AddServiceException if the service could not be created (empty name or already registered) + * @return The parsed service name + */ + std::string validate_service_name(const std::string& service_name); + modulo_core::communication::PublisherType publisher_type_; ///< Type of the output publishers (one of PUBLISHER, LIFECYCLE_PUBLISHER) @@ -384,6 +403,11 @@ class ComponentInterface : public NodeT { predicate_publishers_; ///< Map of predicate publishers std::map triggers_; ///< Map of triggers + std::map>> + empty_services_; ///< Map of EmptyTrigger services + std::map>> + string_services_; ///< Map of StringTrigger services + std::map> periodic_callbacks_; ///< Map of periodic function callbacks state_representation::ParameterMap parameter_map_; ///< ParameterMap for handling parameters @@ -672,6 +696,20 @@ inline void ComponentInterface::set_predicate( this->set_variant_predicate(name, utilities::PredicateVariant(predicate)); } +template +inline std::string ComponentInterface::validate_input_signal_name(const std::string& signal_name) { + std::string parsed_signal_name = utilities::parse_topic_name(signal_name); + if (parsed_signal_name.empty()) { + throw exceptions::AddSignalException( + "Failed to add input '" + signal_name + "': Parsed signal name is empty." + ); + } + if (this->inputs_.find(parsed_signal_name) != this->inputs_.cend()) { + throw exceptions::AddSignalException("Failed to add input '" + signal_name + "': Input already exists"); + } + return parsed_signal_name; +} + template template inline void ComponentInterface::add_input( @@ -680,15 +718,7 @@ inline void ComponentInterface::add_input( ) { using namespace modulo_core::communication; try { - std::string parsed_signal_name = utilities::parse_signal_name(signal_name); - if (parsed_signal_name.empty()) { - throw exceptions::AddSignalException( - "Failed to add input '" + signal_name + "': Parsed signal name is empty." - ); - } - if (this->inputs_.find(parsed_signal_name) != this->inputs_.end()) { - throw exceptions::AddSignalException("Failed to add input '" + signal_name + "': Input already exists"); - } + std::string parsed_signal_name = this->validate_input_signal_name(signal_name); std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; auto parameter_name = parsed_signal_name + "_topic"; if (NodeT::has_parameter(parameter_name) && this->get_parameter(parameter_name)->is_empty()) { @@ -762,15 +792,7 @@ inline void ComponentInterface::add_input( ) { using namespace modulo_core::communication; try { - std::string parsed_signal_name = utilities::parse_signal_name(signal_name); - if (parsed_signal_name.empty()) { - throw exceptions::AddSignalException( - "Failed to add input '" + signal_name + "': Parsed signal name is empty." - ); - } - if (this->inputs_.find(parsed_signal_name) != this->inputs_.end()) { - throw exceptions::AddSignalException("Failed to add input '" + signal_name + "': Input already exists"); - } + std::string parsed_signal_name = this->validate_input_signal_name(signal_name); std::string topic_name = default_topic.empty() ? "~/" + parsed_signal_name : default_topic; this->add_parameter( parsed_signal_name + "_topic", topic_name, "Output topic name of signal '" + parsed_signal_name + "'", @@ -788,14 +810,29 @@ inline void ComponentInterface::add_input( } } +template +inline std::string ComponentInterface::validate_service_name(const std::string& service_name) { + std::string parsed_service_name = utilities::parse_topic_name(service_name); + if (parsed_service_name.empty()) { + throw exceptions::AddServiceException( + "Failed to add service '" + service_name + "': Parsed service name is empty." + ); + } + if (this->empty_services_.find(parsed_service_name) != this->empty_services_.cend() + || this->string_services_.find(parsed_service_name) != this->string_services_.cend()) { + throw exceptions::AddServiceException("Failed to add service '" + service_name + "': Service already exists"); + } + return parsed_service_name; +} + template inline void ComponentInterface::add_service( const std::string& service_name, const std::function& callback ) { try { - std::string parsed_service_name = utilities::parse_signal_name(service_name); + std::string parsed_service_name = validate_service_name(service_name); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding empty service '" << parsed_service_name << "'."); - NodeT::template create_service( + auto service = NodeT::template create_service( "~/" + parsed_service_name, [callback]( const std::shared_ptr, std::shared_ptr response @@ -809,6 +846,7 @@ inline void ComponentInterface::add_service( response->message = ex.what(); } }); + this->empty_services_.insert_or_assign(parsed_service_name, service); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add service '" << service_name << "': " << ex.what()); } @@ -819,9 +857,9 @@ inline void ComponentInterface::add_service( const std::string& service_name, const std::function& callback ) { try { - std::string parsed_service_name = utilities::parse_signal_name(service_name); + std::string parsed_service_name = validate_service_name(service_name); RCLCPP_DEBUG_STREAM(this->get_logger(), "Adding string service '" << parsed_service_name << "'."); - NodeT::template create_service( + auto service = NodeT::template create_service( "~/" + parsed_service_name, [callback]( const std::shared_ptr request, std::shared_ptr response @@ -835,6 +873,7 @@ inline void ComponentInterface::add_service( response->message = ex.what(); } }); + this->string_services_.insert_or_assign(parsed_service_name, service); } catch (const std::exception& ex) { RCLCPP_ERROR_STREAM(this->get_logger(), "Failed to add service '" << service_name << "': " << ex.what()); } @@ -964,7 +1003,7 @@ inline std::string ComponentInterface::create_output( ) { using namespace modulo_core::communication; try { - auto parsed_signal_name = utilities::parse_signal_name(signal_name); + auto parsed_signal_name = utilities::parse_topic_name(signal_name); if (parsed_signal_name.empty()) { throw exceptions::AddSignalException( "The parsed signal name for signal '" + signal_name diff --git a/source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp b/source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp new file mode 100644 index 000000000..7f2a861a2 --- /dev/null +++ b/source/modulo_components/include/modulo_components/exceptions/AddServiceException.hpp @@ -0,0 +1,16 @@ +#pragma once + +#include "modulo_components/exceptions/ComponentException.hpp" + +namespace modulo_components::exceptions { + +/** + * @class AddServiceException + * @brief An exception class to notify errors when adding a service. + * @details This is an exception class to be thrown if there is a problem while adding a service to the component. + */ +class AddServiceException : public ComponentException { +public: + explicit AddServiceException(const std::string& msg) : ComponentException(msg) {}; +}; +}// namespace modulo_components::exceptions diff --git a/source/modulo_components/include/modulo_components/utilities/utilities.hpp b/source/modulo_components/include/modulo_components/utilities/utilities.hpp index dd313e3eb..272c2fd05 100644 --- a/source/modulo_components/include/modulo_components/utilities/utilities.hpp +++ b/source/modulo_components/include/modulo_components/utilities/utilities.hpp @@ -40,15 +40,15 @@ parse_node_name(const rclcpp::NodeOptions& options, const std::string& fallback } /** - * @brief Parse a string signal name from a user-provided input. + * @brief Parse a string topic name from a user-provided input. * @details This functions removes all characters different from * a-z, A-Z, 0-9, and _ from a string. - * @param signal_name The input string + * @param topic_name The input string * @return The sanitized string */ -[[maybe_unused]] static std::string parse_signal_name(const std::string& signal_name) { +[[maybe_unused]] static std::string parse_topic_name(const std::string& topic_name) { std::string output; - for (char c: signal_name) { + for (char c: topic_name) { if ((c >= 'a' && c <= 'z') || (c >= 'A' && c <= 'Z') || (c >= '0' && c <= '9') || c == '_') { if (!(c == '_' && output.empty())) { output.insert(output.end(), std::tolower(c)); diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index fde1d651b..902a38fbc 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -9,8 +9,8 @@ import state_representation as sr from geometry_msgs.msg import TransformStamped from modulo_component_interfaces.srv import EmptyTrigger, StringTrigger -from modulo_components.exceptions import AddSignalError, ComponentParameterError, LookupTransformError -from modulo_components.utilities import generate_predicate_topic, parse_signal_name +from modulo_components.exceptions import AddServiceError, AddSignalError, ComponentParameterError, LookupTransformError +from modulo_components.utilities import generate_predicate_topic, parse_topic_name from modulo_core import EncodedState from modulo_core.exceptions import MessageTranslationError, ParameterTranslationError from modulo_core.translators.parameter_translators import get_ros_parameter_type, read_parameter_const, write_parameter @@ -44,6 +44,7 @@ def __init__(self, node_name: str, *kargs, **kwargs): self._periodic_callbacks: Dict[str, Callable[[], None]] = {} self._inputs = {} self._outputs = {} + self._services = {} self.__tf_buffer: Optional[Buffer] = None self.__tf_listener: Optional[TransformListener] = None self.__tf_broadcaster: Optional[TransformBroadcaster] = None @@ -329,7 +330,7 @@ def _create_output(self, signal_name: str, data: str, message_type: MsgT, clprot try: if message_type == EncodedState and clproto_message_type == clproto.MessageType.UNKNOWN_MESSAGE: raise AddSignalError(f"Provide a valid clproto message type for outputs of type EncodedState.") - parsed_signal_name = parse_signal_name(signal_name) + parsed_signal_name = parse_topic_name(signal_name) if not parsed_signal_name: raise AddSignalError("The parsed signal name is empty. Provide a string with valid " "characters for the signal name ([a-zA-Z0-9_]).") @@ -385,7 +386,7 @@ def add_input(self, signal_name: str, subscription: Union[str, Callable], messag :param fixed_topic: If true, the topic name of the output signal is fixed """ try: - parsed_signal_name = parse_signal_name(signal_name) + parsed_signal_name = parse_topic_name(signal_name) if not parsed_signal_name: raise AddSignalError(f"Failed to add input '{signal_name}': Parsed signal name is empty.") if parsed_signal_name in self._inputs.keys(): @@ -459,7 +460,11 @@ def callback_wrapper(request, response, cb): return response try: - parsed_service_name = parse_signal_name(service_name) + parsed_service_name = parse_topic_name(service_name) + if not parsed_service_name: + raise AddServiceError(f"Failed to add service '{service_name}': Parsed service name is empty.") + if parsed_service_name in self._services.keys(): + raise AddServiceError(f"Failed to add service '{service_name}': Service already exists") signature = inspect.signature(callback) if len(signature.parameters) == 0: self.get_logger().debug(f"Adding empty service '{parsed_service_name}'") @@ -467,8 +472,9 @@ def callback_wrapper(request, response, cb): else: self.get_logger().debug(f"Adding string service '{parsed_service_name}'") service_type = StringTrigger - self.create_service(service_type, "~/" + parsed_service_name, - lambda request, response: callback_wrapper(request, response, callback)) + self._services[parsed_service_name] = \ + self.create_service(service_type, "~/" + parsed_service_name, + lambda request, response: callback_wrapper(request, response, callback)) except Exception as e: self.get_logger().error(f"Failed to add service '{service_name}': {e}") diff --git a/source/modulo_components/modulo_components/exceptions/__init__.py b/source/modulo_components/modulo_components/exceptions/__init__.py index 55c2bb1dc..93d58adec 100644 --- a/source/modulo_components/modulo_components/exceptions/__init__.py +++ b/source/modulo_components/modulo_components/exceptions/__init__.py @@ -1,2 +1,2 @@ -from modulo_components.exceptions.component_exceptions import AddSignalError, ComponentError, ComponentParameterError, \ - LookupTransformError +from modulo_components.exceptions.component_exceptions import AddServiceError, AddSignalError, ComponentError, \ + ComponentParameterError, LookupTransformError diff --git a/source/modulo_components/modulo_components/exceptions/component_exceptions.py b/source/modulo_components/modulo_components/exceptions/component_exceptions.py index 50fe21e9d..5aafd2f42 100644 --- a/source/modulo_components/modulo_components/exceptions/component_exceptions.py +++ b/source/modulo_components/modulo_components/exceptions/component_exceptions.py @@ -7,6 +7,16 @@ def __init__(self, message: str): super().__init__(message) +class AddServiceError(ComponentError): + """ + An exception class to notify errors when adding a service. This is an exception class to be thrown if there is a + problem while adding a service to the component. + """ + + def __init__(self, message: str): + super().__init__(message) + + class AddSignalError(ComponentError): """ An exception class to notify errors when adding a signal. This is an exception class to be thrown if there is a diff --git a/source/modulo_components/modulo_components/utilities/__init__.py b/source/modulo_components/modulo_components/utilities/__init__.py index 899b322a7..d661061c7 100644 --- a/source/modulo_components/modulo_components/utilities/__init__.py +++ b/source/modulo_components/modulo_components/utilities/__init__.py @@ -1 +1 @@ -from modulo_components.utilities.utilities import generate_predicate_topic, parse_signal_name +from modulo_components.utilities.utilities import generate_predicate_topic, parse_topic_name diff --git a/source/modulo_components/modulo_components/utilities/utilities.py b/source/modulo_components/modulo_components/utilities/utilities.py index f8ec4fa19..2e90ee541 100644 --- a/source/modulo_components/modulo_components/utilities/utilities.py +++ b/source/modulo_components/modulo_components/utilities/utilities.py @@ -12,14 +12,14 @@ def generate_predicate_topic(node_name: str, predicate_name: str) -> str: return f'/predicates/{node_name}/{predicate_name}' -def parse_signal_name(signal_name: str) -> str: +def parse_topic_name(topic_name: str) -> str: """ - Parse a string signal name from a user-provided input. + Parse a string topic name from a user-provided input. This functions removes all characters different from a-z, A-Z, 0-9, and _ from a string. - :param signal_name: The input string + :param topic_name: The input string :return: The sanitized string """ - sanitized_string = re.sub("\W", "", signal_name, flags=re.ASCII).lower() + sanitized_string = re.sub("\W", "", topic_name, flags=re.ASCII).lower() sanitized_string = sanitized_string.lstrip("_") return sanitized_string diff --git a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp index ab2748486..7622acc2e 100644 --- a/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp +++ b/source/modulo_components/test/cpp/include/test_modulo_components/component_public_interfaces.hpp @@ -32,6 +32,8 @@ class ComponentInterfacePublicInterface : public ComponentInterface { using ComponentInterface::inputs_; using ComponentInterface::create_output; using ComponentInterface::outputs_; + using ComponentInterface::empty_services_; + using ComponentInterface::string_services_; using ComponentInterface::add_tf_broadcaster; using ComponentInterface::add_tf_listener; using ComponentInterface::send_transform; diff --git a/source/modulo_components/test/cpp/test_component_interface.cpp b/source/modulo_components/test/cpp/test_component_interface.cpp index 6c8839ccf..b4a36c7c7 100644 --- a/source/modulo_components/test/cpp/test_component_interface.cpp +++ b/source/modulo_components/test/cpp/test_component_interface.cpp @@ -100,15 +100,45 @@ TYPED_TEST(ComponentInterfaceTest, AddInput) { } TYPED_TEST(ComponentInterfaceTest, AddService) { + EXPECT_EQ(static_cast(this->component_->empty_services_.size()), 0); + EXPECT_EQ(static_cast(this->component_->string_services_.size()), 0); + auto empty_callback = []() -> ComponentServiceResponse { return ComponentServiceResponse(); }; EXPECT_NO_THROW(this->component_->add_service("empty", empty_callback)); + EXPECT_EQ(static_cast(this->component_->empty_services_.size()), 1); + EXPECT_NE(this->component_->empty_services_.find("empty"), this->component_->empty_services_.cend()); auto string_callback = [](const std::string&) -> ComponentServiceResponse { return ComponentServiceResponse(); }; EXPECT_NO_THROW(this->component_->add_service("string", string_callback)); + EXPECT_EQ(static_cast(this->component_->string_services_.size()), 1); + EXPECT_NE(this->component_->string_services_.find("string"), this->component_->string_services_.cend()); + + // adding a service under an existing name should fail for either callback type but is exception safe + EXPECT_NO_THROW(this->component_->add_service("empty", empty_callback)); + EXPECT_NO_THROW(this->component_->add_service("empty", string_callback)); + EXPECT_EQ(static_cast(this->component_->empty_services_.size()), 1); + EXPECT_EQ(static_cast(this->component_->string_services_.size()), 1); + + EXPECT_NO_THROW(this->component_->add_service("string", empty_callback)); + EXPECT_NO_THROW(this->component_->add_service("string", string_callback)); + EXPECT_EQ(static_cast(this->component_->empty_services_.size()), 1); + EXPECT_EQ(static_cast(this->component_->string_services_.size()), 1); + + // adding an empty service name should fail + EXPECT_NO_THROW(this->component_->add_service("", empty_callback)); + EXPECT_NO_THROW(this->component_->add_service("", string_callback)); + EXPECT_EQ(static_cast(this->component_->empty_services_.size()), 1); + EXPECT_EQ(static_cast(this->component_->string_services_.size()), 1); + + + // adding a mangled service name should succeed under a sanitized name + EXPECT_NO_THROW(this->component_->add_service("_tEsT_#1@3", empty_callback)); + EXPECT_EQ(static_cast(this->component_->empty_services_.size()), 2); + EXPECT_NE(this->component_->empty_services_.find("test_13"), this->component_->empty_services_.cend()); // TODO: use a service client to trigger the service and test the behaviour } diff --git a/source/modulo_components/test/python/test_component_interface.py b/source/modulo_components/test/python/test_component_interface.py index 644e7d650..14b58b3f0 100644 --- a/source/modulo_components/test/python/test_component_interface.py +++ b/source/modulo_components/test/python/test_component_interface.py @@ -69,10 +69,28 @@ def test_add_service(component_interface): def empty_callback(): return {"success": True, "message": "test"} component_interface.add_service("empty", empty_callback) + assert len(component_interface._services) == 1 + assert "empty" in component_interface._services.keys() def string_callback(): return {"success": True, "message": "test"} + component_interface.add_service("string", string_callback) + assert len(component_interface._services) == 2 + assert "string" in component_interface._services.keys() + + # adding a service under an existing name should fail for either callback type, but is exception safe + component_interface.add_service("empty", empty_callback) component_interface.add_service("empty", string_callback) + assert len(component_interface._services) == 2 + + component_interface.add_service("string", empty_callback) + component_interface.add_service("string", string_callback) + assert len(component_interface._services) == 2 + + # adding a mangled service name should succeed under a sanitized name + component_interface.add_service("_tEsT_#1@3", empty_callback) + assert len(component_interface._services) == 3 + assert "test_13" in component_interface._services.keys() # TODO: use a service client to trigger the service and test the behaviour From 0dd3b7629875efa6fe4de25855892a441407111a Mon Sep 17 00:00:00 2001 From: Enrico Eberhard <32450951+eeberhard@users.noreply.github.com> Date: Tue, 2 Aug 2022 09:30:01 +0200 Subject: [PATCH 63/71] Generate documentation (#106) * Add doxygen configuration * Generate documentation * Add workflow to generate documentation with doxygen and push it to the docs branch under a versions subdirectory * Add index page to fetch and list all documented versions * Add README for docs branch main page * Ignore html directory in case of local generation --- .github/workflows/generate-docs.yml | 48 + .gitignore | 2 + doxygen/docs/README.md | 9 + doxygen/docs/index.html | 37 + doxygen/doxygen.conf | 2453 +++++++++++++++++++++++++++ 5 files changed, 2549 insertions(+) create mode 100644 .github/workflows/generate-docs.yml create mode 100644 doxygen/docs/README.md create mode 100644 doxygen/docs/index.html create mode 100644 doxygen/doxygen.conf diff --git a/.github/workflows/generate-docs.yml b/.github/workflows/generate-docs.yml new file mode 100644 index 000000000..96cdb983f --- /dev/null +++ b/.github/workflows/generate-docs.yml @@ -0,0 +1,48 @@ +name: Generate and Deploy Documentation +on: + push: + branches: + - main + - develop + release: + types: [published] + workflow_dispatch: + +jobs: + deploy: + name: Generate and Deploy + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@master + + - name: Generate docs + uses: mattnotmitt/doxygen-action@v1 + with: + working-directory: 'doxygen' + doxyfile-path: 'doxygen.conf' + + - name: Tag release version + if: ${{ github.event_name == 'release' }} + shell: bash + run: | + TAG="${GITHUB_REF#refs/tags/}" + TAG="${TAG/\//-}" + mkdir -p doxygen/docs/versions + sudo mv doxygen/docs/html doxygen/docs/versions/${TAG} + + - name: Tag branch version + if: ${{ github.event_name == 'push' || github.event_name == 'workflow_dispatch' }} + shell: bash + run: | + BRANCH="${GITHUB_REF#refs/heads/}" + BRANCH="${BRANCH/\//-}" + mkdir -p doxygen/docs/versions + sudo mv doxygen/docs/html doxygen/docs/versions/${BRANCH} + + - name: Deploy to documentation branch + uses: peaceiris/actions-gh-pages@v3 + with: + github_token: ${{ secrets.GITHUB_TOKEN }} + publish_branch: docs + publish_dir: ./doxygen/docs + keep_files: true diff --git a/.gitignore b/.gitignore index cc56490ff..cb2c31758 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,5 @@ .idea cmake-build-* + +doxygen/docs/html diff --git a/doxygen/docs/README.md b/doxygen/docs/README.md new file mode 100644 index 000000000..85273c1f8 --- /dev/null +++ b/doxygen/docs/README.md @@ -0,0 +1,9 @@ +# https://epfl-lasa.github.io/modulo + +This branch hosts html documentation auto-generated from the [Modulo](https://github.com/epfl-lasa/modulo) source code +using doxygen and hosted using GitHub Pages. + +The content of this branch is not intended to be viewed or edited directly. + +Instead, please refer to the following page to view the documentation: +https://epfl-lasa.github.io/modulo diff --git a/doxygen/docs/index.html b/doxygen/docs/index.html new file mode 100644 index 000000000..ba0edfc75 --- /dev/null +++ b/doxygen/docs/index.html @@ -0,0 +1,37 @@ + + + +Modulo Documentation + + +

Modulo

+

Documented branches and tags

+
    + + + diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf new file mode 100644 index 000000000..f09c19e6f --- /dev/null +++ b/doxygen/doxygen.conf @@ -0,0 +1,2453 @@ +# Doxyfile 1.8.14 + +# This file describes the settings to be used by the documentation system +# doxygen (www.doxygen.org) for a project. +# +# All text after a double hash (##) is considered a comment and is placed in +# front of the TAG it is preceding. +# +# All text after a single hash (#) is considered a comment and will be ignored. +# The format is: +# TAG = value [value, ...] +# For lists, items can also be appended using: +# TAG += value [value, ...] +# Values that contain spaces should be placed between quotes (\" \"). + +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- + +# This tag specifies the encoding used for all characters in the config file +# that follow. The default is UTF-8 which is also the encoding used for all text +# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv +# built into libc) for the transcoding. See +# https://www.gnu.org/software/libiconv/ for the list of possible encodings. +# The default value is: UTF-8. + +DOXYFILE_ENCODING = UTF-8 + +# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by +# double-quotes, unless you are using Doxywizard) that should identify the +# project for which the documentation is generated. This name is used in the +# title of most generated pages and in a few other places. +# The default value is: My Project. + +PROJECT_NAME = "Modulo" + +# The PROJECT_NUMBER tag can be used to enter a project or revision number. This +# could be handy for archiving the generated documentation or if some version +# control system is used. + +PROJECT_NUMBER = 2.0.0-rc001 + +# Using the PROJECT_BRIEF tag one can provide an optional one line description +# for a project that appears at the top of each page and should give viewer a +# quick idea about the purpose of the project. Keep the description short. + +PROJECT_BRIEF = + +# With the PROJECT_LOGO tag one can specify a logo or an icon that is included +# in the documentation. The maximum height of the logo should not exceed 55 +# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy +# the logo to the output directory. + +PROJECT_LOGO = + +# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path +# into which the generated documentation will be written. If a relative path is +# entered, it will be relative to the location where doxygen was started. If +# left blank the current directory will be used. + +OUTPUT_DIRECTORY = docs + +# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub- +# directories (in 2 levels) under the output directory of each output format and +# will distribute the generated files over these directories. Enabling this +# option can be useful when feeding doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise causes +# performance problems for the file system. +# The default value is: NO. + +CREATE_SUBDIRS = NO + +# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# characters to appear in the names of generated files. If set to NO, non-ASCII +# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode +# U+3044. +# The default value is: NO. + +ALLOW_UNICODE_NAMES = NO + +# The OUTPUT_LANGUAGE tag is used to specify the language in which all +# documentation generated by doxygen is written. Doxygen will use this +# information to generate all constant output in the proper language. +# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese, +# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States), +# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian, +# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages), +# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian, +# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian, +# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish, +# Ukrainian and Vietnamese. +# The default value is: English. + +OUTPUT_LANGUAGE = English + +# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# descriptions after the members that are listed in the file and class +# documentation (similar to Javadoc). Set to NO to disable this. +# The default value is: YES. + +BRIEF_MEMBER_DESC = YES + +# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# description of a member or function before the detailed description +# +# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the +# brief descriptions will be completely suppressed. +# The default value is: YES. + +REPEAT_BRIEF = YES + +# This tag implements a quasi-intelligent brief description abbreviator that is +# used to form the text in various listings. Each string in this list, if found +# as the leading text of the brief description, will be stripped from the text +# and the result, after processing the whole list, is used as the annotated +# text. Otherwise, the brief description is used as-is. If left blank, the +# following values are used ($name is automatically replaced with the name of +# the entity):The $name class, The $name widget, The $name file, is, provides, +# specifies, contains, represents, a, an and the. + +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the + +# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then +# doxygen will generate a detailed section even if there is only a brief +# description. +# The default value is: NO. + +ALWAYS_DETAILED_SEC = NO + +# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# inherited members of a class in the documentation of that class as if those +# members were ordinary class members. Constructors, destructors and assignment +# operators of the base classes will not be shown. +# The default value is: NO. + +INLINE_INHERITED_MEMB = NO + +# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# before files name in the file list and in the header files. If set to NO the +# shortest path that makes the file name unique will be used +# The default value is: YES. + +FULL_PATH_NAMES = YES + +# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. +# Stripping is only done if one of the specified strings matches the left-hand +# part of the path. The tag can be used to show relative paths in the file list. +# If left blank the directory from which doxygen is run is used as the path to +# strip. +# +# Note that you can specify absolute paths here, but also relative paths, which +# will be relative from the directory where doxygen is started. +# This tag requires that the tag FULL_PATH_NAMES is set to YES. + +STRIP_FROM_PATH = + +# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the +# path mentioned in the documentation of a class, which tells the reader which +# header file to include in order to use a class. If left blank only the name of +# the header file containing the class definition is used. Otherwise one should +# specify the list of include paths that are normally passed to the compiler +# using the -I flag. + +STRIP_FROM_INC_PATH = + +# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but +# less readable) file names. This can be useful is your file systems doesn't +# support long names like on DOS, Mac, or CD-ROM. +# The default value is: NO. + +SHORT_NAMES = NO + +# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the +# first line (until the first dot) of a Javadoc-style comment as the brief +# description. If set to NO, the Javadoc-style will behave just like regular Qt- +# style comments (thus requiring an explicit @brief command for a brief +# description.) +# The default value is: NO. + +JAVADOC_AUTOBRIEF = NO + +# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first +# line (until the first dot) of a Qt-style comment as the brief description. If +# set to NO, the Qt-style will behave just like regular Qt-style comments (thus +# requiring an explicit \brief command for a brief description.) +# The default value is: NO. + +QT_AUTOBRIEF = NO + +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# multi-line C++ special comment block (i.e. a block of //! or /// comments) as +# a brief description. This used to be the default behavior. The new default is +# to treat a multi-line C++ comment block as a detailed description. Set this +# tag to YES if you prefer the old behavior instead. +# +# Note that setting this tag to YES also means that rational rose comments are +# not recognized any more. +# The default value is: NO. + +MULTILINE_CPP_IS_BRIEF = NO + +# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the +# documentation from any documented member that it re-implements. +# The default value is: YES. + +INHERIT_DOCS = YES + +# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# page for each member. If set to NO, the documentation of a member will be part +# of the file/class/namespace that contains it. +# The default value is: NO. + +SEPARATE_MEMBER_PAGES = NO + +# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen +# uses this value to replace tabs by spaces in code fragments. +# Minimum value: 1, maximum value: 16, default value: 4. + +TAB_SIZE = 4 + +# This tag can be used to specify a number of aliases that act as commands in +# the documentation. An alias has the form: +# name=value +# For example adding +# "sideeffect=@par Side Effects:\n" +# will allow you to put the command \sideeffect (or @sideeffect) in the +# documentation, which will result in a user-defined paragraph with heading +# "Side Effects:". You can put \n's in the value part of an alias to insert +# newlines (in the resulting output). You can put ^^ in the value part of an +# alias to insert a newline as if a physical newline was in the original file. + +ALIASES = + +# This tag can be used to specify a number of word-keyword mappings (TCL only). +# A mapping has the form "name=value". For example adding "class=itcl::class" +# will allow you to use the command class in the itcl::class meaning. + +TCL_SUBST = + +# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources +# only. Doxygen will then generate output that is more tailored for C. For +# instance, some of the names that are used will be different. The list of all +# members will be omitted, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_FOR_C = NO + +# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or +# Python sources only. Doxygen will then generate output that is more tailored +# for that language. For instance, namespaces will be presented as packages, +# qualified scopes will look different, etc. +# The default value is: NO. + +OPTIMIZE_OUTPUT_JAVA = NO + +# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran +# sources. Doxygen will then generate output that is tailored for Fortran. +# The default value is: NO. + +OPTIMIZE_FOR_FORTRAN = NO + +# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL +# sources. Doxygen will then generate output that is tailored for VHDL. +# The default value is: NO. + +OPTIMIZE_OUTPUT_VHDL = NO + +# Doxygen selects the parser to use depending on the extension of the files it +# parses. With this tag you can assign which parser to use for a given +# extension. Doxygen has a built-in mapping, but you can override or extend it +# using this tag. The format is ext=language, where ext is a file extension, and +# language is one of the parsers supported by doxygen: IDL, Java, Javascript, +# C#, C, C++, D, PHP, Objective-C, Python, Fortran (fixed format Fortran: +# FortranFixed, free formatted Fortran: FortranFree, unknown formatted Fortran: +# Fortran. In the later case the parser tries to guess whether the code is fixed +# or free formatted code, this is the default for Fortran type files), VHDL. For +# instance to make doxygen treat .inc files as Fortran files (default is PHP), +# and .f files as C (default is Fortran), use: inc=Fortran f=C. +# +# Note: For files without extension you can use no_extension as a placeholder. +# +# Note that for custom extensions you also need to set FILE_PATTERNS otherwise +# the files are not read by doxygen. + +EXTENSION_MAPPING = + +# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# according to the Markdown format, which allows for more readable +# documentation. See http://daringfireball.net/projects/markdown/ for details. +# The output of markdown processing is further processed by doxygen, so you can +# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# case of backward compatibilities issues. +# The default value is: YES. + +MARKDOWN_SUPPORT = YES + +# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up +# to that level are automatically included in the table of contents, even if +# they do not have an id attribute. +# Note: This feature currently applies only to Markdown headings. +# Minimum value: 0, maximum value: 99, default value: 0. +# This tag requires that the tag MARKDOWN_SUPPORT is set to YES. + +TOC_INCLUDE_HEADINGS = 0 + +# When enabled doxygen tries to link words that correspond to documented +# classes, or namespaces to their corresponding documentation. Such a link can +# be prevented in individual cases by putting a % sign in front of the word or +# globally by setting AUTOLINK_SUPPORT to NO. +# The default value is: YES. + +AUTOLINK_SUPPORT = YES + +# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want +# to include (a tag file for) the STL sources as input, then you should set this +# tag to YES in order to let doxygen match functions declarations and +# definitions whose arguments contain STL classes (e.g. func(std::string); +# versus func(std::string) {}). This also make the inheritance and collaboration +# diagrams that involve STL classes more complete and accurate. +# The default value is: NO. + +BUILTIN_STL_SUPPORT = NO + +# If you use Microsoft's C++/CLI language, you should set this option to YES to +# enable parsing support. +# The default value is: NO. + +CPP_CLI_SUPPORT = NO + +# Set the SIP_SUPPORT tag to YES if your project consists of sip (see: +# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen +# will parse them like normal C++ but will assume all classes use public instead +# of private inheritance when no explicit protection keyword is present. +# The default value is: NO. + +SIP_SUPPORT = NO + +# For Microsoft's IDL there are propget and propput attributes to indicate +# getter and setter methods for a property. Setting this option to YES will make +# doxygen to replace the get and set methods by a property in the documentation. +# This will only work if the methods are indeed getting or setting a simple +# type. If this is not the case, or you want to show the methods anyway, you +# should set this option to NO. +# The default value is: YES. + +IDL_PROPERTY_SUPPORT = YES + +# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC +# tag is set to YES then doxygen will reuse the documentation of the first +# member in the group (if any) for the other members of the group. By default +# all members of a group must be documented explicitly. +# The default value is: NO. + +DISTRIBUTE_GROUP_DOC = NO + +# If one adds a struct or class to a group and this option is enabled, then also +# any nested class or struct is added to the same group. By default this option +# is disabled and one has to add nested compounds explicitly via \ingroup. +# The default value is: NO. + +GROUP_NESTED_COMPOUNDS = NO + +# Set the SUBGROUPING tag to YES to allow class member groups of the same type +# (for instance a group of public functions) to be put as a subgroup of that +# type (e.g. under the Public Functions section). Set it to NO to prevent +# subgrouping. Alternatively, this can be done per class using the +# \nosubgrouping command. +# The default value is: YES. + +SUBGROUPING = YES + +# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions +# are shown inside the group in which they are included (e.g. using \ingroup) +# instead of on a separate page (for HTML and Man pages) or section (for LaTeX +# and RTF). +# +# Note that this feature does not work in combination with +# SEPARATE_MEMBER_PAGES. +# The default value is: NO. + +INLINE_GROUPED_CLASSES = NO + +# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions +# with only public data fields or simple typedef fields will be shown inline in +# the documentation of the scope in which they are defined (i.e. file, +# namespace, or group documentation), provided this scope is documented. If set +# to NO, structs, classes, and unions are shown on a separate page (for HTML and +# Man pages) or section (for LaTeX and RTF). +# The default value is: NO. + +INLINE_SIMPLE_STRUCTS = NO + +# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or +# enum is documented as struct, union, or enum with the name of the typedef. So +# typedef struct TypeS {} TypeT, will appear in the documentation as a struct +# with name TypeT. When disabled the typedef will appear as a member of a file, +# namespace, or class. And the struct will be named TypeS. This can typically be +# useful for C code in case the coding convention dictates that all compound +# types are typedef'ed and only the typedef is referenced, never the tag name. +# The default value is: NO. + +TYPEDEF_HIDES_STRUCT = NO + +# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This +# cache is used to resolve symbols given their name and scope. Since this can be +# an expensive process and often the same symbol appears multiple times in the +# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# doxygen will become slower. If the cache is too large, memory is wasted. The +# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range +# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 +# symbols. At the end of a run doxygen will report the cache usage and suggest +# the optimal cache size from a speed point of view. +# Minimum value: 0, maximum value: 9, default value: 0. + +LOOKUP_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- + +# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# documentation are documented, even if no documentation was available. Private +# class members and static file members will be hidden unless the +# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. +# Note: This will also disable the warnings about undocumented members that are +# normally produced when WARNINGS is set to YES. +# The default value is: NO. + +EXTRACT_ALL = NO + +# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will +# be included in the documentation. +# The default value is: NO. + +EXTRACT_PRIVATE = NO + +# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal +# scope will be included in the documentation. +# The default value is: NO. + +EXTRACT_PACKAGE = NO + +# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be +# included in the documentation. +# The default value is: NO. + +EXTRACT_STATIC = NO + +# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined +# locally in source files will be included in the documentation. If set to NO, +# only classes defined in header files are included. Does not have any effect +# for Java sources. +# The default value is: YES. + +EXTRACT_LOCAL_CLASSES = NO + +# This flag is only useful for Objective-C code. If set to YES, local methods, +# which are defined in the implementation section but not in the interface are +# included in the documentation. If set to NO, only methods in the interface are +# included. +# The default value is: NO. + +EXTRACT_LOCAL_METHODS = NO + +# If this flag is set to YES, the members of anonymous namespaces will be +# extracted and appear in the documentation as a namespace called +# 'anonymous_namespace{file}', where file will be replaced with the base name of +# the file that contains the anonymous namespace. By default anonymous namespace +# are hidden. +# The default value is: NO. + +EXTRACT_ANON_NSPACES = NO + +# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# undocumented members inside documented classes or files. If set to NO these +# members will be included in the various overviews, but no documentation +# section is generated. This option has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_MEMBERS = NO + +# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# undocumented classes that are normally visible in the class hierarchy. If set +# to NO, these classes will be included in the various overviews. This option +# has no effect if EXTRACT_ALL is enabled. +# The default value is: NO. + +HIDE_UNDOC_CLASSES = NO + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# (class|struct|union) declarations. If set to NO, these declarations will be +# included in the documentation. +# The default value is: NO. + +HIDE_FRIEND_COMPOUNDS = NO + +# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# documentation blocks found inside the body of a function. If set to NO, these +# blocks will be appended to the function's detailed documentation block. +# The default value is: NO. + +HIDE_IN_BODY_DOCS = NO + +# The INTERNAL_DOCS tag determines if documentation that is typed after a +# \internal command is included. If the tag is set to NO then the documentation +# will be excluded. Set it to YES to include the internal documentation. +# The default value is: NO. + +INTERNAL_DOCS = NO + +# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file +# names in lower-case letters. If set to YES, upper-case letters are also +# allowed. This is useful if you have classes or files whose names only differ +# in case and if your file system supports case sensitive file names. Windows +# and Mac users are advised to set this option to NO. +# The default value is: system dependent. + +CASE_SENSE_NAMES = NO + +# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# their full class and namespace scopes in the documentation. If set to YES, the +# scope will be hidden. +# The default value is: NO. + +HIDE_SCOPE_NAMES = NO + +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# append additional text to a page's title, such as Class Reference. If set to +# YES the compound reference will be hidden. +# The default value is: NO. + +HIDE_COMPOUND_REFERENCE= NO + +# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# the files that are included by a file in the documentation of that file. +# The default value is: YES. + +SHOW_INCLUDE_FILES = YES + +# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each +# grouped member an include statement to the documentation, telling the reader +# which file to include in order to use the member. +# The default value is: NO. + +SHOW_GROUPED_MEMB_INC = NO + +# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# files with double quotes in the documentation rather than with sharp brackets. +# The default value is: NO. + +FORCE_LOCAL_INCLUDES = NO + +# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the +# documentation for inline members. +# The default value is: YES. + +INLINE_INFO = YES + +# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# (detailed) documentation of file and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. +# The default value is: YES. + +SORT_MEMBER_DOCS = YES + +# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# descriptions of file, namespace and class members alphabetically by member +# name. If set to NO, the members will appear in declaration order. Note that +# this will also influence the order of the classes in the class list. +# The default value is: NO. + +SORT_BRIEF_DOCS = NO + +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# (brief and detailed) documentation of class members so that constructors and +# destructors are listed first. If set to NO the constructors will appear in the +# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. +# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief +# member documentation. +# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting +# detailed member documentation. +# The default value is: NO. + +SORT_MEMBERS_CTORS_1ST = NO + +# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# of group names into alphabetical order. If set to NO the group names will +# appear in their defined order. +# The default value is: NO. + +SORT_GROUP_NAMES = NO + +# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by +# fully-qualified names, including namespaces. If set to NO, the class list will +# be sorted only by class name, not including the namespace part. +# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES. +# Note: This option applies only to the class list, not to the alphabetical +# list. +# The default value is: NO. + +SORT_BY_SCOPE_NAME = NO + +# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# type resolution of all parameters of a function it will reject a match between +# the prototype and the implementation of a member function even if there is +# only one candidate or it is obvious which candidate to choose by doing a +# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# accept a match between prototype and implementation in such cases. +# The default value is: NO. + +STRICT_PROTO_MATCHING = NO + +# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo +# list. This list is created by putting \todo commands in the documentation. +# The default value is: YES. + +GENERATE_TODOLIST = YES + +# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test +# list. This list is created by putting \test commands in the documentation. +# The default value is: YES. + +GENERATE_TESTLIST = YES + +# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug +# list. This list is created by putting \bug commands in the documentation. +# The default value is: YES. + +GENERATE_BUGLIST = YES + +# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO) +# the deprecated list. This list is created by putting \deprecated commands in +# the documentation. +# The default value is: YES. + +GENERATE_DEPRECATEDLIST= YES + +# The ENABLED_SECTIONS tag can be used to enable conditional documentation +# sections, marked by \if ... \endif and \cond +# ... \endcond blocks. + +ENABLED_SECTIONS = + +# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the +# initial value of a variable or macro / define can have for it to appear in the +# documentation. If the initializer consists of more lines than specified here +# it will be hidden. Use a value of 0 to hide initializers completely. The +# appearance of the value of individual variables and macros / defines can be +# controlled using \showinitializer or \hideinitializer command in the +# documentation regardless of this setting. +# Minimum value: 0, maximum value: 10000, default value: 30. + +MAX_INITIALIZER_LINES = 30 + +# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at +# the bottom of the documentation of classes and structs. If set to YES, the +# list will mention the files that were used to generate the documentation. +# The default value is: YES. + +SHOW_USED_FILES = YES + +# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This +# will remove the Files entry from the Quick Index and from the Folder Tree View +# (if specified). +# The default value is: YES. + +SHOW_FILES = YES + +# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces +# page. This will remove the Namespaces entry from the Quick Index and from the +# Folder Tree View (if specified). +# The default value is: YES. + +SHOW_NAMESPACES = YES + +# The FILE_VERSION_FILTER tag can be used to specify a program or script that +# doxygen should invoke to get the current version for each file (typically from +# the version control system). Doxygen will invoke the program by executing (via +# popen()) the command command input-file, where command is the value of the +# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided +# by doxygen. Whatever the program writes to standard output is used as the file +# version. For an example see the documentation. + +FILE_VERSION_FILTER = + +# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed +# by doxygen. The layout file controls the global structure of the generated +# output files in an output format independent way. To create the layout file +# that represents doxygen's defaults, run doxygen with the -l option. You can +# optionally specify a file name after the option, if omitted DoxygenLayout.xml +# will be used as the name of the layout file. +# +# Note that if you run doxygen from a directory containing a file called +# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# tag is left empty. + +LAYOUT_FILE = + +# The CITE_BIB_FILES tag can be used to specify one or more bib files containing +# the reference definitions. This must be a list of .bib files. The .bib +# extension is automatically appended if omitted. This requires the bibtex tool +# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info. +# For LaTeX the style of the bibliography can be controlled using +# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the +# search path. See also \cite for info how to create references. + +CITE_BIB_FILES = + +#--------------------------------------------------------------------------- +# Configuration options related to warning and progress messages +#--------------------------------------------------------------------------- + +# The QUIET tag can be used to turn on/off the messages that are generated to +# standard output by doxygen. If QUIET is set to YES this implies that the +# messages are off. +# The default value is: NO. + +QUIET = NO + +# The WARNINGS tag can be used to turn on/off the warning messages that are +# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# this implies that the warnings are on. +# +# Tip: Turn warnings on while writing the documentation. +# The default value is: YES. + +WARNINGS = YES + +# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag +# will automatically be disabled. +# The default value is: YES. + +WARN_IF_UNDOCUMENTED = YES + +# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# potential errors in the documentation, such as not documenting some parameters +# in a documented function, or documenting parameters that don't exist or using +# markup commands wrongly. +# The default value is: YES. + +WARN_IF_DOC_ERROR = YES + +# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that +# are documented, but have no documentation for their parameters or return +# value. If set to NO, doxygen will only warn about wrong or incomplete +# parameter documentation, but not about the absence of documentation. +# The default value is: NO. + +WARN_NO_PARAMDOC = NO + +# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# a warning is encountered. +# The default value is: NO. + +WARN_AS_ERROR = NO + +# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# can produce. The string should contain the $file, $line, and $text tags, which +# will be replaced by the file and line number from which the warning originated +# and the warning text. Optionally the format may contain $version, which will +# be replaced by the version of the file (if it could be obtained via +# FILE_VERSION_FILTER) +# The default value is: $file:$line: $text. + +WARN_FORMAT = "$file:$line: $text" + +# The WARN_LOGFILE tag can be used to specify a file to which warning and error +# messages should be written. If left blank the output is written to standard +# error (stderr). + +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# Configuration options related to the input files +#--------------------------------------------------------------------------- + +# The INPUT tag is used to specify the files and/or directories that contain +# documented source files. You may enter file names like myfile.cpp or +# directories like /usr/src/myproject. Separate the files or directories with +# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING +# Note: If this tag is empty the current directory is searched. + +INPUT = ../README.md ../source + +# This tag can be used to specify the character encoding of the source files +# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# libiconv (or the iconv built into libc) for the transcoding. See the libiconv +# documentation (see: https://www.gnu.org/software/libiconv/) for the list of +# possible encodings. +# The default value is: UTF-8. + +INPUT_ENCODING = UTF-8 + +# If the value of the INPUT tag contains directories, you can use the +# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and +# *.h) to filter out the source-files in the directories. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# read by doxygen. +# +# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp, +# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, +# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, +# *.m, *.markdown, *.md, *.mm, *.dox, *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, +# *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf and *.qsf. + +FILE_PATTERNS = *.c \ + *.cc \ + *.cxx \ + *.cpp \ + *.c++ \ + *.h \ + *.hh \ + *.hxx \ + *.hpp \ + *.h++ \ + *.proto \ + *.m \ + *.markdown \ + *.md + +# The RECURSIVE tag can be used to specify whether or not subdirectories should +# be searched for input files as well. +# The default value is: NO. + +RECURSIVE = YES + +# The EXCLUDE tag can be used to specify files and/or directories that should be +# excluded from the INPUT source files. This way you can easily exclude a +# subdirectory from a directory tree whose root is specified with the INPUT tag. +# +# Note that relative paths are relative to the directory from which doxygen is +# run. + +EXCLUDE = + +# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or +# directories that are symbolic links (a Unix file system feature) are excluded +# from the input. +# The default value is: NO. + +EXCLUDE_SYMLINKS = NO + +# If the value of the INPUT tag contains directories, you can use the +# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude +# certain files from those directories. +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories for example use the pattern */test/* + +EXCLUDE_PATTERNS = */tests/* */test/* + +# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names +# (namespaces, classes, functions, etc.) that should be excluded from the +# output. The symbol name can be a fully qualified name, a word, or if the +# wildcard * is used, a substring. Examples: ANamespace, AClass, +# AClass::ANamespace, ANamespace::*Test +# +# Note that the wildcards are matched against the file with absolute path, so to +# exclude all test directories use the pattern */test/* + +EXCLUDE_SYMBOLS = + +# The EXAMPLE_PATH tag can be used to specify one or more files or directories +# that contain example code fragments that are included (see the \include +# command). + +# TODO +# EXAMPLE_PATH = + +# If the value of the EXAMPLE_PATH tag contains directories, you can use the +# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and +# *.h) to filter out the source-files in the directories. If left blank all +# files are included. + +EXAMPLE_PATTERNS = * + +# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be +# searched for input files to be used with the \include or \dontinclude commands +# irrespective of the value of the RECURSIVE tag. +# The default value is: NO. + +EXAMPLE_RECURSIVE = NO + +# The IMAGE_PATH tag can be used to specify one or more files or directories +# that contain images that are to be included in the documentation (see the +# \image command). + +IMAGE_PATH = + +# The INPUT_FILTER tag can be used to specify a program that doxygen should +# invoke to filter for each input file. Doxygen will invoke the filter program +# by executing (via popen()) the command: +# +# +# +# where is the value of the INPUT_FILTER tag, and is the +# name of an input file. Doxygen will then use the output that the filter +# program writes to standard output. If FILTER_PATTERNS is specified, this tag +# will be ignored. +# +# Note that the filter must not add or remove lines; it is applied before the +# code is scanned, but not when the output code is generated. If lines are added +# or removed, the anchors will not be placed correctly. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +INPUT_FILTER = + +# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern +# basis. Doxygen will compare the file name with each pattern and apply the +# filter if there is a match. The filters are a list of the form: pattern=filter +# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how +# filters are used. If the FILTER_PATTERNS tag is empty or if none of the +# patterns match the file name, INPUT_FILTER is applied. +# +# Note that for custom extensions or not directly supported extensions you also +# need to set EXTENSION_MAPPING for the extension otherwise the files are not +# properly processed by doxygen. + +FILTER_PATTERNS = + +# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using +# INPUT_FILTER) will also be used to filter the input files that are used for +# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES). +# The default value is: NO. + +FILTER_SOURCE_FILES = NO + +# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file +# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and +# it is also possible to disable source filtering for a specific pattern using +# *.ext= (so without naming a filter). +# This tag requires that the tag FILTER_SOURCE_FILES is set to YES. + +FILTER_SOURCE_PATTERNS = + +# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that +# is part of the input, its contents will be placed on the main page +# (index.html). This can be useful if you have a project on for instance GitHub +# and want to reuse the introduction page also for the doxygen output. + +USE_MDFILE_AS_MAINPAGE = ../README.md + +#--------------------------------------------------------------------------- +# Configuration options related to source browsing +#--------------------------------------------------------------------------- + +# If the SOURCE_BROWSER tag is set to YES then a list of source files will be +# generated. Documented entities will be cross-referenced with these sources. +# +# Note: To get rid of all source code in the generated output, make sure that +# also VERBATIM_HEADERS is set to NO. +# The default value is: NO. + +SOURCE_BROWSER = YES + +# Setting the INLINE_SOURCES tag to YES will include the body of functions, +# classes and enums directly into the documentation. +# The default value is: NO. + +INLINE_SOURCES = NO + +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# special comment blocks from generated source code fragments. Normal C, C++ and +# Fortran comments will always remain visible. +# The default value is: YES. + +STRIP_CODE_COMMENTS = YES + +# If the REFERENCED_BY_RELATION tag is set to YES then for each documented +# function all documented functions referencing it will be listed. +# The default value is: NO. + +REFERENCED_BY_RELATION = NO + +# If the REFERENCES_RELATION tag is set to YES then for each documented function +# all documented entities called/used by that function will be listed. +# The default value is: NO. + +REFERENCES_RELATION = NO + +# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set +# to YES then the hyperlinks from functions in REFERENCES_RELATION and +# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will +# link to the documentation. +# The default value is: YES. + +REFERENCES_LINK_SOURCE = YES + +# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the +# source code will show a tooltip with additional information such as prototype, +# brief description and links to the definition and documentation. Since this +# will make the HTML file larger and loading of large files a bit slower, you +# can opt to disable this feature. +# The default value is: YES. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +SOURCE_TOOLTIPS = YES + +# If the USE_HTAGS tag is set to YES then the references to source code will +# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# source browser. The htags tool is part of GNU's global source tagging system +# (see https://www.gnu.org/software/global/global.html). You will need version +# 4.8.6 or higher. +# +# To use it do the following: +# - Install the latest version of global +# - Enable SOURCE_BROWSER and USE_HTAGS in the config file +# - Make sure the INPUT points to the root of the source tree +# - Run doxygen as normal +# +# Doxygen will invoke htags (and that will in turn invoke gtags), so these +# tools must be available from the command line (i.e. in the search path). +# +# The result: instead of the source browser generated by doxygen, the links to +# source code will now point to the output of htags. +# The default value is: NO. +# This tag requires that the tag SOURCE_BROWSER is set to YES. + +USE_HTAGS = NO + +# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# verbatim copy of the header file for each class for which an include is +# specified. Set to NO to disable this. +# See also: Section \class. +# The default value is: YES. + +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# Configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- + +# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all +# compounds will be generated. Enable this if the project contains a lot of +# classes, structs, unions or interfaces. +# The default value is: YES. + +ALPHABETICAL_INDEX = YES + +# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in +# which the alphabetical index list will be split. +# Minimum value: 1, maximum value: 20, default value: 5. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +COLS_IN_ALPHA_INDEX = 5 + +# In case all classes in a project start with a common prefix, all classes will +# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag +# can be used to specify a prefix (or a list of prefixes) that should be ignored +# while generating the index headers. +# This tag requires that the tag ALPHABETICAL_INDEX is set to YES. + +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the HTML output +#--------------------------------------------------------------------------- + +# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# The default value is: YES. + +GENERATE_HTML = YES + +# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a +# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of +# it. +# The default directory is: html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_OUTPUT = + +# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each +# generated HTML page (for example: .htm, .php, .asp). +# The default value is: .html. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FILE_EXTENSION = .html + +# The HTML_HEADER tag can be used to specify a user-defined HTML header file for +# each generated HTML page. If the tag is left blank doxygen will generate a +# standard header. +# +# To get valid HTML the header file that includes any scripts and style sheets +# that doxygen needs, which is dependent on the configuration options used (e.g. +# the setting GENERATE_TREEVIEW). It is highly recommended to start with a +# default header using +# doxygen -w html new_header.html new_footer.html new_stylesheet.css +# YourConfigFile +# and then modify the file new_header.html. See also section "Doxygen usage" +# for information on how to generate the default header that doxygen normally +# uses. +# Note: The header is subject to change so you typically have to regenerate the +# default header when upgrading to a newer version of doxygen. For a description +# of the possible markers and block names see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_HEADER = + +# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each +# generated HTML page. If the tag is left blank doxygen will generate a standard +# footer. See HTML_HEADER for more information on how to generate a default +# footer and what special commands can be used inside the footer. See also +# section "Doxygen usage" for information on how to generate the default footer +# that doxygen normally uses. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_FOOTER = + +# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style +# sheet that is used by each HTML page. It can be used to fine-tune the look of +# the HTML output. If left blank doxygen will generate a default style sheet. +# See also section "Doxygen usage" for information on how to generate the style +# sheet that doxygen normally uses. +# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as +# it is more robust and this tag (HTML_STYLESHEET) will in the future become +# obsolete. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_STYLESHEET = + +# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined +# cascading style sheets that are included after the standard style sheets +# created by doxygen. Using this option one can overrule certain style aspects. +# This is preferred over using HTML_STYLESHEET since it does not replace the +# standard style sheet and is therefore more robust against future updates. +# Doxygen will copy the style sheet files to the output directory. +# Note: The order of the extra style sheet files is of importance (e.g. the last +# style sheet in the list overrules the setting of the previous ones in the +# list). For an example see the documentation. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_STYLESHEET = + +# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or +# other source files which should be copied to the HTML output directory. Note +# that these files will be copied to the base HTML output directory. Use the +# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these +# files. In the HTML_STYLESHEET file, use the file name only. Also note that the +# files will be copied as-is; there are no commands or markers available. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_EXTRA_FILES = + +# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen +# will adjust the colors in the style sheet and background images according to +# this color. Hue is specified as an angle on a colorwheel, see +# https://en.wikipedia.org/wiki/Hue for more information. For instance the value +# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300 +# purple, and 360 is red again. +# Minimum value: 0, maximum value: 359, default value: 220. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_HUE = 220 + +# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors +# in the HTML output. For a value of 0 the output will use grayscales only. A +# value of 255 will produce the most vivid colors. +# Minimum value: 0, maximum value: 255, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_SAT = 100 + +# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the +# luminance component of the colors in the HTML output. Values below 100 +# gradually make the output lighter, whereas values above 100 make the output +# darker. The value divided by 100 is the actual gamma applied, so 80 represents +# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not +# change the gamma. +# Minimum value: 40, maximum value: 240, default value: 80. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_COLORSTYLE_GAMMA = 80 + +# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML +# page will contain the date and time when the page was generated. Setting this +# to YES can help to show when doxygen was last run and thus if the +# documentation is up to date. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_TIMESTAMP = NO + +# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML +# documentation will contain a main index with vertical navigation menus that +# are dynamically created via Javascript. If disabled, the navigation index will +# consists of multiple levels of tabs that are statically embedded in every HTML +# page. Disable this option to support browsers that do not have Javascript, +# like the Qt help browser. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_MENUS = YES + +# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML +# documentation will contain sections that can be hidden and shown after the +# page has loaded. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_DYNAMIC_SECTIONS = NO + +# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries +# shown in the various tree structured indices initially; the user can expand +# and collapse entries dynamically later on. Doxygen will expand the tree to +# such a level that at most the specified number of entries are visible (unless +# a fully collapsed tree already exceeds this amount). So setting the number of +# entries 1 will produce a full collapsed tree by default. 0 is a special value +# representing an infinite number of entries and will result in a full expanded +# tree by default. +# Minimum value: 0, maximum value: 9999, default value: 100. +# This tag requires that the tag GENERATE_HTML is set to YES. + +HTML_INDEX_NUM_ENTRIES = 100 + +# If the GENERATE_DOCSET tag is set to YES, additional index files will be +# generated that can be used as input for Apple's Xcode 3 integrated development +# environment (see: https://developer.apple.com/tools/xcode/), introduced with +# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a +# Makefile in the HTML output directory. Running make will produce the docset in +# that directory and running make install will install the docset in +# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at +# startup. See https://developer.apple.com/tools/creatingdocsetswithdoxygen.html +# for more information. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_DOCSET = NO + +# This tag determines the name of the docset feed. A documentation feed provides +# an umbrella under which multiple documentation sets from a single provider +# (such as a company or product suite) can be grouped. +# The default value is: Doxygen generated docs. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_FEEDNAME = "Doxygen generated docs" + +# This tag specifies a string that should uniquely identify the documentation +# set bundle. This should be a reverse domain-name style string, e.g. +# com.mycompany.MyDocSet. Doxygen will append .docset to the name. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_BUNDLE_ID = org.doxygen.Project + +# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify +# the documentation publisher. This should be a reverse domain-name style +# string, e.g. com.mycompany.MyDocSet.documentation. +# The default value is: org.doxygen.Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_ID = org.doxygen.Publisher + +# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher. +# The default value is: Publisher. +# This tag requires that the tag GENERATE_DOCSET is set to YES. + +DOCSET_PUBLISHER_NAME = Publisher + +# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# additional HTML index files: index.hhp, index.hhc, and index.hhk. The +# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop +# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on +# Windows. +# +# The HTML Help Workshop contains a compiler that can convert all HTML output +# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# files are now used as the Windows 98 help format, and will replace the old +# Windows help format (.hlp) on all Windows platforms in the future. Compressed +# HTML files also contain an index, a table of contents, and you can search for +# words in the documentation. The HTML workshop also contains a viewer for +# compressed HTML files. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_HTMLHELP = NO + +# The CHM_FILE tag can be used to specify the file name of the resulting .chm +# file. You can add a path in front of the file if the result should not be +# written to the html output directory. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_FILE = + +# The HHC_LOCATION tag can be used to specify the location (absolute path +# including file name) of the HTML help compiler (hhc.exe). If non-empty, +# doxygen will try to run the HTML help compiler on the generated index.hhp. +# The file has to be specified with full path. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +HHC_LOCATION = + +# The GENERATE_CHI flag controls if a separate .chi index file is generated +# (YES) or that it should be included in the master .chm file (NO). +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +GENERATE_CHI = NO + +# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc) +# and project file content. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +CHM_INDEX_ENCODING = + +# The BINARY_TOC flag controls whether a binary table of contents is generated +# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it +# enables the Previous and Next buttons. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +BINARY_TOC = NO + +# The TOC_EXPAND flag can be set to YES to add extra items for group members to +# the table of contents of the HTML help documentation and to the tree view. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTMLHELP is set to YES. + +TOC_EXPAND = NO + +# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and +# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that +# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help +# (.qch) of the generated HTML documentation. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_QHP = NO + +# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify +# the file name of the resulting .qch file. The path specified is relative to +# the HTML output folder. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QCH_FILE = + +# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help +# Project output. For more information please see Qt Help Project / Namespace +# (see: http://doc.qt.io/qt-4.8/qthelpproject.html#namespace). +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_NAMESPACE = org.doxygen.Project + +# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt +# Help Project output. For more information please see Qt Help Project / Virtual +# Folders (see: http://doc.qt.io/qt-4.8/qthelpproject.html#virtual-folders). +# The default value is: doc. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_VIRTUAL_FOLDER = doc + +# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom +# filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://doc.qt.io/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_NAME = + +# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the +# custom filter to add. For more information please see Qt Help Project / Custom +# Filters (see: http://doc.qt.io/qt-4.8/qthelpproject.html#custom-filters). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_CUST_FILTER_ATTRS = + +# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this +# project's filter section matches. Qt Help Project / Filter Attributes (see: +# http://doc.qt.io/qt-4.8/qthelpproject.html#filter-attributes). +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHP_SECT_FILTER_ATTRS = + +# The QHG_LOCATION tag can be used to specify the location of Qt's +# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the +# generated .qhp file. +# This tag requires that the tag GENERATE_QHP is set to YES. + +QHG_LOCATION = + +# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be +# generated, together with the HTML files, they form an Eclipse help plugin. To +# install this plugin and make it available under the help contents menu in +# Eclipse, the contents of the directory containing the HTML and XML files needs +# to be copied into the plugins directory of eclipse. The name of the directory +# within the plugins directory should be the same as the ECLIPSE_DOC_ID value. +# After copying Eclipse needs to be restarted before the help appears. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_ECLIPSEHELP = NO + +# A unique identifier for the Eclipse help plugin. When installing the plugin +# the directory name containing the HTML and XML files should also have this +# name. Each documentation set should have its own identifier. +# The default value is: org.doxygen.Project. +# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES. + +ECLIPSE_DOC_ID = org.doxygen.Project + +# If you want full control over the layout of the generated HTML pages it might +# be necessary to disable the index and replace it with your own. The +# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top +# of each HTML page. A value of NO enables the index and the value YES disables +# it. Since the tabs in the index contain the same information as the navigation +# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +DISABLE_INDEX = NO + +# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index +# structure should be generated to display hierarchical information. If the tag +# value is set to YES, a side panel will be generated containing a tree-like +# index structure (just like the one that is generated for HTML Help). For this +# to work a browser that supports JavaScript, DHTML, CSS and frames is required +# (i.e. any modern browser). Windows users are probably better off using the +# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can +# further fine-tune the look of the index. As an example, the default style +# sheet generated by doxygen has an example that shows how to put an image at +# the root of the tree instead of the PROJECT_NAME. Since the tree basically has +# the same information as the tab index, you could consider setting +# DISABLE_INDEX to YES when enabling this option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +GENERATE_TREEVIEW = NO + +# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that +# doxygen will group on one line in the generated HTML documentation. +# +# Note that a value of 0 will completely suppress the enum values from appearing +# in the overview section. +# Minimum value: 0, maximum value: 20, default value: 4. +# This tag requires that the tag GENERATE_HTML is set to YES. + +ENUM_VALUES_PER_LINE = 4 + +# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used +# to set the initial width (in pixels) of the frame in which the tree is shown. +# Minimum value: 0, maximum value: 1500, default value: 250. +# This tag requires that the tag GENERATE_HTML is set to YES. + +TREEVIEW_WIDTH = 250 + +# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# external symbols imported via tag files in a separate window. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +EXT_LINKS_IN_WINDOW = NO + +# Use this tag to change the font size of LaTeX formulas included as images in +# the HTML documentation. When you change the font size after a successful +# doxygen run you need to manually remove any form_*.png images from the HTML +# output directory to force them to be regenerated. +# Minimum value: 8, maximum value: 50, default value: 10. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_FONTSIZE = 10 + +# Use the FORMULA_TRANSPARENT tag to determine whether or not the images +# generated for formulas are transparent PNGs. Transparent PNGs are not +# supported properly for IE 6.0, but are supported on all modern browsers. +# +# Note that when changing this option you need to delete any form_*.png files in +# the HTML output directory before the changes have effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +FORMULA_TRANSPARENT = YES + +# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see +# https://www.mathjax.org) which uses client side Javascript for the rendering +# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX +# installed or if you want to formulas look prettier in the HTML output. When +# enabled you may also need to install MathJax separately and configure the path +# to it using the MATHJAX_RELPATH option. +# The default value is: NO. +# This tag requires that the tag GENERATE_HTML is set to YES. + +USE_MATHJAX = NO + +# When MathJax is enabled you can set the default output format to be used for +# the MathJax output. See the MathJax site (see: +# http://docs.mathjax.org/en/latest/output.html) for more details. +# Possible values are: HTML-CSS (which is slower, but has the best +# compatibility), NativeMML (i.e. MathML) and SVG. +# The default value is: HTML-CSS. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_FORMAT = HTML-CSS + +# When MathJax is enabled you need to specify the location relative to the HTML +# output directory using the MATHJAX_RELPATH option. The destination directory +# should contain the MathJax.js script. For instance, if the mathjax directory +# is located at the same level as the HTML output directory, then +# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax +# Content Delivery Network so you can quickly see the result without installing +# MathJax. However, it is strongly recommended to install a local copy of +# MathJax from https://www.mathjax.org before deployment. +# The default value is: https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.2/. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_RELPATH = https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.2/ + +# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax +# extension names that should be enabled during MathJax rendering. For example +# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_EXTENSIONS = + +# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# of code that will be used on startup of the MathJax code. See the MathJax site +# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an +# example see the documentation. +# This tag requires that the tag USE_MATHJAX is set to YES. + +MATHJAX_CODEFILE = + +# When the SEARCHENGINE tag is enabled doxygen will generate a search box for +# the HTML output. The underlying search engine uses javascript and DHTML and +# should work on any modern browser. Note that when using HTML help +# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) +# there is already a search function so this one should typically be disabled. +# For large projects the javascript based search engine can be slow, then +# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to +# search using the keyboard; to jump to the search box use + S +# (what the is depends on the OS and browser, but it is typically +# , /