diff --git a/.dockerignore b/.dockerignore deleted file mode 100644 index 829765a8d..000000000 --- a/.dockerignore +++ /dev/null @@ -1,5 +0,0 @@ -.git -.gitmodules -*.md -LICENSE -*.sh diff --git a/.github/workflows/build-push.yml b/.github/workflows/build-push.yml index 978a92b97..879671fb2 100644 --- a/.github/workflows/build-push.yml +++ b/.github/workflows/build-push.yml @@ -4,7 +4,7 @@ name: Build and Push on: push: branches: - - master + - main - develop release: types: [published] @@ -21,8 +21,7 @@ jobs: - name: Build image run: | - docker build . --file ./Dockerfile.development --tag dev-image - docker build . --file ./Dockerfile.production --build-arg BASE_IMAGE=dev-image --tag modulo + docker build --tag modulo . - name: Login to GitHub Container Registry run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u "${{ github.actor }}" --password-stdin 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/Dockerfile b/Dockerfile new file mode 100644 index 000000000..6c99f2dd9 --- /dev/null +++ b/Dockerfile @@ -0,0 +1,16 @@ +ARG ROS_VERSION=galactic +FROM ghcr.io/aica-technology/ros2-control-libraries:${ROS_VERSION} as development + +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 development as production + +# build modulo +RUN /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build" diff --git a/Dockerfile.development b/Dockerfile.development deleted file mode 100644 index 0b10dd134..000000000 --- a/Dockerfile.development +++ /dev/null @@ -1,7 +0,0 @@ -ARG ROS_VERSION=foxy -FROM ghcr.io/aica-technology/ros2-control-libraries:${ROS_VERSION} - -WORKDIR ${HOME}/ros2_ws -# copy sources and build ROS workspace with user permissions -WORKDIR ${HOME}/ros2_ws/ -COPY --chown=${USER} ./source/ ./src/ diff --git a/Dockerfile.production b/Dockerfile.production deleted file mode 100644 index 95e3e8836..000000000 --- a/Dockerfile.production +++ /dev/null @@ -1,6 +0,0 @@ -ARG ROS_VERSION=foxy -ARG BASE_IMAGE=epfl-lasa/modulo/development:${ROS_VERSION} -FROM ${BASE_IMAGE} as remote-development - -# build modulo -RUN su ${USER} -c /bin/bash -c "source /opt/ros/$ROS_DISTRO/setup.bash; colcon build" diff --git a/build-server.sh b/build-server.sh new file mode 100755 index 000000000..e84f75012 --- /dev/null +++ b/build-server.sh @@ -0,0 +1,44 @@ +#!/bin/bash +ROS_VERSION=galactic + +IMAGE_NAME=epfl-lasa/modulo +IMAGE_TAG=latest + +REMOTE_SSH_PORT=4440 +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 + + -r, --rebuild Rebuild the image(s) using the docker + --no-cache option + + -v, --verbose Use the verbose option during the building + process + + -s, --serve Start the remove development server +" + +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 ;; + -r|--rebuild) BUILD_FLAGS+=(--no-cache) ; shift ;; + -v|--verbose) BUILD_FLAGS+=(--progress=plain) ; shift ;; + -s|--serve) SERVE_REMOTE=true ; shift ;; + -h|--help) echo "${HELP_MESSAGE}" ; exit 0 ;; + *) echo 'Error in command line parsing' >&2 + echo -e "\n${HELP_MESSAGE}" + exit 1 + esac +done + +docker pull ghcr.io/aica-technology/ros2-control-libraries:"${ROS_VERSION}" +DOCKER_BUILDKIT=1 docker build -t "${IMAGE_NAME}:${IMAGE_TAG}" "${BUILD_FLAGS[@]}" . || exit 1 + +if [ "${SERVE_REMOTE}" = true ]; then + aica-docker server "${IMAGE_NAME}:${IMAGE_TAG}" -u ros2 -p "${REMOTE_SSH_PORT}" +fi \ No newline at end of file diff --git a/build.sh b/build.sh deleted file mode 100755 index 8e727034d..000000000 --- a/build.sh +++ /dev/null @@ -1,44 +0,0 @@ -#!/bin/bash -ROS_VERSION=foxy - -BUILD_PROD=false -IMAGE_NAME=epfl-lasa/modulo - -HELP_MESSAGE="Usage: build.sh [-p] [-r] - -Options: - -p, --production Build the production ready image on top of - of the development one. - - -r, --rebuild Rebuild the image(s) using the docker - --no-cache option -" - -PARAM_BUILD_FLAGS=() -while [[ $# -gt 0 ]]; do - opt="$1" - case $opt in - -p|--production) BUILD_PROD=true ; shift ;; - -r|--rebuild) PARAM_BUILD_FLAGS+=(--no-cache) ; shift ;; - -h|--help) echo "${HELP_MESSAGE}" ; exit 0 ;; - *) echo 'Error in command line parsing' >&2 - echo -e "\n${HELP_MESSAGE}" - exit 1 - esac -done - -BUILD_FLAGS=() -BUILD_FLAGS+=(--build-arg ROS_VERSION="${ROS_VERSION}") -BUILD_FLAGS+=(-t "${IMAGE_NAME}/development":"${ROS_VERSION}") -BUILD_FLAGS+=("${PARAM_BUILD_FLAGS[@]}") - -docker pull ghcr.io/aica-technology/ros2-control-libraries:"${ROS_VERSION}" -DOCKER_BUILDKIT=1 docker build --file ./Dockerfile.development "${BUILD_FLAGS[@]}" . - -if [ $BUILD_PROD = true ]; then - BUILD_FLAGS=() - BUILD_FLAGS+=(--build-arg ROS_VERSION="${ROS_VERSION}") - BUILD_FLAGS+=(-t "${IMAGE_NAME}":"${ROS_VERSION}") - BUILD_FLAGS+=("${PARAM_BUILD_FLAGS[@]}") - DOCKER_BUILDKIT=1 docker build --file ./Dockerfile.production "${BUILD_FLAGS[@]}" . -fi diff --git a/source/modulo_core/CMakeLists.txt b/source/modulo_core/CMakeLists.txt index f823084f2..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) @@ -65,17 +65,20 @@ target_link_libraries(${PROJECT_NAME} robot_model ) -ament_auto_add_executable(${PROJECT_NAME}_test_cartesian tests/testCartesian.cpp) -target_link_libraries(${PROJECT_NAME}_test_cartesian ${PROJECT_NAME}) +ament_auto_add_executable(example_cartesian_linear examples/cartesianLinear.cpp) +target_link_libraries(example_cartesian_linear ${PROJECT_NAME}) -ament_auto_add_executable(${PROJECT_NAME}_test_cartesian_circular tests/testCartesianCircular.cpp) -target_link_libraries(${PROJECT_NAME}_test_cartesian_circular ${PROJECT_NAME}) +ament_auto_add_executable(example_cartesian_circular examples/cartesianCircular.cpp) +target_link_libraries(example_cartesian_circular ${PROJECT_NAME}) -ament_auto_add_executable(${PROJECT_NAME}_test_joints tests/testJoints.cpp) -target_link_libraries(${PROJECT_NAME}_test_joints ${PROJECT_NAME}) +ament_auto_add_executable(example_helicoidal examples/helicoidal.cpp) +target_link_libraries(example_helicoidal ${PROJECT_NAME}) -ament_auto_add_executable(${PROJECT_NAME}_test_moving_reference_frame tests/testMovingReferenceFrame.cpp) -target_link_libraries(${PROJECT_NAME}_test_moving_reference_frame ${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}) ament_auto_package() @@ -83,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/README.md b/source/modulo_core/README.md index 39e627a3f..2351d5107 100644 --- a/source/modulo_core/README.md +++ b/source/modulo_core/README.md @@ -57,4 +57,4 @@ public: 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 recommand to keep this step function short, especially if you use a small period. Otherwise, latency will occur in your control loop. Complete examples of control loop including multiple nodes are written in the [tests](./tests/) for each representation spaces. +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. diff --git a/source/modulo_core/tests/testCartesianCircular.cpp b/source/modulo_core/examples/cartesianCircular.cpp similarity index 55% rename from source/modulo_core/tests/testCartesianCircular.cpp rename to source/modulo_core/examples/cartesianCircular.cpp index 86b041a5b..a0e4dccbb 100644 --- a/source/modulo_core/tests/testCartesianCircular.cpp +++ b/source/modulo_core/examples/cartesianCircular.cpp @@ -1,24 +1,34 @@ -#include #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; - dynamical_systems::Circular motion_generator; + 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(state_representation::CartesianPose("robot_test", 0., 0., 0.)) { - this->add_parameters(this->motion_generator.get_parameters()); + 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( + DynamicalSystemFactory::create_dynamical_system( + DynamicalSystemFactory::DYNAMICAL_SYSTEM::CIRCULAR + )) { + motion_generator->set_parameter_value("limit_cycle", Ellipsoid("attractor")); } bool on_configure() { @@ -29,7 +39,7 @@ class MotionGenerator : public modulo::core::Cell { void step() { if (!this->current_pose->is_empty()) { - *this->desired_twist = this->motion_generator.evaluate(*this->current_pose); + *this->desired_twist = this->motion_generator->evaluate(*this->current_pose); } else { this->desired_twist->initialize(); } @@ -38,13 +48,14 @@ class MotionGenerator : public modulo::core::Cell { class ConsoleVisualizer : public modulo::core::Cell { private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; + 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")) {} + 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); @@ -65,15 +76,18 @@ class ConsoleVisualizer : public modulo::core::Cell { class SimulatedRobotInterface : public modulo::core::Cell { private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; + 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) {} + 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); diff --git a/source/modulo_core/tests/testCartesian.cpp b/source/modulo_core/examples/cartesianLinear.cpp similarity index 51% rename from source/modulo_core/tests/testCartesian.cpp rename to source/modulo_core/examples/cartesianLinear.cpp index 262134171..2050c866a 100644 --- a/source/modulo_core/tests/testCartesian.cpp +++ b/source/modulo_core/examples/cartesianLinear.cpp @@ -1,25 +1,35 @@ -#include #include #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; - dynamical_systems::Linear motion_generator; + 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(state_representation::CartesianPose::Random("robot_test"), 1.0) { - this->add_parameters(this->motion_generator.get_parameters()); + 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( + DynamicalSystemFactory::create_dynamical_system( + DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR + )) { + this->motion_generator->set_parameter_value("attractor", CartesianPose::Random("robot_test")); } bool on_configure() { @@ -30,27 +40,28 @@ class LinearMotionGenerator : public modulo::core::Cell { void step() { if (!this->current_pose->is_empty()) { - *this->desired_twist = this->motion_generator.evaluate(*this->current_pose); + *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_attractor()) < 1e-3) { - this->set_parameter_value("attractor", state_representation::CartesianPose::Random("robot_test")); + 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_attractor(), "attractor"); + 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; + 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")) {} + 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); @@ -71,19 +82,22 @@ class ConsoleVisualizer : public modulo::core::Component { class SimulatedRobotInterface : public modulo::core::Cell { private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; + 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) {} + 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, 0); + this->add_state_publisher("/robot_test/pose", this->robot_pose); return true; } diff --git a/source/modulo_core/examples/helicoidal.cpp b/source/modulo_core/examples/helicoidal.cpp new file mode 100644 index 000000000..601c3d604 --- /dev/null +++ b/source/modulo_core/examples/helicoidal.cpp @@ -0,0 +1,156 @@ +#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( + DynamicalSystemFactory::create_dynamical_system( + DynamicalSystemFactory::DYNAMICAL_SYSTEM::RING + )), + linear_motion_generator( + DynamicalSystemFactory::create_dynamical_system( + DynamicalSystemFactory::DYNAMICAL_SYSTEM::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/tests/testJoints.cpp b/source/modulo_core/examples/joints.cpp similarity index 55% rename from source/modulo_core/tests/testJoints.cpp rename to source/modulo_core/examples/joints.cpp index 3ad9873a3..809af7f15 100644 --- a/source/modulo_core/tests/testJoints.cpp +++ b/source/modulo_core/examples/joints.cpp @@ -1,22 +1,31 @@ -#include #include -#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; - dynamical_systems::Linear motion_generator; + 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(state_representation::JointPositions("robot", Eigen::VectorXd::Random(6))) { - this->add_parameters(this->motion_generator.get_parameters()); + 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( + DynamicalSystemFactory::create_dynamical_system( + DynamicalSystemFactory::DYNAMICAL_SYSTEM::POINT_ATTRACTOR + )) { + motion_generator->set_parameter_value("attractor", JointState::Random("robot", 6)); } bool on_configure() { @@ -27,7 +36,7 @@ class LinearMotionGenerator : public modulo::core::Cell { void step() { if (!this->current_positions->is_empty()) { - *this->desired_velocities = this->motion_generator.evaluate(*this->current_positions); + *this->desired_velocities = this->motion_generator->evaluate(*this->current_positions); } else { this->desired_velocities->initialize(); } @@ -36,13 +45,14 @@ class LinearMotionGenerator : public modulo::core::Cell { class ConsoleVisualizer : public modulo::core::Cell { private: - std::shared_ptr robot_positions; - std::shared_ptr desired_velocities; + 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)) {} + 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); @@ -62,20 +72,21 @@ class ConsoleVisualizer : public modulo::core::Cell { class SimulatedRobotInterface : public modulo::core::Cell { private: - std::shared_ptr robot_state; - std::shared_ptr desired_velocities; + 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) {} + 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, 0); + this->add_publisher("/robot/joint_state", this->robot_state); return true; } diff --git a/source/modulo_core/tests/testMovingReferenceFrame.cpp b/source/modulo_core/examples/movingReferenceFrame.cpp similarity index 50% rename from source/modulo_core/tests/testMovingReferenceFrame.cpp rename to source/modulo_core/examples/movingReferenceFrame.cpp index 27e8cf3c7..afaf4dd2f 100644 --- a/source/modulo_core/tests/testMovingReferenceFrame.cpp +++ b/source/modulo_core/examples/movingReferenceFrame.cpp @@ -1,30 +1,36 @@ -#include -#include #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; - dynamical_systems::Circular motion_generator; + 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(state_representation::CartesianPose("robot_test_attractor", 0., 0., 0., "object")) { - this->add_parameters(this->motion_generator.get_parameters()); + 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( + DynamicalSystemFactory::create_dynamical_system( + DynamicalSystemFactory::DYNAMICAL_SYSTEM::CIRCULAR + )) { + motion_generator->set_parameter_value("limit_cycle", Ellipsoid("attractor", "object")); } bool on_configure() { @@ -37,25 +43,26 @@ class MotionGenerator : public modulo::core::Cell { void step() { if (!this->current_pose->is_empty()) { - *this->desired_twist = this->motion_generator.evaluate(*this->current_pose); + *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); + 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; + 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")) {} + 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); @@ -76,20 +83,22 @@ class ConsoleVisualizer : public modulo::core::Cell { class SimulatedObject : public modulo::core::Cell { private: - std::shared_ptr object_pose; - std::shared_ptr object_twist; - dynamical_systems::Linear motion_generator; + 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(state_representation::CartesianPose::Identity("object"))), - object_twist(std::make_shared(state_representation::CartesianTwist("object"))), - motion_generator(state_representation::CartesianPose("object_attractor", 2., 0., 0.), 1.0), - dt(period), - sign(-1) { - this->add_parameters(this->motion_generator.get_parameters()); + 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) { + motion_generator->set_parameter_value("attractor", CartesianPose("object_attractor", 2., 0., 0.)); } bool on_configure() { @@ -100,14 +109,15 @@ class SimulatedObject : public modulo::core::Cell { void step() { // compute the dynamics of the object - *this->object_twist = this->motion_generator.evaluate(*this->object_pose); + *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_attractor()) < 1e-3) { - this->set_parameter_value("attractor", state_representation::CartesianPose("object_attractor", sign * 2., 0., 0.)); + 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"); @@ -116,15 +126,18 @@ class SimulatedObject : public modulo::core::Cell { class SimulatedRobotInterface : public modulo::core::Cell { private: - std::shared_ptr robot_pose; - std::shared_ptr desired_twist; + 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) {} + 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); diff --git a/source/modulo_core/include/modulo_core/Component.hpp b/source/modulo_core/include/modulo_core/Component.hpp index 7814d7bed..8844a5c4d 100644 --- a/source/modulo_core/include/modulo_core/Component.hpp +++ b/source/modulo_core/include/modulo_core/Component.hpp @@ -131,21 +131,11 @@ class Component : public core::Cell { */ explicit Component(const rclcpp::NodeOptions& options); - /** - * @brief Destructor - */ - ~Component(); - /** * @brief Get the list of predicates of the action * @return the list of predicates */ const std::list> get_predicates() const; - - /** - * @brief Function computing one step of calculation. It is called periodically in the run function. - */ - virtual void step() override = 0; }; template diff --git a/source/modulo_core/include/modulo_core/communication/EncodedState.hpp b/source/modulo_core/include/modulo_core/communication/EncodedState.hpp index c049b6b77..8f44f9b6f 100644 --- a/source/modulo_core/include/modulo_core/communication/EncodedState.hpp +++ b/source/modulo_core/include/modulo_core/communication/EncodedState.hpp @@ -1,7 +1,7 @@ #pragma once -#include +#include namespace modulo::core { - typedef std_msgs::msg::ByteMultiArray EncodedState; + 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_core/include/modulo_core/communication/message_passing/ReadStateConversion.hpp index 8046f1d5e..213ed245d 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/ReadStateConversion.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/ReadStateConversion.hpp @@ -1,17 +1,24 @@ #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 @@ -19,12 +26,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include #include "modulo_core/communication/EncodedState.hpp" diff --git a/source/modulo_core/include/modulo_core/communication/message_passing/WriteStateConversion.hpp b/source/modulo_core/include/modulo_core/communication/message_passing/WriteStateConversion.hpp index 05226bd0b..4856efc25 100644 --- a/source/modulo_core/include/modulo_core/communication/message_passing/WriteStateConversion.hpp +++ b/source/modulo_core/include/modulo_core/communication/message_passing/WriteStateConversion.hpp @@ -1,19 +1,27 @@ #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 @@ -21,13 +29,6 @@ #include #include #include -#include -#include -#include -#include -#include -#include -#include #include "modulo_core/communication/EncodedState.hpp" diff --git a/source/modulo_core/launch/test_control_loop.sh b/source/modulo_core/launch/example_control_loop.sh similarity index 100% rename from source/modulo_core/launch/test_control_loop.sh rename to source/modulo_core/launch/example_control_loop.sh diff --git a/source/modulo_core/launch/test_moving_reference_frame.sh b/source/modulo_core/launch/example_moving_reference_frame.sh similarity index 100% rename from source/modulo_core/launch/test_moving_reference_frame.sh rename to source/modulo_core/launch/example_moving_reference_frame.sh diff --git a/source/modulo_core/launch/test.launch.py b/source/modulo_core/launch/test.launch.py deleted file mode 100644 index fbaa26823..000000000 --- a/source/modulo_core/launch/test.launch.py +++ /dev/null @@ -1,119 +0,0 @@ -#!/usr/bin/python -import sys - -import launch -import launch.actions -import launch.substitutions -import launch.events - -from launch_ros import get_default_launch_description -import launch_ros.actions -import launch_ros.events -import launch_ros.events.lifecycle - -import lifecycle_msgs.msg - -def generate_launch_description(): - """Main.""" - ld = launch.LaunchDescription() - - # Prepare the visualization node. - visualizer_node = launch_ros.actions.LifecycleNode( - node_name='visualizer', - package='modulo_core', - node_executable="modulo_core_test_cartesian", - output='screen') - - # Prepare the robot node. - robot_interface_node = launch_ros.actions.LifecycleNode( - node_name='robot_interface', - package='modulo_core', - node_executable="modulo_core_test_cartesian", - output='screen') - - # Prepare the motion generator node. - motion_generator_node = launch_ros.actions.LifecycleNode( - node_name='motion_generator', - package='modulo_core', - node_executable="modulo_core_test_cartesian", - output='screen') - - - # Make the visualization reach the configure - emit_event_to_request_visualization_configure = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action(visualizer_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - - # Make the robot_interface reach the configure - emit_event_to_request_robot_interface_configure = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action(robot_interface_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - - # Make the motion_generator reach the configure - emit_event_to_request_motion_generator_configure = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action(motion_generator_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_CONFIGURE, - ) - ) - - # Make the visualization reach the activate - emit_event_to_request_visualization_activate = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action(visualizer_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - ) - ) - - # Make the robot_interface reach the configure - emit_event_to_request_robot_interface_activate = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action(robot_interface_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - ) - ) - - # Make the motion_generator reach the configure - emit_event_to_request_motion_generator_activate = launch.actions.EmitEvent( - event=launch_ros.events.lifecycle.ChangeState( - lifecycle_node_matcher=launch.events.matches_action(motion_generator_node), - transition_id=lifecycle_msgs.msg.Transition.TRANSITION_ACTIVATE, - ) - ) - - # Add the actions to the launch description. - # The order they are added reflects the order in which they will be executed. - ld.add_action(visualizer_node) - ld.add_action(robot_interface_node) - ld.add_action(motion_generator_node) - ld.add_action(emit_event_to_request_visualization_configure) - ld.add_action(emit_event_to_request_robot_interface_configure) - ld.add_action(emit_event_to_request_motion_generator_configure) - ld.add_action(emit_event_to_request_visualization_activate) - ld.add_action(emit_event_to_request_robot_interface_activate) - ld.add_action(emit_event_to_request_motion_generator_activate) - - # print('Starting introspection of launch description...') - # print('') - - # print(launch.LaunchIntrospector().format_launch_description(ld)) - - # print('') - # print('Starting launch of launch description...') - # print('') - - # # ls = launch.LaunchService(argv=argv, debug=True) - # ls = launch.LaunchService(argv=argv) - # ls.include_launch_description(get_default_launch_description()) - # ls.include_launch_description(ld) - return ld - - -# if __name__ == '__main__': -# main() \ 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 diff --git a/source/modulo_core/src/Cell.cpp b/source/modulo_core/src/Cell.cpp index 3db32df32..1a4ece412 100644 --- a/source/modulo_core/src/Cell.cpp +++ b/source/modulo_core/src/Cell.cpp @@ -1,12 +1,16 @@ #include "modulo_core/Cell.hpp" -#include -#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) : @@ -20,7 +24,10 @@ Cell::Cell(const rclcpp::NodeOptions& options) : } Cell::~Cell() { - this->on_shutdown(); + 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() { @@ -30,22 +37,42 @@ void Cell::reset() { for (auto it = this->handlers_.cbegin(); it != this->handlers_.cend(); /* no increment */) { if (!it->second.second) { it = this->handlers_.erase(it); - } - else { + } else { ++it; } } this->timers_.clear(); } -template +template void Cell::add_parameter(const std::shared_ptr>& parameter, const std::string& prefix) { - std::string tprefix = (prefix != "") ? 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); @@ -58,62 +85,92 @@ template void Cell::add_parameter(const std::shared_ptr>(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 != "") ? 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 != "") ? 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)); 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 != "") ? 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 != "") ? 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)); 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 != "") ? 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 != "") ? 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)); - std::vector value = std::vector(parameter->get_value().data(), parameter->get_value().data() + parameter->get_value().size()); + 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) { +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_type()) { + case StateType::PARAMETER_INT: { + this->add_parameter(std::static_pointer_cast>(param), prefix); + break; + } + + case StateType::PARAMETER_INT_ARRAY: { + this->add_parameter(std::static_pointer_cast>>(param), prefix); + break; + } + case StateType::PARAMETER_DOUBLE: { this->add_parameter(std::static_pointer_cast>(param), prefix); break; @@ -181,11 +238,15 @@ void Cell::add_parameters(const std::list +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); @@ -200,37 +261,37 @@ template void Cell::set_parameter_value(const std::string& parameter_name, const template void Cell::set_parameter_value(const std::string& parameter_name, const std::vector& value); -template <> +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 <> +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 <> +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 <> +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 <> +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 <> +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); @@ -240,6 +301,16 @@ void Cell::set_parameter_value(const std::shared_ptrget_type()) { + case StateType::PARAMETER_INT: { + this->set_parameter_value(std::static_pointer_cast>(parameter)); + break; + } + + case StateType::PARAMETER_INT_ARRAY: { + this->set_parameter_value(std::static_pointer_cast>>(parameter)); + break; + } + case StateType::PARAMETER_DOUBLE: { this->set_parameter_value(std::static_pointer_cast>(parameter)); break; @@ -306,51 +377,63 @@ void Cell::set_parameter_value(const std::shared_ptr& recipient, - bool always_active, - int 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::add_transform_broadcaster(const state_representation::CartesianPose& recipient, - bool always_active, - int 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) { +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"); + 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 != "") transform_copy.set_name(name); - std::static_pointer_cast(this->handlers_.at("tf_broadcaster").first)->send_transform(transform_copy); + 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); + 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); } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cell::on_configure(const rclcpp_lifecycle::State&) { +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 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + 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 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool Cell::on_configure() { @@ -358,19 +441,19 @@ bool Cell::on_configure() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cell::on_activate(const rclcpp_lifecycle::State&) { +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 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return LifecycleNodeInterface::CallbackReturn::FAILURE; } // set all handlers to activated this->active_ = true; for (auto& h : this->handlers_) { h.second.first->activate(); } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool Cell::on_activate() { @@ -378,12 +461,12 @@ bool Cell::on_activate() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cell::on_deactivate(const rclcpp_lifecycle::State&) { +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 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return LifecycleNodeInterface::CallbackReturn::FAILURE; } // set all handlers to not activated except for the one always active this->active_ = false; @@ -392,7 +475,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cell:: h.second.first->deactivate(); } } - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool Cell::on_deactivate() { @@ -400,16 +483,16 @@ bool Cell::on_deactivate() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cell::on_cleanup(const rclcpp_lifecycle::State&) { +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 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return LifecycleNodeInterface::CallbackReturn::FAILURE; } // reset all handlers this->reset(); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool Cell::on_cleanup() { @@ -417,12 +500,34 @@ bool Cell::on_cleanup() { return true; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cell::on_shutdown(const rclcpp_lifecycle::State& state) { +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 rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + return LifecycleNodeInterface::CallbackReturn::FAILURE; } // reset all handlers for clean shutdown this->reset(); @@ -430,7 +535,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Cell:: this->daemons_.clear(); this->parameters_.clear(); this->shutdown_ = true; - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + return LifecycleNodeInterface::CallbackReturn::SUCCESS; } bool Cell::on_shutdown() { @@ -469,6 +574,19 @@ void Cell::update_parameters() { try { for (auto& [key, param] : this->parameters_) { switch (param->get_type()) { + case StateType::PARAMETER_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: { + 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: { double value = this->get_parameter(param->get_name()).as_double(); std::static_pointer_cast>(param)->set_value(value); @@ -554,7 +672,10 @@ void Cell::update_parameters() { 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"); + 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; diff --git a/source/modulo_core/src/Component.cpp b/source/modulo_core/src/Component.cpp index a69230cdb..02acff68b 100644 --- a/source/modulo_core/src/Component.cpp +++ b/source/modulo_core/src/Component.cpp @@ -17,10 +17,6 @@ Component::Component(const rclcpp::NodeOptions& options) : Cell(options) { this->on_init(); } -Component::~Component() { - this->on_shutdown(); -} - void Component::evaluate_predicate_functions() { for (auto const& [key, val] : this->predicate_functions_) { this->set_predicate_value(key, (val)());