diff --git a/.gitmodules b/.gitmodules deleted file mode 100644 index 522ae73..0000000 --- a/.gitmodules +++ /dev/null @@ -1,4 +0,0 @@ -[submodule "pybind11"] - path = pybind11 - url = https://github.com/pybind/pybind11.git - branch = v3.0 diff --git a/CMakeLists.txt b/CMakeLists.txt index e39e99e..27e92ca 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,61 +1,64 @@ -cmake_minimum_required(VERSION 3.11) +cmake_minimum_required(VERSION 3.16) project(sas_core) -option(ROS2_BUILD "Enable ROS2/ament build (examples + ament integration)" ON) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# ---- Core libraries (always built) ---- -include(cmake/cpplib.cmake) - -if(ROS2_BUILD) - find_package(ament_cmake REQUIRED) - ament_python_install_package(${PROJECT_NAME}) - set(_SAS_PYTHON_INSTALL_DIR "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}") -endif() - -include(cmake/pythonlib.cmake) - -# ---- ROS2 / ament integration (conditional) ---- -if(ROS2_BUILD) - # INTERFACE alias for downstream ROS packages - add_library(sas_core INTERFACE) - target_link_libraries(sas_core INTERFACE sas_core_pure) - - ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) - ament_export_dependencies(Eigen3) - - install(DIRECTORY include/ DESTINATION include) - - install(TARGETS sas_core sas_core_pure - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib - INCLUDES DESTINATION include - ) - - # Example executables - foreach(_name IN ITEMS sas_core_example sas_clock_example - sas_clock_sched_fifo_example) - add_executable(${_name} src/examples/${_name}.cpp) - target_link_libraries(${_name} sas_core_pure) - if(_name STREQUAL "sas_core_example") - target_link_libraries(${_name} -ldqrobotics) - endif() - install(TARGETS ${_name} DESTINATION lib/${PROJECT_NAME}) - endforeach() - - add_executable(sas_robot_driver_example - src/examples/sas_robot_driver_example_main.cpp) - target_link_libraries(sas_robot_driver_example sas_core_pure) - install(TARGETS sas_robot_driver_example DESTINATION lib/${PROJECT_NAME}) - - install(DIRECTORY - scripts/ - FILE_PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ - DESTINATION lib/${PROJECT_NAME} - ) - - ament_package() -endif() +# ============================================================================= +# sas_core is a THIN WRAPPER around the SmartArmStack core: +# +# - C++ library: libmarinholab_sas_core, provided by the +# libmarinholab-sas-core .deb (source: https://github.com/MarinhoLab/sas_cpp). +# The .deb installs headers under include/marinholab/sas/core/, the shared +# library, and the CMake package config (find_package(marinholab_sas_core)). +# - Python bindings: the PyPI package marinholab-sas-core +# (https://github.com/MarinhoLab/sas_py), imported as marinholab.sas.core. +# +# This package adds no implementation; it only: +# 1. re-exports the ament target `sas_core` so downstream ROS packages keep +# working with `ament_target_dependencies( sas_core ...)`; +# 2. installs compatibility headers include/sas_core/*.hpp that forward to +# the .deb's include/marinholab/sas/core/*.hpp and re-alias `namespace sas`; +# 3. installs the pure-Python `sas_core` shim (re-exporting +# marinholab.sas.core) so `from sas_core import Clock` keeps working. +# ============================================================================= + +find_package(marinholab_sas_core REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +# Eigen3 is a PUBLIC dependency of the core (its headers include ); +# declare it so downstream ament packages can resolve it through this package. +find_package(Eigen3 REQUIRED) + +# ---- ament target --------------------------------------------------------- +# INTERFACE alias forwarding downstream packages to the core target installed +# by the libmarinholab-sas-core .deb. +add_library(sas_core INTERFACE) +target_link_libraries(sas_core INTERFACE marinholab::sas::core Eigen3::Eigen) + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) +# Eigen3 and marinholab_sas_core are consumed transitively by downstream +# packages that link the `sas_core` target; propagate them so their +# find_package() calls run in the consumer. +ament_export_dependencies(Eigen3 marinholab_sas_core) + +# ---- Compatibility headers ------------------------------------------------ +# include/sas_core/*.hpp are thin shims forwarding to the .deb's +# include/marinholab/sas/core/*.hpp and re-aliasing `namespace sas`. +install(DIRECTORY include/ DESTINATION include) + +install(TARGETS sas_core + EXPORT export_${PROJECT_NAME} +) + +# ---- Python compatibility shim --------------------------------------------- +# Pure-Python module re-exporting marinholab.sas.core (installed from PyPI by +# the docker environment / user). See sas_core/__init__.py. +ament_python_install_package(${PROJECT_NAME}) + +# Example scripts (run against the PyPI-installed bindings). +install( + DIRECTORY scripts/ + FILE_PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() diff --git a/README.md b/README.md index e549058..a562fe9 100644 --- a/README.md +++ b/README.md @@ -1,59 +1,81 @@ -# sas_core +# sas_core (thin wrapper) > [!TIP] > Repository for this module: https://github.com/SmartArmStack/sas_core.
> More information about SmartArmStack is available in https://smartarmstack.github.io/. -## Contents - -- `include/sas_core/` — public C++ headers. -- `src/` — implementation of the shared library and pybind11 bindings. -- `scripts/` — example Python scripts. -- `src/examples/` — C++ example programs and test nodes. +`sas_core` is a **thin ROS 2 wrapper** around the SmartArmStack core. It +contains no C++ implementation and no Python extension module of its own; it +forwards to the two packages that provide the core: -## Using as a non-ROS2 dependency (CMake FetchContent) +| Piece | Provided by | Installed as | +|---|---|---| +| C++ library (`libmarinholab_sas_core`) + headers + CMake config | [MarinhoLab/sas_cpp](https://github.com/MarinhoLab/sas_cpp) | `libmarinholab-sas-core` `.deb` (target namespace `marinholab::sas::core`) | +| Python bindings | [MarinhoLab/sas_py](https://github.com/MarinhoLab/sas_py) | `marinholab-sas-core` on PyPI (import `marinholab.sas.core`) | -To include `sas_core_pure` in a plain CMake project (no ROS2/ament required): +On Ubuntu the `.deb` links the dynamic `libdqrobotics` from the +[dqrobotics PPA](https://launchpad.net/~dqrobotics-dev/+archive/ubuntu/development); +in the provided docker environment both are already installed. -```cmake -include(FetchContent) -FetchContent_Declare( - sas_core - GIT_REPOSITORY https://github.com/SmartArmStack/sas_core.git - GIT_TAG jazzy -) +## What this package provides -set(ROS2_BUILD OFF CACHE BOOL "" FORCE) -FetchContent_MakeAvailable(sas_core) - -target_link_libraries(your_target PRIVATE sas_core_pure) -``` +- **C++**: the ament target `sas_core` (`ament_target_dependencies( sas_core ...)`) + forwarding to `marinholab::sas::core`, plus **compatibility headers** + `include/sas_core/*.hpp` that keep the legacy include paths + (`#include `) and the legacy `namespace sas` working + via `#include ` + a namespace alias. +- **Python**: a pure-Python `sas_core` shim that re-exports + `Clock`, `Statistics`, `RobotDriver`, `ShutdownSignaler` from + `marinholab.sas.core`, so `from sas_core import Clock` keeps working. -The library depends on **Eigen3** and **dqrobotics**; make sure both are -available on your system. +## Contents -## Examples +- `include/sas_core/` — compatibility C++ headers (one-line forwards). +- `sas_core/__init__.py` — Python compatibility shim. +- `scripts/` — example Python scripts + `sas_core_smoke_test.py`. +- `docker/` — build environment and integration smoke test. -Testing on a docker container. +## Installation ```bash -docker run --rm murilomarinho/sas:jazzy bash -c "ros2 run sas_core sas_clock_example" +# C++ core (until the .deb is published to an apt repository, build it from source): +sudo add-apt-repository ppa:dqrobotics-dev/development +sudo apt-get update +sudo apt-get install -y libdqrobotics +# then build sas_cpp with dpkg-buildpackage and dpkg -i the result, +# or simply: sudo apt-get install libmarinholab-sas-core (once published) + +# Python bindings: +python3 -m pip install marinholab-sas-core + +# This wrapper (inside a ROS 2 workspace): +colcon build ``` -```bash -ros2 run sas_core sas_core_example -ros2 run sas_core sas_clock_example -ros2 run sas_core sas_clock_sched_fifo_example -ros2 run sas_core sas_robot_driver_example -``` +## Examples + +The C++ example programs live with the C++ core +([MarinhoLab/sas_cpp](https://github.com/MarinhoLab/sas_cpp), built with +`-DMARINHO_LAB_SAS_CORE_BUILD_EXAMPLES=ON`). The Python examples in +`scripts/` run against the PyPI-installed bindings: ```bash ros2 run sas_core sas_clock_example_py.py -ros2 run sas_core sas_robot_driver_subclass_example_py.py ros2 run sas_core sas_clock_sched_fifo_example_py.py +ros2 run sas_core sas_robot_driver_subclass_example_py.py ``` -The `scripts/sas_robot_driver_subclass_example_py.py` file demonstrates how to -subclass `sas_core.RobotDriver` in Python and contains a minimal working -example. +`sas_robot_driver_subclass_example_py.py` demonstrates subclassing +`sas_core.RobotDriver` in Python. +## Testing + +The docker environment builds the wrapper with `colcon` and runs a smoke test +covering both consumption paths (Python shim + C++ compatibility headers +against the installed library): + +```bash +cd docker +docker compose build +docker compose up +``` diff --git a/cmake/cpplib.cmake b/cmake/cpplib.cmake deleted file mode 100644 index c3e0646..0000000 --- a/cmake/cpplib.cmake +++ /dev/null @@ -1,25 +0,0 @@ -# Build the pure C++ library (no ROS2/ament dependencies) -find_package(Eigen3 REQUIRED) - -add_library(sas_core_pure STATIC) -target_sources(sas_core_pure PRIVATE - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_clock.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_core.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_object.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_shutdown_signaler.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_robot_driver.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/examples/sas_robot_driver_example.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/eigen3_std_conversions.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_thread_manager.cpp -) - -set_target_properties(sas_core_pure PROPERTIES - POSITION_INDEPENDENT_CODE ON -) - -target_include_directories(sas_core_pure PUBLIC - $ - $ -) - -target_link_libraries(sas_core_pure PUBLIC -ldqrobotics Eigen3::Eigen) \ No newline at end of file diff --git a/cmake/pythonlib.cmake b/cmake/pythonlib.cmake deleted file mode 100644 index ee1f3dd..0000000 --- a/cmake/pythonlib.cmake +++ /dev/null @@ -1,27 +0,0 @@ -# Python wrapper module via pybind11 -find_package(Python3 REQUIRED COMPONENTS Development) - -set(PYBIND11_FINDPYTHON ON) -add_subdirectory(${CMAKE_CURRENT_LIST_DIR}/../pybind11 ${CMAKE_CURRENT_BINARY_DIR}/pybind11) - -pybind11_add_module(_sas_core SHARED - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_core_py.cpp - ${CMAKE_CURRENT_LIST_DIR}/../src/sas_robot_driver_py.cpp -) - -target_include_directories(_sas_core PUBLIC - $ - $ -) - -target_compile_definitions(_sas_core PRIVATE IS_SAS_PYTHON_BUILD) -target_link_libraries(_sas_core PRIVATE sas_core_pure -ldqrobotics) - -# Install path: ament PYTHON_INSTALL_DIR if available, else site-packages -if(DEFINED _SAS_PYTHON_INSTALL_DIR) - set(_SAS_PY_DEST "${_SAS_PYTHON_INSTALL_DIR}") -else() - set(_SAS_PY_DEST "lib/python3/dist-packages/sas_core") -endif() - -install(TARGETS _sas_core DESTINATION "${_SAS_PY_DEST}") \ No newline at end of file diff --git a/docker/Dockerfile b/docker/Dockerfile index 6cd2542..06c49d2 100644 --- a/docker/Dockerfile +++ b/docker/Dockerfile @@ -4,7 +4,31 @@ SHELL ["/bin/bash", "-c"] ENV BASH_ENV="/etc/bash_env" RUN sudo apt-get update && sudo apt-get upgrade -y -RUN python3 -m pip install --upgrade dqrobotics --break-system-packages -RUN sudo sudo apt-get remove -y ros-jazzy-sas-core + +# Remove the legacy monolithic sas_core package shipped by the base image; +# it would otherwise shadow this thin wrapper. +RUN sudo apt-get remove -y ros-jazzy-sas-core + +# Python bindings from PyPI (self-contained wheel, core statically linked). +# dqrobotics (python) is already present in the base image. +RUN python3 -m pip install --upgrade marinholab-sas-core --break-system-packages + +# C++ core .deb -- TEMPORARY: built from source because the +# libmarinholab-sas-core .deb is not yet published to an apt repository. +# libdqrobotics (C++) is already present in the base image, so no PPA is +# needed here. The build is architecture-agnostic (amd64 or arm64). +# +# Once the .deb is published to an apt repository, replace this block with: +# RUN sudo apt-get update \ +# && sudo apt-get install -y libmarinholab-sas-core +RUN sudo apt-get update \ + && sudo apt-get install -y debhelper dpkg-dev \ + && git clone --depth 1 --branch 26.09 https://github.com/MarinhoLab/sas_cpp.git /tmp/sas_cpp \ + && cd /tmp/sas_cpp \ + && dpkg-buildpackage -us -uc -b \ + && cd /tmp \ + && sudo dpkg -i ./libmarinholab-sas-core_*.deb \ + && sudo rm -rf /tmp/sas_cpp /tmp/libmarinholab-sas-core_*.deb + RUN mkdir -p /root/sas_core_devel/src/ -COPY . /root/sas_core_devel/src/sas_core \ No newline at end of file +COPY . /root/sas_core_devel/src/sas_core diff --git a/docker/compose.yml b/docker/compose.yml index ea4e18c..8874396 100644 --- a/docker/compose.yml +++ b/docker/compose.yml @@ -5,15 +5,4 @@ services: dockerfile: docker/Dockerfile environment: PYTHONUNBUFFERED: 1 - command: /bin/bash -c " - cd /root/sas_core_devel/src/ - && ls . - && colcon build - && source install/setup.bash - && ros2 run sas_core sas_clock_example - && ros2 run sas_core sas_clock_example_py.py - && ros2 run sas_core sas_robot_driver_example - && ros2 run sas_core sas_robot_driver_subclass_example_py.py - && ros2 run sas_core sas_clock_sched_fifo_example - && ros2 run sas_core sas_clock_sched_fifo_example_py.py - " \ No newline at end of file + command: /bin/bash -c "bash /root/sas_core_devel/src/sas_core/docker/smoke_test.sh" diff --git a/docker/smoke_test.sh b/docker/smoke_test.sh new file mode 100755 index 0000000..c6f09a0 --- /dev/null +++ b/docker/smoke_test.sh @@ -0,0 +1,46 @@ +#!/usr/bin/env bash +# Integration smoke test for the sas_core thin wrapper. +# +# Runs inside the docker environment provided by docker/compose.yml: +# [1/3] colcon build of the wrapper package +# [2/3] Python shim smoke test (sas_core -> marinholab.sas.core) +# [3/3] C++ compatibility-header consumer compiled against the installed +# libmarinholab_sas_core (legacy include path + namespace sas) +set -e + +cd /root/sas_core_devel/src/ + +echo '=== [1/3] colcon build (thin wrapper) ===' +colcon build + +echo '=== [2/3] Python shim smoke test ===' +source install/setup.bash +python3 sas_core/scripts/sas_core_smoke_test.py + +echo '=== [3/3] C++ compatibility header + installed library test ===' +# Wrapper-installed compat headers and the .deb-installed shared library. +SAS_INC=/root/sas_core_devel/src/install/sas_core/include +LIBDIR="$(dirname "$(ldconfig -p | awk '/libmarinholab_sas_core\.so/ {print $NF; exit}')")" +echo "using headers: ${SAS_INC}" +echo "using library: ${LIBDIR}" + +cat > /tmp/sas_core_compat_test.cpp <<'CPP' +#include +#include + +int main() +{ + sas::Clock c(0.01); + c.init(); + c.update_and_sleep(); + std::cout << "C++ compat OK, elapsed=" << c.get_elapsed_time_sec() << std::endl; + return 0; +} +CPP + +g++ /tmp/sas_core_compat_test.cpp -o /tmp/sas_core_compat_test \ + -I"${SAS_INC}" -L"${LIBDIR}" -lmarinholab_sas_core \ + -Wl,-rpath,"${LIBDIR}" +/tmp/sas_core_compat_test + +echo '=== ALL CHECKS PASSED ===' diff --git a/include/sas_core/eigen3_std_conversions.hpp b/include/sas_core/eigen3_std_conversions.hpp index 69827a8..0520ed7 100644 --- a/include/sas_core/eigen3_std_conversions.hpp +++ b/include/sas_core/eigen3_std_conversions.hpp @@ -1,78 +1,10 @@ +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include +// path and the legacy "namespace sas" used by downstream packages. #pragma once -/* -# Copyright (c) 2016-2020 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file eigen3_std_conversions.hpp - * @brief Conversions between Eigen, std::vector and DQ types. - * - * Small utility functions to convert between Eigen's Vector/Matrix types, - * std::vector and DQ (dual quaternion) representations used across the - * project. - */ -#include -#include -#include - -using namespace Eigen; -using namespace DQ_robotics; +#include namespace sas { -/** - * @brief Convert an Eigen::VectorXd to a std::vector. - * @param vectorxd Source Eigen vector. - * @return std::vector containing the same elements in order. - */ -std::vector vectorxd_to_std_vector_double(const VectorXd& vectorxd); - -/** - * @brief Convert an Eigen::VectorXi to a std::vector. - * @param vectorxi Source Eigen integer vector. - * @return std::vector containing the same elements in order. - */ -std::vector vectorxi_to_std_vector_int(const VectorXi& vectorxi); - -/** - * @brief Convert a std::vector to an Eigen::VectorXd. - * @param std_vector_double Source std::vector. - * @return VectorXd containing the same elements. - */ -VectorXd std_vector_double_to_vectorxd(std::vector std_vector_double); - -/** - * @brief Convert a std::vector to an Eigen::VectorXi. - * @param std_vector_int Source std::vector. - * @return VectorXi containing the same elements. - */ -VectorXi std_vector_int_to_vectorxi(std::vector std_vector_int); - -/** - * @brief Convert a std::vector to a DQ (dual quaternion) object. - * @param std_vector_double Source std::vector containing the DQ coefficients. - * @return DQ constructed from the provided coefficients. - */ -DQ std_vector_double_to_dq(const std::vector& std_vector_double); + using namespace marinholab::sas::core; } - diff --git a/include/sas_core/examples/sas_robot_driver_example.hpp b/include/sas_core/examples/sas_robot_driver_example.hpp index eeddce1..d2e1937 100644 --- a/include/sas_core/examples/sas_robot_driver_example.hpp +++ b/include/sas_core/examples/sas_robot_driver_example.hpp @@ -1,114 +1,9 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ -/** - * @file sas_robot_driver_example.hpp - * @brief Example RobotDriver implementation for testing and demonstration. - * - * Provides a simple in-memory RobotDriverExample used as a reference - * implementation for testing the RobotDriver interfaces without requiring - * real hardware. Useful for unit tests and examples. - */ +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include path and namespace. #pragma once - -#include -#include -#include - -using namespace Eigen; +#include namespace sas { -/** - * @brief Configuration for the RobotDriverExample. - * - * Holds example-specific parameters such as a human-readable name, initial - * joint positions and joint limits used by the in-memory example driver. - */ -struct RobotDriverExampleConfiguration -{ - std::string name; - VectorXd initial_joint_positions; - std::tuple joint_limits; -}; - -/** - * @brief Simple in-memory RobotDriver implementation for testing. - * - * RobotDriverExample is a lightweight, non-hardware driver that implements - * the RobotDriver interface. It is intended for unit tests and examples and - * simulates joint state updates using the provided configuration. - */ - class RobotDriverExample: public RobotDriver -{ -protected: - const RobotDriverExampleConfiguration configuration_; - VectorXd joint_positions_; - -public: - RobotDriverExample(RobotDriverExample&) = delete; - RobotDriverExample()=delete; - - /** - * @brief Construct a RobotDriverExample with configuration - * @param configuration Example configuration (name, initial positions, joint limits) - * @param break_loops Optional pointer to an atomic_bool used to break loops - */ - RobotDriverExample(const RobotDriverExampleConfiguration& configuration, const std::shared_ptr& shutdown_signaler_); - [[deprecated("Use RobotDriverExample(const RobotDriverExampleConfiguration& configuration, const std::shared_ptr& shutdown_signaler_) instead.")]] - RobotDriverExample(const RobotDriverExampleConfiguration& configuration, std::atomic_bool* break_loops); - - /** - * @brief Get the current joint positions - * @return Vector of joint positions (radians) - */ - virtual VectorXd get_joint_positions() override; - - /** - * @brief Set target joint positions - * @param set_target_joint_positions_rad Target joint positions in radians - * @throws std::runtime_error if the input vector has incorrect size - */ - virtual void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad) override; - - /** - * @brief Connect the example driver (establish resources) - */ - virtual void connect() override; - - /** - * @brief Disconnect the example driver (release resources) - */ - virtual void disconnect() override; - - /** - * @brief Initialize the example driver - */ - virtual void initialize() override; - - /** - * @brief Deinitialize the example driver - */ - virtual void deinitialize() override; -}; + using namespace marinholab::sas::core; } diff --git a/include/sas_core/sas_clock.hpp b/include/sas_core/sas_clock.hpp index 0949b6c..af12b98 100644 --- a/include/sas_core/sas_clock.hpp +++ b/include/sas_core/sas_clock.hpp @@ -1,172 +1,10 @@ +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include +// path and the legacy "namespace sas" used by downstream packages. #pragma once -/* -# Copyright (c) 2016-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_clock.hpp - * @brief Timing utilities for control loops and statistics collection. - * - * The Clock class manages loop timing, sleep with - * early exit, compute elapsed times and collect optional statistics for - * computational time, idle time and effective sampling time used by - * control threads. - */ -#include -#include -#include - -#include -#include +#include namespace sas { - -/** - * @brief Clock utility for timing and statistics in control loops. - */ -class Clock : private sas::Object -{ -public: - /** @brief Enumeration of time types, see get_time() and get_statistics() */ - enum class TimeType{ - Computational, ///< Time spent between when the instant the previous sleep ended and the instant when the current one started. - EffectiveSampling, ///< The effective sampling time between sleeps (EffectiveSampling = Computational + Idle). - Idle ///< Time spent between the instant when the previous sleep started and the instant when it ended. - }; -private: - - // https://stackoverflow.com/questions/65397041/apple-clang-why-can-i-not-create-a-time-point-from-stdchrononanoseconds - std::chrono::time_point time_initial_; - std::chrono::time_point next_loop_deadline_; - - std::chrono::time_point time_before_sleep_; - std::chrono::time_point time_after_sleep_; - - const std::chrono::nanoseconds target_sampling_time_; - - std::map kept_times_map_; - - long overrun_sampling_time_count_; - - const bool enable_statistics_; - - std::map,std::tuple> statistics_map_; - void _compute_statistics_(); - -public: - - Clock()=delete; - Clock(const int&)=delete; - - /** - * @brief Construct a Clock - * @param sampling_time_in_seconds Desired sampling time in seconds - * @param enable_statistics Whether to enable internal statistics collection (default: true) - */ - explicit Clock(const double& sampling_time_in_seconds, const bool& enable_statistics=true); - - /** - * @brief Initialize the clock internal state and timers - */ - void init(); - - /** - * @brief Update internal timing measurements and sleep to respect target sampling time - */ - void update_and_sleep(); - - /** - * @brief Get elapsed time since last update in seconds - * @return Elapsed time in seconds - */ - double get_elapsed_time_sec() const; - - /** - * @brief Get the initial time point recorded by the clock - * @return time_point of the initial time - */ - std::chrono::time_point get_initial_time() const; - - /** - * @brief Get the time point of the last update - * @return time_point of the last update - */ - std::chrono::time_point get_last_update_time() const; - - /** - * @brief Sleep for the specified duration while allowing early exit via break_loop - * @param seconds Sleep duration in seconds - * @param break_loop Pointer to an atomic boolean that will interrupt the sleep if set - */ - void safe_sleep_seconds(const double& seconds, std::atomic_bool* break_loop); - - /** - * @brief Block the calling thread for the specified duration (no early exit) - * @param seconds Sleep duration in seconds - */ - void blocking_sleep_seconds(const double& seconds); - - /** - * @brief Return the desired sampling time for the thread in seconds - * @return Desired sampling time in seconds - */ - double get_desired_thread_sampling_time_sec() const; - - /** - * @brief Return the number of times the sampling has overrun the target period - * @return Overrun count - */ - long get_overrun_count() const; - - /** - * @brief Get a time value for the provided TimeType - * @param time_type The TimeType (Computational, EffectiveSampling, Idle) - * @return Time value in seconds corresponding to time_type - */ - double get_time(const TimeType& time_type) const; - - /** - * @brief Get a statistic value for the given statistic type and TimeType - * @param statistics The statistic to query (see sas::Statistics) - * @param time_type The TimeType to which the statistic applies - * @return The requested statistic value as a double - * @throws std::runtime_error if statistics collection was not enabled at construction - * or if the requested statistic is not available for the provided TimeType. - */ - double get_statistics(const Statistics &statistics, const TimeType &time_type) const; - - ///Deprecated - [[deprecated("Use get_time(sas::Clock::Computational) instead.")]] - double get_computation_time() const; - [[deprecated("Use get_time(sas::Clock::Idle) instead.")]] - double get_sleep_time() const; - [[deprecated("Use get_time(sas::Clock::EffectiveSampling) instead.")]] - double get_effective_thread_sampling_time_sec() const; -}; - + using namespace marinholab::sas::core; } - - - - diff --git a/include/sas_core/sas_core.hpp b/include/sas_core/sas_core.hpp index 002057a..8b492a4 100644 --- a/include/sas_core/sas_core.hpp +++ b/include/sas_core/sas_core.hpp @@ -1,109 +1,10 @@ -/* -# Copyright (c) 2022-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ -/** - * @file sas_core.hpp - * @brief Core numerical utilities used across the project. - * - * This header contains lightweight, frequently used functions such as - * incremental mean computation, vector/matrix concatenation, block-diagonal - * assembly and vector splitting utilities. These utilities are intended to be - * header-only and efficient for use in real-time control loops. - */ +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include +// path and the legacy "namespace sas" used by downstream packages. #pragma once - -#include - -using namespace Eigen; +#include namespace sas { - -/** - * @brief Statistical measures supported by sas_core utilities. - * - * Enumeration of lightweight statistics that classes and functions may - * compute (for example, Clock statistics). Add new entries here as new - * statistics are supported. - */ -enum class Statistics{ - Mean -}; - -template -/** - * @brief incremental_mean a simple implementation of incremental mean, for the many cases in which - * keeping a vector of all values would be impractical. - * @param current_mean the current value of the mean. - * @param current_number_of_samples the current number of samples, not considering the new_sample. - * @param new_sample the new sample that will change the mean. - * @throws std::range_error if current_number_of_samples is negative. - * @return the incremental mean, considering the new_sample. - */ -constexpr T incremental_mean(const T ¤t_mean, const int ¤t_number_of_samples, const T &new_sample) -{ - if(current_number_of_samples<0) - throw std::range_error("incremental_mean::current_number_of_samples should be larger than 0"); - return (current_mean * current_number_of_samples + new_sample)/(current_number_of_samples+1); -} - -/** - * @brief Concatenate two vectors by appending b after a. - * @param a First vector. - * @param b Second vector. - * @return Concatenated vector containing all elements of a followed by b. - */ -VectorXd concatenate(const VectorXd& a, const VectorXd& b); - -/** - * @brief Concatenate a list of vectors into a single vector. - * @param as Vector of vectors to concatenate in order. - * @return Concatenated vector containing the elements of each input vector in order. - */ -VectorXd concatenate(const std::vector& as); - -/** - * @brief Stack two matrices vertically (A above B). - * @param A Top matrix. - * @param B Bottom matrix. - * @return Matrix formed by stacking A on top of B. Columns must match. - * @throws std::range_error if A and B have different numbers of columns. - */ -MatrixXd vstack(const MatrixXd& A, const MatrixXd& B); - -/** - * @brief Create a block-diagonal matrix from a list of matrices. - * @param As Vector of matrices to place on the block diagonal. - * @return Block-diagonal matrix containing the input matrices along its diagonal. - */ -MatrixXd block_diag(const std::vector& As); - -/** - * @brief Split a vector into pieces with sizes specified by ns. - * @param a Vector to split. - * @param ns Sizes of each piece; their sum must equal a.size(). - * @return Vector containing the split VectorXd pieces. - */ -std::vector split(const VectorXd& a, const std::vector& ns); - + using namespace marinholab::sas::core; } diff --git a/include/sas_core/sas_object.hpp b/include/sas_core/sas_object.hpp index a8bcd67..6823dda 100644 --- a/include/sas_core/sas_object.hpp +++ b/include/sas_core/sas_object.hpp @@ -1,53 +1,10 @@ +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include +// path and the legacy "namespace sas" used by downstream packages. #pragma once -/* -# Copyright (c) 2022-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_object.hpp - * @brief Base object class for identification and license utilities. - * - * Defines the sas::Object class standardises sas Objects, such as license header printing. - */ -#include +#include namespace sas { -/** - * @brief Base class for SAS objects. - */ -class Object -{ -protected: - const std::string class_name_; - void _print_license_header(const std::string& class_name); - Object(const std::string& class_name); -public: - Object() = delete; - /** - * @brief Get the class name identifier - * @return The class name as a std::string - */ - std::string get_class_name() const; -}; + using namespace marinholab::sas::core; } diff --git a/include/sas_core/sas_robot_driver.hpp b/include/sas_core/sas_robot_driver.hpp index 95ab125..2e9b69e 100644 --- a/include/sas_core/sas_robot_driver.hpp +++ b/include/sas_core/sas_robot_driver.hpp @@ -1,239 +1,10 @@ -#pragma once -/* -# Copyright (c) 2016-2025 Murilo Marques Marinho -# -# This file is part of sas_robot_driver. -# -# sas_robot_driver is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_robot_driver is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_robot_driver. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################ -# Contributors: -# -# 1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) -# Added the Watchdog functionaly initially proposed in -# https://github.com/SmartArmStack/sas_core/pull/1 -*/ - -/** - * @file sas_robot_driver.hpp - * @brief Abstract RobotDriver interface and watchdog support. - * - * Declares the RobotDriver abstract base class which concrete drivers must - * implement to interact with hardware. The header also contains watchdog - * management used by driver implementations. - */ -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace Eigen; +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include +// path and the legacy "namespace sas" used by downstream packages. +#pragma once +#include namespace sas { -/** - * @brief Abstract interface for robot hardware drivers. - * - * RobotDriver declares the virtual API that concrete drivers must implement - * to interact with robot hardware. - */ -class RobotDriver -{ -protected: - std::atomic_bool* break_loops_; //Deprecated - std::shared_ptr shutdown_signaler_; - std::tuple joint_limits_; - VectorXd joint_velocities_; - VectorXd joint_torques_; - - std::unique_ptr clock_; - std::unique_ptr watchdog_thread_; - std::chrono::time_point time_point_from_the_client_; - std::chrono::time_point time_point_from_the_server_; - bool watchdog_status_; - void _watchdog_thread_function(); - std::mutex mutex_watchdog_; - double max_acceptable_delay_ = 0.1; - double watchdog_period_; - - RobotDriver(const std::shared_ptr& shutdown_signaler_); - [[deprecated("Use RobotDriver(const std::shared_ptr& shutdown_signaler_) instead.")]] - RobotDriver(std::atomic_bool* break_loops); - - RobotDriver()=delete; - RobotDriver(const RobotDriver&)=delete; - - std::exception_ptr watchdog_exception_{nullptr}; - std::mutex watchdog_exception_mutex_; - - - std::function control_loop_callback_; - - public: - /** - * @brief Enumeration of optional driver functionalities - */ - enum class Functionality{ - None=0, - PositionControl, - VelocityControl, - ForceControl, - Homing, - ClearPositions, - Watchdog - }; - - /** - * @brief Virtual destructor for RobotDriver - */ - virtual ~RobotDriver(); - - /** - * @brief Get current joint positions - * @return Vector of joint positions (radians) - */ - virtual VectorXd get_joint_positions() = 0; - - /** - * @brief Set target joint positions - * @param set_target_joint_positions_rad Target joint positions (radians) - */ - virtual void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad) = 0; - - /** - * @brief Get current joint velocities - * @return Vector of joint velocities - * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) - */ - virtual VectorXd get_joint_velocities(); - - /** - * @brief Set target joint velocities - * @param set_target_joint_velocities Target joint velocities - * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) - */ - virtual void set_target_joint_velocities(const VectorXd& set_target_joint_velocities); - - /** - * @brief Get current joint torques - * @return Vector of joint torques - * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) - */ - virtual VectorXd get_joint_torques(); - - /** - * @brief Set target joint torques - * @param set_target_joint_torques Target joint torques - * @throws std::runtime_error if the default implementation is called (not implemented by derived driver) - */ - virtual void set_target_joint_torques(const VectorXd& set_target_joint_torques); - - /** - * @brief Get joint limits (min, max) - * @return Tuple of (min_limits, max_limits) - */ - virtual std::tuple get_joint_limits(); - - /** - * @brief Set joint limits (min, max) - * @param joint_limits Tuple of (min_limits, max_limits) - */ - virtual void set_joint_limits(const std::tuple& joint_limits); - - /** - * @brief Start the watchdog thread with the given period - * @param period Watchdog period as nanoseconds - */ - void watchdog_start(const std::chrono::nanoseconds& period); - - /** - * @brief Trigger the watchdog with timestamps from client/server and the current status - * @param time_point_from_the_client Time point provided by the client - * @param time_point_from_the_server Time point provided by the server - * @param status Current watchdog status flag - */ - void watchdog_trigger(const std::chrono::time_point& time_point_from_the_client, - const std::chrono::time_point& time_point_from_the_server, - const bool& status); - - /** - * @brief Set the maximum acceptable delay for the watchdog (seconds) - * @param max_acceptable_delay Maximum delay in seconds - */ - void watchdog_set_maximum_acceptable_delay(const double& max_acceptable_delay); - - /** - * @brief Check for exceptions thrown by the watchdog thread and rethrow if present - * @throws std::runtime_error if the watchdog thread detected a timing or status error - * @throws std::exception rethrows any exception captured from the watchdog thread - */ - void check_for_watchdog_exceptions(); - - /** - * @brief Connect to the underlying robot/hardware - */ - virtual void connect()=0; - - /** - * @brief Disconnect from the underlying robot/hardware - */ - virtual void disconnect()=0; - - /** - * @brief Initialize the driver resources - */ - virtual void initialize()=0; - - /** - * @brief Deinitialize the driver resources - */ - virtual void deinitialize()=0; - - - /** - * @brief Set the control loop callback function - * @param callback The callback function to be executed in the control loop - * - */ - void set_control_loop_callback(std::function callback); - - - /** - * @brief Execute the control loop callback if it has been set - * - * This method should be called by RobotDriverROS or any other class - * that runs the control loop. It will execute the callback that was - * set by the concrete RobotDriver implementation. - */ - void execute_control_loop_callback(); - - - /** - * @brief Check if a control loop callback has been set - * @return true if a callback has been set, false otherwise - */ - bool control_loop_callback_is_set(); -}; + using namespace marinholab::sas::core; } - - - diff --git a/include/sas_core/sas_shutdown_signaler.hpp b/include/sas_core/sas_shutdown_signaler.hpp index bc80692..5c680b0 100644 --- a/include/sas_core/sas_shutdown_signaler.hpp +++ b/include/sas_core/sas_shutdown_signaler.hpp @@ -1,75 +1,10 @@ +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include +// path and the legacy "namespace sas" used by downstream packages. #pragma once -/* -# Copyright (c) 2022-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_shutdown_signaler.hpp - * @brief Cross-module shutdown signaling class. - * - * Provides a class that coordinates shutdown requests between different - * parts of the system. Useful to unify shutdown behavior in both ROS and - * non-ROS contexts. - */ -#include +#include namespace sas { - /** - * @brief Shutdown coordinator. - * - * ShutdownSignaler provides a thread-safe mechanism to unify shutdown - * requests. - */ - class ShutdownSignaler - { - private: - std::atomic_bool* external_shutdown_signal_{nullptr}; - bool internal_shutdown_signal_{false}; - public: - /** - * @brief Default constructor - */ - ShutdownSignaler() = default; - /** - * @brief Construct a ShutdownSignaler using an external atomic flag - * @param external_shutdown_signal Pointer to an external atomic_bool used for shutdown signaling - */ - ShutdownSignaler(std::atomic_bool* external_shutdown_signal): - external_shutdown_signal_(external_shutdown_signal) - { - - }; - - /** - * @brief Check whether a shutdown has been requested - * @return true if shutdown requested (external or internal), false otherwise - */ - bool should_shutdown(); - - /** - * @brief Trigger a shutdown signal - */ - void shutdown(); - }; + using namespace marinholab::sas::core; } diff --git a/include/sas_core/sas_thread_manager.hpp b/include/sas_core/sas_thread_manager.hpp index 99dbfc8..934601b 100644 --- a/include/sas_core/sas_thread_manager.hpp +++ b/include/sas_core/sas_thread_manager.hpp @@ -1,140 +1,10 @@ -/* -# Copyright (c) 2022-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Juan Jose Quiroz Omana, email: juanjose.quirozomana@manchester.ac.uk -# -# ################################################################*/ - +// Compatibility shim: the C++ core now lives in the libmarinholab-sas-core +// .deb (MarinhoLab/sas_cpp). Preserves the legacy #include +// path and the legacy "namespace sas" used by downstream packages. #pragma once -#include -#include -#include -#include -#include -#include -#include +#include namespace sas { - -class ThreadManager -{ -public: - /** - * @brief Abstract priority levels for thread_manager threads. - * - * These levels are mapped to concrete Linux scheduling policies and - * priority/nice values in apply_priority(). The enum values themselves - * (0, 10, 50, 80, 90, 99) are NOT passed directly to the OS in most cases — - * they exist purely to establish a monotonic ordering (LOWEST < ... - * CRITICAL) that is easy to read and compare in application code. The - * actual OS-level values used for each level are documented per-enumerator - * below, and are applied in sas_thread_manager.cpp. - * - * @details Linux scheduling background: - * - SCHED_OTHER (the default, non-realtime policy) does not use - * sched_priority (it must be 0); differentiation between - * SCHED_OTHER threads is done via the nice value, range -20 - * (highest priority) to 19 (lowest), see setpriority(2). - * - SCHED_FIFO and SCHED_RR (realtime policies) use - * sched_priority directly, range 1 (low) to 99 (high) on - * Linux, see sched(7). Using these policies requires root or - * the CAP_SYS_NICE capability. - * - * @note The specific numeric choices below (nice 19/10/0, sched_priority - * 80/90/99) are this library's own convention, not values mandated - * by POSIX or Linux. Only the *range* (nice: -20..19, real-time - * sched_priority: 1..99) and the *meaning of SCHED_OTHER's priority - * field being 0* are OS-defined. CRITICAL uses 99 specifically - * because that is the maximum legal sched_priority for SCHED_FIFO/ - * SCHED_RR on Linux (sched_get_priority_max()), i.e. "as high as - * the OS allows." HIGH (80) and REALTIME (90) are spaced below - * that ceiling to preserve headroom and a clear ordering, not - * because those exact numbers carry any special OS meaning. - * - * @warning Real-time priorities are Linux static priorities: within a - * given policy, EQUAL priorities are round-robined (SCHED_RR) - * or run strictly FIFO (SCHED_FIFO) — see sched(7). Two threads - * both at REALTIME or CRITICAL can starve each other under - * SCHED_FIFO if neither blocks or yields. - * - * @see apply_priority() for the concrete Linux policy/value mapping. - */ - enum class PRIORITY { - LOWEST = 0, ///< SCHED_OTHER, nice 19 (least favorable, lowest priority) - BACKGROUND = 10, ///< SCHED_OTHER, nice 10 - NORMAL = 50, ///< SCHED_OTHER, nice 0 (default OS priority) - HIGH = 80, ///< SCHED_RR, sched_priority 80 (requires root/CAP_SYS_NICE) - REALTIME = 90, ///< SCHED_FIFO, sched_priority 90 (requires root/CAP_SYS_NICE) - CRITICAL = 99 ///< SCHED_FIFO, sched_priority 99 — the OS-defined maximum - }; - - -private: - // Thread optimization parameters - PRIORITY priority_{PRIORITY::NORMAL}; - int cpu_core_{-1}; - void apply_priority(); - void apply_cpu_affinity(); - mutable std::mutex mutex_; - - -protected: - sas::Clock clock_; - std::function loop_callback_; - std::thread thread_; - std::string thread_name_; - std::atomic running_{false}; - std::atomic stop_requested_{false}; - -protected: - void run(); -public: - ~ThreadManager(); - - ThreadManager(const ThreadManager&) = delete; // Copy constructor - ThreadManager& operator=(const ThreadManager&) = delete; // Copy assignment - ThreadManager(ThreadManager&&) = delete; // Move constructor - ThreadManager& operator=(ThreadManager&&) = delete; // Move assignment - - ThreadManager(const std::string& thread_name, - const double& period, - std::function callback, - PRIORITY priority = PRIORITY::NORMAL, - int cpu_core = -1); - - - const sas::Clock& get_clock() const; - - // Lifecycle - void start(); - void stop(); - bool is_running() const; - - - // Getters - std::string get_thread_name() const; - PRIORITY get_priority() const; - int get_cpu_core() const; - double get_period() const; -}; - + using namespace marinholab::sas::core; } - diff --git a/package.xml b/package.xml index 62a447b..d9d1201 100644 --- a/package.xml +++ b/package.xml @@ -3,11 +3,26 @@ sas_core 0.0.0 - TODO: Package description + + Thin ROS 2 wrapper around the SmartArmStack core. The C++ library + (libmarinholab_sas_core) is provided by the libmarinholab-sas-core .deb + (source: https://github.com/MarinhoLab/sas_cpp); the Python bindings are + provided by the PyPI package marinholab-sas-core + (https://github.com/MarinhoLab/sas_py). This package re-exports the ament + target `sas_core`, installs compatibility headers (namespace sas) and a + pure-Python `sas_core` shim so existing downstream packages keep working. + murilo - TODO: License declaration + LGPL-3.0 ament_cmake + ament_cmake_python + + ament_lint_auto ament_lint_common diff --git a/pybind11 b/pybind11 deleted file mode 160000 index d03662f..0000000 --- a/pybind11 +++ /dev/null @@ -1 +0,0 @@ -Subproject commit d03662f0984f652b60e7ddce53d3868002275197 diff --git a/sas_core/__init__.py b/sas_core/__init__.py index c7f58e5..797ab85 100644 --- a/sas_core/__init__.py +++ b/sas_core/__init__.py @@ -1,20 +1,24 @@ """ @file __init__.py -@brief Package entry for the sas_core Python bindings. +@brief Package entry for the sas_core Python bindings (compatibility shim). -This module re-exports the primary Python bindings provided by the -compiled extension module :mod:`sas_core._sas_core`. +The Python bindings now live in the PyPI package ``marinholab-sas-core`` +(https://github.com/MarinhoLab/sas_py), imported as ``marinholab.sas.core``. +This module re-exports its public names so existing code keeps working: -- Clock: high-resolution timing and sleep class. -- Statistics: enumeration for statistical types. -- RobotDriver: abstract robot driver interface that can be inherited by Python classes. -- ShutdownSignaler: request and wait for orderly shutdown. + from sas_core import Clock, Statistics, RobotDriver, ShutdownSignaler -The concrete implementations live in the compiled extension module -``sas_core._sas_core``. +``sas_core`` no longer ships its own compiled extension module; it is a thin +wrapper around the PyPI package. Install the bindings with: + pip install marinholab-sas-core """ -from sas_core._sas_core import Clock, Statistics, RobotDriver, ShutdownSignaler +from marinholab.sas.core import ( + Clock, + Statistics, + RobotDriver, + ShutdownSignaler, +) __all__ = ["Clock", "Statistics", "RobotDriver", "ShutdownSignaler"] diff --git a/scripts/sas_core_smoke_test.py b/scripts/sas_core_smoke_test.py new file mode 100755 index 0000000..c1f66f7 --- /dev/null +++ b/scripts/sas_core_smoke_test.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 +# Copyright (c) 2016-2026 Murilo Marques Marinho +# +# This file is part of sas_core. +# +# sas_core is free software: you can redistribute it and/or modify +# it under the terms of the GNU Lesser General Public License as published by +# the Free Software Foundation, either version 3 of the License, or +# (at your option) any later version. +# +# sas_core is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +# GNU Lesser General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public License +# along with sas_core. If not, see . + +"""Smoke test for the sas_core thin wrapper. + +Verifies both consumption paths used by downstream packages: + +- Python: ``from sas_core import ...`` resolves through the compatibility + shim to the ``marinholab.sas.core`` extension installed from PyPI, and a + Clock actually runs. +- C++: the CMake package config installed by the libmarinholab-sas-core .deb + is locatable (covers ``find_package(marinholab_sas_core)``, which also + resolves Eigen3 and recreates dqrobotics::dqrobotics). + +Exits non-zero on any failure. +""" + +import os +import subprocess +import sys +import tempfile + + +def check_python_bindings(): + from sas_core import Clock, Statistics, RobotDriver, ShutdownSignaler + import marinholab.sas.core as real + + # The shim must expose the exact same classes as the PyPI package. + assert Clock is real.Clock, "Clock shim mismatch" + assert Statistics is real.Statistics, "Statistics shim mismatch" + assert RobotDriver is real.RobotDriver, "RobotDriver shim mismatch" + assert ShutdownSignaler is real.ShutdownSignaler, "ShutdownSignaler mismatch" + + # Exercise a real Clock briefly. + clock = Clock(0.01) + clock.init() + clock.update_and_sleep() + elapsed = clock.get_elapsed_time_sec() + assert elapsed >= 0.0, f"unexpected elapsed time {elapsed}" + assert isinstance(clock.get_time(Clock.TimeType.Computational), float) + assert isinstance( + clock.get_statistics(Statistics.Mean, Clock.TimeType.Computational), + float, + ) + print(f"[python] sas_core shim OK (Clock elapsed {elapsed:.4f}s)") + + +def check_cmake_package(): + # Faithful consumer probe: configure+build+run a minimal CMake *project* + # (project mode, like colcon) against the installed .deb package. The + # .deb's config runs find_dependency(Eigen3) and recreates + # dqrobotics::dqrobotics, so this covers the full downstream path. + src = ( + "#include \n" + "#include \n" + "int main() {\n" + " marinholab::sas::core::Clock c(0.01);\n" + " c.init();\n" + " c.update_and_sleep();\n" + ' std::cout << "probe ok " << c.get_elapsed_time_sec() << std::endl;\n' + " return 0;\n" + "}\n" + ) + cmake = ( + "cmake_minimum_required(VERSION 3.16)\n" + "project(probe CXX)\n" + "find_package(marinholab_sas_core REQUIRED)\n" + "add_executable(probe main.cpp)\n" + "target_link_libraries(probe marinholab::sas::core)\n" + ) + with tempfile.TemporaryDirectory(prefix="sas_core_probe_") as tmp: + with open(os.path.join(tmp, "main.cpp"), "w") as fh: + fh.write(src) + with open(os.path.join(tmp, "CMakeLists.txt"), "w") as fh: + fh.write(cmake) + build_dir = os.path.join(tmp, "build") + cfg = subprocess.run( + ["cmake", "-S", tmp, "-B", build_dir], + capture_output=True, + text=True, + ) + if cfg.returncode != 0: + raise RuntimeError( + "find_package(marinholab_sas_core) configure failed:\n" + + cfg.stdout + + cfg.stderr + ) + build = subprocess.run( + ["cmake", "--build", build_dir], + capture_output=True, + text=True, + ) + if build.returncode != 0: + raise RuntimeError( + "probe build (link against libmarinholab_sas_core) failed:\n" + + build.stdout + + build.stderr + ) + run = subprocess.run( + [os.path.join(build_dir, "probe")], + capture_output=True, + text=True, + ) + if run.returncode != 0: + raise RuntimeError( + "probe run failed:\n" + run.stdout + run.stderr + ) + print("[cmake] consumer probe OK (find_package + link + run)") + + +def main() -> int: + try: + check_python_bindings() + check_cmake_package() + except Exception as exc: # noqa: BLE001 + print(f"[FAIL] {exc}", file=sys.stderr) + return 1 + print("[OK] sas_core thin wrapper smoke test passed") + return 0 + + +if __name__ == "__main__": + sys.exit(main()) diff --git a/src/eigen3_std_conversions.cpp b/src/eigen3_std_conversions.cpp deleted file mode 100644 index 0bad593..0000000 --- a/src/eigen3_std_conversions.cpp +++ /dev/null @@ -1,67 +0,0 @@ -/* -# Copyright (c) 2016-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file eigen3_std_conversions.cpp - * @brief Conversion functions between Eigen, STL containers, and DQ. - */ -#include - -namespace sas -{ - -std::vector vectorxd_to_std_vector_double(const VectorXd& vectorxd) -{ - std::vector vec(vectorxd.data(), vectorxd.data() + vectorxd.rows() * vectorxd.cols()); - return vec; -} - -VectorXd std_vector_double_to_vectorxd(std::vector std_vector_double) -{ - double* ptr = &std_vector_double[0]; - Eigen::Map vec(ptr,std_vector_double.size()); //We need access to the pointer here so we cannot use const ref - return vec; -} - -DQ std_vector_double_to_dq(const std::vector &std_vector_double) -{ - return DQ(std_vector_double_to_vectorxd(std_vector_double)); -} - -std::vector vectorxi_to_std_vector_int(const VectorXi &vectorxi) -{ - std::vector vec(vectorxi.data(), vectorxi.data() + vectorxi.rows() * vectorxi.cols()); - return vec; -} - -VectorXi std_vector_int_to_vectorxi(std::vector std_vector_int) -{ - int* ptr = &std_vector_int[0]; - Eigen::Map vec(ptr,std_vector_int.size()); //We need access to the pointer here so we cannot use const ref - return vec; -} - -} - - diff --git a/src/examples/sas_clock_example.cpp b/src/examples/sas_clock_example.cpp deleted file mode 100644 index 4e6bc99..0000000 --- a/src/examples/sas_clock_example.cpp +++ /dev/null @@ -1,65 +0,0 @@ -/* -# Copyright (c) 2016-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_clock_example.cpp - * @brief Example showing how to use sas::Clock to time loops. - */ - -#include - -#include - -int main(int, char**) -{ - // 10 ms - sas::Clock clock(0.01); - //Alternatively, use sas::Clock clock(0.01,false); to disable automatic statistics calculation. - //They do not take much time, but might affect whatever it is that we're trying to time. - - // Always initialize right before the loop, to reduce slowdown in the object allocation/initialization. - clock.init(); - for(int i=0;i<50;i++) - { - // Starting the loop with an update reduces overhead when entering the loop the first time. - clock.update_and_sleep(); - std::cout << "Loop n = " << i << std::endl; - std::cout << " Elapsed time: " << clock.get_elapsed_time_sec() << std::endl; - - std::cout << " Latest computation time: " << clock.get_time(sas::Clock::TimeType::Computational) << std::endl; - std::cout << " Latest idle time: " << clock.get_time(sas::Clock::TimeType::Idle) << std::endl; - std::cout << " Latest effective thread sampling time: " << clock.get_time(sas::Clock::TimeType::EffectiveSampling) << std::endl; - - std::cout << " Desired thread sampling time: " << clock.get_desired_thread_sampling_time_sec() << std::endl; - std::cout << " Overrun count: " << clock.get_overrun_count() << std::endl << std::endl; - } - - //Statistics - std::cout << "Statistics for the entire loop" << std::endl; - std::cout << " Mean computation time: " << clock.get_statistics(sas::Statistics::Mean,sas::Clock::TimeType::Computational) << std::endl; - std::cout << " Mean idle time: " << clock.get_statistics(sas::Statistics::Mean,sas::Clock::TimeType::Idle) << std::endl; - std::cout << " Mean effective thread sampling time: " << clock.get_statistics(sas::Statistics::Mean,sas::Clock::TimeType::EffectiveSampling) << std::endl; - - return 0; -} diff --git a/src/examples/sas_clock_sched_fifo_example.cpp b/src/examples/sas_clock_sched_fifo_example.cpp deleted file mode 100644 index 3f6107e..0000000 --- a/src/examples/sas_clock_sched_fifo_example.cpp +++ /dev/null @@ -1,66 +0,0 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_clock_sched_fifo_example.cpp - * @brief Example demonstrating Clock with realtime SCHED_FIFO. - */ - -#include - -#include - -int main(int, char**) -{ - // 1 ms - sas::Clock clock(0.001); - //Alternatively, use sas::Clock clock(0.01,false); to disable automatic statistics calculation. - //They do not take much time, but might affect whatever it is that we're trying to time. - - //Set the communication thread to be realtime with SCHED_FIFO. - sched_param sch; - int policy; - pthread_getschedparam(pthread_self(), &policy, &sch); - sch.sched_priority = 20; - if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch)) { - std::cout << "Failed to setschedparam: " << std::strerror(errno) << '\n'; - } - - // Always initialize right before the loop, to reduce slowdown in the object allocation/initialization. - clock.init(); - for(int i=0;i<50;i++) - { - // Starting the loop with an update reduces overhead when entering the loop the first time. - clock.update_and_sleep(); - } - - //Statistics - std::cout << "Statistics for the entire loop" << std::endl; - std::cout << " Mean computation time: " << clock.get_statistics(sas::Statistics::Mean,sas::Clock::TimeType::Computational) << std::endl; - std::cout << " Mean idle time: " << clock.get_statistics(sas::Statistics::Mean,sas::Clock::TimeType::Idle) << std::endl; - std::cout << " Mean effective thread sampling time: " << clock.get_statistics(sas::Statistics::Mean,sas::Clock::TimeType::EffectiveSampling) << std::endl; - std::cout << " Overrun count: " << clock.get_overrun_count() << std::endl << std::endl; - - return 0; -} diff --git a/src/examples/sas_core_example.cpp b/src/examples/sas_core_example.cpp deleted file mode 100644 index fd21ae7..0000000 --- a/src/examples/sas_core_example.cpp +++ /dev/null @@ -1,68 +0,0 @@ -/* -# Copyright (c) 2022-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_core_example.cpp - * @brief Example testing core utilities. - */ - -#include -#include - -using namespace Eigen; -using namespace sas; - -int main(int,char**) -{ - //VectorXd concatenate(const VectorXd& a, const VectorXd& b); - VectorXd a(3); a << 1,2,3; - VectorXd b(3); b << 4,5,6; - VectorXd c(6); c << 1,2,3,4,5,6; - assert((concatenate(a,b)==c)); - - //VectorXd concatenate(const std::vector& as); - auto as = {a,b}; - assert((concatenate(as)==c)); - - //MatrixXd vstack(const MatrixXd& A, const MatrixXd& B); - MatrixXd A(2,2); A << 1,2,3,4; - MatrixXd B(2,2); B << 5,6,7,8; - MatrixXd C(4,2); C << A,B; - assert((vstack(A,B)==C)); - - //MatrixXd block_diag(const std::vector& As); - auto As = {A,B}; - MatrixXd C_block_diag(4,4); C_block_diag << A,MatrixXd::Zero(2,2),MatrixXd::Zero(2,2),B; - assert((block_diag(As)==C_block_diag)); - - //std::vector split(const VectorXd& a, const std::vector& ns); - VectorXd a_split(10); a_split << 1,2,3,4,5,6,7,8,9,10; - std::vector ns = {2,5,3}; - auto split_result = split(a_split,ns); - assert((split_result[0]==(VectorXd(2)<<1,2).finished())); - assert((split_result[1]==(VectorXd(5)<<3,4,5,6,7).finished())); - assert((split_result[2]==(VectorXd(3)<<8,9,10).finished())); - - return 0; -} diff --git a/src/examples/sas_robot_driver_example.cpp b/src/examples/sas_robot_driver_example.cpp deleted file mode 100644 index 056cfb8..0000000 --- a/src/examples/sas_robot_driver_example.cpp +++ /dev/null @@ -1,77 +0,0 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_robot_driver_example.cpp - * @brief Example implementation of a RobotDriver. - */ -#include -#include - -sas::RobotDriverExample::RobotDriverExample(const RobotDriverExampleConfiguration &configuration, std::atomic_bool *break_loops): - RobotDriver(break_loops), - configuration_(configuration) -{ - set_joint_limits(configuration.joint_limits); -} - -sas::RobotDriverExample::RobotDriverExample(const RobotDriverExampleConfiguration& configuration, const std::shared_ptr& shutdown_signaler_): - RobotDriver(shutdown_signaler_), - configuration_(configuration) -{ - set_joint_limits(configuration.joint_limits); -} - -VectorXd sas::RobotDriverExample::get_joint_positions() -{ - return joint_positions_; -} - -void sas::RobotDriverExample::set_target_joint_positions(const VectorXd &set_target_joint_positions_rad) -{ - if(joint_positions_.size() != set_target_joint_positions_rad.size()) - throw std::runtime_error("sas::RobotDriverExample::set_target_joint_positions invalid size for set_target_joint_positions_rad"); - joint_positions_ = set_target_joint_positions_rad; -} - -void sas::RobotDriverExample::connect() -{ - std::cout << "Connecting to " << configuration_.name << std::endl; -} - -void sas::RobotDriverExample::disconnect() -{ - std::cout << "Disconnecting from " << configuration_.name << std::endl; -} - -void sas::RobotDriverExample::initialize() -{ - std::cout << "Initializing " << configuration_.name << std::endl; - joint_positions_ = configuration_.initial_joint_positions; -} - -void sas::RobotDriverExample::deinitialize() -{ - std::cout << "Deinitializing " << configuration_.name << std::endl; -} diff --git a/src/examples/sas_robot_driver_example_main.cpp b/src/examples/sas_robot_driver_example_main.cpp deleted file mode 100644 index 867a140..0000000 --- a/src/examples/sas_robot_driver_example_main.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_robot_driver_example_main.cpp - * @brief C++ example showcasing a sample RobotDriver subclass. - */ -#include -#include - -#include -static std::shared_ptr shutdown_signaler = std::make_shared(); -void sig_int_handler(int) -{ - shutdown_signaler->shutdown(); -} - -int main(int,char**) -{ - if(signal(SIGINT, sig_int_handler) == SIG_ERR) - { - throw std::runtime_error("sas_robot_driver_example_main::Error setting the signal int handler."); - } - - auto configuration = sas::RobotDriverExampleConfiguration(); - configuration.name = "Example_Robot_123"; - configuration.initial_joint_positions = VectorXd::Random(7); - - auto robot_driver_example = sas::RobotDriverExample(configuration, shutdown_signaler); - - robot_driver_example.connect(); - robot_driver_example.initialize(); - - std::cout << "Initial joint positions: " << robot_driver_example.get_joint_positions().transpose() << std::endl; - - VectorXd target_joint_positions = VectorXd::Random(7); - std::cout << "Target joint positions: " << target_joint_positions.transpose() << std::endl; - - robot_driver_example.set_target_joint_positions(target_joint_positions); - std::cout << "Joint positions after change: " << robot_driver_example.get_joint_positions().transpose() << std::endl; - - assert((target_joint_positions == robot_driver_example.get_joint_positions())); - - robot_driver_example.deinitialize(); - robot_driver_example.disconnect(); - - return 0; -} diff --git a/src/examples/thread_manager_example.cpp b/src/examples/thread_manager_example.cpp deleted file mode 100644 index 920ff08..0000000 --- a/src/examples/thread_manager_example.cpp +++ /dev/null @@ -1,45 +0,0 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) -# -# ################################################################*/ - -#include -#include -#include - -int main() { - int counter = 0; - - // Create and start thread - sas::ThreadManager tm("test", 0.1, [&counter]() { - std::cout << ++counter << "\n"; - }); - - tm.start(); - - // Run for 1 second - std::this_thread::sleep_for(std::chrono::seconds(1)); - - // Clean up - tm.stop(); - return 0; -} diff --git a/src/sas_clock.cpp b/src/sas_clock.cpp deleted file mode 100644 index f5eae80..0000000 --- a/src/sas_clock.cpp +++ /dev/null @@ -1,183 +0,0 @@ -/* -# Copyright (c) 2016-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_clock.cpp - * @brief Timing utilities for control loops. - */ - -#include - -#include "sas_core/sas_clock.hpp" - -namespace sas -{ - -Clock::Clock(const double& sampling_time_in_seconds, const bool &enable_statistics): - Object("sas::Clock"), - target_sampling_time_(std::chrono::nanoseconds(int(sampling_time_in_seconds*1e9))), - overrun_sampling_time_count_(0), - enable_statistics_(enable_statistics), - statistics_map_({{{TimeType::Computational,Statistics::Mean},{std::chrono::nanoseconds(0),0}}}) -{ - -} - -void Clock::_compute_statistics_() -{ - for(const auto& time_type : {TimeType::Computational,TimeType::EffectiveSampling,TimeType::Idle}) - { - auto [mean,size] = statistics_map_[{time_type,Statistics::Mean}]; - statistics_map_[{time_type,Statistics::Mean}]={incremental_mean(mean,size,kept_times_map_.at(time_type)),size+1}; - } -} - -void Clock::init() -{ - // Get initial time - time_initial_ = std::chrono::system_clock::now(); - - // Initialize all time points - time_before_sleep_ = time_initial_; - time_after_sleep_ = time_initial_; - next_loop_deadline_ = time_initial_; - - overrun_sampling_time_count_ = 0; -} - -void Clock::update_and_sleep() -{ - time_before_sleep_ = std::chrono::system_clock::now(); - - // The required computation time - kept_times_map_[TimeType::Computational] = time_before_sleep_ - time_after_sleep_; - - // Define the next deadline - // It is important to not define an impossible deadline. We also add the number - // of deadline overruns safely to a counter. - next_loop_deadline_ += target_sampling_time_; - while(next_loop_deadline_ < (time_before_sleep_)) - { - next_loop_deadline_ += target_sampling_time_; - // Increase counter only if it won't cause an overflow - if(overrun_sampling_time_count_+1 < std::numeric_limits().max()) - { - overrun_sampling_time_count_++; - } - } - - //Sleep until the deadline - std::this_thread::sleep_until(next_loop_deadline_); - - time_after_sleep_ = std::chrono::system_clock::now(); - // The time spend sleeping - kept_times_map_[TimeType::Idle] = time_after_sleep_ - time_before_sleep_; - - // The effective sampling time - kept_times_map_[TimeType::EffectiveSampling] = kept_times_map_.at(TimeType::Computational) + kept_times_map_.at(TimeType::Idle); - if(enable_statistics_) - { - _compute_statistics_(); - } -} - -std::chrono::time_point Clock::get_initial_time() const -{ - return time_initial_; -} - -double Clock::get_sleep_time() const -{ - return kept_times_map_.at(TimeType::Idle).count(); -} - -std::chrono::time_point Clock::get_last_update_time() const -{ - return time_after_sleep_; -} - -double Clock::get_computation_time() const -{ - return std::chrono::duration_cast>(kept_times_map_.at(TimeType::Computational)).count(); -} - -double Clock::get_desired_thread_sampling_time_sec() const -{ - return std::chrono::duration_cast>(target_sampling_time_).count(); -} - -double Clock::get_effective_thread_sampling_time_sec() const -{ - return std::chrono::duration_cast>(kept_times_map_.at(TimeType::EffectiveSampling)).count(); -} - -double Clock::get_elapsed_time_sec() const -{ - std::chrono::system_clock::time_point current_time = std::chrono::system_clock::now(); - return std::chrono::duration_cast>(current_time - get_initial_time()).count(); -} - -void Clock::safe_sleep_seconds(const double& seconds, std::atomic_bool* break_loop) -{ - for(int i=0;i - >(kept_times_map_.at(time_type)).count(); -} - -double Clock::get_statistics(const Statistics& statistics, const TimeType& time_type) const -{ - if(not enable_statistics_) - throw std::runtime_error("Statistics were not enabled, get_statistics will not return a valid value"); - if(statistics_map_.count({time_type,statistics})<=0) - throw std::runtime_error("Requested statistics is not available."); - - return std::chrono::duration_cast< - std::chrono::duration - >(std::get<0>(statistics_map_.at({time_type,statistics}))).count(); -} - -}; - - diff --git a/src/sas_core.cpp b/src/sas_core.cpp deleted file mode 100644 index 94d869a..0000000 --- a/src/sas_core.cpp +++ /dev/null @@ -1,147 +0,0 @@ -/* -# Copyright (c) 2022-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_core.cpp - * @brief Implementation of core functions. - */ -#include -#include - -namespace sas -{ - -/** - * @brief concatenate two VectorXd. - * @param a a VectorXd. - * @param b a VectorXd. - * @return the result of the concatenated vectors. - */ -VectorXd concatenate(const VectorXd& a, const VectorXd& b) -{ - return (VectorXd (a.size() + b.size()) << a, b).finished(); -} - -/** - * @brief concatenate a std::vector of VectorXd. - * @param a an std::vector of VectorXd. - * @return the result of the concatenated vectors. - */ -VectorXd concatenate(const std::vector& as) -{ - VectorXd b; - for(const auto& c : as) - { - b = concatenate(b, c); - } - return b; -} - -/** - * @brief vstack vertically (row-wise) stack two MatrixXd. - * @param A the first MatrixXd. - * @param B the second MatrixXd. - * @return the vstacked MatrixXd. - * @exception a std::range_error if @a A and @a B don't have - * the same number of columns. - * @note returns an empty matrix if both arguments are empty - * or return the other argument of only one of the arguments - * is empty. - */ -MatrixXd vstack(const MatrixXd& A, const MatrixXd& B) -{ - if((A.size() == 0) && (B.size()==0)) - return MatrixXd(); - if(A.size() == 0) - return B; - if(B.size() == 0) - return A; - if(A.cols()!=B.cols()) - throw std::range_error("vstack needs inputs a and b with the same number of columns."); - - return(MatrixXd(A.rows()+B.rows(),A.cols()) << A, B).finished(); -} - -/** - * @brief block_diag creates a block diagonal matrix - * using an input of std::vector. - * e.g. if As= [A, B, C], - * then - * block_diag(As) = - * |A 0 0| - * |0 B 0| - * |0 0 C| - * @param As the std::vector contaning - * the matrix to form the block diagonal matrix. - * @return the block diagonal matrix. - */ -MatrixXd block_diag(const std::vector& As) -{ - int rows = 0; - int cols = 0; - for(const auto& A : As) - { - rows+=A.rows(); - cols+=A.cols(); - } - - MatrixXd B = MatrixXd::Zero(rows,cols); - int start_row = 0; - int start_col = 0; - for(const auto& A : As) - { - const int& this_rows = A.rows(); - const int& this_columns = A.cols(); - - B.block(start_row,start_col,this_rows,this_columns) = A; - - start_row+=this_rows; - start_col+=this_columns; - } - - return B; -} - -/** - * @brief split splits the input VectorXd @a as into a set of subvectors - * defined by ns. - * @param as the VectorXd to be split. - * @param ns the sizes of the subvectors. - * @return an std::vector of the splitted vectors. - */ -std::vector split(const VectorXd& a, const std::vector& ns) -{ - std::vector as; - int n_acc = 0; - for(const auto& n : ns) - { - as.push_back(a.segment(n_acc,n)); - n_acc+=n; - } - return as; -} - - - -} diff --git a/src/sas_core_py.cpp b/src/sas_core_py.cpp deleted file mode 100644 index 690e7c7..0000000 --- a/src/sas_core_py.cpp +++ /dev/null @@ -1,128 +0,0 @@ -/* -# Copyright (c) 2016-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_core_py.cpp - * @brief Python bindings for core classes. - */ - -#include "sas_core_py.hpp" -#include -#include - -using C = sas::Clock; -using SS = sas::ShutdownSignaler; - -PYBIND11_MODULE(_sas_core, m) { - - // Module docstring (Doxygen-style tags included for downstream extraction) - m.doc() = R"pbdoc( -@file _sas_core -@brief Python bindings for core utilities (Clock, ShutdownSignaler, RobotDriver classes). -)pbdoc"; - - sas::init_sas_robot_driver_py(m); - - py::enum_(m, "Statistics", R"pbdoc(@brief Statistic types used by the Clock class. - -Currently only `Mean` is supported and it is used by Clock when statistics -collection is enabled.)pbdoc") - .value("Mean", sas::Statistics::Mean) - .export_values(); - - py::class_ shutdown_signaler(m, "ShutdownSignaler", - R"pbdoc(@brief Cross-module shutdown signaling class. - -ShutdownSignaler coordinates shutdown requests between different parts of the -system.)pbdoc"); - shutdown_signaler.def(py::init<>(), R"pbdoc(@brief Construct a ShutdownSignaler. - -Creates a ShutdownSignaler to coordinate shutdown -requests between threads.)pbdoc"); - shutdown_signaler.def("should_shutdown", &SS::should_shutdown, R"pbdoc(@brief Check whether a shutdown has been requested. - -@return True if a shutdown has been requested (internal or external), False otherwise.)pbdoc"); - shutdown_signaler.def("shutdown", &SS::shutdown, R"pbdoc(@brief Trigger a shutdown request. - -Callers must wait on `should_shutdown()` to observe the change.)pbdoc"); - - py::class_ clock(m, "Clock", - R"pbdoc(@brief Timing utilities for control loops and statistics collection.)pbdoc"); - - clock.def(py::init(), R"pbdoc(@brief Construct a Clock. - -@param sampling_time_in_seconds Desired sampling time (seconds).)pbdoc"); - clock.def("init", &C::init, R"pbdoc(@brief Initialize the clock internal state and timers. - -Call once before using the clock to set the initial reference time.)pbdoc"); - clock.def("update_and_sleep", &C::update_and_sleep, R"pbdoc(@brief Update internal timing measurements and sleep to respect the target sampling time. - -This records the computation time since the last update, computes the next -deadline and sleeps until that deadline. If statistics collection is enabled -the internal statistics are updated.)pbdoc"); - clock.def("get_elapsed_time_sec", &C::get_elapsed_time_sec, R"pbdoc(@brief Get elapsed time since initialization. - -@return Elapsed time in seconds since the clock was initialized via `init()`.)pbdoc"); - clock.def("get_desired_thread_sampling_time_sec", &C::get_desired_thread_sampling_time_sec, R"pbdoc(@brief Get the configured sampling interval. - -@return Desired sampling time in seconds as configured at construction.)pbdoc"); - - // std::chrono time points are not bound to Python here (returned types are C++ types) - - clock.def("safe_sleep_seconds", &C::safe_sleep_seconds, R"pbdoc(@brief Sleep for approximately the given duration while allowing early exit. - -@param seconds Duration in seconds to sleep. -@param break_loop Optional pointer to an atomic boolean (C++ side) used to interrupt the sleep early. If `None` is passed from Python it will behave as a blocking sleep loop.)pbdoc"); - clock.def("blocking_sleep_seconds", &C::blocking_sleep_seconds, R"pbdoc(@brief Block the calling thread for the specified duration (no early exit). - -@param seconds Duration in seconds to block.)pbdoc"); - - clock.def("get_overrun_count", &C::get_overrun_count, R"pbdoc(@brief Return the number of times the sampling period has been missed (overrun). - -@return Overrun count as a long integer.)pbdoc"); - clock.def("get_time", &C::get_time, R"pbdoc(@brief Get a time measurement for a given TimeType. - -@param time_type One of Clock.TimeType (Computational, EffectiveSampling, Idle). -@return Time value in seconds corresponding to the requested type.)pbdoc"); - clock.def("get_statistics", &C::get_statistics, R"pbdoc(@brief Get a collected statistic value. - -@param statistics The statistic type to query (see `sas.Statistics`). -@param time_type The Clock.TimeType to which the statistic applies. -@return The requested statistic value as a double. - -@throw RuntimeError If statistics collection was not enabled at construction - or if the requested statistic is not available.)pbdoc"); - - py::enum_(clock, "TimeType", R"pbdoc(@brief Enumeration of time types recorded by Clock. - -Values: -- Computational: Time spent computing between updates. -- EffectiveSampling: Computational + Idle time between updates. -- Idle: Time spent sleeping between updates.)pbdoc") - .value("Computational", C::TimeType::Computational) - .value("EffectiveSampling", C::TimeType::EffectiveSampling) - .value("Idle", C::TimeType::Idle) - .export_values(); - -} diff --git a/src/sas_core_py.hpp b/src/sas_core_py.hpp deleted file mode 100644 index a4834e2..0000000 --- a/src/sas_core_py.hpp +++ /dev/null @@ -1,40 +0,0 @@ -#pragma once -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_core_py.hpp - * @brief Functions to separately initialise more complex Python bindings. - */ - -#include -#include -#include - -namespace py = pybind11; - -namespace sas -{ - void init_sas_robot_driver_py(py::module& m); -} diff --git a/src/sas_object.cpp b/src/sas_object.cpp deleted file mode 100644 index 4e9e291..0000000 --- a/src/sas_object.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* -# Copyright (c) 2022-2023 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################*/ - -/** - * @file sas_object.cpp - * @brief Implementation of the Object base class. - */ -#include "sas_core/sas_object.hpp" - -#include - -namespace sas -{ - -/** - * @brief Object::_print_license_header A Copyright header to help enforce LGPLv3 compliance. - * - * If you modify this method in any shape or form, you have therefore made a derivative work of - * this library, hence you must abide to the Copyleft side of the LGPLv3. - */ -void Object::_print_license_header(const std::string& class_name) -{ - // Using colors: https://stackoverflow.com/questions/2616906/how-do-i-output-coloured-text-to-a-linux-terminal - std::string blue_text("\033[1;34m"); - std::string default_color("\033[0m"); - - - std::cout << blue_text << std::string(class_name.size(),'*') - << "****************************************************************" << default_color << std::endl; - std::cout << blue_text << class_name - + " (c) Murilo M. Marinho (murilomarinho.info) 2016-2026 LGPLv3" << default_color << std::endl; - std::cout << blue_text << std::string(class_name.size(),'*') - << "****************************************************************" << default_color << std::endl; -} - -Object::Object(const std::string &class_name): - class_name_(class_name) -{ - _print_license_header(class_name); -} - -std::string sas::Object::get_class_name() const -{ - return class_name_; -} - - -} diff --git a/src/sas_robot_driver.cpp b/src/sas_robot_driver.cpp deleted file mode 100644 index 63463ea..0000000 --- a/src/sas_robot_driver.cpp +++ /dev/null @@ -1,247 +0,0 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_robot_driver. -# -# sas_robot_driver is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_robot_driver is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_robot_driver. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################# Contributors: -# -# 1. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) -# Added the Watchdog functionality initially proposed in -# https://github.com/SmartArmStack/sas_core/pull/1 -#*/ - -/** - * @file sas_robot_driver.cpp - * @brief Implementation of the RobotDriver base class and watchdog. - */ - - -#include -#include - -namespace sas -{ - -RobotDriver::RobotDriver(std::atomic_bool *break_loops): - break_loops_(break_loops), - shutdown_signaler_(std::make_shared(break_loops)) -{ - -} - -RobotDriver::RobotDriver(const std::shared_ptr& shutdown_signaler): - break_loops_(nullptr), - shutdown_signaler_(shutdown_signaler) -{ -} - -RobotDriver::~RobotDriver() -{ - if (watchdog_thread_ && watchdog_thread_->joinable()) { - shutdown_signaler_->shutdown(); //To force the thread to shutdown if it hasn't already done so - watchdog_thread_->join(); - } -} - -VectorXd RobotDriver::get_joint_velocities() -{ - throw std::runtime_error("Not implemented yet."); -} - -void RobotDriver::set_target_joint_velocities(const VectorXd&) -{ - throw std::runtime_error("Not implemented yet."); -} - -VectorXd RobotDriver::get_joint_torques() -{ - throw std::runtime_error("Not implemented yet."); -} - -void RobotDriver::set_target_joint_torques(const VectorXd &) -{ - throw std::runtime_error("Not implemented yet."); -} - -std::tuple RobotDriver::get_joint_limits() -{ - return joint_limits_; -} - -void RobotDriver::set_joint_limits(const std::tuple &joint_limits) -{ - joint_limits_ = joint_limits; -} - - -/** - * @brief RobotDriver::_watchdog_thread_function throws an exception if the elapsed time since the last trigger - * exceeds the specified period. - */ -void RobotDriver::_watchdog_thread_function() -{ - const double thread_freq =1.0/clock_->get_desired_thread_sampling_time_sec(); - clock_->init(); - while(!shutdown_signaler_->should_shutdown()) - { - try{ - std::chrono::system_clock::time_point current_time = std::chrono::system_clock::now(); - double elapsed_time; - double elapsed_time_same_clock; - bool wstatus; - { - std::scoped_lock lock(mutex_watchdog_); - elapsed_time = std::chrono::duration_cast>(current_time - time_point_from_the_client_).count(); - elapsed_time_same_clock = std::chrono::duration_cast>(current_time - time_point_from_the_server_).count(); - wstatus = watchdog_status_; - } - - double clock_difference = std::abs(elapsed_time - elapsed_time_same_clock); - if (clock_difference > max_acceptable_delay_) - throw std::runtime_error( - std::string("RobotDriver:: The watchdog signal is delayed, or the clocks between the client and server are out of synch! ") + - "Watchdog signal delay: " + std::to_string(1000*clock_difference) +"ms." - ); - - - - if (elapsed_time_same_clock > watchdog_period_) - throw std::runtime_error( - std::string("RobotDriver:: The watchdog signal was lost! ") + - "The elapsed time was " + std::to_string(elapsed_time_same_clock) + - " seconds, but the period is " + std::to_string(watchdog_period_) + ". There was a watchdog signal delay of " + std::to_string(1000*clock_difference) + - "ms." + - "The watchdog thread runs at " + std::to_string(thread_freq)+ "Hz." - ); - - if(!wstatus) - throw std::runtime_error("RobotDriver:: The watchdog status is false!"); - clock_->update_and_sleep(); - }catch (...) - { - std::scoped_lock lock(watchdog_exception_mutex_); - watchdog_exception_ = std::current_exception(); - } - } -} - -/** - * @brief RobotDriver::watchdog_start starts the watchdog thread - * @param period The period of time. - */ -void RobotDriver::watchdog_start(const std::chrono::nanoseconds& period) -{ - if (!clock_) - { - watchdog_period_ = std::chrono::duration_cast>(period).count(); - // If the watchdog period is 1 second, the watchdog thread control is going to check five times per second. - clock_ = std::make_unique(watchdog_period_/5.0); - } - - if (!watchdog_thread_) - watchdog_thread_ = std::make_unique(&RobotDriver::_watchdog_thread_function, this); - -} - - -/** - * @brief RobotDriver::watchdog_trigger updates the trigger signal. - * @param time_point_from_the_client This time point corresponds to the moment the signal was sent, as recorded by the client computer's clock. - * @param time_point_from_the_server The time point when the watchdog signal was received. This time point uses - * the computer's clock on which the server (robot) is running. - * @param status The desired status. If false, the driver is going to stop. - */ -void RobotDriver::watchdog_trigger(const std::chrono::time_point& time_point_from_the_client, - const std::chrono::time_point& time_point_from_the_server, - const bool& status) -{ - { - std::scoped_lock lock(mutex_watchdog_); - time_point_from_the_client_ = time_point_from_the_client; - time_point_from_the_server_ = time_point_from_the_server; - watchdog_status_ = status; - } -} - - -/** - * @brief RobotDriver::watchdog_set_maximum_acceptable_delay sets the maximum acceptable clock skew to check the synchronization or - * potential delays between the time point of watchdog signal sent by the client and - * the time point when the watchdog signal was received. - * @param max_acceptable_delay - */ -void RobotDriver::watchdog_set_maximum_acceptable_delay(const double &max_acceptable_delay) -{ - max_acceptable_delay_ = max_acceptable_delay; -} - -/** - * @brief RobotDriver::check_for_watchdog_exceptions this method rethrows any exception thrown in the watchdog thread control loop. - */ -void RobotDriver::check_for_watchdog_exceptions() -{ - std::scoped_lock lock(watchdog_exception_mutex_); - if (watchdog_exception_) - std::rethrow_exception(watchdog_exception_); -} - -/** - * @brief RobotDriver::set_control_loop_callback - * @param callback - */ -void RobotDriver::set_control_loop_callback(std::function callback) -{ - control_loop_callback_ = std::move(callback); -} - - -/** - * @brief Execute the control loop callback if it has been set - * - * This method should be called by RobotDriverROS or any other class - * that runs the control loop. It will execute the callback that was - * set by the concrete RobotDriver implementation. - */ -void RobotDriver::execute_control_loop_callback() -{ - if (control_loop_callback_) - { - try - { - control_loop_callback_(); - } - catch(const std::exception& e) - { - throw std::runtime_error("Control loop callback exception: " + std::string(e.what())); - } - } -} - -/** - * @brief Check if a control loop callback has been set - * @return true if a callback has been set, false otherwise - */ -bool RobotDriver::control_loop_callback_is_set() -{ - return static_cast(control_loop_callback_); -} - - -} diff --git a/src/sas_robot_driver_py.cpp b/src/sas_robot_driver_py.cpp deleted file mode 100644 index 760c3a8..0000000 --- a/src/sas_robot_driver_py.cpp +++ /dev/null @@ -1,198 +0,0 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_robot_driver. -# -# sas_robot_driver is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_robot_driver is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_robot_driver. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################# -# Contributors: -#*/ - -/** - * @file sas_robot_driver_py.cpp - * @brief Pybind11 trampoline and bindings for RobotDriver. - */ - -#include "sas_core_py.hpp" -#include - -namespace sas -{ - -//https://pybind11.readthedocs.io/en/stable/advanced/classes.html -//Trampoline class -class RobotDriverPy: public RobotDriver, public py::trampoline_self_life_support -{ -public: - RobotDriverPy(const std::shared_ptr& ss): - RobotDriver(ss) - { - }; - ~RobotDriverPy()=default; - - /* Trampoline (need one for each virtual function) */ - VectorXd get_joint_positions() override - { - PYBIND11_OVERRIDE_PURE( - VectorXd, /* Return type */ - RobotDriver, /* Parent class */ - get_joint_positions, /* Name of function in C++ (must match Python name) */ - /* Argument(s) */ - ); - } - void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad) override - { - PYBIND11_OVERRIDE_PURE( - void, - RobotDriver, - set_target_joint_positions, - set_target_joint_positions_rad - ); - } - - VectorXd get_joint_velocities() override - { - PYBIND11_OVERRIDE( - VectorXd, - RobotDriver, - get_joint_velocities, - ); - } - - void set_target_joint_velocities(const VectorXd& set_target_joint_velocities) override - { - PYBIND11_OVERRIDE( - void, - RobotDriver, - set_target_joint_velocities, - set_target_joint_velocities - ); - } - - VectorXd get_joint_torques() override - { - PYBIND11_OVERRIDE( - VectorXd, - RobotDriver, - get_joint_torques, - ); - } - - - void set_target_joint_torques(const VectorXd& set_target_joint_torques) override - { - PYBIND11_OVERRIDE( - void, - RobotDriver, - set_target_joint_torques, - set_target_joint_torques - ); - } - - //std::tuple get_joint_limits() override - //{ - // PYBIND11_OVERRIDE( - // std::tuple, - // RobotDriver, - // get_joint_limits, - // ); - //} - - void set_joint_limits(const std::tuple& joint_limits) override - { - PYBIND11_OVERRIDE( - void, - RobotDriver, - set_joint_limits, - joint_limits - ); - } - - void connect() override - { - PYBIND11_OVERRIDE_PURE( - void, - RobotDriver, - connect, - ); - } - - void disconnect() override - { - PYBIND11_OVERRIDE_PURE( - void, - RobotDriver, - disconnect, - ); - } - - void initialize() override - { - PYBIND11_OVERRIDE_PURE( - void, - RobotDriver, - initialize, - ); - } - - void deinitialize() override - { - PYBIND11_OVERRIDE_PURE( - void, - RobotDriver, - deinitialize, - ); - } -}; - -void init_sas_robot_driver_py(py::module& m) -{ - py::class_< - RobotDriver, - RobotDriverPy, - py::smart_holder - > c(m,"RobotDriver"); - - c.def(py::init&>()); - - c.def("get_joint_positions", &RobotDriver::get_joint_positions, ""); - c.def("set_target_joint_positions", &RobotDriver::set_target_joint_positions, ""); - - c.def("get_joint_velocities", &RobotDriver::get_joint_velocities, ""); - c.def("set_target_joint_velocities", &RobotDriver::set_target_joint_velocities, ""); - - c.def("get_joint_torques", &RobotDriver::get_joint_torques, ""); - c.def("set_target_joint_torques", &RobotDriver::set_target_joint_torques, ""); - - c.def("get_joint_limits", &RobotDriver::get_joint_limits, ""); - c.def("set_joint_limits", &RobotDriver::set_joint_limits, ""); - - c.def("watchdog_start", &RobotDriver::watchdog_start, ""); - c.def("watchdog_trigger", &RobotDriver::watchdog_trigger, ""); - c.def("watchdog_set_maximum_acceptable_delay", &RobotDriver::watchdog_set_maximum_acceptable_delay, ""); - c.def("check_for_watchdog_exceptions", &RobotDriver::check_for_watchdog_exceptions, ""); - - c.def("connect", &RobotDriver::connect, ""); - c.def("disconnect", &RobotDriver::disconnect, ""); - - c.def("initialize", &RobotDriver::initialize, ""); - c.def("deinitialize", &RobotDriver::deinitialize, ""); -} - -} \ No newline at end of file diff --git a/src/sas_shutdown_signaler.cpp b/src/sas_shutdown_signaler.cpp deleted file mode 100644 index 5d75622..0000000 --- a/src/sas_shutdown_signaler.cpp +++ /dev/null @@ -1,55 +0,0 @@ -/* -# Copyright (c) 2016-2026 Murilo Marques Marinho -# -# This file is part of sas_robot_driver. -# -# sas_robot_driver is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_robot_driver is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_robot_driver. If not, see . -# -# ################################################################ -# -# Author: Murilo M. Marinho, email: murilomarinho@ieee.org -# -# ################################################################# -# Contributors: -#*/ - -/** - * @file sas_shutdown_signaler.cpp - * @brief Implements ShutdownSignaler for coordinated shutdown requests. - */ - -#include - -namespace sas -{ - - bool ShutdownSignaler::should_shutdown() - { - if(external_shutdown_signal_ != nullptr) - { - return *external_shutdown_signal_; - } - else - return internal_shutdown_signal_; - } - void ShutdownSignaler::shutdown() - { - if(external_shutdown_signal_ != nullptr) - { - *external_shutdown_signal_ = true; - } - else - internal_shutdown_signal_ = true; - } -} diff --git a/src/sas_thread_manager.cpp b/src/sas_thread_manager.cpp deleted file mode 100644 index 7a514e9..0000000 --- a/src/sas_thread_manager.cpp +++ /dev/null @@ -1,457 +0,0 @@ -/* -# Copyright (c) 2022-2026 Murilo Marques Marinho -# -# This file is part of sas_core. -# -# sas_core is free software: you can redistribute it and/or modify -# it under the terms of the GNU Lesser General Public License as published by -# the Free Software Foundation, either version 3 of the License, or -# (at your option) any later version. -# -# sas_core is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied warranty of -# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -# GNU Lesser General Public License for more details. -# -# You should have received a copy of the GNU Lesser General Public License -# along with sas_core. If not, see . -# -# ################################################################ -# -# Author: Juan Jose Quiroz Omana, email: juanjose.quirozomana@manchester.ac.uk -# -# ################################################################*/ - -#include -#include -#include -#include -#include - -#ifdef __linux__ -#include -#include -#include -#endif - -namespace sas -{ - -/** - * @brief Constructor for ThreadManager - * - * Creates a thread manager that executes a callback function periodically - * with configurable priority and CPU affinity. - * - * @param thread_name Name of the thread for identification and debugging. - * On Linux, names are truncated to 15 characters. - * @param period Period in seconds between callback executions. - * Must be greater than 0.0. - * @param callback Function to call periodically. The callback should be - * thread-safe and should not block for extended periods - * to avoid missing deadlines. - * @param priority Thread priority level (default: PRIORITY::NORMAL). - * Higher priorities get more CPU time: - * - LOWEST (0): SCHED_OTHER, nice 19 - * - BACKGROUND (10): SCHED_OTHER, nice 10 - * - NORMAL (50): SCHED_OTHER, nice 0 - * - HIGH (80): SCHED_RR, priority 80 (requires root) - * - REALTIME (90): SCHED_FIFO, priority 90 (requires root) - * - CRITICAL (99): SCHED_FIFO, priority 99 (requires root) - * @param cpu_core CPU core to pin the thread to (-1 = no affinity, default). - * Pinning to a specific core improves cache locality and - * reduces context switching for real-time applications. - * - * @throws std::invalid_argument if period <= 0.0. - * - * @note Real-time priorities (HIGH, REALTIME, CRITICAL) require root - * privileges or CAP_SYS_NICE capability. - * - * @warning The callback must NEVER call start() or stop() on this same - * ThreadManager instance, whether directly or indirectly (e.g. - * through a nested function call). start() and stop() run under - * an internal mutex that is held while the loop thread is joined; - * calling either from within the callback (which runs on that - * same loop thread) will deadlock, since the thread would be - * waiting to acquire a mutex held by the very call that is - * waiting for the thread to finish. A stop() call from the - * callback would additionally attempt to join the thread from - * within itself, which is undefined behavior regardless of the - * mutex. If the callback needs to end the loop, it should signal - * that externally (e.g. via a flag it exposes) and let a - * different thread call stop(). - * - * @see PRIORITY - * @see start() - * @see apply_priority() - * @see apply_cpu_affinity() - * - * @code - * // Create a normal priority thread with no CPU affinity - * ThreadManager worker("Worker", 0.01, []() { do_work(); }); - * - * // Create a real-time thread pinned to CPU core 2 - * ThreadManager control( - * "Control", - * 0.001, - * []() { compute_control(); }, - * ThreadManager::PRIORITY::CRITICAL, - * 2 - * ); - * @endcode - */ -ThreadManager::ThreadManager(const std::string& thread_name, - const double& period, - std::function callback, - PRIORITY priority, - int cpu_core) - :priority_{priority}, - cpu_core_{cpu_core}, - clock_{period}, - loop_callback_{std::move(callback)}, - thread_name_{thread_name} -{ - if (period <= 0.0) { - throw std::invalid_argument("ThreadManager: period must be positive"); - } -} - -/** - * @brief Get the CPU core that this thread is pinned to. - * @return CPU core number, or -1 if no affinity is set. - */ -int ThreadManager::get_cpu_core() const -{ - return cpu_core_; -} - -/** - * @brief Get the desired loop period (sampling time). - * @return Period in seconds. - * @details Returns the target period configured at construction time. - */ -double ThreadManager::get_period() const -{ - return clock_.get_desired_thread_sampling_time_sec(); -} - -/** - * @brief Get the priority level of this thread. - * @return The PRIORITY enum value. - */ -ThreadManager::PRIORITY ThreadManager::get_priority() const -{ - return priority_; -} - -/** - * @brief ThreadManager::~ThreadManager destructor of the class - */ -ThreadManager::~ThreadManager() -{ - stop(); -} - - -/** - * @brief Start the thread execution - */ -void ThreadManager::start() -{ - // Lock enabled - std::scoped_lock lock(mutex_); - - if (!running_.exchange(true)) - { - stop_requested_ = false; - clock_.init(); - thread_ = std::thread(&ThreadManager::run, this); - } - // Lock released automatically when scoped_lock goes out of scope -} - -/** - * @brief Stop the thread execution and wait for completion - */ -void ThreadManager::stop() -{ - std::scoped_lock lock(mutex_); - - if (running_.load()) - { - //This is semantically equivalent to stop_requested_ = true - //Just to emphasize that it is an atomic variable. - stop_requested_.store(true); - - if (thread_.joinable()) { - if (std::this_thread::get_id() == thread_.get_id()) { - std::cerr << "Fatal: ThreadManager::stop() called from its own " - "callback thread ('" << thread_name_ - << "'); this would deadlock." << std::endl; - } - thread_.join(); - } - running_.store(false); - } -} - - -/** - * @brief Check if the thread is currently running - * @return true if running, false otherwise - */ -bool ThreadManager::is_running() const -{ - // Returns the current value of running_ in thread-safe way. - return running_.load(); -} - - -/** - * @brief Get the thread name - * @return Thread name string - */ -std::string ThreadManager::get_thread_name() const -{ - return thread_name_; -} - - -/** - * @brief Main thread entry function - */ -void ThreadManager::run() -{ - // Apply thread optimizations - apply_priority(); - apply_cpu_affinity(); - - - // Set thread name (if supported by platform) -#ifdef __linux__ - // pthread_setname_np has a 16 character limit (including null terminator) - // see https://man7.org/linux/man-pages/man3/pthread_setname_np.3.html - std::string truncated_name = thread_name_; - if (truncated_name.length() > 15) { - truncated_name.resize(15); - } - pthread_setname_np(pthread_self(), truncated_name.c_str()); -#endif - - // Main loop - while (!stop_requested_.load()) { - // Execute callback - if (loop_callback_) { - loop_callback_(); - } - clock_.update_and_sleep(); - } -} - -/** - * @brief Apply the configured priority and scheduling policy to the thread. - * - * Sources: https://man7.org/linux/man-pages/man7/sched.7.html - * https://man7.org/linux/man-pages/man2/setpriority.2.html - * https://www.ibm.com/docs/en/zos/3.2.0?topic=functions-setpriority-set-process-scheduling-priority - * The Linux Programming Interface by Michael Kerrisk (Chapter 35 PROCESS PRIORITIES AND SCHEDULING) - * - * - * This method sets the thread's scheduling priority and policy based on the - * PRIORITY enum value configured at construction time. It uses Linux's - * pthread_setschedparam API to apply the settings. - * - * @details The method maps the abstract PRIORITY levels to specific Linux - * scheduler policies and priority values: - * - * | PRIORITY Level | Linux Policy | Priority Value | Description | - * |------------------|--------------|----------------|-------------| - * | LOWEST | SCHED_OTHER | 0 (nice 19) | Low priority background tasks | - * | BACKGROUND | SCHED_OTHER | 0 (nice 10) | Background processing | - * | NORMAL | SCHED_OTHER | 0 (nice 0) | Standard scheduling | - * | HIGH | SCHED_RR | 80 | Round-robin real-time | - * | REALTIME | SCHED_FIFO | 90 | FIFO real-time | - * | CRITICAL | SCHED_FIFO | 99 | Highest priority (max) | - * - * @note This method is only available on Linux systems. - * @note Real-time policies (SCHED_RR and SCHED_FIFO) require root privileges - * or the CAP_SYS_NICE capability. - * @note This method should be called from within the thread context (after - * the thread has started). - * - * @warning Real-time priorities (HIGH, REALTIME, CRITICAL) can starve other - * threads if not used carefully. Only use them for time-critical tasks. - * @warning This method does NOT throw exceptions on failure; it only prints - * a warning to std::cerr. - * - * @see PRIORITY - Enum definition for priority levels - * @see apply_cpu_affinity() - Related thread optimization method - * @see start() - The method that triggers the priority application - * - * @code - * // Create a critical real-time thread - * ThreadManager control( - * "Control", - * 0.001, - * []() { compute_control(); }, - * PRIORITY::CRITICAL, // Will use SCHED_FIFO with priority 99 - * 2 // Pin to core 2 - * ); - * control.start(); // Priority and affinity are applied in run() - * @endcode - */ -void ThreadManager::apply_priority() { -#ifdef __linux__ - pthread_t thread_handle = pthread_self(); - - struct sched_param param = {0}; - int policy = SCHED_OTHER; - int nice_value = 0; - bool use_nice = false; - - // Map to Linux scheduling - switch (priority_) { - case PRIORITY::LOWEST: - policy = SCHED_OTHER; - param.sched_priority = 0; - nice_value = 19; - use_nice = true; - break; - case PRIORITY::BACKGROUND: - policy = SCHED_OTHER; - param.sched_priority = 0; - nice_value = 10; - use_nice = true; - break; - case PRIORITY::NORMAL: - policy = SCHED_OTHER; - param.sched_priority = 0; - nice_value = 0; - use_nice = true; - break; - case PRIORITY::HIGH: - policy = SCHED_RR; - param.sched_priority = 80; - break; - case PRIORITY::REALTIME: - policy = SCHED_FIFO; - param.sched_priority = 90; - break; - case PRIORITY::CRITICAL: - policy = SCHED_FIFO; - param.sched_priority = 99; - break; - } - - // pthread_setschedparam() returns the error number directly on failure — - // it does NOT set errno. strerror() is called on the return value itself. - // Note: C++17 If-Initializer Syntax - if (int rc = pthread_setschedparam(thread_handle, policy, ¶m); rc != 0) { - std::cerr << "Warning: Failed to set scheduling policy/priority for thread '" - << thread_name_ << "': " << std::strerror(rc) << std::endl; - } - - // setpriority(), unlike pthread_setschedparam(), does follow the errno - // convention: it returns -1 and sets errno on failure. - // https://man7.org/linux/man-pages/man3/errno.3.html - if (use_nice) { - errno = 0; - if (setpriority(PRIO_PROCESS, 0, nice_value) != 0) { - std::cerr << "Warning: Failed to set nice value for thread '" - << thread_name_ << "': " << std::strerror(errno) << std::endl; - } - } -#endif -} - - -/** - * @brief Apply CPU core affinity to the thread. - * - * This method pins the thread to a specific CPU core to improve cache locality, - * reduce context switching, and ensure predictable performance for real-time - * applications. - * - * @details The method uses the Linux pthread_setaffinity_np API to bind the - * thread to the CPU core specified by cpu_core_. This is particularly - * important for: - * - Real-time control loops that need deterministic timing - * - High-frequency communication threads (UDP, CAN, etc.) - * - Threads that benefit from dedicated cache resources - * - * @note This method is only available on Linux systems. - * @note If cpu_core_ is negative (< 0), affinity is not applied and the - * thread can run on any available CPU core. - * @note This method should be called from within the thread context (after - * the thread has started). - * - * @warning Setting CPU affinity can degrade performance if the specified core - * becomes overloaded. Use with caution in multi-threaded systems. - * @warning Thread must be joinable and running when this method is called. - * - * @see set_cpu_affinity() - Setter for the cpu_core_ member - * @see apply_priority() - Related thread optimization method - * - * @code - * // Pin control thread to CPU core 2 - * ThreadManager control("Control", 0.001, []() { - * compute_control(); - * }, PRIORITY::REALTIME, 2); - * - * // Thread will be pinned to core 2 when start() is called - * control.start(); - * @endcode - */ -void ThreadManager::apply_cpu_affinity() { -#ifdef __linux__ - if (cpu_core_ < 0) return; - - cpu_set_t cpuset; - CPU_ZERO(&cpuset); - CPU_SET(cpu_core_, &cpuset); - - pthread_t thread_handle = thread_.native_handle(); - if (pthread_setaffinity_np(thread_handle, sizeof(cpu_set_t), &cpuset) != 0) { - std::cerr << "Warning: Failed to set CPU affinity for thread '" - << thread_name_ << "' to core " << cpu_core_ << std::endl; - } -#endif -} - - -/** - * @brief Get a const reference to the internal clock instance. - * @return Const reference to the sas::Clock object used for timing and statistics. - * @details Provides access to the clock that manages timing, performance monitoring, - * and statistics collection for this thread. This is useful for: - * - Reading detailed timing statistics - * - Monitoring thread performance in real-time - * - Accessing clock configuration and state - * - * @note Returns a const reference to prevent modification of the clock's internal state. - * @note The clock is updated in the thread's main loop via clock_.update_and_sleep(). - * - * @see get_computation_time() - * @see get_sleep_time() - * @see get_effective_sampling_time() - * @see get_overrun_count() - * @see get_statistics() - * - * @code - * ThreadManager worker("Worker", 0.01, []() { do_work(); }); - * worker.start(); - * - * // Access clock information - * const auto& clock = worker.get_clock(); - * double mean_computation = clock.get_statistics(Statistics::Mean, - * sas::Clock::TimeType::Computational); - * @endcode - */ -const sas::Clock& ThreadManager::get_clock() const -{ - return clock_; -} - - - -}